[PATCH net-next v1 2/4] net: phy: micrel: add EEE configuration support for KSZ9477 variants of PHYs

[Oleksij Rempel]

KSZ9477 variants of PHYs are not completely compatible with generic
phy_ethtool_get/set_eee() handlers. For example MDIO_PCS_EEE_ABLE acts
like a mirror of MDIO_AN_EEE_ADV register. If MDIO_AN_EEE_ADV set to 0,
MDIO_PCS_EEE_ABLE will be 0 too. It means, if we do
"ethtool --set-eee lan2 eee off", we won't be able to enable it again.

With this patch, instead of reading MDIO_PCS_EEE_ABLE register, the
driver will provide proper abilities.

Signed-off-by: Oleksij Rempel <o.rempel@xxxxxxxxxxxxxx>
---
 drivers/net/phy/micrel.c | 92 ++++++++++++++++++++++++++++++++++++++++
 1 file changed, 92 insertions(+)

diff --git a/drivers/net/phy/micrel.c b/drivers/net/phy/micrel.c
index d5b80c31ab91..099f1e83c08c 100644
--- a/drivers/net/phy/micrel.c
+++ b/drivers/net/phy/micrel.c
@@ -1370,6 +1370,96 @@ static int ksz9131_config_aneg(struct phy_device *phydev)
 	return genphy_config_aneg(phydev);
 }
 
+static int ksz9477_get_eee_caps(struct phy_device *phydev,
+				struct ethtool_eee *data)
+{
+	int val;
+
+	/* At least on KSZ8563 (which has same PHY_ID as KSZ9477), the
+	 * MDIO_PCS_EEE_ABLE register is a mirror of MDIO_AN_EEE_ADV register.
+	 * So, we need to provide this information by driver.
+	 */
+	data->supported = SUPPORTED_100baseT_Full;
+
+	/* KSZ8563 is able to advertise not supported MDIO_EEE_1000T.
+	 * We need to test if the PHY is 1Gbit capable.
+	 */
+	val = phy_read(phydev, MII_BMSR);
+	if (val < 0)
+		return val;
+
+	if (val & BMSR_ERCAP)
+		data->supported |= SUPPORTED_1000baseT_Full;
+
+	return 0;
+}
+
+static int ksz9477_get_eee(struct phy_device *phydev, struct ethtool_eee *data)
+{
+	int val;
+
+	val = ksz9477_get_eee_caps(phydev, data);
+	if (val)
+		return val;
+
+	/* Get advertisement EEE */
+	val = phy_read_mmd(phydev, MDIO_MMD_AN, MDIO_AN_EEE_ADV);
+	if (val < 0)
+		return val;
+	data->advertised = mmd_eee_adv_to_ethtool_adv_t(val);
+	data->eee_enabled = !!data->advertised;
+
+	/* Get LP advertisement EEE */
+	val = phy_read_mmd(phydev, MDIO_MMD_AN, MDIO_AN_EEE_LPABLE);
+	if (val < 0)
+		return val;
+	data->lp_advertised = mmd_eee_adv_to_ethtool_adv_t(val);
+
+	data->eee_active = !!(data->advertised & data->lp_advertised);
+
+	return 0;
+}
+
+static int ksz9477_set_eee(struct phy_device *phydev, struct ethtool_eee *data)
+{
+	int old_adv, adv = 0, ret;
+
+	ret = ksz9477_get_eee_caps(phydev, data);
+	if (ret)
+		return ret;
+
+	old_adv = phy_read_mmd(phydev, MDIO_MMD_AN, MDIO_AN_EEE_ADV);
+	if (old_adv < 0)
+		return old_adv;
+
+	if (data->eee_enabled) {
+		if (!data->advertised)
+			adv = ethtool_adv_to_mmd_eee_adv_t(data->supported);
+		else
+			adv = ethtool_adv_to_mmd_eee_adv_t(data->advertised &
+							   data->supported);
+		/* Mask prohibited EEE modes */
+		adv &= ~phydev->eee_broken_modes;
+	}
+
+	if (old_adv != adv) {
+		ret = phy_write_mmd(phydev, MDIO_MMD_AN, MDIO_AN_EEE_ADV, adv);
+		if (ret < 0)
+			return ret;
+
+		/* Restart autonegotiation so the new modes get sent to the
+		 * link partner.
+		 */
+		if (phydev->autoneg == AUTONEG_ENABLE) {
+			ret = phy_restart_aneg(phydev);
+			if (ret < 0)
+				return ret;
+		}
+	}
+
+	return 0;
+}
+
 #define KSZ8873MLL_GLOBAL_CONTROL_4	0x06
 #define KSZ8873MLL_GLOBAL_CONTROL_4_DUPLEX	BIT(6)
 #define KSZ8873MLL_GLOBAL_CONTROL_4_SPEED	BIT(4)
@@ -3422,6 +3512,8 @@ static struct phy_driver ksphy_driver[] = {
 	.handle_interrupt = kszphy_handle_interrupt,
 	.suspend	= genphy_suspend,
 	.resume		= genphy_resume,
+	.get_eee	= ksz9477_get_eee,
+	.set_eee	= ksz9477_set_eee,
 } };
 
 module_phy_driver(ksphy_driver);
-- 
2.30.2