realtek: implement Clause-45 MDIO write on rtl931x

 * Add missing Clause-45 write support for rtl931x
 * Switch to use helper functions in all Clause-45 access functions to
   make the code more readable.
 * More meaningful/unified debugging output (dynamic kprintf)

Signed-off-by: Daniel Golle <daniel@makrotopia.org>

2 files changed, 65 insertions, 26 deletions

diff --git a/target/linux/realtek/files-5.10/drivers/net/dsa/rtl83xx/rtl931x.c b/target/linux/realtek/files-5.10/drivers/net/dsa/rtl83xx/rtl931x.c
index 9ba120899a..48692ac7ea 100644
--- a/target/linux/realtek/files-5.10/drivers/net/dsa/rtl83xx/rtl931x.c
+++ b/target/linux/realtek/files-5.10/drivers/net/dsa/rtl83xx/rtl931x.c
@@ -370,7 +370,13 @@ int rtl931x_read_mmd_phy(u32 port, u32 devnum, u32 regnum, u32 *val)
 {
 	int err = 0;
 	u32 v;
-	int type = 2; // TODO:2, for C45 PHYs need to set to 1 sometimes
+	/* Select PHY register type
+	 * If select 1G/10G MMD register type, registers EXT_PAGE, MAIN_PAGE and REG settings are don’t care.
+	 * 0x0  Normal register (Clause 22)
+	 * 0x1: 1G MMD register (MMD via Clause 22 registers 13 and 14)
+	 * 0x2: 10G MMD register (MMD via Clause 45)
+	 */
+	int type = (regnum & MII_ADDR_C45)?2:1;
 
 	mutex_lock(&smi_lock);
 
@@ -378,7 +384,7 @@ int rtl931x_read_mmd_phy(u32 port, u32 devnum, u32 regnum, u32 *val)
 	sw_w32(port << 5, RTL931X_SMI_INDRT_ACCESS_BC_PHYID_CTRL);
 
 	// Set MMD device number and register to write to
-	sw_w32(devnum << 16 | (regnum & 0xffff), RTL931X_SMI_INDRT_ACCESS_MMD_CTRL);
+	sw_w32(devnum << 16 | mdiobus_c45_regad(regnum), RTL931X_SMI_INDRT_ACCESS_MMD_CTRL);
 
 	v = type << 2 | BIT(0); // MMD-access-type | EXEC
 	sw_w32(v, RTL931X_SMI_INDRT_ACCESS_CTRL_0);
@@ -393,7 +399,8 @@ int rtl931x_read_mmd_phy(u32 port, u32 devnum, u32 regnum, u32 *val)
 
 	*val = sw_r32(RTL931X_SMI_INDRT_ACCESS_CTRL_3) >> 16;
 
-	pr_debug("%s: port %d, regnum: %x, val: %x (err %d)\n", __func__, port, regnum, *val, err);
+	pr_debug("%s: port %d, dev: %x, regnum: %x, val: %x (err %d)\n", __func__,
+		 port, devnum, mdiobus_c45_regad(regnum), *val, err);
 
 	mutex_unlock(&smi_lock);
 
@@ -407,18 +414,21 @@ int rtl931x_write_mmd_phy(u32 port, u32 devnum, u32 regnum, u32 val)
 {
 	int err = 0;
 	u32 v;
-	int type = 1; // TODO: For C45 PHYs need to set to 2
+	int type = (regnum & MII_ADDR_C45)?2:1;
+	u64 pm;
 
 	mutex_lock(&smi_lock);
 
-	// Set PHY to access via port-number
-	sw_w32(port << 5, RTL931X_SMI_INDRT_ACCESS_BC_PHYID_CTRL);
+	// Set PHY to access via port-mask
+	pm = (u64)1 << port;
+	sw_w32((u32)pm, RTL931X_SMI_INDRT_ACCESS_CTRL_2);
+	sw_w32((u32)(pm >> 32), RTL931X_SMI_INDRT_ACCESS_CTRL_2 + 4);
 
 	// Set data to write
 	sw_w32_mask(0xffff, val, RTL931X_SMI_INDRT_ACCESS_CTRL_3);
 
 	// Set MMD device number and register to write to
-	sw_w32(devnum << 16 | (regnum & 0xffff), RTL931X_SMI_INDRT_ACCESS_MMD_CTRL);
+	sw_w32(devnum << 16 | mdiobus_c45_regad(regnum), RTL931X_SMI_INDRT_ACCESS_MMD_CTRL);
 
 	v = BIT(4) | type << 2 | BIT(0); // WRITE | MMD-access-type | EXEC
 	sw_w32(v, RTL931X_SMI_INDRT_ACCESS_CTRL_0);
@@ -427,7 +437,8 @@ int rtl931x_write_mmd_phy(u32 port, u32 devnum, u32 regnum, u32 val)
 		v = sw_r32(RTL931X_SMI_INDRT_ACCESS_CTRL_0);
 	} while (v & BIT(0));
 
-	pr_debug("%s: port %d, regnum: %x, val: %x (err %d)\n", __func__, port, regnum, val, err);
+	pr_debug("%s: port %d, dev: %x, regnum: %x, val: %x (err %d)\n", __func__,
+		 port, devnum, mdiobus_c45_regad(regnum), val, err);
 	mutex_unlock(&smi_lock);
 	return err;
 }
