[PATCH net-next v2 0/2] net: ethernet: mtk_eth_soc: add paths and SerDes modes

[PATCH net-next v2 0/2] net: ethernet: mtk_eth_soc: add paths and SerDes modes

[Daniel Golle]

The series brings support for SerDes interface modes up to 10 Gb/s for
the MediaTek MT7988 SoC.

As the Ethernet driver now requires a few more phandles in device tree
referencing various other parts of the SoC also add corresponding
dt-binding changes.

It may be argued that instead of referencing those units as syscon and
access them as regmap one may want to implement propper clk or reset
drivers. However, only a single register write is require to the
otherwise completely undocumented pextp_pll unit which will not make a
nice clk driver...

The toprgu/watchdog reset controller is used to synchronously
assert/deassert multiple resets as the same time which is not
supported by Linux reset controller API which always asserts/deasserts
resets sequentially. It is not known whether asserting/deasserting
pextp, sgmii, xfi and usxgii resets simultanously is stricly required
for the hardware to work reliably, however, it's what the vendor
implementation is doing as well and there are no other drivers needing
to access the toprgu reset controller.

This series has been tested with the MT7988 reference board as well as
pre-mass-production BananaPi BPi-R4 board also featuring MT7988.
Various SFP(+) modules have been successfully tested on both boards and
also the on-board AQR133 10GE PHY found on the MT7988 reference board
works nicely.

To make sure no existing older SoCs have been broken run-time tests
have also been carried out on various MT7981, MT7986, MT7622 and
MT7623 boards. The patch is also present in OpenWrt for some months
now already and hence supposedly hundreds of users also on older
MT7621 systems should have tested it while I didn't hear about any
complaints so far.

For the interfaces modes covered by the existing LynxI PCS driver
(Cisco SGMII, 1000Base-X, 2500Base-X) to work on MT7988 this series
depends on commit 90308679c297f ("net: pcs: lynxi: implement pcs_disable op").

Changes since v1:
 * add missing macro defines (v1 didn't build for that reason)
 * add dt-bindings changes

Daniel Golle (2):
  dt-bindings: net: mediatek,net: add phandles for SerDes on MT7988
  net: ethernet: mtk_eth_soc: add paths and SerDes modes for MT7988

 .../devicetree/bindings/net/mediatek,net.yaml |  28 +
 drivers/net/ethernet/mediatek/Kconfig         |  16 +
 drivers/net/ethernet/mediatek/Makefile        |   1 +
 drivers/net/ethernet/mediatek/mtk_eth_path.c  | 123 ++-
 drivers/net/ethernet/mediatek/mtk_eth_soc.c   | 182 ++++-
 drivers/net/ethernet/mediatek/mtk_eth_soc.h   | 237 +++++-
 drivers/net/ethernet/mediatek/mtk_usxgmii.c   | 698 ++++++++++++++++++
 7 files changed, 1254 insertions(+), 31 deletions(-)
 create mode 100644 drivers/net/ethernet/mediatek/mtk_usxgmii.c

-- 
2.42.0

[PATCH net-next v2 1/2] dt-bindings: net: mediatek,net: add phandles for SerDes on MT7988

[Daniel Golle]

Add several phandles needed for Ethernet SerDes interfaces on the
MediaTek MT7988 SoC.

Signed-off-by: Daniel Golle <daniel@makrotopia.org>
---
 .../devicetree/bindings/net/mediatek,net.yaml | 28 +++++++++++++++++++
 1 file changed, 28 insertions(+)

diff --git a/Documentation/devicetree/bindings/net/mediatek,net.yaml b/Documentation/devicetree/bindings/net/mediatek,net.yaml
index e74502a0afe86..78219158b96af 100644
--- a/Documentation/devicetree/bindings/net/mediatek,net.yaml
+++ b/Documentation/devicetree/bindings/net/mediatek,net.yaml
@@ -385,6 +385,34 @@ allOf:
           minItems: 2
           maxItems: 2
 
+        mediatek,toprgu:
+          $ref: /schemas/types.yaml#/definitions/phandle
+          description:
+            Phandle to the syscon representing the reset controller.
+
+        mediatek,usxgmiisys:
+          $ref: /schemas/types.yaml#/definitions/phandle-array
+          minItems: 2
+          maxItems: 2
+          items:
+            maxItems: 1
+          description:
+            A list of phandle to the syscon node referencing the USXGMII PCS.
+
+        mediatek,xfi-pextp:
+          $ref: /schemas/types.yaml#/definitions/phandle-array
+          minItems: 2
+          maxItems: 2
+          items:
+            maxItems: 1
+          description:
+            A list of phandle to the syscon node that handles the 10GE SerDes PHY.
+
+        mediatek,xfi-pll:
+          $ref: /schemas/types.yaml#/definitions/phandle
+          description:
+            Phandle to the syscon node handling the 10GE SerDes clock setup.
+
 patternProperties:
   "^mac@[0-1]$":
     type: object
-- 
2.42.0

[PATCH net-next v2 2/2] net: ethernet: mtk_eth_soc: add paths and SerDes modes for MT7988

[Daniel Golle]

MT7988 comes with a built-in 2.5G PHY as well as SerDes lanes to
connect external PHYs or transceivers in USXGMII, 10GBase-R, 5GBase-R,
2500Base-X, 1000Base-X and Cisco SGMII interface modes.

Implement support for configuring for the new paths to SerDes interfaces
and the internal 2.5G PHY.

Add USXGMII PCS driver for 10GBase-R, 5GBase-R and USXGMII mode, and
setup the new PHYA on MT7988 to access the also still existing old
LynxI PCS for 1000Base-X, 2500Base-X and Cisco SGMII PCS interface
modes.

Signed-off-by: Daniel Golle <daniel@makrotopia.org>
---
 drivers/net/ethernet/mediatek/Kconfig        |  16 +
 drivers/net/ethernet/mediatek/Makefile       |   1 +
 drivers/net/ethernet/mediatek/mtk_eth_path.c | 123 +++-
 drivers/net/ethernet/mediatek/mtk_eth_soc.c  | 182 ++++-
 drivers/net/ethernet/mediatek/mtk_eth_soc.h  | 237 ++++++-
 drivers/net/ethernet/mediatek/mtk_usxgmii.c  | 698 +++++++++++++++++++
 6 files changed, 1226 insertions(+), 31 deletions(-)
 create mode 100644 drivers/net/ethernet/mediatek/mtk_usxgmii.c

diff --git a/drivers/net/ethernet/mediatek/Kconfig b/drivers/net/ethernet/mediatek/Kconfig
index da0db417ab690..b942b4622d146 100644
--- a/drivers/net/ethernet/mediatek/Kconfig
+++ b/drivers/net/ethernet/mediatek/Kconfig
@@ -25,6 +25,22 @@ 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 10GE SerDes which can be found on MT7988.
+	  If this kernel should run on SoCs with 10 GBit/s Ethernet you
+	  will need to select this option to use GMAC2 and GMAC3 with
+	  external PHYs, SFP(+) cages in 10GBase-R, 5GBase-R or USXGMII
+	  interface modes.
+
+	  Note that as the 2500Base-X/1000Base-X/Cisco SGMII SerDes PCS
+	  unit (MediaTek LynxI) in MT7988 is connected via the new 10GE
+	  SerDes, you will also need to select this option in case you
+	  want to use any of those SerDes modes.
+
 config NET_MEDIATEK_STAR_EMAC
 	tristate "MediaTek STAR Ethernet MAC support"
 	select PHYLIB
diff --git a/drivers/net/ethernet/mediatek/Makefile b/drivers/net/ethernet/mediatek/Makefile
index 03e008fbc859b..115ef0faa0e4b 100644
--- a/drivers/net/ethernet/mediatek/Makefile
+++ b/drivers/net/ethernet/mediatek/Makefile
@@ -5,6 +5,7 @@
 
 obj-$(CONFIG_NET_MEDIATEK_SOC) += mtk_eth.o
 mtk_eth-y := mtk_eth_soc.o mtk_eth_path.o mtk_ppe.o mtk_ppe_debugfs.o mtk_ppe_offload.o
+mtk_eth-$(CONFIG_NET_MEDIATEK_SOC_USXGMII) += mtk_usxgmii.o
 mtk_eth-$(CONFIG_NET_MEDIATEK_SOC_WED) += mtk_wed.o mtk_wed_mcu.o mtk_wed_wo.o
 ifdef CONFIG_DEBUG_FS
 mtk_eth-$(CONFIG_NET_MEDIATEK_SOC_WED) += mtk_wed_debugfs.o
diff --git a/drivers/net/ethernet/mediatek/mtk_eth_path.c b/drivers/net/ethernet/mediatek/mtk_eth_path.c
index 7c27a19c4d8f4..0463b0ef4f334 100644
--- a/drivers/net/ethernet/mediatek/mtk_eth_path.c
+++ b/drivers/net/ethernet/mediatek/mtk_eth_path.c
@@ -31,10 +31,20 @@ static const char *mtk_eth_path_name(u64 path)
 		return "gmac2_rgmii";
 	case MTK_ETH_PATH_GMAC2_SGMII:
 		return "gmac2_sgmii";
+	case MTK_ETH_PATH_GMAC2_2P5GPHY:
+		return "gmac2_2p5gphy";
 	case MTK_ETH_PATH_GMAC2_GEPHY:
 		return "gmac2_gephy";
+	case MTK_ETH_PATH_GMAC3_SGMII:
+		return "gmac3_sgmii";
 	case MTK_ETH_PATH_GDM1_ESW:
 		return "gdm1_esw";
+	case MTK_ETH_PATH_GMAC1_USXGMII:
+		return "gmac1_usxgmii";
+	case MTK_ETH_PATH_GMAC2_USXGMII:
+		return "gmac2_usxgmii";
+	case MTK_ETH_PATH_GMAC3_USXGMII:
+		return "gmac3_usxgmii";
 	default:
 		return "unknown path";
 	}
@@ -127,6 +137,27 @@ static int set_mux_u3_gmac2_to_qphy(struct mtk_eth *eth, u64 path)
 	return 0;
 }
 
