mediatek: sync MT7988 USXGMII with SDK driver

The USXGMII driver in SDK was heavily refactored, some bugs have been
fixed and it has switched to use phylink_pcs. Follow up with changes
in SDK driver and sync our on-top-of-mainline driver with the SDK
driver.

Signed-off-by: Daniel Golle <[email protected]>

target/linux/generic/pending-5.15/737-07-net-ethernet-mtk_eth_soc-add-paths-and-SerDes-modes-.patch

@@ -1,23 +1,40 @@
-From 20ac14fedba025b6b336a821ea60660afe2d46cd Mon Sep 17 00:00:00 2001
+From 3d833ad2cfc1ab503d9aae2967b7f10811bb3c9c Mon Sep 17 00:00:00 2001
 From: Daniel Golle <[email protected]>
 Date: Wed, 1 Mar 2023 11:56:04 +0000
-Subject: [PATCH 7/7] net: ethernet: mtk_eth_soc: add paths and SerDes modes
+Subject: [PATCH 7/7] net: ethernet: mtk_eth_soc: add paths and SerDes modes 
  for MT7988
 
-MT7988 comes with a built-in 2.5G PHY as well as USXGMII or 10Base-KR
-compatible SerDes lanes for external PHYs.
+MT7988 comes with a built-in 2.5G PHY as well as
+USXGMII/10GBase-KR/5GBase-KR compatible SerDes lanes for external PHYs.
 Add support for configuring the MAC and SerDes parts for the new paths.
 
 Signed-off-by: Daniel Golle <[email protected]>
 ---
- drivers/net/ethernet/mediatek/Makefile       |   2 +-
- drivers/net/ethernet/mediatek/mtk_eth_path.c | 154 ++++-
- drivers/net/ethernet/mediatek/mtk_eth_soc.c  | 291 +++++++-
- drivers/net/ethernet/mediatek/mtk_eth_soc.h  | 162 ++++-
- drivers/net/ethernet/mediatek/mtk_usxgmii.c  | 659 +++++++++++++++++++
- 5 files changed, 1236 insertions(+), 32 deletions(-)
+ drivers/net/ethernet/mediatek/Kconfig        |   7 +
+ drivers/net/ethernet/mediatek/Makefile       |   1 +
+ drivers/net/ethernet/mediatek/mtk_eth_path.c | 154 +++-
+ drivers/net/ethernet/mediatek/mtk_eth_soc.c  | 270 +++++-
+ drivers/net/ethernet/mediatek/mtk_eth_soc.h  | 194 ++++-
+ drivers/net/ethernet/mediatek/mtk_usxgmii.c  | 835 +++++++++++++++++++
+ 6 files changed, 1428 insertions(+), 33 deletions(-)
  create mode 100644 drivers/net/ethernet/mediatek/mtk_usxgmii.c
 
+--- a/drivers/net/ethernet/mediatek/Kconfig
++++ b/drivers/net/ethernet/mediatek/Kconfig
+@@ -24,6 +24,13 @@ config NET_MEDIATEK_SOC
+ 	  This driver supports the gigabit ethernet MACs in the
+ 	  MediaTek SoC family.
+ 
++config NET_MEDIATEK_SOC_USXGMII
++	bool "Support USXGMII SerDes on MT7988"
++	depends on (ARCH_MEDIATEK && ARM64) || COMPILE_TEST
++	def_bool NET_MEDIATEK_SOC != n
++	help
++	  Include support for 10G SerDes which can be found on MT7988.
++
+ config NET_MEDIATEK_STAR_EMAC
+ 	tristate "MediaTek STAR Ethernet MAC support"
+ 	select PHYLIB
 --- a/drivers/net/ethernet/mediatek/Makefile
 +++ b/drivers/net/ethernet/mediatek/Makefile
 @@ -5,6 +5,7 @@
@@ -291,7 +308,20 @@ Signed-off-by: Daniel Golle <[email protected]>
  static struct phylink_pcs *mtk_mac_select_pcs(struct phylink_config *config,
  					      phy_interface_t interface)
  {
-@@ -462,7 +479,7 @@ static void mtk_mac_config(struct phylin
+@@ -451,6 +468,12 @@ static struct phylink_pcs *mtk_mac_selec
+ 		       0 : mac->id;
+ 
+ 		return eth->sgmii_pcs[sid];
++	} else if ((interface == PHY_INTERFACE_MODE_USXGMII ||
++		    interface == PHY_INTERFACE_MODE_10GKR ||
++		    interface == PHY_INTERFACE_MODE_5GBASER) &&
++		   MTK_HAS_CAPS(eth->soc->caps, MTK_NETSYS_V3) &&
++		   mac->id != MTK_GMAC1_ID) {
++			return mtk_usxgmii_select_pcs(eth, mac->id);
+ 	}
+ 
+ 	return NULL;
+@@ -462,7 +485,7 @@ static void mtk_mac_config(struct phylin
  	struct mtk_mac *mac = container_of(config, struct mtk_mac,
  					   phylink_config);
  	struct mtk_eth *eth = mac->hw;
@@ -300,7 +330,7 @@ Signed-off-by: Daniel Golle <[email protected]>
  	u32 i;
  
  	/* MT76x8 has no hardware settings between for the MAC */
-@@ -506,6 +523,23 @@ static void mtk_mac_config(struct phylin
+@@ -506,6 +529,23 @@ static void mtk_mac_config(struct phylin
  					goto init_err;
  			}
  			break;
@@ -324,7 +354,7 @@ Signed-off-by: Daniel Golle <[email protected]>
  		default:
  			goto err_phy;
  		}
-@@ -584,14 +618,92 @@ static void mtk_mac_config(struct phylin
+@@ -584,14 +624,78 @@ static void mtk_mac_config(struct phylin
  				   SYSCFG0_SGMII_MASK,
  				   ~(u32)SYSCFG0_SGMII_MASK);
  
@@ -338,26 +368,14 @@ Signed-off-by: Daniel Golle <[email protected]>
 +		}
  		/* Save the syscfg0 value for mac_finish */
  		mac->syscfg0 = val;
-+	} else if (state->interface == PHY_INTERFACE_MODE_USXGMII ||
-+		   state->interface == PHY_INTERFACE_MODE_10GKR ||
-+		   state->interface == PHY_INTERFACE_MODE_5GBASER) {
-+
-+		if (!MTK_HAS_CAPS(eth->soc->caps, MTK_NETSYS_V3)) {
-+			err = -EINVAL;
-+			goto init_err;
-+		}
-+		if (phylink_autoneg_inband(mode))
-+			err = mtk_usxgmii_setup_mode_force(eth, mac->id,
-+							   state);
-+		else
-+			err = mtk_usxgmii_setup_mode_an(eth, mac->id,
-+							SPEED_10000);
-+
-+		if (err)
-+			goto init_err;
- 	} else if (phylink_autoneg_inband(mode)) {
+-	} else if (phylink_autoneg_inband(mode)) {
++	} else if (state->interface != PHY_INTERFACE_MODE_USXGMII &&
++		   state->interface != PHY_INTERFACE_MODE_10GKR &&
++		   state->interface != PHY_INTERFACE_MODE_5GBASER &&
++		   phylink_autoneg_inband(mode)) {
  		dev_err(eth->dev,
- 			"In-band mode not supported in non SGMII mode!\n");
+-			"In-band mode not supported in non SGMII mode!\n");
++			"In-band mode not supported in non-SerDes modes!\n");
  		return;
  	}
  
@@ -417,7 +435,7 @@ Signed-off-by: Daniel Golle <[email protected]>
  	return;
  
  err_phy:
