diff --git a/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_eth_reset.c b/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_eth_reset.c
index f246e85..eebec74 100644
--- a/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_eth_reset.c
+++ b/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_eth_reset.c
@@ -209,9 +209,9 @@
 	if (MTK_HAS_CAPS(eth->soc->caps, MTK_NETSYS_V3)) {
 		mtk_dump_reg(eth, "XGMAC0", 0x12000, 0x300);
 		mtk_dump_reg(eth, "XGMAC1", 0x13000, 0x300);
-		mtk_dump_usxgmii(eth->xgmii->regmap_usxgmii[0],
+		mtk_dump_usxgmii(eth->usxgmii->pcs[0].regmap,
 			"USXGMII0", 0, 0x1000);
-		mtk_dump_usxgmii(eth->xgmii->regmap_usxgmii[1],
+		mtk_dump_usxgmii(eth->usxgmii->pcs[1].regmap,
 			"USXGMII1", 0, 0x1000);
 	}
 }
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 134e579..1368a7b 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
@@ -500,6 +500,34 @@
 		mtk_w32(eth, mcr, MTK_MAC_MCR(mac->id));
 }
 
+static struct phylink_pcs *mtk_mac_select_pcs(struct phylink_config *config,
+					      phy_interface_t interface)
+{
+	struct mtk_mac *mac = container_of(config, struct mtk_mac,
+					   phylink_config);
+	struct mtk_eth *eth = mac->hw;
+	unsigned int sid;
+
+	if (interface == PHY_INTERFACE_MODE_SGMII ||
+	    phy_interface_mode_is_8023z(interface)) {
+		sid = (MTK_HAS_CAPS(eth->soc->caps, MTK_SHARED_SGMII)) ?
+		       0 : mtk_mac2xgmii_id(eth, mac->id);
+
+		return mtk_sgmii_select_pcs(eth->sgmii, sid);
+	} else if (interface == PHY_INTERFACE_MODE_USXGMII ||
+		   interface == PHY_INTERFACE_MODE_10GKR ||
+		   interface == PHY_INTERFACE_MODE_5GBASER) {
+		if (MTK_HAS_CAPS(eth->soc->caps, MTK_NETSYS_V3) &&
+		    mac->id != MTK_GMAC1_ID) {
+			sid = mtk_mac2xgmii_id(eth, mac->id);
+
+			return mtk_usxgmii_select_pcs(eth->usxgmii, sid);
+		}
+	}
+
+	return NULL;
+}
+
 static void mtk_mac_config(struct phylink_config *config, unsigned int mode,
 			   const struct phylink_link_state *state)
 {
@@ -649,38 +677,13 @@
 		sid = (MTK_HAS_CAPS(eth->soc->caps, MTK_SHARED_SGMII)) ?
 		       0 : mac->id;
 
-		/* Setup SGMIISYS with the determined property */
-		if (state->interface != PHY_INTERFACE_MODE_SGMII)
-			err = mtk_sgmii_setup_mode_force(eth->xgmii, sid,
-							 state);
-		else
-			err = mtk_sgmii_setup_mode_an(eth->xgmii, sid);
-
-		if (err) {
-			spin_unlock(&eth->syscfg0_lock);
-			goto init_err;
-		}
-
-		regmap_update_bits(eth->ethsys, ETHSYS_SYSCFG0,
-				   SYSCFG0_SGMII_MASK, val);
+		/* Save the syscfg0 value for mac_finish */
+		mac->syscfg0 = val;
 		spin_unlock(&eth->syscfg0_lock);
 	} else if (state->interface == PHY_INTERFACE_MODE_USXGMII ||
 		   state->interface == PHY_INTERFACE_MODE_10GKR ||
 		   state->interface == PHY_INTERFACE_MODE_5GBASER) {
-		sid = mac->id;
-
-		if (MTK_HAS_CAPS(eth->soc->caps, MTK_NETSYS_V3) &&
-		    sid != MTK_GMAC1_ID) {
-			if (phylink_autoneg_inband(mode))
-				err = mtk_usxgmii_setup_mode_force(eth->xgmii, sid,
-								   state);
-			else
-				err = mtk_usxgmii_setup_mode_an(eth->xgmii, sid,
-								SPEED_10000);
-
-			if (err)
-				goto init_err;
-		}
+		/* Nothing to do */
 	} else if (phylink_autoneg_inband(mode)) {
 		dev_err(eth->dev,
 			"In-band mode not supported in non SGMII mode!\n");
@@ -730,6 +733,10 @@
 			}
 		}
 
+		/* FIXME: In current hardware design, we have to reset FE
+		 * when swtiching XGDM to GDM. Therefore, here trigger an SER
+		 * to let GDM go back to the initial state.
+		 */
 		if (mac->type != mac_type) {
 			if (atomic_read(&reset_pending) == 0) {
 				atomic_inc(&force);
@@ -752,6 +759,22 @@
 		mac->id, phy_modes(state->interface), err);
 }
 
+static int mtk_mac_finish(struct phylink_config *config, unsigned int mode,
+			  phy_interface_t interface)
+{
+	struct mtk_mac *mac = container_of(config, struct mtk_mac,
+					   phylink_config);
+	struct mtk_eth *eth = mac->hw;
+
+	/* Enable SGMII */
+	if (interface == PHY_INTERFACE_MODE_SGMII ||
+	    phy_interface_mode_is_8023z(interface))
+		regmap_update_bits(eth->ethsys, ETHSYS_SYSCFG0,
+				   SYSCFG0_SGMII_MASK, mac->syscfg0);
+
+	return 0;
+}
+
 static int mtk_mac_pcs_get_state(struct phylink_config *config,
 				 struct phylink_link_state *state)
 {
@@ -764,7 +787,7 @@
 		if (mac->id == MTK_GMAC2_ID)
 			sts = sts >> 16;
 
-		state->duplex = 1;
+		state->duplex = DUPLEX_FULL;
 
 		switch (FIELD_GET(MTK_USXGMII_PCS_MODE, sts)) {
 		case 0:
@@ -785,18 +808,19 @@
 		state->link = FIELD_GET(MTK_USXGMII_PCS_LINK, sts);
 	} else if (mac->type == MTK_GDM_TYPE) {
 		struct mtk_eth *eth = mac->hw;
-		struct mtk_xgmii *ss = eth->xgmii;
+		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 val = 0;
+		u32 rgc3, val = 0;
 
-		regmap_read(ss->regmap_sgmii[id], SGMSYS_PCS_CONTROL_1, &val);
+		regmap_read(ss->pcs[id].regmap, SGMSYS_PCS_CONTROL_1, &val);
 
 		state->interface = mac->interface;
 		state->link = FIELD_GET(SGMII_LINK_STATYS, val);
 
 		if (FIELD_GET(SGMII_AN_ENABLE, val)) {
-			regmap_read(ss->regmap_sgmii[id], SGMII_PCS_SPEED_ABILITY, &val);
+			regmap_read(ss->pcs[id].regmap,
+				    SGMII_PCS_SPEED_ABILITY, &val);
 
 			val = val >> 16;
 
@@ -814,9 +838,10 @@
 				break;
 			}
 		} else {
-			regmap_read(ss->regmap_sgmii[id], SGMSYS_SGMII_MODE, &val);
+			regmap_read(ss->pcs[id].regmap,
+				    SGMSYS_SGMII_MODE, &val);
 
-			state->duplex = !FIELD_GET(SGMII_DUPLEX_FULL, val);
+			state->duplex = !FIELD_GET(SGMII_DUPLEX_HALF, val);
 
 			switch (FIELD_GET(SGMII_SPEED_MASK, val)) {
 			case 0:
@@ -826,8 +851,10 @@
 				state->speed = SPEED_100;
 				break;
 			case 2:
-				regmap_read(ss->regmap_sgmii[id], ss->ana_rgc3, &val);
-				state->speed = (FIELD_GET(RG_PHY_SPEED_3_125G, val)) ? SPEED_2500 : SPEED_1000;
+				regmap_read(ss->pcs[id].regmap,
+					    ss->pcs[id].ana_rgc3, &val);
+				rgc3 = FIELD_GET(RG_PHY_SPEED_3_125G, val);
+				state->speed = rgc3 ? SPEED_2500 : SPEED_1000;
 				break;
 			}
 		}
@@ -842,15 +869,6 @@
 	return 1;
 }
 