+static int set_mux_gmac2_to_2p5gphy(struct mtk_eth *eth, u64 path)
+{
+	int ret;
+
+	if (path == MTK_ETH_PATH_GMAC2_2P5GPHY) {
+		ret = regmap_clear_bits(eth->ethsys, ETHSYS_SYSCFG0, SYSCFG0_SGMII_GMAC2_V2);
+		if (ret)
+			return ret;
+
+		/* Setup mux to 2p5g PHY */
+		ret = regmap_clear_bits(eth->infra, TOP_MISC_NETSYS_PCS_MUX, MUX_G2_USXGMII_SEL);
+		if (ret)
+			return ret;
+
+		dev_dbg(eth->dev, "path %s in %s updated\n",
+			mtk_eth_path_name(path), __func__);
+	}
+
+	return 0;
+}
+
 static int set_mux_gmac1_gmac2_to_sgmii_rgmii(struct mtk_eth *eth, u64 path)
 {
 	unsigned int val = 0;
@@ -165,7 +196,48 @@ static int set_mux_gmac1_gmac2_to_sgmii_rgmii(struct mtk_eth *eth, u64 path)
 	return 0;
 }
 
-static int set_mux_gmac12_to_gephy_sgmii(struct mtk_eth *eth, u64 path)
+static int set_mux_gmac123_to_usxgmii(struct mtk_eth *eth, u64 path)
+{
+	unsigned int val = 0;
+	bool updated = true;
+	int mac_id = 0;
+
+	/* Disable SYSCFG1 SGMII */
+	regmap_read(eth->ethsys, ETHSYS_SYSCFG0, &val);
+
+	switch (path) {
+	case MTK_ETH_PATH_GMAC1_USXGMII:
+		val &= ~(u32)SYSCFG0_SGMII_GMAC1_V2;
+		mac_id = MTK_GMAC1_ID;
+		break;
+	case MTK_ETH_PATH_GMAC2_USXGMII:
+		val &= ~(u32)SYSCFG0_SGMII_GMAC2_V2;
+		mac_id = MTK_GMAC2_ID;
+		break;
+	case MTK_ETH_PATH_GMAC3_USXGMII:
+		val &= ~(u32)SYSCFG0_SGMII_GMAC3_V2;
+		mac_id = MTK_GMAC3_ID;
+		break;
+	default:
+		updated = false;
+	};
+
+	if (updated) {
+		regmap_update_bits(eth->ethsys, ETHSYS_SYSCFG0,
+				   SYSCFG0_SGMII_MASK, val);
+
+		if (mac_id == MTK_GMAC2_ID)
+			regmap_set_bits(eth->infra, TOP_MISC_NETSYS_PCS_MUX,
+					MUX_G2_USXGMII_SEL);
+	}
+
+	dev_dbg(eth->dev, "path %s in %s updated = %d\n",
+		mtk_eth_path_name(path), __func__, updated);
+
+	return 0;
+}
+
+static int set_mux_gmac123_to_gephy_sgmii(struct mtk_eth *eth, u64 path)
 {
 	unsigned int val = 0;
 	bool updated = true;
@@ -182,6 +254,9 @@ static int set_mux_gmac12_to_gephy_sgmii(struct mtk_eth *eth, u64 path)
 	case MTK_ETH_PATH_GMAC2_SGMII:
 		val |= SYSCFG0_SGMII_GMAC2_V2;
 		break;
+	case MTK_ETH_PATH_GMAC3_SGMII:
+		val |= SYSCFG0_SGMII_GMAC3_V2;
+		break;
 	default:
 		updated = false;
 	}
@@ -209,6 +284,10 @@ static const struct mtk_eth_muxc mtk_eth_muxc[] = {
 		.name = "mux_u3_gmac2_to_qphy",
 		.cap_bit = MTK_ETH_MUX_U3_GMAC2_TO_QPHY,
 		.set_path = set_mux_u3_gmac2_to_qphy,
+	}, {
+		.name = "mux_gmac2_to_2p5gphy",
+		.cap_bit = MTK_ETH_MUX_GMAC2_TO_2P5GPHY,
+		.set_path = set_mux_gmac2_to_2p5gphy,
 	}, {
 		.name = "mux_gmac1_gmac2_to_sgmii_rgmii",
 		.cap_bit = MTK_ETH_MUX_GMAC1_GMAC2_TO_SGMII_RGMII,
@@ -216,7 +295,15 @@ static const struct mtk_eth_muxc mtk_eth_muxc[] = {
 	}, {
 		.name = "mux_gmac12_to_gephy_sgmii",
 		.cap_bit = MTK_ETH_MUX_GMAC12_TO_GEPHY_SGMII,
-		.set_path = set_mux_gmac12_to_gephy_sgmii,
+		.set_path = set_mux_gmac123_to_gephy_sgmii,
+	}, {
+		.name = "mux_gmac123_to_gephy_sgmii",
+		.cap_bit = MTK_ETH_MUX_GMAC123_TO_GEPHY_SGMII,
+		.set_path = set_mux_gmac123_to_gephy_sgmii,
+	}, {
+		.name = "mux_gmac123_to_usxgmii",
+		.cap_bit = MTK_ETH_MUX_GMAC123_TO_USXGMII,
+		.set_path = set_mux_gmac123_to_usxgmii,
 	},
 };
 
@@ -249,12 +336,39 @@ static int mtk_eth_mux_setup(struct mtk_eth *eth, u64 path)
 	return err;
 }
 
+int mtk_gmac_usxgmii_path_setup(struct mtk_eth *eth, int mac_id)
+{
+	u64 path;
+
+	path = (mac_id == MTK_GMAC1_ID) ?  MTK_ETH_PATH_GMAC1_USXGMII :
+	       (mac_id == MTK_GMAC2_ID) ?  MTK_ETH_PATH_GMAC2_USXGMII :
+					   MTK_ETH_PATH_GMAC3_USXGMII;
+
+	/* Setup proper MUXes along the path */
+	return mtk_eth_mux_setup(eth, path);
+}
+
 int mtk_gmac_sgmii_path_setup(struct mtk_eth *eth, int mac_id)
 {
 	u64 path;
 
-	path = (mac_id == 0) ?  MTK_ETH_PATH_GMAC1_SGMII :
-				MTK_ETH_PATH_GMAC2_SGMII;
+	path = (mac_id == MTK_GMAC1_ID) ? MTK_ETH_PATH_GMAC1_SGMII :
+	       (mac_id == MTK_GMAC2_ID) ? MTK_ETH_PATH_GMAC2_SGMII :
+					  MTK_ETH_PATH_GMAC3_SGMII;
+
+	/* Setup proper MUXes along the path */
+	return mtk_eth_mux_setup(eth, path);
+}
+
+int mtk_gmac_2p5gphy_path_setup(struct mtk_eth *eth, int mac_id)
+{
+	u64 path = 0;
+
+	if (mac_id == MTK_GMAC2_ID)
+		path = MTK_ETH_PATH_GMAC2_2P5GPHY;
+
+	if (!path)
+		return -EINVAL;
 
 	/* Setup proper MUXes along the path */
 	return mtk_eth_mux_setup(eth, path);
diff --git a/drivers/net/ethernet/mediatek/mtk_eth_soc.c b/drivers/net/ethernet/mediatek/mtk_eth_soc.c
index 3cffd1bd30677..d6010ca1bafe9 100644
--- a/drivers/net/ethernet/mediatek/mtk_eth_soc.c
+++ b/drivers/net/ethernet/mediatek/mtk_eth_soc.c
@@ -507,6 +507,30 @@ static void mtk_setup_bridge_switch(struct mtk_eth *eth)
 		MTK_GSW_CFG);
 }
 
