diff --git a/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_eth_soc.c b/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_eth_soc.c
index 1368a7b..ad73fdb 100755
--- a/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_eth_soc.c
+++ b/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_eth_soc.c
@@ -811,52 +811,44 @@
 		struct mtk_sgmii *ss = eth->sgmii;
 		u32 id = mtk_mac2xgmii_id(eth, mac->id);
 		u32 pmsr = mtk_r32(mac->hw, MTK_MAC_MSR(mac->id));
-		u32 rgc3, val = 0;
-
-		regmap_read(ss->pcs[id].regmap, SGMSYS_PCS_CONTROL_1, &val);
+		u32 bm, adv, rgc3, sgm_mode;
 
 		state->interface = mac->interface;
-		state->link = FIELD_GET(SGMII_LINK_STATYS, val);
 
-		if (FIELD_GET(SGMII_AN_ENABLE, val)) {
+		regmap_read(ss->pcs[id].regmap, SGMSYS_PCS_CONTROL_1, &bm);
+		if (bm & SGMII_AN_ENABLE) {
 			regmap_read(ss->pcs[id].regmap,
-				    SGMII_PCS_SPEED_ABILITY, &val);
-
-			val = val >> 16;
-
-			state->duplex = FIELD_GET(SGMII_PCS_SPEED_DUPLEX, val);
+				    SGMSYS_PCS_ADVERTISE, &adv);
 
-			switch (FIELD_GET(SGMII_PCS_SPEED_MASK, val)) {
-			case 0:
-				state->speed = SPEED_10;
-				break;
-			case 1:
-				state->speed = SPEED_100;
-				break;
-			case 2:
-				state->speed = SPEED_1000;
-				break;
-			}
+			phylink_mii_c22_pcs_decode_state(
+				state,
+				FIELD_GET(SGMII_BMSR, bm),
+				FIELD_GET(SGMII_LPA, adv));
 		} else {
-			regmap_read(ss->pcs[id].regmap,
-				    SGMSYS_SGMII_MODE, &val);
+			state->link = !!(bm & SGMII_LINK_STATYS);
 
-			state->duplex = !FIELD_GET(SGMII_DUPLEX_HALF, val);
+			regmap_read(ss->pcs[id].regmap,
+				    SGMSYS_SGMII_MODE, &sgm_mode);
 
-			switch (FIELD_GET(SGMII_SPEED_MASK, val)) {
-			case 0:
+			switch (sgm_mode & SGMII_SPEED_MASK) {
+			case SGMII_SPEED_10:
 				state->speed = SPEED_10;
 				break;
-			case 1:
+			case SGMII_SPEED_100:
 				state->speed = SPEED_100;
 				break;
-			case 2:
+			case SGMII_SPEED_1000:
 				regmap_read(ss->pcs[id].regmap,
-					    ss->pcs[id].ana_rgc3, &val);
-				rgc3 = FIELD_GET(RG_PHY_SPEED_3_125G, val);
+					    ss->pcs[id].ana_rgc3, &rgc3);
+				rgc3 = FIELD_GET(RG_PHY_SPEED_3_125G, rgc3);
 				state->speed = rgc3 ? SPEED_2500 : SPEED_1000;
 				break;
 			}
+
+			if (sgm_mode & SGMII_DUPLEX_HALF)
+				state->duplex = DUPLEX_HALF;
+			else
+				state->duplex = DUPLEX_FULL;
 		}
 
 		state->pause &= (MLO_PAUSE_RX | MLO_PAUSE_TX);
diff --git a/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_eth_soc.h b/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_eth_soc.h
index 646e557..f279ad5 100755
--- a/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_eth_soc.h
+++ b/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_eth_soc.h
@@ -820,8 +820,8 @@
 #define ETHSYS_DMA_AG_MAP_PPE	BIT(2)
 
 /* SGMII subsystem config registers */
-/* Register to auto-negotiation restart */
 #define SGMSYS_PCS_CONTROL_1	0x0
+#define SGMII_BMSR		GENMASK(31, 16)
 #define SGMII_AN_RESTART	BIT(9)
 #define SGMII_ISOLATE		BIT(10)
 #define SGMII_AN_ENABLE		BIT(12)
@@ -832,13 +832,15 @@
 #define SGMII_AN_EXPANSION_CLR	BIT(30)
 
 /* Register to set SGMII speed */
-#define SGMII_PCS_SPEED_ABILITY	0x08
-#define SGMII_PCS_SPEED_MASK	GENMASK(11, 10)
-#define SGMII_PCS_SPEED_10	0
-#define SGMII_PCS_SPEED_100	1
-#define SGMII_PCS_SPEED_1000	2
-#define SGMII_PCS_SPEED_DUPLEX	BIT(12)
-#define SGMII_PCS_SPEED_LINK	BIT(15)
+#define SGMSYS_PCS_ADVERTISE	0x08
+#define SGMII_ADVERTISE		GENMASK(15, 0)
+#define SGMII_LPA		GENMASK(31, 16)
+#define SGMII_LPA_SPEED_MASK	GENMASK(11, 10)
+#define SGMII_LPA_SPEED_10	0
+#define SGMII_LPA_SPEED_100	1
+#define SGMII_LPA_SPEED_1000	2
+#define SGMII_LPA_DUPLEX	BIT(12)
+#define SGMII_LPA_LINK		BIT(15)
 
 /* Register to programmable link timer, the unit in 2 * 8ns */
 #define SGMSYS_PCS_LINK_TIMER	0x18
@@ -846,7 +848,7 @@
 
 /* Register to control remote fault */
 #define SGMSYS_SGMII_MODE		0x20
-#define SGMII_IF_MODE_BIT0		BIT(0)
+#define SGMII_IF_MODE_SGMII		BIT(0)
 #define SGMII_SPEED_DUPLEX_AN		BIT(1)
 #define SGMII_SPEED_MASK		GENMASK(3, 2)
 #define SGMII_SPEED_10			0x0
