diff --git a/drivers/net/phy/phylink.c b/drivers/net/phy/phylink.c
index 2af665790564..6dcd0d1f960a 100644
--- a/drivers/net/phy/phylink.c
+++ b/drivers/net/phy/phylink.c
@@ -61,6 +61,8 @@ struct phylink {
u8 link_port; /* The current non-phy ethtool port */
__ETHTOOL_DECLARE_LINK_MODE_MASK(supported);
__ETHTOOL_DECLARE_LINK_MODE_MASK(supported_lpi);
+ __ETHTOOL_DECLARE_LINK_MODE_MASK(advertising);
+ __ETHTOOL_DECLARE_LINK_MODE_MASK(user_disabled_advertising);
/* The link configuration settings */
struct phylink_link_state link_config;@@ -717,6 +719,41 @@ static int phylink_validate(struct phylink *pl, unsigned long *supported,
return ret;
}
+static void phylink_update_capabilities(struct phylink *pl,
+ struct phylink_link_state *config)
+{
+ lockdep_assert_held(&pl->state_mutex);
+
+ /* Calculate the full supported advertisement */
+ linkmode_copy(config->advertising, pl->supported);
+ __phylink_validate(pl, pl->supported, config);
+
+ /* Save the newly calculated full advertisement */
+ linkmode_copy(pl->advertising, config->advertising);
+
+ /* Keep user-disabled modes out of the active advertisement. */
+ linkmode_andnot(config->advertising,
+ pl->advertising, pl->user_disabled_advertising);
+}
+
+static void phylink_update_user_disabled_advertising(struct phylink *pl,
+ const unsigned long *advertising)
+{
+ __ETHTOOL_DECLARE_LINK_MODE_MASK(disabled);
+
+ /* Preserve restrictions for modes absent from the current full mask. */
+ linkmode_andnot(disabled, pl->user_disabled_advertising,
+ pl->advertising);
+
+ /* Update restrictions for modes in the current full advertisement. */
+ linkmode_andnot(pl->user_disabled_advertising, pl->advertising,
+ advertising);
+
+ /* Combine the updated restrictions with those preserved above. */
+ linkmode_or(pl->user_disabled_advertising,
+ pl->user_disabled_advertising, disabled);
+}
+
static void phylink_fill_fixedlink_supported(unsigned long *supported)
{
linkmode_set_bit(ETHTOOL_LINK_MODE_Pause_BIT, supported);@@ -815,6 +852,7 @@ static int phylink_parse_fixedlink(struct phylink *pl,
linkmode_copy(pl->link_config.advertising, pl->supported);
phylink_validate(pl, pl->supported, &pl->link_config);
+ linkmode_copy(pl->advertising, pl->link_config.advertising);
c = phy_caps_lookup(pl->link_config.speed, pl->link_config.duplex,
pl->supported, true);
@@ -840,6 +878,7 @@ static int phylink_parse_fixedlink(struct phylink *pl,
linkmode_and(pl->link_config.advertising, pl->link_config.advertising,
pl->supported);
+ linkmode_copy(pl->advertising, pl->link_config.advertising);
pl->link_config.link = 1;
pl->link_config.an_complete = 1;
@@ -922,6 +961,8 @@ static int phylink_parse_mode(struct phylink *pl,
"failed to validate link configuration for in-band status\n");
return -EINVAL;
}
+
+ linkmode_copy(pl->advertising, pl->link_config.advertising);
}
return 0;
@@ -2152,7 +2193,7 @@ void phylink_update_mac_pause_capabilities(struct phylink *pl, unsigned long mac
else if (pl->sfp_bus)
linkmode_and(pl->supported, pl->supported, pl->sfp_support);
- __phylink_validate(pl, pl->supported, config);
+ phylink_update_capabilities(pl, config);
pause_state = config->pause;
@@ -2252,6 +2293,8 @@ struct phylink *phylink_create(struct phylink_config *config,
pl->link_config.pause = MLO_PAUSE_AN;
pl->link_config.speed = SPEED_UNKNOWN;
pl->link_config.duplex = DUPLEX_UNKNOWN;
+ linkmode_fill(pl->advertising);
+ linkmode_fill(pl->link_config.advertising);
pl->pcs_state = PCS_STATE_DOWN;
pl->mac_ops = mac_ops;
__set_bit(PHYLINK_DISABLE_STOPPED, &pl->phylink_disable_state);
@@ -2275,9 +2318,10 @@ struct phylink *phylink_create(struct phylink_config *config,
pl->supported_interfaces,
pcs->supported_interfaces);
+ mutex_lock(&pl->state_mutex);
linkmode_fill(pl->supported);
- linkmode_copy(pl->link_config.advertising, pl->supported);
- phylink_validate(pl, pl->supported, &pl->link_config);
+ phylink_update_capabilities(pl, &pl->link_config);
+ mutex_unlock(&pl->state_mutex);
ret = phylink_parse_mode(pl, fwnode);
if (ret < 0)
@@ -2534,6 +2578,7 @@ static int phylink_bringup_phy(struct phylink *pl, struct phy_device *phy,
pl->phy_state.rate_matching = RATE_MATCH_NONE;
linkmode_copy(pl->supported, supported);
linkmode_copy(pl->link_config.advertising, config.advertising);
+ linkmode_copy(pl->advertising, pl->link_config.advertising);
/* Restrict the phy advertisement according to the MAC support. */
linkmode_copy(phy->advertising, config.advertising);
@@ -3396,6 +3441,7 @@ static bool phylink_validate_pcs_inband_autoneg(struct phylink *pl,
int phylink_ethtool_ksettings_set(struct phylink *pl,
const struct ethtool_link_ksettings *kset)
{
+ __ETHTOOL_DECLARE_LINK_MODE_MASK(requested_advertising);
__ETHTOOL_DECLARE_LINK_MODE_MASK(support);
const struct link_capabilities *c;
struct phylink_link_state config;@@ -3485,6 +3531,7 @@ int phylink_ethtool_ksettings_set(struct phylink *pl,
*/
linkmode_mod_bit(ETHTOOL_LINK_MODE_Autoneg_BIT, config.advertising,
kset->base.autoneg == AUTONEG_ENABLE);
+ linkmode_copy(requested_advertising, config.advertising);
/* If this link is with an SFP, ensure that changes to advertised modes
* also cause the associated interface to be selected such that the
@@ -3532,6 +3579,7 @@ int phylink_ethtool_ksettings_set(struct phylink *pl,
return -EINVAL;
mutex_lock(&pl->state_mutex);
+ phylink_update_user_disabled_advertising(pl, requested_advertising);
pl->link_config.speed = config.speed;
pl->link_config.duplex = config.duplex;
@@ -4015,19 +4063,34 @@ static void phylink_sfp_detach(void *upstream, struct sfp_bus *bus)
}
static phy_interface_t phylink_choose_sfp_interface(struct phylink *pl,
- const unsigned long *intf)
+ const unsigned long *intf,
+ const unsigned long *supported,
+ const struct phylink_link_state *state)
{
+ __ETHTOOL_DECLARE_LINK_MODE_MASK(tmp_supported);
+ struct phylink_link_state tmp_state;
phy_interface_t interface;
size_t i;
- interface = PHY_INTERFACE_MODE_NA;
- for (i = 0; i < ARRAY_SIZE(phylink_sfp_interface_preference); i++)
- if (test_bit(phylink_sfp_interface_preference[i], intf)) {
- interface = phylink_sfp_interface_preference[i];
- break;
- }
+ for (i = 0; i < ARRAY_SIZE(phylink_sfp_interface_preference); i++) {
+ interface = phylink_sfp_interface_preference[i];
+ if (!test_bit(interface, intf))
+ continue;
- return interface;
+ tmp_state = *state;
+ tmp_state.interface = interface;
+ linkmode_copy(tmp_supported, supported);
+ if (phylink_validate(pl, tmp_supported, &tmp_state))
+ continue;
+
+ /* Skip this preferred interface if user-disabled in advertising */
+ linkmode_andnot(tmp_state.advertising, tmp_state.advertising,
+ pl->user_disabled_advertising);
+ if (!phylink_is_empty_linkmode(tmp_state.advertising))
+ return interface;
+ }
+
+ return PHY_INTERFACE_MODE_NA;
}
static void phylink_sfp_set_config(struct phylink *pl, unsigned long *supported,@@ -4045,6 +4108,11 @@ static void phylink_sfp_set_config(struct phylink *pl, unsigned long *supported,
changed = true;
}
+ linkmode_copy(pl->advertising, state->advertising);
+ if (!pl->phydev)
+ linkmode_andnot(state->advertising, pl->advertising,
+ pl->user_disabled_advertising);
+
if (!linkmode_equal(pl->link_config.advertising, state->advertising)) {
linkmode_copy(pl->link_config.advertising, state->advertising);
changed = true;@@ -4160,7 +4228,8 @@ static int phylink_sfp_config_optical(struct phylink *pl)
return ret;
}
- interface = phylink_choose_sfp_interface(pl, pl->sfp_interfaces);
+ interface = phylink_choose_sfp_interface(pl, pl->sfp_interfaces,
+ pl->sfp_support, &config);
if (interface == PHY_INTERFACE_MODE_NA) {
phylink_err(pl, "failed to select SFP interface\n");
return -EINVAL;--
2.55.0