+static bool mtk_check_gmac23_idle(struct mtk_mac *mac)
+{
+	u32 mac_fsm, gdm_fsm;
+
+	mac_fsm = mtk_r32(mac->hw, MTK_MAC_FSM(mac->id));
+
+	switch (mac->id) {
+	case MTK_GMAC2_ID:
+		gdm_fsm = mtk_r32(mac->hw, MTK_FE_GDM2_FSM);
+		break;
+	case MTK_GMAC3_ID:
+		gdm_fsm = mtk_r32(mac->hw, MTK_FE_GDM3_FSM);
+		break;
+	default:
+		return true;
+	};
+
+	if ((mac_fsm & 0xFFFF0000) == 0x01010000 &&
+	    (gdm_fsm & 0xFFFF0000) == 0x00000000)
+		return true;
+
+	return false;
+}
+
 static struct phylink_pcs *mtk_mac_select_pcs(struct phylink_config *config,
 					      phy_interface_t interface)
 {
@@ -515,12 +539,20 @@ static struct phylink_pcs *mtk_mac_select_pcs(struct phylink_config *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 : mac->id;
-
-		return eth->sgmii_pcs[sid];
+	if ((interface == PHY_INTERFACE_MODE_SGMII ||
+	     phy_interface_mode_is_8023z(interface)) &&
+	    MTK_HAS_CAPS(eth->soc->caps, MTK_SGMII)) {
+		sid = mtk_mac2xgmii_id(eth, mac->id);
+		if (MTK_HAS_CAPS(eth->soc->caps, MTK_USXGMII))
+			return mtk_sgmii_wrapper_select_pcs(eth, mac->id);
+		else
+			return eth->sgmii_pcs[sid];
+	} else if ((interface == PHY_INTERFACE_MODE_USXGMII ||
+		    interface == PHY_INTERFACE_MODE_10GBASER ||
+		    interface == PHY_INTERFACE_MODE_5GBASER) &&
+		   MTK_HAS_CAPS(eth->soc->caps, MTK_USXGMII) &&
+		   mac->id != MTK_GMAC1_ID) {
+		return mtk_usxgmii_select_pcs(eth, mac->id);
 	}
 
 	return NULL;
@@ -566,7 +598,22 @@ static void mtk_mac_config(struct phylink_config *config, unsigned int mode,
 					goto init_err;
 			}
 			break;
+		case PHY_INTERFACE_MODE_USXGMII:
+		case PHY_INTERFACE_MODE_10GBASER:
+		case PHY_INTERFACE_MODE_5GBASER:
+			if (MTK_HAS_CAPS(eth->soc->caps, MTK_USXGMII)) {
+				err = mtk_gmac_usxgmii_path_setup(eth, mac->id);
+				if (err)
+					goto init_err;
+			}
+			break;
 		case PHY_INTERFACE_MODE_INTERNAL:
+			if (mac->id == MTK_GMAC2_ID &&
+			    MTK_HAS_CAPS(eth->soc->caps, MTK_2P5GPHY)) {
+				err = mtk_gmac_2p5gphy_path_setup(eth, mac->id);
+				if (err)
+					goto init_err;
+			}
 			break;
 		default:
 			goto err_phy;
@@ -613,8 +660,6 @@ static void mtk_mac_config(struct phylink_config *config, unsigned int mode,
 		val &= ~SYSCFG0_GE_MODE(SYSCFG0_GE_MASK, mac->id);
 		val |= SYSCFG0_GE_MODE(ge_mode, mac->id);
 		regmap_write(eth->ethsys, ETHSYS_SYSCFG0, val);
-
-		mac->interface = state->interface;
 	}
 
 	/* SGMII */
@@ -631,21 +676,40 @@ static void mtk_mac_config(struct phylink_config *config, unsigned int mode,
 
 		/* Save the syscfg0 value for mac_finish */
 		mac->syscfg0 = val;
-	} else if (phylink_autoneg_inband(mode)) {
+	} else if (state->interface != PHY_INTERFACE_MODE_USXGMII &&
+		   state->interface != PHY_INTERFACE_MODE_10GBASER &&
+		   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-SerDes modes!\n");
 		return;
 	}
 
 	/* Setup gmac */
-	if (mtk_is_netsys_v3_or_greater(eth) &&
-	    mac->interface == PHY_INTERFACE_MODE_INTERNAL) {
-		mtk_w32(mac->hw, MTK_GDMA_XGDM_SEL, MTK_GDMA_EG_CTRL(mac->id));
-		mtk_w32(mac->hw, MAC_MCR_FORCE_LINK_DOWN, MTK_MAC_MCR(mac->id));
+	if (mtk_is_netsys_v3_or_greater(eth)) {
+		if (mtk_interface_mode_is_xgmii(state->interface)) {
+			mtk_w32(mac->hw, MTK_GDMA_XGDM_SEL, MTK_GDMA_EG_CTRL(mac->id));
+			mtk_w32(mac->hw, MAC_MCR_FORCE_LINK_DOWN, MTK_MAC_MCR(mac->id));
 
-		mtk_setup_bridge_switch(eth);
+			if (mac->id == MTK_GMAC1_ID)
+				mtk_setup_bridge_switch(eth);
+		} else {
+			mtk_w32(eth, 0, MTK_GDMA_EG_CTRL(mac->id));
+
+			/* 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 ((mtk_interface_mode_is_xgmii(mac->interface) ||
+			     mac->interface == PHY_INTERFACE_MODE_NA) &&
+			    !mtk_check_gmac23_idle(mac) &&
+			    !test_bit(MTK_RESETTING, &eth->state))
+				schedule_work(&eth->pending_work);
+		}
 	}
 
+	mac->interface = state->interface;
+
 	return;
 
 err_phy:
@@ -691,10 +755,13 @@ static void mtk_mac_link_down(struct phylink_config *config, unsigned int mode,
 {
 	struct mtk_mac *mac = container_of(config, struct mtk_mac,
 					   phylink_config);
-	u32 mcr = mtk_r32(mac->hw, MTK_MAC_MCR(mac->id));
 
-	mcr &= ~(MAC_MCR_TX_EN | MAC_MCR_RX_EN);
-	mtk_w32(mac->hw, mcr, MTK_MAC_MCR(mac->id));
+	if (!mtk_interface_mode_is_xgmii(interface)) {
+		mtk_m32(mac->hw, MAC_MCR_TX_EN | MAC_MCR_RX_EN, 0, MTK_MAC_MCR(mac->id));
+		mtk_m32(mac->hw, MTK_XGMAC_FORCE_LINK(mac->id), 0, MTK_XGMAC_STS(mac->id));
+	} else if (mac->id != MTK_GMAC1_ID) {
+		mtk_m32(mac->hw, XMAC_MCR_TRX_DISABLE, XMAC_MCR_TRX_DISABLE, MTK_XMAC_MCR(mac->id));
+	}
 }
 
 static void mtk_set_queue_speed(struct mtk_eth *eth, unsigned int idx,
@@ -766,13 +833,11 @@ static void mtk_set_queue_speed(struct mtk_eth *eth, unsigned int idx,
 	mtk_w32(eth, val, soc->reg_map->qdma.qtx_sch + ofs);
 }
 
-static void mtk_mac_link_up(struct phylink_config *config,
-			    struct phy_device *phy,
-			    unsigned int mode, phy_interface_t interface,
-			    int speed, int duplex, bool tx_pause, bool rx_pause)
+static void mtk_gdm_mac_link_up(struct mtk_mac *mac,
+				struct phy_device *phy,
+				unsigned int mode, phy_interface_t interface,
+				int speed, int duplex, bool tx_pause, bool rx_pause)
 {
-	struct mtk_mac *mac = container_of(config, struct mtk_mac,
-					   phylink_config);
 	u32 mcr;
 
 	mcr = mtk_r32(mac->hw, MTK_MAC_MCR(mac->id));
@@ -806,6 +871,55 @@ static void mtk_mac_link_up(struct phylink_config *config,
 	mtk_w32(mac->hw, mcr, MTK_MAC_MCR(mac->id));
 }
 
+static void mtk_xgdm_mac_link_up(struct mtk_mac *mac,
+				 struct phy_device *phy,
+				 unsigned int mode, phy_interface_t interface,
+				 int speed, int duplex, bool tx_pause, bool rx_pause)
+{
+	u32 mcr, force_link = 0;
+
+	if (mac->id == MTK_GMAC1_ID)
+		return;
+
+	/* Eliminate the interference(before link-up) caused by PHY noise */
+	mtk_m32(mac->hw, XMAC_LOGIC_RST, 0, MTK_XMAC_LOGIC_RST(mac->id));
+	mdelay(20);
+	mtk_m32(mac->hw, XMAC_GLB_CNTCLR, XMAC_GLB_CNTCLR, MTK_XMAC_CNT_CTRL(mac->id));
+
+	if (mac->interface == PHY_INTERFACE_MODE_INTERNAL || mac->id == MTK_GMAC3_ID)
+		force_link = MTK_XGMAC_FORCE_LINK(mac->id);
+
+	mtk_m32(mac->hw, MTK_XGMAC_FORCE_LINK(mac->id), force_link, MTK_XGMAC_STS(mac->id));
+
+	mcr = mtk_r32(mac->hw, MTK_XMAC_MCR(mac->id));
+	mcr &= ~(XMAC_MCR_FORCE_TX_FC | XMAC_MCR_FORCE_RX_FC | XMAC_MCR_TRX_DISABLE);
+	/* Configure pause modes -
+	 * phylink will avoid these for half duplex
+	 */
+	if (tx_pause)
+		mcr |= XMAC_MCR_FORCE_TX_FC;
+	if (rx_pause)
+		mcr |= XMAC_MCR_FORCE_RX_FC;
+
+	mtk_w32(mac->hw, mcr, MTK_XMAC_MCR(mac->id));
+}
+
+static void mtk_mac_link_up(struct phylink_config *config,
+			    struct phy_device *phy,
+			    unsigned int mode, phy_interface_t interface,
+			    int speed, int duplex, bool tx_pause, bool rx_pause)
+{
+	struct mtk_mac *mac = container_of(config, struct mtk_mac,
+					   phylink_config);
+
+	if (mtk_interface_mode_is_xgmii(interface))
+		mtk_xgdm_mac_link_up(mac, phy, mode, interface, speed, duplex,
+				     tx_pause, rx_pause);
+	else
+		mtk_gdm_mac_link_up(mac, phy, mode, interface, speed, duplex,
+				    tx_pause, rx_pause);
+}
+
 static const struct phylink_mac_ops mtk_phylink_ops = {
 	.mac_select_pcs = mtk_mac_select_pcs,
 	.mac_config = mtk_mac_config,
@@ -4609,8 +4723,21 @@ static int mtk_add_mac(struct mtk_eth *eth, struct device_node *np)
 		phy_interface_zero(mac->phylink_config.supported_interfaces);
 		__set_bit(PHY_INTERFACE_MODE_INTERNAL,
 			  mac->phylink_config.supported_interfaces);
+	} else if (MTK_HAS_CAPS(mac->hw->soc->caps, MTK_USXGMII)) {
+		mac->phylink_config.mac_capabilities |= MAC_5000FD | MAC_10000FD;
+		__set_bit(PHY_INTERFACE_MODE_5GBASER,
+			  mac->phylink_config.supported_interfaces);
+		__set_bit(PHY_INTERFACE_MODE_10GBASER,
+			  mac->phylink_config.supported_interfaces);
+		__set_bit(PHY_INTERFACE_MODE_USXGMII,
+			  mac->phylink_config.supported_interfaces);
 	}
 
+	if (MTK_HAS_CAPS(mac->hw->soc->caps, MTK_2P5GPHY) &&
+	    id == MTK_GMAC2_ID)
+		__set_bit(PHY_INTERFACE_MODE_INTERNAL,
+			  mac->phylink_config.supported_interfaces);
+
 	phylink = phylink_create(&mac->phylink_config,
 				 of_fwnode_handle(mac->of_node),
 				 phy_mode, &mtk_phylink_ops);
@@ -4811,6 +4938,13 @@ static int mtk_probe(struct platform_device *pdev)
 			return err;
 	}
 
+	if (MTK_HAS_CAPS(eth->soc->caps, MTK_USXGMII)) {
+		err = mtk_usxgmii_init(eth);
+
+		if (err)
+			return err;
+	}
+
 	if (eth->soc->required_pctl) {
 		eth->pctl = syscon_regmap_lookup_by_phandle(pdev->dev.of_node,
 							    "mediatek,pctl");
diff --git a/drivers/net/ethernet/mediatek/mtk_eth_soc.h b/drivers/net/ethernet/mediatek/mtk_eth_soc.h
index 403219d987eff..4a9f8a413c846 100644
--- a/drivers/net/ethernet/mediatek/mtk_eth_soc.h
+++ b/drivers/net/ethernet/mediatek/mtk_eth_soc.h
@@ -503,6 +503,21 @@
 #define INTF_MODE_RGMII_1000    (TRGMII_MODE | TRGMII_CENTRAL_ALIGNED)
 #define INTF_MODE_RGMII_10_100  0
 
+/* XFI Mac control registers */
+#define MTK_XMAC_BASE(x)	(0x12000 + (((x) - 1) * 0x1000))
+#define MTK_XMAC_MCR(x)		(MTK_XMAC_BASE(x))
+#define XMAC_MCR_TRX_DISABLE	0xf
+#define XMAC_MCR_FORCE_TX_FC	BIT(5)
+#define XMAC_MCR_FORCE_RX_FC	BIT(4)
+
+/* XFI Mac logic reset registers */
+#define MTK_XMAC_LOGIC_RST(x)	(MTK_XMAC_BASE(x) + 0x10)
+#define XMAC_LOGIC_RST		BIT(0)
+
+/* XFI Mac count global control */
+#define MTK_XMAC_CNT_CTRL(x)	(MTK_XMAC_BASE(x) + 0x100)
+#define XMAC_GLB_CNTCLR		BIT(0)
+
 /* GPIO port control registers for GMAC 2*/
 #define GPIO_OD33_CTRL8		0x4c0
 #define GPIO_BIAS_CTRL		0xed0
@@ -528,6 +543,7 @@
 #define SYSCFG0_SGMII_GMAC2    ((3 << 8) & SYSCFG0_SGMII_MASK)
 #define SYSCFG0_SGMII_GMAC1_V2 BIT(9)
 #define SYSCFG0_SGMII_GMAC2_V2 BIT(8)
+#define SYSCFG0_SGMII_GMAC3_V2 BIT(7)
 
 
 /* ethernet subsystem clock register */
@@ -560,12 +576,79 @@
 #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 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 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_PCS_AN_WORD	GENMASK(15, 0)
+#define USXGMII_LPA_LATCH	BIT(31)
+
+/* Register to read PCS link status */
+#define RG_PCS_RX_STATUS0	0x904
+#define RG_PCS_RX_STATUS_UPDATE	BIT(16)
+#define RG_PCS_RX_LINK_STATUS	BIT(2)
+
+/* Register to control USXGMII XFI PLL digital */
+#define XFI_PLL_DIG_GLB8	0x08
+#define RG_XFI_PLL_EN		BIT(31)
+
+/* Register to control USXGMII XFI PLL analog */
+#define XFI_PLL_ANA_GLB8	0x108
+#define RG_XFI_PLL_ANA_SWWA	0x02283248
+
 /* Infrasys subsystem config registers */
 #define INFRA_MISC2            0x70c
 #define CO_QPHY_SEL            BIT(0)
 #define GEPHY_MAC_SEL          BIT(1)
 
+/* Toprgu subsystem config registers */
+#define TOPRGU_SWSYSRST		0x18
+#define SWSYSRST_UNLOCK_KEY	GENMASK(31, 24)
+#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
+
 /* Top misc registers */
+#define TOP_MISC_NETSYS_PCS_MUX	0x84
+#define NETSYS_PCS_MUX_MASK	GENMASK(1, 0)
+#define	MUX_G2_USXGMII_SEL	BIT(1)
+#define MUX_HSGMII1_G1_SEL	BIT(0)
+
 #define USB_PHY_SWITCH_REG	0x218
 #define QPHY_SEL_MASK		GENMASK(1, 0)
 #define SGMII_QPHY_SEL		0x2
@@ -590,6 +673,8 @@
 #define MT7628_SDM_RBCNT	(MT7628_SDM_OFFSET + 0x10c)
 #define MT7628_SDM_CS_ERR	(MT7628_SDM_OFFSET + 0x110)
 
+/* Debug Purpose Register */
+#define MTK_PSE_FQFC_CFG	0x100
 #define MTK_FE_CDM1_FSM		0x220
 #define MTK_FE_CDM2_FSM		0x224
 #define MTK_FE_CDM3_FSM		0x238
@@ -598,6 +683,11 @@
 #define MTK_FE_CDM6_FSM		0x328
 #define MTK_FE_GDM1_FSM		0x228
 #define MTK_FE_GDM2_FSM		0x22C
+#define MTK_FE_GDM3_FSM		0x23C
+#define MTK_FE_PSE_FREE		0x240
+#define MTK_FE_DROP_FQ		0x244
+#define MTK_FE_DROP_FC		0x248
+#define MTK_FE_DROP_PPE		0x24C
 
 #define MTK_MAC_FSM(x)		(0x1010C + ((x) * 0x100))
 
@@ -944,6 +1034,8 @@ enum mkt_eth_capabilities {
 	MTK_RGMII_BIT = 0,
 	MTK_TRGMII_BIT,
 	MTK_SGMII_BIT,
+	MTK_USXGMII_BIT,
+	MTK_2P5GPHY_BIT,
 	MTK_ESW_BIT,
 	MTK_GEPHY_BIT,
 	MTK_MUX_BIT,
@@ -964,8 +1056,11 @@ 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,
+	MTK_ETH_MUX_GMAC2_TO_2P5GPHY_BIT,
 	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,
+	MTK_ETH_MUX_GMAC123_TO_USXGMII_BIT,
 
 	/* PATH BITS */
 	MTK_ETH_PATH_GMAC1_RGMII_BIT,
@@ -973,14 +1068,21 @@ enum mkt_eth_capabilities {
 	MTK_ETH_PATH_GMAC1_SGMII_BIT,
 	MTK_ETH_PATH_GMAC2_RGMII_BIT,
 	MTK_ETH_PATH_GMAC2_SGMII_BIT,
+	MTK_ETH_PATH_GMAC2_2P5GPHY_BIT,
 	MTK_ETH_PATH_GMAC2_GEPHY_BIT,
+	MTK_ETH_PATH_GMAC3_SGMII_BIT,
 	MTK_ETH_PATH_GDM1_ESW_BIT,
+	MTK_ETH_PATH_GMAC1_USXGMII_BIT,
+	MTK_ETH_PATH_GMAC2_USXGMII_BIT,
+	MTK_ETH_PATH_GMAC3_USXGMII_BIT,
 };
 
 /* Supported hardware group on SoCs */
 #define MTK_RGMII		BIT_ULL(MTK_RGMII_BIT)
 #define MTK_TRGMII		BIT_ULL(MTK_TRGMII_BIT)
 #define MTK_SGMII		BIT_ULL(MTK_SGMII_BIT)
+#define MTK_USXGMII		BIT_ULL(MTK_USXGMII_BIT)
+#define MTK_2P5GPHY		BIT_ULL(MTK_2P5GPHY_BIT)
 #define MTK_ESW			BIT_ULL(MTK_ESW_BIT)
 #define MTK_GEPHY		BIT_ULL(MTK_GEPHY_BIT)
 #define MTK_MUX			BIT_ULL(MTK_MUX_BIT)
@@ -1003,10 +1105,16 @@ 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)
+#define MTK_ETH_MUX_GMAC2_TO_2P5GPHY		\
+	BIT_ULL(MTK_ETH_MUX_GMAC2_TO_2P5GPHY_BIT)
 #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	\
 	BIT_ULL(MTK_ETH_MUX_GMAC12_TO_GEPHY_SGMII_BIT)
+#define MTK_ETH_MUX_GMAC123_TO_GEPHY_SGMII	\
+	BIT_ULL(MTK_ETH_MUX_GMAC123_TO_GEPHY_SGMII_BIT)
+#define MTK_ETH_MUX_GMAC123_TO_USXGMII	\
+	BIT_ULL(MTK_ETH_MUX_GMAC123_TO_USXGMII_BIT)
 
 /* Supported path present on SoCs */
 #define MTK_ETH_PATH_GMAC1_RGMII	BIT_ULL(MTK_ETH_PATH_GMAC1_RGMII_BIT)
@@ -1014,8 +1122,13 @@ 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)
+#define MTK_ETH_PATH_GMAC2_2P5GPHY	BIT_ULL(MTK_ETH_PATH_GMAC2_2P5GPHY_BIT)
 #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)
+#define MTK_ETH_PATH_GMAC1_USXGMII	BIT_ULL(MTK_ETH_PATH_GMAC1_USXGMII_BIT)
+#define MTK_ETH_PATH_GMAC2_USXGMII	BIT_ULL(MTK_ETH_PATH_GMAC2_USXGMII_BIT)
+#define MTK_ETH_PATH_GMAC3_USXGMII	BIT_ULL(MTK_ETH_PATH_GMAC3_USXGMII_BIT)
 
 #define MTK_GMAC1_RGMII		(MTK_ETH_PATH_GMAC1_RGMII | MTK_RGMII)
 #define MTK_GMAC1_TRGMII	(MTK_ETH_PATH_GMAC1_TRGMII | MTK_TRGMII)
@@ -1023,7 +1136,12 @@ 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)
+#define MTK_GMAC2_2P5GPHY	(MTK_ETH_PATH_GMAC2_2P5GPHY | MTK_2P5GPHY)
+#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)
+#define MTK_GMAC2_USXGMII	(MTK_ETH_PATH_GMAC2_USXGMII | MTK_USXGMII)
+#define MTK_GMAC3_USXGMII	(MTK_ETH_PATH_GMAC3_USXGMII | MTK_USXGMII)
 
 /* MUXes present on SoCs */
 /* 0: GDM1 -> GMAC1, 1: GDM1 -> ESW */
@@ -1042,10 +1160,20 @@ enum mkt_eth_capabilities {
 	(MTK_ETH_MUX_GMAC1_GMAC2_TO_SGMII_RGMII | MTK_MUX | \
 	MTK_SHARED_SGMII)
 
+/* 2: GMAC2 -> XGMII */
+#define MTK_MUX_GMAC2_TO_2P5GPHY      \
+	(MTK_ETH_MUX_GMAC2_TO_2P5GPHY | MTK_MUX | MTK_INFRA)
+
 /* 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)
 
+#define MTK_MUX_GMAC123_TO_GEPHY_SGMII   \
+	(MTK_ETH_MUX_GMAC123_TO_GEPHY_SGMII | MTK_MUX)
+
+#define MTK_MUX_GMAC123_TO_USXGMII   \
+	(MTK_ETH_MUX_GMAC123_TO_USXGMII | MTK_MUX | MTK_INFRA)
+
 #define MTK_HAS_CAPS(caps, _x)		(((caps) & (_x)) == (_x))
 
 #define MT7621_CAPS  (MTK_GMAC1_RGMII | MTK_GMAC1_TRGMII | \
@@ -1077,8 +1205,12 @@ enum mkt_eth_capabilities {
 		      MTK_MUX_GMAC12_TO_GEPHY_SGMII | MTK_QDMA | \
 		      MTK_RSTCTRL_PPE1 | MTK_SRAM)
 
-#define MT7988_CAPS  (MTK_36BIT_DMA | MTK_GDM1_ESW | MTK_QDMA | \
-		      MTK_RSTCTRL_PPE1 | MTK_RSTCTRL_PPE2 | MTK_SRAM)
+#define MT7988_CAPS  (MTK_36BIT_DMA | MTK_GDM1_ESW | MTK_GMAC1_SGMII | \
+		      MTK_GMAC2_2P5GPHY | MTK_GMAC2_SGMII | MTK_GMAC2_USXGMII | \
+		      MTK_GMAC3_SGMII | MTK_GMAC3_USXGMII | \
+		      MTK_MUX_GMAC123_TO_GEPHY_SGMII | \
+		      MTK_MUX_GMAC123_TO_USXGMII | MTK_MUX_GMAC2_TO_2P5GPHY | \
+		      MTK_QDMA | MTK_RSTCTRL_PPE1 | MTK_RSTCTRL_PPE2 | MTK_SRAM)
 
 struct mtk_tx_dma_desc_info {
 	dma_addr_t	addr;
@@ -1188,6 +1320,24 @@ struct mtk_soc_data {
 /* currently no SoC has more than 3 macs */
 #define MTK_MAX_DEVS	3
 
+/* 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_usxgmii_pcs {
+	struct mtk_eth		*eth;
+	struct regmap		*regmap;
+	struct phylink_pcs	*wrapped_sgmii_pcs;
+	phy_interface_t		interface;
+	u8			id;
+	unsigned int		neg_mode;
+	struct phylink_pcs	pcs;
+};
+
 /* struct mtk_eth -	This is the main datasructure for holding the state
  *			of the driver
  * @dev:		The device pointer
@@ -1208,6 +1358,12 @@ 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
+ * @sgmii_wrapped_pcs:	Pointers to NETSYSv3 wrapper 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
@@ -1251,6 +1407,10 @@ struct mtk_eth {
 	struct regmap			*ethsys;
 	struct regmap			*infra;
 	struct phylink_pcs		*sgmii_pcs[MTK_MAX_DEVS];
+	struct regmap			*toprgu;
+	struct regmap			*usxgmii_pll;
+	struct regmap			*regmap_pextp[MTK_MAX_DEVS];
+	struct mtk_usxgmii_pcs		*usxgmii_pcs[MTK_MAX_DEVS];
 	struct regmap			*pctl;
 	bool				hwlro;
 	refcount_t			dma_refcnt;
@@ -1421,6 +1581,19 @@ static inline u32 mtk_get_ib2_multicast_mask(struct mtk_eth *eth)
 	return MTK_FOE_IB2_MULTICAST;
 }
 
+static inline bool mtk_interface_mode_is_xgmii(phy_interface_t interface)
+{
+	switch (interface) {
+	case PHY_INTERFACE_MODE_INTERNAL:
+	case PHY_INTERFACE_MODE_USXGMII:
+	case PHY_INTERFACE_MODE_10GBASER:
+	case PHY_INTERFACE_MODE_5GBASER:
+		return true;
+	default:
+		return false;
+	}
+}
+
 /* read the hardware status register */
 void mtk_stats_update_mac(struct mtk_mac *mac);
 
@@ -1429,8 +1602,10 @@ u32 mtk_r32(struct mtk_eth *eth, unsigned reg);
 u32 mtk_m32(struct mtk_eth *eth, u32 mask, u32 set, unsigned int reg);
 
 int mtk_gmac_sgmii_path_setup(struct mtk_eth *eth, int mac_id);
+int mtk_gmac_2p5gphy_path_setup(struct mtk_eth *eth, int mac_id);
 int mtk_gmac_gephy_path_setup(struct mtk_eth *eth, int mac_id);
 int mtk_gmac_rgmii_path_setup(struct mtk_eth *eth, int mac_id);
+int mtk_gmac_usxgmii_path_setup(struct mtk_eth *eth, int mac_id);
 
 int mtk_eth_offload_init(struct mtk_eth *eth);
 int mtk_eth_setup_tc(struct net_device *dev, enum tc_setup_type type,
@@ -1440,5 +1615,63 @@ int mtk_flow_offload_cmd(struct mtk_eth *eth, struct flow_cls_offload *cls,
 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);
 
+static inline int mtk_mac2xgmii_id(struct mtk_eth *eth, int mac_id)
+{
+	int xgmii_id = mac_id;
+
+	if (mtk_is_netsys_v3_or_greater(eth)) {
+		switch (mac_id) {
+		case MTK_GMAC1_ID:
+		case MTK_GMAC2_ID:
+			xgmii_id = 1;
+			break;
+		case MTK_GMAC3_ID:
+			xgmii_id = 0;
+			break;
+		default:
+			xgmii_id = -1;
+		}
+	}
+
+	return MTK_HAS_CAPS(eth->soc->caps, MTK_SHARED_SGMII) ? 0 : xgmii_id;
+}
+
+static inline int mtk_xgmii2mac_id(struct mtk_eth *eth, int xgmii_id)
+{
+	int mac_id = xgmii_id;
+
+	if (mtk_is_netsys_v3_or_greater(eth)) {
+		switch (xgmii_id) {
+		case 0:
+			mac_id = 2;
+			break;
+		case 1:
+			mac_id = 1;
+			break;
+		default:
+			mac_id = -1;
+		}
+	}
+
+	return mac_id;
+}
+
+#ifdef CONFIG_NET_MEDIATEK_SOC_USXGMII
+struct phylink_pcs *mtk_sgmii_wrapper_select_pcs(struct mtk_eth *eth, int id);
+struct phylink_pcs *mtk_usxgmii_select_pcs(struct mtk_eth *eth, int id);
+int mtk_usxgmii_init(struct mtk_eth *eth);
+#else
+static inline struct phylink_pcs *mtk_sgmii_wrapper_select_pcs(struct mtk_eth *eth, int id)
+{
+	return NULL;
+}
+
+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; }
+#endif /* NET_MEDIATEK_SOC_USXGMII */
 
 #endif /* MTK_ETH_H */
diff --git a/drivers/net/ethernet/mediatek/mtk_usxgmii.c b/drivers/net/ethernet/mediatek/mtk_usxgmii.c
new file mode 100644
index 0000000000000..523e98aa4df71
--- /dev/null
+++ b/drivers/net/ethernet/mediatek/mtk_usxgmii.c
@@ -0,0 +1,698 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2023 MediaTek Inc.
+ * Author: Henry Yen <henry.yen@mediatek.com>
+ *         Daniel Golle <daniel@makrotopia.org>
+ */
+
+#include <linux/mfd/syscon.h>
+#include <linux/of.h>
+#include <linux/regmap.h>
+#include "mtk_eth_soc.h"
+
+static struct mtk_usxgmii_pcs *pcs_to_mtk_usxgmii_pcs(struct phylink_pcs *pcs)
+{
+	return container_of(pcs, struct mtk_usxgmii_pcs, pcs);
+}
+
+static int mtk_xfi_pextp_init(struct mtk_eth *eth)
+{
+	struct device *dev = eth->dev;
+	struct device_node *r = dev->of_node;
+	struct device_node *np;
+	int i;
+
+	for (i = 0; i < MTK_MAX_DEVS; i++) {
+		np = of_parse_phandle(r, "mediatek,xfi-pextp", i);
+		if (!np)
+			break;
+
+		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;
+}
+
+static int mtk_xfi_pll_init(struct mtk_eth *eth)
+{
+	struct device_node *r = eth->dev->of_node;
+	struct device_node *np;
+
+	np = of_parse_phandle(r, "mediatek,xfi-pll", 0);
+	if (!np)
+		return -1;
+
+	eth->usxgmii_pll = syscon_node_to_regmap(np);
+	if (IS_ERR(eth->usxgmii_pll))
+		return PTR_ERR(eth->usxgmii_pll);
+
+	return 0;
+}
+
+static int mtk_toprgu_init(struct mtk_eth *eth)
+{
+	struct device_node *r = eth->dev->of_node;
+	struct device_node *np;
+
+	np = of_parse_phandle(r, "mediatek,toprgu", 0);
+	if (!np)
+		return -1;
+
+	eth->toprgu = syscon_node_to_regmap(np);
+	if (IS_ERR(eth->toprgu))
+		return PTR_ERR(eth->toprgu);
+
+	return 0;
+}
+
+static int mtk_xfi_pll_enable(struct mtk_eth *eth)
+{
+	u32 val = 0;
+
+	if (!eth->usxgmii_pll)
+		return -EINVAL;
+
+	/* Add software workaround for USXGMII PLL TCL issue */
+	regmap_write(eth->usxgmii_pll, XFI_PLL_ANA_GLB8, RG_XFI_PLL_ANA_SWWA);
+
+	regmap_read(eth->usxgmii_pll, XFI_PLL_DIG_GLB8, &val);
+	val |= RG_XFI_PLL_EN;
+	regmap_write(eth->usxgmii_pll, XFI_PLL_DIG_GLB8, val);
+
+	return 0;
+}
+
+static void mtk_usxgmii_setup_phya(struct regmap *pextp, phy_interface_t interface, int id)
+{
+	bool is_10g = (interface == PHY_INTERFACE_MODE_10GBASER ||
+		       interface == PHY_INTERFACE_MODE_USXGMII);
+	bool is_2p5g = (interface == PHY_INTERFACE_MODE_2500BASEX);
+	bool is_5g = (interface == PHY_INTERFACE_MODE_5GBASER);
+
+	/* Setup operation mode */
+	if (is_10g)
+		regmap_write(pextp, 0x9024, 0x00C9071C);
+	else
+		regmap_write(pextp, 0x9024, 0x00D9071C);
+
+	if (is_5g)
+		regmap_write(pextp, 0x2020, 0xAAA5A5AA);
+	else
+		regmap_write(pextp, 0x2020, 0xAA8585AA);
+
+	if (is_2p5g || is_5g || is_10g) {
+		regmap_write(pextp, 0x2030, 0x0C020707);
+		regmap_write(pextp, 0x2034, 0x0E050F0F);
+		regmap_write(pextp, 0x2040, 0x00140032);
+	} else {
+		regmap_write(pextp, 0x2030, 0x0C020207);
+		regmap_write(pextp, 0x2034, 0x0E05050F);
+		regmap_write(pextp, 0x2040, 0x00200032);
+	}
+
+	if (is_2p5g || is_10g)
+		regmap_write(pextp, 0x50F0, 0x00C014AA);
+	else if (is_5g)
+		regmap_write(pextp, 0x50F0, 0x00C018AA);
+	else
+		regmap_write(pextp, 0x50F0, 0x00C014BA);
+
+	if (is_5g) {
+		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);
+	} else {
+		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);
+	}
+
+	if (is_2p5g || is_10g) {
+		regmap_write(pextp, 0x5080, 0x0E000AAF);
+		regmap_write(pextp, 0x5084, 0x08080D0D);
+		regmap_write(pextp, 0x5088, 0x02030909);
+	} else if (is_5g) {
+		regmap_write(pextp, 0x5080, 0x0E001ABF);
+		regmap_write(pextp, 0x5084, 0x080B0D0D);
+		regmap_write(pextp, 0x5088, 0x02050909);
+	} else {
+		regmap_write(pextp, 0x5080, 0x0E000EAF);
+		regmap_write(pextp, 0x5084, 0x08080E0D);
+		regmap_write(pextp, 0x5088, 0x02030B09);
+	}
+
+	if (is_5g) {
+		regmap_write(pextp, 0x50E4, 0x0C000000);
+		regmap_write(pextp, 0x50E8, 0x04000000);
+	} else {
+		regmap_write(pextp, 0x50E4, 0x0C0C0000);
+		regmap_write(pextp, 0x50E8, 0x04040000);
+	}
+
+	if (is_2p5g || mtk_interface_mode_is_xgmii(interface))
+		regmap_write(pextp, 0x50EC, 0x0F0F0C06);
+	else
+		regmap_write(pextp, 0x50EC, 0x0F0F0606);
+
+	if (is_5g) {
+		regmap_write(pextp, 0x50A8, 0x50808C8C);
+		regmap_write(pextp, 0x6004, 0x18000000);
+	} else {
+		regmap_write(pextp, 0x50A8, 0x506E8C8C);
+		regmap_write(pextp, 0x6004, 0x18190000);
+	}
+
+	if (is_10g)
+		regmap_write(pextp, 0x00F8, 0x01423342);
+	else if (is_5g)
+		regmap_write(pextp, 0x00F8, 0x00A132A1);
+	else if (is_2p5g)
+		regmap_write(pextp, 0x00F8, 0x009C329C);
+	else
+		regmap_write(pextp, 0x00F8, 0x00FA32FA);
+
+	/* Force SGDT_OUT off and select PCS */
+	if (mtk_interface_mode_is_xgmii(interface))
+		regmap_write(pextp, 0x00F4, 0x80201F20);
+	else
+		regmap_write(pextp, 0x00F4, 0x80201F21);
+
+	/* Force GLB_CKDET_OUT */
+	regmap_write(pextp, 0x0030, 0x00050C00);
+
+	/* Force AEQ on */
+	regmap_write(pextp, 0x0070, 0x02002800);
+	ndelay(1020);
+
+	/* Setup DA default value */
+	regmap_write(pextp, 0x30B0, 0x00000020);
+	regmap_write(pextp, 0x3028, 0x00008A01);
+	regmap_write(pextp, 0x302C, 0x0000A884);
+	regmap_write(pextp, 0x3024, 0x00083002);
+	if (mtk_interface_mode_is_xgmii(interface)) {
+		regmap_write(pextp, 0x3010, 0x00022220);
+		regmap_write(pextp, 0x5064, 0x0F020A01);
+		regmap_write(pextp, 0x50B4, 0x06100600);
+		if (interface == PHY_INTERFACE_MODE_USXGMII)
+			regmap_write(pextp, 0x3048, 0x40704000);
+		else
+			regmap_write(pextp, 0x3048, 0x47684100);
+	} else {
+		regmap_write(pextp, 0x3010, 0x00011110);
+		regmap_write(pextp, 0x3048, 0x40704000);
+	}
+
+	if (!mtk_interface_mode_is_xgmii(interface) && !is_2p5g)
+		regmap_write(pextp, 0x3064, 0x0000C000);
+
+	if (interface == PHY_INTERFACE_MODE_USXGMII) {
+		regmap_write(pextp, 0x3050, 0xA8000000);
+		regmap_write(pextp, 0x3054, 0x000000AA);
+	} else if (mtk_interface_mode_is_xgmii(interface)) {
+		regmap_write(pextp, 0x3050, 0x00000000);
+		regmap_write(pextp, 0x3054, 0x00000000);
+	} else {
+		regmap_write(pextp, 0x3050, 0xA8000000);
+		regmap_write(pextp, 0x3054, 0x000000AA);
+	}
+
+	if (mtk_interface_mode_is_xgmii(interface))
+		regmap_write(pextp, 0x306C, 0x00000F00);
+	else if (is_2p5g)
+		regmap_write(pextp, 0x306C, 0x22000F00);
+	else
+		regmap_write(pextp, 0x306C, 0x20200F00);
+
+	if (interface == PHY_INTERFACE_MODE_10GBASER && id == 0)
+		regmap_write(pextp, 0xA008, 0x0007B400);
+
+	if (mtk_interface_mode_is_xgmii(interface))
+		regmap_write(pextp, 0xA060, 0x00040000);
+	else
+		regmap_write(pextp, 0xA060, 0x00050000);
+
+	if (is_10g)
+		regmap_write(pextp, 0x90D0, 0x00000001);
+	else if (is_5g)
+		regmap_write(pextp, 0x90D0, 0x00000003);
+	else if (is_2p5g)
+		regmap_write(pextp, 0x90D0, 0x00000005);
+	else
+		regmap_write(pextp, 0x90D0, 0x00000007);
+
+	/* Release reset */
+	regmap_write(pextp, 0x0070, 0x0200E800);
+	usleep_range(150, 500);
+
+	/* Switch to P0 */
+	regmap_write(pextp, 0x0070, 0x0200C111);
+	ndelay(1020);
+	regmap_write(pextp, 0x0070, 0x0200C101);
+	usleep_range(15, 50);
+
+	if (mtk_interface_mode_is_xgmii(interface)) {
+		/* Switch to Gen3 */
+		regmap_write(pextp, 0x0070, 0x0202C111);
+	} else {
+		/* Switch to Gen2 */
+		regmap_write(pextp, 0x0070, 0x0201C111);
+	}
+	ndelay(1020);
+	if (mtk_interface_mode_is_xgmii(interface))
+		regmap_write(pextp, 0x0070, 0x0202C101);
+	else
+		regmap_write(pextp, 0x0070, 0x0201C101);
+	usleep_range(100, 500);
+	regmap_write(pextp, 0x30B0, 0x00000030);
+	if (mtk_interface_mode_is_xgmii(interface))
+		regmap_write(pextp, 0x00F4, 0x80201F00);
+	else
+		regmap_write(pextp, 0x00F4, 0x80201F01);
+
+	regmap_write(pextp, 0x3040, 0x30000000);
+	usleep_range(400, 1000);
+}
+
+static void mtk_usxgmii_reset(struct mtk_eth *eth, int id)
+{
+	u32 toggle, val;
+
+	if (id >= MTK_MAX_DEVS || !eth->toprgu)
+		return;
+
+	switch (id) {
+	case 0:
+		toggle = SWSYSRST_XFI_PEXPT0_GRST | SWSYSRST_XFI0_GRST |
+			 SWSYSRST_SGMII0_GRST;
+		break;
+	case 1:
+		toggle = SWSYSRST_XFI_PEXPT1_GRST | SWSYSRST_XFI1_GRST |
+			 SWSYSRST_SGMII1_GRST;
+		break;
+	default:
+		return;
+	}
+
+	/* Enable software reset */
+	regmap_set_bits(eth->toprgu, TOPRGU_SWSYSRST_EN, toggle);
+
+	/* Assert USXGMII reset */
+	regmap_set_bits(eth->toprgu, TOPRGU_SWSYSRST,
+			FIELD_PREP(SWSYSRST_UNLOCK_KEY, 0x88) | toggle);
+
+	usleep_range(100, 500);
+
+	/* De-assert USXGMII reset */
+	regmap_read(eth->toprgu, TOPRGU_SWSYSRST, &val);
+	val |= FIELD_PREP(SWSYSRST_UNLOCK_KEY, 0x88);
+	val &= ~toggle;
+	regmap_write(eth->toprgu, TOPRGU_SWSYSRST, val);
+
+	/* Disable software reset */
+	regmap_clear_bits(eth->toprgu, TOPRGU_SWSYSRST_EN, toggle);
+
+	mdelay(10);
+}
+
+/* As the USXGMII PHYA is shared with the 1000Base-X/2500Base-X/Cisco SGMII unit
+ * the psc-mtk-lynxi instance needs to be wrapped, so that calls to .pcs_config
+ * also trigger an initial reset and subsequent configuration of the PHYA.
+ */
+struct mtk_sgmii_wrapper_pcs {
+	struct mtk_eth		*eth;
+	struct phylink_pcs	*wrapped_pcs;
+	u8			id;
+	struct phylink_pcs	pcs;
+};
+
+static int mtk_sgmii_wrapped_pcs_config(struct phylink_pcs *pcs,
+					unsigned int neg_mode,
+					phy_interface_t interface,
+					const unsigned long *advertising,
+					bool permit_pause_to_mac)
+{
+	struct mtk_sgmii_wrapper_pcs *wp = container_of(pcs, struct mtk_sgmii_wrapper_pcs, pcs);
+	bool full_reconf;
+	int ret;
+
+	full_reconf = interface != wp->eth->usxgmii_pcs[wp->id]->interface;
+	if (full_reconf) {
+		mtk_xfi_pll_enable(wp->eth);
+		mtk_usxgmii_reset(wp->eth, wp->id);
+	}
+
+	ret = wp->wrapped_pcs->ops->pcs_config(wp->wrapped_pcs, neg_mode, interface,
+					       advertising, permit_pause_to_mac);
+
+	if (full_reconf)
+		mtk_usxgmii_setup_phya(wp->eth->regmap_pextp[wp->id], interface, wp->id);
+
+	wp->eth->usxgmii_pcs[wp->id]->interface = interface;
+
+	return ret;
+}
+
+static void mtk_sgmii_wrapped_pcs_get_state(struct phylink_pcs *pcs,
+					    struct phylink_link_state *state)
+{
+	struct mtk_sgmii_wrapper_pcs *wp = container_of(pcs, struct mtk_sgmii_wrapper_pcs, pcs);
+
+	return wp->wrapped_pcs->ops->pcs_get_state(wp->wrapped_pcs, state);
+}
+
+static void mtk_sgmii_wrapped_pcs_an_restart(struct phylink_pcs *pcs)
+{
+	struct mtk_sgmii_wrapper_pcs *wp = container_of(pcs, struct mtk_sgmii_wrapper_pcs, pcs);
+
+	wp->wrapped_pcs->ops->pcs_an_restart(wp->wrapped_pcs);
+}
+
+static void mtk_sgmii_wrapped_pcs_link_up(struct phylink_pcs *pcs,
+					  unsigned int neg_mode,
+					  phy_interface_t interface, int speed,
+					  int duplex)
+{
+	struct mtk_sgmii_wrapper_pcs *wp = container_of(pcs, struct mtk_sgmii_wrapper_pcs, pcs);
+
+	wp->wrapped_pcs->ops->pcs_link_up(wp->wrapped_pcs, neg_mode, interface, speed, duplex);
+}
+
+static void mtk_sgmii_wrapped_pcs_disable(struct phylink_pcs *pcs)
+{
+	struct mtk_sgmii_wrapper_pcs *wp = container_of(pcs, struct mtk_sgmii_wrapper_pcs, pcs);
+
+	wp->wrapped_pcs->ops->pcs_disable(wp->wrapped_pcs);
+
+	wp->eth->usxgmii_pcs[wp->id]->interface = PHY_INTERFACE_MODE_NA;
+}
+
+static const struct phylink_pcs_ops mtk_sgmii_wrapped_pcs_ops = {
+	.pcs_get_state = mtk_sgmii_wrapped_pcs_get_state,
+	.pcs_config = mtk_sgmii_wrapped_pcs_config,
+	.pcs_an_restart = mtk_sgmii_wrapped_pcs_an_restart,
+	.pcs_link_up = mtk_sgmii_wrapped_pcs_link_up,
+	.pcs_disable = mtk_sgmii_wrapped_pcs_disable,
+};
+
+static int mtk_sgmii_wrapper_init(struct mtk_eth *eth)
+{
+	struct mtk_sgmii_wrapper_pcs *wp;
+	int i;
+
+	for (i = 0; i < MTK_MAX_DEVS; i++) {
+		if (!eth->sgmii_pcs[i])
+			continue;
+
+		if (!eth->usxgmii_pcs[i])
+			continue;
+
+		/* Make sure all PCS ops are supported by wrapped PCS */
+		if (!eth->sgmii_pcs[i]->ops->pcs_get_state ||
+		    !eth->sgmii_pcs[i]->ops->pcs_config ||
+		    !eth->sgmii_pcs[i]->ops->pcs_an_restart ||
+		    !eth->sgmii_pcs[i]->ops->pcs_link_up ||
+		    !eth->sgmii_pcs[i]->ops->pcs_disable)
+			return -EOPNOTSUPP;
+
+		wp = devm_kzalloc(eth->dev, sizeof(*wp), GFP_KERNEL);
+		if (!wp)
+			return -ENOMEM;
+
+		wp->wrapped_pcs = eth->sgmii_pcs[i];
+		wp->id = i;
+		wp->pcs.neg_mode = true;
+		wp->pcs.poll = true;
+		wp->pcs.ops = &mtk_sgmii_wrapped_pcs_ops;
+		wp->eth = eth;
+
+		eth->usxgmii_pcs[i]->wrapped_sgmii_pcs = &wp->pcs;
+	}
+
+	return 0;
+}
+
+struct phylink_pcs *mtk_sgmii_wrapper_select_pcs(struct mtk_eth *eth, int mac_id)
+{
+	u32 xgmii_id = mtk_mac2xgmii_id(eth, mac_id);
+
+	if (!eth->usxgmii_pcs[xgmii_id])
+		return NULL;
+
+	return eth->usxgmii_pcs[xgmii_id]->wrapped_sgmii_pcs;
+}
+
+static int mtk_usxgmii_pcs_config(struct phylink_pcs *pcs, unsigned int neg_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;
+	struct regmap *pextp = eth->regmap_pextp[mpcs->id];
+	unsigned int an_ctrl = 0, link_timer = 0, xfi_mode = 0, adapt_mode = 0;
+	bool mode_changed = false;
+
+	if (!pextp)
+		return -ENODEV;
+
+	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_10GBASER) {
+		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);
+
+	mpcs->neg_mode = neg_mode;
+
+	/* 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);
+
+	usleep_range(1, 10);
+
+	/* Un-gated MAC CK */
+	regmap_update_bits(mpcs->regmap, RG_PHY_TOP_SPEED_CTRL1,
+			   USXGMII_MAC_CK_GATED, 0);
+
+	usleep_range(1, 10);
+
+	/* 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 */
+	mtk_usxgmii_setup_phya(pextp, interface, mpcs->id);
+
+	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);
+
+		phylink_decode_usxgmii_word(state, FIELD_GET(USXGMII_PCS_AN_WORD,
+							     val));
+	} else {
+		val = mtk_r32(mac->hw, MTK_XGMAC_STS(mac->id));
+
+		if (mac->id == MTK_GMAC2_ID)
+			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->link = FIELD_GET(MTK_USXGMII_PCS_LINK, val);
+		state->duplex = DUPLEX_FULL;
+	}
+
+	/* Refresh USXGMII link status by toggling RG_PCS_AN_STATUS_UPDATE */
+	regmap_set_bits(mpcs->regmap, RG_PCS_RX_STATUS0, RG_PCS_RX_STATUS_UPDATE);
+	ndelay(1020);
+	regmap_clear_bits(mpcs->regmap, RG_PCS_RX_STATUS0, RG_PCS_RX_STATUS_UPDATE);
+	ndelay(1020);
+
+	/* Read USXGMII link status */
+	regmap_read(mpcs->regmap, RG_PCS_RX_STATUS0, &val);
+	state->link = state->link && FIELD_GET(RG_PCS_RX_LINK_STATUS, val);
+
+	/* Continuously repeat re-configuration sequence until link comes up */
+	if (!state->link)
+		mtk_usxgmii_pcs_config(pcs, mpcs->neg_mode,
+				       state->interface, NULL, false);
+}
+
+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 neg_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, neg_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;
+
+	for (i = 0; i < MTK_MAX_DEVS; i++) {
+		np = of_parse_phandle(r, "mediatek,usxgmiisys", i);
+		if (!np)
+			break;
+
+		eth->usxgmii_pcs[i] = devm_kzalloc(dev, sizeof(*eth->usxgmii_pcs[i]), 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]->pcs.neg_mode = true;
+		eth->usxgmii_pcs[i]->interface = PHY_INTERFACE_MODE_NA;
+		eth->usxgmii_pcs[i]->neg_mode = -1;
+
+		of_node_put(np);
+	}
+
+	ret = mtk_xfi_pextp_init(eth);
+	if (ret)
+		return ret;
+
+	ret = mtk_xfi_pll_init(eth);
+	if (ret)
+		return ret;
+
+	ret = mtk_toprgu_init(eth);
+	if (ret)
+		return ret;
+
+	return mtk_sgmii_wrapper_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;
+}
-- 
2.42.0

