[PATCH net-next 2/6] net: phylink: Allow more interfaces in SFP interface selection

[Maxime Chevallier <maxime.chevallier@bootlin.com>]

When phylink handles an SFP module that contains a PHY, it selects a
phy_interface to use to communicate with it. This selection ensures that
the highest speed gets achieved, based on the linkmodes we want to
support in the module.

This approach doesn't take into account the supported interfaces
reported by the module, but also doesn't handle the fact that the same
linkmode may be achieved using different phy_interface modes.

It becomes an issue when trying to support SGMII to 100FX modules. We
can't feed it 100BaseX, and its MDI isn't 1000BaseT (the modes we expect
for SGMII to be selected).

Let's therefore use a different approach :

 - Get the common interfaces between what the module and the SFP Bus
   support
 - From that common interface list, select the one that can allow us to
   achieve the highest speed

Signed-off-by: Maxime Chevallier <maxime.chevallier@bootlin.com>
---
 drivers/net/phy/phy-caps.h |  5 ++++
 drivers/net/phy/phy_caps.c | 47 ++++++++++++++++++++++++++++++++++++++
 drivers/net/phy/phylink.c  | 24 ++++++-------------
 3 files changed, 59 insertions(+), 17 deletions(-)

diff --git a/drivers/net/phy/phy-caps.h b/drivers/net/phy/phy-caps.h
index 5f3f757e0b2f..4a07ac74ef13 100644
--- a/drivers/net/phy/phy-caps.h
+++ b/drivers/net/phy/phy-caps.h
@@ -10,6 +10,7 @@
 #include <linux/ethtool.h>
 #include <linux/phy.h>
 
+/* this must be sorted by speed */
 enum {
 	LINK_CAPA_10HD = 0,
 	LINK_CAPA_10FD,
@@ -66,4 +67,8 @@ void phy_caps_medium_get_supported(unsigned long *supported,
 				   int lanes);
 u32 phy_caps_mediums_from_linkmodes(unsigned long *linkmodes);
 
+phy_interface_t
+phy_caps_select_fastest_interface(const unsigned long *interfaces,
+				  const unsigned long *linkmodes);
+
 #endif /* __PHY_CAPS_H */
diff --git a/drivers/net/phy/phy_caps.c b/drivers/net/phy/phy_caps.c
index 17a63c931335..11e7a1efcf30 100644
--- a/drivers/net/phy/phy_caps.c
+++ b/drivers/net/phy/phy_caps.c
@@ -443,3 +443,50 @@ u32 phy_caps_mediums_from_linkmodes(unsigned long *linkmodes)
 	return mediums;
 }
 EXPORT_SYMBOL_GPL(phy_caps_mediums_from_linkmodes);
+
+/**
+ * phy_caps_select_fastest_interface - Select the fastest interface that can
+ *				       support the fastest of the given
+ *				       linkmodes
+ * @interfaces: The interface list to lookup from
+ * @linkmodes: Linkmodes we want to support
+ *
+ * Returns: The fastest matching interface, PHY_INTERFACE_MODE_NA otherwise.
+ */
+phy_interface_t
+phy_caps_select_fastest_interface(const unsigned long *interfaces,
+				  const unsigned long *linkmodes)
+{
+	phy_interface_t interface = PHY_INTERFACE_MODE_NA;
+	u32 target_link_caps = 0;
+	int i, max_capa = 0;
+
+	/* Link caps from the linkmodes */
+	for_each_set_bit(i, linkmodes, __ETHTOOL_LINK_MODE_MASK_NBITS) {
+		const struct link_mode_info *linkmode;
+
+		linkmode = &link_mode_params[i];
+		target_link_caps |= speed_duplex_to_capa(linkmode->speed,
+							 linkmode->duplex);
+	}
+
+	for_each_set_bit(i, interfaces, PHY_INTERFACE_MODE_MAX) {
+		u32 interface_caps = phy_caps_from_interface(i);
+		u32 interface_max_capa;
+
+		/* Can we achieve at least one mode with this interface ? */
+		if (!(interface_caps & target_link_caps))
+			continue;
+
+		/* Biggest link_capa we can achieve with this interface */
+		interface_max_capa = fls(interface_caps & target_link_caps);
+
+		if (interface_max_capa > max_capa) {
+			max_capa = interface_max_capa;
+			interface = i;
+		}
+	}
+
+	return interface;
+}
+EXPORT_SYMBOL_GPL(phy_caps_select_fastest_interface);
diff --git a/drivers/net/phy/phylink.c b/drivers/net/phy/phylink.c
index 43d8380aaefb..18fa417b87dd 100644
--- a/drivers/net/phy/phylink.c
+++ b/drivers/net/phy/phylink.c
@@ -2808,27 +2808,19 @@ EXPORT_SYMBOL_GPL(phylink_ethtool_set_wol);
 static phy_interface_t phylink_sfp_select_interface(struct phylink *pl,
 						const unsigned long *link_modes)
 {
-	phy_interface_t interface;
+	DECLARE_PHY_INTERFACE_MASK(common_interfaces);
 
-	interface = sfp_select_interface(pl->sfp_bus, link_modes);
-	if (interface == PHY_INTERFACE_MODE_NA) {
-		phylink_err(pl,
-			    "selection of interface failed, advertisement %*pb\n",
-			    __ETHTOOL_LINK_MODE_MASK_NBITS,
-			    link_modes);
-		return interface;
-	}
+	/* Interfaces supported both by the module and the bus */
+	phy_interface_and(common_interfaces, pl->sfp_interfaces,
+			  pl->config->supported_interfaces);
 
-	if (!test_bit(interface, pl->config->supported_interfaces)) {
+	if (phy_interface_empty(common_interfaces)) {
 		phylink_err(pl,
-			    "selection of interface failed, SFP selected %s (%u) but MAC supports %*pbl\n",
-			    phy_modes(interface), interface,
-			    (int)PHY_INTERFACE_MODE_MAX,
-			    pl->config->supported_interfaces);
+			    "selection of interface failed, no common interface between MAC and SFP\n");
 		return PHY_INTERFACE_MODE_NA;
 	}
 
-	return interface;
+	return phy_caps_select_fastest_interface(common_interfaces, link_modes);
 }
 
 static phy_interface_t phylink_sfp_select_interface_speed(struct phylink *pl,
@@ -3697,8 +3689,6 @@ static int phylink_sfp_config_phy(struct phylink *pl, struct phy_device *phy)
 	struct phylink_link_state config;
 	int ret;
 
-	/* We're not using pl->sfp_interfaces, so clear it. */
-	phy_interface_zero(pl->sfp_interfaces);
 	linkmode_copy(support, phy->supported);
 
 	memset(&config, 0, sizeof(config));
-- 
2.49.0