-static void mtk_mac_an_restart(struct phylink_config *config)
-{
-	struct mtk_mac *mac = container_of(config, struct mtk_mac,
-					   phylink_config);
-
-	if (mac->type != MTK_XGDM_TYPE)
-		mtk_sgmii_restart_an(mac->hw, mac->id);
-}
-
 static void mtk_mac_link_down(struct phylink_config *config, unsigned int mode,
 			      phy_interface_t interface)
 {
@@ -1074,9 +1092,10 @@
 
 static const struct phylink_mac_ops mtk_phylink_ops = {
 	.validate = mtk_validate,
+	.mac_select_pcs = mtk_mac_select_pcs,
 	.mac_link_state = mtk_mac_pcs_get_state,
-	.mac_an_restart = mtk_mac_an_restart,
 	.mac_config = mtk_mac_config,
+	.mac_finish = mtk_mac_finish,
 	.mac_link_down = mtk_mac_link_down,
 	.mac_link_up = mtk_mac_link_up,
 };
@@ -3397,6 +3416,7 @@
 	struct mtk_mac *mac = netdev_priv(dev);
 	struct mtk_eth *eth = mac->hw;
 	struct mtk_phylink_priv *phylink_priv = &mac->phylink_priv;
+	u32 id = mtk_mac2xgmii_id(eth, mac->id);
 	int err, i;
 	struct device_node *phy_node;
 
@@ -3472,8 +3492,9 @@
 	phylink_start(mac->phylink);
 	netif_start_queue(dev);
 	phy_node = of_parse_phandle(mac->of_node, "phy-handle", 0);
-	if (!phy_node && eth->xgmii->regmap_sgmii[mac->id])
-		regmap_write(eth->xgmii->regmap_sgmii[mac->id], SGMSYS_QPHY_PWR_STATE_CTRL, 0);
+	if (!phy_node && eth->sgmii->pcs[id].regmap)
+		regmap_write(eth->sgmii->pcs[id].regmap,
+			     SGMSYS_QPHY_PWR_STATE_CTRL, 0);
 
 	mtk_gdm_config(eth, mac->id, MTK_GDMA_TO_PDMA);
 
@@ -3508,6 +3529,7 @@
 	struct mtk_mac *mac = netdev_priv(dev);
 	struct mtk_eth *eth = mac->hw;
 	int i;
+	u32 id = mtk_mac2xgmii_id(eth, mac->id);
 	u32 val = 0;
 	struct device_node *phy_node;
 
@@ -3515,10 +3537,12 @@
 	netif_tx_disable(dev);
 
 	phy_node = of_parse_phandle(mac->of_node, "phy-handle", 0);
-	if (!phy_node && eth->xgmii->regmap_sgmii[mac->id]) {
-		regmap_read(eth->xgmii->regmap_sgmii[mac->id], SGMSYS_QPHY_PWR_STATE_CTRL, &val);
+	if (!phy_node && eth->sgmii->pcs[id].regmap) {
+		regmap_read(eth->sgmii->pcs[id].regmap,
+			    SGMSYS_QPHY_PWR_STATE_CTRL, &val);
 		val |= SGMII_PHYA_PWD;
-		regmap_write(eth->xgmii->regmap_sgmii[mac->id], SGMSYS_QPHY_PWR_STATE_CTRL, val);
+		regmap_write(eth->sgmii->pcs[id].regmap,
+			     SGMSYS_QPHY_PWR_STATE_CTRL, val);
 	}
 
 	//GMAC RX disable
@@ -4589,29 +4613,24 @@
 	}
 
 	if (MTK_HAS_CAPS(eth->soc->caps, MTK_SGMII)) {
-		eth->xgmii = devm_kzalloc(eth->dev, sizeof(*eth->xgmii),
+		eth->sgmii = devm_kzalloc(eth->dev, sizeof(*eth->sgmii),
 					  GFP_KERNEL);
-		if (!eth->xgmii)
+		if (!eth->sgmii)
 			return -ENOMEM;
 
-		eth->xgmii->eth = eth;
-		err = mtk_sgmii_init(eth->xgmii, pdev->dev.of_node,
+		err = mtk_sgmii_init(eth, pdev->dev.of_node,
 				     eth->soc->ana_rgc3);
-
 		if (err)
 			return err;
 	}
 
 	if (MTK_HAS_CAPS(eth->soc->caps, MTK_USXGMII)) {
-		err = mtk_usxgmii_init(eth->xgmii, pdev->dev.of_node);
-		if (err)
-			return err;
-
-		err = mtk_xfi_pextp_init(eth->xgmii, pdev->dev.of_node);
-		if (err)
-			return err;
+		eth->usxgmii = devm_kzalloc(eth->dev, sizeof(*eth->usxgmii),
+					    GFP_KERNEL);
+		if (!eth->usxgmii)
+			return -ENOMEM;
 
-		err = mtk_xfi_pll_init(eth->xgmii, pdev->dev.of_node);
+		err = mtk_usxgmii_init(eth, pdev->dev.of_node);
 		if (err)
 			return err;
 
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 6035f46..646e557 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
@@ -852,7 +852,7 @@
 #define SGMII_SPEED_10			0x0
 #define SGMII_SPEED_100			BIT(2)
 #define SGMII_SPEED_1000		BIT(3)
-#define SGMII_DUPLEX_FULL		BIT(4)
+#define SGMII_DUPLEX_HALF		BIT(4)
 #define SGMII_IF_MODE_BIT5		BIT(5)
 #define SGMII_REMOTE_FAULT_DIS		BIT(8)
 #define SGMII_CODE_SYNC_SET_VAL		BIT(9)
@@ -902,7 +902,21 @@
 
 /* Register to control PCS AN */
 #define RG_PCS_AN_CTRL0		0x810
-#define RG_AN_ENABLE		BIT(0)
+#define USXGMII_AN_RESTART	BIT(31)
+#define USXGMII_AN_ENABLE	BIT(0)
+
+/* Register to control PCS AN */
+#define RG_PCS_AN_STS0		0x81C
+#define USXGMII_LPA_SPEED_MASK	GENMASK(11, 9)
+#define USXGMII_LPA_SPEED_10	0
+#define USXGMII_LPA_SPEED_100	1
+#define USXGMII_LPA_SPEED_1000	2
+#define USXGMII_LPA_SPEED_10000	3
+#define USXGMII_LPA_SPEED_2500	4
+#define USXGMII_LPA_SPEED_5000	5
+#define USXGMII_LPA_DUPLEX	BIT(12)
+#define USXGMII_LPA_LINK	BIT(15)
+#define USXGMII_LPA_LATCH	BIT(31)
 
 /* Register to control USXGMII XFI PLL digital */
 #define XFI_PLL_DIG_GLB8	0x08
@@ -1610,24 +1624,66 @@
 #define MTK_SGMII_PN_SWAP	       BIT(16)
 #define MTK_HAS_FLAGS(flags, _x)       (((flags) & (_x)) == (_x))
 
-/* struct mtk_xgmii -  This is the structure holding sgmii/usxgmii regmap and
- *		       its characteristics
- * @regmap:            The register map pointing at the range used to setup
- *                     SGMII/USXGMII modes
- * @flags:             The enum refers to which mode the sgmii wants to run on
- * @ana_rgc3:          The offset refers to register ANA_RGC3 related to regmap
+/* struct mtk_sgmii_pcs - This structure holds each sgmii regmap and associated
+ *			data
+ * @regmap:		The register map pointing at the range used to setup
+ *			SGMII modes
+ * @regmap_pextp:	The register map pointing at the range used to setup
+ *			PHYA
+ * @ana_rgc3:		The offset refers to register ANA_RGC3 related to regmap
+ * @id:			The element is used to record the index of PCS
+ * @pcs:		Phylink PCS structure
  */
+struct mtk_sgmii_pcs {
+	struct mtk_eth		*eth;
+	struct regmap		*regmap;
+	struct regmap		*regmap_pextp;
+	phy_interface_t		interface;
+	u32			flags;
+	u32			ana_rgc3;
+	u8			id;
+	struct phylink_pcs	pcs;
+};
 
-struct mtk_xgmii {
-	struct mtk_eth	*eth;
-	struct regmap   *regmap_sgmii[MTK_MAX_DEVS];
-	struct regmap   *regmap_usxgmii[MTK_MAX_DEVS];
-	struct regmap   *regmap_pextp[MTK_MAX_DEVS];
-	struct regmap	*regmap_pll;
-	u32             flags[MTK_MAX_DEVS];
-	u32             ana_rgc3;
+/* struct mtk_sgmii -	This is the structure holding sgmii regmap and its
+ *			characteristics
+ * @pll:		The register map pointing at the range used to setup
+ *			PLL
+ * @pcs			Array of individual PCS structures
+ */
+struct mtk_sgmii {
+	struct mtk_sgmii_pcs	pcs[MTK_MAX_DEVS];
+	struct regmap		*pll;
 };
 
+/* struct mtk_usxgmii_pcs - This structure holds each usxgmii regmap and
+ *			associated data
+ * @regmap:		The register map pointing at the range used to setup
+ *			USXGMII modes
+ * @regmap_pextp:	The register map pointing at the range used to setup
+ *			PHYA
+ * @id:			The element is used to record the index of PCS
+ * @pcs:		Phylink PCS structure
+ */
+struct mtk_usxgmii_pcs {
+	struct mtk_eth		*eth;
+	struct regmap		*regmap;
+	struct regmap		*regmap_pextp;
+	phy_interface_t		interface;
+	u8			id;
+	struct phylink_pcs	pcs;
+};
+
+/* struct mtk_usxgmii -	This is the structure holding usxgmii regmap and its
+ *			characteristics
+ * @pll:		The register map pointing at the range used to setup
+ *			PLL
+ * @pcs			Array of individual PCS structures
+ */
+struct mtk_usxgmii {
+	struct mtk_usxgmii_pcs	pcs[MTK_MAX_DEVS];
+	struct regmap		*pll;
+};
 
 /* struct mtk_reset_event - This is the structure holding statistics counters
  *			for reset events
@@ -1705,7 +1761,8 @@
 	struct regmap			*ethsys;
 	struct regmap                   *infra;
 	struct regmap                   *toprgu;
-	struct mtk_xgmii                *xgmii;
+	struct mtk_sgmii		*sgmii;
+	struct mtk_usxgmii		*usxgmii;
 	struct regmap			*pctl;
 	bool				hwlro;
 	refcount_t			dma_refcnt;
@@ -1754,6 +1811,7 @@
 	struct mtk_hw_stats		*hw_stats;
 	__be32				hwlro_ip[MTK_MAX_LRO_IP_CNT];
 	int				hwlro_ip_cnt;
+	unsigned int			syscfg0;
 	bool				tx_lpi_enabled;
 	u32				tx_lpi_timer;
 };
@@ -1770,13 +1828,9 @@
 u32 mtk_r32(struct mtk_eth *eth, unsigned reg);
 u32 mtk_m32(struct mtk_eth *eth, u32 mask, u32 set, unsigned reg);
 
-int mtk_sgmii_init(struct mtk_xgmii *ss, struct device_node *np,
+struct phylink_pcs *mtk_sgmii_select_pcs(struct mtk_sgmii *ss, int id);
+int mtk_sgmii_init(struct mtk_eth *eth, struct device_node *np,
 		   u32 ana_rgc3);
-int mtk_sgmii_setup_mode_an(struct mtk_xgmii *ss, unsigned int mac_id);
-int mtk_sgmii_setup_mode_force(struct mtk_xgmii *ss, unsigned int mac_id,
-			       const struct phylink_link_state *state);
-void mtk_sgmii_restart_an(struct mtk_eth *eth, int mac_id);
-void mtk_sgmii_setup_phya_gen2(struct mtk_xgmii *ss, int mac_id);
 
 int mtk_gmac_sgmii_path_setup(struct mtk_eth *eth, int mac_id);
 int mtk_gmac_xgmii_path_setup(struct mtk_eth *eth, int mac_id);
@@ -1787,17 +1841,9 @@
 void ethsys_reset(struct mtk_eth *eth, u32 reset_bits);
 
 int mtk_mac2xgmii_id(struct mtk_eth *eth, int mac_id);
-int mtk_usxgmii_init(struct mtk_xgmii *ss, struct device_node *r);
-int mtk_xfi_pextp_init(struct mtk_xgmii *ss, struct device_node *r);
-int mtk_xfi_pll_init(struct mtk_xgmii *ss, struct device_node *r);
+struct phylink_pcs *mtk_usxgmii_select_pcs(struct mtk_usxgmii *ss, int id);
+int mtk_usxgmii_init(struct mtk_eth *eth, struct device_node *r);
 int mtk_toprgu_init(struct mtk_eth *eth, struct device_node *r);
-int mtk_xfi_pll_enable(struct mtk_xgmii *ss);
-int mtk_usxgmii_setup_mode_an(struct mtk_xgmii *ss, int mac_id,
-			      int max_speed);
-int mtk_usxgmii_setup_mode_force(struct mtk_xgmii *ss, int mac_id,
-				 const struct phylink_link_state *state);
-void mtk_usxgmii_setup_phya_an_10000(struct mtk_xgmii *ss, int mac_id);
-void mtk_usxgmii_reset(struct mtk_xgmii *ss, int mac_id);
 int mtk_dump_usxgmii(struct regmap *pmap, char *name, u32 offset, u32 range);
 
 void mtk_eth_set_dma_device(struct mtk_eth *eth, struct device *dma_dev);
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 5d792ae..3c37656 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
@@ -13,52 +13,85 @@
 
 #include "mtk_eth_soc.h"
 
-int mtk_sgmii_init(struct mtk_xgmii *ss, struct device_node *r, u32 ana_rgc3)
+static struct mtk_sgmii_pcs *pcs_to_mtk_sgmii_pcs(struct phylink_pcs *pcs)
+{
+	return container_of(pcs, struct mtk_sgmii_pcs, pcs);
+}
+
+static int mtk_sgmii_xfi_pextp_init(struct mtk_sgmii *ss, struct device_node *r)
 {
 	struct device_node *np;
 	int i;
 
-	ss->ana_rgc3 = ana_rgc3;
-
 	for (i = 0; i < MTK_MAX_DEVS; i++) {
-		np = of_parse_phandle(r, "mediatek,sgmiisys", i);
+		np = of_parse_phandle(r, "mediatek,xfi_pextp", i);
 		if (!np)
 			break;
 
-		ss->regmap_sgmii[i] = syscon_node_to_regmap(np);
-		if (IS_ERR(ss->regmap_sgmii[i]))
-			return PTR_ERR(ss->regmap_sgmii[i]);
+		ss->pcs[i].regmap_pextp = syscon_node_to_regmap(np);
+		if (IS_ERR(ss->pcs[i].regmap_pextp))
+			return PTR_ERR(ss->pcs[i].regmap_pextp);
 
-		ss->flags[i] &= ~(MTK_SGMII_PN_SWAP);
-		if (of_property_read_bool(np, "pn_swap"))
-			ss->flags[i] |= MTK_SGMII_PN_SWAP;
+		of_node_put(np);
 	}
 
 	return 0;
 }
 
+static int mtk_sgmii_xfi_pll_init(struct mtk_sgmii *ss, struct device_node *r)
+{
+	struct device_node *np;
+
+	np = of_parse_phandle(r, "mediatek,xfi_pll", 0);
+	if (!np)
+		return -1;
+
+	ss->pll = syscon_node_to_regmap(np);
+	if (IS_ERR(ss->pll))
+		return PTR_ERR(ss->pll);
+
+	of_node_put(np);
+
+	return 0;
+}
+
-void mtk_sgmii_reset(struct mtk_xgmii *ss, int mac_id)
+static int mtk_sgmii_xfi_pll_enable(struct mtk_sgmii *ss)
 {
-	struct mtk_eth *eth = ss->eth;
-	u32 id = mtk_mac2xgmii_id(eth, mac_id);
 	u32 val = 0;
 
-	if (id >= MTK_MAX_DEVS || !eth->toprgu)
+	if (!ss->pll)
+		return -EINVAL;
+
+	/* Add software workaround for USXGMII PLL TCL issue */
+	regmap_write(ss->pll, XFI_PLL_ANA_GLB8, RG_XFI_PLL_ANA_SWWA);
+
+	regmap_read(ss->pll, XFI_PLL_DIG_GLB8, &val);
+	val |= RG_XFI_PLL_EN;
+	regmap_write(ss->pll, XFI_PLL_DIG_GLB8, val);
+
+	return 0;
+}
+
+void mtk_sgmii_reset(struct mtk_eth *eth, int id)
+{
+	u32 val = 0;
+
+	if (!eth->toprgu)
 		return;
 
-	switch (mac_id) {
-	case MTK_GMAC2_ID:
+	switch (id) {
+	case 0:
 		/* Enable software reset */
 		regmap_read(eth->toprgu, TOPRGU_SWSYSRST_EN, &val);
-		val |= SWSYSRST_XFI_PEXPT1_GRST |
-		       SWSYSRST_SGMII1_GRST;
+		val |= SWSYSRST_XFI_PEXPT0_GRST |
+		       SWSYSRST_SGMII0_GRST;
 		regmap_write(eth->toprgu, TOPRGU_SWSYSRST_EN, val);
 
 		/* Assert SGMII reset */
 		regmap_read(eth->toprgu, TOPRGU_SWSYSRST, &val);
 		val |= FIELD_PREP(SWSYSRST_UNLOCK_KEY, 0x88) |
-		       SWSYSRST_XFI_PEXPT1_GRST |
-		       SWSYSRST_SGMII1_GRST;
+		       SWSYSRST_XFI_PEXPT0_GRST |
+		       SWSYSRST_SGMII0_GRST;
 		regmap_write(eth->toprgu, TOPRGU_SWSYSRST, val);
 
 		udelay(100);
@@ -66,28 +99,28 @@
 		/* De-assert SGMII reset */
 		regmap_read(eth->toprgu, TOPRGU_SWSYSRST, &val);
 		val |= FIELD_PREP(SWSYSRST_UNLOCK_KEY, 0x88);
-		val &= ~(SWSYSRST_XFI_PEXPT1_GRST |
-			 SWSYSRST_SGMII1_GRST);
+		val &= ~(SWSYSRST_XFI_PEXPT0_GRST |
+			 SWSYSRST_SGMII0_GRST);
 		regmap_write(eth->toprgu, TOPRGU_SWSYSRST, val);
 
 		/* Disable software reset */
 		regmap_read(eth->toprgu, TOPRGU_SWSYSRST_EN, &val);
-		val &= ~(SWSYSRST_XFI_PEXPT1_GRST |
-			 SWSYSRST_SGMII1_GRST);
+		val &= ~(SWSYSRST_XFI_PEXPT0_GRST |
+			 SWSYSRST_SGMII0_GRST);
 		regmap_write(eth->toprgu, TOPRGU_SWSYSRST_EN, val);
 		break;
-	case MTK_GMAC3_ID:
-		/* Enable Software reset */
+	case 1:
+		/* Enable software reset */
 		regmap_read(eth->toprgu, TOPRGU_SWSYSRST_EN, &val);
-		val |= SWSYSRST_XFI_PEXPT0_GRST |
-		       SWSYSRST_SGMII0_GRST;
+		val |= SWSYSRST_XFI_PEXPT1_GRST |
+		       SWSYSRST_SGMII1_GRST;
 		regmap_write(eth->toprgu, TOPRGU_SWSYSRST_EN, val);
 
 		/* Assert SGMII reset */
 		regmap_read(eth->toprgu, TOPRGU_SWSYSRST, &val);
 		val |= FIELD_PREP(SWSYSRST_UNLOCK_KEY, 0x88) |
-		       SWSYSRST_XFI_PEXPT0_GRST |
-		       SWSYSRST_SGMII0_GRST;
+		       SWSYSRST_XFI_PEXPT1_GRST |
+		       SWSYSRST_SGMII1_GRST;
 		regmap_write(eth->toprgu, TOPRGU_SWSYSRST, val);
 
 		udelay(100);
@@ -95,14 +128,14 @@
 		/* De-assert SGMII reset */
 		regmap_read(eth->toprgu, TOPRGU_SWSYSRST, &val);
 		val |= FIELD_PREP(SWSYSRST_UNLOCK_KEY, 0x88);
-		val &= ~(SWSYSRST_XFI_PEXPT0_GRST |
-			 SWSYSRST_SGMII0_GRST);
+		val &= ~(SWSYSRST_XFI_PEXPT1_GRST |
+			 SWSYSRST_SGMII1_GRST);
 		regmap_write(eth->toprgu, TOPRGU_SWSYSRST, val);
 
 		/* Disable software reset */
 		regmap_read(eth->toprgu, TOPRGU_SWSYSRST_EN, &val);
-		val &= ~(SWSYSRST_XFI_PEXPT0_GRST |
-			 SWSYSRST_SGMII0_GRST);
+		val &= ~(SWSYSRST_XFI_PEXPT1_GRST |
+			 SWSYSRST_SGMII1_GRST);
 		regmap_write(eth->toprgu, TOPRGU_SWSYSRST_EN, val);
 		break;
 	}
@@ -110,18 +143,19 @@
 	mdelay(1);
 }
 
-int mtk_sgmii_need_powerdown(struct mtk_xgmii *ss, int id, int speed)
+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(ss->regmap_sgmii[id], SGMSYS_PCS_CONTROL_1, &val);
+	regmap_read(mpcs->regmap, SGMSYS_PCS_CONTROL_1, &val);
 	if (!(val & SGMII_LINK_STATYS))
 		return true;
 
-	/* need to power down sgmii if link speed changed */
-	regmap_read(ss->regmap_sgmii[id], ss->ana_rgc3, &val);
-	if (speed == SPEED_2500) {
+	/* 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 {
@@ -132,272 +166,469 @@
 	return false;
 }
 
-void mtk_sgmii_setup_phya_gen1(struct mtk_xgmii *ss, int mac_id)
+void mtk_sgmii_setup_phya_gen1(struct mtk_sgmii_pcs *mpcs)
 {
-	u32 id = mtk_mac2xgmii_id(ss->eth, mac_id);
-
-	if (id >= MTK_MAX_DEVS ||
-	    !ss->regmap_sgmii[id] || !ss->regmap_pextp[id])
+	if (!mpcs->regmap_pextp)
 		return;
 
-	regmap_update_bits(ss->regmap_pextp[id], 0x9024, GENMASK(31, 0), 0x00D9071C);
-	regmap_update_bits(ss->regmap_pextp[id], 0x2020, GENMASK(31, 0), 0xAA8585AA);
-	regmap_update_bits(ss->regmap_pextp[id], 0x2030, GENMASK(31, 0), 0x0C020207);
-	regmap_update_bits(ss->regmap_pextp[id], 0x2034, GENMASK(31, 0), 0x0E05050F);
-	regmap_update_bits(ss->regmap_pextp[id], 0x2040, GENMASK(31, 0), 0x00200032);
-	regmap_update_bits(ss->regmap_pextp[id], 0x50F0, GENMASK(31, 0), 0x00C014BA);
-	regmap_update_bits(ss->regmap_pextp[id], 0x50E0, GENMASK(31, 0), 0x3777C12B);
-	regmap_update_bits(ss->regmap_pextp[id], 0x506C, GENMASK(31, 0), 0x005F9CFF);
-	regmap_update_bits(ss->regmap_pextp[id], 0x5070, GENMASK(31, 0), 0x9D9DFAFA);
-	regmap_update_bits(ss->regmap_pextp[id], 0x5074, GENMASK(31, 0), 0x27273F3F);
-	regmap_update_bits(ss->regmap_pextp[id], 0x5078, GENMASK(31, 0), 0xA7883C68);
-	regmap_update_bits(ss->regmap_pextp[id], 0x507C, GENMASK(31, 0), 0x11661166);
-	regmap_update_bits(ss->regmap_pextp[id], 0x5080, GENMASK(31, 0), 0x0E000EAF);
-	regmap_update_bits(ss->regmap_pextp[id], 0x5084, GENMASK(31, 0), 0x08080E0D);
-	regmap_update_bits(ss->regmap_pextp[id], 0x5088, GENMASK(31, 0), 0x02030B09);
-	regmap_update_bits(ss->regmap_pextp[id], 0x50E4, GENMASK(31, 0), 0x0C0C0000);
-	regmap_update_bits(ss->regmap_pextp[id], 0x50E8, GENMASK(31, 0), 0x04040000);
-	regmap_update_bits(ss->regmap_pextp[id], 0x50EC, GENMASK(31, 0), 0x0F0F0606);
-	regmap_update_bits(ss->regmap_pextp[id], 0x50A8, GENMASK(31, 0), 0x506E8C8C);
-	regmap_update_bits(ss->regmap_pextp[id], 0x6004, GENMASK(31, 0), 0x18190000);
-	regmap_update_bits(ss->regmap_pextp[id], 0x00F8, GENMASK(31, 0), 0x00FA32FA);
-	regmap_update_bits(ss->regmap_pextp[id], 0x00F4, GENMASK(31, 0), 0x80201F21);
-	regmap_update_bits(ss->regmap_pextp[id], 0x0030, GENMASK(31, 0), 0x00050C00);
-	regmap_update_bits(ss->regmap_pextp[id], 0x0070, GENMASK(31, 0), 0x02002800);
+	regmap_update_bits(mpcs->regmap_pextp, 0x9024, GENMASK(31, 0),
+			   0x00D9071C);
+	regmap_update_bits(mpcs->regmap_pextp, 0x2020, GENMASK(31, 0),
+			   0xAA8585AA);
+	regmap_update_bits(mpcs->regmap_pextp, 0x2030, GENMASK(31, 0),
+			   0x0C020207);
+	regmap_update_bits(mpcs->regmap_pextp, 0x2034, GENMASK(31, 0),
+			   0x0E05050F);
+	regmap_update_bits(mpcs->regmap_pextp, 0x2040, GENMASK(31, 0),
+			   0x00200032);
+	regmap_update_bits(mpcs->regmap_pextp, 0x50F0, GENMASK(31, 0),
+			   0x00C014BA);
+	regmap_update_bits(mpcs->regmap_pextp, 0x50E0, GENMASK(31, 0),
+			   0x3777C12B);
+	regmap_update_bits(mpcs->regmap_pextp, 0x506C, GENMASK(31, 0),
+			   0x005F9CFF);
+	regmap_update_bits(mpcs->regmap_pextp, 0x5070, GENMASK(31, 0),
+			   0x9D9DFAFA);
+	regmap_update_bits(mpcs->regmap_pextp, 0x5074, GENMASK(31, 0),
+			   0x27273F3F);
+	regmap_update_bits(mpcs->regmap_pextp, 0x5078, GENMASK(31, 0),
+			   0xA7883C68);
+	regmap_update_bits(mpcs->regmap_pextp, 0x507C, GENMASK(31, 0),
+			   0x11661166);
+	regmap_update_bits(mpcs->regmap_pextp, 0x5080, GENMASK(31, 0),
+			   0x0E000EAF);
+	regmap_update_bits(mpcs->regmap_pextp, 0x5084, GENMASK(31, 0),
+			   0x08080E0D);
+	regmap_update_bits(mpcs->regmap_pextp, 0x5088, GENMASK(31, 0),
+			   0x02030B09);
+	regmap_update_bits(mpcs->regmap_pextp, 0x50E4, GENMASK(31, 0),
+			   0x0C0C0000);
+	regmap_update_bits(mpcs->regmap_pextp, 0x50E8, GENMASK(31, 0),
+			   0x04040000);
+	regmap_update_bits(mpcs->regmap_pextp, 0x50EC, GENMASK(31, 0),
+			   0x0F0F0606);
+	regmap_update_bits(mpcs->regmap_pextp, 0x50A8, GENMASK(31, 0),
+			   0x506E8C8C);
+	regmap_update_bits(mpcs->regmap_pextp, 0x6004, GENMASK(31, 0),
+			   0x18190000);
+	regmap_update_bits(mpcs->regmap_pextp, 0x00F8, GENMASK(31, 0),
+			   0x00FA32FA);
+	regmap_update_bits(mpcs->regmap_pextp, 0x00F4, GENMASK(31, 0),
+			   0x80201F21);
+	regmap_update_bits(mpcs->regmap_pextp, 0x0030, GENMASK(31, 0),
+			   0x00050C00);
+	regmap_update_bits(mpcs->regmap_pextp, 0x0070, GENMASK(31, 0),
+			   0x02002800);
 	ndelay(1020);
-	regmap_update_bits(ss->regmap_pextp[id], 0x30B0, GENMASK(31, 0), 0x00000020);
-	regmap_update_bits(ss->regmap_pextp[id], 0x3028, GENMASK(31, 0), 0x00008A01);
-	regmap_update_bits(ss->regmap_pextp[id], 0x302C, GENMASK(31, 0), 0x0000A884);
-	regmap_update_bits(ss->regmap_pextp[id], 0x3024, GENMASK(31, 0), 0x00083002);
-	regmap_update_bits(ss->regmap_pextp[id], 0x3010, GENMASK(31, 0), 0x00011110);
-	regmap_update_bits(ss->regmap_pextp[id], 0x3048, GENMASK(31, 0), 0x40704000);
-	regmap_update_bits(ss->regmap_pextp[id], 0x3064, GENMASK(31, 0), 0x0000C000);
-	regmap_update_bits(ss->regmap_pextp[id], 0x3050, GENMASK(31, 0), 0xA8000000);
-	regmap_update_bits(ss->regmap_pextp[id], 0x3054, GENMASK(31, 0), 0x000000AA);
-	regmap_update_bits(ss->regmap_pextp[id], 0x306C, GENMASK(31, 0), 0x20200F00);
-	regmap_update_bits(ss->regmap_pextp[id], 0xA060, GENMASK(31, 0), 0x00050000);
-	regmap_update_bits(ss->regmap_pextp[id], 0x90D0, GENMASK(31, 0), 0x00000007);
-	regmap_update_bits(ss->regmap_pextp[id], 0x0070, GENMASK(31, 0), 0x0200E800);
+	regmap_update_bits(mpcs->regmap_pextp, 0x30B0, GENMASK(31, 0),
+			   0x00000020);
+	regmap_update_bits(mpcs->regmap_pextp, 0x3028, GENMASK(31, 0),
+			   0x00008A01);
+	regmap_update_bits(mpcs->regmap_pextp, 0x302C, GENMASK(31, 0),
+			   0x0000A884);
+	regmap_update_bits(mpcs->regmap_pextp, 0x3024, GENMASK(31, 0),
+			   0x00083002);
+	regmap_update_bits(mpcs->regmap_pextp, 0x3010, GENMASK(31, 0),
+			   0x00011110);
+	regmap_update_bits(mpcs->regmap_pextp, 0x3048, GENMASK(31, 0),
+			   0x40704000);
+	regmap_update_bits(mpcs->regmap_pextp, 0x3064, GENMASK(31, 0),
+			   0x0000C000);
+	regmap_update_bits(mpcs->regmap_pextp, 0x3050, GENMASK(31, 0),
+			   0xA8000000);
+	regmap_update_bits(mpcs->regmap_pextp, 0x3054, GENMASK(31, 0),
+			   0x000000AA);
+	regmap_update_bits(mpcs->regmap_pextp, 0x306C, GENMASK(31, 0),
+			   0x20200F00);
+	regmap_update_bits(mpcs->regmap_pextp, 0xA060, GENMASK(31, 0),
+			   0x00050000);
+	regmap_update_bits(mpcs->regmap_pextp, 0x90D0, GENMASK(31, 0),
+			   0x00000007);
+	regmap_update_bits(mpcs->regmap_pextp, 0x0070, GENMASK(31, 0),
+			   0x0200E800);
 	udelay(150);
-	regmap_update_bits(ss->regmap_pextp[id], 0x0070, GENMASK(31, 0), 0x0200C111);
+	regmap_update_bits(mpcs->regmap_pextp, 0x0070, GENMASK(31, 0),
+			   0x0200C111);
 	ndelay(1020);
-	regmap_update_bits(ss->regmap_pextp[id], 0x0070, GENMASK(31, 0), 0x0200C101);
+	regmap_update_bits(mpcs->regmap_pextp, 0x0070, GENMASK(31, 0),
+			   0x0200C101);
 	udelay(15);
-	regmap_update_bits(ss->regmap_pextp[id], 0x0070, GENMASK(31, 0), 0x0201C111);
+	regmap_update_bits(mpcs->regmap_pextp, 0x0070, GENMASK(31, 0),
+			   0x0201C111);
 	ndelay(1020);
-	regmap_update_bits(ss->regmap_pextp[id], 0x0070, GENMASK(31, 0), 0x0201C101);
+	regmap_update_bits(mpcs->regmap_pextp, 0x0070, GENMASK(31, 0),
+			   0x0201C101);
 	udelay(100);
-	regmap_update_bits(ss->regmap_pextp[id], 0x30B0, GENMASK(31, 0), 0x00000030);
-	regmap_update_bits(ss->regmap_pextp[id], 0x00F4, GENMASK(31, 0), 0x80201F01);
-	regmap_update_bits(ss->regmap_pextp[id], 0x3040, GENMASK(31, 0), 0x30000000);
+	regmap_update_bits(mpcs->regmap_pextp, 0x30B0, GENMASK(31, 0),
+			   0x00000030);
+	regmap_update_bits(mpcs->regmap_pextp, 0x00F4, GENMASK(31, 0),
+			   0x80201F01);
+	regmap_update_bits(mpcs->regmap_pextp, 0x3040, GENMASK(31, 0),
+			   0x30000000);
 	udelay(400);
 }
 
-void mtk_sgmii_setup_phya_gen2(struct mtk_xgmii *ss, int mac_id)
+void mtk_sgmii_setup_phya_gen2(struct mtk_sgmii_pcs *mpcs)
 {
-	u32 id = mtk_mac2xgmii_id(ss->eth, mac_id);
-
-	if (id >= MTK_MAX_DEVS ||
-	    !ss->regmap_sgmii[id] || !ss->regmap_pextp[id])
+	if (!mpcs->regmap_pextp)
 		return;
 
-	regmap_update_bits(ss->regmap_pextp[id], 0x9024, GENMASK(31, 0), 0x00D9071C);
-	regmap_update_bits(ss->regmap_pextp[id], 0x2020, GENMASK(31, 0), 0xAA8585AA);
-	regmap_update_bits(ss->regmap_pextp[id], 0x2030, GENMASK(31, 0), 0x0C020707);
-	regmap_update_bits(ss->regmap_pextp[id], 0x2034, GENMASK(31, 0), 0x0E050F0F);
-	regmap_update_bits(ss->regmap_pextp[id], 0x2040, GENMASK(31, 0), 0x00140032);
-	regmap_update_bits(ss->regmap_pextp[id], 0x50F0, GENMASK(31, 0), 0x00C014AA);
-	regmap_update_bits(ss->regmap_pextp[id], 0x50E0, GENMASK(31, 0), 0x3777C12B);
-	regmap_update_bits(ss->regmap_pextp[id], 0x506C, GENMASK(31, 0), 0x005F9CFF);
-	regmap_update_bits(ss->regmap_pextp[id], 0x5070, GENMASK(31, 0), 0x9D9DFAFA);
-	regmap_update_bits(ss->regmap_pextp[id], 0x5074, GENMASK(31, 0), 0x27273F3F);
-	regmap_update_bits(ss->regmap_pextp[id], 0x5078, GENMASK(31, 0), 0xA7883C68);
-	regmap_update_bits(ss->regmap_pextp[id], 0x507C, GENMASK(31, 0), 0x11661166);
-	regmap_update_bits(ss->regmap_pextp[id], 0x5080, GENMASK(31, 0), 0x0E000AAF);
-	regmap_update_bits(ss->regmap_pextp[id], 0x5084, GENMASK(31, 0), 0x08080D0D);
-	regmap_update_bits(ss->regmap_pextp[id], 0x5088, GENMASK(31, 0), 0x02030909);
-	regmap_update_bits(ss->regmap_pextp[id], 0x50E4, GENMASK(31, 0), 0x0C0C0000);
-	regmap_update_bits(ss->regmap_pextp[id], 0x50E8, GENMASK(31, 0), 0x04040000);
-	regmap_update_bits(ss->regmap_pextp[id], 0x50EC, GENMASK(31, 0), 0x0F0F0C06);
-	regmap_update_bits(ss->regmap_pextp[id], 0x50A8, GENMASK(31, 0), 0x506E8C8C);
-	regmap_update_bits(ss->regmap_pextp[id], 0x6004, GENMASK(31, 0), 0x18190000);
-	regmap_update_bits(ss->regmap_pextp[id], 0x00F8, GENMASK(31, 0), 0x009C329C);
-	regmap_update_bits(ss->regmap_pextp[id], 0x00F4, GENMASK(31, 0), 0x80201F21);
-	regmap_update_bits(ss->regmap_pextp[id], 0x0030, GENMASK(31, 0), 0x00050C00);
-	regmap_update_bits(ss->regmap_pextp[id], 0x0070, GENMASK(31, 0), 0x02002800);
+	regmap_update_bits(mpcs->regmap_pextp, 0x9024, GENMASK(31, 0),
+			   0x00D9071C);
+	regmap_update_bits(mpcs->regmap_pextp, 0x2020, GENMASK(31, 0),
+			   0xAA8585AA);
+	regmap_update_bits(mpcs->regmap_pextp, 0x2030, GENMASK(31, 0),
+			   0x0C020707);
+	regmap_update_bits(mpcs->regmap_pextp, 0x2034, GENMASK(31, 0),
+			   0x0E050F0F);
+	regmap_update_bits(mpcs->regmap_pextp, 0x2040, GENMASK(31, 0),
+			   0x00140032);
+	regmap_update_bits(mpcs->regmap_pextp, 0x50F0, GENMASK(31, 0),
+			   0x00C014AA);
+	regmap_update_bits(mpcs->regmap_pextp, 0x50E0, GENMASK(31, 0),
+			   0x3777C12B);
+	regmap_update_bits(mpcs->regmap_pextp, 0x506C, GENMASK(31, 0),
+			   0x005F9CFF);
+	regmap_update_bits(mpcs->regmap_pextp, 0x5070, GENMASK(31, 0),
+			   0x9D9DFAFA);
+	regmap_update_bits(mpcs->regmap_pextp, 0x5074, GENMASK(31, 0),
+			   0x27273F3F);
+	regmap_update_bits(mpcs->regmap_pextp, 0x5078, GENMASK(31, 0),
+			   0xA7883C68);
+	regmap_update_bits(mpcs->regmap_pextp, 0x507C, GENMASK(31, 0),
+			   0x11661166);
+	regmap_update_bits(mpcs->regmap_pextp, 0x5080, GENMASK(31, 0),
+			   0x0E000AAF);
+	regmap_update_bits(mpcs->regmap_pextp, 0x5084, GENMASK(31, 0),
+			   0x08080D0D);
+	regmap_update_bits(mpcs->regmap_pextp, 0x5088, GENMASK(31, 0),
+			   0x02030909);
+	regmap_update_bits(mpcs->regmap_pextp, 0x50E4, GENMASK(31, 0),
+			   0x0C0C0000);
+	regmap_update_bits(mpcs->regmap_pextp, 0x50E8, GENMASK(31, 0),
+			   0x04040000);
+	regmap_update_bits(mpcs->regmap_pextp, 0x50EC, GENMASK(31, 0),
+			   0x0F0F0C06);
+	regmap_update_bits(mpcs->regmap_pextp, 0x50A8, GENMASK(31, 0),
+			   0x506E8C8C);
+	regmap_update_bits(mpcs->regmap_pextp, 0x6004, GENMASK(31, 0),
+			   0x18190000);
+	regmap_update_bits(mpcs->regmap_pextp, 0x00F8, GENMASK(31, 0),
+			   0x009C329C);
+	regmap_update_bits(mpcs->regmap_pextp, 0x00F4, GENMASK(31, 0),
+			   0x80201F21);
+	regmap_update_bits(mpcs->regmap_pextp, 0x0030, GENMASK(31, 0),
+			   0x00050C00);
+	regmap_update_bits(mpcs->regmap_pextp, 0x0070, GENMASK(31, 0),
+			   0x02002800);
 	ndelay(1020);
-	regmap_update_bits(ss->regmap_pextp[id], 0x30B0, GENMASK(31, 0), 0x00000020);
-	regmap_update_bits(ss->regmap_pextp[id], 0x3028, GENMASK(31, 0), 0x00008A01);
-	regmap_update_bits(ss->regmap_pextp[id], 0x302C, GENMASK(31, 0), 0x0000A884);
-	regmap_update_bits(ss->regmap_pextp[id], 0x3024, GENMASK(31, 0), 0x00083002);
-	regmap_update_bits(ss->regmap_pextp[id], 0x3010, GENMASK(31, 0), 0x00011110);
-	regmap_update_bits(ss->regmap_pextp[id], 0x3048, GENMASK(31, 0), 0x40704000);
-	regmap_update_bits(ss->regmap_pextp[id], 0x3050, GENMASK(31, 0), 0xA8000000);
-	regmap_update_bits(ss->regmap_pextp[id], 0x3054, GENMASK(31, 0), 0x000000AA);
-	regmap_update_bits(ss->regmap_pextp[id], 0x306C, GENMASK(31, 0), 0x22000F00);
-	regmap_update_bits(ss->regmap_pextp[id], 0xA060, GENMASK(31, 0), 0x00050000);
-	regmap_update_bits(ss->regmap_pextp[id], 0x90D0, GENMASK(31, 0), 0x00000005);
-	regmap_update_bits(ss->regmap_pextp[id], 0x0070, GENMASK(31, 0), 0x0200E800);
+	regmap_update_bits(mpcs->regmap_pextp, 0x30B0, GENMASK(31, 0),
+			   0x00000020);
+	regmap_update_bits(mpcs->regmap_pextp, 0x3028, GENMASK(31, 0),
+			   0x00008A01);
+	regmap_update_bits(mpcs->regmap_pextp, 0x302C, GENMASK(31, 0),
+			   0x0000A884);
+	regmap_update_bits(mpcs->regmap_pextp, 0x3024, GENMASK(31, 0),
+			   0x00083002);
+	regmap_update_bits(mpcs->regmap_pextp, 0x3010, GENMASK(31, 0),
+			   0x00011110);
+	regmap_update_bits(mpcs->regmap_pextp, 0x3048, GENMASK(31, 0),
+			   0x40704000);
+	regmap_update_bits(mpcs->regmap_pextp, 0x3050, GENMASK(31, 0),
+			   0xA8000000);
+	regmap_update_bits(mpcs->regmap_pextp, 0x3054, GENMASK(31, 0),
+			   0x000000AA);
+	regmap_update_bits(mpcs->regmap_pextp, 0x306C, GENMASK(31, 0),
+			   0x22000F00);
+	regmap_update_bits(mpcs->regmap_pextp, 0xA060, GENMASK(31, 0),
+			   0x00050000);
+	regmap_update_bits(mpcs->regmap_pextp, 0x90D0, GENMASK(31, 0),
+			   0x00000005);
+	regmap_update_bits(mpcs->regmap_pextp, 0x0070, GENMASK(31, 0),
+			   0x0200E800);
 	udelay(150);
-	regmap_update_bits(ss->regmap_pextp[id], 0x0070, GENMASK(31, 0), 0x0200C111);
+	regmap_update_bits(mpcs->regmap_pextp, 0x0070, GENMASK(31, 0),
+			   0x0200C111);
 	ndelay(1020);
-	regmap_update_bits(ss->regmap_pextp[id], 0x0070, GENMASK(31, 0), 0x0200C101);
+	regmap_update_bits(mpcs->regmap_pextp, 0x0070, GENMASK(31, 0),
+			   0x0200C101);
 	udelay(15);
-	regmap_update_bits(ss->regmap_pextp[id], 0x0070, GENMASK(31, 0), 0x0201C111);
+	regmap_update_bits(mpcs->regmap_pextp, 0x0070, GENMASK(31, 0),
+			   0x0201C111);
 	ndelay(1020);
-	regmap_update_bits(ss->regmap_pextp[id], 0x0070, GENMASK(31, 0), 0x0201C101);
+	regmap_update_bits(mpcs->regmap_pextp, 0x0070, GENMASK(31, 0),
+			   0x0201C101);
 	udelay(100);
-	regmap_update_bits(ss->regmap_pextp[id], 0x30B0, GENMASK(31, 0), 0x00000030);
-	regmap_update_bits(ss->regmap_pextp[id], 0x00F4, GENMASK(31, 0), 0x80201F01);
-	regmap_update_bits(ss->regmap_pextp[id], 0x3040, GENMASK(31, 0), 0x30000000);
+	regmap_update_bits(mpcs->regmap_pextp, 0x30B0, GENMASK(31, 0),
+			   0x00000030);
+	regmap_update_bits(mpcs->regmap_pextp, 0x00F4, GENMASK(31, 0),
+			   0x80201F01);
+	regmap_update_bits(mpcs->regmap_pextp, 0x3040, GENMASK(31, 0),
+			   0x30000000);
 	udelay(400);
 }
 
-int mtk_sgmii_setup_mode_an(struct mtk_xgmii *ss, unsigned int mac_id)
+int mtk_sgmii_setup_mode_an(struct mtk_sgmii_pcs *mpcs)
 {
-	struct mtk_eth *eth = ss->eth;
+	struct mtk_eth *eth = mpcs->eth;
 	unsigned int val = 0;
-	u32 id = mtk_mac2xgmii_id(ss->eth, mac_id);
 
-	if (!ss->regmap_sgmii[id])
+	if (!mpcs->regmap)
 		return -EINVAL;
 
 	if (MTK_HAS_CAPS(eth->soc->caps, MTK_NETSYS_V3)) {
-		mtk_xfi_pll_enable(ss);
-		mtk_sgmii_reset(ss, mac_id);
+		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(ss, id, SPEED_1000))
-		regmap_write(ss->regmap_sgmii[id], SGMSYS_QPHY_PWR_STATE_CTRL,
+	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(ss->regmap_sgmii[id], SGMII_RESERVED_0, SGMII_SW_RESET);
+	regmap_write(mpcs->regmap, SGMII_RESERVED_0, SGMII_SW_RESET);
 
-	regmap_read(ss->regmap_sgmii[id], ss->ana_rgc3, &val);
+	regmap_read(mpcs->regmap, mpcs->ana_rgc3, &val);
 	val &= ~RG_PHY_SPEED_3_125G;
-	regmap_write(ss->regmap_sgmii[id], ss->ana_rgc3, val);
+	regmap_write(mpcs->regmap, mpcs->ana_rgc3, val);
 
 	/* Setup the link timer and QPHY power up inside SGMIISYS */
-	regmap_write(ss->regmap_sgmii[id], SGMSYS_PCS_LINK_TIMER,
+	regmap_write(mpcs->regmap, SGMSYS_PCS_LINK_TIMER,
 		     SGMII_LINK_TIMER_DEFAULT);
 
-	regmap_read(ss->regmap_sgmii[id], SGMSYS_SGMII_MODE, &val);
+	regmap_read(mpcs->regmap, SGMSYS_SGMII_MODE, &val);
 	val |= SGMII_REMOTE_FAULT_DIS;
-	regmap_write(ss->regmap_sgmii[id], SGMSYS_SGMII_MODE, val);
+	regmap_write(mpcs->regmap, SGMSYS_SGMII_MODE, val);
 
 	/* SGMII AN mode setting */
-	regmap_read(ss->regmap_sgmii[id], SGMSYS_SGMII_MODE, &val);
+	regmap_read(mpcs->regmap, SGMSYS_SGMII_MODE, &val);
 	val &= ~SGMII_IF_MODE_MASK;
 	val |= SGMII_SPEED_DUPLEX_AN;
-	regmap_write(ss->regmap_sgmii[id], SGMSYS_SGMII_MODE, val);
+	regmap_write(mpcs->regmap, SGMSYS_SGMII_MODE, val);
 
 	/* Enable SGMII AN */
-	regmap_read(ss->regmap_sgmii[id], SGMSYS_PCS_CONTROL_1, &val);
+	regmap_read(mpcs->regmap, SGMSYS_PCS_CONTROL_1, &val);
 	val |= SGMII_AN_ENABLE;
-	regmap_write(ss->regmap_sgmii[id], SGMSYS_PCS_CONTROL_1, val);
+	regmap_write(mpcs->regmap, SGMSYS_PCS_CONTROL_1, val);
 
-	if(MTK_HAS_FLAGS(ss->flags[id],MTK_SGMII_PN_SWAP))
-		regmap_update_bits(ss->regmap_sgmii[id], SGMSYS_QPHY_WRAP_CTRL,
+	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);
 
 	/* Release PHYA power down state */
-	regmap_write(ss->regmap_sgmii[id], SGMSYS_QPHY_PWR_STATE_CTRL, 0);
+	regmap_write(mpcs->regmap, SGMSYS_QPHY_PWR_STATE_CTRL, 0);
 
 	if (MTK_HAS_CAPS(eth->soc->caps, MTK_NETSYS_V3))
-		mtk_sgmii_setup_phya_gen1(ss, mac_id);
+		mtk_sgmii_setup_phya_gen1(mpcs);
 
 	return 0;
 }
 
-int mtk_sgmii_setup_mode_force(struct mtk_xgmii *ss, unsigned int mac_id,
-			       const struct phylink_link_state *state)
+int mtk_sgmii_setup_mode_force(struct mtk_sgmii_pcs *mpcs,
+			       phy_interface_t interface)
 {
-	struct mtk_eth *eth = ss->eth;
+	struct mtk_eth *eth = mpcs->eth;
 	unsigned int val = 0;
-	u32 id = mtk_mac2xgmii_id(eth, mac_id);
 
-	if (!ss->regmap_sgmii[id])
+	if (!mpcs->regmap)
 		return -EINVAL;
 
 	if (MTK_HAS_CAPS(eth->soc->caps, MTK_NETSYS_V3)) {
-		mtk_xfi_pll_enable(ss);
-		mtk_sgmii_reset(ss, mac_id);
+		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(ss, id, state->speed))
-		regmap_write(ss->regmap_sgmii[id], SGMSYS_QPHY_PWR_STATE_CTRL,
+	if (mtk_sgmii_need_powerdown(mpcs, interface))
+		regmap_write(mpcs->regmap, SGMSYS_QPHY_PWR_STATE_CTRL,
 			     SGMII_PHYA_PWD);
 
 	/* Reset SGMII PCS state */
-	regmap_write(ss->regmap_sgmii[id], SGMII_RESERVED_0, SGMII_SW_RESET);
+	regmap_write(mpcs->regmap, SGMII_RESERVED_0, SGMII_SW_RESET);
 
-	regmap_read(ss->regmap_sgmii[id], ss->ana_rgc3, &val);
+	regmap_read(mpcs->regmap, mpcs->ana_rgc3, &val);
 	val &= ~RG_PHY_SPEED_MASK;
-	if (state->interface == PHY_INTERFACE_MODE_2500BASEX)
+	if (interface == PHY_INTERFACE_MODE_2500BASEX)
 		val |= RG_PHY_SPEED_3_125G;
-	regmap_write(ss->regmap_sgmii[id], ss->ana_rgc3, val);
+	regmap_write(mpcs->regmap, mpcs->ana_rgc3, val);
 
 	/* Disable SGMII AN */
-	regmap_read(ss->regmap_sgmii[id], SGMSYS_PCS_CONTROL_1, &val);
+	regmap_read(mpcs->regmap, SGMSYS_PCS_CONTROL_1, &val);
 	val &= ~SGMII_AN_ENABLE;
-	regmap_write(ss->regmap_sgmii[id], SGMSYS_PCS_CONTROL_1, val);
+	regmap_write(mpcs->regmap, SGMSYS_PCS_CONTROL_1, val);
 
-	/* SGMII force mode setting */
-	regmap_read(ss->regmap_sgmii[id], SGMSYS_SGMII_MODE, &val);
-	val &= ~SGMII_IF_MODE_MASK;
+	/* 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);
 
-	switch (state->speed) {
-	case SPEED_10:
-		val |= SGMII_SPEED_10;
-		break;
-	case SPEED_100:
-		val |= SGMII_SPEED_100;
-		break;
-	case SPEED_2500:
-	case SPEED_1000:
-	default:
-		val |= SGMII_SPEED_1000;
-		break;
-	};
-
-	/* SGMII 1G and 2.5G force mode can only work in full duplex
-	 * mode, no matter SGMII_FORCE_HALF_DUPLEX is set or not.
-	 */
-	if (state->duplex != DUPLEX_FULL)
-		val |= SGMII_DUPLEX_FULL;
-
-	regmap_write(ss->regmap_sgmii[id], SGMSYS_SGMII_MODE, val);
-
-	if(MTK_HAS_FLAGS(ss->flags[id],MTK_SGMII_PN_SWAP))
-		regmap_update_bits(ss->regmap_sgmii[id], SGMSYS_QPHY_WRAP_CTRL,
+	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);
 
 	/* Release PHYA power down state */
-	regmap_write(ss->regmap_sgmii[id], SGMSYS_QPHY_PWR_STATE_CTRL, 0);
+	regmap_write(mpcs->regmap, SGMSYS_QPHY_PWR_STATE_CTRL, 0);
 
 	if (MTK_HAS_CAPS(eth->soc->caps, MTK_NETSYS_V3))
-		mtk_sgmii_setup_phya_gen2(ss, mac_id);
+		mtk_sgmii_setup_phya_gen2(mpcs);
 
 	return 0;
 }
 
+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;
+
+	state->interface = mpcs->interface;
+
+	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);
+
+		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;
+		}
+	}
+}
+
-void mtk_sgmii_restart_an(struct mtk_eth *eth, int mac_id)
+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_xgmii *ss = eth->xgmii;
-	unsigned int val = 0, sid = mtk_mac2xgmii_id(eth, mac_id);
+	struct mtk_sgmii_pcs *mpcs = pcs_to_mtk_sgmii_pcs(pcs);
+	int err = 0;
+
+	mpcs->interface = interface;
 
-	/* Decide how GMAC and SGMIISYS be mapped */
-	sid = (MTK_HAS_CAPS(eth->soc->caps, MTK_SHARED_SGMII)) ?
-	       0 : sid;
+	/* 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);
 
-	if (!ss->regmap_sgmii[sid])
+	return err;
+}
+
+void mtk_sgmii_pcs_restart_an(struct phylink_pcs *pcs)
+{
+	struct mtk_sgmii_pcs *mpcs = pcs_to_mtk_sgmii_pcs(pcs);
+	unsigned int val = 0;
+
+	if (!mpcs->regmap)
 		return;
 
-	regmap_read(ss->regmap_sgmii[sid], SGMSYS_PCS_CONTROL_1, &val);
+	regmap_read(mpcs->regmap, SGMSYS_PCS_CONTROL_1, &val);
 	val |= SGMII_AN_RESTART;
-	regmap_write(ss->regmap_sgmii[sid], SGMSYS_PCS_CONTROL_1, val);
+	regmap_write(mpcs->regmap, SGMSYS_PCS_CONTROL_1, val);
+}
+
+static void mtk_sgmii_pcs_link_up(struct phylink_pcs *pcs, unsigned int mode,
+				  phy_interface_t interface,
+				  int speed, int duplex)
+{
+	struct mtk_sgmii_pcs *mpcs = pcs_to_mtk_sgmii_pcs(pcs);
+	unsigned int val;
+
+	if (!phy_interface_mode_is_8023z(interface))
+		return;
+
+	/* SGMII force duplex setting */
+	regmap_read(mpcs->regmap, SGMSYS_SGMII_MODE, &val);
+	val &= ~SGMII_DUPLEX_HALF;
+	if (duplex != DUPLEX_FULL)
+		val |= SGMII_DUPLEX_HALF;
+
+	regmap_write(mpcs->regmap, SGMSYS_SGMII_MODE, val);
+}
+
+static const struct phylink_pcs_ops mtk_sgmii_pcs_ops = {
+	.pcs_config = mtk_sgmii_pcs_config,
+	.pcs_get_state = mtk_sgmii_pcs_get_state,
+	.pcs_an_restart = mtk_sgmii_pcs_restart_an,
+	.pcs_link_up = mtk_sgmii_pcs_link_up,
+};
+
+int mtk_sgmii_init(struct mtk_eth *eth, struct device_node *r, u32 ana_rgc3)
+{
+	struct mtk_sgmii *ss = eth->sgmii;
+	struct device_node *np;
+	int ret, i;
+
+	for (i = 0; i < MTK_MAX_DEVS; i++) {
+		np = of_parse_phandle(r, "mediatek,sgmiisys", i);
+		if (!np)
+			break;
+
+		ss->pcs[i].id = i;
+		ss->pcs[i].eth = eth;
+		ss->pcs[i].ana_rgc3 = ana_rgc3;
+
+		ss->pcs[i].regmap = syscon_node_to_regmap(np);
+		if (IS_ERR(ss->pcs[i].regmap))
+			return PTR_ERR(ss->pcs[i].regmap);
+
+		ss->pcs[i].flags &= ~(MTK_SGMII_PN_SWAP);
+		if (of_property_read_bool(np, "pn_swap"))
+			ss->pcs[i].flags |= MTK_SGMII_PN_SWAP;
+
+		ss->pcs[i].pcs.ops = &mtk_sgmii_pcs_ops;
+		ss->pcs[i].pcs.poll = true;
+		ss->pcs[i].interface = PHY_INTERFACE_MODE_NA;
+
+		of_node_put(np);
+	}
+
+	if (MTK_HAS_CAPS(eth->soc->caps, MTK_NETSYS_V3)) {
+		ret = mtk_sgmii_xfi_pextp_init(ss, r);
+		if (ret)
+			return ret;
+
+		ret = mtk_sgmii_xfi_pll_init(ss, r);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+struct phylink_pcs *mtk_sgmii_select_pcs(struct mtk_sgmii *ss, int id)
+{
+	if (!ss->pcs[id].regmap)
+		return NULL;
+
+	return &ss->pcs[id].pcs;
 }
diff --git a/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_usxgmii.c b/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_usxgmii.c
index 8fb28ca..73f84f6 100644
--- a/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_usxgmii.c
+++ b/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_usxgmii.c
@@ -9,25 +9,12 @@
 #include <linux/regmap.h>
 #include "mtk_eth_soc.h"
 
-int mtk_usxgmii_init(struct mtk_xgmii *ss, struct device_node *r)
+static struct mtk_usxgmii_pcs *pcs_to_mtk_usxgmii_pcs(struct phylink_pcs *pcs)
 {
-	struct device_node *np;
-	int i;
-
-	for (i = 0; i < MTK_MAX_DEVS; i++) {
-		np = of_parse_phandle(r, "mediatek,usxgmiisys", i);
-		if (!np)
-			break;
-
-		ss->regmap_usxgmii[i] = syscon_node_to_regmap(np);
-		if (IS_ERR(ss->regmap_usxgmii[i]))
-			return PTR_ERR(ss->regmap_usxgmii[i]);
-	}
-
-	return 0;
+	return container_of(pcs, struct mtk_usxgmii_pcs, pcs);
 }
 
-int mtk_xfi_pextp_init(struct mtk_xgmii *ss, struct device_node *r)
+int mtk_usxgmii_xfi_pextp_init(struct mtk_usxgmii *ss, struct device_node *r)
 {
 	struct device_node *np;
 	int i;
@@ -37,25 +24,32 @@
 		if (!np)
 			break;
 
-		ss->regmap_pextp[i] = syscon_node_to_regmap(np);
-		if (IS_ERR(ss->regmap_pextp[i]))
-			return PTR_ERR(ss->regmap_pextp[i]);
+		ss->pcs[i].regmap_pextp = syscon_node_to_regmap(np);
+		if (IS_ERR(ss->pcs[i].regmap_pextp))
+			return PTR_ERR(ss->pcs[i].regmap_pextp);
+
+		of_node_put(np);
 	}
 
 	return 0;
 }
 
-int mtk_xfi_pll_init(struct mtk_xgmii *ss, struct device_node *r)
+int mtk_usxgmii_xfi_pll_init(struct mtk_usxgmii *ss, struct device_node *r)
 {
 	struct device_node *np;
+	int i;
 
 	np = of_parse_phandle(r, "mediatek,xfi_pll", 0);
 	if (!np)
 		return -1;
 
+	for (i = 0; i < MTK_MAX_DEVS; i++) {
+		ss->pll = syscon_node_to_regmap(np);
+		if (IS_ERR(ss->pll))
+			return PTR_ERR(ss->pll);
+	}
+
-	ss->regmap_pll = syscon_node_to_regmap(np);
-	if (IS_ERR(ss->regmap_pll))
-		return PTR_ERR(ss->regmap_pll);
+	of_node_put(np);
 
 	return 0;
 }
@@ -75,19 +69,19 @@
 	return 0;
 }
 
-int mtk_xfi_pll_enable(struct mtk_xgmii *ss)
+static int mtk_usxgmii_xfi_pll_enable(struct mtk_usxgmii *ss)
 {
 	u32 val = 0;
 
-	if (!ss->regmap_pll)
+	if (!ss->pll)
 		return -EINVAL;
 
 	/* Add software workaround for USXGMII PLL TCL issue */
-	regmap_write(ss->regmap_pll, XFI_PLL_ANA_GLB8, RG_XFI_PLL_ANA_SWWA);
+	regmap_write(ss->pll, XFI_PLL_ANA_GLB8, RG_XFI_PLL_ANA_SWWA);
 
-	regmap_read(ss->regmap_pll, XFI_PLL_DIG_GLB8, &val);
+	regmap_read(ss->pll, XFI_PLL_DIG_GLB8, &val);
 	val |= RG_XFI_PLL_EN;
-	regmap_write(ss->regmap_pll, XFI_PLL_DIG_GLB8, val);
+	regmap_write(ss->pll, XFI_PLL_DIG_GLB8, val);
 
 	return 0;
 }
@@ -114,376 +108,444 @@
 	return xgmii_id;
 }
 
-void mtk_usxgmii_setup_phya_an_10000(struct mtk_xgmii *ss, int mac_id)
+int mtk_xgmii2mac_id(struct mtk_eth *eth, int xgmii_id)
 {
-	u32 id = mtk_mac2xgmii_id(ss->eth, mac_id);
+	u32 mac_id = xgmii_id;
 
-	if (id >= MTK_MAX_DEVS ||
-	    !ss->regmap_usxgmii[id] || !ss->regmap_pextp[id])
-		return;
+	if (MTK_HAS_CAPS(eth->soc->caps, MTK_NETSYS_V3)) {
+		switch (xgmii_id) {
+		case 0:
+			mac_id = 2;
+			break;
+		case 1:
+			mac_id = 1;
+			break;
+		default:
+			pr_info("[%s] Warning: get illegal xgmii_id=%d !=!!!\n",
+				__func__, xgmii_id);
+		}
+	}
+
+	return mac_id;
+}
+
+int mtk_usxgmii_setup_phya_an_10000(struct mtk_usxgmii_pcs *mpcs)
+{
+	if (!mpcs->regmap || !mpcs->regmap_pextp)
+		return -EINVAL;
 
-	regmap_update_bits(ss->regmap_usxgmii[id], 0x810, GENMASK(31, 0), 0x000FFE6D);
-	regmap_update_bits(ss->regmap_usxgmii[id], 0x818, GENMASK(31, 0), 0x07B1EC7B);
-	regmap_update_bits(ss->regmap_usxgmii[id], 0x80C, GENMASK(31, 0), 0x30000000);
+	regmap_update_bits(mpcs->regmap, 0x810, GENMASK(31, 0),
+			   0x000FFE6D);
+	regmap_update_bits(mpcs->regmap, 0x818, GENMASK(31, 0),
+			   0x07B1EC7B);
+	regmap_update_bits(mpcs->regmap, 0x80C, GENMASK(31, 0),
+			   0x30000000);
 	ndelay(1020);
-	regmap_update_bits(ss->regmap_usxgmii[id], 0x80C, GENMASK(31, 0), 0x10000000);
+	regmap_update_bits(mpcs->regmap, 0x80C, GENMASK(31, 0),
+			   0x10000000);
 	ndelay(1020);
-	regmap_update_bits(ss->regmap_usxgmii[id], 0x80C, GENMASK(31, 0), 0x00000000);
+	regmap_update_bits(mpcs->regmap, 0x80C, GENMASK(31, 0),
+			   0x00000000);
 
-	regmap_update_bits(ss->regmap_pextp[id], 0x9024, GENMASK(31, 0), 0x00C9071C);
-	regmap_update_bits(ss->regmap_pextp[id], 0x2020, GENMASK(31, 0), 0xAA8585AA);
-	regmap_update_bits(ss->regmap_pextp[id], 0x2030, GENMASK(31, 0), 0x0C020707);
-	regmap_update_bits(ss->regmap_pextp[id], 0x2034, GENMASK(31, 0), 0x0E050F0F);
-	regmap_update_bits(ss->regmap_pextp[id], 0x2040, GENMASK(31, 0), 0x00140032);
-	regmap_update_bits(ss->regmap_pextp[id], 0x50F0, GENMASK(31, 0), 0x00C014AA);
-	regmap_update_bits(ss->regmap_pextp[id], 0x50E0, GENMASK(31, 0), 0x3777C12B);
-	regmap_update_bits(ss->regmap_pextp[id], 0x506C, GENMASK(31, 0), 0x005F9CFF);
-	regmap_update_bits(ss->regmap_pextp[id], 0x5070, GENMASK(31, 0), 0x9D9DFAFA);
-	regmap_update_bits(ss->regmap_pextp[id], 0x5074, GENMASK(31, 0), 0x27273F3F);
-	regmap_update_bits(ss->regmap_pextp[id], 0x5078, GENMASK(31, 0), 0xA7883C68);
-	regmap_update_bits(ss->regmap_pextp[id], 0x507C, GENMASK(31, 0), 0x11661166);
-	regmap_update_bits(ss->regmap_pextp[id], 0x5080, GENMASK(31, 0), 0x0E000AAF);
-	regmap_update_bits(ss->regmap_pextp[id], 0x5084, GENMASK(31, 0), 0x08080D0D);
-	regmap_update_bits(ss->regmap_pextp[id], 0x5088, GENMASK(31, 0), 0x02030909);
-	regmap_update_bits(ss->regmap_pextp[id], 0x50E4, GENMASK(31, 0), 0x0C0C0000);
-	regmap_update_bits(ss->regmap_pextp[id], 0x50E8, GENMASK(31, 0), 0x04040000);
-	regmap_update_bits(ss->regmap_pextp[id], 0x50EC, GENMASK(31, 0), 0x0F0F0C06);
-	regmap_update_bits(ss->regmap_pextp[id], 0x50A8, GENMASK(31, 0), 0x506E8C8C);
-	regmap_update_bits(ss->regmap_pextp[id], 0x6004, GENMASK(31, 0), 0x18190000);
-	regmap_update_bits(ss->regmap_pextp[id], 0x00F8, GENMASK(31, 0), 0x01423342);
-	regmap_update_bits(ss->regmap_pextp[id], 0x00F4, GENMASK(31, 0), 0x80201F20);
-	regmap_update_bits(ss->regmap_pextp[id], 0x0030, GENMASK(31, 0), 0x00050C00);
-	regmap_update_bits(ss->regmap_pextp[id], 0x0070, GENMASK(31, 0), 0x02002800);
+	regmap_update_bits(mpcs->regmap_pextp, 0x9024, GENMASK(31, 0),
+			   0x00C9071C);
+	regmap_update_bits(mpcs->regmap_pextp, 0x2020, GENMASK(31, 0),
+			   0xAA8585AA);
+	regmap_update_bits(mpcs->regmap_pextp, 0x2030, GENMASK(31, 0),
+			   0x0C020707);
+	regmap_update_bits(mpcs->regmap_pextp, 0x2034, GENMASK(31, 0),
+			   0x0E050F0F);
+	regmap_update_bits(mpcs->regmap_pextp, 0x2040, GENMASK(31, 0),
+			   0x00140032);
+	regmap_update_bits(mpcs->regmap_pextp, 0x50F0, GENMASK(31, 0),
+			   0x00C014AA);
+	regmap_update_bits(mpcs->regmap_pextp, 0x50E0, GENMASK(31, 0),
+			   0x3777C12B);
+	regmap_update_bits(mpcs->regmap_pextp, 0x506C, GENMASK(31, 0),
+			   0x005F9CFF);
+	regmap_update_bits(mpcs->regmap_pextp, 0x5070, GENMASK(31, 0),
+			   0x9D9DFAFA);
+	regmap_update_bits(mpcs->regmap_pextp, 0x5074, GENMASK(31, 0),
+			   0x27273F3F);
+	regmap_update_bits(mpcs->regmap_pextp, 0x5078, GENMASK(31, 0),
+			   0xA7883C68);
+	regmap_update_bits(mpcs->regmap_pextp, 0x507C, GENMASK(31, 0),
+			   0x11661166);
+	regmap_update_bits(mpcs->regmap_pextp, 0x5080, GENMASK(31, 0),
+			   0x0E000AAF);
+	regmap_update_bits(mpcs->regmap_pextp, 0x5084, GENMASK(31, 0),
+			   0x08080D0D);
+	regmap_update_bits(mpcs->regmap_pextp, 0x5088, GENMASK(31, 0),
+			   0x02030909);
+	regmap_update_bits(mpcs->regmap_pextp, 0x50E4, GENMASK(31, 0),
+			   0x0C0C0000);
+	regmap_update_bits(mpcs->regmap_pextp, 0x50E8, GENMASK(31, 0),
+			   0x04040000);
+	regmap_update_bits(mpcs->regmap_pextp, 0x50EC, GENMASK(31, 0),
+			   0x0F0F0C06);
+	regmap_update_bits(mpcs->regmap_pextp, 0x50A8, GENMASK(31, 0),
+			   0x506E8C8C);
+	regmap_update_bits(mpcs->regmap_pextp, 0x6004, GENMASK(31, 0),
+			   0x18190000);
+	regmap_update_bits(mpcs->regmap_pextp, 0x00F8, GENMASK(31, 0),
+			   0x01423342);
+	regmap_update_bits(mpcs->regmap_pextp, 0x00F4, GENMASK(31, 0),
+			   0x80201F20);
+	regmap_update_bits(mpcs->regmap_pextp, 0x0030, GENMASK(31, 0),
+			   0x00050C00);
+	regmap_update_bits(mpcs->regmap_pextp, 0x0070, GENMASK(31, 0),
+			   0x02002800);
 	ndelay(1020);
-	regmap_update_bits(ss->regmap_pextp[id], 0x30B0, GENMASK(31, 0), 0x00000020);
-	regmap_update_bits(ss->regmap_pextp[id], 0x3028, GENMASK(31, 0), 0x00008A01);
-	regmap_update_bits(ss->regmap_pextp[id], 0x302C, GENMASK(31, 0), 0x0000A884);
-	regmap_update_bits(ss->regmap_pextp[id], 0x3024, GENMASK(31, 0), 0x00083002);
-	regmap_update_bits(ss->regmap_pextp[id], 0x3010, GENMASK(31, 0), 0x00022220);
-	regmap_update_bits(ss->regmap_pextp[id], 0x5064, GENMASK(31, 0), 0x0F020A01);
-	regmap_update_bits(ss->regmap_pextp[id], 0x50B4, GENMASK(31, 0), 0x06100600);
-	regmap_update_bits(ss->regmap_pextp[id], 0x3048, GENMASK(31, 0), 0x40704000);
-	regmap_update_bits(ss->regmap_pextp[id], 0x3050, GENMASK(31, 0), 0xA8000000);
-	regmap_update_bits(ss->regmap_pextp[id], 0x3054, GENMASK(31, 0), 0x000000AA);
-	regmap_update_bits(ss->regmap_pextp[id], 0x306C, GENMASK(31, 0), 0x00000F00);
-	regmap_update_bits(ss->regmap_pextp[id], 0xA060, GENMASK(31, 0), 0x00040000);
-	regmap_update_bits(ss->regmap_pextp[id], 0x90D0, GENMASK(31, 0), 0x00000001);
-	regmap_update_bits(ss->regmap_pextp[id], 0x0070, GENMASK(31, 0), 0x0200E800);
+	regmap_update_bits(mpcs->regmap_pextp, 0x30B0, GENMASK(31, 0),
+			   0x00000020);
+	regmap_update_bits(mpcs->regmap_pextp, 0x3028, GENMASK(31, 0),
+			   0x00008A01);
+	regmap_update_bits(mpcs->regmap_pextp, 0x302C, GENMASK(31, 0),
+			   0x0000A884);
+	regmap_update_bits(mpcs->regmap_pextp, 0x3024, GENMASK(31, 0),
+			   0x00083002);
+	regmap_update_bits(mpcs->regmap_pextp, 0x3010, GENMASK(31, 0),
+			   0x00022220);
+	regmap_update_bits(mpcs->regmap_pextp, 0x5064, GENMASK(31, 0),
+			   0x0F020A01);
+	regmap_update_bits(mpcs->regmap_pextp, 0x50B4, GENMASK(31, 0),
+			   0x06100600);
+	regmap_update_bits(mpcs->regmap_pextp, 0x3048, GENMASK(31, 0),
+			   0x40704000);
+	regmap_update_bits(mpcs->regmap_pextp, 0x3050, GENMASK(31, 0),
+			   0xA8000000);
+	regmap_update_bits(mpcs->regmap_pextp, 0x3054, GENMASK(31, 0),
+			   0x000000AA);
+	regmap_update_bits(mpcs->regmap_pextp, 0x306C, GENMASK(31, 0),
+			   0x00000F00);
+	regmap_update_bits(mpcs->regmap_pextp, 0xA060, GENMASK(31, 0),
+			   0x00040000);
+	regmap_update_bits(mpcs->regmap_pextp, 0x90D0, GENMASK(31, 0),
+			   0x00000001);
+	regmap_update_bits(mpcs->regmap_pextp, 0x0070, GENMASK(31, 0),
+			   0x0200E800);
 	udelay(150);
-	regmap_update_bits(ss->regmap_pextp[id], 0x0070, GENMASK(31, 0), 0x0200C111);
+	regmap_update_bits(mpcs->regmap_pextp, 0x0070, GENMASK(31, 0),
+			   0x0200C111);
 	ndelay(1020);
-	regmap_update_bits(ss->regmap_pextp[id], 0x0070, GENMASK(31, 0), 0x0200C101);
+	regmap_update_bits(mpcs->regmap_pextp, 0x0070, GENMASK(31, 0),
+			   0x0200C101);
 	udelay(15);
-	regmap_update_bits(ss->regmap_pextp[id], 0x0070, GENMASK(31, 0), 0x0202C111);
+	regmap_update_bits(mpcs->regmap_pextp, 0x0070, GENMASK(31, 0),
+			   0x0202C111);
 	ndelay(1020);
-	regmap_update_bits(ss->regmap_pextp[id], 0x0070, GENMASK(31, 0), 0x0202C101);
+	regmap_update_bits(mpcs->regmap_pextp, 0x0070, GENMASK(31, 0),
+			   0x0202C101);
 	udelay(100);
-	regmap_update_bits(ss->regmap_pextp[id], 0x30B0, GENMASK(31, 0), 0x00000030);
-	regmap_update_bits(ss->regmap_pextp[id], 0x00F4, GENMASK(31, 0), 0x80201F00);
-	regmap_update_bits(ss->regmap_pextp[id], 0x3040, GENMASK(31, 0), 0x30000000);
+	regmap_update_bits(mpcs->regmap_pextp, 0x30B0, GENMASK(31, 0),
+			   0x00000030);
+	regmap_update_bits(mpcs->regmap_pextp, 0x00F4, GENMASK(31, 0),
+			   0x80201F00);
+	regmap_update_bits(mpcs->regmap_pextp, 0x3040, GENMASK(31, 0),
+			   0x30000000);
 	udelay(400);
+
+	return 0;
 }
 
-void mtk_usxgmii_setup_phya_force_5000(struct mtk_xgmii *ss, int mac_id)
+int mtk_usxgmii_setup_phya_force_5000(struct mtk_usxgmii_pcs *mpcs)
 {
 	unsigned int val;
-	u32 id = mtk_mac2xgmii_id(ss->eth, mac_id);
 
-	if (id >= MTK_MAX_DEVS ||
-	    !ss->regmap_usxgmii[id] || !ss->regmap_pextp[id])
-		return;
+	if (!mpcs->regmap || !mpcs->regmap_pextp)
+		return -EINVAL;
 
 	/* Setup USXGMII speed */
 	val = FIELD_PREP(RG_XFI_RX_MODE, RG_XFI_RX_MODE_5G) |
 	      FIELD_PREP(RG_XFI_TX_MODE, RG_XFI_TX_MODE_5G);
-	regmap_write(ss->regmap_usxgmii[id], RG_PHY_TOP_SPEED_CTRL1, val);
+	regmap_write(mpcs->regmap, RG_PHY_TOP_SPEED_CTRL1, val);
 
 	/* Disable USXGMII AN mode */
-	regmap_read(ss->regmap_usxgmii[id], RG_PCS_AN_CTRL0, &val);
-	val &= ~RG_AN_ENABLE;
-	regmap_write(ss->regmap_usxgmii[id], RG_PCS_AN_CTRL0, val);
+	regmap_read(mpcs->regmap, RG_PCS_AN_CTRL0, &val);
+	val &= ~USXGMII_AN_ENABLE;
+	regmap_write(mpcs->regmap, RG_PCS_AN_CTRL0, val);
 
 	/* Gated USXGMII */
-	regmap_read(ss->regmap_usxgmii[id], RG_PHY_TOP_SPEED_CTRL1, &val);
+	regmap_read(mpcs->regmap, RG_PHY_TOP_SPEED_CTRL1, &val);
 	val |= RG_MAC_CK_GATED;
-	regmap_write(ss->regmap_usxgmii[id], RG_PHY_TOP_SPEED_CTRL1, val);
+	regmap_write(mpcs->regmap, RG_PHY_TOP_SPEED_CTRL1, val);
 
 	ndelay(1020);
 
 	/* USXGMII force mode setting */
-	regmap_read(ss->regmap_usxgmii[id], RG_PHY_TOP_SPEED_CTRL1, &val);
+	regmap_read(mpcs->regmap, RG_PHY_TOP_SPEED_CTRL1, &val);
 	val |= RG_USXGMII_RATE_UPDATE_MODE;
 	val |= RG_IF_FORCE_EN;
 	val |= FIELD_PREP(RG_RATE_ADAPT_MODE, RG_RATE_ADAPT_MODE_X1);
-	regmap_write(ss->regmap_usxgmii[id], RG_PHY_TOP_SPEED_CTRL1, val);
+	regmap_write(mpcs->regmap, RG_PHY_TOP_SPEED_CTRL1, val);
 
 	/* Un-gated USXGMII */
-	regmap_read(ss->regmap_usxgmii[id], RG_PHY_TOP_SPEED_CTRL1, &val);
+	regmap_read(mpcs->regmap, RG_PHY_TOP_SPEED_CTRL1, &val);
 	val &= ~RG_MAC_CK_GATED;
-	regmap_write(ss->regmap_usxgmii[id], RG_PHY_TOP_SPEED_CTRL1, val);
+	regmap_write(mpcs->regmap, RG_PHY_TOP_SPEED_CTRL1, val);
 
 	ndelay(1020);
 
-	regmap_update_bits(ss->regmap_pextp[id], 0x9024, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x9024, GENMASK(31, 0),
 			   0x00D9071C);
-	regmap_update_bits(ss->regmap_pextp[id], 0x2020, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x2020, GENMASK(31, 0),
 			   0xAAA5A5AA);
-	regmap_update_bits(ss->regmap_pextp[id], 0x2030, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x2030, GENMASK(31, 0),
 			   0x0C020707);
-	regmap_update_bits(ss->regmap_pextp[id], 0x2034, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x2034, GENMASK(31, 0),
 			   0x0E050F0F);
-	regmap_update_bits(ss->regmap_pextp[id], 0x2040, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x2040, GENMASK(31, 0),
 			   0x00140032);
-	regmap_update_bits(ss->regmap_pextp[id], 0x50F0, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x50F0, GENMASK(31, 0),
 			   0x00C018AA);
-	regmap_update_bits(ss->regmap_pextp[id], 0x50E0, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x50E0, GENMASK(31, 0),
 			   0x3777812B);
-	regmap_update_bits(ss->regmap_pextp[id], 0x506C, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x506C, GENMASK(31, 0),
 			   0x005C9CFF);
-	regmap_update_bits(ss->regmap_pextp[id], 0x5070, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x5070, GENMASK(31, 0),
 			   0x9DFAFAFA);
-	regmap_update_bits(ss->regmap_pextp[id], 0x5074, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x5074, GENMASK(31, 0),
 			   0x273F3F3F);
-	regmap_update_bits(ss->regmap_pextp[id], 0x5078, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x5078, GENMASK(31, 0),
 			   0xA8883868);
-	regmap_update_bits(ss->regmap_pextp[id], 0x507C, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x507C, GENMASK(31, 0),
 			   0x14661466);
-	regmap_update_bits(ss->regmap_pextp[id], 0x5080, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x5080, GENMASK(31, 0),
 			   0x0E001ABF);
-	regmap_update_bits(ss->regmap_pextp[id], 0x5084, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x5084, GENMASK(31, 0),
 			   0x080B0D0D);
-	regmap_update_bits(ss->regmap_pextp[id], 0x5088, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x5088, GENMASK(31, 0),
 			   0x02050909);
-	regmap_update_bits(ss->regmap_pextp[id], 0x50E4, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x50E4, GENMASK(31, 0),
 			   0x0C000000);
-	regmap_update_bits(ss->regmap_pextp[id], 0x50E8, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x50E8, GENMASK(31, 0),
 			   0x04000000);
-	regmap_update_bits(ss->regmap_pextp[id], 0x50EC, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x50EC, GENMASK(31, 0),
 			   0x0F0F0C06);
-	regmap_update_bits(ss->regmap_pextp[id], 0x50A8, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x50A8, GENMASK(31, 0),
 			   0x50808C8C);
-	regmap_update_bits(ss->regmap_pextp[id], 0x6004, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x6004, GENMASK(31, 0),
 			   0x18000000);
-	regmap_update_bits(ss->regmap_pextp[id], 0x00F8, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x00F8, GENMASK(31, 0),
 			   0x00A132A1);
-	regmap_update_bits(ss->regmap_pextp[id], 0x00F4, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x00F4, GENMASK(31, 0),
 			   0x80201F20);
-	regmap_update_bits(ss->regmap_pextp[id], 0x0030, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x0030, GENMASK(31, 0),
 			   0x00050C00);
-	regmap_update_bits(ss->regmap_pextp[id], 0x0070, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x0070, GENMASK(31, 0),
 			   0x02002800);
 	ndelay(1020);
-	regmap_update_bits(ss->regmap_pextp[id], 0x30B0, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x30B0, GENMASK(31, 0),
 			   0x00000020);
-	regmap_update_bits(ss->regmap_pextp[id], 0x3028, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x3028, GENMASK(31, 0),
 			   0x00008A01);
-	regmap_update_bits(ss->regmap_pextp[id], 0x302C, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x302C, GENMASK(31, 0),
 			   0x0000A884);
-	regmap_update_bits(ss->regmap_pextp[id], 0x3024, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x3024, GENMASK(31, 0),
 			   0x00083002);
-	regmap_update_bits(ss->regmap_pextp[id], 0x3010, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x3010, GENMASK(31, 0),
 			   0x00022220);
-	regmap_update_bits(ss->regmap_pextp[id], 0x5064, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x5064, GENMASK(31, 0),
 			   0x0F020A01);
-	regmap_update_bits(ss->regmap_pextp[id], 0x50B4, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x50B4, GENMASK(31, 0),
 			   0x06100600);
-	regmap_update_bits(ss->regmap_pextp[id], 0x3048, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x3048, GENMASK(31, 0),
 			   0x40704000);
-	regmap_update_bits(ss->regmap_pextp[id], 0x3050, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x3050, GENMASK(31, 0),
 			   0xA8000000);
-	regmap_update_bits(ss->regmap_pextp[id], 0x3054, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x3054, GENMASK(31, 0),
 			   0x000000AA);
-	regmap_update_bits(ss->regmap_pextp[id], 0x306C, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x306C, GENMASK(31, 0),
 			   0x00000F00);
-	regmap_update_bits(ss->regmap_pextp[id], 0xA060, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0xA060, GENMASK(31, 0),
 			   0x00040000);
-	regmap_update_bits(ss->regmap_pextp[id], 0x90D0, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x90D0, GENMASK(31, 0),
 			   0x00000003);
-	regmap_update_bits(ss->regmap_pextp[id], 0x0070, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x0070, GENMASK(31, 0),
 			   0x0200E800);
 	udelay(150);
-	regmap_update_bits(ss->regmap_pextp[id], 0x0070, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x0070, GENMASK(31, 0),
 			   0x0200C111);
 	ndelay(1020);
-	regmap_update_bits(ss->regmap_pextp[id], 0x0070, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x0070, GENMASK(31, 0),
 			   0x0200C101);
 	udelay(15);
-	regmap_update_bits(ss->regmap_pextp[id], 0x0070, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x0070, GENMASK(31, 0),
 			   0x0202C111);
 	ndelay(1020);
-	regmap_update_bits(ss->regmap_pextp[id], 0x0070, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x0070, GENMASK(31, 0),
 			   0x0202C101);
 	udelay(100);
-	regmap_update_bits(ss->regmap_pextp[id], 0x30B0, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x30B0, GENMASK(31, 0),
 			   0x00000030);
-	regmap_update_bits(ss->regmap_pextp[id], 0x00F4, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x00F4, GENMASK(31, 0),
 			   0x80201F00);
-	regmap_update_bits(ss->regmap_pextp[id], 0x3040, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x3040, GENMASK(31, 0),
 			   0x30000000);
 	udelay(400);
+
+	return 0;
 }
 
-void mtk_usxgmii_setup_phya_force_10000(struct mtk_xgmii *ss, int mac_id)
+int mtk_usxgmii_setup_phya_force_10000(struct mtk_usxgmii_pcs *mpcs)
 {
 	unsigned int val;
-	u32 id = mtk_mac2xgmii_id(ss->eth, mac_id);
 
-	if (id >= MTK_MAX_DEVS ||
-	    !ss->regmap_usxgmii[id] || !ss->regmap_pextp[id])
-		return;
+	if (!mpcs->regmap || !mpcs->regmap_pextp)
+		return -EINVAL;
 
 	/* Setup USXGMII speed */
 	val = FIELD_PREP(RG_XFI_RX_MODE, RG_XFI_RX_MODE_10G) |
 	      FIELD_PREP(RG_XFI_TX_MODE, RG_XFI_TX_MODE_10G);
-	regmap_write(ss->regmap_usxgmii[id], RG_PHY_TOP_SPEED_CTRL1, val);
+	regmap_write(mpcs->regmap, RG_PHY_TOP_SPEED_CTRL1, val);
 
 	/* Disable USXGMII AN mode */
-	regmap_read(ss->regmap_usxgmii[id], RG_PCS_AN_CTRL0, &val);
-	val &= ~RG_AN_ENABLE;
-	regmap_write(ss->regmap_usxgmii[id], RG_PCS_AN_CTRL0, val);
+	regmap_read(mpcs->regmap, RG_PCS_AN_CTRL0, &val);
+	val &= ~USXGMII_AN_ENABLE;
+	regmap_write(mpcs->regmap, RG_PCS_AN_CTRL0, val);
 
 	/* Gated USXGMII */
-	regmap_read(ss->regmap_usxgmii[id], RG_PHY_TOP_SPEED_CTRL1, &val);
+	regmap_read(mpcs->regmap, RG_PHY_TOP_SPEED_CTRL1, &val);
 	val |= RG_MAC_CK_GATED;
-	regmap_write(ss->regmap_usxgmii[id], RG_PHY_TOP_SPEED_CTRL1, val);
+	regmap_write(mpcs->regmap, RG_PHY_TOP_SPEED_CTRL1, val);
 
 	ndelay(1020);
 
 	/* USXGMII force mode setting */
-	regmap_read(ss->regmap_usxgmii[id], RG_PHY_TOP_SPEED_CTRL1, &val);
+	regmap_read(mpcs->regmap, RG_PHY_TOP_SPEED_CTRL1, &val);
 	val |= RG_USXGMII_RATE_UPDATE_MODE;
 	val |= RG_IF_FORCE_EN;
 	val |= FIELD_PREP(RG_RATE_ADAPT_MODE, RG_RATE_ADAPT_MODE_X1);
-	regmap_write(ss->regmap_usxgmii[id], RG_PHY_TOP_SPEED_CTRL1, val);
+	regmap_write(mpcs->regmap, RG_PHY_TOP_SPEED_CTRL1, val);
 
 	/* Un-gated USXGMII */
-	regmap_read(ss->regmap_usxgmii[id], RG_PHY_TOP_SPEED_CTRL1, &val);
+	regmap_read(mpcs->regmap, RG_PHY_TOP_SPEED_CTRL1, &val);
 	val &= ~RG_MAC_CK_GATED;
-	regmap_write(ss->regmap_usxgmii[id], RG_PHY_TOP_SPEED_CTRL1, val);
+	regmap_write(mpcs->regmap, RG_PHY_TOP_SPEED_CTRL1, val);
 
 	ndelay(1020);
 
-	regmap_update_bits(ss->regmap_pextp[id], 0x9024, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x9024, GENMASK(31, 0),
 			   0x00C9071C);
-	regmap_update_bits(ss->regmap_pextp[id], 0x2020, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x2020, GENMASK(31, 0),
 			   0xAA8585AA);
-	regmap_update_bits(ss->regmap_pextp[id], 0x2030, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x2030, GENMASK(31, 0),
 			   0x0C020707);
-	regmap_update_bits(ss->regmap_pextp[id], 0x2034, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x2034, GENMASK(31, 0),
 			   0x0E050F0F);
-	regmap_update_bits(ss->regmap_pextp[id], 0x2040, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x2040, GENMASK(31, 0),
 			   0x00140032);
-	regmap_update_bits(ss->regmap_pextp[id], 0x50F0, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x50F0, GENMASK(31, 0),
 			   0x00C014AA);
-	regmap_update_bits(ss->regmap_pextp[id], 0x50E0, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x50E0, GENMASK(31, 0),
 			   0x3777C12B);
-	regmap_update_bits(ss->regmap_pextp[id], 0x506C, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x506C, GENMASK(31, 0),
 			   0x005F9CFF);
-	regmap_update_bits(ss->regmap_pextp[id], 0x5070, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x5070, GENMASK(31, 0),
 			   0x9D9DFAFA);
-	regmap_update_bits(ss->regmap_pextp[id], 0x5074, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x5074, GENMASK(31, 0),
 			   0x27273F3F);
-	regmap_update_bits(ss->regmap_pextp[id], 0x5078, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x5078, GENMASK(31, 0),
 			   0xA7883C68);
-	regmap_update_bits(ss->regmap_pextp[id], 0x507C, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x507C, GENMASK(31, 0),
 			   0x11661166);
-	regmap_update_bits(ss->regmap_pextp[id], 0x5080, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x5080, GENMASK(31, 0),
 			   0x0E000AAF);
-	regmap_update_bits(ss->regmap_pextp[id], 0x5084, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x5084, GENMASK(31, 0),
 			   0x08080D0D);
-	regmap_update_bits(ss->regmap_pextp[id], 0x5088, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x5088, GENMASK(31, 0),
 			   0x02030909);
-	regmap_update_bits(ss->regmap_pextp[id], 0x50E4, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x50E4, GENMASK(31, 0),
 			   0x0C0C0000);
-	regmap_update_bits(ss->regmap_pextp[id], 0x50E8, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x50E8, GENMASK(31, 0),
 			   0x04040000);
-	regmap_update_bits(ss->regmap_pextp[id], 0x50EC, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x50EC, GENMASK(31, 0),
 			   0x0F0F0C06);
-	regmap_update_bits(ss->regmap_pextp[id], 0x50A8, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x50A8, GENMASK(31, 0),
 			   0x506E8C8C);
-	regmap_update_bits(ss->regmap_pextp[id], 0x6004, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x6004, GENMASK(31, 0),
 			   0x18190000);
-	regmap_update_bits(ss->regmap_pextp[id], 0x00F8, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x00F8, GENMASK(31, 0),
 			   0x01423342);
-	regmap_update_bits(ss->regmap_pextp[id], 0x00F4, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x00F4, GENMASK(31, 0),
 			   0x80201F20);
-	regmap_update_bits(ss->regmap_pextp[id], 0x0030, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x0030, GENMASK(31, 0),
 			   0x00050C00);
-	regmap_update_bits(ss->regmap_pextp[id], 0x0070, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x0070, GENMASK(31, 0),
 			   0x02002800);
 	ndelay(1020);
-	regmap_update_bits(ss->regmap_pextp[id], 0x30B0, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x30B0, GENMASK(31, 0),
 			   0x00000020);
-	regmap_update_bits(ss->regmap_pextp[id], 0x3028, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x3028, GENMASK(31, 0),
 			   0x00008A01);
-	regmap_update_bits(ss->regmap_pextp[id], 0x302C, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x302C, GENMASK(31, 0),
 			   0x0000A884);
-	regmap_update_bits(ss->regmap_pextp[id], 0x3024, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x3024, GENMASK(31, 0),
 			   0x00083002);
-	regmap_update_bits(ss->regmap_pextp[id], 0x3010, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x3010, GENMASK(31, 0),
 			   0x00022220);
-	regmap_update_bits(ss->regmap_pextp[id], 0x5064, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x5064, GENMASK(31, 0),
 			   0x0F020A01);
-	regmap_update_bits(ss->regmap_pextp[id], 0x50B4, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x50B4, GENMASK(31, 0),
 			   0x06100600);
-	regmap_update_bits(ss->regmap_pextp[id], 0x3048, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x3048, GENMASK(31, 0),
 			   0x49664100);
-	regmap_update_bits(ss->regmap_pextp[id], 0x3050, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x3050, GENMASK(31, 0),
 			   0x00000000);
-	regmap_update_bits(ss->regmap_pextp[id], 0x3054, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x3054, GENMASK(31, 0),
 			   0x00000000);
-	regmap_update_bits(ss->regmap_pextp[id], 0x306C, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x306C, GENMASK(31, 0),
 			   0x00000F00);
-	regmap_update_bits(ss->regmap_pextp[id], 0xA060, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0xA060, GENMASK(31, 0),
 			   0x00040000);
-	regmap_update_bits(ss->regmap_pextp[id], 0x90D0, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x90D0, GENMASK(31, 0),
 			   0x00000001);
-	regmap_update_bits(ss->regmap_pextp[id], 0x0070, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x0070, GENMASK(31, 0),
 			   0x0200E800);
 	udelay(150);
-	regmap_update_bits(ss->regmap_pextp[id], 0x0070, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x0070, GENMASK(31, 0),
 			   0x0200C111);
 	ndelay(1020);
-	regmap_update_bits(ss->regmap_pextp[id], 0x0070, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x0070, GENMASK(31, 0),
 			   0x0200C101);
 	udelay(15);
-	regmap_update_bits(ss->regmap_pextp[id], 0x0070, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x0070, GENMASK(31, 0),
 			   0x0202C111);
 	ndelay(1020);
-	regmap_update_bits(ss->regmap_pextp[id], 0x0070, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x0070, GENMASK(31, 0),
 			   0x0202C101);
 	udelay(100);
-	regmap_update_bits(ss->regmap_pextp[id], 0x30B0, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x30B0, GENMASK(31, 0),
 			   0x00000030);
-	regmap_update_bits(ss->regmap_pextp[id], 0x00F4, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x00F4, GENMASK(31, 0),
 			   0x80201F00);
-	regmap_update_bits(ss->regmap_pextp[id], 0x3040, GENMASK(31, 0),
+	regmap_update_bits(mpcs->regmap_pextp, 0x3040, GENMASK(31, 0),
 			   0x30000000);
 	udelay(400);
+
+	return 0;
 }
 
-void mtk_usxgmii_reset(struct mtk_xgmii *ss, int mac_id)
+void mtk_usxgmii_reset(struct mtk_eth *eth, int id)
 {
-	struct mtk_eth *eth = ss->eth;
-	u32 id = mtk_mac2xgmii_id(eth, mac_id);
 	u32 val = 0;
 
 	if (id >= MTK_MAX_DEVS || !eth->toprgu)
 		return;
 
-	switch (mac_id) {
-	case MTK_GMAC2_ID:
+	switch (id) {
+	case 0:
 		/* Enable software reset */
 		regmap_read(eth->toprgu, TOPRGU_SWSYSRST_EN, &val);
-		val |= SWSYSRST_XFI_PEXPT1_GRST |
-		       SWSYSRST_XFI1_GRST;
+		val |= SWSYSRST_XFI_PEXPT0_GRST |
+		       SWSYSRST_XFI0_GRST;
 		regmap_write(eth->toprgu, TOPRGU_SWSYSRST_EN, val);
 
 		/* Assert USXGMII reset */
 		regmap_read(eth->toprgu, TOPRGU_SWSYSRST, &val);
 		val |= FIELD_PREP(SWSYSRST_UNLOCK_KEY, 0x88) |
-		       SWSYSRST_XFI_PEXPT1_GRST |
-		       SWSYSRST_XFI1_GRST;
+		       SWSYSRST_XFI_PEXPT0_GRST |
+		       SWSYSRST_XFI0_GRST;
 		regmap_write(eth->toprgu, TOPRGU_SWSYSRST, val);
 
 		udelay(100);
@@ -491,28 +553,28 @@
 		/* De-assert USXGMII reset */
 		regmap_read(eth->toprgu, TOPRGU_SWSYSRST, &val);
 		val |= FIELD_PREP(SWSYSRST_UNLOCK_KEY, 0x88);
-		val &= ~(SWSYSRST_XFI_PEXPT1_GRST |
-			 SWSYSRST_XFI1_GRST);
+		val &= ~(SWSYSRST_XFI_PEXPT0_GRST |
+			 SWSYSRST_XFI0_GRST);
 		regmap_write(eth->toprgu, TOPRGU_SWSYSRST, val);
 
 		/* Disable software reset */
 		regmap_read(eth->toprgu, TOPRGU_SWSYSRST_EN, &val);
-		val &= ~(SWSYSRST_XFI_PEXPT1_GRST |
-			 SWSYSRST_XFI1_GRST);
+		val &= ~(SWSYSRST_XFI_PEXPT0_GRST |
+			 SWSYSRST_XFI0_GRST);
 		regmap_write(eth->toprgu, TOPRGU_SWSYSRST_EN, val);
 		break;
-	case MTK_GMAC3_ID:
+	case 1:
 		/* Enable software reset */
 		regmap_read(eth->toprgu, TOPRGU_SWSYSRST_EN, &val);
-		val |= SWSYSRST_XFI_PEXPT0_GRST |
-		       SWSYSRST_XFI0_GRST;
+		val |= SWSYSRST_XFI_PEXPT1_GRST |
+		       SWSYSRST_XFI1_GRST;
 		regmap_write(eth->toprgu, TOPRGU_SWSYSRST_EN, val);
 
 		/* Assert USXGMII reset */
 		regmap_read(eth->toprgu, TOPRGU_SWSYSRST, &val);
 		val |= FIELD_PREP(SWSYSRST_UNLOCK_KEY, 0x88) |
-		       SWSYSRST_XFI_PEXPT0_GRST |
-		       SWSYSRST_XFI0_GRST;
+		       SWSYSRST_XFI_PEXPT1_GRST |
+		       SWSYSRST_XFI1_GRST;
 		regmap_write(eth->toprgu, TOPRGU_SWSYSRST, val);
 
 		udelay(100);
@@ -520,14 +582,14 @@
 		/* De-assert USXGMII reset */
 		regmap_read(eth->toprgu, TOPRGU_SWSYSRST, &val);
 		val |= FIELD_PREP(SWSYSRST_UNLOCK_KEY, 0x88);
-		val &= ~(SWSYSRST_XFI_PEXPT0_GRST |
-			 SWSYSRST_XFI0_GRST);
+		val &= ~(SWSYSRST_XFI_PEXPT1_GRST |
+			 SWSYSRST_XFI1_GRST);
 		regmap_write(eth->toprgu, TOPRGU_SWSYSRST, val);
 
 		/* Disable software reset */
 		regmap_read(eth->toprgu, TOPRGU_SWSYSRST_EN, &val);
-		val &= ~(SWSYSRST_XFI_PEXPT0_GRST |
-			 SWSYSRST_XFI0_GRST);
+		val &= ~(SWSYSRST_XFI_PEXPT1_GRST |
+			 SWSYSRST_XFI1_GRST);
 		regmap_write(eth->toprgu, TOPRGU_SWSYSRST_EN, val);
 		break;
 	}
@@ -535,37 +597,163 @@
 	mdelay(10);
 }
 
-int mtk_usxgmii_setup_mode_an(struct mtk_xgmii *ss, int mac_id, int max_speed)
+static void mtk_usxgmii_pcs_get_state(struct phylink_pcs *pcs,
+				    struct phylink_link_state *state)
 {
-	if (mac_id < 0 || mac_id >= MTK_MAX_DEVS)
-		return -EINVAL;
+	struct mtk_usxgmii_pcs *mpcs = pcs_to_mtk_usxgmii_pcs(pcs);
+	struct mtk_eth *eth = mpcs->eth;
+	struct mtk_mac *mac = eth->mac[mtk_xgmii2mac_id(eth, mpcs->id)];
+	u32 val = 0;
 
-	if ((max_speed != SPEED_10000) && (max_speed != SPEED_5000))
-		return -EINVAL;
+	regmap_read(mpcs->regmap, RG_PCS_AN_CTRL0, &val);
+	if (FIELD_GET(USXGMII_AN_ENABLE, val)) {
+		/* Refresh LPA by inverting LPA_LATCH */
+		regmap_read(mpcs->regmap, RG_PCS_AN_STS0, &val);
+		regmap_update_bits(mpcs->regmap, RG_PCS_AN_STS0,
+				   USXGMII_LPA_LATCH,
+				   !(val & USXGMII_LPA_LATCH));
 
-	mtk_xfi_pll_enable(ss);
-	mtk_usxgmii_reset(ss, mac_id);
-	mtk_usxgmii_setup_phya_an_10000(ss, mac_id);
+		regmap_read(mpcs->regmap, RG_PCS_AN_STS0, &val);
 
-	return 0;
+		state->interface = mpcs->interface;
+		state->link = FIELD_GET(USXGMII_LPA_LINK, val);
+		state->duplex = FIELD_GET(USXGMII_LPA_DUPLEX, val);
+
+		switch (FIELD_GET(USXGMII_LPA_SPEED_MASK, val)) {
+		case USXGMII_LPA_SPEED_10:
+			state->speed = SPEED_10;
+			break;
+		case USXGMII_LPA_SPEED_100:
+			state->speed = SPEED_100;
+			break;
+		case USXGMII_LPA_SPEED_1000:
+			state->speed = SPEED_1000;
+			break;
+		case USXGMII_LPA_SPEED_2500:
+			state->speed = SPEED_2500;
+			break;
+		case USXGMII_LPA_SPEED_5000:
+			state->speed = SPEED_5000;
+			break;
+		case USXGMII_LPA_SPEED_10000:
+			state->speed = SPEED_10000;
+			break;
+		}
+	} else {
+		val = mtk_r32(mac->hw, MTK_XGMAC_STS(mac->id));
+
+		if (mac->id == MTK_GMAC2_ID)
+			val = val >> 16;
+
+		switch (FIELD_GET(MTK_USXGMII_PCS_MODE, val)) {
+		case 0:
+			state->speed = SPEED_10000;
+			break;
+		case 1:
+			state->speed = SPEED_5000;
+			break;
+		case 2:
+			state->speed = SPEED_2500;
+			break;
+		case 3:
+			state->speed = SPEED_1000;
+			break;
+		}
+
+		state->interface = mpcs->interface;
+		state->link = FIELD_GET(MTK_USXGMII_PCS_LINK, val);
+		state->duplex = DUPLEX_FULL;
+	}
 }
 
-int mtk_usxgmii_setup_mode_force(struct mtk_xgmii *ss, int mac_id,
-				 const struct phylink_link_state *state)
+static int mtk_usxgmii_pcs_config(struct phylink_pcs *pcs, unsigned int mode,
+				  phy_interface_t interface,
+				  const unsigned long *advertising,
+				  bool permit_pause_to_mac)
 {
-	if (mac_id < 0 || mac_id >= MTK_MAX_DEVS)
-		return -EINVAL;
+	struct mtk_usxgmii_pcs *mpcs = pcs_to_mtk_usxgmii_pcs(pcs);
+	struct mtk_eth *eth = mpcs->eth;
+	int err = 0;
+
+	mpcs->interface = interface;
+
+	mtk_usxgmii_xfi_pll_enable(eth->usxgmii);
+	mtk_usxgmii_reset(eth, mpcs->id);
+
+	/* Setup USXGMIISYS with the determined property */
+	if (interface == PHY_INTERFACE_MODE_USXGMII)
+		err = mtk_usxgmii_setup_phya_an_10000(mpcs);
+	else if (interface == PHY_INTERFACE_MODE_10GKR)
+		err = mtk_usxgmii_setup_phya_force_10000(mpcs);
+	else if (interface == PHY_INTERFACE_MODE_5GBASER)
+		err = mtk_usxgmii_setup_phya_force_5000(mpcs);
+
+	return err;
+}
+
+void mtk_usxgmii_pcs_restart_an(struct phylink_pcs *pcs)
+{
+	struct mtk_usxgmii_pcs *mpcs = pcs_to_mtk_usxgmii_pcs(pcs);
+	unsigned int val = 0;
+
+	if (!mpcs->regmap)
+		return;
 
-	mtk_xfi_pll_enable(ss);
-	mtk_usxgmii_reset(ss, mac_id);
-	if (state->interface == PHY_INTERFACE_MODE_5GBASER)
-		mtk_usxgmii_setup_phya_force_5000(ss, mac_id);
-	else
-		mtk_usxgmii_setup_phya_force_10000(ss, mac_id);
+	regmap_read(mpcs->regmap, RG_PCS_AN_CTRL0, &val);
+	val |= USXGMII_AN_RESTART;
+	regmap_write(mpcs->regmap, RG_PCS_AN_CTRL0, val);
+}
+
+static const struct phylink_pcs_ops mtk_usxgmii_pcs_ops = {
+	.pcs_config = mtk_usxgmii_pcs_config,
+	.pcs_get_state = mtk_usxgmii_pcs_get_state,
+	.pcs_an_restart = mtk_usxgmii_pcs_restart_an,
+};
+
+int mtk_usxgmii_init(struct mtk_eth *eth, struct device_node *r)
+{
+	struct mtk_usxgmii *ss = eth->usxgmii;
+	struct device_node *np;
+	int ret, i;
+
+	for (i = 0; i < MTK_MAX_DEVS; i++) {
+		np = of_parse_phandle(r, "mediatek,usxgmiisys", i);
+		if (!np)
+			break;
+
+		ss->pcs[i].id = i;
+		ss->pcs[i].eth = eth;
+
+		ss->pcs[i].regmap = syscon_node_to_regmap(np);
+		if (IS_ERR(ss->pcs[i].regmap))
+			return PTR_ERR(ss->pcs[i].regmap);
+
+		ss->pcs[i].pcs.ops = &mtk_usxgmii_pcs_ops;
+		ss->pcs[i].pcs.poll = true;
+		ss->pcs[i].interface = PHY_INTERFACE_MODE_NA;
+
+		of_node_put(np);
+	}
+
+	ret = mtk_usxgmii_xfi_pextp_init(ss, r);
+	if (ret)
+		return ret;
+
+	ret = mtk_usxgmii_xfi_pll_init(ss, r);
+	if (ret)
+		return ret;
 
 	return 0;
 }
 
+struct phylink_pcs *mtk_usxgmii_select_pcs(struct mtk_usxgmii *ss, int id)
+{
+	if (!ss->pcs[id].regmap)
+		return NULL;
+
+	return &ss->pcs[id].pcs;
+}
+
 int mtk_dump_usxgmii(struct regmap *pmap, char *name, u32 offset, u32 range)
 {
 	unsigned int cur = offset;