[PATCH net-next 2/2] net: phy: broadcom: add 1000Base-X support for BCM54616S

[Tao Ren]

Configure the BCM54616S for 1000Base-X mode when "brcm-phy-mode-1000bx"
is set in device tree. This is needed when the PHY is used for fiber and
backplane connections.

The patch is inspired by commit cd9af3dac6d1 ("PHYLIB: Add 1000Base-X
support for Broadcom bcm5482").

Signed-off-by: Tao Ren <taoren at fb.com>
---
 drivers/net/phy/broadcom.c | 58 +++++++++++++++++++++++++++++++++++---
 include/linux/brcmphy.h    |  4 +--
 2 files changed, 56 insertions(+), 6 deletions(-)

diff --git a/drivers/net/phy/broadcom.c b/drivers/net/phy/broadcom.c
index 2b4e41a9d35a..6c22ac3a844b 100644
--- a/drivers/net/phy/broadcom.c
+++ b/drivers/net/phy/broadcom.c
@@ -383,9 +383,9 @@ static int bcm5482_config_init(struct phy_device *phydev)
 		/*
 		 * Select 1000BASE-X register set (primary SerDes)
 		 */
-		reg = bcm_phy_read_shadow(phydev, BCM5482_SHD_MODE);
-		bcm_phy_write_shadow(phydev, BCM5482_SHD_MODE,
-				     reg | BCM5482_SHD_MODE_1000BX);
+		reg = bcm_phy_read_shadow(phydev, BCM54XX_SHD_MODE);
+		bcm_phy_write_shadow(phydev, BCM54XX_SHD_MODE,
+				     reg | BCM54XX_SHD_MODE_1000BX);
 
 		/*
 		 * LED1=ACTIVITYLED, LED3=LINKSPD[2]
@@ -451,6 +451,34 @@ static int bcm5481_config_aneg(struct phy_device *phydev)
 	return ret;
 }
 
+static int bcm54616s_config_init(struct phy_device *phydev)
+{
+	int err, reg;
+	struct device_node *np = phydev->mdio.dev.of_node;
+
+	err = bcm54xx_config_init(phydev);
+
+	if (of_property_read_bool(np, "brcm-phy-mode-1000bx")) {
+		/* Select 1000BASE-X register set. */
+		reg = bcm_phy_read_shadow(phydev, BCM54XX_SHD_MODE);
+		bcm_phy_write_shadow(phydev, BCM54XX_SHD_MODE,
+				     reg | BCM54XX_SHD_MODE_1000BX);
+
+		/* Auto-negotiation doesn't seem to work quite right
+		 * in this mode, so we disable it and force it to the
+		 * right speed/duplex setting.  Only 'link status'
+		 * is important.
+		 */
+		phydev->autoneg = AUTONEG_DISABLE;
+		phydev->speed = SPEED_1000;
+		phydev->duplex = DUPLEX_FULL;
+
+		phydev->dev_flags |= PHY_BCM_FLAGS_MODE_1000BX;
+	}
+
+	return err;
+}
+
 static int bcm54616s_config_aneg(struct phy_device *phydev)
 {
 	int ret;
@@ -464,6 +492,27 @@ static int bcm54616s_config_aneg(struct phy_device *phydev)
 	return ret;
 }
 
+static int bcm54616s_read_status(struct phy_device *phydev)
+{
+	int ret;
+
+	ret = genphy_read_status(phydev);
+	if (ret < 0)
+		return ret;
+
+	if (phydev->dev_flags & PHY_BCM_FLAGS_MODE_1000BX) {
+		/* Only link status matters for 1000Base-X mode, so force
+		 * 1000 Mbit/s full-duplex status.
+		 */
+		if (phydev->link) {
+			phydev->speed = SPEED_1000;
+			phydev->duplex = DUPLEX_FULL;
+		}
+	}
+
+	return 0;
+}
+
 static int brcm_phy_setbits(struct phy_device *phydev, int reg, int set)
 {
 	int val;
@@ -651,8 +700,9 @@ static struct phy_driver broadcom_drivers[] = {
 	.phy_id_mask	= 0xfffffff0,
 	.name		= "Broadcom BCM54616S",
 	.features       = PHY_GBIT_FEATURES,
-	.config_init	= bcm54xx_config_init,
+	.config_init	= bcm54616s_config_init,
 	.config_aneg	= bcm54616s_config_aneg,
+	.read_status	= bcm54616s_read_status,
 	.ack_interrupt	= bcm_phy_ack_intr,
 	.config_intr	= bcm_phy_config_intr,
 }, {
diff --git a/include/linux/brcmphy.h b/include/linux/brcmphy.h
index 6db2d9a6e503..82030155558c 100644
--- a/include/linux/brcmphy.h
+++ b/include/linux/brcmphy.h
@@ -200,8 +200,8 @@
 #define BCM5482_SHD_SSD		0x14	/* 10100: Secondary SerDes control */
 #define BCM5482_SHD_SSD_LEDM	0x0008	/* SSD LED Mode enable */
 #define BCM5482_SHD_SSD_EN	0x0001	/* SSD enable */
-#define BCM5482_SHD_MODE	0x1f	/* 11111: Mode Control Register */
-#define BCM5482_SHD_MODE_1000BX	0x0001	/* Enable 1000BASE-X registers */
+#define BCM54XX_SHD_MODE	0x1f	/* 11111: Mode Control Register */
+#define BCM54XX_SHD_MODE_1000BX	0x0001	/* Enable 1000BASE-X registers */
 
 
 /*
-- 
2.17.1