+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)
+ return 0;
+
+ /* 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;
+ val |= MII_BCM54XX_AUXCTL_MISC_WREN;
+ 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);
+}
+