Re: [PATCH net-next v2 1/2] dt-bindings: net: mediatek,net: add phandles for SerDes on MT7988

[Rob Herring]

On Mon, Sep 18, 2023 at 11:26:34PM +0100, Daniel Golle wrote:
> Add several phandles needed for Ethernet SerDes interfaces on the
> MediaTek MT7988 SoC.
> 
> Signed-off-by: Daniel Golle <daniel@makrotopia.org>
> ---
>  .../devicetree/bindings/net/mediatek,net.yaml | 28 +++++++++++++++++++
>  1 file changed, 28 insertions(+)
> 
> diff --git a/Documentation/devicetree/bindings/net/mediatek,net.yaml b/Documentation/devicetree/bindings/net/mediatek,net.yaml
> index e74502a0afe86..78219158b96af 100644
> --- a/Documentation/devicetree/bindings/net/mediatek,net.yaml
> +++ b/Documentation/devicetree/bindings/net/mediatek,net.yaml
> @@ -385,6 +385,34 @@ allOf:
>            minItems: 2
>            maxItems: 2
>  
> +        mediatek,toprgu:
> +          $ref: /schemas/types.yaml#/definitions/phandle
> +          description:
> +            Phandle to the syscon representing the reset controller.

Use the reset binding

> +
> +        mediatek,usxgmiisys:
> +          $ref: /schemas/types.yaml#/definitions/phandle-array
> +          minItems: 2
> +          maxItems: 2
> +          items:
> +            maxItems: 1
> +          description:
> +            A list of phandle to the syscon node referencing the USXGMII PCS.