diff --git a/target/linux/realtek/files-5.10/drivers/net/ethernet/rtl838x_eth.c b/target/linux/realtek/files-5.10/drivers/net/ethernet/rtl838x_eth.c
index f13c4b0c02..eb566766ca 100644
--- a/target/linux/realtek/files-5.10/drivers/net/ethernet/rtl838x_eth.c
+++ b/target/linux/realtek/files-5.10/drivers/net/ethernet/rtl838x_eth.c
@@ -1662,12 +1662,15 @@ static int rtl930x_mdio_read(struct mii_bus *bus, int mii_id, int regnum)
 		return rtl930x_read_sds_phy(priv->sds_id[mii_id], 0, regnum);
 
 	if (regnum & MII_ADDR_C45) {
-		regnum &= ~MII_ADDR_C45;
-		err = rtl930x_read_mmd_phy(mii_id, regnum >> 16, regnum & 0xffff, &val);
-		pr_debug("MMD: %d register %d read %x, err %d\n", mii_id, regnum & 0xffff, val, err);
+		err = rtl930x_read_mmd_phy(mii_id,
+					   mdiobus_c45_devad(regnum),
+					   regnum, &val);
+		pr_debug("MMD: %d dev %x register %x read %x, err %d\n", mii_id,
+			 mdiobus_c45_devad(regnum), mdiobus_c45_regad(regnum),
+			 val, err);
 	} else {
 		err = rtl930x_read_phy(mii_id, 0, regnum, &val);
-		pr_debug("PHY: %d register %d read %x, err %d\n", mii_id, regnum, val, err);
+		pr_debug("PHY: %d register %x read %x, err %d\n", mii_id, regnum, val, err);
 	}
 	if (err)
 		return err;
@@ -1692,12 +1695,16 @@ static int rtl931x_mdio_read(struct mii_bus *bus, int mii_id, int regnum)
 		}
 	} else {
 		if (regnum & MII_ADDR_C45) {
-			regnum &= ~MII_ADDR_C45;
-			err = rtl931x_read_mmd_phy(mii_id, regnum >> 16, regnum & 0xffff, &val);
+			err = rtl931x_read_mmd_phy(mii_id,
+						   mdiobus_c45_devad(regnum),
+						   regnum, &val);
+			pr_debug("MMD: %d dev %x register %x read %x, err %d\n", mii_id,
+				 mdiobus_c45_devad(regnum), mdiobus_c45_regad(regnum),
+				 val, err);
 		} else {
 			err = rtl931x_read_phy(mii_id, 0, regnum, &val);
+			pr_debug("PHY: %d register %x read %x, err %d\n", mii_id, regnum, val, err);
 		}
-		pr_debug("%s: phy %d, register %d value %x\n", __func__, mii_id, regnum, val);
 	}
 
 	if (err)
