drivers: ethernet: vsc8541: Implement cfg_link function

The cfg_link function was not implemented in this driver, which could
cause the MAC driver to report errors. Add an implementation based on
the phy_mii driver.

Signed-off-by: Robert Hancock <robert.hancock@calian.com>

Author: robhancocksed

drivers/ethernet/phy/phy_microchip_vsc8541.c

@@ -405,13 +405,80 @@ static int phy_mc_vsc8541_get_link(const struct device *dev, struct phy_link_sta
 }
 
 /**
- * @brief Reconfigure the link speed; currently unused
+ * @brief Reconfigure the link speed
  *
+ * @param dev device structure to phy
+ * @param adv_speeds speeds to be advertised
  */
-static int phy_mc_vsc8541_cfg_link(const struct device *dev, enum phy_link_speed speeds)
+static int phy_mc_vsc8541_cfg_link(const struct device *dev, enum phy_link_speed adv_speeds)
 {
-	/* the initial version does not support reconfiguration */
-	return -ENOTSUP;
+	uint16_t anar_reg;
+	uint16_t bmcr_reg;
+	uint16_t c1kt_reg;
+
+	if (phy_mc_vsc8541_read(dev, PHY_REG_PAGE0_ADV, &anar_reg) < 0) {
+		return -EIO;
+	}
+
+	if (phy_mc_vsc8541_read(dev, PHY_REG_PAGE0_BMCR, &bmcr_reg) < 0) {
+		return -EIO;
+	}
+
+	if (phy_mc_vsc8541_read(dev, PHY_REG_PAGE0_CTRL1000, &c1kt_reg) < 0) {
+		return -EIO;
+	}
+
+	if (adv_speeds & LINK_FULL_10BASE) {
+		anar_reg |= MII_ADVERTISE_10_FULL;
+	} else {
+		anar_reg &= ~MII_ADVERTISE_10_FULL;
+	}
+
+	if (adv_speeds & LINK_HALF_10BASE) {
+		anar_reg |= MII_ADVERTISE_10_HALF;
+	} else {
+		anar_reg &= ~MII_ADVERTISE_10_HALF;
+	}
+
+	if (adv_speeds & LINK_FULL_100BASE) {
+		anar_reg |= MII_ADVERTISE_100_FULL;
+	} else {
+		anar_reg &= ~MII_ADVERTISE_100_FULL;
+	}
+
+	if (adv_speeds & LINK_HALF_100BASE) {
+		anar_reg |= MII_ADVERTISE_100_HALF;
+	} else {
+		anar_reg &= ~MII_ADVERTISE_100_HALF;
+	}
+
+	if (adv_speeds & LINK_FULL_1000BASE) {
+		c1kt_reg |= MII_ADVERTISE_1000_FULL;
+	} else {
+		c1kt_reg &= ~MII_ADVERTISE_1000_FULL;
+	}
+
+	if (adv_speeds & LINK_HALF_1000BASE) {
+		c1kt_reg |= MII_ADVERTISE_1000_HALF;
+	} else {
+		c1kt_reg &= ~MII_ADVERTISE_1000_HALF;
+	}
+
+	if (phy_mc_vsc8541_write(dev, PHY_REG_PAGE0_CTRL1000, c1kt_reg) < 0) {
+		return -EIO;
+	}
+
+	bmcr_reg |= MII_BMCR_AUTONEG_ENABLE;
+
+	if (phy_mc_vsc8541_write(dev, PHY_REG_PAGE0_ADV, anar_reg) < 0) {
+		return -EIO;
+	}
+
+	if (phy_mc_vsc8541_write(dev, PHY_REG_PAGE0_BMCR, bmcr_reg) < 0) {
+		return -EIO;
+	}
+
+	return 0;
 }
 
 /**