Use the PCS binding

> +
> +        mediatek,xfi-pextp:
> +          $ref: /schemas/types.yaml#/definitions/phandle-array
> +          minItems: 2
> +          maxItems: 2
> +          items:
> +            maxItems: 1
> +          description:
> +            A list of phandle to the syscon node that handles the 10GE SerDes PHY.

Use the phy binding (phys, not phy-handle for ethernet PHY).

> +
> +        mediatek,xfi-pll:
> +          $ref: /schemas/types.yaml#/definitions/phandle
> +          description:
> +            Phandle to the syscon node handling the 10GE SerDes clock setup.

Use the clock binding


> +
>  patternProperties:
>    "^mac@[0-1]$":
>      type: object
> -- 
> 2.42.0

Re: [PATCH net-next v2 1/2] dt-bindings: net: mediatek,net: add phandles for SerDes on MT7988

[Daniel Golle]

Hi Rob,

thank you for the review!

On Tue, Sep 19, 2023 at 01:09:09PM -0500, Rob Herring wrote:
> On Mon, Sep 18, 2023 at 11:26:34PM +0100, Daniel Golle wrote:
> > Add several phandles needed for Ethernet SerDes interfaces on the
> > MediaTek MT7988 SoC.
> > 
> > Signed-off-by: Daniel Golle <daniel@makrotopia.org>
> > ---
> >  .../devicetree/bindings/net/mediatek,net.yaml | 28 +++++++++++++++++++
> >  1 file changed, 28 insertions(+)
> > 
> > diff --git a/Documentation/devicetree/bindings/net/mediatek,net.yaml b/Documentation/devicetree/bindings/net/mediatek,net.yaml
> > index e74502a0afe86..78219158b96af 100644
> > --- a/Documentation/devicetree/bindings/net/mediatek,net.yaml
> > +++ b/Documentation/devicetree/bindings/net/mediatek,net.yaml
> > @@ -385,6 +385,34 @@ allOf:
> >            minItems: 2
> >            maxItems: 2
> >  
> > +        mediatek,toprgu:
> > +          $ref: /schemas/types.yaml#/definitions/phandle
> > +          description:
> > +            Phandle to the syscon representing the reset controller.
> 
> Use the reset binding

