1 files changed, 78 insertions, 14 deletions

diff --git a/drivers/net/mtk_eth/mt7988.c b/drivers/net/mtk_eth/mt7988.c
index a416d87840c..29b6363cbd7 100644
--- a/drivers/net/mtk_eth/mt7988.c
+++ b/drivers/net/mtk_eth/mt7988.c
@@ -6,6 +6,7 @@
  * Author: Mark Lee <mark-mc.lee@mediatek.com>
  */
 
+#include <malloc.h>
 #include <miiphy.h>
 #include <linux/delay.h>
 #include <linux/mdio.h>
@@ -14,6 +15,8 @@
 #include "mtk_eth.h"
 #include "mt753x.h"
 
+#include "../mdio-mt7531-mmio.h"
+
 static int mt7988_reg_read(struct mt753x_switch_priv *priv, u32 reg, u32 *data)
 {
 	*data = readl(priv->epriv.ethsys_base + GSW_BASE + reg);
@@ -30,20 +33,34 @@ static int mt7988_reg_write(struct mt753x_switch_priv *priv, u32 reg, u32 data)
 
 static void mt7988_phy_setting(struct mt753x_switch_priv *priv)
 {
+	struct mii_dev *mdio_bus = priv->mdio_bus;
 	u16 val;
 	u32 i;
 
 	for (i = 0; i < MT753X_NUM_PHYS; i++) {
+		u16 addr = MT753X_PHY_ADDR(priv->phy_base, i);
+
+		/* Set PHY to PHY page 1 */
+		mt7531_mdio_mmio_write(mdio_bus, addr, MDIO_DEVAD_NONE,
+				       0x1f, 0x1);
+
 		/* Enable HW auto downshift */
-		mt7531_mii_write(priv, i, 0x1f, 0x1);
-		val = mt7531_mii_read(priv, i, PHY_EXT_REG_14);
+		val = mt7531_mdio_mmio_read(mdio_bus, addr, MDIO_DEVAD_NONE,
+					    PHY_EXT_REG_14);
 		val |= PHY_EN_DOWN_SHFIT;
-		mt7531_mii_write(priv, i, PHY_EXT_REG_14, val);
+		mt7531_mdio_mmio_write(mdio_bus, addr, MDIO_DEVAD_NONE,
+				       PHY_EXT_REG_14, val);
 
 		/* PHY link down power saving enable */
-		val = mt7531_mii_read(priv, i, PHY_EXT_REG_17);
+		val = mt7531_mdio_mmio_read(mdio_bus, addr, MDIO_DEVAD_NONE,
+					    PHY_EXT_REG_17);
 		val |= PHY_LINKDOWN_POWER_SAVING_EN;
-		mt7531_mii_write(priv, i, PHY_EXT_REG_17, val);
+		mt7531_mdio_mmio_write(mdio_bus, addr, MDIO_DEVAD_NONE,
+				       PHY_EXT_REG_17, val);
+
+		/* Restore PHY to PHY page 0 */
+		mt7531_mdio_mmio_write(mdio_bus, addr, MDIO_DEVAD_NONE,
+				       0x1f, 0x0);
 	}
 }
 
@@ -58,24 +75,66 @@ static void mt7988_mac_control(struct mtk_eth_switch_priv *swpriv, bool enable)
 	mt7988_reg_write(priv, PMCR_REG(6), pmcr);
 }
 
