[PATCH RFC net-next 4/5] net: phylink: add phylink_set_max_fixed_link()

[Russell King (Oracle)]

Add a function for DSA to use to configure phylink, in the absence of
any other configuration, to a fixed link operating at the maximum
supported link speed.

This is needed so we can support phylink usage on CPU and DSA ports.

We use the default interface that the DSA driver provides (if any)
otherwise we attempt to find the first supported interface that gives
the maximum speed for the link.

Signed-off-by: Russell King (Oracle) <rmk+kernel at armlinux.org.uk>
---
 drivers/net/phy/phylink.c | 119 ++++++++++++++++++++++++++++++++++++++
 include/linux/phylink.h   |   5 ++
 2 files changed, 124 insertions(+)

diff --git a/drivers/net/phy/phylink.c b/drivers/net/phy/phylink.c
index 2069fc902e19..7ed3b2c3a359 100644
--- a/drivers/net/phy/phylink.c
+++ b/drivers/net/phy/phylink.c
@@ -1333,6 +1333,125 @@ void phylink_destroy(struct phylink *pl)
 }
 EXPORT_SYMBOL_GPL(phylink_destroy);
 
+static struct {
+	unsigned long fd_mask;
+	unsigned long hd_mask;
+	int speed;
+} phylink_caps_speeds[] = {
+	{ MAC_400000FD, 0,          SPEED_400000 },
+	{ MAC_200000FD, 0,          SPEED_200000 },
+	{ MAC_100000FD, 0,          SPEED_100000 },
+	{ MAC_56000FD,  0,          SPEED_56000  },
+	{ MAC_50000FD,  0,          SPEED_50000  },
+	{ MAC_40000FD,  0,          SPEED_40000  },
+	{ MAC_25000FD,  0,          SPEED_40000  },
+	{ MAC_20000FD,  0,          SPEED_20000  },
+	{ MAC_10000FD,  0,          SPEED_10000  },
+	{ MAC_5000FD,   0,          SPEED_5000   },
+	{ MAC_2500FD,   0,          SPEED_2500   },
+	{ MAC_1000FD,   MAC_1000HD, SPEED_1000   },
+	{ MAC_100FD,    MAC_100HD,  SPEED_100    },
+	{ MAC_10FD,     MAC_10HD,   SPEED_10     },
+};
+
+/**
+ * phylink_set_max_fixed_link() - set a fixed link configuration for phylink
+ * @pl: a pointer to a &struct phylink returned from phylink_create()
+ *
+ * Set a maximum speed fixed-link configuration for the chosen interface
+ * mode and MAC capabilities for the phylink instance. If the interface mode
+ * is PHY_INTERFACE_MODE_NA, then search the supported interfaces bitmap for
+ * the first interface that gives the fastest supported speed.
+ *
+ * This is only valid for use immediately after phylink_create(). Must not
+ * be used at any other time.
+ *
+ * The user must have initialised mac_capabilities and set a valid interface.
+ */
+int phylink_set_max_fixed_link(struct phylink *pl)
+{
+	phy_interface_t intf, interface;
+	unsigned long caps, max_caps;
+	unsigned long *interfaces;
+	int speed, duplex;
+	int i;
+
+	interface = pl->link_interface;
+
+	phylink_dbg(pl, "sif=%*pbl if=%d(%s) cap=%lx\n",
+		    (int)PHY_INTERFACE_MODE_MAX,
+		    pl->config->supported_interfaces,
+		    interface, phy_modes(interface),
+		    pl->config->mac_capabilities);
+
+	/* If we are not in PHY mode, or have a PHY, or have a SFP bus,
+	 * then we must not default to a fixed link.
+	 */
+	if (pl->cfg_link_an_mode != MLO_AN_PHY || pl->phydev || pl->sfp_bus)
+		return -EBUSY;
+
+	if (interface != PHY_INTERFACE_MODE_NA) {
+		/* Get the speed/duplex capabilities and reduce according to the
+		 * specified interface mode.
+		 */
+		caps = pl->config->mac_capabilities;
+		caps &= phylink_interface_to_caps(interface);
+	} else {
+		interfaces = pl->config->supported_interfaces;
+		max_caps = 0;
+
+		/* Find the supported interface mode which gives the maximum
+		 * speed.
+		 */
+		for_each_set_bit(intf, interfaces, PHY_INTERFACE_MODE_MAX) {
+			caps = pl->config->mac_capabilities;
+			caps &= phylink_interface_to_caps(intf);
+			if (caps > max_caps) {
+				max_caps = caps;
+				interface = intf;
+			}
+		}
+
+		caps = max_caps;
+	}
+
+	caps &= ~(MAC_SYM_PAUSE | MAC_ASYM_PAUSE);
+
+	/* If there are no capabilities, then we are not using this default. */
+	if (!caps)
+		return -EINVAL;
+
+	/* Decode to fastest speed and duplex */
+	duplex = DUPLEX_UNKNOWN;
+	speed = SPEED_UNKNOWN;
+	for (i = 0; i < ARRAY_SIZE(phylink_caps_speeds); i++) {
+		if (caps & phylink_caps_speeds[i].fd_mask) {
+			duplex = DUPLEX_FULL;
+			speed = phylink_caps_speeds[i].speed;
+			break;
+		} else if (caps & phylink_caps_speeds[i].hd_mask) {
+			duplex = DUPLEX_HALF;
+			speed = phylink_caps_speeds[i].speed;
+			break;
+		}
+	}
+
+	/* If we didn't find anything, bail. */
+	if (speed == SPEED_UNKNOWN)
+		return -EINVAL;
+
+	pl->link_interface = interface;
+	pl->link_config.interface = interface;
+	pl->link_config.speed = speed;
+	pl->link_config.duplex = duplex;
+	pl->link_config.link = 1;
+	pl->cfg_link_an_mode = MLO_AN_FIXED;
+	pl->cur_link_an_mode = MLO_AN_FIXED;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(phylink_set_max_fixed_link);
+
 static void phylink_phy_change(struct phy_device *phydev, bool up)
 {
 	struct phylink *pl = phydev->phylink;
diff --git a/include/linux/phylink.h b/include/linux/phylink.h
index 6d06896fc20d..9e2fb476d19c 100644
--- a/include/linux/phylink.h
+++ b/include/linux/phylink.h
@@ -23,6 +23,9 @@ enum {
 
 	MAC_SYM_PAUSE	= BIT(0),
 	MAC_ASYM_PAUSE	= BIT(1),
+	/* These speed bits must be sorted according to speed for
+	 * phylink_set_max_fixed_link()
+	 */
 	MAC_10HD	= BIT(2),
 	MAC_10FD	= BIT(3),
 	MAC_10		= MAC_10HD | MAC_10FD,
@@ -529,6 +532,8 @@ struct phylink *phylink_create(struct phylink_config *, struct fwnode_handle *,
 			       const struct phylink_mac_ops *mac_ops);
 void phylink_destroy(struct phylink *);
 
+int phylink_set_max_fixed_link(struct phylink *pl);
+
 int phylink_connect_phy(struct phylink *, struct phy_device *);
 int phylink_of_phy_connect(struct phylink *, struct device_node *, u32 flags);
 int phylink_fwnode_phy_connect(struct phylink *pl,
-- 
2.30.2