From bedbef8ad3e75a304af6361af235302bcc61d06b Mon Sep 17 00:00:00 2001
From: hc <hc@nodka.com>
Date: Tue, 14 May 2024 06:39:01 +0000
Subject: [PATCH] 修改内核路径
---
kernel/drivers/net/phy/phy-c45.c | 397 ++++++++++++++++++++++++++++++++++++++++++++------------
1 files changed, 311 insertions(+), 86 deletions(-)
diff --git a/kernel/drivers/net/phy/phy-c45.c b/kernel/drivers/net/phy/phy-c45.c
index 0ba3607..bd11e62 100644
--- a/kernel/drivers/net/phy/phy-c45.c
+++ b/kernel/drivers/net/phy/phy-c45.c
@@ -1,3 +1,4 @@
+// SPDX-License-Identifier: GPL-2.0-only
/*
* Clause 45 PHY support
*/
@@ -47,6 +48,16 @@
/* Assume 1000base-T */
ctrl2 |= MDIO_PMA_CTRL2_1000BT;
break;
+ case SPEED_2500:
+ ctrl1 |= MDIO_CTRL1_SPEED2_5G;
+ /* Assume 2.5Gbase-T */
+ ctrl2 |= MDIO_PMA_CTRL2_2_5GBT;
+ break;
+ case SPEED_5000:
+ ctrl1 |= MDIO_CTRL1_SPEED5G;
+ /* Assume 5Gbase-T */
+ ctrl2 |= MDIO_PMA_CTRL2_5GBT;
+ break;
case SPEED_10000:
ctrl1 |= MDIO_CTRL1_SPEED10G;
/* Assume 10Gbase-T */
@@ -60,9 +71,58 @@
if (ret < 0)
return ret;
- return phy_write_mmd(phydev, MDIO_MMD_PMAPMD, MDIO_CTRL2, ctrl2);
+ ret = phy_write_mmd(phydev, MDIO_MMD_PMAPMD, MDIO_CTRL2, ctrl2);
+ if (ret < 0)
+ return ret;
+
+ return genphy_c45_an_disable_aneg(phydev);
}
EXPORT_SYMBOL_GPL(genphy_c45_pma_setup_forced);
+
+/**
+ * genphy_c45_an_config_aneg - configure advertisement registers
+ * @phydev: target phy_device struct
+ *
+ * Configure advertisement registers based on modes set in phydev->advertising
+ *
+ * Returns negative errno code on failure, 0 if advertisement didn't change,
+ * or 1 if advertised modes changed.
+ */
+int genphy_c45_an_config_aneg(struct phy_device *phydev)
+{
+ int changed, ret;
+ u32 adv;
+
+ linkmode_and(phydev->advertising, phydev->advertising,
+ phydev->supported);
+
+ changed = genphy_config_eee_advert(phydev);
+
+ adv = linkmode_adv_to_mii_adv_t(phydev->advertising);
+
+ ret = phy_modify_mmd_changed(phydev, MDIO_MMD_AN, MDIO_AN_ADVERTISE,
+ ADVERTISE_ALL | ADVERTISE_100BASE4 |
+ ADVERTISE_PAUSE_CAP | ADVERTISE_PAUSE_ASYM,
+ adv);
+ if (ret < 0)
+ return ret;
+ if (ret > 0)
+ changed = 1;
+
+ adv = linkmode_adv_to_mii_10gbt_adv_t(phydev->advertising);
+
+ ret = phy_modify_mmd_changed(phydev, MDIO_MMD_AN, MDIO_AN_10GBT_CTRL,
+ MDIO_AN_10GBT_CTRL_ADV10G |
+ MDIO_AN_10GBT_CTRL_ADV5G |
+ MDIO_AN_10GBT_CTRL_ADV2_5G, adv);
+ if (ret < 0)
+ return ret;
+ if (ret > 0)
+ changed = 1;
+
+ return changed;
+}
+EXPORT_SYMBOL_GPL(genphy_c45_an_config_aneg);
/**
* genphy_c45_an_disable_aneg - disable auto-negotiation
@@ -75,15 +135,9 @@
*/
int genphy_c45_an_disable_aneg(struct phy_device *phydev)
{
- int val;
- val = phy_read_mmd(phydev, MDIO_MMD_AN, MDIO_CTRL1);
- if (val < 0)
- return val;
-
- val &= ~(MDIO_AN_CTRL1_ENABLE | MDIO_AN_CTRL1_RESTART);
-
- return phy_write_mmd(phydev, MDIO_MMD_AN, MDIO_CTRL1, val);
+ return phy_clear_bits_mmd(phydev, MDIO_MMD_AN, MDIO_CTRL1,
+ MDIO_AN_CTRL1_ENABLE | MDIO_AN_CTRL1_RESTART);
}
EXPORT_SYMBOL_GPL(genphy_c45_an_disable_aneg);
@@ -97,17 +151,40 @@
*/
int genphy_c45_restart_aneg(struct phy_device *phydev)
{
- int val;
-
- val = phy_read_mmd(phydev, MDIO_MMD_AN, MDIO_CTRL1);
- if (val < 0)
- return val;
-
- val |= MDIO_AN_CTRL1_ENABLE | MDIO_AN_CTRL1_RESTART;
-
- return phy_write_mmd(phydev, MDIO_MMD_AN, MDIO_CTRL1, val);
+ return phy_set_bits_mmd(phydev, MDIO_MMD_AN, MDIO_CTRL1,
+ MDIO_AN_CTRL1_ENABLE | MDIO_AN_CTRL1_RESTART);
}
EXPORT_SYMBOL_GPL(genphy_c45_restart_aneg);
+
+/**
+ * genphy_c45_check_and_restart_aneg - Enable and restart auto-negotiation
+ * @phydev: target phy_device struct
+ * @restart: whether aneg restart is requested
+ *
+ * This assumes that the auto-negotiation MMD is present.
+ *
+ * Check, and restart auto-negotiation if needed.
+ */
+int genphy_c45_check_and_restart_aneg(struct phy_device *phydev, bool restart)
+{
+ int ret;
+
+ if (!restart) {
+ /* Configure and restart aneg if it wasn't set before */
+ ret = phy_read_mmd(phydev, MDIO_MMD_AN, MDIO_CTRL1);
+ if (ret < 0)
+ return ret;
+
+ if (!(ret & MDIO_AN_CTRL1_ENABLE))
+ restart = true;
+ }
+
+ if (restart)
+ return genphy_c45_restart_aneg(phydev);
+
+ return 0;
+}
+EXPORT_SYMBOL_GPL(genphy_c45_check_and_restart_aneg);
/**
* genphy_c45_aneg_done - return auto-negotiation complete status
@@ -131,29 +208,46 @@
/**
* genphy_c45_read_link - read the overall link status from the MMDs
* @phydev: target phy_device struct
- * @mmd_mask: MMDs to read status from
*
* Read the link status from the specified MMDs, and if they all indicate
- * that the link is up, return positive. If an error is encountered,
+ * that the link is up, set phydev->link to 1. If an error is encountered,
* a negative errno will be returned, otherwise zero.
*/
-int genphy_c45_read_link(struct phy_device *phydev, u32 mmd_mask)
+int genphy_c45_read_link(struct phy_device *phydev)
{
+ u32 mmd_mask = MDIO_DEVS_PMAPMD;
int val, devad;
bool link = true;
- while (mmd_mask) {
+ if (phydev->c45_ids.mmds_present & MDIO_DEVS_AN) {
+ val = phy_read_mmd(phydev, MDIO_MMD_AN, MDIO_CTRL1);
+ if (val < 0)
+ return val;
+
+ /* Autoneg is being started, therefore disregard current
+ * link status and report link as down.
+ */
+ if (val & MDIO_AN_CTRL1_RESTART) {
+ phydev->link = 0;
+ return 0;
+ }
+ }
+
+ while (mmd_mask && link) {
devad = __ffs(mmd_mask);
mmd_mask &= ~BIT(devad);
/* The link state is latched low so that momentary link
* drops can be detected. Do not double-read the status
- * in polling mode to detect such short link drops.
+ * in polling mode to detect such short link drops except
+ * the link was already down.
*/
- if (!phy_polling_mode(phydev)) {
+ if (!phy_polling_mode(phydev) || !phydev->link) {
val = phy_read_mmd(phydev, devad, MDIO_STAT1);
if (val < 0)
return val;
+ else if (val & MDIO_STAT1_LSTATUS)
+ continue;
}
val = phy_read_mmd(phydev, devad, MDIO_STAT1);
@@ -164,7 +258,9 @@
link = false;
}
- return link;
+ phydev->link = link;
+
+ return 0;
}
EXPORT_SYMBOL_GPL(genphy_c45_read_link);
@@ -182,12 +278,30 @@
{
int val;
+ val = phy_read_mmd(phydev, MDIO_MMD_AN, MDIO_STAT1);
+ if (val < 0)
+ return val;
+
+ if (!(val & MDIO_AN_STAT1_COMPLETE)) {
+ linkmode_clear_bit(ETHTOOL_LINK_MODE_Autoneg_BIT,
+ phydev->lp_advertising);
+ mii_10gbt_stat_mod_linkmode_lpa_t(phydev->lp_advertising, 0);
+ mii_adv_mod_linkmode_adv_t(phydev->lp_advertising, 0);
+ phydev->pause = 0;
+ phydev->asym_pause = 0;
+
+ return 0;
+ }
+
+ linkmode_mod_bit(ETHTOOL_LINK_MODE_Autoneg_BIT, phydev->lp_advertising,
+ val & MDIO_AN_STAT1_LPABLE);
+
/* Read the link partner's base page advertisement */
val = phy_read_mmd(phydev, MDIO_MMD_AN, MDIO_AN_LPA);
if (val < 0)
return val;
- phydev->lp_advertising = mii_lpa_to_ethtool_lpa_t(val);
+ mii_adv_mod_linkmode_adv_t(phydev->lp_advertising, val);
phydev->pause = val & LPA_PAUSE_CAP ? 1 : 0;
phydev->asym_pause = val & LPA_PAUSE_ASYM ? 1 : 0;
@@ -196,8 +310,7 @@
if (val < 0)
return val;
- if (val & MDIO_AN_10GBT_STAT_LP10G)
- phydev->lp_advertising |= ADVERTISED_10000baseT_Full;
+ mii_10gbt_stat_mod_linkmode_lpa_t(phydev->lp_advertising, val);
return 0;
}
@@ -210,6 +323,8 @@
int genphy_c45_read_pma(struct phy_device *phydev)
{
int val;
+
+ linkmode_zero(phydev->lp_advertising);
val = phy_read_mmd(phydev, MDIO_MMD_PMAPMD, MDIO_CTRL1);
if (val < 0)
@@ -224,6 +339,12 @@
break;
case MDIO_PMA_CTRL1_SPEED1000:
phydev->speed = SPEED_1000;
+ break;
+ case MDIO_CTRL1_SPEED2_5G:
+ phydev->speed = SPEED_2500;
+ break;
+ case MDIO_CTRL1_SPEED5G:
+ phydev->speed = SPEED_5000;
break;
case MDIO_CTRL1_SPEED10G:
phydev->speed = SPEED_10000;
@@ -272,6 +393,165 @@
}
EXPORT_SYMBOL_GPL(genphy_c45_read_mdix);
+/**
+ * genphy_c45_pma_read_abilities - read supported link modes from PMA
+ * @phydev: target phy_device struct
+ *
+ * Read the supported link modes from the PMA Status 2 (1.8) register. If bit
+ * 1.8.9 is set, the list of supported modes is build using the values in the
+ * PMA Extended Abilities (1.11) register, indicating 1000BASET an 10G related
+ * modes. If bit 1.11.14 is set, then the list is also extended with the modes
+ * in the 2.5G/5G PMA Extended register (1.21), indicating if 2.5GBASET and
+ * 5GBASET are supported.
+ */
+int genphy_c45_pma_read_abilities(struct phy_device *phydev)
+{
+ int val;
+
+ linkmode_clear_bit(ETHTOOL_LINK_MODE_Autoneg_BIT, phydev->supported);
+ if (phydev->c45_ids.mmds_present & MDIO_DEVS_AN) {
+ val = phy_read_mmd(phydev, MDIO_MMD_AN, MDIO_STAT1);
+ if (val < 0)
+ return val;
+
+ if (val & MDIO_AN_STAT1_ABLE)
+ linkmode_set_bit(ETHTOOL_LINK_MODE_Autoneg_BIT,
+ phydev->supported);
+ }
+
+ val = phy_read_mmd(phydev, MDIO_MMD_PMAPMD, MDIO_STAT2);
+ if (val < 0)
+ return val;
+
+ linkmode_mod_bit(ETHTOOL_LINK_MODE_10000baseSR_Full_BIT,
+ phydev->supported,
+ val & MDIO_PMA_STAT2_10GBSR);
+
+ linkmode_mod_bit(ETHTOOL_LINK_MODE_10000baseLR_Full_BIT,
+ phydev->supported,
+ val & MDIO_PMA_STAT2_10GBLR);
+
+ linkmode_mod_bit(ETHTOOL_LINK_MODE_10000baseER_Full_BIT,
+ phydev->supported,
+ val & MDIO_PMA_STAT2_10GBER);
+
+ if (val & MDIO_PMA_STAT2_EXTABLE) {
+ val = phy_read_mmd(phydev, MDIO_MMD_PMAPMD, MDIO_PMA_EXTABLE);
+ if (val < 0)
+ return val;
+
+ linkmode_mod_bit(ETHTOOL_LINK_MODE_10000baseLRM_Full_BIT,
+ phydev->supported,
+ val & MDIO_PMA_EXTABLE_10GBLRM);
+ linkmode_mod_bit(ETHTOOL_LINK_MODE_10000baseT_Full_BIT,
+ phydev->supported,
+ val & MDIO_PMA_EXTABLE_10GBT);
+ linkmode_mod_bit(ETHTOOL_LINK_MODE_10000baseKX4_Full_BIT,
+ phydev->supported,
+ val & MDIO_PMA_EXTABLE_10GBKX4);
+ linkmode_mod_bit(ETHTOOL_LINK_MODE_10000baseKR_Full_BIT,
+ phydev->supported,
+ val & MDIO_PMA_EXTABLE_10GBKR);
+ linkmode_mod_bit(ETHTOOL_LINK_MODE_1000baseT_Full_BIT,
+ phydev->supported,
+ val & MDIO_PMA_EXTABLE_1000BT);
+ linkmode_mod_bit(ETHTOOL_LINK_MODE_1000baseKX_Full_BIT,
+ phydev->supported,
+ val & MDIO_PMA_EXTABLE_1000BKX);
+
+ linkmode_mod_bit(ETHTOOL_LINK_MODE_100baseT_Full_BIT,
+ phydev->supported,
+ val & MDIO_PMA_EXTABLE_100BTX);
+ linkmode_mod_bit(ETHTOOL_LINK_MODE_100baseT_Half_BIT,
+ phydev->supported,
+ val & MDIO_PMA_EXTABLE_100BTX);
+
+ linkmode_mod_bit(ETHTOOL_LINK_MODE_10baseT_Full_BIT,
+ phydev->supported,
+ val & MDIO_PMA_EXTABLE_10BT);
+ linkmode_mod_bit(ETHTOOL_LINK_MODE_10baseT_Half_BIT,
+ phydev->supported,
+ val & MDIO_PMA_EXTABLE_10BT);
+
+ if (val & MDIO_PMA_EXTABLE_NBT) {
+ val = phy_read_mmd(phydev, MDIO_MMD_PMAPMD,
+ MDIO_PMA_NG_EXTABLE);
+ if (val < 0)
+ return val;
+
+ linkmode_mod_bit(ETHTOOL_LINK_MODE_2500baseT_Full_BIT,
+ phydev->supported,
+ val & MDIO_PMA_NG_EXTABLE_2_5GBT);
+
+ linkmode_mod_bit(ETHTOOL_LINK_MODE_5000baseT_Full_BIT,
+ phydev->supported,
+ val & MDIO_PMA_NG_EXTABLE_5GBT);
+ }
+ }
+
+ return 0;
+}
+EXPORT_SYMBOL_GPL(genphy_c45_pma_read_abilities);
+
+/**
+ * genphy_c45_read_status - read PHY status
+ * @phydev: target phy_device struct
+ *
+ * Reads status from PHY and sets phy_device members accordingly.
+ */
+int genphy_c45_read_status(struct phy_device *phydev)
+{
+ int ret;
+
+ ret = genphy_c45_read_link(phydev);
+ if (ret)
+ return ret;
+
+ phydev->speed = SPEED_UNKNOWN;
+ phydev->duplex = DUPLEX_UNKNOWN;
+ phydev->pause = 0;
+ phydev->asym_pause = 0;
+
+ if (phydev->autoneg == AUTONEG_ENABLE) {
+ ret = genphy_c45_read_lpa(phydev);
+ if (ret)
+ return ret;
+
+ phy_resolve_aneg_linkmode(phydev);
+ } else {
+ ret = genphy_c45_read_pma(phydev);
+ }
+
+ return ret;
+}
+EXPORT_SYMBOL_GPL(genphy_c45_read_status);
+
+/**
+ * genphy_c45_config_aneg - restart auto-negotiation or forced setup
+ * @phydev: target phy_device struct
+ *
+ * Description: If auto-negotiation is enabled, we configure the
+ * advertising, and then restart auto-negotiation. If it is not
+ * enabled, then we force a configuration.
+ */
+int genphy_c45_config_aneg(struct phy_device *phydev)
+{
+ bool changed = false;
+ int ret;
+
+ if (phydev->autoneg == AUTONEG_DISABLE)
+ return genphy_c45_pma_setup_forced(phydev);
+
+ ret = genphy_c45_an_config_aneg(phydev);
+ if (ret < 0)
+ return ret;
+ if (ret > 0)
+ changed = true;
+
+ return genphy_c45_check_and_restart_aneg(phydev, changed);
+}
+EXPORT_SYMBOL_GPL(genphy_c45_config_aneg);
+
/* The gen10g_* functions are the old Clause 45 stub */
int gen10g_config_aneg(struct phy_device *phydev)
@@ -280,64 +560,9 @@
}
EXPORT_SYMBOL_GPL(gen10g_config_aneg);
-int gen10g_read_status(struct phy_device *phydev)
-{
- u32 mmd_mask = phydev->c45_ids.devices_in_package;
- int ret;
-
- /* For now just lie and say it's 10G all the time */
- phydev->speed = SPEED_10000;
- phydev->duplex = DUPLEX_FULL;
-
- /* Avoid reading the vendor MMDs */
- mmd_mask &= ~(BIT(MDIO_MMD_VEND1) | BIT(MDIO_MMD_VEND2));
-
- ret = genphy_c45_read_link(phydev, mmd_mask);
-
- phydev->link = ret > 0 ? 1 : 0;
-
- return 0;
-}
-EXPORT_SYMBOL_GPL(gen10g_read_status);
-
-int gen10g_no_soft_reset(struct phy_device *phydev)
-{
- /* Do nothing for now */
- return 0;
-}
-EXPORT_SYMBOL_GPL(gen10g_no_soft_reset);
-
-int gen10g_config_init(struct phy_device *phydev)
-{
- /* Temporarily just say we support everything */
- phydev->supported = SUPPORTED_10000baseT_Full;
- phydev->advertising = SUPPORTED_10000baseT_Full;
-
- return 0;
-}
-EXPORT_SYMBOL_GPL(gen10g_config_init);
-
-int gen10g_suspend(struct phy_device *phydev)
-{
- return 0;
-}
-EXPORT_SYMBOL_GPL(gen10g_suspend);
-
-int gen10g_resume(struct phy_device *phydev)
-{
- return 0;
-}
-EXPORT_SYMBOL_GPL(gen10g_resume);
-
-struct phy_driver genphy_10g_driver = {
+struct phy_driver genphy_c45_driver = {
.phy_id = 0xffffffff,
.phy_id_mask = 0xffffffff,
- .name = "Generic 10G PHY",
- .soft_reset = gen10g_no_soft_reset,
- .config_init = gen10g_config_init,
- .features = 0,
- .config_aneg = gen10g_config_aneg,
- .read_status = gen10g_read_status,
- .suspend = gen10g_suspend,
- .resume = gen10g_resume,
+ .name = "Generic Clause 45 PHY",
+ .read_status = genphy_c45_read_status,
};
--
Gitblit v1.6.2