diff --git a/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_sgmii.c b/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_sgmii.c
index 3c37656..282fd30 100755
--- a/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_sgmii.c
+++ b/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_sgmii.c
@@ -9,6 +9,7 @@
 
 #include <linux/mfd/syscon.h>
 #include <linux/of.h>
+#include <linux/phylink.h>
 #include <linux/regmap.h>
 
 #include "mtk_eth_soc.h"
@@ -143,29 +144,6 @@
 	mdelay(1);
 }
 
-int mtk_sgmii_need_powerdown(struct mtk_sgmii_pcs *mpcs,
-			     phy_interface_t interface)
-{
-	u32 val;
-
-	/* need to power down sgmii if link down */
-	regmap_read(mpcs->regmap, SGMSYS_PCS_CONTROL_1, &val);
-	if (!(val & SGMII_LINK_STATYS))
-		return true;
-
-	/* need to power down sgmii if interface changed */
-	regmap_read(mpcs->regmap, mpcs->ana_rgc3, &val);
-	if (interface == PHY_INTERFACE_MODE_2500BASEX) {
-		if (!(val & RG_PHY_SPEED_3_125G))
-			return true;
-	} else {
-		if (val & RG_PHY_SPEED_3_125G)
-			return true;
-	}
-
-	return false;
-}
-
 void mtk_sgmii_setup_phya_gen1(struct mtk_sgmii_pcs *mpcs)
 {
 	if (!mpcs->regmap_pextp)
@@ -368,179 +346,140 @@
 	udelay(400);
 }
 
-int mtk_sgmii_setup_mode_an(struct mtk_sgmii_pcs *mpcs)
+static void mtk_sgmii_pcs_get_state(struct phylink_pcs *pcs,
+				    struct phylink_link_state *state)
 {
-	struct mtk_eth *eth = mpcs->eth;
-	unsigned int val = 0;
-
-	if (!mpcs->regmap)
-		return -EINVAL;
-
-	if (MTK_HAS_CAPS(eth->soc->caps, MTK_NETSYS_V3)) {
-		mtk_sgmii_xfi_pll_enable(eth->sgmii);
-		mtk_sgmii_reset(eth, mpcs->id);
-	}
-
-	/* Assert PHYA power down state when needed */
-	if (mtk_sgmii_need_powerdown(mpcs, PHY_INTERFACE_MODE_SGMII))
-		regmap_write(mpcs->regmap, SGMSYS_QPHY_PWR_STATE_CTRL,
-			     SGMII_PHYA_PWD);
-
-	/* Reset SGMII PCS state */
-	regmap_write(mpcs->regmap, SGMII_RESERVED_0, SGMII_SW_RESET);
-
-	regmap_read(mpcs->regmap, mpcs->ana_rgc3, &val);
-	val &= ~RG_PHY_SPEED_3_125G;
-	regmap_write(mpcs->regmap, mpcs->ana_rgc3, val);
-
-	/* Setup the link timer and QPHY power up inside SGMIISYS */
-	regmap_write(mpcs->regmap, SGMSYS_PCS_LINK_TIMER,
-		     SGMII_LINK_TIMER_DEFAULT);
-
-	regmap_read(mpcs->regmap, SGMSYS_SGMII_MODE, &val);
-	val |= SGMII_REMOTE_FAULT_DIS;
-	regmap_write(mpcs->regmap, SGMSYS_SGMII_MODE, val);
+	struct mtk_sgmii_pcs *mpcs = pcs_to_mtk_sgmii_pcs(pcs);
+	unsigned int bm, adv, rgc3, sgm_mode;
 
-	/* SGMII AN mode setting */
-	regmap_read(mpcs->regmap, SGMSYS_SGMII_MODE, &val);
-	val &= ~SGMII_IF_MODE_MASK;
-	val |= SGMII_SPEED_DUPLEX_AN;
-	regmap_write(mpcs->regmap, SGMSYS_SGMII_MODE, val);
+	state->interface = mpcs->interface;
 
-	/* Enable SGMII AN */
-	regmap_read(mpcs->regmap, SGMSYS_PCS_CONTROL_1, &val);
-	val |= SGMII_AN_ENABLE;
-	regmap_write(mpcs->regmap, SGMSYS_PCS_CONTROL_1, val);
+	regmap_read(mpcs->regmap, SGMSYS_PCS_CONTROL_1, &bm);
+	if (bm & SGMII_AN_ENABLE) {
+		regmap_read(mpcs->regmap, SGMSYS_PCS_ADVERTISE, &adv);
 
-	if (MTK_HAS_FLAGS(mpcs->flags, MTK_SGMII_PN_SWAP))
-		regmap_update_bits(mpcs->regmap, SGMSYS_QPHY_WRAP_CTRL,
-				   SGMII_PN_SWAP_MASK, SGMII_PN_SWAP_TX_RX);
+		phylink_mii_c22_pcs_decode_state(state,
+						 FIELD_GET(SGMII_BMSR, bm),
+						 FIELD_GET(SGMII_LPA, adv));
+	} else {
+		state->link = !!(bm & SGMII_LINK_STATYS);
 
-	/* Release PHYA power down state */
-	regmap_write(mpcs->regmap, SGMSYS_QPHY_PWR_STATE_CTRL, 0);
+		regmap_read(mpcs->regmap, SGMSYS_SGMII_MODE, &sgm_mode);
 
-	if (MTK_HAS_CAPS(eth->soc->caps, MTK_NETSYS_V3))
-		mtk_sgmii_setup_phya_gen1(mpcs);
+		switch (sgm_mode & SGMII_SPEED_MASK) {
+		case SGMII_SPEED_10:
+			state->speed = SPEED_10;
+			break;
+		case SGMII_SPEED_100:
+			state->speed = SPEED_100;
+			break;
+		case SGMII_SPEED_1000:
+			regmap_read(mpcs->regmap, mpcs->ana_rgc3, &rgc3);
+			rgc3 = FIELD_GET(RG_PHY_SPEED_3_125G, rgc3);
+			state->speed = rgc3 ? SPEED_2500 : SPEED_1000;
+			break;
+		}
 
-	return 0;
+		if (sgm_mode & SGMII_DUPLEX_HALF)
+			state->duplex = DUPLEX_HALF;
+		else
+			state->duplex = DUPLEX_FULL;
+	}
 }
 
