From c816d165754d8fd002478cce6eb774b9390c795f Mon Sep 17 00:00:00 2001
From: Sam Shih <sam.shih@mediatek.com>
Date: Fri, 2 Jun 2023 13:06:01 +0800
Subject: [PATCH] 
 [backport-networking-drivers][999-1707-add-mdio-bus-for-gphy-calibration.patch]

---
 drivers/net/dsa/mt7530.c | 115 +++++++++++++++++++++++++++++++++++++++
 1 file changed, 115 insertions(+)

diff --git a/drivers/net/dsa/mt7530.c b/drivers/net/dsa/mt7530.c
index 2cd5dae9c..290a2e77a 100644
--- a/drivers/net/dsa/mt7530.c
+++ b/drivers/net/dsa/mt7530.c
@@ -847,6 +847,117 @@ mt7531_ind_phy_write(struct dsa_switch *ds, int port, int regnum,
 	return ret;
 }
 
+static int mt753x_mdio_read(struct mii_bus *bus, int addr, int regnum)
+{
+	struct mt7530_priv *priv = bus->priv;
+	struct mt7530_dummy_poll p;
+	int ret;
+	u32 val;
+
+	INIT_MT7530_DUMMY_POLL(&p, priv, MT7531_PHY_IAC);
+
+	mutex_lock_nested(&priv->bus->mdio_lock, MDIO_MUTEX_NESTED);
+
+	ret = readx_poll_timeout(_mt7530_unlocked_read, &p, val,
+				 !(val & MT7531_PHY_ACS_ST), 20, 100000);
+	if (ret < 0) {
+		dev_err(priv->dev, "poll timeout\n");
+		goto out;
+	}
+
+	val = MT7531_MDIO_CL22_READ | MT7531_MDIO_PHY_ADDR(addr) |
+		  MT7531_MDIO_REG_ADDR(regnum);
+
+	mt7530_mii_write(priv, MT7531_PHY_IAC, val | MT7531_PHY_ACS_ST);
+
+	ret = readx_poll_timeout(_mt7530_unlocked_read, &p, val,
+				 !(val & MT7531_PHY_ACS_ST), 20, 100000);
+	if (ret < 0) {
+		dev_err(priv->dev, "poll timeout\n");
+		goto out;
+	}
+
+	ret = val & MT7531_MDIO_RW_DATA_MASK;
+out:
+	mutex_unlock(&priv->bus->mdio_lock);
+
+	return ret;
+}
+
+static int mt753x_mdio_write(struct mii_bus *bus, int addr, int regnum, u16 val)
+{
+	struct mt7530_priv *priv = bus->priv;
+	struct mt7530_dummy_poll p;
+	int ret;
+	u32 reg;
+
+	INIT_MT7530_DUMMY_POLL(&p, priv, MT7531_PHY_IAC);
+
+	mutex_lock_nested(&priv->bus->mdio_lock, MDIO_MUTEX_NESTED);
+
+	ret = readx_poll_timeout(_mt7530_unlocked_read, &p, reg,
+				 !(reg & MT7531_PHY_ACS_ST), 20, 100000);
+	if (ret < 0) {
+		dev_err(priv->dev, "poll timeout\n");
+		goto out;
+	}
+
+	reg = MT7531_MDIO_CL22_WRITE | MT7531_MDIO_PHY_ADDR(addr) |
+		  MT7531_MDIO_REG_ADDR(regnum) | val;
+
+	mt7530_mii_write(priv, MT7531_PHY_IAC, reg | MT7531_PHY_ACS_ST);
+
+	ret = readx_poll_timeout(_mt7530_unlocked_read, &p, reg,
+				 !(reg & MT7531_PHY_ACS_ST), 20, 100000);
+	if (ret < 0) {
+		dev_err(priv->dev, "poll timeout\n");
+		goto out;
+	}
+
+out:
+	mutex_unlock(&priv->bus->mdio_lock);
+
+	return ret;
+}
+
+static int mt753x_setup_mdio(struct dsa_switch *ds)
+{
+	struct mt7530_priv *priv = ds->priv;
+	struct device_node *mdio_np;
+	int ret;
+
+	mdio_np = of_get_compatible_child(priv->dev->of_node, "mediatek,dsa-slave-mdio");
+	if (!mdio_np) {
+		dev_err(priv->dev, "no MDIO bus node\n");
+		return -ENODEV;
+	}
+
+	priv->ds->slave_mii_bus = devm_mdiobus_alloc(priv->dev);
+	if (!priv->ds->slave_mii_bus) {
+		ret = -ENOMEM;
+		goto err_put_node;
+	}
+	priv->ds->slave_mii_bus->name = "mediatek,dsa-slave-mdio";
+	priv->ds->slave_mii_bus->priv = priv;
+	priv->ds->slave_mii_bus->parent = priv->dev;
+	priv->ds->slave_mii_bus->phy_mask = ~priv->ds->phys_mii_mask;
+	priv->ds->slave_mii_bus->read = mt753x_mdio_read;
+	priv->ds->slave_mii_bus->write = mt753x_mdio_write;
+	snprintf(priv->ds->slave_mii_bus->id, MII_BUS_ID_SIZE, "dsa-%d.%d",
+		 priv->ds->dst->index, priv->ds->index);
+	priv->ds->slave_mii_bus->dev.of_node = mdio_np;
+
+	ret = of_mdiobus_register(priv->ds->slave_mii_bus, mdio_np);
+	if (ret)
+		dev_err(priv->dev, "unable to register MDIO bus %s\n",
+			priv->ds->slave_mii_bus->id);
+
+err_put_node:
+	of_node_put(mdio_np);
+
+	return ret;
+}
+
 static void
 mt7530_get_strings(struct dsa_switch *ds, int port, u32 stringset,
 		   uint8_t *data)
@@ -2695,6 +2806,10 @@ mt7988_setup(struct dsa_switch *ds)
 	if (ret < 0)
 		return ret;
 
+	ret = mt753x_setup_mdio(ds);
+	if (ret < 0)
+		dev_err(priv->dev, "mt753x_setup_mdio failed\n");
+
 	return 0;
 }
 
-- 
2.34.1