net: phylink: use phy_interface_t bitmaps for optical modules

Where a MAC provides a phy_interface_t bitmap, use these bitmaps to
select the operating interface mode for optical SFP modules, rather
than using the linkmode bitmaps.

Signed-off-by: Russell King <rmk+kernel@armlinux.org.uk>

1 files changed, 108 insertions, 7 deletions

diff --git a/drivers/net/phy/phylink.c b/drivers/net/phy/phylink.c
index f9bfa928f270..76d8cccd8276 100644
--- a/drivers/net/phy/phylink.c
+++ b/drivers/net/phy/phylink.c
@@ -2178,6 +2178,41 @@ static void phylink_sfp_detach(void *upstream, struct sfp_bus *bus)
 	pl->netdev->sfp_bus = NULL;
 }
 
+static const phy_interface_t phylink_sfp_interface_preference[] = {
+	PHY_INTERFACE_MODE_USXGMII,
+	PHY_INTERFACE_MODE_10GBASER,
+	PHY_INTERFACE_MODE_10GKR,
+	PHY_INTERFACE_MODE_2500BASEX,
+	PHY_INTERFACE_MODE_SGMII,
+	PHY_INTERFACE_MODE_1000BASEX,
+};
+
+static phy_interface_t phylink_select_interface(struct phylink *pl,
+						const unsigned long *intf,
+						const char *intf_name)
+{
+	DECLARE_PHY_INTERFACE_MASK(u);
+	phy_interface_t interface;
+	size_t i;
+
+	phy_interface_and(u, intf, pl->config->supported_interfaces);
+
+	interface = PHY_INTERFACE_MODE_NA;
+	for (i = 0; i < ARRAY_SIZE(phylink_sfp_interface_preference); i++)
+		if (test_bit(phylink_sfp_interface_preference[i], u)) {
+			interface = phylink_sfp_interface_preference[i];
+			break;
+		}
+
+	phylink_dbg(pl, "interfaces=[mac=%*pbl %s=%*pbl] selected %d (%s)\n",
+		    (int)PHY_INTERFACE_MODE_MAX,
+		    pl->config->supported_interfaces,
+		    intf_name, (int)PHY_INTERFACE_MODE_MAX, intf,
+		    interface, phy_modes(interface));
+
+	return interface;
+}
+
 static int phylink_sfp_config(struct phylink *pl, u8 mode,
 			      const unsigned long *supported,
 			      const unsigned long *advertising)
@@ -2262,25 +2297,92 @@ static int phylink_sfp_config(struct phylink *pl, u8 mode,
 	return ret;
 }
 
+static int phylink_sfp_config_nophy(struct phylink *pl)
+{
+	__ETHTOOL_DECLARE_LINK_MODE_MASK(support);
+	struct phylink_link_state config;
+	phy_interface_t interface;
+	bool changed;
+	int ret;
+
+	if (phy_interface_empty(pl->config->supported_interfaces))
+		return phylink_sfp_config(pl, MLO_AN_INBAND,
+					  pl->sfp_support, pl->sfp_support);
+
+	interface = phylink_select_interface(pl, pl->sfp_interfaces, "sfp");
+	if (interface == PHY_INTERFACE_MODE_NA)
+		return -EINVAL;
+
+	linkmode_copy(support, pl->sfp_support);
+
+	memset(&config, 0, sizeof(config));
+	linkmode_copy(config.advertising, pl->sfp_support);
+	config.interface = interface;
+	config.speed = SPEED_UNKNOWN;
+	config.duplex = DUPLEX_UNKNOWN;
+	config.pause = MLO_PAUSE_AN;
+	config.an_enabled = true;
+
+	/* Ignore errors if we're expecting a PHY to attach later */
+	ret = phylink_validate(pl, support, &config);
+	if (ret) {
+		phylink_err(pl, "validation with support %*pb failed: %d\n",
+			    __ETHTOOL_LINK_MODE_MASK_NBITS, support, ret);
+		return ret;
+	}
+
+	phylink_dbg(pl, "requesting link mode %s/%s with support %*pb\n",
+		    phylink_an_mode_str(pl->sfp_link_an_mode),
+		    phy_modes(config.interface),
+		    __ETHTOOL_LINK_MODE_MASK_NBITS, support);
+
+	changed = !linkmode_equal(pl->supported, support) ||
+		  !linkmode_equal(pl->link_config.advertising,
+				  config.advertising);
+	if (changed) {
+		linkmode_copy(pl->supported, support);
+		linkmode_copy(pl->link_config.advertising, config.advertising);
+	}
+
+	if (pl->cur_link_an_mode != pl->sfp_link_an_mode ||
+	    pl->link_config.interface != config.interface) {
+		pl->link_config.interface = config.interface;
+		pl->cur_link_an_mode = pl->sfp_link_an_mode;
+
+		changed = true;
+
+		phylink_info(pl, "switched to %s/%s link mode\n",
+			     phylink_an_mode_str(pl->sfp_link_an_mode),
+			     phy_modes(config.interface));
+	}
+
+	pl->link_port = pl->sfp_port;
+
+	if (changed && !test_bit(PHYLINK_DISABLE_STOPPED,
+				 &pl->phylink_disable_state))
+		phylink_mac_initial_config(pl, false);
+
+	return 0;
+}
+
 static int phylink_sfp_module_insert(void *upstream,
 				     const struct sfp_eeprom_id *id)
 {
 	struct phylink *pl = upstream;
-	unsigned long *support = pl->sfp_support;
 
 	ASSERT_RTNL();
 
-	linkmode_zero(support);
+	linkmode_zero(pl->sfp_support);
 	phy_interface_zero(pl->sfp_interfaces);
-	sfp_parse_support(pl->sfp_bus, id, support, pl->sfp_interfaces);
-	pl->sfp_port = sfp_parse_port(pl->sfp_bus, id, support);
+	sfp_parse_support(pl->sfp_bus, id, pl->sfp_support, pl->sfp_interfaces);
+	pl->sfp_port = sfp_parse_port(pl->sfp_bus, id, pl->sfp_support);
 
 	/* If this module may have a PHY connecting later, defer until later */
 	pl->sfp_may_have_phy = sfp_may_have_phy(pl->sfp_bus, id);
 	if (pl->sfp_may_have_phy)
 		return 0;
 
-	return phylink_sfp_config(pl, MLO_AN_INBAND, support, support);
+	return phylink_sfp_config_nophy(pl);
 }
 
 static int phylink_sfp_module_start(void *upstream)
@@ -2299,8 +2401,7 @@ static int phylink_sfp_module_start(void *upstream)
 	if (!pl->sfp_may_have_phy)
 		return 0;
 
-	return phylink_sfp_config(pl, MLO_AN_INBAND,
-				  pl->sfp_support, pl->sfp_support);
+	return phylink_sfp_config_nophy(pl);
 }
 
 static void phylink_sfp_module_stop(void *upstream)