[PATCH RFC net-next 4/6] 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.

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

diff --git a/drivers/net/phy/phylink.c b/drivers/net/phy/phylink.c
index 4645ac6e553d..23be3f041705 100644
--- a/drivers/net/phy/phylink.c
+++ b/drivers/net/phy/phylink.c
@@ -1333,6 +1333,116 @@ 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. Only valid for use
+ * immediately after creation. 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;
+
+	/* 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;
+
+	interface = pl->link_interface;
+	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 (intf = 0; intf < PHY_INTERFACE_MODE_MAX; intf++) {
+			if (test_bit(intf, interfaces)) {
+				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