[PATCH net-next 4/6] net: mii: Add mii_lpa_mod_linkmode_lpa_t

[Andrew Lunn <andrew@lunn.ch>]

Add a _mod_ variant of mii_lpa_to_linkmode_lpa_t. Use this to fix the
genphy_read_status() where the 1G link partner features are getting
lost.

Fixes: c0ec3c273677 ("net: phy: Convert u32 phydev->lp_advertising to linkmode")
Reported-by: Heiner Kallweit <hkallweit1@gmail.com>
Signed-off-by: Andrew Lunn <andrew@lunn.ch>
---
 drivers/net/phy/phy_device.c |  2 +-
 include/linux/mii.h          | 68 +++++++++++++++++++++++++++---------
 2 files changed, 53 insertions(+), 17 deletions(-)

diff --git a/drivers/net/phy/phy_device.c b/drivers/net/phy/phy_device.c
index c20b5ecc0f4b..7d5d698604aa 100644
--- a/drivers/net/phy/phy_device.c
+++ b/drivers/net/phy/phy_device.c
@@ -1748,7 +1748,7 @@ int genphy_read_status(struct phy_device *phydev)
 		if (lpa < 0)
 			return lpa;
 
-		mii_lpa_to_linkmode_lpa_t(phydev->lp_advertising, lpa);
+		mii_lpa_mod_linkmode_lpa_t(phydev->lp_advertising, lpa);
 
 		adv = phy_read(phydev, MII_ADVERTISE);
 		if (adv < 0)
diff --git a/include/linux/mii.h b/include/linux/mii.h
index b915ef6c3692..e72447778a08 100644
--- a/include/linux/mii.h
+++ b/include/linux/mii.h
@@ -372,6 +372,36 @@ static inline u32 mii_lpa_to_ethtool_lpa_x(u32 lpa)
 	return result | mii_adv_to_ethtool_adv_x(lpa);
 }
 
+/**
+ * mii_adv_mod_linkmode_adv_t
+ * @advertising:pointer to destination link mode.
+ * @adv: value of the MII_ADVERTISE register
+ *
+ * A small helper function that translates MII_ADVERTISE bits to
+ * linkmode advertisement settings. Leaves other bits unchanged.
+ */
+static inline void mii_adv_mod_linkmode_adv_t(unsigned long *advertising,
+					      u32 adv)
+{
+	linkmode_mod_bit(ETHTOOL_LINK_MODE_10baseT_Half_BIT,
+			 advertising, adv & ADVERTISE_10HALF);
+
+	linkmode_mod_bit(ETHTOOL_LINK_MODE_10baseT_Full_BIT,
+			 advertising, adv & ADVERTISE_10FULL);
+
+	linkmode_mod_bit(ETHTOOL_LINK_MODE_100baseT_Half_BIT,
+			 advertising, adv & ADVERTISE_100HALF);
+
+	linkmode_mod_bit(ETHTOOL_LINK_MODE_100baseT_Full_BIT,
+			 advertising, adv & ADVERTISE_100FULL);
+
+	linkmode_mod_bit(ETHTOOL_LINK_MODE_Pause_BIT, advertising,
+			 adv & ADVERTISE_PAUSE_CAP);
+
+	linkmode_mod_bit(ETHTOOL_LINK_MODE_Asym_Pause_BIT,
+			 advertising, adv & ADVERTISE_PAUSE_ASYM);
+}
+
 /**
  * mii_adv_to_linkmode_adv_t
  * @advertising:pointer to destination link mode.
@@ -386,22 +416,7 @@ static inline void mii_adv_to_linkmode_adv_t(unsigned long *advertising,
 {
 	linkmode_zero(advertising);
 
-	if (adv & ADVERTISE_10HALF)
-		linkmode_set_bit(ETHTOOL_LINK_MODE_10baseT_Half_BIT,
-				 advertising);
-	if (adv & ADVERTISE_10FULL)
-		linkmode_set_bit(ETHTOOL_LINK_MODE_10baseT_Full_BIT,
-				 advertising);
-	if (adv & ADVERTISE_100HALF)
-		linkmode_set_bit(ETHTOOL_LINK_MODE_100baseT_Half_BIT,
-				 advertising);
-	if (adv & ADVERTISE_100FULL)
-		linkmode_set_bit(ETHTOOL_LINK_MODE_100baseT_Full_BIT,
-				 advertising);
-	if (adv & ADVERTISE_PAUSE_CAP)
-		linkmode_set_bit(ETHTOOL_LINK_MODE_Pause_BIT, advertising);
-	if (adv & ADVERTISE_PAUSE_ASYM)
-		linkmode_set_bit(ETHTOOL_LINK_MODE_Asym_Pause_BIT, advertising);
+	mii_adv_mod_linkmode_adv_t(advertising, adv);
 }
 
 /**
@@ -423,6 +438,27 @@ static inline void mii_lpa_to_linkmode_lpa_t(unsigned long *lp_advertising,
 
 }
 
+/**
+ * mii_lpa_mod_linkmode_lpa_t
+ * @adv: value of the MII_LPA register
+ *
+ * A small helper function that translates MII_LPA bits, when in
+ * 1000Base-T mode, to linkmode LP advertisement settings. Leaves
+ * other bits unchanged.
+ */
+static inline void mii_lpa_mod_linkmode_lpa_t(unsigned long *lp_advertising,
+					      u32 lpa)
+{
+	mii_adv_mod_linkmode_adv_t(lp_advertising, lpa);
+
+	if (lpa & LPA_LPACK)
+		linkmode_set_bit(ETHTOOL_LINK_MODE_Autoneg_BIT,
+				 lp_advertising);
+	else
+		linkmode_clear_bit(ETHTOOL_LINK_MODE_Autoneg_BIT,
+				   lp_advertising);
+}
+
 /**
  * linkmode_adv_to_lcl_adv_t
  * @advertising:pointer to linkmode advertising
-- 
2.19.1