phy: add generic function to support ksetting support

The old ethtool api (get_setting and set_setting) has
generic phy functions phy_ethtool_sset and phy_ethtool_gset.
To supprt the new ethtool api (get_link_ksettings and
set_link_ksettings), we add generic phy function
phy_ethtool_ksettings_get and phy_ethtool_ksettings_set.

Signed-off-by: Philippe Reynes <tremyfr@gmail.com>
Signed-off-by: David S. Miller <davem@davemloft.net>

2 files changed, 85 insertions, 0 deletions

diff --git a/drivers/net/phy/phy.c b/drivers/net/phy/phy.c
index 5590b9c182c9..6f221c8c2a7f 100644
--- a/drivers/net/phy/phy.c
+++ b/drivers/net/phy/phy.c
@@ -362,6 +362,60 @@ int phy_ethtool_sset(struct phy_device *phydev, struct ethtool_cmd *cmd)
 }
 EXPORT_SYMBOL(phy_ethtool_sset);
 
+int phy_ethtool_ksettings_set(struct phy_device *phydev,
+                              const struct ethtool_link_ksettings *cmd)
+{
+        u8 autoneg = cmd->base.autoneg;
+        u8 duplex = cmd->base.duplex;
+        u32 speed = cmd->base.speed;
+        u32 advertising;
+
+        if (cmd->base.phy_address != phydev->mdio.addr)
+                return -EINVAL;
+
+        ethtool_convert_link_mode_to_legacy_u32(&advertising,
+                                                cmd->link_modes.advertising);
+
+        /* We make sure that we don't pass unsupported values in to the PHY */
+        advertising &= phydev->supported;
+
+        /* Verify the settings we care about. */
+        if (autoneg != AUTONEG_ENABLE && autoneg != AUTONEG_DISABLE)
+                return -EINVAL;
+
+        if (autoneg == AUTONEG_ENABLE && advertising == 0)
+                return -EINVAL;
+
+        if (autoneg == AUTONEG_DISABLE &&
+            ((speed != SPEED_1000 &&
+              speed != SPEED_100 &&
+              speed != SPEED_10) ||
+             (duplex != DUPLEX_HALF &&
+              duplex != DUPLEX_FULL)))
+                return -EINVAL;
+
+        phydev->autoneg = autoneg;
+
+        phydev->speed = speed;
+
+        phydev->advertising = advertising;
+
+        if (autoneg == AUTONEG_ENABLE)
+                phydev->advertising |= ADVERTISED_Autoneg;
+        else
+                phydev->advertising &= ~ADVERTISED_Autoneg;
+
+        phydev->duplex = duplex;
+
+        phydev->mdix = cmd->base.eth_tp_mdix_ctrl;
+
+        /* Restart the PHY */
+        phy_start_aneg(phydev);
+
+        return 0;
+}
+EXPORT_SYMBOL(phy_ethtool_ksettings_set);
+
 int phy_ethtool_gset(struct phy_device *phydev, struct ethtool_cmd *cmd)
 {
         cmd->supported = phydev->supported;
@@ -385,6 +439,33 @@ int phy_ethtool_gset(struct phy_device *phydev, struct ethtool_cmd *cmd)
 }
 EXPORT_SYMBOL(phy_ethtool_gset);
 
+int phy_ethtool_ksettings_get(struct phy_device *phydev,
+                              struct ethtool_link_ksettings *cmd)
+{
+        ethtool_convert_legacy_u32_to_link_mode(cmd->link_modes.supported,
+                                                phydev->supported);
+
+        ethtool_convert_legacy_u32_to_link_mode(cmd->link_modes.advertising,
+                                                phydev->advertising);
+
+        ethtool_convert_legacy_u32_to_link_mode(cmd->link_modes.lp_advertising,
+                                                phydev->lp_advertising);
+
+        cmd->base.speed = phydev->speed;
+        cmd->base.duplex = phydev->duplex;
+        if (phydev->interface == PHY_INTERFACE_MODE_MOCA)
+                cmd->base.port = PORT_BNC;
+        else
+                cmd->base.port = PORT_MII;
+
+        cmd->base.phy_address = phydev->mdio.addr;
+        cmd->base.autoneg = phydev->autoneg;
+        cmd->base.eth_tp_mdix_ctrl = phydev->mdix;
+
+        return 0;
+}
+EXPORT_SYMBOL(phy_ethtool_ksettings_get);
+
 /**
  * phy_mii_ioctl - generic PHY MII ioctl interface
  * @phydev: the phy_device struct
diff --git a/include/linux/phy.h b/include/linux/phy.h
index 2abd7918f64f..be3f83bbdc0b 100644
--- a/include/linux/phy.h
+++ b/include/linux/phy.h
@@ -805,6 +805,10 @@ void phy_start_machine(struct phy_device *phydev);
 void phy_stop_machine(struct phy_device *phydev);
 int phy_ethtool_sset(struct phy_device *phydev, struct ethtool_cmd *cmd);
 int phy_ethtool_gset(struct phy_device *phydev, struct ethtool_cmd *cmd);
+int phy_ethtool_ksettings_get(struct phy_device *phydev,
+                              struct ethtool_link_ksettings *cmd);
+int phy_ethtool_ksettings_set(struct phy_device *phydev,
+                              const struct ethtool_link_ksettings *cmd);
 int phy_mii_ioctl(struct phy_device *phydev, struct ifreq *ifr, int cmd);
 int phy_start_interrupts(struct phy_device *phydev);
 void phy_print_status(struct phy_device *phydev);