I got an alternative implementation ready which implements an actual
reset controller (by extending drivers/watchdog/mtk_wdt.c to cover
also MT7988 and its addition sw-reset-enable bits) and uses single
phandles for each reset bit assigned to the corresponding units
instead of listing them all for the ethernet controller (maybe that's
one step too far though...)

However, as mentioned in the cover letter, using the Linux reset
controller API (which having to use is a consequence of having to use
the reset bindings) doesn't allow to simultanously deassert the
resets of pextp, usxgmii pcs and/or sgmii pcs which is how the vendor
implementation is doing it as all reset bits are on the same 32-bit
register and the Ethernet driver is the only driver needing to access
that register.

Asserting the resets in sequence and subsequently deasserting in
sequence works for me, but it will have to be confirmed to not create
any problems because it's clearly a deviation from the behavior of the
reference implementation.


> 
> > +
> > +        mediatek,usxgmiisys:
> > +          $ref: /schemas/types.yaml#/definitions/phandle-array
> > +          minItems: 2
> > +          maxItems: 2
> > +          items:
> > +            maxItems: 1
> > +          description:
> > +            A list of phandle to the syscon node referencing the USXGMII PCS.
> 
> Use the PCS binding

Ack, I will ie. implement standalone PCS driver similar to eg.
pcs-rzn1-miic.c.

