Subject: [PATCH RFC net-next v2 5/6] net: dsa: create swnode fixed-link if using default params

From: "Russell King (Oracle)" <[email-masked]>
Date: Wed, 13 Jul 2022 15:08:03 +0100

Create and use a swnode fixed-link specification for phylink if no
parameters are given in DT for a fixed-link. This allows phylink to
be used for "default" cases for DSA and CPU ports.

Signed-off-by: Vladimir Oltean <[email-masked]>
Signed-off-by: Russell King (Oracle) <[email-masked]>
---
 net/dsa/port.c | 152 ++++++++++++++++++++++++++++++++++++++++++++-----
 1 file changed, 139 insertions(+), 13 deletions(-)

diff --git a/net/dsa/port.c b/net/dsa/port.c
index 35b4e1f8dc05..abcf7899abf8 100644
--- a/net/dsa/port.c
+++ b/net/dsa/port.c
@@ -1521,10 +1521,131 @@ static const struct phylink_mac_ops dsa_port_phylink_mac_ops = {
 	.mac_link_up = dsa_port_phylink_mac_link_up,
 };
 
+static struct {
+	unsigned long mask;
+	int speed;
+	int duplex;
+} phylink_caps_params[] = {
+	{ MAC_400000FD, SPEED_400000, DUPLEX_FULL },
+	{ MAC_200000FD, SPEED_200000, DUPLEX_FULL },
+	{ MAC_100000FD, SPEED_100000, DUPLEX_FULL },
+	{ MAC_56000FD,  SPEED_56000,  DUPLEX_FULL },
+	{ MAC_50000FD,  SPEED_50000,  DUPLEX_FULL },
+	{ MAC_40000FD,  SPEED_40000,  DUPLEX_FULL },
+	{ MAC_25000FD,  SPEED_25000,  DUPLEX_FULL },
+	{ MAC_20000FD,  SPEED_20000,  DUPLEX_FULL },
+	{ MAC_10000FD,  SPEED_10000,  DUPLEX_FULL },
+	{ MAC_5000FD,   SPEED_5000,   DUPLEX_FULL },
+	{ MAC_2500FD,   SPEED_2500,   DUPLEX_FULL },
+	{ MAC_1000FD,   SPEED_1000,   DUPLEX_FULL },
+	{ MAC_100FD,    SPEED_100,    DUPLEX_FULL },
+	{ MAC_10FD,     SPEED_10,     DUPLEX_FULL },
+	{ MAC_1000HD,   SPEED_1000,   DUPLEX_HALF },
+	{ MAC_100HD,    SPEED_100,    DUPLEX_HALF },
+	{ MAC_10HD,     SPEED_10,     DUPLEX_HALF },
+};
+
+static int dsa_port_find_max_speed(unsigned long caps, int *speed, int *duplex)
+{
+	int i;
+
+	*speed = SPEED_UNKNOWN;
+	*duplex = DUPLEX_UNKNOWN;
+
+	for (i = 0; i < ARRAY_SIZE(phylink_caps_params); i++) {
+		if (caps & phylink_caps_params[i].mask) {
+			*speed = phylink_caps_params[i].speed;
+			*duplex = phylink_caps_params[i].duplex;
+			break;
+		}
+	}
+
+	return *speed == SPEED_UNKNOWN ? -EINVAL : 0;
+}
+
+static void dsa_port_find_max_caps(struct dsa_port *dp,
+				   phy_interface_t *max_interface,
+				   unsigned long *max_caps)
+{
+	struct phylink_config *config = &dp->pl_config;
+	phy_interface_t interface;
+	unsigned long caps;
+
+	*max_interface = PHY_INTERFACE_MODE_NA;
+	*max_caps = 0;
+
+	for_each_set_bit(interface, config->supported_interfaces,
+			 PHY_INTERFACE_MODE_MAX) {
+		caps = config->mac_capabilities &
+		       phylink_interface_to_caps(interface);
+		if (caps > *max_caps) {
+			*max_caps = caps;
+			*max_interface = interface;
+		}
+	}
+}
+
+static struct fwnode_handle *dsa_port_get_fwnode(struct dsa_port *dp,
+						 phy_interface_t mode)
+{
+	struct property_entry fixed_link_props[3] = { };
+	struct property_entry port_props[3] = {};
+	struct fwnode_handle *fixed_link_fwnode;
+	struct fwnode_handle *new_port_fwnode;
+	struct device_node *dn = dp->dn;
+	struct device_node *phy_node;
+	int err, speed, duplex;
+	unsigned long caps;
+
+	phy_node = of_parse_phandle(dn, "phy-handle", 0);
+	of_node_put(phy_node);
+	if (phy_node || of_phy_is_fixed_link(dn))
+		/* Nothing broken, nothing to fix.
+		 * TODO: As discussed with Russell, maybe phylink could provide
+		 * a more comprehensive helper to determine what constitutes a
+		 * valid fwnode binding than this guerilla kludge.
+		 */
+		return of_fwnode_handle(dn);
+
+	if (mode == PHY_INTERFACE_MODE_NA)
+		dsa_port_find_max_caps(dp, &mode, &caps);
+	else
+		caps = dp->pl_config.mac_capabilities &
+		       phylink_interface_to_caps(mode);
+
+	err = dsa_port_find_max_speed(caps, &speed, &duplex);
+	if (err)
+		return ERR_PTR(err);
+
+	fixed_link_props[0] = PROPERTY_ENTRY_U32("speed", speed);
+	if (duplex == DUPLEX_FULL)
+		fixed_link_props[1] = PROPERTY_ENTRY_BOOL("full-duplex");
+
+	port_props[0] = PROPERTY_ENTRY_STRING("phy-mode", phy_modes(mode));
+
+	new_port_fwnode = fwnode_create_software_node(port_props, NULL);
+	if (IS_ERR(new_port_fwnode))
+		return new_port_fwnode;
+
+	/* Node needs to be named so that phylink's call to
+	 * fwnode_get_named_child_node() finds it.
+	 */
+	fixed_link_fwnode = fwnode_create_named_software_node(fixed_link_props,
+							      new_port_fwnode,
+							      "fixed-link");
+	if (IS_ERR(fixed_link_fwnode)) {
+		fwnode_remove_software_node(new_port_fwnode);
+		return fixed_link_fwnode;
+	}
+
+	return new_port_fwnode;
+}
+
 int dsa_port_phylink_create(struct dsa_port *dp)
 {
 	struct dsa_switch *ds = dp->ds;
 	phy_interface_t mode, def_mode;
+	struct fwnode_handle *fwnode;
 	int err;
 
 	/* Presence of phylink_mac_link_state or phylink_mac_an_restart is
@@ -1552,8 +1673,19 @@ int dsa_port_phylink_create(struct dsa_port *dp)
 			mode = PHY_INTERFACE_MODE_NA;
 	}
 
-	dp->pl = phylink_create(&dp->pl_config, of_fwnode_handle(dp->dn),
-				mode, &dsa_port_phylink_mac_ops);
+	fwnode = dsa_port_get_fwnode(dp, mode);
+	if (IS_ERR(fwnode)) {
+		dev_err(ds->dev,
+			"Failed to get fwnode for port %d: %pe\n",
+			dp->index, fwnode);
+		return PTR_ERR(fwnode);
+	}
+
+	dp->pl = phylink_create(&dp->pl_config, fwnode, mode,
+				&dsa_port_phylink_mac_ops);
+
+	fwnode_remove_software_node(fwnode);
+
 	if (IS_ERR(dp->pl)) {
 		pr_err("error creating PHYLINK: %ld\n", PTR_ERR(dp->pl));
 		return PTR_ERR(dp->pl);
@@ -1663,20 +1795,14 @@ static int dsa_port_phylink_register(struct dsa_port *dp)
 int dsa_port_link_register_of(struct dsa_port *dp)
 {
 	struct dsa_switch *ds = dp->ds;
-	struct device_node *phy_np;
 	int port = dp->index;
 
 	if (!ds->ops->adjust_link) {
-		phy_np = of_parse_phandle(dp->dn, "phy-handle", 0);
-		if (of_phy_is_fixed_link(dp->dn) || phy_np) {
-			if (ds->ops->phylink_mac_link_down)
-				ds->ops->phylink_mac_link_down(ds, port,
-					MLO_AN_FIXED, PHY_INTERFACE_MODE_NA);
-			of_node_put(phy_np);
-			return dsa_port_phylink_register(dp);
-		}
-		of_node_put(phy_np);
-		return 0;
+		if (ds->ops->phylink_mac_link_down)
+			ds->ops->phylink_mac_link_down(ds, port, MLO_AN_FIXED,
+						       PHY_INTERFACE_MODE_NA);
+
+		return dsa_port_phylink_register(dp);
 	}
 
 	dev_warn(ds->dev,
-- 
2.30.2