net: phy: marvell: add downshift support for M88E1111

This patch adds downshift support for M88E1111. This PHY version uses
another register for downshift configuration, reading downshift status
is possible via the same register as for other PHY versions.

Signed-off-by: Heiner Kallweit <hkallweit1@gmail.com>
Reviewed-by: Florian Fainelli <f.fainelli@gmail.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 68ef84c..aa4864c 100644 (file)
--- a/drivers/net/phy/marvell.c
+++ b/drivers/net/phy/marvell.c
@@ -66,6 +66,9 @@
 #define MII_M1111_PHY_LED_DIRECT       0x4100
 #define MII_M1111_PHY_LED_COMBINE      0x411c
 #define MII_M1111_PHY_EXT_CR           0x14
+#define MII_M1111_PHY_EXT_CR_DOWNSHIFT_MASK    GENMASK(11, 9)
+#define MII_M1111_PHY_EXT_CR_DOWNSHIFT_MAX     8
+#define MII_M1111_PHY_EXT_CR_DOWNSHIFT_EN      BIT(8)
 #define MII_M1111_RGMII_RX_DELAY       BIT(7)
 #define MII_M1111_RGMII_TX_DELAY       BIT(1)
 #define MII_M1111_PHY_EXT_SR           0x1b
@@ -784,6 +787,64 @@ static int m88e1111_config_init(struct phy_device *phydev)
        return genphy_soft_reset(phydev);
 }
 
+static int m88e1111_get_downshift(struct phy_device *phydev, u8 *data)
+{
+       int val, cnt, enable;
+
+       val = phy_read(phydev, MII_M1111_PHY_EXT_CR);
+       if (val < 0)
+               return val;
+
+       enable = FIELD_GET(MII_M1111_PHY_EXT_CR_DOWNSHIFT_EN, val);
+       cnt = FIELD_GET(MII_M1111_PHY_EXT_CR_DOWNSHIFT_MASK, val) + 1;
+
+       *data = enable ? cnt : DOWNSHIFT_DEV_DISABLE;
+
+       return 0;
+}
+
+static int m88e1111_set_downshift(struct phy_device *phydev, u8 cnt)
+{
+       int val;
+
+       if (cnt > MII_M1111_PHY_EXT_CR_DOWNSHIFT_MAX)
+               return -E2BIG;
+
+       if (!cnt)
+               return phy_clear_bits(phydev, MII_M1111_PHY_EXT_CR,
+                                     MII_M1111_PHY_EXT_CR_DOWNSHIFT_EN);
+
+       val = MII_M1111_PHY_EXT_CR_DOWNSHIFT_EN;
+       val |= FIELD_PREP(MII_M1111_PHY_EXT_CR_DOWNSHIFT_MASK, cnt - 1);
+
+       return phy_modify(phydev, MII_M1111_PHY_EXT_CR,
+                         MII_M1111_PHY_EXT_CR_DOWNSHIFT_EN |
+                         MII_M1111_PHY_EXT_CR_DOWNSHIFT_MASK,
+                         val);
+}
+
+static int m88e1111_get_tunable(struct phy_device *phydev,
+                               struct ethtool_tunable *tuna, void *data)
+{
+       switch (tuna->id) {
+       case ETHTOOL_PHY_DOWNSHIFT:
+               return m88e1111_get_downshift(phydev, data);
+       default:
+               return -EOPNOTSUPP;
+       }
+}
+
+static int m88e1111_set_tunable(struct phy_device *phydev,
+                               struct ethtool_tunable *tuna, const void *data)
+{
+       switch (tuna->id) {
+       case ETHTOOL_PHY_DOWNSHIFT:
+               return m88e1111_set_downshift(phydev, *(const u8 *)data);
+       default:
+               return -EOPNOTSUPP;
+       }
+}
+
 static int m88e1011_get_downshift(struct phy_device *phydev, u8 *data)
 {
        int val, cnt, enable;
@@ -2245,6 +2306,9 @@ static struct phy_driver marvell_drivers[] = {
                .get_sset_count = marvell_get_sset_count,
                .get_strings = marvell_get_strings,
                .get_stats = marvell_get_stats,
+               .get_tunable = m88e1111_get_tunable,
+               .set_tunable = m88e1111_set_tunable,
+               .link_change_notify = m88e1011_link_change_notify,
        },
        {
                .phy_id = MARVELL_PHY_ID_88E1118,