1 files changed, 70 insertions, 0 deletions

diff --git a/include/phy.h b/include/phy.h
index f23ca63f3b..d01435d1aa 100644
--- a/include/phy.h
+++ b/include/phy.h
@@ -101,6 +101,14 @@ struct phy_driver {
 	int (*readext)(struct phy_device *phydev, int addr, int devad, int reg);
 	int (*writeext)(struct phy_device *phydev, int addr, int devad, int reg,
 			u16 val);
+
+	/* Phy specific driver override for reading a MMD register */
+	int (*read_mmd)(struct phy_device *phydev, int devad, int reg);
+
+	/* Phy specific driver override for writing a MMD register */
+	int (*write_mmd)(struct phy_device *phydev, int devad, int reg,
+			 u16 val);
+
 	struct list_head list;
 };
 
@@ -165,6 +173,68 @@ static inline int phy_write(struct phy_device *phydev, int devad, int regnum,
 	return bus->write(bus, phydev->addr, devad, regnum, val);
 }
 
+static inline void phy_mmd_start_indirect(struct phy_device *phydev, int devad,
+					  int regnum)
+{
+	/* Write the desired MMD Devad */
+	phy_write(phydev, MDIO_DEVAD_NONE, MII_MMD_CTRL, devad);
+
+	/* Write the desired MMD register address */
+	phy_write(phydev, MDIO_DEVAD_NONE, MII_MMD_DATA, regnum);
+
+	/* Select the Function : DATA with no post increment */
+	phy_write(phydev, MDIO_DEVAD_NONE, MII_MMD_CTRL,
+		  (devad | MII_MMD_CTRL_NOINCR));
+}
+
+static inline int phy_read_mmd(struct phy_device *phydev, int devad,
+			       int regnum)
+{
+	struct phy_driver *drv = phydev->drv;
+
+	if (regnum > (u16)~0 || devad > 32)
+		return -EINVAL;
+
+	/* driver-specific access */
+	if (drv->read_mmd)
+		return drv->read_mmd(phydev, devad, regnum);
+
+	/* direct C45 / C22 access */
+	if ((drv->features & PHY_10G_FEATURES) == PHY_10G_FEATURES ||
+	    devad == MDIO_DEVAD_NONE || !devad)
+		return phy_read(phydev, devad, regnum);
+
+	/* indirect C22 access */
+	phy_mmd_start_indirect(phydev, devad, regnum);
+
+	/* Read the content of the MMD's selected register */
+	return phy_read(phydev, MDIO_DEVAD_NONE, MII_MMD_DATA);
+}
+
+static inline int phy_write_mmd(struct phy_device *phydev, int devad,
+				int regnum, u16 val)
+{
+	struct phy_driver *drv = phydev->drv;
+
+	if (regnum > (u16)~0 || devad > 32)
+		return -EINVAL;
+
+	/* driver-specific access */
+	if (drv->write_mmd)
+		return drv->write_mmd(phydev, devad, regnum, val);
+
+	/* direct C45 / C22 access */
+	if ((drv->features & PHY_10G_FEATURES) == PHY_10G_FEATURES ||
+	    devad == MDIO_DEVAD_NONE || !devad)
+		return phy_write(phydev, devad, regnum, val);
+
+	/* indirect C22 access */
+	phy_mmd_start_indirect(phydev, devad, regnum);
+
+	/* Write the data into MMD's selected register */
+	return phy_write(phydev, MDIO_DEVAD_NONE, MII_MMD_DATA, val);
+}
+
 #ifdef CONFIG_PHYLIB_10G
 extern struct phy_driver gen10g_driver;