[][kernel][common][eth][Add 1000basex support and refactor to upstream style for the SGMII]
[Description]
Add 1000basex support and refactor to upstream style for the SGMII.
Current support types:
- 1000BASEX w/wo autoneg
- SGMII w autoneg
- HSGMII wo autoneg
If without this patch, the SGMII might unable to link with the 1000basex
link partner.
[Release-log]
N/A
Change-Id: I457b84ee944c259b7fe1c61936bb7d2b431cfee2
Reviewed-on: https://gerrit.mediatek.inc/c/openwrt/feeds/mtk_openwrt_feeds/+/7378663
diff --git a/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_eth_soc.c b/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_eth_soc.c
index 1368a7b..ad73fdb 100755
--- a/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_eth_soc.c
+++ b/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_eth_soc.c
@@ -811,52 +811,44 @@
struct mtk_sgmii *ss = eth->sgmii;
u32 id = mtk_mac2xgmii_id(eth, mac->id);
u32 pmsr = mtk_r32(mac->hw, MTK_MAC_MSR(mac->id));
- u32 rgc3, val = 0;
-
- regmap_read(ss->pcs[id].regmap, SGMSYS_PCS_CONTROL_1, &val);
+ u32 bm, adv, rgc3, sgm_mode;
state->interface = mac->interface;
- state->link = FIELD_GET(SGMII_LINK_STATYS, val);
- if (FIELD_GET(SGMII_AN_ENABLE, val)) {
+ regmap_read(ss->pcs[id].regmap, SGMSYS_PCS_CONTROL_1, &bm);
+ if (bm & SGMII_AN_ENABLE) {
regmap_read(ss->pcs[id].regmap,
- SGMII_PCS_SPEED_ABILITY, &val);
-
- val = val >> 16;
-
- state->duplex = FIELD_GET(SGMII_PCS_SPEED_DUPLEX, val);
+ SGMSYS_PCS_ADVERTISE, &adv);
- switch (FIELD_GET(SGMII_PCS_SPEED_MASK, val)) {
- case 0:
- state->speed = SPEED_10;
- break;
- case 1:
- state->speed = SPEED_100;
- break;
- case 2:
- state->speed = SPEED_1000;
- break;
- }
+ phylink_mii_c22_pcs_decode_state(
+ state,
+ FIELD_GET(SGMII_BMSR, bm),
+ FIELD_GET(SGMII_LPA, adv));
} else {
- regmap_read(ss->pcs[id].regmap,
- SGMSYS_SGMII_MODE, &val);
+ state->link = !!(bm & SGMII_LINK_STATYS);
- state->duplex = !FIELD_GET(SGMII_DUPLEX_HALF, val);
+ regmap_read(ss->pcs[id].regmap,
+ SGMSYS_SGMII_MODE, &sgm_mode);
- switch (FIELD_GET(SGMII_SPEED_MASK, val)) {
- case 0:
+ switch (sgm_mode & SGMII_SPEED_MASK) {
+ case SGMII_SPEED_10:
state->speed = SPEED_10;
break;
- case 1:
+ case SGMII_SPEED_100:
state->speed = SPEED_100;
break;
- case 2:
+ case SGMII_SPEED_1000:
regmap_read(ss->pcs[id].regmap,
- ss->pcs[id].ana_rgc3, &val);
- rgc3 = FIELD_GET(RG_PHY_SPEED_3_125G, val);
+ ss->pcs[id].ana_rgc3, &rgc3);
+ rgc3 = FIELD_GET(RG_PHY_SPEED_3_125G, rgc3);
state->speed = rgc3 ? SPEED_2500 : SPEED_1000;
break;
}
+
+ if (sgm_mode & SGMII_DUPLEX_HALF)
+ state->duplex = DUPLEX_HALF;
+ else
+ state->duplex = DUPLEX_FULL;
}
state->pause &= (MLO_PAUSE_RX | MLO_PAUSE_TX);
diff --git a/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_eth_soc.h b/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_eth_soc.h
index 646e557..f279ad5 100755
--- a/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_eth_soc.h
+++ b/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_eth_soc.h
@@ -820,8 +820,8 @@
#define ETHSYS_DMA_AG_MAP_PPE BIT(2)
/* SGMII subsystem config registers */
-/* Register to auto-negotiation restart */
#define SGMSYS_PCS_CONTROL_1 0x0
+#define SGMII_BMSR GENMASK(31, 16)
#define SGMII_AN_RESTART BIT(9)
#define SGMII_ISOLATE BIT(10)
#define SGMII_AN_ENABLE BIT(12)
@@ -832,13 +832,15 @@
#define SGMII_AN_EXPANSION_CLR BIT(30)
/* Register to set SGMII speed */
-#define SGMII_PCS_SPEED_ABILITY 0x08
-#define SGMII_PCS_SPEED_MASK GENMASK(11, 10)
-#define SGMII_PCS_SPEED_10 0
-#define SGMII_PCS_SPEED_100 1
-#define SGMII_PCS_SPEED_1000 2
-#define SGMII_PCS_SPEED_DUPLEX BIT(12)
-#define SGMII_PCS_SPEED_LINK BIT(15)
+#define SGMSYS_PCS_ADVERTISE 0x08
+#define SGMII_ADVERTISE GENMASK(15, 0)
+#define SGMII_LPA GENMASK(31, 16)
+#define SGMII_LPA_SPEED_MASK GENMASK(11, 10)
+#define SGMII_LPA_SPEED_10 0
+#define SGMII_LPA_SPEED_100 1
+#define SGMII_LPA_SPEED_1000 2
+#define SGMII_LPA_DUPLEX BIT(12)
+#define SGMII_LPA_LINK BIT(15)
/* Register to programmable link timer, the unit in 2 * 8ns */
#define SGMSYS_PCS_LINK_TIMER 0x18
@@ -846,7 +848,7 @@
/* Register to control remote fault */
#define SGMSYS_SGMII_MODE 0x20
-#define SGMII_IF_MODE_BIT0 BIT(0)
+#define SGMII_IF_MODE_SGMII BIT(0)
#define SGMII_SPEED_DUPLEX_AN BIT(1)
#define SGMII_SPEED_MASK GENMASK(3, 2)
#define SGMII_SPEED_10 0x0
diff --git a/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_sgmii.c b/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_sgmii.c
index 3c37656..282fd30 100755
--- a/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_sgmii.c
+++ b/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_sgmii.c
@@ -9,6 +9,7 @@
#include <linux/mfd/syscon.h>
#include <linux/of.h>
+#include <linux/phylink.h>
#include <linux/regmap.h>
#include "mtk_eth_soc.h"
@@ -143,29 +144,6 @@
mdelay(1);
}
-int mtk_sgmii_need_powerdown(struct mtk_sgmii_pcs *mpcs,
- phy_interface_t interface)
-{
- u32 val;
-
- /* need to power down sgmii if link down */
- regmap_read(mpcs->regmap, SGMSYS_PCS_CONTROL_1, &val);
- if (!(val & SGMII_LINK_STATYS))
- return true;
-
- /* need to power down sgmii if interface changed */
- regmap_read(mpcs->regmap, mpcs->ana_rgc3, &val);
- if (interface == PHY_INTERFACE_MODE_2500BASEX) {
- if (!(val & RG_PHY_SPEED_3_125G))
- return true;
- } else {
- if (val & RG_PHY_SPEED_3_125G)
- return true;
- }
-
- return false;
-}
-
void mtk_sgmii_setup_phya_gen1(struct mtk_sgmii_pcs *mpcs)
{
if (!mpcs->regmap_pextp)
@@ -368,179 +346,140 @@
udelay(400);
}
-int mtk_sgmii_setup_mode_an(struct mtk_sgmii_pcs *mpcs)
+static void mtk_sgmii_pcs_get_state(struct phylink_pcs *pcs,
+ struct phylink_link_state *state)
{
- struct mtk_eth *eth = mpcs->eth;
- unsigned int val = 0;
-
- if (!mpcs->regmap)
- return -EINVAL;
-
- if (MTK_HAS_CAPS(eth->soc->caps, MTK_NETSYS_V3)) {
- mtk_sgmii_xfi_pll_enable(eth->sgmii);
- mtk_sgmii_reset(eth, mpcs->id);
- }
-
- /* Assert PHYA power down state when needed */
- if (mtk_sgmii_need_powerdown(mpcs, PHY_INTERFACE_MODE_SGMII))
- regmap_write(mpcs->regmap, SGMSYS_QPHY_PWR_STATE_CTRL,
- SGMII_PHYA_PWD);
-
- /* Reset SGMII PCS state */
- regmap_write(mpcs->regmap, SGMII_RESERVED_0, SGMII_SW_RESET);
-
- regmap_read(mpcs->regmap, mpcs->ana_rgc3, &val);
- val &= ~RG_PHY_SPEED_3_125G;
- regmap_write(mpcs->regmap, mpcs->ana_rgc3, val);
-
- /* Setup the link timer and QPHY power up inside SGMIISYS */
- regmap_write(mpcs->regmap, SGMSYS_PCS_LINK_TIMER,
- SGMII_LINK_TIMER_DEFAULT);
-
- regmap_read(mpcs->regmap, SGMSYS_SGMII_MODE, &val);
- val |= SGMII_REMOTE_FAULT_DIS;
- regmap_write(mpcs->regmap, SGMSYS_SGMII_MODE, val);
+ struct mtk_sgmii_pcs *mpcs = pcs_to_mtk_sgmii_pcs(pcs);
+ unsigned int bm, adv, rgc3, sgm_mode;
- /* SGMII AN mode setting */
- regmap_read(mpcs->regmap, SGMSYS_SGMII_MODE, &val);
- val &= ~SGMII_IF_MODE_MASK;
- val |= SGMII_SPEED_DUPLEX_AN;
- regmap_write(mpcs->regmap, SGMSYS_SGMII_MODE, val);
+ state->interface = mpcs->interface;
- /* Enable SGMII AN */
- regmap_read(mpcs->regmap, SGMSYS_PCS_CONTROL_1, &val);
- val |= SGMII_AN_ENABLE;
- regmap_write(mpcs->regmap, SGMSYS_PCS_CONTROL_1, val);
+ regmap_read(mpcs->regmap, SGMSYS_PCS_CONTROL_1, &bm);
+ if (bm & SGMII_AN_ENABLE) {
+ regmap_read(mpcs->regmap, SGMSYS_PCS_ADVERTISE, &adv);
- if (MTK_HAS_FLAGS(mpcs->flags, MTK_SGMII_PN_SWAP))
- regmap_update_bits(mpcs->regmap, SGMSYS_QPHY_WRAP_CTRL,
- SGMII_PN_SWAP_MASK, SGMII_PN_SWAP_TX_RX);
+ phylink_mii_c22_pcs_decode_state(state,
+ FIELD_GET(SGMII_BMSR, bm),
+ FIELD_GET(SGMII_LPA, adv));
+ } else {
+ state->link = !!(bm & SGMII_LINK_STATYS);
- /* Release PHYA power down state */
- regmap_write(mpcs->regmap, SGMSYS_QPHY_PWR_STATE_CTRL, 0);
+ regmap_read(mpcs->regmap, SGMSYS_SGMII_MODE, &sgm_mode);
- if (MTK_HAS_CAPS(eth->soc->caps, MTK_NETSYS_V3))
- mtk_sgmii_setup_phya_gen1(mpcs);
+ switch (sgm_mode & SGMII_SPEED_MASK) {
+ case SGMII_SPEED_10:
+ state->speed = SPEED_10;
+ break;
+ case SGMII_SPEED_100:
+ state->speed = SPEED_100;
+ break;
+ case SGMII_SPEED_1000:
+ regmap_read(mpcs->regmap, mpcs->ana_rgc3, &rgc3);
+ rgc3 = FIELD_GET(RG_PHY_SPEED_3_125G, rgc3);
+ state->speed = rgc3 ? SPEED_2500 : SPEED_1000;
+ break;
+ }
- return 0;
+ if (sgm_mode & SGMII_DUPLEX_HALF)
+ state->duplex = DUPLEX_HALF;
+ else
+ state->duplex = DUPLEX_FULL;
+ }
}
-int mtk_sgmii_setup_mode_force(struct mtk_sgmii_pcs *mpcs,
- phy_interface_t interface)
+static int mtk_sgmii_pcs_config(struct phylink_pcs *pcs, unsigned int mode,
+ phy_interface_t interface,
+ const unsigned long *advertising,
+ bool permit_pause_to_mac)
{
+ struct mtk_sgmii_pcs *mpcs = pcs_to_mtk_sgmii_pcs(pcs);
struct mtk_eth *eth = mpcs->eth;
- unsigned int val = 0;
+ unsigned int rgc3, sgm_mode = 0, bmcr = 0, speed = 0;
+ bool mode_changed = false, changed;
+ int advertise, link_timer;
- if (!mpcs->regmap)
- return -EINVAL;
+ advertise = phylink_mii_c22_pcs_encode_advertisement(interface,
+ advertising);
+ if (advertise < 0)
+ return advertise;
if (MTK_HAS_CAPS(eth->soc->caps, MTK_NETSYS_V3)) {
mtk_sgmii_xfi_pll_enable(eth->sgmii);
mtk_sgmii_reset(eth, mpcs->id);
}
- /* Assert PHYA power down state when needed */
- if (mtk_sgmii_need_powerdown(mpcs, interface))
- regmap_write(mpcs->regmap, SGMSYS_QPHY_PWR_STATE_CTRL,
- SGMII_PHYA_PWD);
+ /* Clearing IF_MODE_BIT0 switches the PCS to BASE-X mode, and
+ * we assume that fixes it's speed at bitrate = line rate (in
+ * other words, 1000Mbps or 2500Mbps).
+ */
+ if (interface == PHY_INTERFACE_MODE_SGMII) {
+ bmcr = SGMII_AN_ENABLE;
+ sgm_mode = SGMII_IF_MODE_SGMII |
+ SGMII_REMOTE_FAULT_DIS |
+ SGMII_SPEED_DUPLEX_AN;
+ } else {
+ /* 1000base-X or HSGMII without autoneg */
+ speed = SGMII_SPEED_1000;
+ if (interface == PHY_INTERFACE_MODE_2500BASEX)
+ sgm_mode = SGMII_IF_MODE_SGMII;
+ }
- /* Reset SGMII PCS state */
- regmap_write(mpcs->regmap, SGMII_RESERVED_0, SGMII_SW_RESET);
+ if (mpcs->interface != interface) {
+ link_timer = phylink_get_link_timer_ns(interface);
+ if (link_timer < 0)
+ return link_timer;
- regmap_read(mpcs->regmap, mpcs->ana_rgc3, &val);
- val &= ~RG_PHY_SPEED_MASK;
- if (interface == PHY_INTERFACE_MODE_2500BASEX)
- val |= RG_PHY_SPEED_3_125G;
- regmap_write(mpcs->regmap, mpcs->ana_rgc3, val);
+ /* PHYA power down */
+ regmap_update_bits(mpcs->regmap, SGMSYS_QPHY_PWR_STATE_CTRL,
+ SGMII_PHYA_PWD, SGMII_PHYA_PWD);
- /* Disable SGMII AN */
- regmap_read(mpcs->regmap, SGMSYS_PCS_CONTROL_1, &val);
- val &= ~SGMII_AN_ENABLE;
- regmap_write(mpcs->regmap, SGMSYS_PCS_CONTROL_1, val);
+ /* Reset SGMII PCS state */
+ regmap_update_bits(mpcs->regmap, SGMII_RESERVED_0,
+ SGMII_SW_RESET, SGMII_SW_RESET);
- /* Set the speed etc but leave the duplex unchanged */
- regmap_read(mpcs->regmap, SGMSYS_SGMII_MODE, &val);
- val &= SGMII_DUPLEX_HALF | ~SGMII_IF_MODE_MASK;
- val &= ~SGMII_REMOTE_FAULT_DIS;
- val |= SGMII_SPEED_1000;
- regmap_write(mpcs->regmap, SGMSYS_SGMII_MODE, val);
+ if (interface == PHY_INTERFACE_MODE_2500BASEX)
+ rgc3 = RG_PHY_SPEED_3_125G;
+ else
+ rgc3 = 0;
- if (MTK_HAS_FLAGS(mpcs->flags, MTK_SGMII_PN_SWAP))
- regmap_update_bits(mpcs->regmap, SGMSYS_QPHY_WRAP_CTRL,
- SGMII_PN_SWAP_MASK, SGMII_PN_SWAP_TX_RX);
+ /* Configure the underlying interface speed */
+ regmap_update_bits(mpcs->regmap, mpcs->ana_rgc3,
+ RG_PHY_SPEED_3_125G, rgc3);
- /* Release PHYA power down state */
- regmap_write(mpcs->regmap, SGMSYS_QPHY_PWR_STATE_CTRL, 0);
+ /* Setup the link timer */
+ regmap_write(mpcs->regmap, SGMSYS_PCS_LINK_TIMER,
+ link_timer / 2 / 8);
- if (MTK_HAS_CAPS(eth->soc->caps, MTK_NETSYS_V3))
- mtk_sgmii_setup_phya_gen2(mpcs);
+ mpcs->interface = interface;
+ mode_changed = true;
+ }
- return 0;
-}
+ /* Update the advertisement, noting whether it has changed */
+ regmap_update_bits_check(mpcs->regmap, SGMSYS_PCS_ADVERTISE,
+ SGMII_ADVERTISE, advertise, &changed);
-static void mtk_sgmii_pcs_get_state(struct phylink_pcs *pcs,
- struct phylink_link_state *state)
-{
- struct mtk_sgmii_pcs *mpcs = pcs_to_mtk_sgmii_pcs(pcs);
- u32 rgc3, val = 0;
+ /* Update the sgmsys mode register */
+ regmap_update_bits(mpcs->regmap, SGMSYS_SGMII_MODE,
+ SGMII_REMOTE_FAULT_DIS | SGMII_DUPLEX_HALF |
+ SGMII_SPEED_MASK | SGMII_SPEED_DUPLEX_AN |
+ SGMII_IF_MODE_SGMII, sgm_mode | speed);
- state->interface = mpcs->interface;
+ /* Update the BMCR */
+ regmap_update_bits(mpcs->regmap, SGMSYS_PCS_CONTROL_1,
+ SGMII_AN_ENABLE, bmcr);
- regmap_read(mpcs->regmap, SGMSYS_PCS_CONTROL_1, &val);
- if (FIELD_GET(SGMII_AN_ENABLE, val)) {
- regmap_read(mpcs->regmap, SGMII_PCS_SPEED_ABILITY, &val);
- val = val >> 16;
- state->link = FIELD_GET(SGMII_PCS_SPEED_LINK, val);
- state->duplex = FIELD_GET(SGMII_PCS_SPEED_DUPLEX, val);
- switch (FIELD_GET(SGMII_PCS_SPEED_MASK, val)) {
- case 0:
- state->speed = SPEED_10;
- break;
- case 1:
- state->speed = SPEED_100;
- break;
- case 2:
- state->speed = SPEED_1000;
- break;
- }
- } else {
- state->link = FIELD_GET(SGMII_LINK_STATYS, val);
+ /* Release PHYA power down state */
+ usleep_range(50, 100);
+ regmap_write(mpcs->regmap, SGMSYS_QPHY_PWR_STATE_CTRL, 0);
- regmap_read(mpcs->regmap, SGMSYS_SGMII_MODE, &val);
- state->duplex = !FIELD_GET(SGMII_DUPLEX_HALF, val);
- switch (FIELD_GET(SGMII_SPEED_MASK, val)) {
- case 0:
- state->speed = SPEED_10;
- break;
- case 1:
- state->speed = SPEED_100;
- break;
- case 2:
- regmap_read(mpcs->regmap, mpcs->ana_rgc3, &val);
- rgc3 = FIELD_GET(RG_PHY_SPEED_3_125G, val);
- state->speed = rgc3 ? SPEED_2500 : SPEED_1000;
- break;
- }
+ if (MTK_HAS_CAPS(eth->soc->caps, MTK_NETSYS_V3)) {
+ if (interface == PHY_INTERFACE_MODE_2500BASEX)
+ mtk_sgmii_setup_phya_gen2(mpcs);
+ else
+ mtk_sgmii_setup_phya_gen1(mpcs);
}
-}
-
-static int mtk_sgmii_pcs_config(struct phylink_pcs *pcs, unsigned int mode,
- phy_interface_t interface,
- const unsigned long *advertising,
- bool permit_pause_to_mac)
-{
- struct mtk_sgmii_pcs *mpcs = pcs_to_mtk_sgmii_pcs(pcs);
- int err = 0;
- mpcs->interface = interface;
-
- /* Setup SGMIISYS with the determined property */
- if (interface != PHY_INTERFACE_MODE_SGMII)
- err = mtk_sgmii_setup_mode_force(mpcs, interface);
- else
- err = mtk_sgmii_setup_mode_an(mpcs);
-
- return err;
+ return changed || mode_changed;
}
void mtk_sgmii_pcs_restart_an(struct phylink_pcs *pcs)
@@ -561,18 +500,29 @@
int speed, int duplex)
{
struct mtk_sgmii_pcs *mpcs = pcs_to_mtk_sgmii_pcs(pcs);
- unsigned int val;
+ unsigned int sgm_mode, val;
- if (!phy_interface_mode_is_8023z(interface))
+ /* If autoneg is enabled, the force speed and duplex
+ * are not useful, so don't go any further.
+ */
+ regmap_read(mpcs->regmap, SGMSYS_PCS_CONTROL_1, &val);
+ if (val & SGMII_AN_ENABLE)
return;
+ /* SGMII force speed and duplex setting */
+ if (speed == SPEED_10)
+ sgm_mode = SGMII_SPEED_10;
+ else if (speed == SPEED_100)
+ sgm_mode = SGMII_SPEED_100;
+ else
+ sgm_mode = SGMII_SPEED_1000;
+
- /* SGMII force duplex setting */
- regmap_read(mpcs->regmap, SGMSYS_SGMII_MODE, &val);
- val &= ~SGMII_DUPLEX_HALF;
if (duplex != DUPLEX_FULL)
- val |= SGMII_DUPLEX_HALF;
+ sgm_mode |= SGMII_DUPLEX_HALF;
- regmap_write(mpcs->regmap, SGMSYS_SGMII_MODE, val);
+ regmap_update_bits(mpcs->regmap, SGMSYS_SGMII_MODE,
+ SGMII_DUPLEX_HALF | SGMII_SPEED_MASK,
+ sgm_mode);
}
static const struct phylink_pcs_ops mtk_sgmii_pcs_ops = {
diff --git a/target/linux/mediatek/patches-5.4/758-net-phy-add-phylink-pcs-decode-helper.patch b/target/linux/mediatek/patches-5.4/758-net-phy-add-phylink-pcs-decode-helper.patch
new file mode 100644
index 0000000..164578a
--- /dev/null
+++ b/target/linux/mediatek/patches-5.4/758-net-phy-add-phylink-pcs-decode-helper.patch
@@ -0,0 +1,479 @@
+diff --git a/drivers/net/phy/Makefile b/drivers/net/phy/Makefile
+index 799ff9c..54ad7e8 100644
+--- a/drivers/net/phy/Makefile
++++ b/drivers/net/phy/Makefile
+@@ -1,7 +1,8 @@
+ # SPDX-License-Identifier: GPL-2.0
+ # Makefile for Linux PHY drivers and MDIO bus drivers
+
+-libphy-y := phy.o phy-c45.o phy-core.o phy_device.o
++libphy-y := phy.o phy-c45.o phy-core.o phy_device.o \
++ linkmode.o
+ mdio-bus-y += mdio_bus.o mdio_device.o
+
+ ifdef CONFIG_MDIO_DEVICE
+diff --git a/drivers/net/phy/linkmode.c b/drivers/net/phy/linkmode.c
+new file mode 100644
+index 0000000..a5a347b
+--- /dev/null
++++ b/drivers/net/phy/linkmode.c
+@@ -0,0 +1,95 @@
++// SPDX-License-Identifier: GPL-2.0+
++#include <linux/linkmode.h>
++
++/**
++ * linkmode_resolve_pause - resolve the allowable pause modes
++ * @local_adv: local advertisement in ethtool format
++ * @partner_adv: partner advertisement in ethtool format
++ * @tx_pause: pointer to bool to indicate whether transmit pause should be
++ * enabled.
++ * @rx_pause: pointer to bool to indicate whether receive pause should be
++ * enabled.
++ *
++ * Flow control is resolved according to our and the link partners
++ * advertisements using the following drawn from the 802.3 specs:
++ * Local device Link partner
++ * Pause AsymDir Pause AsymDir Result
++ * 0 X 0 X Disabled
++ * 0 1 1 0 Disabled
++ * 0 1 1 1 TX
++ * 1 0 0 X Disabled
++ * 1 X 1 X TX+RX
++ * 1 1 0 1 RX
++ */
++void linkmode_resolve_pause(unsigned long *local_adv,
++ unsigned long *partner_adv,
++ bool *tx_pause, bool *rx_pause)
++{
++ __ETHTOOL_DECLARE_LINK_MODE_MASK(m);
++
++ linkmode_and(m, local_adv, partner_adv);
++ if (linkmode_test_bit(ETHTOOL_LINK_MODE_Pause_BIT, m)) {
++ *tx_pause = true;
++ *rx_pause = true;
++ } else if (linkmode_test_bit(ETHTOOL_LINK_MODE_Asym_Pause_BIT, m)) {
++ *tx_pause = linkmode_test_bit(ETHTOOL_LINK_MODE_Pause_BIT,
++ partner_adv);
++ *rx_pause = linkmode_test_bit(ETHTOOL_LINK_MODE_Pause_BIT,
++ local_adv);
++ } else {
++ *tx_pause = false;
++ *rx_pause = false;
++ }
++}
++EXPORT_SYMBOL_GPL(linkmode_resolve_pause);
++
++/**
++ * linkmode_set_pause - set the pause mode advertisement
++ * @advertisement: advertisement in ethtool format
++ * @tx: boolean from ethtool struct ethtool_pauseparam tx_pause member
++ * @rx: boolean from ethtool struct ethtool_pauseparam rx_pause member
++ *
++ * Configure the advertised Pause and Asym_Pause bits according to the
++ * capabilities of provided in @tx and @rx.
++ *
++ * We convert as follows:
++ * tx rx Pause AsymDir
++ * 0 0 0 0
++ * 0 1 1 1
++ * 1 0 0 1
++ * 1 1 1 0
++ *
++ * Note: this translation from ethtool tx/rx notation to the advertisement
++ * is actually very problematical. Here are some examples:
++ *
++ * For tx=0 rx=1, meaning transmit is unsupported, receive is supported:
++ *
++ * Local device Link partner
++ * Pause AsymDir Pause AsymDir Result
++ * 1 1 1 0 TX + RX - but we have no TX support.
++ * 1 1 0 1 Only this gives RX only
++ *
++ * For tx=1 rx=1, meaning we have the capability to transmit and receive
++ * pause frames:
++ *
++ * Local device Link partner
++ * Pause AsymDir Pause AsymDir Result
++ * 1 0 0 1 Disabled - but since we do support tx and rx,
++ * this should resolve to RX only.
++ *
++ * Hence, asking for:
++ * rx=1 tx=0 gives Pause+AsymDir advertisement, but we may end up
++ * resolving to tx+rx pause or only rx pause depending on
++ * the partners advertisement.
++ * rx=0 tx=1 gives AsymDir only, which will only give tx pause if
++ * the partners advertisement allows it.
++ * rx=1 tx=1 gives Pause only, which will only allow tx+rx pause
++ * if the other end also advertises Pause.
++ */
++void linkmode_set_pause(unsigned long *advertisement, bool tx, bool rx)
++{
++ linkmode_mod_bit(ETHTOOL_LINK_MODE_Pause_BIT, advertisement, rx);
++ linkmode_mod_bit(ETHTOOL_LINK_MODE_Asym_Pause_BIT, advertisement,
++ rx ^ tx);
++}
++EXPORT_SYMBOL_GPL(linkmode_set_pause);
+diff --git a/drivers/net/phy/phylink.c b/drivers/net/phy/phylink.c
+index ead9b37..fec6d0e 100644
+--- a/drivers/net/phy/phylink.c
++++ b/drivers/net/phy/phylink.c
+@@ -2184,4 +2184,192 @@ void phylink_helper_basex_speed(struct phylink_link_state *state)
+ }
+ EXPORT_SYMBOL_GPL(phylink_helper_basex_speed);
+
++static void phylink_decode_c37_word(struct phylink_link_state *state,
++ uint16_t config_reg, int speed)
++{
++ bool tx_pause, rx_pause;
++ int fd_bit;
++
++ if (speed == SPEED_2500)
++ fd_bit = ETHTOOL_LINK_MODE_2500baseX_Full_BIT;
++ else
++ fd_bit = ETHTOOL_LINK_MODE_1000baseX_Full_BIT;
++
++ mii_lpa_mod_linkmode_x(state->lp_advertising, config_reg, fd_bit);
++
++ if (linkmode_test_bit(fd_bit, state->advertising) &&
++ linkmode_test_bit(fd_bit, state->lp_advertising)) {
++ state->speed = speed;
++ state->duplex = DUPLEX_FULL;
++ } else {
++ /* negotiation failure */
++ state->link = false;
++ }
++
++ linkmode_resolve_pause(state->advertising, state->lp_advertising,
++ &tx_pause, &rx_pause);
++
++ if (tx_pause)
++ state->pause |= MLO_PAUSE_TX;
++ if (rx_pause)
++ state->pause |= MLO_PAUSE_RX;
++}
++
++static void phylink_decode_sgmii_word(struct phylink_link_state *state,
++ uint16_t config_reg)
++{
++ if (!(config_reg & LPA_SGMII_LINK)) {
++ state->link = false;
++ return;
++ }
++
++ switch (config_reg & LPA_SGMII_SPD_MASK) {
++ case LPA_SGMII_10:
++ state->speed = SPEED_10;
++ break;
++ case LPA_SGMII_100:
++ state->speed = SPEED_100;
++ break;
++ case LPA_SGMII_1000:
++ state->speed = SPEED_1000;
++ break;
++ default:
++ state->link = false;
++ return;
++ }
++ if (config_reg & LPA_SGMII_FULL_DUPLEX)
++ state->duplex = DUPLEX_FULL;
++ else
++ state->duplex = DUPLEX_HALF;
++}
++
++/**
++ * phylink_decode_usxgmii_word() - decode the USXGMII word from a MAC PCS
++ * @state: a pointer to a struct phylink_link_state.
++ * @lpa: a 16 bit value which stores the USXGMII auto-negotiation word
++ *
++ * Helper for MAC PCS supporting the USXGMII protocol and the auto-negotiation
++ * code word. Decode the USXGMII code word and populate the corresponding fields
++ * (speed, duplex) into the phylink_link_state structure.
++ */
++void phylink_decode_usxgmii_word(struct phylink_link_state *state,
++ uint16_t lpa)
++{
++ switch (lpa & MDIO_USXGMII_SPD_MASK) {
++ case MDIO_USXGMII_10:
++ state->speed = SPEED_10;
++ break;
++ case MDIO_USXGMII_100:
++ state->speed = SPEED_100;
++ break;
++ case MDIO_USXGMII_1000:
++ state->speed = SPEED_1000;
++ break;
++ case MDIO_USXGMII_2500:
++ state->speed = SPEED_2500;
++ break;
++ case MDIO_USXGMII_5000:
++ state->speed = SPEED_5000;
++ break;
++ case MDIO_USXGMII_10G:
++ state->speed = SPEED_10000;
++ break;
++ default:
++ state->link = false;
++ return;
++ }
++
++ if (lpa & MDIO_USXGMII_FULL_DUPLEX)
++ state->duplex = DUPLEX_FULL;
++ else
++ state->duplex = DUPLEX_HALF;
++}
++EXPORT_SYMBOL_GPL(phylink_decode_usxgmii_word);
++
++/**
++ * phylink_mii_c22_pcs_decode_state() - Decode MAC PCS state from MII registers
++ * @state: a pointer to a &struct phylink_link_state.
++ * @bmsr: The value of the %MII_BMSR register
++ * @lpa: The value of the %MII_LPA register
++ *
++ * Helper for MAC PCS supporting the 802.3 clause 22 register set for
++ * clause 37 negotiation and/or SGMII control.
++ *
++ * Parse the Clause 37 or Cisco SGMII link partner negotiation word into
++ * the phylink @state structure. This is suitable to be used for implementing
++ * the mac_pcs_get_state() member of the struct phylink_mac_ops structure if
++ * accessing @bmsr and @lpa cannot be done with MDIO directly.
++ */
++void phylink_mii_c22_pcs_decode_state(struct phylink_link_state *state,
++ u16 bmsr, u16 lpa)
++{
++ state->link = !!(bmsr & BMSR_LSTATUS);
++ state->an_complete = !!(bmsr & BMSR_ANEGCOMPLETE);
++ /* If there is no link or autonegotiation is disabled, the LP advertisement
++ * data is not meaningful, so don't go any further.
++ */
++ if (!state->link || !state->an_enabled)
++ return;
++
++ switch (state->interface) {
++ case PHY_INTERFACE_MODE_1000BASEX:
++ phylink_decode_c37_word(state, lpa, SPEED_1000);
++ break;
++
++ case PHY_INTERFACE_MODE_2500BASEX:
++ phylink_decode_c37_word(state, lpa, SPEED_2500);
++ break;
++
++ case PHY_INTERFACE_MODE_SGMII:
++ case PHY_INTERFACE_MODE_QSGMII:
++ phylink_decode_sgmii_word(state, lpa);
++ break;
++
++ default:
++ state->link = false;
++ break;
++ }
++}
++EXPORT_SYMBOL_GPL(phylink_mii_c22_pcs_decode_state);
++
++/**
++ * phylink_mii_c22_pcs_encode_advertisement() - configure the clause 37 PCS
++ * advertisement
++ * @interface: the PHY interface mode being configured
++ * @advertising: the ethtool advertisement mask
++ *
++ * Helper for MAC PCS supporting the 802.3 clause 22 register set for
++ * clause 37 negotiation and/or SGMII control.
++ *
++ * Encode the clause 37 PCS advertisement as specified by @interface and
++ * @advertising.
++ *
++ * Return: The new value for @adv, or ``-EINVAL`` if it should not be changed.
++ */
++int phylink_mii_c22_pcs_encode_advertisement(phy_interface_t interface,
++ const unsigned long *advertising)
++{
++ u16 adv;
++
++ switch (interface) {
++ case PHY_INTERFACE_MODE_1000BASEX:
++ case PHY_INTERFACE_MODE_2500BASEX:
++ adv = ADVERTISE_1000XFULL;
++ if (linkmode_test_bit(ETHTOOL_LINK_MODE_Pause_BIT,
++ advertising))
++ adv |= ADVERTISE_1000XPAUSE;
++ if (linkmode_test_bit(ETHTOOL_LINK_MODE_Asym_Pause_BIT,
++ advertising))
++ adv |= ADVERTISE_1000XPSE_ASYM;
++ return adv;
++ case PHY_INTERFACE_MODE_SGMII:
++ case PHY_INTERFACE_MODE_QSGMII:
++ return 0x0001;
++ default:
++ /* Nothing to do for other modes */
++ return -EINVAL;
++ }
++}
++EXPORT_SYMBOL_GPL(phylink_mii_c22_pcs_encode_advertisement);
++
+ MODULE_LICENSE("GPL v2");
+diff --git a/include/linux/linkmode.h b/include/linux/linkmode.h
+index a99c588..d38da4e 100644
+--- a/include/linux/linkmode.h
++++ b/include/linux/linkmode.h
+@@ -82,4 +82,10 @@ static inline int linkmode_equal(const unsigned long *src1,
+ return bitmap_equal(src1, src2, __ETHTOOL_LINK_MODE_MASK_NBITS);
+ }
+
++void linkmode_resolve_pause(unsigned long *local_adv,
++ unsigned long *partner_adv,
++ bool *tx_pause, bool *rx_pause);
++
++void linkmode_set_pause(unsigned long *advertisement, bool tx, bool rx);
++
+ #endif /* __LINKMODE_H */
+diff --git a/include/linux/mii.h b/include/linux/mii.h
+index 4ce8901..54b7f64 100644
+--- a/include/linux/mii.h
++++ b/include/linux/mii.h
+@@ -485,6 +485,45 @@ static inline u32 linkmode_adv_to_lcl_adv_t(unsigned long *advertising)
+ return lcl_adv;
+ }
+
++/**
++ * mii_lpa_mod_linkmode_x - decode the link partner's config_reg to linkmodes
++ * @linkmodes: link modes array
++ * @lpa: config_reg word from link partner
++ * @fd_bit: link mode for 1000XFULL bit
++ */
++static inline void mii_lpa_mod_linkmode_x(unsigned long *linkmodes, u16 lpa,
++ int fd_bit)
++{
++ linkmode_mod_bit(ETHTOOL_LINK_MODE_Autoneg_BIT, linkmodes,
++ lpa & LPA_LPACK);
++ linkmode_mod_bit(ETHTOOL_LINK_MODE_Pause_BIT, linkmodes,
++ lpa & LPA_1000XPAUSE);
++ linkmode_mod_bit(ETHTOOL_LINK_MODE_Asym_Pause_BIT, linkmodes,
++ lpa & LPA_1000XPAUSE_ASYM);
++ linkmode_mod_bit(fd_bit, linkmodes,
++ lpa & LPA_1000XFULL);
++}
++
++/**
++ * linkmode_adv_to_mii_adv_x - encode a linkmode to config_reg
++ * @linkmodes: linkmodes
++ * @fd_bit: full duplex bit
++ */
++static inline u16 linkmode_adv_to_mii_adv_x(unsigned long *linkmodes,
++ int fd_bit)
++{
++ u16 adv = 0;
++
++ if (linkmode_test_bit(fd_bit, linkmodes))
++ adv |= ADVERTISE_1000XFULL;
++ if (linkmode_test_bit(ETHTOOL_LINK_MODE_Pause_BIT, linkmodes))
++ adv |= ADVERTISE_1000XPAUSE;
++ if (linkmode_test_bit(ETHTOOL_LINK_MODE_Asym_Pause_BIT, linkmodes))
++ adv |= ADVERTISE_1000XPSE_ASYM;
++
++ return adv;
++}
++
+ /**
+ * mii_advertise_flowctrl - get flow control advertisement flags
+ * @cap: Flow control capabilities (FLOW_CTRL_RX, FLOW_CTRL_TX or both)
+diff --git a/include/linux/phylink.h b/include/linux/phylink.h
+index ba0f09d..48ff9fe 100644
+--- a/include/linux/phylink.h
++++ b/include/linux/phylink.h
+@@ -490,4 +490,34 @@ int phylink_mii_ioctl(struct phylink *, struct ifreq *, int);
+ void phylink_set_port_modes(unsigned long *bits);
+ void phylink_helper_basex_speed(struct phylink_link_state *state);
+
++/**
++ * phylink_get_link_timer_ns - return the PCS link timer value
++ * @interface: link &typedef phy_interface_t mode
++ *
++ * Return the PCS link timer setting in nanoseconds for the PHY @interface
++ * mode, or -EINVAL if not appropriate.
++ */
++static inline int phylink_get_link_timer_ns(phy_interface_t interface)
++{
++ switch (interface) {
++ case PHY_INTERFACE_MODE_SGMII:
++ case PHY_INTERFACE_MODE_QSGMII:
++ case PHY_INTERFACE_MODE_USXGMII:
++ return 1600000;
++
++ case PHY_INTERFACE_MODE_1000BASEX:
++ case PHY_INTERFACE_MODE_2500BASEX:
++ return 10000000;
++
++ default:
++ return -EINVAL;
++ }
++}
++
++void phylink_mii_c22_pcs_decode_state(struct phylink_link_state *state,
++ u16 bmsr, u16 lpa);
++int phylink_mii_c22_pcs_encode_advertisement(phy_interface_t interface,
++ const unsigned long *advertising);
++void phylink_decode_usxgmii_word(struct phylink_link_state *state,
++ uint16_t lpa);
+ #endif
+diff --git a/include/uapi/linux/mdio.h b/include/uapi/linux/mdio.h
+index 4bcb41c..3f302e2 100644
+--- a/include/uapi/linux/mdio.h
++++ b/include/uapi/linux/mdio.h
+@@ -324,4 +324,30 @@ static inline __u16 mdio_phy_id_c45(int prtad, int devad)
+ return MDIO_PHY_ID_C45 | (prtad << 5) | devad;
+ }
+
++/* UsxgmiiChannelInfo[15:0] for USXGMII in-band auto-negotiation.*/
++#define MDIO_USXGMII_EEE_CLK_STP 0x0080 /* EEE clock stop supported */
++#define MDIO_USXGMII_EEE 0x0100 /* EEE supported */
++#define MDIO_USXGMII_SPD_MASK 0x0e00 /* USXGMII speed mask */
++#define MDIO_USXGMII_FULL_DUPLEX 0x1000 /* USXGMII full duplex */
++#define MDIO_USXGMII_DPX_SPD_MASK 0x1e00 /* USXGMII duplex and speed bits */
++#define MDIO_USXGMII_10 0x0000 /* 10Mbps */
++#define MDIO_USXGMII_10HALF 0x0000 /* 10Mbps half-duplex */
++#define MDIO_USXGMII_10FULL 0x1000 /* 10Mbps full-duplex */
++#define MDIO_USXGMII_100 0x0200 /* 100Mbps */
++#define MDIO_USXGMII_100HALF 0x0200 /* 100Mbps half-duplex */
++#define MDIO_USXGMII_100FULL 0x1200 /* 100Mbps full-duplex */
++#define MDIO_USXGMII_1000 0x0400 /* 1000Mbps */
++#define MDIO_USXGMII_1000HALF 0x0400 /* 1000Mbps half-duplex */
++#define MDIO_USXGMII_1000FULL 0x1400 /* 1000Mbps full-duplex */
++#define MDIO_USXGMII_10G 0x0600 /* 10Gbps */
++#define MDIO_USXGMII_10GHALF 0x0600 /* 10Gbps half-duplex */
++#define MDIO_USXGMII_10GFULL 0x1600 /* 10Gbps full-duplex */
++#define MDIO_USXGMII_2500 0x0800 /* 2500Mbps */
++#define MDIO_USXGMII_2500HALF 0x0800 /* 2500Mbps half-duplex */
++#define MDIO_USXGMII_2500FULL 0x1800 /* 2500Mbps full-duplex */
++#define MDIO_USXGMII_5000 0x0a00 /* 5000Mbps */
++#define MDIO_USXGMII_5000HALF 0x0a00 /* 5000Mbps half-duplex */
++#define MDIO_USXGMII_5000FULL 0x1a00 /* 5000Mbps full-duplex */
++#define MDIO_USXGMII_LINK 0x8000 /* PHY link with copper-side partner */
++
+ #endif /* _UAPI__LINUX_MDIO_H__ */
+diff --git a/include/uapi/linux/mii.h b/include/uapi/linux/mii.h
+index 51b48e4..90f9b4e 100644
+--- a/include/uapi/linux/mii.h
++++ b/include/uapi/linux/mii.h
+@@ -131,6 +131,23 @@
+ #define NWAYTEST_LOOPBACK 0x0100 /* Enable loopback for N-way */
+ #define NWAYTEST_RESV2 0xfe00 /* Unused... */
+
++/* MAC and PHY tx_config_Reg[15:0] for SGMII in-band auto-negotiation.*/
++#define ADVERTISE_SGMII 0x0001 /* MAC can do SGMII */
++#define LPA_SGMII 0x0001 /* PHY can do SGMII */
++#define LPA_SGMII_SPD_MASK 0x0c00 /* SGMII speed mask */
++#define LPA_SGMII_FULL_DUPLEX 0x1000 /* SGMII full duplex */
++#define LPA_SGMII_DPX_SPD_MASK 0x1C00 /* SGMII duplex and speed bits */
++#define LPA_SGMII_10 0x0000 /* 10Mbps */
++#define LPA_SGMII_10HALF 0x0000 /* Can do 10mbps half-duplex */
++#define LPA_SGMII_10FULL 0x1000 /* Can do 10mbps full-duplex */
++#define LPA_SGMII_100 0x0400 /* 100Mbps */
++#define LPA_SGMII_100HALF 0x0400 /* Can do 100mbps half-duplex */
++#define LPA_SGMII_100FULL 0x1400 /* Can do 100mbps full-duplex */
++#define LPA_SGMII_1000 0x0800 /* 1000Mbps */
++#define LPA_SGMII_1000HALF 0x0800 /* Can do 1000mbps half-duplex */
++#define LPA_SGMII_1000FULL 0x1800 /* Can do 1000mbps full-duplex */
++#define LPA_SGMII_LINK 0x8000 /* PHY link with copper-side partner */
++
+ /* 1000BASE-T Control register */
+ #define ADVERTISE_1000FULL 0x0200 /* Advertise 1000BASE-T full duplex */
+ #define ADVERTISE_1000HALF 0x0100 /* Advertise 1000BASE-T half duplex */