+static int mt7988_mdio_register(struct mt753x_switch_priv *priv)
+{
+	struct mt7531_mdio_mmio_priv *mdio_priv;
+	struct mii_dev *mdio_bus = mdio_alloc();
+	int ret;
+
+	if (!mdio_bus)
+		return -ENOMEM;
+
+	mdio_priv = malloc(sizeof(*mdio_priv));
+	if (!mdio_priv)
+		return -ENOMEM;
+
+	mdio_priv->switch_regs = (phys_addr_t)priv->epriv.ethsys_base + GSW_BASE;
+
+	mdio_bus->read = mt7531_mdio_mmio_read;
+	mdio_bus->write = mt7531_mdio_mmio_write;
+	snprintf(mdio_bus->name, sizeof(mdio_bus->name), priv->epriv.sw->name);
+
+	mdio_bus->priv = mdio_priv;
+
+	ret = mdio_register(mdio_bus);
+	if (ret) {
+		free(mdio_bus->priv);
+		mdio_free(mdio_bus);
+		return ret;
+	}
+
+	priv->mdio_bus = mdio_bus;
+
+	return 0;
+}
+
 static int mt7988_setup(struct mtk_eth_switch_priv *swpriv)
 {
 	struct mt753x_switch_priv *priv = (struct mt753x_switch_priv *)swpriv;
+	struct mii_dev *mdio_bus;
 	u16 phy_addr, phy_val;
+	int ret, i;
 	u32 pmcr;
-	int i;
 
 	priv->smi_addr = MT753X_DFL_SMI_ADDR;
 	priv->phy_base = (priv->smi_addr + 1) & MT753X_SMI_ADDR_MASK;
 	priv->reg_read = mt7988_reg_read;
 	priv->reg_write = mt7988_reg_write;
 
+	ret = mt7988_mdio_register(priv);
+	if (ret)
+		return ret;
+
+	mdio_bus = priv->mdio_bus;
+
 	/* Turn off PHYs */
 	for (i = 0; i < MT753X_NUM_PHYS; i++) {
 		phy_addr = MT753X_PHY_ADDR(priv->phy_base, i);
-		phy_val = mt7531_mii_read(priv, phy_addr, MII_BMCR);
+		phy_val = mt7531_mdio_mmio_read(mdio_bus, phy_addr,
+						MDIO_DEVAD_NONE, MII_BMCR);
 		phy_val |= BMCR_PDOWN;
-		mt7531_mii_write(priv, phy_addr, MII_BMCR, phy_val);
+		mt7531_mdio_mmio_write(mdio_bus, phy_addr, MDIO_DEVAD_NONE,
+				       MII_BMCR, phy_val);
 	}
 
 	switch (priv->epriv.phy_interface) {
@@ -129,21 +188,26 @@ static int mt7988_setup(struct mtk_eth_switch_priv *swpriv)
 	/* Turn on PHYs */
 	for (i = 0; i < MT753X_NUM_PHYS; i++) {
 		phy_addr = MT753X_PHY_ADDR(priv->phy_base, i);
-		phy_val = mt7531_mii_read(priv, phy_addr, MII_BMCR);
+		phy_val = mt7531_mdio_mmio_read(mdio_bus, phy_addr,
+						MDIO_DEVAD_NONE, MII_BMCR);
 		phy_val &= ~BMCR_PDOWN;
-		mt7531_mii_write(priv, phy_addr, MII_BMCR, phy_val);
+		mt7531_mdio_mmio_write(mdio_bus, phy_addr, MDIO_DEVAD_NONE,
+				       MII_BMCR, phy_val);
 	}
 
 	mt7988_phy_setting(priv);
 
-	return mt7531_mdio_register(priv);
+	return 0;
 }
 
-static int mt7531_cleanup(struct mtk_eth_switch_priv *swpriv)
+static int mt7988_cleanup(struct mtk_eth_switch_priv *swpriv)
 {
 	struct mt753x_switch_priv *priv = (struct mt753x_switch_priv *)swpriv;
+	struct mii_dev *mdio_bus = priv->mdio_bus;
 
-	mdio_unregister(priv->mdio_bus);
+	mdio_unregister(mdio_bus);
+	free(mdio_bus->priv);
+	mdio_free(mdio_bus);
 
 	return 0;
 }
@@ -155,6 +219,6 @@ MTK_ETH_SWITCH(mt7988) = {
 	.reset_wait_time = 50,
 
 	.setup = mt7988_setup,
-	.cleanup = mt7531_cleanup,
+	.cleanup = mt7988_cleanup,
 	.mac_control = mt7988_mac_control,
 };