[PATCH net 2/6 RESEND] net: dsa: b53: manage SerDes power through the PCS callbacks

@@ -1630,10 +1630,6 @@ static void b53_phylink_mac_link_down(struct phylink_config *config,
 		b53_force_link(dev, port, false);
 		return;
 	}
-
-	if (phy_interface_mode_is_8023z(interface) &&
-	    dev->ops->serdes_link_set)
-		dev->ops->serdes_link_set(dev, port, mode, interface, false);
 }
 
 static void b53_phylink_mac_link_up(struct phylink_config *config,

@@ -1672,10 +1668,6 @@ static void b53_phylink_mac_link_up(struct phylink_config *config,
 		b53_force_link(dev, port, true);
 		return;
 	}
-
-	if (phy_interface_mode_is_8023z(interface) &&
-	    dev->ops->serdes_link_set)
-		dev->ops->serdes_link_set(dev, port, mode, interface, true);
 }
 
 int b53_vlan_filtering(struct dsa_switch *ds, int port, bool vlan_filtering,

@@ -54,9 +54,6 @@ struct b53_io_ops {
 						      int port,
 						      phy_interface_t interface);
 	u8 (*serdes_map_lane)(struct b53_device *dev, int port);
-	void (*serdes_link_set)(struct b53_device *dev, int port,
-				unsigned int mode, phy_interface_t interface,
-				bool link_up);
 };
 
 struct b53_arl_entry;

@@ -107,6 +107,39 @@ static int b53_serdes_read(struct b53_device *dev, u8 lane,
 	return ret;
 }
 
+/* Return one if the register changed, zero if unchanged, or an error. */
+static int b53_serdes_modify(struct b53_device *dev, u8 lane, u8 offset,
+			     u16 block, u16 mask, u16 set)
+{
+	int ret, old;
+	u16 new;
+
+	mutex_lock(&dev->reg_mutex);
+	ret = b53_serdes_set_lane(dev, lane);
+	if (ret)
+		goto out;
+
+	old = b53_serdes_read_blk(dev, offset, block);
+	if (old < 0) {
+		ret = old;
+		goto out;
+	}
+
+	new = (old & ~mask) | set;
+	if (new == old)
+		goto out;
+
+	ret = b53_serdes_write_blk(dev, offset, block, new);
+	if (!ret)
+		ret = 1;
+out:
+	if (ret < 0)
+		dev->serdes_lane = B53_INVALID_LANE;
+	mutex_unlock(&dev->reg_mutex);
+
+	return ret;
+}
+
 static void b53_serdes_error(struct b53_device *dev, u8 lane, int ret)
 {
 	if (ret < 0)

@@ -202,32 +235,31 @@ static void b53_serdes_get_state(struct phylink_pcs *pcs, unsigned int neg_mode,
 		state->pause |= MLO_PAUSE_TX;
 }
 
-void b53_serdes_link_set(struct b53_device *dev, int port, unsigned int mode,
-			 phy_interface_t interface, bool link_up)
+static int b53_serdes_enable(struct phylink_pcs *pcs)
 {
-	u8 lane = b53_serdes_map_lane(dev, port);
-	int reg;
+	struct b53_pcs *priv = pcs_to_b53_pcs(pcs);
+	int ret;
 
-	if (lane == B53_INVALID_LANE)
-		return;
+	ret = b53_serdes_modify(priv->dev, priv->lane,
+				B53_SERDES_MII_REG(MII_BMCR), SERDES_MII_BLK,
+				BMCR_PDOWN, 0);
+	return ret < 0 ? ret : 0;
+}
 
-	reg = b53_serdes_read(dev, lane, B53_SERDES_MII_REG(MII_BMCR),
-			      SERDES_MII_BLK);
-	if (reg < 0)
-		goto out;
+static void b53_serdes_disable(struct phylink_pcs *pcs)
+{
+	struct b53_pcs *priv = pcs_to_b53_pcs(pcs);
+	int ret;
 
-	if (link_up)
-		reg &= ~BMCR_PDOWN;
-	else
-		reg |= BMCR_PDOWN;
-	reg = b53_serdes_write(dev, lane, B53_SERDES_MII_REG(MII_BMCR),
-			       SERDES_MII_BLK, reg);
-out:
-	b53_serdes_error(dev, lane, reg);
+	ret = b53_serdes_modify(priv->dev, priv->lane,
+				B53_SERDES_MII_REG(MII_BMCR), SERDES_MII_BLK,
+				BMCR_PDOWN, BMCR_PDOWN);
+	b53_serdes_error(priv->dev, priv->lane, ret);
 }
-EXPORT_SYMBOL(b53_serdes_link_set);
 
 static const struct phylink_pcs_ops b53_pcs_ops = {
+	.pcs_enable = b53_serdes_enable,
+	.pcs_disable = b53_serdes_disable,
 	.pcs_get_state = b53_serdes_get_state,
 	.pcs_config = b53_serdes_config,
 	.pcs_an_restart = b53_serdes_an_restart,

@@ -107,8 +107,6 @@ static inline u8 b53_serdes_map_lane(struct b53_device *dev, int port)
 	return dev->ops->serdes_map_lane(dev, port);
 }
 
-void b53_serdes_link_set(struct b53_device *dev, int port, unsigned int mode,
-			 phy_interface_t interface, bool link_up);
 struct phylink_pcs *b53_serdes_phylink_mac_select_pcs(struct b53_device *dev,
 						      int port,
 						      phy_interface_t interface);

@@ -493,7 +493,6 @@ static const struct b53_io_ops b53_srab_ops = {
 #if IS_ENABLED(CONFIG_B53_SERDES)
 	.phylink_mac_select_pcs = b53_serdes_phylink_mac_select_pcs,
 	.serdes_map_lane = b53_srab_serdes_map_lane,
-	.serdes_link_set = b53_serdes_link_set,
 #endif
 };