libmii: add a bunch of additional PHY support

(this commit will be broken up)

Signed-off-by: Russell King <rmk+kernel@armlinux.org.uk>

1 files changed, 34 insertions, 2 deletions

diff --git a/mii-diag.c b/mii-diag.c
index f3f3f2f..5426603 100644
--- a/mii-diag.c
+++ b/mii-diag.c
@@ -403,7 +403,7 @@ int do_one_xcvr(int skfd)
 	return 0;
 }
 
-int mdio_read(int skfd, int phy_id, int location)
+static int __mdio_read(int skfd, int phy_id, int location)
 {
 	u16 *data = (u16 *)(&ifr.ifr_data);
 
@@ -418,7 +418,7 @@ int mdio_read(int skfd, int phy_id, int location)
 	return data[3];
 }
 
-void mdio_write(int skfd, int phy_id, int location, int value)
+static void __mdio_write(int skfd, int phy_id, int location, int value)
 {
 	u16 *data = (u16 *)(&ifr.ifr_data);
 
@@ -432,6 +432,38 @@ void mdio_write(int skfd, int phy_id, int location, int value)
 	}
 }
 
+#define MII_MMD_CTRL 0x0d
+#define MII_MMD_DATA 0x0e
+#define MII_MMD_CTRL_NOINCR 0x4000
+
+int mdio_read(int skfd, int phy_id, int location)
+{
+	if ((phy_id & 0xc000) == 0xc000) {
+		unsigned int devad = phy_id & 31;
+		int pkgad = (phy_id >> 5) & 31;
+		__mdio_write(skfd, pkgad, MII_MMD_CTRL, devad);
+		__mdio_write(skfd, pkgad, MII_MMD_DATA, location);
+		__mdio_write(skfd, pkgad, MII_MMD_CTRL, devad | MII_MMD_CTRL_NOINCR);
+		return __mdio_read(skfd, pkgad, MII_MMD_DATA);
+	} else {
+		return __mdio_read(skfd, phy_id, location);
+	}
+}
+
+void mdio_write(int skfd, int phy_id, int location, int value)
+{
+	if ((phy_id & 0xc000) == 0xc000) {
+		unsigned int devad = phy_id & 31;
+		int pkgad = (phy_id >> 5) & 31;
+		__mdio_write(skfd, pkgad, MII_MMD_CTRL, devad);
+		__mdio_write(skfd, pkgad, MII_MMD_DATA, location);
+		__mdio_write(skfd, pkgad, MII_MMD_CTRL, devad | MII_MMD_CTRL_NOINCR);
+		__mdio_write(skfd, pkgad, MII_MMD_DATA, value);
+	} else {
+		__mdio_write(skfd, phy_id, location, value);
+	}
+}
+
 /* Parse the command line argument for advertised capabilities. */
 static int parse_advertise(const char *capabilities)
 {