[v5,11/17] net: designware: socfpga: Add ATF support for MAC driver

diff --git a/drivers/net/dwmac_socfpga.c b/drivers/net/dwmac_socfpga.c
index e93561d..600b2e6 100644
--- a/drivers/net/dwmac_socfpga.c
+++ b/drivers/net/dwmac_socfpga.c
@@ -14,8 +14,10 @@ 
 #include <reset.h>
 #include <syscon.h>
 #include "designware.h"
+#include <asm/arch/misc.h>
 #include <dm/device_compat.h>
 #include <linux/err.h>
+#include <linux/intel-smc.h>
 
 #include <asm/arch/system_manager.h>
 
@@ -64,6 +66,35 @@  static int dwmac_socfpga_ofdata_to_platdata(struct udevice *dev)
 	return designware_eth_ofdata_to_platdata(dev);
 }
 
+#if !defined(CONFIG_SPL_BUILD) && defined(CONFIG_SPL_ATF)
+static int dwmac_socfpga_fw_setphy(struct udevice *dev, u32 modereg)
+{
+	struct ofnode_phandle_args pargs;
+	u64 args[2];
+	int ret;
+
+	ret = dev_read_phandle_with_args(dev, "altr,sysmgr-syscon", NULL,
+					 1, 0, &pargs);
+	if (ret) {
+		dev_err(dev, "Failed to get syscon: %d\n", ret);
+		return ret;
+	}
+
+	if (pargs.args_count < 1) {
+		dev_err(dev, "No syscon args found\n");
+		return -EINVAL;
+	}
+
+	args[0] = ((u64)pargs.args[0] - SYSMGR_SOC64_EMAC0) >> 2;
+	args[1] = modereg;
+
+	if (invoke_smc(INTEL_SIP_SMC_HPS_SET_PHYINTF, args, 2, NULL, 0))
+		return -EIO;
+
+	return 0;
+}
+#endif
+
 static int dwmac_socfpga_probe(struct udevice *dev)
 {
 	struct dwmac_socfpga_platdata *pdata = dev_get_platdata(dev);
@@ -71,7 +102,6 @@  static int dwmac_socfpga_probe(struct udevice *dev)
 	struct reset_ctl_bulk reset_bulk;
 	int ret;
 	u32 modereg;
-	u32 modemask;
 
 	switch (edata->phy_interface) {
 	case PHY_INTERFACE_MODE_MII:
@@ -97,9 +127,14 @@  static int dwmac_socfpga_probe(struct udevice *dev)
 
 	reset_assert_bulk(&reset_bulk);
 
-	modemask = SYSMGR_EMACGRP_CTRL_PHYSEL_MASK << pdata->reg_shift;
-	clrsetbits_le32(pdata->phy_intf, modemask,
-			modereg << pdata->reg_shift);
+#if !defined(CONFIG_SPL_BUILD) && defined(CONFIG_SPL_ATF)
+	ret = dwmac_socfpga_fw_setphy(dev, modereg);
+	if (ret)
+		return ret;
+#else
+	clrsetbits_le32(pdata->phy_intf, SYSMGR_EMACGRP_CTRL_PHYSEL_MASK <<
+			pdata->reg_shift, modereg << pdata->reg_shift);
+#endif
 
 	reset_release_bulk(&reset_bulk);