net: phy: marvell: add Marvell specific PHY loopback

Existing genphy_loopback() is not applicable for Marvell PHY. Besides
configuring bit-6 and bit-13 in Page 0 Register 0 (Copper Control
Register), it is also required to configure same bits  in Page 2
Register 21 (MAC Specific Control Register 2) according to speed of
the loopback is operating.

Tested working on Marvell88E1510 PHY for all speeds (1000/100/10Mbps).

FIXME: Based on trial and error test, it seem 1G need to have delay between
soft reset and loopback enablement.

Fixes: 014068dcb5b1 ("net: phy: genphy_loopback: add link speed configuration")
Cc: <stable@vger.kernel.org> # 5.15.x
Signed-off-by: Mohammad Athari Bin Ismail <mohammad.athari.ismail@intel.com>
Signed-off-by: David S. Miller <davem@davemloft.net>

diff --git a/drivers/net/phy/marvell.c b/drivers/net/phy/marvell.c
index 739859c..fa71fb7 100644 (file)
--- a/drivers/net/phy/marvell.c
+++ b/drivers/net/phy/marvell.c
@@ -189,6 +189,8 @@
 #define MII_88E1510_GEN_CTRL_REG_1_MODE_RGMII_SGMII    0x4
 #define MII_88E1510_GEN_CTRL_REG_1_RESET       0x8000  /* Soft reset */
 
+#define MII_88E1510_MSCR_2             0x15
+
 #define MII_VCT5_TX_RX_MDI0_COUPLING   0x10
 #define MII_VCT5_TX_RX_MDI1_COUPLING   0x11
 #define MII_VCT5_TX_RX_MDI2_COUPLING   0x12
@@ -1932,6 +1934,58 @@ static void marvell_get_stats(struct phy_device *phydev,
                data[i] = marvell_get_stat(phydev, i);
 }
 
+static int m88e1510_loopback(struct phy_device *phydev, bool enable)
+{
+       int err;
+
+       if (enable) {
+               u16 bmcr_ctl = 0, mscr2_ctl = 0;
+
+               if (phydev->speed == SPEED_1000)
+                       bmcr_ctl = BMCR_SPEED1000;
+               else if (phydev->speed == SPEED_100)
+                       bmcr_ctl = BMCR_SPEED100;
+
+               if (phydev->duplex == DUPLEX_FULL)
+                       bmcr_ctl |= BMCR_FULLDPLX;
+
+               err = phy_write(phydev, MII_BMCR, bmcr_ctl);
+               if (err < 0)
+                       return err;
+
+               if (phydev->speed == SPEED_1000)
+                       mscr2_ctl = BMCR_SPEED1000;
+               else if (phydev->speed == SPEED_100)
+                       mscr2_ctl = BMCR_SPEED100;
+
+               err = phy_modify_paged(phydev, MII_MARVELL_MSCR_PAGE,
+                                      MII_88E1510_MSCR_2, BMCR_SPEED1000 |
+                                      BMCR_SPEED100, mscr2_ctl);
+               if (err < 0)
+                       return err;
+
+               /* Need soft reset to have speed configuration takes effect */
+               err = genphy_soft_reset(phydev);
+               if (err < 0)
+                       return err;
+
+               /* FIXME: Based on trial and error test, it seem 1G need to have
+                * delay between soft reset and loopback enablement.
+                */
+               if (phydev->speed == SPEED_1000)
+                       msleep(1000);
+
+               return phy_modify(phydev, MII_BMCR, BMCR_LOOPBACK,
+                                 BMCR_LOOPBACK);
+       } else {
+               err = phy_modify(phydev, MII_BMCR, BMCR_LOOPBACK, 0);
+               if (err < 0)
+                       return err;
+
+               return phy_config_aneg(phydev);
+       }
+}
+
 static int marvell_vct5_wait_complete(struct phy_device *phydev)
 {
        int i;
@@ -3078,7 +3132,7 @@ static struct phy_driver marvell_drivers[] = {
                .get_sset_count = marvell_get_sset_count,
                .get_strings = marvell_get_strings,
                .get_stats = marvell_get_stats,
-               .set_loopback = genphy_loopback,
+               .set_loopback = m88e1510_loopback,
                .get_tunable = m88e1011_get_tunable,
                .set_tunable = m88e1011_set_tunable,
                .cable_test_start = marvell_vct7_cable_test_start,