> 
> > +
> > +        mediatek,xfi-pextp:
> > +          $ref: /schemas/types.yaml#/definitions/phandle-array
> > +          minItems: 2
> > +          maxItems: 2
> > +          items:
> > +            maxItems: 1
> > +          description:
> > +            A list of phandle to the syscon node that handles the 10GE SerDes PHY.
> 
> Use the phy binding (phys, not phy-handle for ethernet PHY).

Ack, this can be implemented as a standalone PHY driver using PHY
bindings. I will do that instead.

> 
> > +
> > +        mediatek,xfi-pll:
> > +          $ref: /schemas/types.yaml#/definitions/phandle
> > +          description:
> > +            Phandle to the syscon node handling the 10GE SerDes clock setup.
> 
> Use the clock binding

Does that imply that I should implement a clock driver whith only a
single clock offering only a single operation ('enable') which would
then do the magic register writes?

While one part is actually identifyable as taking care of enabling a
clock, I would not know how to meaningfully abstract the other (first)
part, see vendor driver:

/* Register to control USXGMII XFI PLL digital */
#define XFI_PLL_DIG_GLB8        0x08
#define RG_XFI_PLL_EN           BIT(31)

/* Register to control USXGMII XFI PLL analog */
#define XFI_PLL_ANA_GLB8        0x108
#define RG_XFI_PLL_ANA_SWWA     0x02283248