-int mtk_sgmii_setup_mode_force(struct mtk_sgmii_pcs *mpcs,
-			       phy_interface_t interface)
+static int mtk_sgmii_pcs_config(struct phylink_pcs *pcs, unsigned int mode,
+				phy_interface_t interface,
+				const unsigned long *advertising,
+				bool permit_pause_to_mac)
 {
+	struct mtk_sgmii_pcs *mpcs = pcs_to_mtk_sgmii_pcs(pcs);
 	struct mtk_eth *eth = mpcs->eth;
-	unsigned int val = 0;
+	unsigned int rgc3, sgm_mode = 0, bmcr = 0, speed = 0;
+	bool mode_changed = false, changed;
+	int advertise, link_timer;
 
-	if (!mpcs->regmap)
-		return -EINVAL;
+	advertise = phylink_mii_c22_pcs_encode_advertisement(interface,
+							     advertising);
+	if (advertise < 0)
+		return advertise;
 
 	if (MTK_HAS_CAPS(eth->soc->caps, MTK_NETSYS_V3)) {
 		mtk_sgmii_xfi_pll_enable(eth->sgmii);
 		mtk_sgmii_reset(eth, mpcs->id);
 	}
 
-	/* Assert PHYA power down state when needed */
-	if (mtk_sgmii_need_powerdown(mpcs, interface))
-		regmap_write(mpcs->regmap, SGMSYS_QPHY_PWR_STATE_CTRL,
-			     SGMII_PHYA_PWD);
+	/* Clearing IF_MODE_BIT0 switches the PCS to BASE-X mode, and
+	 * we assume that fixes it's speed at bitrate = line rate (in
+	 * other words, 1000Mbps or 2500Mbps).
+	 */
+	if (interface == PHY_INTERFACE_MODE_SGMII) {
+		bmcr = SGMII_AN_ENABLE;
+		sgm_mode = SGMII_IF_MODE_SGMII |
+			   SGMII_REMOTE_FAULT_DIS |
+			   SGMII_SPEED_DUPLEX_AN;
+	} else {
+		/* 1000base-X or HSGMII without autoneg */
+		speed = SGMII_SPEED_1000;
+		if (interface == PHY_INTERFACE_MODE_2500BASEX)
+			sgm_mode = SGMII_IF_MODE_SGMII;
+	}
 
-	/* Reset SGMII PCS state */
-	regmap_write(mpcs->regmap, SGMII_RESERVED_0, SGMII_SW_RESET);
+	if (mpcs->interface != interface) {
+		link_timer = phylink_get_link_timer_ns(interface);
+		if (link_timer < 0)
+			return link_timer;
 
-	regmap_read(mpcs->regmap, mpcs->ana_rgc3, &val);
-	val &= ~RG_PHY_SPEED_MASK;
-	if (interface == PHY_INTERFACE_MODE_2500BASEX)
-		val |= RG_PHY_SPEED_3_125G;
-	regmap_write(mpcs->regmap, mpcs->ana_rgc3, val);
+		/* PHYA power down */
+		regmap_update_bits(mpcs->regmap, SGMSYS_QPHY_PWR_STATE_CTRL,
+				   SGMII_PHYA_PWD, SGMII_PHYA_PWD);
 
-	/* Disable SGMII AN */
-	regmap_read(mpcs->regmap, SGMSYS_PCS_CONTROL_1, &val);
-	val &= ~SGMII_AN_ENABLE;
-	regmap_write(mpcs->regmap, SGMSYS_PCS_CONTROL_1, val);
+		/* Reset SGMII PCS state */
+		regmap_update_bits(mpcs->regmap, SGMII_RESERVED_0,
+				   SGMII_SW_RESET, SGMII_SW_RESET);
 
-	/* Set the speed etc but leave the duplex unchanged */
-	regmap_read(mpcs->regmap, SGMSYS_SGMII_MODE, &val);
-	val &= SGMII_DUPLEX_HALF | ~SGMII_IF_MODE_MASK;
-	val &= ~SGMII_REMOTE_FAULT_DIS;
-	val |= SGMII_SPEED_1000;
-	regmap_write(mpcs->regmap, SGMSYS_SGMII_MODE, val);
+		if (interface == PHY_INTERFACE_MODE_2500BASEX)
+			rgc3 = RG_PHY_SPEED_3_125G;
+		else
+			rgc3 = 0;
 
-	if (MTK_HAS_FLAGS(mpcs->flags, MTK_SGMII_PN_SWAP))
-		regmap_update_bits(mpcs->regmap, SGMSYS_QPHY_WRAP_CTRL,
-				   SGMII_PN_SWAP_MASK, SGMII_PN_SWAP_TX_RX);
+		/* Configure the underlying interface speed */
+		regmap_update_bits(mpcs->regmap, mpcs->ana_rgc3,
+				   RG_PHY_SPEED_3_125G, rgc3);
 
-	/* Release PHYA power down state */
-	regmap_write(mpcs->regmap, SGMSYS_QPHY_PWR_STATE_CTRL, 0);
+		/* Setup the link timer */
+		regmap_write(mpcs->regmap, SGMSYS_PCS_LINK_TIMER,
+			     link_timer / 2 / 8);
 
-	if (MTK_HAS_CAPS(eth->soc->caps, MTK_NETSYS_V3))
-		mtk_sgmii_setup_phya_gen2(mpcs);
+		mpcs->interface = interface;
+		mode_changed = true;
+	}
 
-	return 0;
-}
+	/* Update the advertisement, noting whether it has changed */
+	regmap_update_bits_check(mpcs->regmap, SGMSYS_PCS_ADVERTISE,
+				 SGMII_ADVERTISE, advertise, &changed);
 