@@ -1710,6 +1717,7 @@ static int rtl838x_mdio_write(struct mii_bus *bus, int mii_id,
 {
 	u32 offset = 0;
 	struct rtl838x_eth_priv *priv = bus->priv;
+	int err;
 
 	if (mii_id >= 24 && mii_id <= 27 && priv->id == 0x8380) {
 		if (mii_id == 26)
@@ -1717,45 +1725,65 @@ static int rtl838x_mdio_write(struct mii_bus *bus, int mii_id,
 		sw_w32(value, RTL838X_SDS4_FIB_REG0 + offset + (regnum << 2));
 		return 0;
 	}
-	return rtl838x_write_phy(mii_id, 0, regnum, value);
+	err = rtl838x_write_phy(mii_id, 0, regnum, value);
+	pr_debug("PHY: %d register %x write %x, err %d\n", mii_id, regnum, value, err);
+	return err;
 }
 
 static int rtl839x_mdio_write(struct mii_bus *bus, int mii_id,
 			      int regnum, u16 value)
 {
 	struct rtl838x_eth_priv *priv = bus->priv;
+	int err;
 
 	if (mii_id >= 48 && mii_id <= 49 && priv->id == 0x8393)
 		return rtl839x_write_sds_phy(mii_id, regnum, value);
 
-	return rtl839x_write_phy(mii_id, 0, regnum, value);
+	err = rtl839x_write_phy(mii_id, 0, regnum, value);
+	pr_debug("PHY: %d register %x write %x, err %d\n", mii_id, regnum, value, err);
+	return err;
 }
 
 static int rtl930x_mdio_write(struct mii_bus *bus, int mii_id,
 			      int regnum, u16 value)
 {
 	struct rtl838x_eth_priv *priv = bus->priv;
+	int err;
 
 	if (priv->sds_id[mii_id] >= 0)
 		return rtl930x_write_sds_phy(priv->sds_id[mii_id], 0, regnum, value);
 
-	if (regnum & MII_ADDR_C45) {
-		regnum &= ~MII_ADDR_C45;
-		return rtl930x_write_mmd_phy(mii_id, regnum >> 16, regnum & 0xffff, value);
-	}
+	if (regnum & MII_ADDR_C45)
+		return rtl930x_write_mmd_phy(mii_id, mdiobus_c45_devad(regnum),
+					     regnum, value);
 
-	return rtl930x_write_phy(mii_id, 0, regnum, value);
+	err = rtl930x_write_phy(mii_id, 0, regnum, value);
+	pr_debug("PHY: %d register %x write %x, err %d\n", mii_id, regnum, value, err);
+	return err;
 }
 
 static int rtl931x_mdio_write(struct mii_bus *bus, int mii_id,
 			      int regnum, u16 value)
 {
 	struct rtl838x_eth_priv *priv = bus->priv;
+	int err;
 
-	if (priv->sds_id[mii_id] >= 0)
+	if (priv->sds_id[mii_id] >= 0 && mii_id >= 52)
 		return rtl931x_write_sds_phy(priv->sds_id[mii_id], 0, regnum, value);
 
-	return rtl931x_write_phy(mii_id, 0, regnum, value);
+	if (regnum & MII_ADDR_C45) {
+		err = rtl931x_write_mmd_phy(mii_id, mdiobus_c45_devad(regnum),
+					    regnum, value);
+		pr_debug("MMD: %d dev %x register %x write %x, err %d\n", mii_id,
+			 mdiobus_c45_devad(regnum), mdiobus_c45_regad(regnum),
+			 value, err);
+
+		return err;
+	}
+
+	err = rtl931x_write_phy(mii_id, 0, regnum, value);
+	pr_debug("PHY: %d register %x write %x, err %d\n", mii_id, regnum, value, err);
+	return err;
 }
 
 static int rtl838x_mdio_reset(struct mii_bus *bus)
@@ -2033,14 +2061,14 @@ static int rtl838x_mdio_init(struct rtl838x_eth_priv *priv)
 		priv->mii_bus->read = rtl930x_mdio_read;
 		priv->mii_bus->write = rtl930x_mdio_write;
 		priv->mii_bus->reset = rtl930x_mdio_reset;
-	//	priv->mii_bus->probe_capabilities = MDIOBUS_C22_C45; TODO for linux 5.9
+		priv->mii_bus->probe_capabilities = MDIOBUS_C22_C45;
 		break;
 	case RTL9310_FAMILY_ID:
 		priv->mii_bus->name = "rtl931x-eth-mdio";
 		priv->mii_bus->read = rtl931x_mdio_read;
 		priv->mii_bus->write = rtl931x_mdio_write;
 		priv->mii_bus->reset = rtl931x_mdio_reset;
-//		priv->mii_bus->probe_capabilities = MDIOBUS_C22_C45;  TODO for linux 5.9
+		priv->mii_bus->probe_capabilities = MDIOBUS_C22_C45;
 		break;
 	}
 	priv->mii_bus->priv = priv;