[v5 10/18] net: designware: socfpga: Add ATF support for MAC driver

[Siew Chin Lim]

From: Chee Hong Ang <chee.hong.ang at intel.com>

In non-secure mode (EL2), MAC driver calls the SMC/PSCI services
provided by ATF to setup the PHY interface.

Signed-off-by: Chee Hong Ang <chee.hong.ang at intel.com>
Signed-off-by: Siew Chin Lim <elly.siew.chin.lim at intel.com>

---
v5
---
Call secure register access helper function to write the secure register.
Return error if fail to write the PHY related secure register.
---
 drivers/net/dwmac_socfpga.c | 38 ++++++++++++++++++++++++++++++++------
 1 file changed, 32 insertions(+), 6 deletions(-)

diff --git a/drivers/net/dwmac_socfpga.c b/drivers/net/dwmac_socfpga.c
index e93561dffa..8efb88c8fb 100644
--- a/drivers/net/dwmac_socfpga.c
+++ b/drivers/net/dwmac_socfpga.c
@@ -6,6 +6,8 @@
  */
 
 #include <common.h>
+#include <asm/arch/secure_reg_helper.h>
+#include <asm/arch/system_manager.h>
 #include <asm/io.h>
 #include <dm.h>
 #include <clk.h>
@@ -17,8 +19,6 @@
 #include <dm/device_compat.h>
 #include <linux/err.h>
 
-#include <asm/arch/system_manager.h>
-
 struct dwmac_socfpga_platdata {
 	struct dw_eth_pdata	dw_eth_pdata;
 	void			*phy_intf;
@@ -64,6 +64,33 @@ static int dwmac_socfpga_ofdata_to_platdata(struct udevice *dev)
 	return designware_eth_ofdata_to_platdata(dev);
 }
 
+static int dwmac_socfpga_do_setphy(struct udevice *dev, u32 modereg)
+{
+	struct dwmac_socfpga_platdata *pdata = dev_get_platdata(dev);
+	u32 modemask = SYSMGR_EMACGRP_CTRL_PHYSEL_MASK << pdata->reg_shift;
+
+#if !defined(CONFIG_SPL_BUILD) && defined(CONFIG_SPL_ATF)
+	u32 val = (readl(pdata->phy_intf) & ~modemask) |
+		  (modereg << pdata->reg_shift);
+
+	u32 index = ((u64)pdata->phy_intf - socfpga_get_sysmgr_addr() -
+		     SYSMGR_SOC64_EMAC0) >> 2;
+
+	u32 id = SOCFPGA_SECURE_REG_SYSMGR_SOC64_EMAC0 + index;
+
+	int ret = socfpga_secure_reg_write32(id, val);
+	if (ret) {
+		dev_err(dev, "Failed to set PHY register via SMC call\n");
+		return ret;
+	}
+#else
+	clrsetbits_le32(pdata->phy_intf, modemask,
+			modereg << pdata->reg_shift);
+#endif
+
+	return 0;
+}
+
 static int dwmac_socfpga_probe(struct udevice *dev)
 {
 	struct dwmac_socfpga_platdata *pdata = dev_get_platdata(dev);
@@ -71,7 +98,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 +123,9 @@ 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);
+	ret = dwmac_socfpga_do_setphy(dev, modereg);
+	if (ret)
+		return ret;
 
 	reset_release_bulk(&reset_bulk);
 
-- 
2.13.0