mediatek: sync MT7988 USXGMII with SDK driver
authorDaniel Golle <daniel@makrotopia.org>
Sat, 27 May 2023 20:03:40 +0000 (21:03 +0100)
committerDaniel Golle <daniel@makrotopia.org>
Mon, 29 May 2023 12:04:14 +0000 (13:04 +0100)
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 <daniel@makrotopia.org>
(cherry picked from commit ba58245e83714de5f47b4b0fc0369930c3661cab)

target/linux/generic/pending-5.15/737-07-net-ethernet-mtk_eth_soc-add-paths-and-SerDes-modes-.patch
target/linux/mediatek/patches-5.15/703-v5.17-net-ethernet-mtk_eth_soc-implement-Clause-45-MDIO-ac.patch
target/linux/ramips/patches-5.15/700-net-ethernet-mediatek-support-net-labels.patch

index 68170b6614b8e20be1e0257b87602ac99ca70c15..0185bed089774d9c8a7e1bcc6d2e4287c7dddd29 100644 (file)
@@ -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 <daniel@makrotopia.org>
 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 <daniel@makrotopia.org>
 ---
- 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 <daniel@makrotopia.org>
  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 <daniel@makrotopia.org>
        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 <daniel@makrotopia.org>
                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 <daniel@makrotopia.org>
 +              }
                /* 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 <daniel@makrotopia.org>
        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 <daniel@makrotopia.org>
 +      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 <daniel@makrotopia.org>
 +              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 <daniel@makrotopia.org>
        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 <daniel@makrotopia.org>
  }
  
  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 <daniel@makrotopia.org>
        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 <daniel@makrotopia.org>
  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 <daniel@makrotopia.org>
        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 <daniel@makrotopia.org>
        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 <daniel@makrotopia.org>
        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 <daniel@makrotopia.org>
  
  
  /* 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 <daniel@makrotopia.org>
 +#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 <daniel@makrotopia.org>
  /* 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 <daniel@makrotopia.org>
  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 <daniel@makrotopia.org>
        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 <daniel@makrotopia.org>
        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 <daniel@makrotopia.org>
        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 <daniel@makrotopia.org>
  #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 <daniel@makrotopia.org>
  #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 <daniel@makrotopia.org>
  #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 <daniel@makrotopia.org>
  #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 <daniel@makrotopia.org>
  /* 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 <daniel@makrotopia.org>
  
  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 <daniel@makrotopia.org>
  /* 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 <daniel@makrotopia.org>
  
  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 <henry.yen@mediatek.com>
++ *         Daniel Golle <daniel@makrotopia.org>
 + */
 +
 +#include <linux/mfd/syscon.h>
@@ -1017,43 +1042,20 @@ Signed-off-by: Daniel Golle <daniel@makrotopia.org>
 +#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 <daniel@makrotopia.org>
 +              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 <daniel@makrotopia.org>
 +
 +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 <daniel@makrotopia.org>
 +      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;
++}
index 04ca80c213d767d03ab15925adfd9398badd6302..84718d300b5df7df04e55914a52204ea069b90e6 100644 (file)
@@ -103,7 +103,7 @@ Signed-off-by: David S. Miller <davem@davemloft.net>
  
        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;
index a613803ee6d0c3f49e7f0b2f6f5b9dad9d91ff84..e57e6fa83b25ce826db7818bc1692cc222adb6ff 100644 (file)
@@ -14,7 +14,7 @@ Signed-off-by: RenĂ© van Dorst <opensource@vdorst.com>
 
 --- 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 <opensource@vdorst.com>
        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);
        }