drivers: ethernet: phy: ksz9131: add getting status for gigabit mode

Read gigabit status from Master Slave Status Register.

Signed-off-by: Tony Han <tony.han@microchip.com>

Author: TonyHan11

drivers/ethernet/phy/phy_microchip_ksz9131.c

@@ -339,6 +339,28 @@ static int phy_mchp_ksz9131_cfg_link(const struct device *dev, enum phy_link_spe
 		goto done;
 	}
 
+	ret = ksz9131_read(dev, MII_1KTCR, &c1kt);
+	if (ret < 0) {
+		goto done;
+	}
+
+	if (adv_speeds & LINK_FULL_1000BASE) {
+		c1kt |= MII_ADVERTISE_1000_FULL;
+	} else {
+		c1kt &= ~MII_ADVERTISE_1000_FULL;
+	}
+
+	if (adv_speeds & LINK_HALF_1000BASE) {
+		c1kt |= MII_ADVERTISE_1000_HALF;
+	} else {
+		c1kt &= ~MII_ADVERTISE_1000_HALF;
+	}
+
+	ret = ksz9131_write(dev, MII_1KTCR, c1kt);
+	if (ret < 0) {
+		goto done;
+	}
+
 	ret = phy_mchp_ksz9131_autonegotiate(dev);
 done:
 	k_sem_give(&data->sem);
@@ -355,6 +377,36 @@ static int phy_mchp_ksz9131_cfg_link(const struct device *dev, enum phy_link_spe
 	return ret;
 }
 
+static bool phy_mchp_ksz9131_gigabit(const struct device *dev, enum phy_link_speed *speed, int *ret)
+{
+	uint16_t mutual_capabilities = 0;
+	uint16_t mscr = 0;
+	uint16_t mssr = 0;
+
+	/* Read AUTO-NEGOTIATION MASTER SLAVE CONTROL REGISTER */
+	*ret = ksz9131_read(dev, MII_1KTCR, &mscr);
+	if (*ret < 0) {
+		return true;
+	}
+
+	/* Read AUTO-NEGOTIATION MASTER SLAVE STATUS REGISTER */
+	*ret = ksz9131_read(dev, MII_1KSTSR, &mssr);
+	if (*ret < 0) {
+		return true;
+	}
+
+	mutual_capabilities = mscr & (mssr >> 2);
+	if (mutual_capabilities & MII_ADVERTISE_1000_FULL) {
+		*speed = LINK_FULL_1000BASE;
+		return true;
+	} else if (mutual_capabilities & MII_ADVERTISE_1000_HALF) {
+		*speed = LINK_HALF_1000BASE;
+		return true;
+	}
+
+	return false;
+}
+
 static int phy_mchp_ksz9131_get_link(const struct device *dev, struct phy_link_state *state)
 {
 	const struct mchp_ksz9131_config *config = dev->config;
@@ -374,6 +426,10 @@ static int phy_mchp_ksz9131_get_link(const struct device *dev, struct phy_link_s
 		goto done;
 	}
 
+	if (phy_mchp_ksz9131_gigabit(dev, &state->speed, &ret)) {
+		goto done;
+	}
+
 	/* Read currently configured advertising options */
 	ret = ksz9131_read(dev, MII_ANAR, &anar);
 	if (ret < 0) {