net: phylink: use phy interface mode bitmaps for SFP PHYs

Select the initial SFP PHY interface mode from the PHY supported
interface bitmaps.

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

1 files changed, 52 insertions, 2 deletions

diff --git a/drivers/net/phy/phylink.c b/drivers/net/phy/phylink.c
index c0251d440d70..afa491f72291 100644
--- a/drivers/net/phy/phylink.c
+++ b/drivers/net/phy/phylink.c
@@ -3820,7 +3820,10 @@ static void phylink_sfp_link_up(void *upstream)
 
 static int phylink_sfp_connect_phy(void *upstream, struct phy_device *phy)
 {
+	DECLARE_PHY_INTERFACE_MASK(interfaces);
 	struct phylink *pl = upstream;
+	phy_interface_t interface;
+	int ret;
 
 	if (!phy->drv) {
 		phylink_err(pl, "PHY %s (id 0x%.8lx) has no driver loaded\n",
@@ -3842,8 +3845,55 @@ static int phylink_sfp_connect_phy(void *upstream, struct phy_device *phy)
 	phy_interface_and(phy->host_interfaces, phylink_sfp_interfaces,
 			  pl->config->supported_interfaces);
 
-	/* Do the initial configuration */
-	return phylink_sfp_config_phy(pl, phy);
+	if (phy_interface_empty(phy->supported_interfaces)) {
+		phylink_dbg(pl, "copper SFP: PHY provides empty supported_interfaces\n");
+
+		/* Do the initial configuration */
+		return phylink_sfp_config_phy(pl, phy);
+	}
+
+	phylink_dbg(pl, "copper SFP: interfaces=[mac=%*pbl, sfp=%*pbl]\n",
+		    (int)PHY_INTERFACE_MODE_MAX,
+		    pl->config->supported_interfaces,
+		    (int)PHY_INTERFACE_MODE_MAX,
+		    phy->supported_interfaces);
+
+	phy_interface_and(interfaces, phy->supported_interfaces,
+			  pl->config->supported_interfaces);
+	interface = phylink_choose_sfp_interface(pl, interfaces);
+	if (interface == PHY_INTERFACE_MODE_NA) {
+		phylink_err(pl,
+			    "selection of interface for PHY failed\n");
+		return -EINVAL;
+	}
+
+	phylink_dbg(pl, "copper SFP: chosen %s interface\n",
+		    phy_modes(interface));
+
+	ret = phylink_attach_phy(pl, phy, interface);
+	if (ret < 0)
+		return ret;
+
+	ret = phylink_bringup_phy(pl, phy, interface);
+	if (ret) {
+		phy_detach(phy);
+		return ret;
+	}
+
+	if (pl->req_link_an_mode != MLO_AN_INBAND ||
+	    pl->link_config.interface != interface) {
+		pl->link_config.interface = interface;
+		pl->req_link_an_mode = MLO_AN_INBAND;
+
+		phylink_info(pl, "switched to %s/%s link mode\n",
+			     phylink_an_mode_str(pl->req_link_an_mode),
+			     phy_modes(pl->link_config.interface));
+	}
+
+	if (!test_bit(PHYLINK_DISABLE_STOPPED, &pl->phylink_disable_state))
+		phylink_mac_initial_config(pl, false);
+
+	return 0;
 }
 
 static void phylink_sfp_disconnect_phy(void *upstream,