-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 @@
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;
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;
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);
+ }
/* 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;
}
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;
}
+ 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:
+ 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,
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;
}
}
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);
}
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));
}
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);
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;
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);
}
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 @@
/* 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
+#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
/* 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
};
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,
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,
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,
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)
#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)
#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)
#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)
#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)
/* 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 | \
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;
}
/* 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);
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>
+#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++) {
+ 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;
+
+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;
+}
+ 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 ð->usxgmii_pcs[xgmii_id]->pcs;
++}
projects / openwrt / staging / wigyori.git / commitdiff