net: dsa: mv88e6xxx: extend phylink to Serdes PHYs

Extend the mv88e6xxx phylink implementation down to Serdes PHYs, which
handle the PCS layer of such links.

- Implement phylink PCS link state reading, so that we can provide
  ethtool with the linkmodes and link speed in the expected manner.
  Note: this will only be called for in-band negotiation, which is
  only supported by the serdes interfaces.
- Implement phylink PCS configuration, so that the in-band AN and
  advertisement can be configured.
- Implement phylink PCS negotiation restart, so that the in-band AN
  can be restarted.
- Implement phylink PCS link up, so that when operating out-of-band,
  the Serdes can be configured for the appropriate fixed speed mode.
Signed-off-by: Russell King <rmk+kernel@armlinux.org.uk>
Reviewed-by: Andrew Lunn <andrew@lunn.ch>
Signed-off-by: David S. Miller <davem@davemloft.net>

drivers/net/dsa/mv88e6xxx/chip.h

@@ -502,6 +502,17 @@ struct mv88e6xxx_ops {
 	/* SERDES lane mapping */
 	u8 (*serdes_get_lane)(struct mv88e6xxx_chip *chip, int port);
 
+	int (*serdes_pcs_get_state)(struct mv88e6xxx_chip *chip, int port,
+				    u8 lane, struct phylink_link_state *state);
+	int (*serdes_pcs_config)(struct mv88e6xxx_chip *chip, int port,
+				 u8 lane, unsigned int mode,
+				 phy_interface_t interface,
+				 const unsigned long *advertise);
+	int (*serdes_pcs_an_restart)(struct mv88e6xxx_chip *chip, int port,
+				     u8 lane);
+	int (*serdes_pcs_link_up)(struct mv88e6xxx_chip *chip, int port,
+				  u8 lane, int speed, int duplex);
+
 	/* SERDES interrupt handling */
 	unsigned int (*serdes_irq_mapping)(struct mv88e6xxx_chip *chip,
 					   int port);
@@ -669,9 +680,6 @@ int mv88e6xxx_wait_mask(struct mv88e6xxx_chip *chip, int addr, int reg,
 			u16 mask, u16 val);
 int mv88e6xxx_wait_bit(struct mv88e6xxx_chip *chip, int addr, int reg,
 		       int bit, int val);
-int mv88e6xxx_port_setup_mac(struct mv88e6xxx_chip *chip, int port, int link,
-			     int speed, int duplex, int pause,
-			     phy_interface_t mode);
 struct mii_bus *mv88e6xxx_default_mdio_bus(struct mv88e6xxx_chip *chip);
 
 static inline void mv88e6xxx_reg_lock(struct mv88e6xxx_chip *chip)

drivers/net/dsa/mv88e6xxx/serdes.c

@@ -49,6 +49,52 @@ static int mv88e6390_serdes_write(struct mv88e6xxx_chip *chip,
 	return mv88e6xxx_phy_write(chip, lane, reg_c45, val);
 }
 
+static int mv88e6xxx_serdes_pcs_get_state(struct mv88e6xxx_chip *chip,
+					  u16 status, u16 lpa,
+					  struct phylink_link_state *state)
+{
+	if (status & MV88E6390_SGMII_PHY_STATUS_SPD_DPL_VALID) {
+		state->link = !!(status & MV88E6390_SGMII_PHY_STATUS_LINK);
+		state->duplex = status &
+				MV88E6390_SGMII_PHY_STATUS_DUPLEX_FULL ?
+			                         DUPLEX_FULL : DUPLEX_HALF;
+
+		if (status & MV88E6390_SGMII_PHY_STATUS_TX_PAUSE)
+			state->pause |= MLO_PAUSE_TX;
+		if (status & MV88E6390_SGMII_PHY_STATUS_RX_PAUSE)
+			state->pause |= MLO_PAUSE_RX;
+
+		switch (status & MV88E6390_SGMII_PHY_STATUS_SPEED_MASK) {
+		case MV88E6390_SGMII_PHY_STATUS_SPEED_1000:
+			if (state->interface == PHY_INTERFACE_MODE_2500BASEX)
+				state->speed = SPEED_2500;
+			else
+				state->speed = SPEED_1000;
+			break;
+		case MV88E6390_SGMII_PHY_STATUS_SPEED_100:
+			state->speed = SPEED_100;
+			break;
+		case MV88E6390_SGMII_PHY_STATUS_SPEED_10:
+			state->speed = SPEED_10;
+			break;
+		default:
+			dev_err(chip->dev, "invalid PHY speed\n");
+			return -EINVAL;
+		}
+	} else {
+		state->link = false;
+	}
+
+	if (state->interface == PHY_INTERFACE_MODE_2500BASEX)
+		mii_lpa_mod_linkmode_x(state->lp_advertising, lpa,
+				       ETHTOOL_LINK_MODE_2500baseX_Full_BIT);
+	else if (state->interface == PHY_INTERFACE_MODE_1000BASEX)
+		mii_lpa_mod_linkmode_x(state->lp_advertising, lpa,
+				       ETHTOOL_LINK_MODE_1000baseX_Full_BIT);
+
+	return 0;
+}
+
 int mv88e6352_serdes_power(struct mv88e6xxx_chip *chip, int port, u8 lane,
 			   bool up)
 {
@@ -70,6 +116,120 @@ int mv88e6352_serdes_power(struct mv88e6xxx_chip *chip, int port, u8 lane,
 	return err;
 }
 
+int mv88e6352_serdes_pcs_config(struct mv88e6xxx_chip *chip, int port,
+				u8 lane, unsigned int mode,
+				phy_interface_t interface,
+				const unsigned long *advertise)
+{
+	u16 adv, bmcr, val;
+	bool changed;
+	int err;
+
+	switch (interface) {
+	case PHY_INTERFACE_MODE_SGMII:
+		adv = 0x0001;
+		break;
+
+	case PHY_INTERFACE_MODE_1000BASEX:
+		adv = linkmode_adv_to_mii_adv_x(advertise,
+					ETHTOOL_LINK_MODE_1000baseX_Full_BIT);
+		break;
+
+	default:
+		return 0;
+	}
+
+	err = mv88e6352_serdes_read(chip, MII_ADVERTISE, &val);
+	if (err)
+		return err;
+
+	changed = val != adv;
+	if (changed) {
+		err = mv88e6352_serdes_write(chip, MII_ADVERTISE, adv);
+		if (err)
+			return err;
+	}
+
+	err = mv88e6352_serdes_read(chip, MII_BMCR, &val);
+	if (err)
+		return err;
+
+	if (phylink_autoneg_inband(mode))
+		bmcr = val | BMCR_ANENABLE;
+	else
+		bmcr = val & ~BMCR_ANENABLE;
+
+	if (bmcr == val)
+		return changed;
+
+	return mv88e6352_serdes_write(chip, MII_BMCR, bmcr);
+}
+
+int mv88e6352_serdes_pcs_get_state(struct mv88e6xxx_chip *chip, int port,
+				   u8 lane, struct phylink_link_state *state)
+{
+	u16 lpa, status;
+	int err;
+
+	err = mv88e6352_serdes_read(chip, 0x11, &status);
+	if (err) {
+		dev_err(chip->dev, "can't read Serdes PHY status: %d\n", err);
+		return err;
+	}
+
+	err = mv88e6352_serdes_read(chip, MII_LPA, &lpa);
+	if (err) {
+		dev_err(chip->dev, "can't read Serdes PHY LPA: %d\n", err);
+		return err;
+	}
+
+	return mv88e6xxx_serdes_pcs_get_state(chip, status, lpa, state);
+}
+
+int mv88e6352_serdes_pcs_an_restart(struct mv88e6xxx_chip *chip, int port,
+				    u8 lane)
+{
+	u16 bmcr;
+	int err;
+
+	err = mv88e6352_serdes_read(chip, MII_BMCR, &bmcr);
+	if (err)
+		return err;
+
+	return mv88e6352_serdes_write(chip, MII_BMCR, bmcr | BMCR_ANRESTART);
+}
+
+int mv88e6352_serdes_pcs_link_up(struct mv88e6xxx_chip *chip, int port,
+				 u8 lane, int speed, int duplex)
+{
+	u16 val, bmcr;
+	int err;
+
+	err = mv88e6352_serdes_read(chip, MII_BMCR, &val);
+	if (err)
+		return err;
+
+	bmcr = val & ~(BMCR_SPEED100 | BMCR_FULLDPLX | BMCR_SPEED1000);
+	switch (speed) {
+	case SPEED_1000:
+		bmcr |= BMCR_SPEED1000;
+		break;
+	case SPEED_100:
+		bmcr |= BMCR_SPEED100;
+		break;
+	case SPEED_10:
+		break;
+	}
+
+	if (duplex == DUPLEX_FULL)
+		bmcr |= BMCR_FULLDPLX;
+
+	if (bmcr == val)
+		return 0;
+
+	return mv88e6352_serdes_write(chip, MII_BMCR, bmcr);
+}
+
 u8 mv88e6352_serdes_get_lane(struct mv88e6xxx_chip *chip, int port)
 {
 	u8 cmode = chip->ports[port].cmode;
@@ -538,71 +698,153 @@ int mv88e6390_serdes_power(struct mv88e6xxx_chip *chip, int port, u8 lane,
 	return err;
 }
 
-static void mv88e6390_serdes_irq_link_sgmii(struct mv88e6xxx_chip *chip,
-					    int port, u8 lane)
+int mv88e6390_serdes_pcs_config(struct mv88e6xxx_chip *chip, int port,
+				u8 lane, unsigned int mode,
+				phy_interface_t interface,
+				const unsigned long *advertise)
 {
-	u8 cmode = chip->ports[port].cmode;
-	struct dsa_switch *ds = chip->ds;
-	int duplex = DUPLEX_UNKNOWN;
-	int speed = SPEED_UNKNOWN;
-	phy_interface_t mode;
-	int link, err;
-	u16 status;
+	u16 val, bmcr, adv;
+	bool changed;
+	int err;
+
+	switch (interface) {
+	case PHY_INTERFACE_MODE_SGMII:
+		adv = 0x0001;
+		break;
+
+	case PHY_INTERFACE_MODE_1000BASEX:
+		adv = linkmode_adv_to_mii_adv_x(advertise,
+					ETHTOOL_LINK_MODE_1000baseX_Full_BIT);
+		break;
+
+	case PHY_INTERFACE_MODE_2500BASEX:
+		adv = linkmode_adv_to_mii_adv_x(advertise,
+					ETHTOOL_LINK_MODE_2500baseX_Full_BIT);
+		break;
+
+	default:
+		return 0;
+	}
+
+	err = mv88e6390_serdes_read(chip, lane, MDIO_MMD_PHYXS,
+				    MV88E6390_SGMII_ADVERTISE, &val);
+	if (err)
+		return err;
+
+	changed = val != adv;
+	if (changed) {
+		err = mv88e6390_serdes_write(chip, lane, MDIO_MMD_PHYXS,
+					     MV88E6390_SGMII_ADVERTISE, adv);
+		if (err)
+			return err;
+	}
+
+	err = mv88e6390_serdes_read(chip, lane, MDIO_MMD_PHYXS,
+				    MV88E6390_SGMII_BMCR, &val);
+	if (err)
+		return err;
+
+	if (phylink_autoneg_inband(mode))
+		bmcr = val | BMCR_ANENABLE;
+	else
+		bmcr = val & ~BMCR_ANENABLE;
+
+	/* setting ANENABLE triggers a restart of negotiation */
+	if (bmcr == val)
+		return changed;
+
+	return mv88e6390_serdes_write(chip, lane, MDIO_MMD_PHYXS,
+				      MV88E6390_SGMII_BMCR, bmcr);
+}
+
+int mv88e6390_serdes_pcs_get_state(struct mv88e6xxx_chip *chip, int port,
+				   u8 lane, struct phylink_link_state *state)
+{
+	u16 lpa, status;
+	int err;
 
 	err = mv88e6390_serdes_read(chip, lane, MDIO_MMD_PHYXS,
 				    MV88E6390_SGMII_PHY_STATUS, &status);
 	if (err) {
-		dev_err(chip->dev, "can't read SGMII PHY status: %d\n", err);
-		return;
+		dev_err(chip->dev, "can't read Serdes PHY status: %d\n", err);
+		return err;
 	}
 
-	link = status & MV88E6390_SGMII_PHY_STATUS_LINK ?
-	       LINK_FORCED_UP : LINK_FORCED_DOWN;
+	err = mv88e6390_serdes_read(chip, lane, MDIO_MMD_PHYXS,
+				    MV88E6390_SGMII_LPA, &lpa);
+	if (err) {
+		dev_err(chip->dev, "can't read Serdes PHY LPA: %d\n", err);
+		return err;
+	}
 
-	if (status & MV88E6390_SGMII_PHY_STATUS_SPD_DPL_VALID) {
-		duplex = status & MV88E6390_SGMII_PHY_STATUS_DUPLEX_FULL ?
-			 DUPLEX_FULL : DUPLEX_HALF;
+	return mv88e6xxx_serdes_pcs_get_state(chip, status, lpa, state);
+}
 
-		switch (status & MV88E6390_SGMII_PHY_STATUS_SPEED_MASK) {
-		case MV88E6390_SGMII_PHY_STATUS_SPEED_1000:
-			if (cmode == MV88E6XXX_PORT_STS_CMODE_2500BASEX)
-				speed = SPEED_2500;
-			else
-				speed = SPEED_1000;
-			break;
-		case MV88E6390_SGMII_PHY_STATUS_SPEED_100:
-			speed = SPEED_100;
-			break;
-		case MV88E6390_SGMII_PHY_STATUS_SPEED_10:
-			speed = SPEED_10;
-			break;
-		default:
-			dev_err(chip->dev, "invalid PHY speed\n");
-			return;
-		}
-	}
+int mv88e6390_serdes_pcs_an_restart(struct mv88e6xxx_chip *chip, int port,
+				    u8 lane)
+{
+	u16 bmcr;
+	int err;
 
-	switch (cmode) {
-	case MV88E6XXX_PORT_STS_CMODE_SGMII:
-		mode = PHY_INTERFACE_MODE_SGMII;
+	err = mv88e6390_serdes_read(chip, lane, MDIO_MMD_PHYXS,
+				    MV88E6390_SGMII_BMCR, &bmcr);
+	if (err)
+		return err;
+
+	return mv88e6390_serdes_write(chip, lane, MDIO_MMD_PHYXS,
+				      MV88E6390_SGMII_BMCR,
+				      bmcr | BMCR_ANRESTART);
+}
+
+int mv88e6390_serdes_pcs_link_up(struct mv88e6xxx_chip *chip, int port,
+				 u8 lane, int speed, int duplex)
+{
+	u16 val, bmcr;
+	int err;
+
+	err = mv88e6390_serdes_read(chip, lane, MDIO_MMD_PHYXS,
+				    MV88E6390_SGMII_BMCR, &val);
+	if (err)
+		return err;
+
+	bmcr = val & ~(BMCR_SPEED100 | BMCR_FULLDPLX | BMCR_SPEED1000);
+	switch (speed) {
+	case SPEED_2500:
+	case SPEED_1000:
+		bmcr |= BMCR_SPEED1000;
 		break;
-	case MV88E6XXX_PORT_STS_CMODE_1000BASEX:
-		mode = PHY_INTERFACE_MODE_1000BASEX;
+	case SPEED_100:
+		bmcr |= BMCR_SPEED100;
 		break;
-	case MV88E6XXX_PORT_STS_CMODE_2500BASEX:
-		mode = PHY_INTERFACE_MODE_2500BASEX;
+	case SPEED_10:
 		break;
-	default:
-		mode = PHY_INTERFACE_MODE_NA;
 	}
 
-	err = mv88e6xxx_port_setup_mac(chip, port, link, speed, duplex,
-				       PAUSE_OFF, mode);
-	if (err)
-		dev_err(chip->dev, "can't propagate PHY settings to MAC: %d\n",
-			err);
-	else
-		dsa_port_phylink_mac_change(ds, port, link == LINK_FORCED_UP);
+	if (duplex == DUPLEX_FULL)
+		bmcr |= BMCR_FULLDPLX;
+
+	if (bmcr == val)
+		return 0;
+
+	return mv88e6390_serdes_write(chip, lane, MDIO_MMD_PHYXS,
+				      MV88E6390_SGMII_BMCR, bmcr);
+}
+
+static void mv88e6390_serdes_irq_link_sgmii(struct mv88e6xxx_chip *chip,
+					    int port, u8 lane)
+{
+	u16 status;
+	int err;
+
+	err = mv88e6390_serdes_read(chip, lane, MDIO_MMD_PHYXS,
+				    MV88E6390_SGMII_PHY_STATUS, &status);
+	if (err) {
+		dev_err(chip->dev, "can't read SGMII PHY status: %d\n", err);
+		return;
+	}
+
+	dsa_port_phylink_mac_change(chip->ds, port,
+				!!(status & MV88E6390_SGMII_PHY_STATUS_LINK));
 }
 
 static int mv88e6390_serdes_irq_enable_sgmii(struct mv88e6xxx_chip *chip,

drivers/net/dsa/mv88e6xxx/serdes.h

@@ -48,6 +48,8 @@
 
 /* 1000BASE-X and SGMII */
 #define MV88E6390_SGMII_BMCR		(0x2000 + MII_BMCR)
+#define MV88E6390_SGMII_ADVERTISE	(0x2000 + MII_ADVERTISE)
+#define MV88E6390_SGMII_LPA		(0x2000 + MII_LPA)
 #define MV88E6390_SGMII_INT_ENABLE	0xa001
 #define MV88E6390_SGMII_INT_SPEED_CHANGE	BIT(14)
 #define MV88E6390_SGMII_INT_DUPLEX_CHANGE	BIT(13)
@@ -66,6 +68,8 @@
 #define MV88E6390_SGMII_PHY_STATUS_DUPLEX_FULL	BIT(13)
 #define MV88E6390_SGMII_PHY_STATUS_SPD_DPL_VALID BIT(11)
 #define MV88E6390_SGMII_PHY_STATUS_LINK		BIT(10)
+#define MV88E6390_SGMII_PHY_STATUS_TX_PAUSE	BIT(3)
+#define MV88E6390_SGMII_PHY_STATUS_RX_PAUSE	BIT(2)
 
 /* Packet generator pad packet checker */
 #define MV88E6390_PG_CONTROL		0xf010
@@ -75,6 +79,26 @@ u8 mv88e6341_serdes_get_lane(struct mv88e6xxx_chip *chip, int port);
 u8 mv88e6352_serdes_get_lane(struct mv88e6xxx_chip *chip, int port);
 u8 mv88e6390_serdes_get_lane(struct mv88e6xxx_chip *chip, int port);
 u8 mv88e6390x_serdes_get_lane(struct mv88e6xxx_chip *chip, int port);
+int mv88e6352_serdes_pcs_config(struct mv88e6xxx_chip *chip, int port,
+				u8 lane, unsigned int mode,
+				phy_interface_t interface,
+				const unsigned long *advertise);
+int mv88e6390_serdes_pcs_config(struct mv88e6xxx_chip *chip, int port,
+				u8 lane, unsigned int mode,
+				phy_interface_t interface,
+				const unsigned long *advertise);
+int mv88e6352_serdes_pcs_get_state(struct mv88e6xxx_chip *chip, int port,
+				   u8 lane, struct phylink_link_state *state);
+int mv88e6390_serdes_pcs_get_state(struct mv88e6xxx_chip *chip, int port,
+				   u8 lane, struct phylink_link_state *state);
+int mv88e6352_serdes_pcs_an_restart(struct mv88e6xxx_chip *chip, int port,
+				    u8 lane);
+int mv88e6390_serdes_pcs_an_restart(struct mv88e6xxx_chip *chip, int port,
+				    u8 lane);
+int mv88e6352_serdes_pcs_link_up(struct mv88e6xxx_chip *chip, int port,
+				 u8 lane, int speed, int duplex);
+int mv88e6390_serdes_pcs_link_up(struct mv88e6xxx_chip *chip, int port,
+				 u8 lane, int speed, int duplex);
 unsigned int mv88e6352_serdes_irq_mapping(struct mv88e6xxx_chip *chip,
 					  int port);
 unsigned int mv88e6390_serdes_irq_mapping(struct mv88e6xxx_chip *chip,