-@@ -632,11 +744,37 @@ static int mtk_mac_finish(struct phylink
+@@ -632,11 +736,40 @@ static int mtk_mac_finish(struct phylink
  	return 0;
  }
  
@@ -430,7 +448,12 @@ Signed-off-by: Daniel Golle <[email protected]>
 +	if (mac->id == MTK_GMAC2_ID)
 +		sts = sts >> 16;
 +
-+	state->duplex = 1;
++	state->link = FIELD_GET(MTK_USXGMII_PCS_LINK, sts);
++	if (!state->link)
++		return;
++
++	state->duplex = DUPLEX_FULL;
++	state->interface = mac->interface;
 +
 +	switch (FIELD_GET(MTK_USXGMII_PCS_MODE, sts)) {
 +	case 0:
@@ -446,8 +469,6 @@ Signed-off-by: Daniel Golle <[email protected]>
 +		state->speed = SPEED_1000;
 +		break;
 +	}
-+
-+	state->link = FIELD_GET(MTK_USXGMII_PCS_LINK, sts);
 +}
 +
 +static void mtk_gdm_pcs_get_state(struct mtk_mac *mac,
@@ -458,7 +479,7 @@ Signed-off-by: Daniel Golle <[email protected]>
  	u32 pmsr = mtk_r32(mac->hw, MTK_MAC_MSR(mac->id));
  
  	state->link = (pmsr & MAC_MSR_LINK);
-@@ -664,15 +802,35 @@ static void mtk_mac_pcs_get_state(struct
+@@ -664,15 +797,35 @@ static void mtk_mac_pcs_get_state(struct
  		state->pause |= MLO_PAUSE_TX;
  }
  
@@ -497,7 +518,7 @@ Signed-off-by: Daniel Golle <[email protected]>
  }
  
  static void mtk_set_queue_speed(struct mtk_eth *eth, unsigned int idx,
-@@ -744,13 +902,11 @@ static void mtk_set_queue_speed(struct m
+@@ -744,13 +897,11 @@ static void mtk_set_queue_speed(struct m
  	mtk_w32(eth, val, soc->reg_map->qdma.qtx_sch + ofs);
  }
  
@@ -515,7 +536,7 @@ Signed-off-by: Daniel Golle <[email protected]>
  	u32 mcr;
  
  	mcr = mtk_r32(mac->hw, MTK_MAC_MCR(mac->id));
-@@ -784,6 +940,47 @@ static void mtk_mac_link_up(struct phyli
+@@ -784,6 +935,47 @@ static void mtk_mac_link_up(struct phyli
  	mtk_w32(mac->hw, mcr, MTK_MAC_MCR(mac->id));
  }
  
@@ -563,7 +584,7 @@ Signed-off-by: Daniel Golle <[email protected]>
  static const struct phylink_mac_ops mtk_phylink_ops = {
  	.validate = phylink_generic_validate,
  	.mac_select_pcs = mtk_mac_select_pcs,
-@@ -836,10 +1033,21 @@ static int mtk_mdio_init(struct mtk_eth
+@@ -836,10 +1028,21 @@ static int mtk_mdio_init(struct mtk_eth
  	}
  	divider = min_t(unsigned int, DIV_ROUND_UP(MDC_MAX_FREQ, max_clk), 63);
  
@@ -586,7 +607,7 @@ Signed-off-by: Daniel Golle <[email protected]>
  	mtk_w32(eth, val, MTK_PPSC);
  
  	dev_dbg(eth->dev, "MDC is running on %d Hz\n", MDC_MAX_FREQ / divider);
-@@ -4433,8 +4641,8 @@ static int mtk_add_mac(struct mtk_eth *e
+@@ -4433,8 +4636,8 @@ static int mtk_add_mac(struct mtk_eth *e
  	const __be32 *_id = of_get_property(np, "reg", NULL);
  	phy_interface_t phy_mode;
  	struct phylink *phylink;
@@ -596,7 +617,7 @@ Signed-off-by: Daniel Golle <[email protected]>
  	int txqs = 1;
  
  	if (!_id) {
-@@ -4525,6 +4733,32 @@ static int mtk_add_mac(struct mtk_eth *e
+@@ -4525,6 +4728,32 @@ static int mtk_add_mac(struct mtk_eth *e
  			  mac->phylink_config.supported_interfaces);
  	}
  
@@ -629,40 +650,20 @@ Signed-off-by: Daniel Golle <[email protected]>
  	phylink = phylink_create(&mac->phylink_config,
  				 of_fwnode_handle(mac->of_node),
  				 phy_mode, &mtk_phylink_ops);
-@@ -4714,6 +4948,33 @@ static int mtk_probe(struct platform_dev
- 			return err;
- 	}
+@@ -4712,6 +4941,13 @@ static int mtk_probe(struct platform_dev
  
+ 		if (err)
+ 			return err;
++	}
++
 +	if (MTK_HAS_CAPS(eth->soc->caps, MTK_USXGMII)) {
-+		eth->xgmii = devm_kzalloc(eth->dev, sizeof(*eth->xgmii), GFP_KERNEL);
 +		err = mtk_usxgmii_init(eth);
-+		if (err) {
-+			dev_err(&pdev->dev, "usxgmii init failed\n");
-+			return err;
-+		}
 +
-+		err = mtk_xfi_pextp_init(eth);
-+		if (err) {
-+			dev_err(&pdev->dev, "pextp init failed\n");
-+			return err;
-+		}
-+
-+		err = mtk_xfi_pll_init(eth);
-+		if (err) {
-+			dev_err(&pdev->dev, "xfi pll init failed\n");
-+			return err;
-+		}
-+
-+		err = mtk_toprgu_init(eth);
-+		if (err) {
-+			dev_err(&pdev->dev, "toprgu init failed\n");
++		if (err)
 +			return err;
-+		}
-+	}
-+
+ 	}
+ 
  	if (eth->soc->required_pctl) {
- 		eth->pctl = syscon_regmap_lookup_by_phandle(pdev->dev.of_node,
- 							    "mediatek,pctl");
 --- a/drivers/net/ethernet/mediatek/mtk_eth_soc.h
 +++ b/drivers/net/ethernet/mediatek/mtk_eth_soc.h
 @@ -126,6 +126,11 @@
@@ -743,34 +744,54 @@ Signed-off-by: Daniel Golle <[email protected]>
  
  
  /* ethernet subsystem clock register */
-@@ -506,16 +548,69 @@
+@@ -506,16 +548,91 @@
  #define ETHSYS_DMA_AG_MAP_QDMA	BIT(1)
  #define ETHSYS_DMA_AG_MAP_PPE	BIT(2)
  
 +/* USXGMII subsystem config registers */
 +/* Register to control speed */
 +#define RG_PHY_TOP_SPEED_CTRL1	0x80C
-+#define RG_USXGMII_RATE_UPDATE_MODE	BIT(31)
-+#define RG_MAC_CK_GATED		BIT(29)
-+#define RG_IF_FORCE_EN		BIT(28)
-+#define RG_RATE_ADAPT_MODE	GENMASK(10, 8)
-+#define RG_RATE_ADAPT_MODE_X1	0
-+#define RG_RATE_ADAPT_MODE_X2	1
-+#define RG_RATE_ADAPT_MODE_X4	2
-+#define RG_RATE_ADAPT_MODE_X10	3
-+#define RG_RATE_ADAPT_MODE_X100	4
-+#define RG_RATE_ADAPT_MODE_X5	5
-+#define RG_RATE_ADAPT_MODE_X50	6
-+#define RG_XFI_RX_MODE		GENMASK(6, 4)
-+#define RG_XFI_RX_MODE_10G	0
-+#define RG_XFI_RX_MODE_5G	1
-+#define RG_XFI_TX_MODE		GENMASK(2, 0)
-+#define RG_XFI_TX_MODE_10G	0
-+#define RG_XFI_TX_MODE_5G	1
++#define USXGMII_RATE_UPDATE_MODE	BIT(31)
++#define USXGMII_MAC_CK_GATED	BIT(29)
++#define USXGMII_IF_FORCE_EN	BIT(28)
++#define USXGMII_RATE_ADAPT_MODE	GENMASK(10, 8)
++#define USXGMII_RATE_ADAPT_MODE_X1	0
++#define USXGMII_RATE_ADAPT_MODE_X2	1
++#define USXGMII_RATE_ADAPT_MODE_X4	2
++#define USXGMII_RATE_ADAPT_MODE_X10	3
++#define USXGMII_RATE_ADAPT_MODE_X100	4
++#define USXGMII_RATE_ADAPT_MODE_X5	5
++#define USXGMII_RATE_ADAPT_MODE_X50	6
++#define USXGMII_XFI_RX_MODE	GENMASK(6, 4)
++#define USXGMII_XFI_RX_MODE_10G	0
++#define USXGMII_XFI_RX_MODE_5G	1
++#define USXGMII_XFI_TX_MODE	GENMASK(2, 0)
++#define USXGMII_XFI_TX_MODE_10G	0
++#define USXGMII_XFI_TX_MODE_5G	1
 +
 +/* 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_SYNC_CNT	GENMASK(30, 11)
++#define USXGMII_AN_ENABLE	BIT(0)
++
++#define RG_PCS_AN_CTRL2		0x818
++#define USXGMII_LINK_TIMER_IDLE_DETECT	GENMASK(29, 20)
++#define USXGMII_LINK_TIMER_COMP_ACK_DETECT	GENMASK(19, 10)
++#define USXGMII_LINK_TIMER_AN_RESTART	GENMASK(9, 0)
++
++/* Register to read PCS AN status */
++#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
@@ -791,6 +812,8 @@ Signed-off-by: Daniel Golle <[email protected]>
 +#define SWSYSRST_XFI_PLL_GRST	BIT(16)
 +#define SWSYSRST_XFI_PEXPT1_GRST	BIT(15)
 +#define SWSYSRST_XFI_PEXPT0_GRST	BIT(14)
++#define SWSYSRST_XFI1_GRST	BIT(13)
++#define SWSYSRST_XFI0_GRST	BIT(12)
 +#define SWSYSRST_SGMII1_GRST	BIT(2)
 +#define SWSYSRST_SGMII0_GRST	BIT(1)
 +#define TOPRGU_SWSYSRST_EN		0xFC
@@ -813,7 +836,7 @@ Signed-off-by: Daniel Golle <[email protected]>
  /* MT7628/88 specific stuff */
  #define MT7628_PDMA_OFFSET	0x0800
  #define MT7628_SDM_OFFSET	0x0c00
-@@ -809,13 +904,6 @@ enum mtk_gmac_id {
+@@ -809,13 +926,6 @@ enum mtk_gmac_id {
  	MTK_GMAC_ID_MAX
  };
  
@@ -827,7 +850,7 @@ Signed-off-by: Daniel Golle <[email protected]>
  enum mtk_tx_buf_type {
  	MTK_TYPE_SKB,
  	MTK_TYPE_XDP_TX,
-@@ -902,6 +990,7 @@ enum mkt_eth_capabilities {
+@@ -902,6 +1012,7 @@ enum mkt_eth_capabilities {
  	MTK_TRGMII_BIT,
  	MTK_SGMII_BIT,
  	MTK_USXGMII_BIT,
@@ -835,7 +858,7 @@ Signed-off-by: Daniel Golle <[email protected]>
  	MTK_ESW_BIT,
  	MTK_GEPHY_BIT,
  	MTK_MUX_BIT,
-@@ -922,6 +1011,7 @@ enum mkt_eth_capabilities {
+@@ -922,6 +1033,7 @@ enum mkt_eth_capabilities {
  	MTK_ETH_MUX_GDM1_TO_GMAC1_ESW_BIT,
  	MTK_ETH_MUX_GMAC2_GMAC0_TO_GEPHY_BIT,
  	MTK_ETH_MUX_U3_GMAC2_TO_QPHY_BIT,
@@ -843,7 +866,7 @@ Signed-off-by: Daniel Golle <[email protected]>
  	MTK_ETH_MUX_GMAC1_GMAC2_TO_SGMII_RGMII_BIT,
  	MTK_ETH_MUX_GMAC12_TO_GEPHY_SGMII_BIT,
  	MTK_ETH_MUX_GMAC123_TO_GEPHY_SGMII_BIT,
-@@ -933,6 +1023,7 @@ enum mkt_eth_capabilities {
+@@ -933,6 +1045,7 @@ enum mkt_eth_capabilities {
  	MTK_ETH_PATH_GMAC1_SGMII_BIT,
  	MTK_ETH_PATH_GMAC2_RGMII_BIT,
  	MTK_ETH_PATH_GMAC2_SGMII_BIT,
@@ -851,7 +874,7 @@ Signed-off-by: Daniel Golle <[email protected]>
  	MTK_ETH_PATH_GMAC2_GEPHY_BIT,
  	MTK_ETH_PATH_GMAC3_SGMII_BIT,
  	MTK_ETH_PATH_GDM1_ESW_BIT,
-@@ -946,6 +1037,7 @@ enum mkt_eth_capabilities {
+@@ -946,6 +1059,7 @@ enum mkt_eth_capabilities {
  #define MTK_TRGMII		BIT_ULL(MTK_TRGMII_BIT)
  #define MTK_SGMII		BIT_ULL(MTK_SGMII_BIT)
  #define MTK_USXGMII		BIT_ULL(MTK_USXGMII_BIT)
@@ -859,7 +882,7 @@ Signed-off-by: Daniel Golle <[email protected]>
  #define MTK_ESW			BIT_ULL(MTK_ESW_BIT)
  #define MTK_GEPHY		BIT_ULL(MTK_GEPHY_BIT)
  #define MTK_MUX			BIT_ULL(MTK_MUX_BIT)
-@@ -968,6 +1060,8 @@ enum mkt_eth_capabilities {
+@@ -968,6 +1082,8 @@ enum mkt_eth_capabilities {
  	BIT_ULL(MTK_ETH_MUX_GMAC2_GMAC0_TO_GEPHY_BIT)
  #define MTK_ETH_MUX_U3_GMAC2_TO_QPHY		\
  	BIT_ULL(MTK_ETH_MUX_U3_GMAC2_TO_QPHY_BIT)
@@ -868,7 +891,7 @@ Signed-off-by: Daniel Golle <[email protected]>
  #define MTK_ETH_MUX_GMAC1_GMAC2_TO_SGMII_RGMII	\
  	BIT_ULL(MTK_ETH_MUX_GMAC1_GMAC2_TO_SGMII_RGMII_BIT)
  #define MTK_ETH_MUX_GMAC12_TO_GEPHY_SGMII	\
-@@ -983,6 +1077,7 @@ enum mkt_eth_capabilities {
+@@ -983,6 +1099,7 @@ enum mkt_eth_capabilities {
  #define MTK_ETH_PATH_GMAC1_SGMII	BIT_ULL(MTK_ETH_PATH_GMAC1_SGMII_BIT)
  #define MTK_ETH_PATH_GMAC2_RGMII	BIT_ULL(MTK_ETH_PATH_GMAC2_RGMII_BIT)
  #define MTK_ETH_PATH_GMAC2_SGMII	BIT_ULL(MTK_ETH_PATH_GMAC2_SGMII_BIT)
@@ -876,7 +899,7 @@ Signed-off-by: Daniel Golle <[email protected]>
  #define MTK_ETH_PATH_GMAC2_GEPHY	BIT_ULL(MTK_ETH_PATH_GMAC2_GEPHY_BIT)
  #define MTK_ETH_PATH_GMAC3_SGMII	BIT_ULL(MTK_ETH_PATH_GMAC3_SGMII_BIT)
  #define MTK_ETH_PATH_GDM1_ESW		BIT_ULL(MTK_ETH_PATH_GDM1_ESW_BIT)
-@@ -996,6 +1091,7 @@ enum mkt_eth_capabilities {
+@@ -996,6 +1113,7 @@ enum mkt_eth_capabilities {
  #define MTK_GMAC2_RGMII		(MTK_ETH_PATH_GMAC2_RGMII | MTK_RGMII)
  #define MTK_GMAC2_SGMII		(MTK_ETH_PATH_GMAC2_SGMII | MTK_SGMII)
  #define MTK_GMAC2_GEPHY		(MTK_ETH_PATH_GMAC2_GEPHY | MTK_GEPHY)
@@ -884,7 +907,7 @@ Signed-off-by: Daniel Golle <[email protected]>
  #define MTK_GMAC3_SGMII		(MTK_ETH_PATH_GMAC3_SGMII | MTK_SGMII)
  #define MTK_GDM1_ESW		(MTK_ETH_PATH_GDM1_ESW | MTK_ESW)
  #define MTK_GMAC1_USXGMII	(MTK_ETH_PATH_GMAC1_USXGMII | MTK_USXGMII)
-@@ -1019,6 +1115,10 @@ enum mkt_eth_capabilities {
+@@ -1019,6 +1137,10 @@ enum mkt_eth_capabilities {
  	(MTK_ETH_MUX_GMAC1_GMAC2_TO_SGMII_RGMII | MTK_MUX | \
  	MTK_SHARED_SGMII)
  
@@ -895,7 +918,7 @@ Signed-off-by: Daniel Golle <[email protected]>
  /* 0: GMACx -> GEPHY, 1: GMACx -> SGMII where x is 1 or 2 */
  #define MTK_MUX_GMAC12_TO_GEPHY_SGMII   \
  	(MTK_ETH_MUX_GMAC12_TO_GEPHY_SGMII | MTK_MUX)
-@@ -1077,7 +1177,8 @@ enum mkt_eth_capabilities {
+@@ -1077,7 +1199,8 @@ enum mkt_eth_capabilities {
  		       MTK_MUX_GMAC123_TO_GEPHY_SGMII |		\
  		       MTK_NETSYS_V3 | MTK_RSTCTRL_PPE1 |	\
  		       MTK_GMAC1_USXGMII | MTK_GMAC2_USXGMII |	\
@@ -905,37 +928,54 @@ Signed-off-by: Daniel Golle <[email protected]>
  
  struct mtk_tx_dma_desc_info {
  	dma_addr_t	addr;
-@@ -1183,6 +1284,19 @@ struct mtk_soc_data {
+@@ -1183,6 +1306,22 @@ struct mtk_soc_data {
  
  #define MTK_DMA_MONITOR_TIMEOUT		msecs_to_jiffies(1000)
  
-+/* 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_usxgmii_pcs - This structure holds each usxgmii regmap and
++ *			associated data
++ * @regmap:		The register map pointing at the range used to setup
++ *			USXGMII modes
++ * @interface:		Currently selected interface mode
++ * @id:			The element is used to record the index of PCS
++ * @pcs:		Phylink PCS structure
 + */
-+struct mtk_xgmii {
-+	struct regmap	**regmap_usxgmii;
-+	struct regmap	**regmap_pextp;
-+	struct regmap	*regmap_pll;
++struct mtk_usxgmii_pcs {
++	struct mtk_eth		*eth;
++	struct regmap		*regmap;
++	phy_interface_t		interface;
++	u8			id;
++	struct phylink_pcs	pcs;
 +};
 +
  /* struct mtk_eth -	This is the main datasructure for holding the state
   *			of the driver
   * @dev:		The device pointer
-@@ -1244,7 +1358,9 @@ struct mtk_eth {
+@@ -1203,6 +1342,11 @@ struct mtk_soc_data {
+  * @infra:              The register map pointing at the range used to setup
+  *                      SGMII and GePHY path
+  * @sgmii_pcs:		Pointers to mtk-pcs-lynxi phylink_pcs instances
++ * @usxgmii_pll:	The register map pointing at the range used to control
++ *			the USXGMII SerDes PLL
++ * @regmap_pextp:	The register map pointing at the range used to setup
++ *			PHYA
++ * @usxgmii_pcs:	Pointer to array of pointers to struct for USXGMII PCS
+  * @pctl:		The register map pointing at the range used to setup
+  *			GMAC port drive/slew values
+  * @dma_refcnt:		track how many netdevs are using the DMA engine
+@@ -1244,7 +1388,11 @@ struct mtk_eth {
  	unsigned long			sysclk;
  	struct regmap			*ethsys;
  	struct regmap			*infra;
 +	struct regmap			*toprgu;
  	struct phylink_pcs		**sgmii_pcs;
-+	struct mtk_xgmii		*xgmii;
++	struct regmap			*usxgmii_pll;
++	struct regmap			**regmap_pextp;
++	struct mtk_usxgmii_pcs		**usxgmii_pcs;
  	struct regmap			*pctl;
  	bool				hwlro;
  	refcount_t			dma_refcnt;
-@@ -1400,6 +1516,19 @@ static inline u32 mtk_get_ib2_multicast_
+@@ -1400,6 +1548,19 @@ static inline u32 mtk_get_ib2_multicast_
  	return MTK_FOE_IB2_MULTICAST;
  }
  
@@ -955,7 +995,7 @@ Signed-off-by: Daniel Golle <[email protected]>
  /* read the hardware status register */
  void mtk_stats_update_mac(struct mtk_mac *mac);
  
-@@ -1407,8 +1536,10 @@ void mtk_w32(struct mtk_eth *eth, u32 va
+@@ -1407,8 +1568,10 @@ void mtk_w32(struct mtk_eth *eth, u32 va
  u32 mtk_r32(struct mtk_eth *eth, unsigned reg);
  
  int mtk_gmac_sgmii_path_setup(struct mtk_eth *eth, int mac_id);
@@ -966,50 +1006,35 @@ Signed-off-by: Daniel Golle <[email protected]>
  
  int mtk_eth_offload_init(struct mtk_eth *eth);
  int mtk_eth_setup_tc(struct net_device *dev, enum tc_setup_type type,
-@@ -1418,5 +1549,36 @@ int mtk_flow_offload_cmd(struct mtk_eth
+@@ -1418,5 +1581,20 @@ int mtk_flow_offload_cmd(struct mtk_eth
  void mtk_flow_offload_cleanup(struct mtk_eth *eth, struct list_head *list);
  void mtk_eth_set_dma_device(struct mtk_eth *eth, struct device *dma_dev);
  
 +#ifdef CONFIG_NET_MEDIATEK_SOC_USXGMII
++struct phylink_pcs *mtk_usxgmii_select_pcs(struct mtk_eth *eth, int id);
 +int mtk_usxgmii_init(struct mtk_eth *eth);
-+int mtk_xfi_pextp_init(struct mtk_eth *eth);
-+int mtk_xfi_pll_init(struct mtk_eth *eth);
-+int mtk_toprgu_init(struct mtk_eth *eth);
 +int mtk_xfi_pll_enable(struct mtk_eth *eth);
-+int mtk_usxgmii_setup_mode_an(struct mtk_eth *eth, int mac_id,
-+			      int max_speed);
-+int mtk_usxgmii_setup_mode_force(struct mtk_eth *eth, int mac_id,
-+				 const struct phylink_link_state *state);
-+void mtk_usxgmii_setup_phya_an_10000(struct mtk_eth *eth, int mac_id);
-+void mtk_usxgmii_reset(struct mtk_eth *eth, int mac_id);
 +void mtk_sgmii_setup_phya_gen1(struct mtk_eth *eth, int mac_id);
 +void mtk_sgmii_setup_phya_gen2(struct mtk_eth *eth, int mac_id);
 +void mtk_sgmii_reset(struct mtk_eth *eth, int mac_id);
 +#else
-+static inline int mtk_usxgmii_init(struct mtk_eth *eth) { return 0; };
-+static inline int mtk_xfi_pextp_init(struct mtk_eth *eth) { return 0; };
-+static inline int mtk_xfi_pll_init(struct mtk_eth *eth) { return 0; };
-+static inline int mtk_toprgu_init(struct mtk_eth *eth) { return 0; };
-+static inline int mtk_xfi_pll_enable(struct mtk_eth *eth) { return 0; };
-+static inline int mtk_usxgmii_setup_mode_an(struct mtk_eth *eth, int mac_id,
-+			      int max_speed) { return 0; };
-+static inline int mtk_usxgmii_setup_mode_force(struct mtk_eth *eth, int mac_id,
-+				 const struct phylink_link_state *state) { return 0; };
-+static inline void mtk_usxgmii_setup_phya_an_10000(struct mtk_eth *eth, int mac_id) { };
-+static inline void mtk_usxgmii_reset(struct mtk_eth *eth, int mac_id) { };
-+static inline void mtk_sgmii_setup_phya_gen1(struct mtk_eth *eth, int mac_id) { };
-+static inline void mtk_sgmii_setup_phya_gen2(struct mtk_eth *eth, int mac_id) { };
-+static inline void mtk_sgmii_reset(struct mtk_eth *eth, int mac_id) { };
-+#endif
++static inline struct phylink_pcs *mtk_usxgmii_select_pcs(struct mtk_eth *eth, int id) { return NULL; }
++static inline int mtk_usxgmii_init(struct mtk_eth *eth) { return 0; }
++static inline int mtk_xfi_pll_enable(struct mtk_eth *eth) { return 0; }
++static inline void mtk_sgmii_setup_phya_gen1(struct mtk_eth *eth, int mac_id) { }
++static inline void mtk_sgmii_setup_phya_gen2(struct mtk_eth *eth, int mac_id) { }
++static inline void mtk_sgmii_reset(struct mtk_eth *eth, int mac_id) { }
++#endif /* NET_MEDIATEK_SOC_USXGMII */
  
  #endif /* MTK_ETH_H */
 --- /dev/null
 +++ b/drivers/net/ethernet/mediatek/mtk_usxgmii.c
-@@ -0,0 +1,646 @@
+@@ -0,0 +1,835 @@
 +/* SPDX-License-Identifier: GPL-2.0
 + *
 + * Copyright (c) 2022 MediaTek Inc.
 + * Author: Henry Yen <[email protected]>
++ *         Daniel Golle <[email protected]>
 + */
 +
 +#include <linux/mfd/syscon.h>
@@ -1017,43 +1042,20 @@ Signed-off-by: Daniel Golle <[email protected]>
 +#include <linux/regmap.h>
 +#include "mtk_eth_soc.h"
 +
-+int mtk_usxgmii_init(struct mtk_eth *eth)
++static struct mtk_usxgmii_pcs *pcs_to_mtk_usxgmii_pcs(struct phylink_pcs *pcs)
 +{
-+	struct device_node *r = eth->dev->of_node;
-+	struct mtk_xgmii *xs = eth->xgmii;
-+	struct device *dev = eth->dev;
-+	struct device_node *np;
-+	int i;
-+
-+	xs->regmap_usxgmii = devm_kzalloc(dev, sizeof(*xs->regmap_usxgmii) *
-+					  eth->soc->num_devs, GFP_KERNEL);
-+	if (!xs->regmap_usxgmii)
-+		return -ENOMEM;
-+
-+	for (i = 0; i < eth->soc->num_devs; i++) {
-+		np = of_parse_phandle(r, "mediatek,usxgmiisys", i);
-+		if (!np)
-+			break;
-+
-+		xs->regmap_usxgmii[i] = syscon_node_to_regmap(np);
-+		if (IS_ERR(xs->regmap_usxgmii[i]))
-+			return PTR_ERR(xs->regmap_usxgmii[i]);
-+	}
-+
-+	return 0;
++	return container_of(pcs, struct mtk_usxgmii_pcs, pcs);
 +}
 +
-+int mtk_xfi_pextp_init(struct mtk_eth *eth)
++static int mtk_xfi_pextp_init(struct mtk_eth *eth)
 +{
 +	struct device *dev = eth->dev;
 +	struct device_node *r = dev->of_node;
-+	struct mtk_xgmii *xs = eth->xgmii;
 +	struct device_node *np;
 +	int i;
 +
-+	xs->regmap_pextp = devm_kzalloc(dev, sizeof(*xs->regmap_pextp) *
-+				        eth->soc->num_devs, GFP_KERNEL);
-+	if (!xs->regmap_pextp)
++	eth->regmap_pextp = devm_kcalloc(dev, eth->soc->num_devs, sizeof(eth->regmap_pextp), GFP_KERNEL);
++	if (!eth->regmap_pextp)
 +		return -ENOMEM;
 +
 +	for (i = 0; i < eth->soc->num_devs; i++) {
@@ -1061,32 +1063,31 @@ Signed-off-by: Daniel Golle <[email protected]>
 +		if (!np)
 +			break;
 +
-+		xs->regmap_pextp[i] = syscon_node_to_regmap(np);
-+		if (IS_ERR(xs->regmap_pextp[i]))
-+			return PTR_ERR(xs->regmap_pextp[i]);
++		eth->regmap_pextp[i] = syscon_node_to_regmap(np);
++		if (IS_ERR(eth->regmap_pextp[i]))
++			return PTR_ERR(eth->regmap_pextp[i]);
 +	}
 +
 +	return 0;
 +}
 +
-+int mtk_xfi_pll_init(struct mtk_eth *eth)
++static int mtk_xfi_pll_init(struct mtk_eth *eth)
 +{
 +	struct device_node *r = eth->dev->of_node;
-+	struct mtk_xgmii *xs = eth->xgmii;
 +	struct device_node *np;
 +
 +	np = of_parse_phandle(r, "mediatek,xfi_pll", 0);
 +	if (!np)
 +		return -1;
 +
-+	xs->regmap_pll = syscon_node_to_regmap(np);
-+	if (IS_ERR(xs->regmap_pll))
-+		return PTR_ERR(xs->regmap_pll);
++	eth->usxgmii_pll = syscon_node_to_regmap(np);
++	if (IS_ERR(eth->usxgmii_pll))
++		return PTR_ERR(eth->usxgmii_pll);
 +
 +	return 0;
 +}
 +
-+int mtk_toprgu_init(struct mtk_eth *eth)
++static int mtk_toprgu_init(struct mtk_eth *eth)
 +{
 +	struct device_node *r = eth->dev->of_node;
 +	struct device_node *np;
@@ -1104,18 +1105,17 @@ Signed-off-by: Daniel Golle <[email protected]>
 +
 +int mtk_xfi_pll_enable(struct mtk_eth *eth)
 +{
-+	struct mtk_xgmii *xs = eth->xgmii;
 +	u32 val = 0;
 +
-+	if (!xs->regmap_pll)
++	if (!eth->usxgmii_pll)
 +		return -EINVAL;
 +
 +	/* Add software workaround for USXGMII PLL TCL issue */
-+	regmap_write(xs->regmap_pll, XFI_PLL_ANA_GLB8, RG_XFI_PLL_ANA_SWWA);
++	regmap_write(eth->usxgmii_pll, XFI_PLL_ANA_GLB8, RG_XFI_PLL_ANA_SWWA);
 +
-+	regmap_read(xs->regmap_pll, XFI_PLL_DIG_GLB8, &val);
++	regmap_read(eth->usxgmii_pll, XFI_PLL_DIG_GLB8, &val);
 +	val |= RG_XFI_PLL_EN;
-+	regmap_write(xs->regmap_pll, XFI_PLL_DIG_GLB8, val);
++	regmap_write(eth->usxgmii_pll, XFI_PLL_DIG_GLB8, val);
 +
 +	return 0;
 +}
@@ -1141,531 +1141,727 @@ Signed-off-by: Daniel Golle <[email protected]>
 +	return xgmii_id;
 +}
 +
-+void mtk_usxgmii_setup_phya_an_10000(struct mtk_eth *eth, int mac_id)
++static int mtk_xgmii2mac_id(struct mtk_eth *eth, int xgmii_id)
 +{
-+	struct mtk_xgmii *xs = eth->xgmii;
-+	u32 id = mtk_mac2xgmii_id(eth, mac_id);
++	int mac_id = xgmii_id;
++
++	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:
++			mac_id = -1;
++		}
++	}
++
++	return mac_id;
++}
++
 +
-+	if (id >= eth->soc->num_devs ||
-+	    !xs->regmap_usxgmii[id] || !xs->regmap_pextp[id])
++static void mtk_usxgmii_setup_phya_usxgmii(struct mtk_usxgmii_pcs *mpcs)
++{
++	struct regmap *pextp;
++
++	if (!mpcs->eth)
 +		return;
 +
-+	regmap_write(xs->regmap_usxgmii[id], RG_PCS_AN_CTRL0, 0x000FFE6D);
-+	regmap_write(xs->regmap_usxgmii[id], 0x818, 0x07B1EC7B);
-+	regmap_write(xs->regmap_usxgmii[id], RG_PHY_TOP_SPEED_CTRL1, 0x30000000);
-+	ndelay(1020);
-+	regmap_write(xs->regmap_usxgmii[id], RG_PHY_TOP_SPEED_CTRL1, 0x10000000);
-+	ndelay(1020);
-+	regmap_write(xs->regmap_usxgmii[id], RG_PHY_TOP_SPEED_CTRL1, 0x00000000);
-+
-+	regmap_write(xs->regmap_pextp[id], 0x9024, 0x00C9071C);
-+	regmap_write(xs->regmap_pextp[id], 0x2020, 0xAA8585AA);
-+	regmap_write(xs->regmap_pextp[id], 0x2030, 0x0C020707);
-+	regmap_write(xs->regmap_pextp[id], 0x2034, 0x0E050F0F);
-+	regmap_write(xs->regmap_pextp[id], 0x2040, 0x00140032);
-+	regmap_write(xs->regmap_pextp[id], 0x50F0, 0x00C014AA);
-+	regmap_write(xs->regmap_pextp[id], 0x50E0, 0x3777C12B);
-+	regmap_write(xs->regmap_pextp[id], 0x506C, 0x005F9CFF);
-+	regmap_write(xs->regmap_pextp[id], 0x5070, 0x9D9DFAFA);
-+	regmap_write(xs->regmap_pextp[id], 0x5074, 0x27273F3F);
-+	regmap_write(xs->regmap_pextp[id], 0x5078, 0xA7883C68);
-+	regmap_write(xs->regmap_pextp[id], 0x507C, 0x11661166);
-+	regmap_write(xs->regmap_pextp[id], 0x5080, 0x0E000AAF);
-+	regmap_write(xs->regmap_pextp[id], 0x5084, 0x08080D0D);
-+	regmap_write(xs->regmap_pextp[id], 0x5088, 0x02030909);
-+	regmap_write(xs->regmap_pextp[id], 0x50E4, 0x0C0C0000);
-+	regmap_write(xs->regmap_pextp[id], 0x50E8, 0x04040000);
-+	regmap_write(xs->regmap_pextp[id], 0x50EC, 0x0F0F0C06);
-+	regmap_write(xs->regmap_pextp[id], 0x50A8, 0x506E8C8C);
-+	regmap_write(xs->regmap_pextp[id], 0x6004, 0x18190000);
-+	regmap_write(xs->regmap_pextp[id], 0x00F8, 0x01423342);
-+	regmap_write(xs->regmap_pextp[id], 0x00F4, 0x80201F20);
-+	regmap_write(xs->regmap_pextp[id], 0x0030, 0x00050C00);
-+	regmap_write(xs->regmap_pextp[id], 0x0070, 0x02002800);
++	pextp = mpcs->eth->regmap_pextp[mpcs->id];
++	if (!pextp)
++		return;
++
++	/* Setup operation mode */
++	regmap_write(pextp, 0x9024, 0x00C9071C);
++	regmap_write(pextp, 0x2020, 0xAA8585AA);
++	regmap_write(pextp, 0x2030, 0x0C020707);
++	regmap_write(pextp, 0x2034, 0x0E050F0F);
++	regmap_write(pextp, 0x2040, 0x00140032);
++	regmap_write(pextp, 0x50F0, 0x00C014AA);
++	regmap_write(pextp, 0x50E0, 0x3777C12B);
++	regmap_write(pextp, 0x506C, 0x005F9CFF);
++	regmap_write(pextp, 0x5070, 0x9D9DFAFA);
++	regmap_write(pextp, 0x5074, 0x27273F3F);
++	regmap_write(pextp, 0x5078, 0xA7883C68);
++	regmap_write(pextp, 0x507C, 0x11661166);
++	regmap_write(pextp, 0x5080, 0x0E000AAF);
++	regmap_write(pextp, 0x5084, 0x08080D0D);
++	regmap_write(pextp, 0x5088, 0x02030909);
++	regmap_write(pextp, 0x50E4, 0x0C0C0000);
++	regmap_write(pextp, 0x50E8, 0x04040000);
++	regmap_write(pextp, 0x50EC, 0x0F0F0C06);
++	regmap_write(pextp, 0x50A8, 0x506E8C8C);
++	regmap_write(pextp, 0x6004, 0x18190000);
++	regmap_write(pextp, 0x00F8, 0x01423342);
++	/* Force SGDT_OUT off and select PCS */
++	regmap_write(pextp, 0x00F4, 0x80201F20);
++	/* Force GLB_CKDET_OUT */
++	regmap_write(pextp, 0x0030, 0x00050C00);
++	/* Force AEQ on */
++	regmap_write(pextp, 0x0070, 0x02002800);
 +	ndelay(1020);
-+	regmap_write(xs->regmap_pextp[id], 0x30B0, 0x00000020);
-+	regmap_write(xs->regmap_pextp[id], 0x3028, 0x00008A01);
-+	regmap_write(xs->regmap_pextp[id], 0x302C, 0x0000A884);
-+	regmap_write(xs->regmap_pextp[id], 0x3024, 0x00083002);
-+	regmap_write(xs->regmap_pextp[id], 0x3010, 0x00022220);
-+	regmap_write(xs->regmap_pextp[id], 0x5064, 0x0F020A01);
-+	regmap_write(xs->regmap_pextp[id], 0x50B4, 0x06100600);
-+	regmap_write(xs->regmap_pextp[id], 0x3048, 0x40704000);
-+	regmap_write(xs->regmap_pextp[id], 0x3050, 0xA8000000);
-+	regmap_write(xs->regmap_pextp[id], 0x3054, 0x000000AA);
-+	regmap_write(xs->regmap_pextp[id], 0x306C, 0x00000F00);
-+	regmap_write(xs->regmap_pextp[id], 0xA060, 0x00040000);
-+	regmap_write(xs->regmap_pextp[id], 0x90D0, 0x00000001);
-+	regmap_write(xs->regmap_pextp[id], 0x0070, 0x0200E800);
++	/* Setup DA default value */
++	regmap_write(pextp, 0x30B0, 0x00000020);
++	regmap_write(pextp, 0x3028, 0x00008A01);
++	regmap_write(pextp, 0x302C, 0x0000A884);
++	regmap_write(pextp, 0x3024, 0x00083002);
++	regmap_write(pextp, 0x3010, 0x00022220);
++	regmap_write(pextp, 0x5064, 0x0F020A01);
++	regmap_write(pextp, 0x50B4, 0x06100600);
++	regmap_write(pextp, 0x3048, 0x40704000);
++	regmap_write(pextp, 0x3050, 0xA8000000);
++	regmap_write(pextp, 0x3054, 0x000000AA);
++	regmap_write(pextp, 0x306C, 0x00000F00);
++	regmap_write(pextp, 0xA060, 0x00040000);
++	regmap_write(pextp, 0x90D0, 0x00000001);
++	/* Release reset */
++	regmap_write(pextp, 0x0070, 0x0200E800);
 +	udelay(150);
-+	regmap_write(xs->regmap_pextp[id], 0x0070, 0x0200C111);
++	/* Switch to P0 */
++	regmap_write(pextp, 0x0070, 0x0200C111);
 +	ndelay(1020);
-+	regmap_write(xs->regmap_pextp[id], 0x0070, 0x0200C101);
++	regmap_write(pextp, 0x0070, 0x0200C101);
 +	udelay(15);
-+	regmap_write(xs->regmap_pextp[id], 0x0070, 0x0202C111);
++	/* Switch to Gen3 */
++	regmap_write(pextp, 0x0070, 0x0202C111);
 +	ndelay(1020);
-+	regmap_write(xs->regmap_pextp[id], 0x0070, 0x0202C101);
++	regmap_write(pextp, 0x0070, 0x0202C101);
 +	udelay(100);
-+	regmap_write(xs->regmap_pextp[id], 0x30B0, 0x00000030);
-+	regmap_write(xs->regmap_pextp[id], 0x00F4, 0x80201F00);
-+	regmap_write(xs->regmap_pextp[id], 0x3040, 0x30000000);
++	regmap_write(pextp, 0x30B0, 0x00000030);
++	regmap_write(pextp, 0x00F4, 0x80201F00);
++	regmap_write(pextp, 0x3040, 0x30000000);
 +	udelay(400);
 +}
 +
-+void mtk_usxgmii_setup_phya_force_5000(struct mtk_eth *eth, int mac_id)
++static void mtk_usxgmii_setup_phya_5gbaser(struct mtk_usxgmii_pcs *mpcs)
 +{
-+	unsigned int val;
-+	struct mtk_xgmii *xs = eth->xgmii;
-+	u32 id = mtk_mac2xgmii_id(eth, mac_id);
++	struct regmap *pextp;
 +
-+	if (id >= eth->soc->num_devs ||
-+	    !xs->regmap_usxgmii[id] || !xs->regmap_pextp[id])
++	if (!mpcs->eth)
 +		return;
 +
-+	/* 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(xs->regmap_usxgmii[id], RG_PHY_TOP_SPEED_CTRL1, val);
-+
-+	/* Disable USXGMII AN mode */
-+	regmap_read(xs->regmap_usxgmii[id], RG_PCS_AN_CTRL0, &val);
-+	val &= ~RG_AN_ENABLE;
-+	regmap_write(xs->regmap_usxgmii[id], RG_PCS_AN_CTRL0, val);
-+
-+	/* Gated USXGMII */
-+	regmap_read(xs->regmap_usxgmii[id], RG_PHY_TOP_SPEED_CTRL1, &val);
-+	val |= RG_MAC_CK_GATED;
-+	regmap_write(xs->regmap_usxgmii[id], RG_PHY_TOP_SPEED_CTRL1, val);
-+
-+	ndelay(1020);
-+
-+	/* USXGMII force mode setting */
-+	regmap_read(xs->regmap_usxgmii[id], 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(xs->regmap_usxgmii[id], RG_PHY_TOP_SPEED_CTRL1, val);
-+
-+	/* Un-gated USXGMII */
-+	regmap_read(xs->regmap_usxgmii[id], RG_PHY_TOP_SPEED_CTRL1, &val);
-+	val &= ~RG_MAC_CK_GATED;
-+	regmap_write(xs->regmap_usxgmii[id], RG_PHY_TOP_SPEED_CTRL1, val);
-+
-+	ndelay(1020);
++	pextp = mpcs->eth->regmap_pextp[mpcs->id];
++	if (!pextp)
++		return;
 +
-+	regmap_write(xs->regmap_pextp[id], 0x9024, 0x00D9071C);
-+	regmap_write(xs->regmap_pextp[id], 0x2020, 0xAAA5A5AA);
-+	regmap_write(xs->regmap_pextp[id], 0x2030, 0x0C020707);
-+	regmap_write(xs->regmap_pextp[id], 0x2034, 0x0E050F0F);
-+	regmap_write(xs->regmap_pextp[id], 0x2040, 0x00140032);
-+	regmap_write(xs->regmap_pextp[id], 0x50F0, 0x00C018AA);
-+	regmap_write(xs->regmap_pextp[id], 0x50E0, 0x3777812B);
-+	regmap_write(xs->regmap_pextp[id], 0x506C, 0x005C9CFF);
-+	regmap_write(xs->regmap_pextp[id], 0x5070, 0x9DFAFAFA);
-+	regmap_write(xs->regmap_pextp[id], 0x5074, 0x273F3F3F);
-+	regmap_write(xs->regmap_pextp[id], 0x5078, 0xA8883868);
-+	regmap_write(xs->regmap_pextp[id], 0x507C, 0x14661466);
-+	regmap_write(xs->regmap_pextp[id], 0x5080, 0x0E001ABF);
-+	regmap_write(xs->regmap_pextp[id], 0x5084, 0x080B0D0D);
-+	regmap_write(xs->regmap_pextp[id], 0x5088, 0x02050909);
-+	regmap_write(xs->regmap_pextp[id], 0x50E4, 0x0C000000);
-+	regmap_write(xs->regmap_pextp[id], 0x50E8, 0x04000000);
-+	regmap_write(xs->regmap_pextp[id], 0x50EC, 0x0F0F0C06);
-+	regmap_write(xs->regmap_pextp[id], 0x50A8, 0x50808C8C);
-+	regmap_write(xs->regmap_pextp[id], 0x6004, 0x18000000);
-+	regmap_write(xs->regmap_pextp[id], 0x00F8, 0x00A132A1);
-+	regmap_write(xs->regmap_pextp[id], 0x00F4, 0x80201F20);
-+	regmap_write(xs->regmap_pextp[id], 0x0030, 0x00050C00);
-+	regmap_write(xs->regmap_pextp[id], 0x0070, 0x02002800);
++	/* Setup operation mode */
++	regmap_write(pextp, 0x9024, 0x00D9071C);
++	regmap_write(pextp, 0x2020, 0xAAA5A5AA);
++	regmap_write(pextp, 0x2030, 0x0C020707);
++	regmap_write(pextp, 0x2034, 0x0E050F0F);
++	regmap_write(pextp, 0x2040, 0x00140032);
++	regmap_write(pextp, 0x50F0, 0x00C018AA);
++	regmap_write(pextp, 0x50E0, 0x3777812B);
++	regmap_write(pextp, 0x506C, 0x005C9CFF);
++	regmap_write(pextp, 0x5070, 0x9DFAFAFA);
++	regmap_write(pextp, 0x5074, 0x273F3F3F);
++	regmap_write(pextp, 0x5078, 0xA8883868);
++	regmap_write(pextp, 0x507C, 0x14661466);
++	regmap_write(pextp, 0x5080, 0x0E001ABF);
++	regmap_write(pextp, 0x5084, 0x080B0D0D);
++	regmap_write(pextp, 0x5088, 0x02050909);
++	regmap_write(pextp, 0x50E4, 0x0C000000);
++	regmap_write(pextp, 0x50E8, 0x04000000);
++	regmap_write(pextp, 0x50EC, 0x0F0F0C06);
++	regmap_write(pextp, 0x50A8, 0x50808C8C);
++	regmap_write(pextp, 0x6004, 0x18000000);
++	regmap_write(pextp, 0x00F8, 0x00A132A1);
++	/* Force SGDT_OUT off and select PCS */
++	regmap_write(pextp, 0x00F4, 0x80201F20);
++	/* Force GLB_CKDET_OUT */
++	regmap_write(pextp, 0x0030, 0x00050C00);
++	/* Force AEQ on */
++	regmap_write(pextp, 0x0070, 0x02002800);
 +	ndelay(1020);
-+	regmap_write(xs->regmap_pextp[id], 0x30B0, 0x00000020);
-+	regmap_write(xs->regmap_pextp[id], 0x3028, 0x00008A01);
-+	regmap_write(xs->regmap_pextp[id], 0x302C, 0x0000A884);
-+	regmap_write(xs->regmap_pextp[id], 0x3024, 0x00083002);
-+	regmap_write(xs->regmap_pextp[id], 0x3010, 0x00022220);
-+	regmap_write(xs->regmap_pextp[id], 0x5064, 0x0F020A01);
-+	regmap_write(xs->regmap_pextp[id], 0x50B4, 0x06100600);
-+	regmap_write(xs->regmap_pextp[id], 0x3048, 0x40704000);
-+	regmap_write(xs->regmap_pextp[id], 0x3050, 0xA8000000);
-+	regmap_write(xs->regmap_pextp[id], 0x3054, 0x000000AA);
-+	regmap_write(xs->regmap_pextp[id], 0x306C, 0x00000F00);
-+	regmap_write(xs->regmap_pextp[id], 0xA060, 0x00040000);
-+	regmap_write(xs->regmap_pextp[id], 0x90D0, 0x00000003);
-+	regmap_write(xs->regmap_pextp[id], 0x0070, 0x0200E800);
++	/* Setup DA default value */
++	regmap_write(pextp, 0x30B0, 0x00000020);
++	regmap_write(pextp, 0x3028, 0x00008A01);
++	regmap_write(pextp, 0x302C, 0x0000A884);
++	regmap_write(pextp, 0x3024, 0x00083002);
++	regmap_write(pextp, 0x3010, 0x00022220);
++	regmap_write(pextp, 0x5064, 0x0F020A01);
++	regmap_write(pextp, 0x50B4, 0x06100600);
++	regmap_write(pextp, 0x3048, 0x40704000);
++	regmap_write(pextp, 0x3050, 0xA8000000);
++	regmap_write(pextp, 0x3054, 0x000000AA);
++	regmap_write(pextp, 0x306C, 0x00000F00);
++	regmap_write(pextp, 0xA060, 0x00040000);
++	regmap_write(pextp, 0x90D0, 0x00000003);
++	/* Release reset */
++	regmap_write(pextp, 0x0070, 0x0200E800);
 +	udelay(150);
-+	regmap_write(xs->regmap_pextp[id], 0x0070, 0x0200C111);
++	/* Switch to P0 */
++	regmap_write(pextp, 0x0070, 0x0200C111);
 +	ndelay(1020);
-+	regmap_write(xs->regmap_pextp[id], 0x0070, 0x0200C101);
++	regmap_write(pextp, 0x0070, 0x0200C101);
 +	udelay(15);
-+	regmap_write(xs->regmap_pextp[id], 0x0070, 0x0202C111);
++	/* Switch to Gen3 */
++	regmap_write(pextp, 0x0070, 0x0202C111);
 +	ndelay(1020);
-+	regmap_write(xs->regmap_pextp[id], 0x0070, 0x0202C101);
++	regmap_write(pextp, 0x0070, 0x0202C101);
 +	udelay(100);
-+	regmap_write(xs->regmap_pextp[id], 0x30B0, 0x00000030);
-+	regmap_write(xs->regmap_pextp[id], 0x00F4, 0x80201F00);
-+	regmap_write(xs->regmap_pextp[id], 0x3040, 0x30000000);
++	regmap_write(pextp, 0x30B0, 0x00000030);
++	regmap_write(pextp, 0x00F4, 0x80201F00);
++	regmap_write(pextp, 0x3040, 0x30000000);
 +	udelay(400);
 +}
 +
-+void mtk_usxgmii_setup_phya_force_10000(struct mtk_eth *eth, int mac_id)
++static void mtk_usxgmii_setup_phya_10gbaser(struct mtk_usxgmii_pcs *mpcs)
 +{
-+	struct mtk_xgmii *xs = eth->xgmii;
-+	unsigned int val;
-+	u32 id = mtk_mac2xgmii_id(eth, mac_id);
++	struct regmap *pextp;
 +
-+	if (id >= eth->soc->num_devs ||
-+	    !xs->regmap_usxgmii[id] || !xs->regmap_pextp[id])
++	if (!mpcs->eth)
 +		return;
 +
-+	/* 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(xs->regmap_usxgmii[id], RG_PHY_TOP_SPEED_CTRL1, val);
-+
-+	/* Disable USXGMII AN mode */
-+	regmap_read(xs->regmap_usxgmii[id], RG_PCS_AN_CTRL0, &val);
-+	val &= ~RG_AN_ENABLE;
-+	regmap_write(xs->regmap_usxgmii[id], RG_PCS_AN_CTRL0, val);
-+
-+	/* Gated USXGMII */
-+	regmap_read(xs->regmap_usxgmii[id], RG_PHY_TOP_SPEED_CTRL1, &val);
-+	val |= RG_MAC_CK_GATED;
-+	regmap_write(xs->regmap_usxgmii[id], RG_PHY_TOP_SPEED_CTRL1, val);
-+
-+	ndelay(1020);
-+
-+	/* USXGMII force mode setting */
-+	regmap_read(xs->regmap_usxgmii[id], 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(xs->regmap_usxgmii[id], RG_PHY_TOP_SPEED_CTRL1, val);
-+
-+	/* Un-gated USXGMII */
-+	regmap_read(xs->regmap_usxgmii[id], RG_PHY_TOP_SPEED_CTRL1, &val);
-+	val &= ~RG_MAC_CK_GATED;
-+	regmap_write(xs->regmap_usxgmii[id], RG_PHY_TOP_SPEED_CTRL1, val);
-+
-+	ndelay(1020);
++	pextp = mpcs->eth->regmap_pextp[mpcs->id];
++	if (!pextp)
++		return;
 +
-+	regmap_write(xs->regmap_pextp[id], 0x9024, 0x00C9071C);
-+	regmap_write(xs->regmap_pextp[id], 0x2020, 0xAA8585AA);
-+	regmap_write(xs->regmap_pextp[id], 0x2030, 0x0C020707);
-+	regmap_write(xs->regmap_pextp[id], 0x2034, 0x0E050F0F);
-+	regmap_write(xs->regmap_pextp[id], 0x2040, 0x00140032);
-+	regmap_write(xs->regmap_pextp[id], 0x50F0, 0x00C014AA);
-+	regmap_write(xs->regmap_pextp[id], 0x50E0, 0x3777C12B);
-+	regmap_write(xs->regmap_pextp[id], 0x506C, 0x005F9CFF);
-+	regmap_write(xs->regmap_pextp[id], 0x5070, 0x9D9DFAFA);
-+	regmap_write(xs->regmap_pextp[id], 0x5074, 0x27273F3F);
-+	regmap_write(xs->regmap_pextp[id], 0x5078, 0xA7883C68);
-+	regmap_write(xs->regmap_pextp[id], 0x507C, 0x11661166);
-+	regmap_write(xs->regmap_pextp[id], 0x5080, 0x0E000AAF);
-+	regmap_write(xs->regmap_pextp[id], 0x5084, 0x08080D0D);
-+	regmap_write(xs->regmap_pextp[id], 0x5088, 0x02030909);
-+	regmap_write(xs->regmap_pextp[id], 0x50E4, 0x0C0C0000);
-+	regmap_write(xs->regmap_pextp[id], 0x50E8, 0x04040000);
-+	regmap_write(xs->regmap_pextp[id], 0x50EC, 0x0F0F0C06);
-+	regmap_write(xs->regmap_pextp[id], 0x50A8, 0x506E8C8C);
-+	regmap_write(xs->regmap_pextp[id], 0x6004, 0x18190000);
-+	regmap_write(xs->regmap_pextp[id], 0x00F8, 0x01423342);
-+	regmap_write(xs->regmap_pextp[id], 0x00F4, 0x80201F20);
-+	regmap_write(xs->regmap_pextp[id], 0x0030, 0x00050C00);
-+	regmap_write(xs->regmap_pextp[id], 0x0070, 0x02002800);
++	/* Setup operation mode */
++	regmap_write(pextp, 0x9024, 0x00C9071C);
++	regmap_write(pextp, 0x2020, 0xAA8585AA);
++	regmap_write(pextp, 0x2030, 0x0C020707);
++	regmap_write(pextp, 0x2034, 0x0E050F0F);
++	regmap_write(pextp, 0x2040, 0x00140032);
++	regmap_write(pextp, 0x50F0, 0x00C014AA);
++	regmap_write(pextp, 0x50E0, 0x3777C12B);
++	regmap_write(pextp, 0x506C, 0x005F9CFF);
++	regmap_write(pextp, 0x5070, 0x9D9DFAFA);
++	regmap_write(pextp, 0x5074, 0x27273F3F);
++	regmap_write(pextp, 0x5078, 0xA7883C68);
++	regmap_write(pextp, 0x507C, 0x11661166);
++	regmap_write(pextp, 0x5080, 0x0E000AAF);
++	regmap_write(pextp, 0x5084, 0x08080D0D);
++	regmap_write(pextp, 0x5088, 0x02030909);
++	regmap_write(pextp, 0x50E4, 0x0C0C0000);
++	regmap_write(pextp, 0x50E8, 0x04040000);
++	regmap_write(pextp, 0x50EC, 0x0F0F0C06);
++	regmap_write(pextp, 0x50A8, 0x506E8C8C);
++	regmap_write(pextp, 0x6004, 0x18190000);
++	regmap_write(pextp, 0x00F8, 0x01423342);
++	/* Force SGDT_OUT off and select PCS */
++	regmap_write(pextp, 0x00F4, 0x80201F20);
++	/* Force GLB_CKDET_OUT */
++	regmap_write(pextp, 0x0030, 0x00050C00);
++	/* Force AEQ on */
++	regmap_write(pextp, 0x0070, 0x02002800);
 +	ndelay(1020);
-+	regmap_write(xs->regmap_pextp[id], 0x30B0, 0x00000020);
-+	regmap_write(xs->regmap_pextp[id], 0x3028, 0x00008A01);
-+	regmap_write(xs->regmap_pextp[id], 0x302C, 0x0000A884);
-+	regmap_write(xs->regmap_pextp[id], 0x3024, 0x00083002);
-+	regmap_write(xs->regmap_pextp[id], 0x3010, 0x00022220);
-+	regmap_write(xs->regmap_pextp[id], 0x5064, 0x0F020A01);
-+	regmap_write(xs->regmap_pextp[id], 0x50B4, 0x06100600);
-+	regmap_write(xs->regmap_pextp[id], 0x3048, 0x49664100);
-+	regmap_write(xs->regmap_pextp[id], 0x3050, 0x00000000);
-+	regmap_write(xs->regmap_pextp[id], 0x3054, 0x00000000);
-+	regmap_write(xs->regmap_pextp[id], 0x306C, 0x00000F00);
-+	regmap_write(xs->regmap_pextp[id], 0xA060, 0x00040000);
-+	regmap_write(xs->regmap_pextp[id], 0x90D0, 0x00000001);
-+	regmap_write(xs->regmap_pextp[id], 0x0070, 0x0200E800);
++	/* Setup DA default value */
++	regmap_write(pextp, 0x30B0, 0x00000020);
++	regmap_write(pextp, 0x3028, 0x00008A01);
++	regmap_write(pextp, 0x302C, 0x0000A884);
++	regmap_write(pextp, 0x3024, 0x00083002);
++	regmap_write(pextp, 0x3010, 0x00022220);
++	regmap_write(pextp, 0x5064, 0x0F020A01);
++	regmap_write(pextp, 0x50B4, 0x06100600);
++	regmap_write(pextp, 0x3048, 0x47684100);
++	regmap_write(pextp, 0x3050, 0x00000000);
++	regmap_write(pextp, 0x3054, 0x00000000);
++	regmap_write(pextp, 0x306C, 0x00000F00);
++	if (mpcs->id == 0)
++		regmap_write(pextp, 0xA008, 0x0007B400);
++
++	regmap_write(pextp, 0xA060, 0x00040000);
++	regmap_write(pextp, 0x90D0, 0x00000001);
++	/* Release reset */
++	regmap_write(pextp, 0x0070, 0x0200E800);
 +	udelay(150);
-+	regmap_write(xs->regmap_pextp[id], 0x0070, 0x0200C111);
++	/* Switch to P0 */
++	regmap_write(pextp, 0x0070, 0x0200C111);
 +	ndelay(1020);
-+	regmap_write(xs->regmap_pextp[id], 0x0070, 0x0200C101);
++	regmap_write(pextp, 0x0070, 0x0200C101);
 +	udelay(15);
-+	regmap_write(xs->regmap_pextp[id], 0x0070, 0x0202C111);
++	/* Switch to Gen3 */
++	regmap_write(pextp, 0x0070, 0x0202C111);
 +	ndelay(1020);
-+	regmap_write(xs->regmap_pextp[id], 0x0070, 0x0202C101);
++	regmap_write(pextp, 0x0070, 0x0202C101);
 +	udelay(100);
-+	regmap_write(xs->regmap_pextp[id], 0x30B0, 0x00000030);
-+	regmap_write(xs->regmap_pextp[id], 0x00F4, 0x80201F00);
-+	regmap_write(xs->regmap_pextp[id], 0x3040, 0x30000000);
++	regmap_write(pextp, 0x30B0, 0x00000030);
++	regmap_write(pextp, 0x00F4, 0x80201F00);
++	regmap_write(pextp, 0x3040, 0x30000000);
 +	udelay(400);
 +}
 +
-+void mtk_usxgmii_reset(struct mtk_eth *eth, int mac_id)
++void mtk_sgmii_setup_phya_gen1(struct mtk_eth *eth, int mac_id)
 +{
 +	u32 id = mtk_mac2xgmii_id(eth, mac_id);
++	struct regmap *pextp;
 +
-+	if (id >= eth->soc->num_devs || !eth->toprgu)
++	if (id >= eth->soc->num_devs)
 +		return;
 +
-+	switch (mac_id) {
-+	case MTK_GMAC2_ID:
-+		regmap_write(eth->toprgu, 0xFC, 0x0000A004);
-+		regmap_write(eth->toprgu, 0x18, 0x88F0A004);
-+		regmap_write(eth->toprgu, 0xFC, 0x00000000);
-+		regmap_write(eth->toprgu, 0x18, 0x88F00000);
-+		regmap_write(eth->toprgu, 0x18, 0x00F00000);
-+		break;
-+	case MTK_GMAC3_ID:
-+		regmap_write(eth->toprgu, 0xFC, 0x00005002);
-+		regmap_write(eth->toprgu, 0x18, 0x88F05002);
-+		regmap_write(eth->toprgu, 0xFC, 0x00000000);
-+		regmap_write(eth->toprgu, 0x18, 0x88F00000);
-+		regmap_write(eth->toprgu, 0x18, 0x00F00000);
-+		break;
-+	}
-+
-+	mdelay(10);
-+}
-+
-+int mtk_usxgmii_setup_mode_an(struct mtk_eth *eth, int mac_id, int max_speed)
-+{
-+	if (mac_id < 0 || mac_id >= eth->soc->num_devs)
-+		return -EINVAL;
-+
-+	if ((max_speed != SPEED_10000) && (max_speed != SPEED_5000))
-+		return -EINVAL;
-+
-+	mtk_xfi_pll_enable(eth);
-+	mtk_usxgmii_reset(eth, mac_id);
-+	mtk_usxgmii_setup_phya_an_10000(eth, mac_id);
-+
-+	return 0;
-+}
-+
-+int mtk_usxgmii_setup_mode_force(struct mtk_eth *eth, int mac_id,
-+				 const struct phylink_link_state *state)
-+{
-+	if (mac_id < 0 || mac_id >= eth->soc->num_devs)
-+		return -EINVAL;
-+
-+	mtk_xfi_pll_enable(eth);
-+	mtk_usxgmii_reset(eth, mac_id);
-+	if (state->interface == PHY_INTERFACE_MODE_5GBASER)
-+		mtk_usxgmii_setup_phya_force_5000(eth, mac_id);
-+	else
-+		mtk_usxgmii_setup_phya_force_10000(eth, mac_id);
-+
-+	return 0;
-+}
-+
-+void mtk_sgmii_setup_phya_gen1(struct mtk_eth *eth, int mac_id)
-+{
-+	u32 id = mtk_mac2xgmii_id(eth, mac_id);
-+	struct mtk_xgmii *xs = eth->xgmii;
-+
-+	if (id >= eth->soc->num_devs || !xs->regmap_pextp[id])
++	pextp = eth->regmap_pextp[id];
++	if (!pextp)
 +		return;
 +
-+	regmap_write(xs->regmap_pextp[id], 0x9024, 0x00D9071C);
-+	regmap_write(xs->regmap_pextp[id], 0x2020, 0xAA8585AA);
-+	regmap_write(xs->regmap_pextp[id], 0x2030, 0x0C020207);
-+	regmap_write(xs->regmap_pextp[id], 0x2034, 0x0E05050F);
-+	regmap_write(xs->regmap_pextp[id], 0x2040, 0x00200032);
-+	regmap_write(xs->regmap_pextp[id], 0x50F0, 0x00C014BA);
-+	regmap_write(xs->regmap_pextp[id], 0x50E0, 0x3777C12B);
-+	regmap_write(xs->regmap_pextp[id], 0x506C, 0x005F9CFF);
-+	regmap_write(xs->regmap_pextp[id], 0x5070, 0x9D9DFAFA);
-+	regmap_write(xs->regmap_pextp[id], 0x5074, 0x27273F3F);
-+	regmap_write(xs->regmap_pextp[id], 0x5078, 0xA7883C68);
-+	regmap_write(xs->regmap_pextp[id], 0x507C, 0x11661166);
-+	regmap_write(xs->regmap_pextp[id], 0x5080, 0x0E000EAF);
-+	regmap_write(xs->regmap_pextp[id], 0x5084, 0x08080E0D);
-+	regmap_write(xs->regmap_pextp[id], 0x5088, 0x02030B09);
-+	regmap_write(xs->regmap_pextp[id], 0x50E4, 0x0C0C0000);
-+	regmap_write(xs->regmap_pextp[id], 0x50E8, 0x04040000);
-+	regmap_write(xs->regmap_pextp[id], 0x50EC, 0x0F0F0606);
-+	regmap_write(xs->regmap_pextp[id], 0x50A8, 0x506E8C8C);
-+	regmap_write(xs->regmap_pextp[id], 0x6004, 0x18190000);
-+	regmap_write(xs->regmap_pextp[id], 0x00F8, 0x00FA32FA);
-+	regmap_write(xs->regmap_pextp[id], 0x00F4, 0x80201F21);
-+	regmap_write(xs->regmap_pextp[id], 0x0030, 0x00050C00);
-+	regmap_write(xs->regmap_pextp[id], 0x0070, 0x02002800);
++	/* Setup operation mode */
++	regmap_write(pextp, 0x9024, 0x00D9071C);
++	regmap_write(pextp, 0x2020, 0xAA8585AA);
++	regmap_write(pextp, 0x2030, 0x0C020207);
++	regmap_write(pextp, 0x2034, 0x0E05050F);
++	regmap_write(pextp, 0x2040, 0x00200032);
++	regmap_write(pextp, 0x50F0, 0x00C014BA);
++	regmap_write(pextp, 0x50E0, 0x3777C12B);
++	regmap_write(pextp, 0x506C, 0x005F9CFF);
++	regmap_write(pextp, 0x5070, 0x9D9DFAFA);
++	regmap_write(pextp, 0x5074, 0x27273F3F);
++	regmap_write(pextp, 0x5078, 0xA7883C68);
++	regmap_write(pextp, 0x507C, 0x11661166);
++	regmap_write(pextp, 0x5080, 0x0E000EAF);
++	regmap_write(pextp, 0x5084, 0x08080E0D);
++	regmap_write(pextp, 0x5088, 0x02030B09);
++	regmap_write(pextp, 0x50E4, 0x0C0C0000);
++	regmap_write(pextp, 0x50E8, 0x04040000);
++	regmap_write(pextp, 0x50EC, 0x0F0F0606);
++	regmap_write(pextp, 0x50A8, 0x506E8C8C);
++	regmap_write(pextp, 0x6004, 0x18190000);
++	regmap_write(pextp, 0x00F8, 0x00FA32FA);
++	/* Force SGDT_OUT off and select PCS */
++	regmap_write(pextp, 0x00F4, 0x80201F21);
++	/* Force GLB_CKDET_OUT */
++	regmap_write(pextp, 0x0030, 0x00050C00);
++	/* Force AEQ on */
++	regmap_write(pextp, 0x0070, 0x02002800);
 +	ndelay(1020);
-+	regmap_write(xs->regmap_pextp[id], 0x30B0, 0x00000020);
-+	regmap_write(xs->regmap_pextp[id], 0x3028, 0x00008A01);
-+	regmap_write(xs->regmap_pextp[id], 0x302C, 0x0000A884);
-+	regmap_write(xs->regmap_pextp[id], 0x3024, 0x00083002);
-+	regmap_write(xs->regmap_pextp[id], 0x3010, 0x00011110);
-+	regmap_write(xs->regmap_pextp[id], 0x3048, 0x40704000);
-+	regmap_write(xs->regmap_pextp[id], 0x3064, 0x0000C000);
-+	regmap_write(xs->regmap_pextp[id], 0x3050, 0xA8000000);
-+	regmap_write(xs->regmap_pextp[id], 0x3054, 0x000000AA);
-+	regmap_write(xs->regmap_pextp[id], 0x306C, 0x20200F00);
-+	regmap_write(xs->regmap_pextp[id], 0xA060, 0x00050000);
-+	regmap_write(xs->regmap_pextp[id], 0x90D0, 0x00000007);
-+	regmap_write(xs->regmap_pextp[id], 0x0070, 0x0200E800);
++	/* Setup DA default value */
++	regmap_write(pextp, 0x30B0, 0x00000020);
++	regmap_write(pextp, 0x3028, 0x00008A01);
++	regmap_write(pextp, 0x302C, 0x0000A884);
++	regmap_write(pextp, 0x3024, 0x00083002);
++	regmap_write(pextp, 0x3010, 0x00011110);
++	regmap_write(pextp, 0x3048, 0x40704000);
++	regmap_write(pextp, 0x3064, 0x0000C000);
++	regmap_write(pextp, 0x3050, 0xA8000000);
++	regmap_write(pextp, 0x3054, 0x000000AA);
++	regmap_write(pextp, 0x306C, 0x20200F00);
++	regmap_write(pextp, 0xA060, 0x00050000);
++	regmap_write(pextp, 0x90D0, 0x00000007);
++	/* Release reset */
++	regmap_write(pextp, 0x0070, 0x0200E800);
 +	udelay(150);
-+	regmap_write(xs->regmap_pextp[id], 0x0070, 0x0200C111);
++	/* Switch to P0 */
++	regmap_write(pextp, 0x0070, 0x0200C111);
 +	ndelay(1020);
-+	regmap_write(xs->regmap_pextp[id], 0x0070, 0x0200C101);
++	regmap_write(pextp, 0x0070, 0x0200C101);
 +	udelay(15);
-+	regmap_write(xs->regmap_pextp[id], 0x0070, 0x0201C111);
++	/* Switch to Gen2 */
++	regmap_write(pextp, 0x0070, 0x0201C111);
 +	ndelay(1020);
-+	regmap_write(xs->regmap_pextp[id], 0x0070, 0x0201C101);
++	regmap_write(pextp, 0x0070, 0x0201C101);
 +	udelay(100);
-+	regmap_write(xs->regmap_pextp[id], 0x30B0, 0x00000030);
-+	regmap_write(xs->regmap_pextp[id], 0x00F4, 0x80201F01);
-+	regmap_write(xs->regmap_pextp[id], 0x3040, 0x30000000);
++	regmap_write(pextp, 0x30B0, 0x00000030);
++	regmap_write(pextp, 0x00F4, 0x80201F01);
++	regmap_write(pextp, 0x3040, 0x30000000);
 +	udelay(400);
 +}
 +
 +void mtk_sgmii_setup_phya_gen2(struct mtk_eth *eth, int mac_id)
 +{
-+	struct mtk_xgmii *xs = eth->xgmii;
 +	u32 id = mtk_mac2xgmii_id(eth, mac_id);
++	struct regmap *pextp;
++
++	if (id >= eth->soc->num_devs)
++		return;
 +
-+	if (id >= eth->soc->num_devs || !xs->regmap_pextp[id])
++	pextp = eth->regmap_pextp[id];
++	if (!pextp)
 +		return;
 +
-+	regmap_write(xs->regmap_pextp[id], 0x9024, 0x00D9071C);
-+	regmap_write(xs->regmap_pextp[id], 0x2020, 0xAA8585AA);
-+	regmap_write(xs->regmap_pextp[id], 0x2030, 0x0C020707);
-+	regmap_write(xs->regmap_pextp[id], 0x2034, 0x0E050F0F);
-+	regmap_write(xs->regmap_pextp[id], 0x2040, 0x00140032);
-+	regmap_write(xs->regmap_pextp[id], 0x50F0, 0x00C014AA);
-+	regmap_write(xs->regmap_pextp[id], 0x50E0, 0x3777C12B);
-+	regmap_write(xs->regmap_pextp[id], 0x506C, 0x005F9CFF);
-+	regmap_write(xs->regmap_pextp[id], 0x5070, 0x9D9DFAFA);
-+	regmap_write(xs->regmap_pextp[id], 0x5074, 0x27273F3F);
-+	regmap_write(xs->regmap_pextp[id], 0x5078, 0xA7883C68);
-+	regmap_write(xs->regmap_pextp[id], 0x507C, 0x11661166);
-+	regmap_write(xs->regmap_pextp[id], 0x5080, 0x0E000AAF);
-+	regmap_write(xs->regmap_pextp[id], 0x5084, 0x08080D0D);
-+	regmap_write(xs->regmap_pextp[id], 0x5088, 0x02030909);
-+	regmap_write(xs->regmap_pextp[id], 0x50E4, 0x0C0C0000);
-+	regmap_write(xs->regmap_pextp[id], 0x50E8, 0x04040000);
-+	regmap_write(xs->regmap_pextp[id], 0x50EC, 0x0F0F0C06);
-+	regmap_write(xs->regmap_pextp[id], 0x50A8, 0x506E8C8C);
-+	regmap_write(xs->regmap_pextp[id], 0x6004, 0x18190000);
-+	regmap_write(xs->regmap_pextp[id], 0x00F8, 0x009C329C);
-+	regmap_write(xs->regmap_pextp[id], 0x00F4, 0x80201F21);
-+	regmap_write(xs->regmap_pextp[id], 0x0030, 0x00050C00);
-+	regmap_write(xs->regmap_pextp[id], 0x0070, 0x02002800);
++	/* Setup operation mode */
++	regmap_write(pextp, 0x9024, 0x00D9071C);
++	regmap_write(pextp, 0x2020, 0xAA8585AA);
++	regmap_write(pextp, 0x2030, 0x0C020707);
++	regmap_write(pextp, 0x2034, 0x0E050F0F);
++	regmap_write(pextp, 0x2040, 0x00140032);
++	regmap_write(pextp, 0x50F0, 0x00C014AA);
++	regmap_write(pextp, 0x50E0, 0x3777C12B);
++	regmap_write(pextp, 0x506C, 0x005F9CFF);
++	regmap_write(pextp, 0x5070, 0x9D9DFAFA);
++	regmap_write(pextp, 0x5074, 0x27273F3F);
++	regmap_write(pextp, 0x5078, 0xA7883C68);
++	regmap_write(pextp, 0x507C, 0x11661166);
++	regmap_write(pextp, 0x5080, 0x0E000AAF);
++	regmap_write(pextp, 0x5084, 0x08080D0D);
++	regmap_write(pextp, 0x5088, 0x02030909);
++	regmap_write(pextp, 0x50E4, 0x0C0C0000);
++	regmap_write(pextp, 0x50E8, 0x04040000);
++	regmap_write(pextp, 0x50EC, 0x0F0F0C06);
++	regmap_write(pextp, 0x50A8, 0x506E8C8C);
++	regmap_write(pextp, 0x6004, 0x18190000);
++	regmap_write(pextp, 0x00F8, 0x009C329C);
++	/* Force SGDT_OUT off and select PCS */
++	regmap_write(pextp, 0x00F4, 0x80201F21);
++	/* Force GLB_CKDET_OUT */
++	regmap_write(pextp, 0x0030, 0x00050C00);
++	/* Force AEQ on */
++	regmap_write(pextp, 0x0070, 0x02002800);
 +	ndelay(1020);
-+	regmap_write(xs->regmap_pextp[id], 0x30B0, 0x00000020);
-+	regmap_write(xs->regmap_pextp[id], 0x3028, 0x00008A01);
-+	regmap_write(xs->regmap_pextp[id], 0x302C, 0x0000A884);
-+	regmap_write(xs->regmap_pextp[id], 0x3024, 0x00083002);
-+	regmap_write(xs->regmap_pextp[id], 0x3010, 0x00011110);
-+	regmap_write(xs->regmap_pextp[id], 0x3048, 0x40704000);
-+	regmap_write(xs->regmap_pextp[id], 0x3050, 0xA8000000);
-+	regmap_write(xs->regmap_pextp[id], 0x3054, 0x000000AA);
-+	regmap_write(xs->regmap_pextp[id], 0x306C, 0x22000F00);
-+	regmap_write(xs->regmap_pextp[id], 0xA060, 0x00050000);
-+	regmap_write(xs->regmap_pextp[id], 0x90D0, 0x00000005);
-+	regmap_write(xs->regmap_pextp[id], 0x0070, 0x0200E800);
++	/* Setup DA default value */
++	regmap_write(pextp, 0x30B0, 0x00000020);
++	regmap_write(pextp, 0x3028, 0x00008A01);
++	regmap_write(pextp, 0x302C, 0x0000A884);
++	regmap_write(pextp, 0x3024, 0x00083002);
++	regmap_write(pextp, 0x3010, 0x00011110);
++	regmap_write(pextp, 0x3048, 0x40704000);
++	regmap_write(pextp, 0x3050, 0xA8000000);
++	regmap_write(pextp, 0x3054, 0x000000AA);
++	regmap_write(pextp, 0x306C, 0x22000F00);
++	regmap_write(pextp, 0xA060, 0x00050000);
++	regmap_write(pextp, 0x90D0, 0x00000005);
++	/* Release reset */
++	regmap_write(pextp, 0x0070, 0x0200E800);
 +	udelay(150);
-+	regmap_write(xs->regmap_pextp[id], 0x0070, 0x0200C111);
++	/* Switch to P0 */
++	regmap_write(pextp, 0x0070, 0x0200C111);
 +	ndelay(1020);
-+	regmap_write(xs->regmap_pextp[id], 0x0070, 0x0200C101);
++	regmap_write(pextp, 0x0070, 0x0200C101);
 +	udelay(15);
-+	regmap_write(xs->regmap_pextp[id], 0x0070, 0x0201C111);
++	/* Switch to Gen2 */
++	regmap_write(pextp, 0x0070, 0x0201C111);
 +	ndelay(1020);
-+	regmap_write(xs->regmap_pextp[id], 0x0070, 0x0201C101);
++	regmap_write(pextp, 0x0070, 0x0201C101);
 +	udelay(100);
-+	regmap_write(xs->regmap_pextp[id], 0x30B0, 0x00000030);
-+	regmap_write(xs->regmap_pextp[id], 0x00F4, 0x80201F01);
-+	regmap_write(xs->regmap_pextp[id], 0x3040, 0x30000000);
++	regmap_write(pextp, 0x30B0, 0x00000030);
++	regmap_write(pextp, 0x00F4, 0x80201F01);
++	regmap_write(pextp, 0x3040, 0x30000000);
 +	udelay(400);
 +}
 +
-+void mtk_sgmii_reset(struct mtk_eth *eth, int mac_id)
++static void mtk_usxgmii_reset(struct mtk_eth *eth, int id)
 +{
-+	u32 id = mtk_mac2xgmii_id(eth, mac_id);
 +	u32 val = 0;
 +
 +	if (id >= eth->soc->num_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_SGMII1_GRST;
++		val |= SWSYSRST_XFI_PEXPT0_GRST |
++		       SWSYSRST_XFI0_GRST;
 +		regmap_write(eth->toprgu, TOPRGU_SWSYSRST_EN, val);
 +
-+		/* Assert SGMII reset */
++		/* Assert USXGMII 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_XFI0_GRST;
 +		regmap_write(eth->toprgu, TOPRGU_SWSYSRST, val);
 +
 +		udelay(100);
 +
-+		/* De-assert SGMII reset */
++		/* De-assert USXGMII 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_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_SGMII1_GRST);
++		val &= ~(SWSYSRST_XFI_PEXPT0_GRST |
++			 SWSYSRST_XFI0_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_XFI1_GRST;
 +		regmap_write(eth->toprgu, TOPRGU_SWSYSRST_EN, val);
 +
-+		/* Assert SGMII reset */
++		/* Assert USXGMII 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_XFI1_GRST;
 +		regmap_write(eth->toprgu, TOPRGU_SWSYSRST, val);
 +
 +		udelay(100);
 +
-+		/* De-assert SGMII reset */
++		/* De-assert USXGMII 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_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_SGMII0_GRST);
++		val &= ~(SWSYSRST_XFI_PEXPT1_GRST |
++			 SWSYSRST_XFI1_GRST);
 +		regmap_write(eth->toprgu, TOPRGU_SWSYSRST_EN, val);
 +		break;
 +	}
 +
-+	mdelay(1);
++	mdelay(10);
++}
++
++void mtk_sgmii_reset(struct mtk_eth *eth, int mac_id)
++{
++	u32 xgmii_id = mtk_mac2xgmii_id(eth, mac_id);
++
++	mtk_usxgmii_reset(eth, xgmii_id);
++}
++
++
++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)
++{
++	struct mtk_usxgmii_pcs *mpcs = pcs_to_mtk_usxgmii_pcs(pcs);
++	struct mtk_eth *eth = mpcs->eth;
++	unsigned int an_ctrl = 0, link_timer = 0, xfi_mode = 0, adapt_mode = 0;
++	bool mode_changed = false;
++
++	if (interface == PHY_INTERFACE_MODE_USXGMII) {
++		an_ctrl = FIELD_PREP(USXGMII_AN_SYNC_CNT, 0x1FF) |
++			  USXGMII_AN_ENABLE;
++		link_timer = FIELD_PREP(USXGMII_LINK_TIMER_IDLE_DETECT, 0x7B) |
++			     FIELD_PREP(USXGMII_LINK_TIMER_COMP_ACK_DETECT, 0x7B) |
++			     FIELD_PREP(USXGMII_LINK_TIMER_AN_RESTART, 0x7B);
++		xfi_mode = FIELD_PREP(USXGMII_XFI_RX_MODE, USXGMII_XFI_RX_MODE_10G) |
++			   FIELD_PREP(USXGMII_XFI_TX_MODE, USXGMII_XFI_TX_MODE_10G);
++	} else if (interface == PHY_INTERFACE_MODE_10GKR) {
++		an_ctrl = FIELD_PREP(USXGMII_AN_SYNC_CNT, 0x1FF);
++		link_timer = FIELD_PREP(USXGMII_LINK_TIMER_IDLE_DETECT, 0x7B) |
++			     FIELD_PREP(USXGMII_LINK_TIMER_COMP_ACK_DETECT, 0x7B) |
++			     FIELD_PREP(USXGMII_LINK_TIMER_AN_RESTART, 0x7B);
++		xfi_mode = FIELD_PREP(USXGMII_XFI_RX_MODE, USXGMII_XFI_RX_MODE_10G) |
++			   FIELD_PREP(USXGMII_XFI_TX_MODE, USXGMII_XFI_TX_MODE_10G);
++		adapt_mode = USXGMII_RATE_UPDATE_MODE;
++	} else if (interface == PHY_INTERFACE_MODE_5GBASER) {
++		an_ctrl = FIELD_PREP(USXGMII_AN_SYNC_CNT, 0xFF);
++		link_timer = FIELD_PREP(USXGMII_LINK_TIMER_IDLE_DETECT, 0x3D) |
++			     FIELD_PREP(USXGMII_LINK_TIMER_COMP_ACK_DETECT, 0x3D) |
++			     FIELD_PREP(USXGMII_LINK_TIMER_AN_RESTART, 0x3D);
++		xfi_mode = FIELD_PREP(USXGMII_XFI_RX_MODE, USXGMII_XFI_RX_MODE_5G) |
++			   FIELD_PREP(USXGMII_XFI_TX_MODE, USXGMII_XFI_TX_MODE_5G);
++		adapt_mode = USXGMII_RATE_UPDATE_MODE;
++	} else
++		return -EINVAL;
++
++	adapt_mode |= FIELD_PREP(USXGMII_RATE_ADAPT_MODE, USXGMII_RATE_ADAPT_MODE_X1);
++
++	if (mpcs->interface != interface) {
++		mpcs->interface = interface;
++		mode_changed = true;
++	}
++
++	mtk_xfi_pll_enable(eth);
++	mtk_usxgmii_reset(eth, mpcs->id);
++
++	/* Setup USXGMII AN ctrl */
++	regmap_update_bits(mpcs->regmap, RG_PCS_AN_CTRL0,
++			   USXGMII_AN_SYNC_CNT | USXGMII_AN_ENABLE,
++			   an_ctrl);
++
++	regmap_update_bits(mpcs->regmap, RG_PCS_AN_CTRL2,
++			   USXGMII_LINK_TIMER_IDLE_DETECT |
++			   USXGMII_LINK_TIMER_COMP_ACK_DETECT |
++			   USXGMII_LINK_TIMER_AN_RESTART,
++			   link_timer);
++
++	/* Gated MAC CK */
++	regmap_update_bits(mpcs->regmap, RG_PHY_TOP_SPEED_CTRL1,
++			   USXGMII_MAC_CK_GATED, USXGMII_MAC_CK_GATED);
++
++	/* Enable interface force mode */
++	regmap_update_bits(mpcs->regmap, RG_PHY_TOP_SPEED_CTRL1,
++			   USXGMII_IF_FORCE_EN, USXGMII_IF_FORCE_EN);
++
++	/* Setup USXGMII adapt mode */
++	regmap_update_bits(mpcs->regmap, RG_PHY_TOP_SPEED_CTRL1,
++			   USXGMII_RATE_UPDATE_MODE | USXGMII_RATE_ADAPT_MODE,
++			   adapt_mode);
++
++	/* Setup USXGMII speed */
++	regmap_update_bits(mpcs->regmap, RG_PHY_TOP_SPEED_CTRL1,
++			   USXGMII_XFI_RX_MODE | USXGMII_XFI_TX_MODE,
++			   xfi_mode);
++
++	udelay(1);
++
++	/* Un-gated MAC CK */
++	regmap_update_bits(mpcs->regmap, RG_PHY_TOP_SPEED_CTRL1,
++			   USXGMII_MAC_CK_GATED, 0);
++
++	udelay(1);
++
++	/* Disable interface force mode for the AN mode */
++	if (an_ctrl & USXGMII_AN_ENABLE)
++		regmap_update_bits(mpcs->regmap, RG_PHY_TOP_SPEED_CTRL1,
++				   USXGMII_IF_FORCE_EN, 0);
++
++	/* Setup USXGMIISYS with the determined property */
++	if (interface == PHY_INTERFACE_MODE_USXGMII)
++		mtk_usxgmii_setup_phya_usxgmii(mpcs);
++	else if (interface == PHY_INTERFACE_MODE_10GKR)
++		mtk_usxgmii_setup_phya_10gbaser(mpcs);
++	else if (interface == PHY_INTERFACE_MODE_5GBASER)
++		mtk_usxgmii_setup_phya_5gbaser(mpcs);
++
++	return mode_changed;
++}
++
++static void mtk_usxgmii_pcs_get_state(struct phylink_pcs *pcs,
++				    struct phylink_link_state *state)
++{
++	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;
++
++	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));
++
++		regmap_read(mpcs->regmap, RG_PCS_AN_STS0, &val);
++
++		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;
++	}
++
++	if (state->link == 0)
++		mtk_usxgmii_pcs_config(pcs, MLO_AN_INBAND,
++				       state->interface, NULL, false);
 +}
---- a/drivers/net/ethernet/mediatek/Kconfig
-+++ b/drivers/net/ethernet/mediatek/Kconfig
-@@ -11,6 +11,14 @@ config NET_MEDIATEK_SOC_WED
- 	depends on ARCH_MEDIATEK || COMPILE_TEST
- 	def_bool NET_MEDIATEK_SOC != n
- 
-+config NET_MEDIATEK_SOC_USXGMII
-+	bool "Support USXGMII SerDes on MT7988"
-+	depends on (ARCH_MEDIATEK && ARM64) || COMPILE_TEST
-+	def_bool NET_MEDIATEK_SOC != n
-+	help
-+	  Include support for 10G USXGMII SerDes unit which can
-+	  be found on MT7988.
 +
- config NET_MEDIATEK_SOC
- 	tristate "MediaTek SoC Gigabit Ethernet support"
- 	depends on NET_DSA || !NET_DSA
++static 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;
++
++	regmap_read(mpcs->regmap, RG_PCS_AN_CTRL0, &val);
++	val |= USXGMII_AN_RESTART;
++	regmap_write(mpcs->regmap, RG_PCS_AN_CTRL0, val);
++}
++
++static void mtk_usxgmii_pcs_link_up(struct phylink_pcs *pcs, unsigned int mode,
++				    phy_interface_t interface,
++				    int speed, int duplex)
++{
++	/* Reconfiguring USXGMII to ensure the quality of the RX signal
++	 * after the line side link up.
++	 */
++	mtk_usxgmii_pcs_config(pcs, mode,
++			       interface, NULL, false);
++}
++
++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,
++	.pcs_link_up = mtk_usxgmii_pcs_link_up,
++};
++
++int mtk_usxgmii_init(struct mtk_eth *eth)
++{
++	struct device_node *r = eth->dev->of_node;
++	struct device *dev = eth->dev;
++	struct device_node *np;
++	int i, ret;
++
++	eth->usxgmii_pcs = devm_kcalloc(dev, eth->soc->num_devs, sizeof(eth->usxgmii_pcs), GFP_KERNEL);
++	if (!eth->usxgmii_pcs)
++		return -ENOMEM;
++
++	for (i = 0; i < eth->soc->num_devs; i++) {
++		np = of_parse_phandle(r, "mediatek,usxgmiisys", i);
++		if (!np)
++			break;
++
++		eth->usxgmii_pcs[i] = devm_kzalloc(dev, sizeof(*eth->usxgmii_pcs), GFP_KERNEL);
++		if (!eth->usxgmii_pcs[i])
++			return -ENOMEM;
++
++		eth->usxgmii_pcs[i]->id = i;
++		eth->usxgmii_pcs[i]->eth = eth;
++		eth->usxgmii_pcs[i]->regmap = syscon_node_to_regmap(np);
++		if (IS_ERR(eth->usxgmii_pcs[i]->regmap))
++			return PTR_ERR(eth->usxgmii_pcs[i]->regmap);
++
++		eth->usxgmii_pcs[i]->pcs.ops = &mtk_usxgmii_pcs_ops;
++		eth->usxgmii_pcs[i]->pcs.poll = true;
++		eth->usxgmii_pcs[i]->interface = PHY_INTERFACE_MODE_NA;
++
++		of_node_put(np);
++	}
++
++	ret = mtk_xfi_pextp_init(eth);
++	if (ret)
++		return ret;
++
++	ret = mtk_xfi_pll_init(eth);
++	if (ret)
++		return ret;
++
++	return mtk_toprgu_init(eth);
++}
++
++struct phylink_pcs *mtk_usxgmii_select_pcs(struct mtk_eth *eth, int mac_id)
++{
++	u32 xgmii_id = mtk_mac2xgmii_id(eth, mac_id);
++
++	if (!eth->usxgmii_pcs[xgmii_id]->regmap)
++		return NULL;
++
++	return &eth->usxgmii_pcs[xgmii_id]->pcs;
++}

target/linux/mediatek/patches-5.15/703-v5.17-net-ethernet-mtk_eth_soc-implement-Clause-45-MDIO-ac.patch

@@ -103,7 +103,7 @@ Signed-off-by: David S. Miller <[email protected]>
  
  	ret = mtk_mdio_busy_wait(eth);
  	if (ret < 0)
-@@ -1018,6 +1061,7 @@ static int mtk_mdio_init(struct mtk_eth
+@@ -1013,6 +1056,7 @@ static int mtk_mdio_init(struct mtk_eth
  	eth->mii_bus->name = "mdio";
  	eth->mii_bus->read = mtk_mdio_read;
  	eth->mii_bus->write = mtk_mdio_write;

target/linux/ramips/patches-5.15/700-net-ethernet-mediatek-support-net-labels.patch

@@ -14,7 +14,7 @@ Signed-off-by: René van Dorst <[email protected]>
 
 --- a/drivers/net/ethernet/mediatek/mtk_eth_soc.c
 +++ b/drivers/net/ethernet/mediatek/mtk_eth_soc.c
-@@ -4638,6 +4638,7 @@ static const struct net_device_ops mtk_n
+@@ -4633,6 +4633,7 @@ static const struct net_device_ops mtk_n
  
  static int mtk_add_mac(struct mtk_eth *eth, struct device_node *np)
  {
@@ -22,7 +22,7 @@ Signed-off-by: René van Dorst <[email protected]>
  	const __be32 *_id = of_get_property(np, "reg", NULL);
  	phy_interface_t phy_mode;
  	struct phylink *phylink;
-@@ -4796,6 +4797,9 @@ static int mtk_add_mac(struct mtk_eth *e
+@@ -4791,6 +4792,9 @@ static int mtk_add_mac(struct mtk_eth *e
  		register_netdevice_notifier(&mac->device_notifier);
  	}