[...]

/* Add software workaround for USXGMII PLL TCL issue */
regmap_write(ss->pll, XFI_PLL_ANA_GLB8, RG_XFI_PLL_ANA_SWWA);
// How would you represent the line above using the abstractions of the
// common clk framework?

regmap_read(ss->pll, XFI_PLL_DIG_GLB8, &val); //    that looks like it
val |= RG_XFI_PLL_EN;                         // <- could be a abstracted
regmap_write(ss->pll, XFI_PLL_DIG_GLB8, val); //    in a meaningful way in
                                                    clock driver.

... which is all we ever do on that regmap. Ever.


Thanks in advance to everybody sharing their ideas and advises.

Re: [PATCH net-next v2 1/2] dt-bindings: net: mediatek,net: add phandles for SerDes on MT7988

[Krzysztof Kozlowski]

On 19/09/2023 22:50, Daniel Golle wrote:
> Hi Rob,
> 
> thank you for the review!
> 
> On Tue, Sep 19, 2023 at 01:09:09PM -0500, Rob Herring wrote:
>> On Mon, Sep 18, 2023 at 11:26:34PM +0100, Daniel Golle wrote:
>>> Add several phandles needed for Ethernet SerDes interfaces on the
>>> MediaTek MT7988 SoC.
>>>
>>> Signed-off-by: Daniel Golle <daniel@makrotopia.org>
>>> ---
>>>  .../devicetree/bindings/net/mediatek,net.yaml | 28 +++++++++++++++++++
>>>  1 file changed, 28 insertions(+)
>>>
>>> diff --git a/Documentation/devicetree/bindings/net/mediatek,net.yaml b/Documentation/devicetree/bindings/net/mediatek,net.yaml
>>> index e74502a0afe86..78219158b96af 100644
>>> --- a/Documentation/devicetree/bindings/net/mediatek,net.yaml
>>> +++ b/Documentation/devicetree/bindings/net/mediatek,net.yaml
>>> @@ -385,6 +385,34 @@ allOf:
>>>            minItems: 2
>>>            maxItems: 2
>>>  
>>> +        mediatek,toprgu:
>>> +          $ref: /schemas/types.yaml#/definitions/phandle
>>> +          description:
>>> +            Phandle to the syscon representing the reset controller.
>>
>> Use the reset binding
> 
> I got an alternative implementation ready which implements an actual
> reset controller (by extending drivers/watchdog/mtk_wdt.c to cover
> also MT7988 and its addition sw-reset-enable bits) and uses single
> phandles for each reset bit assigned to the corresponding units
> instead of listing them all for the ethernet controller (maybe that's
> one step too far though...)
> 
> However, as mentioned in the cover letter, using the Linux reset
> controller API (which having to use is a consequence of having to use
> the reset bindings) doesn't allow to simultanously deassert the
> resets of pextp, usxgmii pcs and/or sgmii pcs which is how the vendor
> implementation is doing it as all reset bits are on the same 32-bit
> register and the Ethernet driver is the only driver needing to access
> that register.

You can have reset for entire register, why not? And even if current
Linux implementation had some troubles with this, you could fix it.


> 
>>
>>> +
>>> +        mediatek,xfi-pll:
>>> +          $ref: /schemas/types.yaml#/definitions/phandle
>>> +          description:
>>> +            Phandle to the syscon node handling the 10GE SerDes clock setup.
>>
>> Use the clock binding
> 
> Does that imply that I should implement a clock driver whith only a
> single clock offering only a single operation ('enable') which would
> then do the magic register writes?

Yes

> 
> While one part is actually identifyable as taking care of enabling a
> clock, I would not know how to meaningfully abstract the other (first)
> part, see vendor driver:
> 
> /* Register to control USXGMII XFI PLL digital */
> #define XFI_PLL_DIG_GLB8        0x08
> #define RG_XFI_PLL_EN           BIT(31)
> 
> /* Register to control USXGMII XFI PLL analog */
> #define XFI_PLL_ANA_GLB8        0x108
> #define RG_XFI_PLL_ANA_SWWA     0x02283248
> 
> [...]
> 
> /* Add software workaround for USXGMII PLL TCL issue */
> regmap_write(ss->pll, XFI_PLL_ANA_GLB8, RG_XFI_PLL_ANA_SWWA);
> // How would you represent the line above using the abstractions of the
> // common clk framework?

What is above line? Please do not ask us to decode your vendor code. You
know, we also have nothing to do with it.

And anyway, why do you need to abstract it? Why not writing unconditionally?


> 
> regmap_read(ss->pll, XFI_PLL_DIG_GLB8, &val); //    that looks like it
> val |= RG_XFI_PLL_EN;                         // <- could be a abstracted
> regmap_write(ss->pll, XFI_PLL_DIG_GLB8, val); //    in a meaningful way in
>                                                     clock driver.
> 
> ... which is all we ever do on that regmap. Ever.

Not only. You will also get all Linux infrastructure associated with
this clock, so proper devlinks, sysfs/debug entries, automatic gating of
unused clocks etc.

Best regards,
Krzysztof