-static void mtk_sgmii_pcs_get_state(struct phylink_pcs *pcs,
-				    struct phylink_link_state *state)
-{
-	struct mtk_sgmii_pcs *mpcs = pcs_to_mtk_sgmii_pcs(pcs);
-	u32 rgc3, val = 0;
+	/* Update the sgmsys mode register */
+	regmap_update_bits(mpcs->regmap, SGMSYS_SGMII_MODE,
+			   SGMII_REMOTE_FAULT_DIS | SGMII_DUPLEX_HALF |
+			   SGMII_SPEED_MASK | SGMII_SPEED_DUPLEX_AN |
+			   SGMII_IF_MODE_SGMII, sgm_mode | speed);
 
-	state->interface = mpcs->interface;
+	/* Update the BMCR */
+	regmap_update_bits(mpcs->regmap, SGMSYS_PCS_CONTROL_1,
+			   SGMII_AN_ENABLE, bmcr);
 
-	regmap_read(mpcs->regmap, SGMSYS_PCS_CONTROL_1, &val);
-	if (FIELD_GET(SGMII_AN_ENABLE, val)) {
-		regmap_read(mpcs->regmap, SGMII_PCS_SPEED_ABILITY, &val);
-		val = val >> 16;
-		state->link = FIELD_GET(SGMII_PCS_SPEED_LINK, val);
-		state->duplex = FIELD_GET(SGMII_PCS_SPEED_DUPLEX, val);
-		switch (FIELD_GET(SGMII_PCS_SPEED_MASK, val)) {
-		case 0:
-			state->speed = SPEED_10;
-			break;
-		case 1:
-			state->speed = SPEED_100;
-			break;
-		case 2:
-			state->speed = SPEED_1000;
-			break;
-		}
-	} else {
-		state->link = FIELD_GET(SGMII_LINK_STATYS, val);
+	/* Release PHYA power down state */
+	usleep_range(50, 100);
+	regmap_write(mpcs->regmap, SGMSYS_QPHY_PWR_STATE_CTRL, 0);
 
-		regmap_read(mpcs->regmap, SGMSYS_SGMII_MODE, &val);
-		state->duplex = !FIELD_GET(SGMII_DUPLEX_HALF, val);
-		switch (FIELD_GET(SGMII_SPEED_MASK, val)) {
-		case 0:
-			state->speed = SPEED_10;
-			break;
-		case 1:
-			state->speed = SPEED_100;
-			break;
-		case 2:
-			regmap_read(mpcs->regmap, mpcs->ana_rgc3, &val);
-			rgc3 = FIELD_GET(RG_PHY_SPEED_3_125G, val);
-			state->speed = rgc3 ? SPEED_2500 : SPEED_1000;
-			break;
-		}
+	if (MTK_HAS_CAPS(eth->soc->caps, MTK_NETSYS_V3)) {
+		if (interface == PHY_INTERFACE_MODE_2500BASEX)
+			mtk_sgmii_setup_phya_gen2(mpcs);
+		else
+			mtk_sgmii_setup_phya_gen1(mpcs);
 	}
-}
-
-static int mtk_sgmii_pcs_config(struct phylink_pcs *pcs, unsigned int mode,
-				phy_interface_t interface,
-				const unsigned long *advertising,
-				bool permit_pause_to_mac)
-{
-	struct mtk_sgmii_pcs *mpcs = pcs_to_mtk_sgmii_pcs(pcs);
-	int err = 0;
 
-	mpcs->interface = interface;
-
-	/* Setup SGMIISYS with the determined property */
-	if (interface != PHY_INTERFACE_MODE_SGMII)
-		err = mtk_sgmii_setup_mode_force(mpcs, interface);
-	else
-		err = mtk_sgmii_setup_mode_an(mpcs);
-
-	return err;
+	return changed || mode_changed;
 }
 
 void mtk_sgmii_pcs_restart_an(struct phylink_pcs *pcs)
@@ -561,18 +500,29 @@
 				  int speed, int duplex)
 {
 	struct mtk_sgmii_pcs *mpcs = pcs_to_mtk_sgmii_pcs(pcs);
-	unsigned int val;
+	unsigned int sgm_mode, val;
 
-	if (!phy_interface_mode_is_8023z(interface))
+	/* If autoneg is enabled, the force speed and duplex
+	 * are not useful, so don't go any further.
+	 */
+	regmap_read(mpcs->regmap, SGMSYS_PCS_CONTROL_1, &val);
+	if (val & SGMII_AN_ENABLE)
 		return;
 
+	/* SGMII force speed and duplex setting */
+	if (speed == SPEED_10)
+		sgm_mode = SGMII_SPEED_10;
+	else if (speed == SPEED_100)
+		sgm_mode = SGMII_SPEED_100;
+	else
+		sgm_mode = SGMII_SPEED_1000;
+
-	/* SGMII force duplex setting */
-	regmap_read(mpcs->regmap, SGMSYS_SGMII_MODE, &val);
-	val &= ~SGMII_DUPLEX_HALF;
 	if (duplex != DUPLEX_FULL)
-		val |= SGMII_DUPLEX_HALF;
+		sgm_mode |= SGMII_DUPLEX_HALF;
 
-	regmap_write(mpcs->regmap, SGMSYS_SGMII_MODE, val);
+	regmap_update_bits(mpcs->regmap, SGMSYS_SGMII_MODE,
+			   SGMII_DUPLEX_HALF | SGMII_SPEED_MASK,
+			   sgm_mode);
 }
 
 static const struct phylink_pcs_ops mtk_sgmii_pcs_ops = {
diff --git a/target/linux/mediatek/patches-5.4/758-net-phy-add-phylink-pcs-decode-helper.patch b/target/linux/mediatek/patches-5.4/758-net-phy-add-phylink-pcs-decode-helper.patch
new file mode 100644
index 0000000..164578a
--- /dev/null
+++ b/target/linux/mediatek/patches-5.4/758-net-phy-add-phylink-pcs-decode-helper.patch
@@ -0,0 +1,479 @@
+diff --git a/drivers/net/phy/Makefile b/drivers/net/phy/Makefile
+index 799ff9c..54ad7e8 100644
+--- a/drivers/net/phy/Makefile
++++ b/drivers/net/phy/Makefile
+@@ -1,7 +1,8 @@
+ # SPDX-License-Identifier: GPL-2.0
+ # Makefile for Linux PHY drivers and MDIO bus drivers
+ 
+-libphy-y			:= phy.o phy-c45.o phy-core.o phy_device.o
++libphy-y			:= phy.o phy-c45.o phy-core.o phy_device.o \
++				   linkmode.o
+ mdio-bus-y			+= mdio_bus.o mdio_device.o
+ 
+ ifdef CONFIG_MDIO_DEVICE
+diff --git a/drivers/net/phy/linkmode.c b/drivers/net/phy/linkmode.c
+new file mode 100644
+index 0000000..a5a347b
+--- /dev/null
++++ b/drivers/net/phy/linkmode.c
+@@ -0,0 +1,95 @@
++// SPDX-License-Identifier: GPL-2.0+
++#include <linux/linkmode.h>
++
++/**
++ * linkmode_resolve_pause - resolve the allowable pause modes
++ * @local_adv: local advertisement in ethtool format
++ * @partner_adv: partner advertisement in ethtool format
++ * @tx_pause: pointer to bool to indicate whether transmit pause should be
++ * enabled.
++ * @rx_pause: pointer to bool to indicate whether receive pause should be
++ * enabled.
++ *
++ * Flow control is resolved according to our and the link partners
++ * advertisements using the following drawn from the 802.3 specs:
++ *  Local device  Link partner
++ *  Pause AsymDir Pause AsymDir Result
++ *    0     X       0     X     Disabled
++ *    0     1       1     0     Disabled
++ *    0     1       1     1     TX
++ *    1     0       0     X     Disabled
++ *    1     X       1     X     TX+RX
++ *    1     1       0     1     RX
++ */
++void linkmode_resolve_pause(unsigned long *local_adv,
++			    unsigned long *partner_adv,
++			    bool *tx_pause, bool *rx_pause)
++{
++	__ETHTOOL_DECLARE_LINK_MODE_MASK(m);
++
++	linkmode_and(m, local_adv, partner_adv);
++	if (linkmode_test_bit(ETHTOOL_LINK_MODE_Pause_BIT, m)) {
++		*tx_pause = true;
++		*rx_pause = true;
++	} else if (linkmode_test_bit(ETHTOOL_LINK_MODE_Asym_Pause_BIT, m)) {
++		*tx_pause = linkmode_test_bit(ETHTOOL_LINK_MODE_Pause_BIT,
++					      partner_adv);
++		*rx_pause = linkmode_test_bit(ETHTOOL_LINK_MODE_Pause_BIT,
++					      local_adv);
++	} else {
++		*tx_pause = false;
++		*rx_pause = false;
++	}
++}
++EXPORT_SYMBOL_GPL(linkmode_resolve_pause);
++
++/**
++ * linkmode_set_pause - set the pause mode advertisement
++ * @advertisement: advertisement in ethtool format
++ * @tx: boolean from ethtool struct ethtool_pauseparam tx_pause member
++ * @rx: boolean from ethtool struct ethtool_pauseparam rx_pause member
++ *
++ * Configure the advertised Pause and Asym_Pause bits according to the
++ * capabilities of provided in @tx and @rx.
++ *
++ * We convert as follows:
++ *  tx rx  Pause AsymDir
++ *  0  0   0     0
++ *  0  1   1     1
++ *  1  0   0     1
++ *  1  1   1     0
++ *
++ * Note: this translation from ethtool tx/rx notation to the advertisement
++ * is actually very problematical. Here are some examples:
++ *
++ * For tx=0 rx=1, meaning transmit is unsupported, receive is supported:
++ *
++ *  Local device  Link partner
++ *  Pause AsymDir Pause AsymDir Result
++ *    1     1       1     0     TX + RX - but we have no TX support.
++ *    1     1       0     1	Only this gives RX only
++ *
++ * For tx=1 rx=1, meaning we have the capability to transmit and receive
++ * pause frames:
++ *
++ *  Local device  Link partner
++ *  Pause AsymDir Pause AsymDir Result
++ *    1     0       0     1     Disabled - but since we do support tx and rx,
++ *				this should resolve to RX only.
++ *
++ * Hence, asking for:
++ *  rx=1 tx=0 gives Pause+AsymDir advertisement, but we may end up
++ *            resolving to tx+rx pause or only rx pause depending on
++ *            the partners advertisement.
++ *  rx=0 tx=1 gives AsymDir only, which will only give tx pause if
++ *            the partners advertisement allows it.
++ *  rx=1 tx=1 gives Pause only, which will only allow tx+rx pause
++ *            if the other end also advertises Pause.
++ */
++void linkmode_set_pause(unsigned long *advertisement, bool tx, bool rx)
++{
++	linkmode_mod_bit(ETHTOOL_LINK_MODE_Pause_BIT, advertisement, rx);
++	linkmode_mod_bit(ETHTOOL_LINK_MODE_Asym_Pause_BIT, advertisement,
++			 rx ^ tx);
++}
++EXPORT_SYMBOL_GPL(linkmode_set_pause);
+diff --git a/drivers/net/phy/phylink.c b/drivers/net/phy/phylink.c
+index ead9b37..fec6d0e 100644
+--- a/drivers/net/phy/phylink.c
++++ b/drivers/net/phy/phylink.c
+@@ -2184,4 +2184,192 @@ void phylink_helper_basex_speed(struct phylink_link_state *state)
+ }
+ EXPORT_SYMBOL_GPL(phylink_helper_basex_speed);
+ 
++static void phylink_decode_c37_word(struct phylink_link_state *state,
++				    uint16_t config_reg, int speed)
++{
++	bool tx_pause, rx_pause;
++	int fd_bit;
++
++	if (speed == SPEED_2500)
++		fd_bit = ETHTOOL_LINK_MODE_2500baseX_Full_BIT;
++	else
++		fd_bit = ETHTOOL_LINK_MODE_1000baseX_Full_BIT;
++
++	mii_lpa_mod_linkmode_x(state->lp_advertising, config_reg, fd_bit);
++
++	if (linkmode_test_bit(fd_bit, state->advertising) &&
++	    linkmode_test_bit(fd_bit, state->lp_advertising)) {
++		state->speed = speed;
++		state->duplex = DUPLEX_FULL;
++	} else {
++		/* negotiation failure */
++		state->link = false;
++	}
++
++	linkmode_resolve_pause(state->advertising, state->lp_advertising,
++			       &tx_pause, &rx_pause);
++
++	if (tx_pause)
++		state->pause |= MLO_PAUSE_TX;
++	if (rx_pause)
++		state->pause |= MLO_PAUSE_RX;
++}
++
++static void phylink_decode_sgmii_word(struct phylink_link_state *state,
++				      uint16_t config_reg)
++{
++	if (!(config_reg & LPA_SGMII_LINK)) {
++		state->link = false;
++		return;
++	}
++
++	switch (config_reg & LPA_SGMII_SPD_MASK) {
++	case LPA_SGMII_10:
++		state->speed = SPEED_10;
++		break;
++	case LPA_SGMII_100:
++		state->speed = SPEED_100;
++		break;
++	case LPA_SGMII_1000:
++		state->speed = SPEED_1000;
++		break;
++	default:
++		state->link = false;
++		return;
++	}
++	if (config_reg & LPA_SGMII_FULL_DUPLEX)
++		state->duplex = DUPLEX_FULL;
++	else
++		state->duplex = DUPLEX_HALF;
++}
++
++/**
++ * phylink_decode_usxgmii_word() - decode the USXGMII word from a MAC PCS
++ * @state: a pointer to a struct phylink_link_state.
++ * @lpa: a 16 bit value which stores the USXGMII auto-negotiation word
++ *
++ * Helper for MAC PCS supporting the USXGMII protocol and the auto-negotiation
++ * code word.  Decode the USXGMII code word and populate the corresponding fields
++ * (speed, duplex) into the phylink_link_state structure.
++ */
++void phylink_decode_usxgmii_word(struct phylink_link_state *state,
++				 uint16_t lpa)
++{
++	switch (lpa & MDIO_USXGMII_SPD_MASK) {
++	case MDIO_USXGMII_10:
++		state->speed = SPEED_10;
++		break;
++	case MDIO_USXGMII_100:
++		state->speed = SPEED_100;
++		break;
++	case MDIO_USXGMII_1000:
++		state->speed = SPEED_1000;
++		break;
++	case MDIO_USXGMII_2500:
++		state->speed = SPEED_2500;
++		break;
++	case MDIO_USXGMII_5000:
++		state->speed = SPEED_5000;
++		break;
++	case MDIO_USXGMII_10G:
++		state->speed = SPEED_10000;
++		break;
++	default:
++		state->link = false;
++		return;
++	}
++
++	if (lpa & MDIO_USXGMII_FULL_DUPLEX)
++		state->duplex = DUPLEX_FULL;
++	else
++		state->duplex = DUPLEX_HALF;
++}
++EXPORT_SYMBOL_GPL(phylink_decode_usxgmii_word);
++
++/**
++ * phylink_mii_c22_pcs_decode_state() - Decode MAC PCS state from MII registers
++ * @state: a pointer to a &struct phylink_link_state.
++ * @bmsr: The value of the %MII_BMSR register
++ * @lpa: The value of the %MII_LPA register
++ *
++ * Helper for MAC PCS supporting the 802.3 clause 22 register set for
++ * clause 37 negotiation and/or SGMII control.
++ *
++ * Parse the Clause 37 or Cisco SGMII link partner negotiation word into
++ * the phylink @state structure. This is suitable to be used for implementing
++ * the mac_pcs_get_state() member of the struct phylink_mac_ops structure if
++ * accessing @bmsr and @lpa cannot be done with MDIO directly.
++ */
++void phylink_mii_c22_pcs_decode_state(struct phylink_link_state *state,
++				      u16 bmsr, u16 lpa)
++{
++	state->link = !!(bmsr & BMSR_LSTATUS);
++	state->an_complete = !!(bmsr & BMSR_ANEGCOMPLETE);
++	/* If there is no link or autonegotiation is disabled, the LP advertisement
++	 * data is not meaningful, so don't go any further.
++	 */
++	if (!state->link || !state->an_enabled)
++		return;
++
++	switch (state->interface) {
++	case PHY_INTERFACE_MODE_1000BASEX:
++		phylink_decode_c37_word(state, lpa, SPEED_1000);
++		break;
++
++	case PHY_INTERFACE_MODE_2500BASEX:
++		phylink_decode_c37_word(state, lpa, SPEED_2500);
++		break;
++
++	case PHY_INTERFACE_MODE_SGMII:
++	case PHY_INTERFACE_MODE_QSGMII:
++		phylink_decode_sgmii_word(state, lpa);
++		break;
++
++	default:
++		state->link = false;
++		break;
++	}
++}
++EXPORT_SYMBOL_GPL(phylink_mii_c22_pcs_decode_state);
++
++/**
++ * phylink_mii_c22_pcs_encode_advertisement() - configure the clause 37 PCS
++ *	advertisement
++ * @interface: the PHY interface mode being configured
++ * @advertising: the ethtool advertisement mask
++ *
++ * Helper for MAC PCS supporting the 802.3 clause 22 register set for
++ * clause 37 negotiation and/or SGMII control.
++ *
++ * Encode the clause 37 PCS advertisement as specified by @interface and
++ * @advertising.
++ *
++ * Return: The new value for @adv, or ``-EINVAL`` if it should not be changed.
++ */
++int phylink_mii_c22_pcs_encode_advertisement(phy_interface_t interface,
++					     const unsigned long *advertising)
++{
++	u16 adv;
++
++	switch (interface) {
++	case PHY_INTERFACE_MODE_1000BASEX:
++	case PHY_INTERFACE_MODE_2500BASEX:
++		adv = ADVERTISE_1000XFULL;
++		if (linkmode_test_bit(ETHTOOL_LINK_MODE_Pause_BIT,
++				      advertising))
++			adv |= ADVERTISE_1000XPAUSE;
++		if (linkmode_test_bit(ETHTOOL_LINK_MODE_Asym_Pause_BIT,
++				      advertising))
++			adv |= ADVERTISE_1000XPSE_ASYM;
++		return adv;
++	case PHY_INTERFACE_MODE_SGMII:
++	case PHY_INTERFACE_MODE_QSGMII:
++		return 0x0001;
++	default:
++		/* Nothing to do for other modes */
++		return -EINVAL;
++	}
++}
++EXPORT_SYMBOL_GPL(phylink_mii_c22_pcs_encode_advertisement);
++
+ MODULE_LICENSE("GPL v2");
+diff --git a/include/linux/linkmode.h b/include/linux/linkmode.h
+index a99c588..d38da4e 100644
+--- a/include/linux/linkmode.h
++++ b/include/linux/linkmode.h
+@@ -82,4 +82,10 @@ static inline int linkmode_equal(const unsigned long *src1,
+ 	return bitmap_equal(src1, src2, __ETHTOOL_LINK_MODE_MASK_NBITS);
+ }
+ 
++void linkmode_resolve_pause(unsigned long *local_adv,
++			    unsigned long *partner_adv,
++			    bool *tx_pause, bool *rx_pause);
++
++void linkmode_set_pause(unsigned long *advertisement, bool tx, bool rx);
++
+ #endif /* __LINKMODE_H */
+diff --git a/include/linux/mii.h b/include/linux/mii.h
+index 4ce8901..54b7f64 100644
+--- a/include/linux/mii.h
++++ b/include/linux/mii.h
+@@ -485,6 +485,45 @@ static inline u32 linkmode_adv_to_lcl_adv_t(unsigned long *advertising)
+ 	return lcl_adv;
+ }
+ 
++/**
++ * mii_lpa_mod_linkmode_x - decode the link partner's config_reg to linkmodes
++ * @linkmodes: link modes array
++ * @lpa: config_reg word from link partner
++ * @fd_bit: link mode for 1000XFULL bit
++ */
++static inline void mii_lpa_mod_linkmode_x(unsigned long *linkmodes, u16 lpa,
++					 int fd_bit)
++{
++	linkmode_mod_bit(ETHTOOL_LINK_MODE_Autoneg_BIT, linkmodes,
++			 lpa & LPA_LPACK);
++	linkmode_mod_bit(ETHTOOL_LINK_MODE_Pause_BIT, linkmodes,
++			 lpa & LPA_1000XPAUSE);
++	linkmode_mod_bit(ETHTOOL_LINK_MODE_Asym_Pause_BIT, linkmodes,
++			 lpa & LPA_1000XPAUSE_ASYM);
++	linkmode_mod_bit(fd_bit, linkmodes,
++			 lpa & LPA_1000XFULL);
++}
++
++/**
++ * linkmode_adv_to_mii_adv_x - encode a linkmode to config_reg
++ * @linkmodes: linkmodes
++ * @fd_bit: full duplex bit
++ */
++static inline u16 linkmode_adv_to_mii_adv_x(unsigned long *linkmodes,
++					    int fd_bit)
++{
++	u16 adv = 0;
++
++	if (linkmode_test_bit(fd_bit, linkmodes))
++		adv |= ADVERTISE_1000XFULL;
++	if (linkmode_test_bit(ETHTOOL_LINK_MODE_Pause_BIT, linkmodes))
++		adv |= ADVERTISE_1000XPAUSE;
++	if (linkmode_test_bit(ETHTOOL_LINK_MODE_Asym_Pause_BIT, linkmodes))
++		adv |= ADVERTISE_1000XPSE_ASYM;
++
++	return adv;
++}
++
+ /**
+  * mii_advertise_flowctrl - get flow control advertisement flags
+  * @cap: Flow control capabilities (FLOW_CTRL_RX, FLOW_CTRL_TX or both)
+diff --git a/include/linux/phylink.h b/include/linux/phylink.h
+index ba0f09d..48ff9fe 100644
+--- a/include/linux/phylink.h
++++ b/include/linux/phylink.h
+@@ -490,4 +490,34 @@ int phylink_mii_ioctl(struct phylink *, struct ifreq *, int);
+ void phylink_set_port_modes(unsigned long *bits);
+ void phylink_helper_basex_speed(struct phylink_link_state *state);
+ 
++/**
++ * phylink_get_link_timer_ns - return the PCS link timer value
++ * @interface: link &typedef phy_interface_t mode
++ *
++ * Return the PCS link timer setting in nanoseconds for the PHY @interface
++ * mode, or -EINVAL if not appropriate.
++ */
++static inline int phylink_get_link_timer_ns(phy_interface_t interface)
++{
++	switch (interface) {
++	case PHY_INTERFACE_MODE_SGMII:
++	case PHY_INTERFACE_MODE_QSGMII:
++	case PHY_INTERFACE_MODE_USXGMII:
++		return 1600000;
++
++	case PHY_INTERFACE_MODE_1000BASEX:
++	case PHY_INTERFACE_MODE_2500BASEX:
++		return 10000000;
++
++	default:
++		return -EINVAL;
++	}
++}
++
++void phylink_mii_c22_pcs_decode_state(struct phylink_link_state *state,
++				      u16 bmsr, u16 lpa);
++int phylink_mii_c22_pcs_encode_advertisement(phy_interface_t interface,
++					     const unsigned long *advertising);
++void phylink_decode_usxgmii_word(struct phylink_link_state *state,
++				 uint16_t lpa);
+ #endif
+diff --git a/include/uapi/linux/mdio.h b/include/uapi/linux/mdio.h
+index 4bcb41c..3f302e2 100644
+--- a/include/uapi/linux/mdio.h
++++ b/include/uapi/linux/mdio.h
+@@ -324,4 +324,30 @@ static inline __u16 mdio_phy_id_c45(int prtad, int devad)
+ 	return MDIO_PHY_ID_C45 | (prtad << 5) | devad;
+ }
+ 
++/* UsxgmiiChannelInfo[15:0] for USXGMII in-band auto-negotiation.*/
++#define MDIO_USXGMII_EEE_CLK_STP	0x0080	/* EEE clock stop supported */
++#define MDIO_USXGMII_EEE		0x0100	/* EEE supported */
++#define MDIO_USXGMII_SPD_MASK		0x0e00	/* USXGMII speed mask */
++#define MDIO_USXGMII_FULL_DUPLEX	0x1000	/* USXGMII full duplex */
++#define MDIO_USXGMII_DPX_SPD_MASK	0x1e00	/* USXGMII duplex and speed bits */
++#define MDIO_USXGMII_10			0x0000	/* 10Mbps */
++#define MDIO_USXGMII_10HALF		0x0000	/* 10Mbps half-duplex */
++#define MDIO_USXGMII_10FULL		0x1000	/* 10Mbps full-duplex */
++#define MDIO_USXGMII_100		0x0200	/* 100Mbps */
++#define MDIO_USXGMII_100HALF		0x0200	/* 100Mbps half-duplex */
++#define MDIO_USXGMII_100FULL		0x1200	/* 100Mbps full-duplex */
++#define MDIO_USXGMII_1000		0x0400	/* 1000Mbps */
++#define MDIO_USXGMII_1000HALF		0x0400	/* 1000Mbps half-duplex */
++#define MDIO_USXGMII_1000FULL		0x1400	/* 1000Mbps full-duplex */
++#define MDIO_USXGMII_10G		0x0600	/* 10Gbps */
++#define MDIO_USXGMII_10GHALF		0x0600	/* 10Gbps half-duplex */
++#define MDIO_USXGMII_10GFULL		0x1600	/* 10Gbps full-duplex */
++#define MDIO_USXGMII_2500		0x0800	/* 2500Mbps */
++#define MDIO_USXGMII_2500HALF		0x0800	/* 2500Mbps half-duplex */
++#define MDIO_USXGMII_2500FULL		0x1800	/* 2500Mbps full-duplex */
++#define MDIO_USXGMII_5000		0x0a00	/* 5000Mbps */
++#define MDIO_USXGMII_5000HALF		0x0a00	/* 5000Mbps half-duplex */
++#define MDIO_USXGMII_5000FULL		0x1a00	/* 5000Mbps full-duplex */
++#define MDIO_USXGMII_LINK		0x8000	/* PHY link with copper-side partner */
++
+ #endif /* _UAPI__LINUX_MDIO_H__ */
+diff --git a/include/uapi/linux/mii.h b/include/uapi/linux/mii.h
+index 51b48e4..90f9b4e 100644
+--- a/include/uapi/linux/mii.h
++++ b/include/uapi/linux/mii.h
+@@ -131,6 +131,23 @@
+ #define NWAYTEST_LOOPBACK	0x0100	/* Enable loopback for N-way   */
+ #define NWAYTEST_RESV2		0xfe00	/* Unused...                   */
+ 
++/* MAC and PHY tx_config_Reg[15:0] for SGMII in-band auto-negotiation.*/
++#define ADVERTISE_SGMII		0x0001	/* MAC can do SGMII            */
++#define LPA_SGMII		0x0001	/* PHY can do SGMII            */
++#define LPA_SGMII_SPD_MASK	0x0c00	/* SGMII speed mask            */
++#define LPA_SGMII_FULL_DUPLEX	0x1000	/* SGMII full duplex           */
++#define LPA_SGMII_DPX_SPD_MASK	0x1C00	/* SGMII duplex and speed bits */
++#define LPA_SGMII_10		0x0000	/* 10Mbps                      */
++#define LPA_SGMII_10HALF	0x0000	/* Can do 10mbps half-duplex   */
++#define LPA_SGMII_10FULL	0x1000	/* Can do 10mbps full-duplex   */
++#define LPA_SGMII_100		0x0400	/* 100Mbps                     */
++#define LPA_SGMII_100HALF	0x0400	/* Can do 100mbps half-duplex  */
++#define LPA_SGMII_100FULL	0x1400	/* Can do 100mbps full-duplex  */
++#define LPA_SGMII_1000		0x0800	/* 1000Mbps                    */
++#define LPA_SGMII_1000HALF	0x0800	/* Can do 1000mbps half-duplex */
++#define LPA_SGMII_1000FULL	0x1800	/* Can do 1000mbps full-duplex */
++#define LPA_SGMII_LINK		0x8000	/* PHY link with copper-side partner */
++
+ /* 1000BASE-T Control register */
+ #define ADVERTISE_1000FULL	0x0200  /* Advertise 1000BASE-T full duplex */
+ #define ADVERTISE_1000HALF	0x0100  /* Advertise 1000BASE-T half duplex */