[RFC PATCH] net: phy/mdio: enable mmd indirect access through phy_mii_ioctl()

[Grygorii Strashko]

This patch enables access to C22 PHY MMD address space through
phy_mii_ioctl() SIOCGMIIREG/SIOCSMIIREG IOCTLs. It checks if R/W request is
received with C45 flag enabled while MDIO bus doesn't support C45 and, in
this case, tries to treat prtad as PHY MMD selector and use MMD API.

With this change it's possible to r/w PHY MMD registers with phytool, for
example, before:

  phytool read eth0/0x1f:0/0x32
  0xffea

after:
  phytool read eth0/0x1f:0/0x32
  0x00d1

This feature is very useful for various PHY issues debugging (now it's
required to modify phy code to collect MMD regs dump).

The patch is marked as RFC as it possible that I've missed something and
such feature already present in Kernel, but I just can't find it. 
It also doesn't cover phylink.

Signed-off-by: Grygorii Strashko <grygorii.strashko@xxxxxx>
---
 drivers/net/phy/phy-core.c | 32 ++++++++++++++++++++++++--------
 drivers/net/phy/phy.c      | 29 ++++++++++++++++++++++++++---
 include/linux/phy.h        |  2 ++
 3 files changed, 52 insertions(+), 11 deletions(-)

diff --git a/drivers/net/phy/phy-core.c b/drivers/net/phy/phy-core.c
index 2870c33b8975..2c83a121a5fa 100644
--- a/drivers/net/phy/phy-core.c
+++ b/drivers/net/phy/phy-core.c
@@ -457,6 +457,28 @@ static void mmd_phy_indirect(struct mii_bus *bus, int phy_addr, int devad,
 			devad | MII_MMD_CTRL_NOINCR);
 }
 
+int __mmd_phy_read(struct mii_bus *bus, int phy_addr, int devad, u32 regnum)
+{
+	int retval;
+
+	mmd_phy_indirect(bus, phy_addr, devad, regnum);
+
+	/* Read the content of the MMD's selected register */
+	retval = __mdiobus_read(bus, phy_addr, MII_MMD_DATA);
+
+	return retval;
+}
+
+int __mmd_phy_write(struct mii_bus *bus, int phy_addr, int devad, u32 regnum, u16 val)
+{
+	mmd_phy_indirect(bus, phy_addr, devad, regnum);
+
+	/* Write the data into MMD's selected register */
+	__mdiobus_write(bus, phy_addr, MII_MMD_DATA, val);
+
+	return 0;
+}
+
 /**
  * __phy_read_mmd - Convenience function for reading a register
  * from an MMD on a given PHY.
@@ -482,10 +504,7 @@ int __phy_read_mmd(struct phy_device *phydev, int devad, u32 regnum)
 		struct mii_bus *bus = phydev->mdio.bus;
 		int phy_addr = phydev->mdio.addr;
 
-		mmd_phy_indirect(bus, phy_addr, devad, regnum);
-
-		/* Read the content of the MMD's selected register */
-		val = __mdiobus_read(bus, phy_addr, MII_MMD_DATA);
+		val = __mmd_phy_read(bus, phy_addr, devad, regnum);
 	}
 	return val;
 }
@@ -538,10 +557,7 @@ int __phy_write_mmd(struct phy_device *phydev, int devad, u32 regnum, u16 val)
 		struct mii_bus *bus = phydev->mdio.bus;
 		int phy_addr = phydev->mdio.addr;
 
-		mmd_phy_indirect(bus, phy_addr, devad, regnum);
-
-		/* Write the data into MMD's selected register */
-		__mdiobus_write(bus, phy_addr, MII_MMD_DATA, val);
+		__mmd_phy_write(bus, phy_addr, devad, regnum, val);
 
 		ret = 0;
 	}
diff --git a/drivers/net/phy/phy.c b/drivers/net/phy/phy.c
index a3bfb156c83d..212ec5954b95 100644
--- a/drivers/net/phy/phy.c
+++ b/drivers/net/phy/phy.c
@@ -300,8 +300,19 @@ int phy_mii_ioctl(struct phy_device *phydev, struct ifreq *ifr, int cmd)
 			prtad = mii_data->phy_id;
 			devad = mii_data->reg_num;
 		}
-		mii_data->val_out = mdiobus_read(phydev->mdio.bus, prtad,
-						 devad);
+		if (mdio_phy_id_is_c45(mii_data->phy_id) &&
+		    phydev->mdio.bus->probe_capabilities <= MDIOBUS_C22) {
+			phy_lock_mdio_bus(phydev);
+
+			mii_data->val_out = __mmd_phy_read(phydev->mdio.bus,
+							   mdio_phy_id_devad(mii_data->phy_id),
+							   prtad,
+							   mii_data->reg_num);
+
+			phy_unlock_mdio_bus(phydev);
+		} else {
+			mii_data->val_out = mdiobus_read(phydev->mdio.bus, prtad, devad);
+		}
 		return 0;
 
 	case SIOCSMIIREG:
@@ -351,7 +362,19 @@ int phy_mii_ioctl(struct phy_device *phydev, struct ifreq *ifr, int cmd)
 			}
 		}
 
-		mdiobus_write(phydev->mdio.bus, prtad, devad, val);
+		if (mdio_phy_id_is_c45(mii_data->phy_id) &&
+		    phydev->mdio.bus->probe_capabilities <= MDIOBUS_C22) {
+			phy_lock_mdio_bus(phydev);
+
+			__mmd_phy_write(phydev->mdio.bus, mdio_phy_id_devad(mii_data->phy_id),
+					prtad,
+					mii_data->reg_num,
+					val);
+
+			phy_unlock_mdio_bus(phydev);
+		} else {
+			mdiobus_write(phydev->mdio.bus, prtad, devad, val);
+		}
 
 		if (prtad == phydev->mdio.addr &&
 		    devad == MII_BMCR &&
diff --git a/include/linux/phy.h b/include/linux/phy.h
index 96e43fbb2dd8..f6032c1708e6 100644
--- a/include/linux/phy.h
+++ b/include/linux/phy.h
@@ -1114,12 +1114,14 @@ int phy_read_mmd(struct phy_device *phydev, int devad, u32 regnum);
  * from an MMD on a given PHY.
  */
 int __phy_read_mmd(struct phy_device *phydev, int devad, u32 regnum);
+int __mmd_phy_read(struct mii_bus *bus, int phy_addr, int devad, u32 regnum);
 
 /*
  * phy_write_mmd - Convenience function for writing a register
  * on an MMD on a given PHY.
  */
 int phy_write_mmd(struct phy_device *phydev, int devad, u32 regnum, u16 val);
+int __mmd_phy_write(struct mii_bus *bus, int phy_addr, int devad, u32 regnum, u16 val);
 
 /*
  * __phy_write_mmd - Convenience function for writing a register
-- 
2.17.1