diff --git a/drivers/net/phy/broadcom.c b/drivers/net/phy/broadcom.c
index 0472b3470c59..78542580f2b2 100644
--- a/drivers/net/phy/broadcom.c
+++ b/drivers/net/phy/broadcom.c
@@ -64,6 +64,63 @@ static int bcm54612e_config_init(struct phy_device *phydev)
 	return 0;
 }
 
+static int bcm54616s_config_init(struct phy_device *phydev)
+{
+	int rc, val;
+
+	if (phydev->interface == PHY_INTERFACE_MODE_SGMII ||
+	    phydev->interface == PHY_INTERFACE_MODE_1000BASEX) {
+		/* Ensure proper interface mode is selected. */
+		/* Disable RGMII mode */
+		val = bcm54xx_auxctl_read(phydev, MII_BCM54XX_AUXCTL_SHDWSEL_MISC);
+		if (val < 0)
+			return val;
+		val &= ~MII_BCM54XX_AUXCTL_SHDWSEL_MISC_RGMII_EN;
+		rc = bcm54xx_auxctl_write(phydev, MII_BCM54XX_AUXCTL_SHDWSEL_MISC,
+					  val);
+		if (rc < 0)
+			return rc;
+
+		/* Select 1000BASE-X register set (primary SerDes) */
+		val = bcm_phy_read_shadow(phydev, BCM54XX_SHD_MODE);
+		if (val < 0)
+			return val;
+		val |= BCM54XX_SHD_MODE_1000BX;
+		rc = bcm_phy_write_shadow(phydev, BCM54XX_SHD_MODE, val);
+		if (rc < 0)
+			return rc;
+
+		/* Power down SerDes interface */
+		rc = phy_set_bits(phydev, MII_BMCR, BMCR_PDOWN);
+		if (rc < 0)
+			return rc;
+
+		/* Select proper interface mode */
+		val &= ~BCM54XX_SHD_INTF_SEL_MASK;
+		val |= phydev->interface == PHY_INTERFACE_MODE_SGMII ?
+			BCM54XX_SHD_INTF_SEL_SGMII :
+			BCM54XX_SHD_INTF_SEL_GBIC;
+		rc = bcm_phy_write_shadow(phydev, BCM54XX_SHD_MODE, val);
+		if (rc < 0)
+			return rc;
+
+		/* Power up SerDes interface */
+		rc = phy_clear_bits(phydev, MII_BMCR, BMCR_PDOWN);
+		if (rc < 0)
+			return rc;
+
+		/* Select copper register set */
+		val &= ~BCM54XX_SHD_MODE_1000BX;
+		rc = bcm_phy_write_shadow(phydev, BCM54XX_SHD_MODE, val);
+		if (rc < 0)
+			return rc;
+
+		/* Power up copper interface */
+		return phy_clear_bits(phydev, MII_BMCR, BMCR_PDOWN);
+	}
+	return 0;
+}
+
 static int bcm54xx_config_clock_delay(struct phy_device *phydev)
 {
 	int rc, val;
@@ -283,15 +340,17 @@ static int bcm54xx_config_init(struct phy_device *phydev)
 
 	bcm54xx_adjust_rxrefclk(phydev);
 
-	if (BRCM_PHY_MODEL(phydev) == PHY_ID_BCM54210E) {
+	switch (BRCM_PHY_MODEL(phydev)) {
+	case PHY_ID_BCM54210E:
 		err = bcm54210e_config_init(phydev);
-		if (err)
-			return err;
-	} else if (BRCM_PHY_MODEL(phydev) == PHY_ID_BCM54612E) {
+		break;
+	case PHY_ID_BCM54612E:
 		err = bcm54612e_config_init(phydev);
-		if (err)
-			return err;
-	} else if (BRCM_PHY_MODEL(phydev) == PHY_ID_BCM54810) {
+		break;
+	case PHY_ID_BCM54616S:
+		err = bcm54616s_config_init(phydev);
+		break;
+	case PHY_ID_BCM54810:
 		/* For BCM54810, we need to disable BroadR-Reach function */
 		val = bcm_phy_read_exp(phydev,
 				       BCM54810_EXP_BROADREACH_LRE_MISC_CTL);
@@ -299,9 +358,10 @@ static int bcm54xx_config_init(struct phy_device *phydev)
 		err = bcm_phy_write_exp(phydev,
 					BCM54810_EXP_BROADREACH_LRE_MISC_CTL,
 					val);
-		if (err < 0)
-			return err;
+		break;
 	}
+	if (err)
+		return err;
 
 	bcm54xx_phydsp_config(phydev);
 
@@ -385,7 +445,7 @@ static int bcm5481_config_aneg(struct phy_device *phydev)
 
 static int bcm54616s_probe(struct phy_device *phydev)
 {
-	int val, intf_sel;
+	int val;
 
 	val = bcm_phy_read_shadow(phydev, BCM54XX_SHD_MODE);
 	if (val < 0)
@@ -397,8 +457,7 @@ static int bcm54616s_probe(struct phy_device *phydev)
 	 * RGMII-1000Base-X is properly supported, but RGMII-100Base-FX
 	 * support is still missing as of now.
 	 */
-	intf_sel = (val & BCM54XX_SHD_INTF_SEL_MASK) >> 1;
-	if (intf_sel == 1) {
+	if ((val & BCM54XX_SHD_INTF_SEL_MASK) == BCM54XX_SHD_INTF_SEL_RGMII) {
 		val = bcm_phy_read_shadow(phydev, BCM54616S_SHD_100FX_CTRL);
 		if (val < 0)
 			return val;
diff --git a/include/linux/brcmphy.h b/include/linux/brcmphy.h
index de9430d55c90..b0a73b91d5ba 100644
--- a/include/linux/brcmphy.h
+++ b/include/linux/brcmphy.h
@@ -137,6 +137,7 @@
 
 #define MII_BCM54XX_AUXCTL_SHDWSEL_MISC			0x07
 #define MII_BCM54XX_AUXCTL_SHDWSEL_MISC_WIRESPEED_EN	0x0010
+#define MII_BCM54XX_AUXCTL_SHDWSEL_MISC_RGMII_EN	0x0080
 #define MII_BCM54XX_AUXCTL_SHDWSEL_MISC_RGMII_SKEW_EN	0x0100
 #define MII_BCM54XX_AUXCTL_MISC_FORCE_AMDIX		0x0200
 #define MII_BCM54XX_AUXCTL_MISC_WREN			0x8000
@@ -223,6 +224,9 @@
 /* 11111: Mode Control Register */
 #define BCM54XX_SHD_MODE		0x1f
 #define BCM54XX_SHD_INTF_SEL_MASK	GENMASK(2, 1)	/* INTERF_SEL[1:0] */
+#define BCM54XX_SHD_INTF_SEL_RGMII	0x02
+#define BCM54XX_SHD_INTF_SEL_SGMII	0x04
+#define BCM54XX_SHD_INTF_SEL_GBIC	0x06
 #define BCM54XX_SHD_MODE_1000BX		BIT(0)	/* Enable 1000-X registers */
 
 /*
