swconfig: revert the portmapping patches, they seem to cause a segfault

Revert "kernel/swconfig: remove obsolete portmapping feature from swconfig"

This reverts commit 675407baa44a8700de20b6b2857009a552a807ba.

Revert "swconfig: remove obsolete portmapping feature"

This reverts commit fca1eb349ef31b133a62880cbd562d6bf17500aa.

Signed-off-by: John Crispin <john@phrozen.org>

3 files changed, 92 insertions, 0 deletions

diff --git a/target/linux/generic/files/drivers/net/phy/swconfig.c b/target/linux/generic/files/drivers/net/phy/swconfig.c
index 8dfcb1aa3b..c70ca74cad 100644
--- a/target/linux/generic/files/drivers/net/phy/swconfig.c
+++ b/target/linux/generic/files/drivers/net/phy/swconfig.c
@@ -915,7 +915,9 @@ static int
 swconfig_send_switch(struct sk_buff *msg, u32 pid, u32 seq, int flags,
 		const struct switch_dev *dev)
 {
+	struct nlattr *p = NULL, *m = NULL;
 	void *hdr;
+	int i;
 
 	hdr = genlmsg_put(msg, pid, seq, &switch_fam, flags,
 			SWITCH_CMD_NEW_ATTR);
@@ -937,6 +939,24 @@ swconfig_send_switch(struct sk_buff *msg, u32 pid, u32 seq, int flags,
 	if (nla_put_u32(msg, SWITCH_ATTR_CPU_PORT, dev->cpu_port))
 		goto nla_put_failure;
 
+	m = nla_nest_start(msg, SWITCH_ATTR_PORTMAP);
+	if (!m)
+		goto nla_put_failure;
+	for (i = 0; i < dev->ports; i++) {
+		p = nla_nest_start(msg, SWITCH_ATTR_PORTS);
+		if (!p)
+			continue;
+		if (dev->portmap[i].s) {
+			if (nla_put_string(msg, SWITCH_PORTMAP_SEGMENT,
+						dev->portmap[i].s))
+				goto nla_put_failure;
+			if (nla_put_u32(msg, SWITCH_PORTMAP_VIRT,
+						dev->portmap[i].virt))
+				goto nla_put_failure;
+		}
+		nla_nest_end(msg, p);
+	}
+	nla_nest_end(msg, m);
 	genlmsg_end(msg, hdr);
 	return msg->len;
 nla_put_failure:
@@ -1029,6 +1049,51 @@ static struct genl_ops swconfig_ops[] = {
 	}
 };
 
+#ifdef CONFIG_OF
+void
+of_switch_load_portmap(struct switch_dev *dev)
+{
+	struct device_node *port;
+
+	if (!dev->of_node)
+		return;
+
+	for_each_child_of_node(dev->of_node, port) {
+		const __be32 *prop;
+		const char *segment;
+		int size, phys;
+
+		if (!of_device_is_compatible(port, "swconfig,port"))
+			continue;
+
+		if (of_property_read_string(port, "swconfig,segment", &segment))
+			continue;
+
+		prop = of_get_property(port, "swconfig,portmap", &size);
+		if (!prop)
+			continue;
+
+		if (size != (2 * sizeof(*prop))) {
+			pr_err("%s: failed to parse port mapping\n",
+					port->name);
+			continue;
+		}
+
+		phys = be32_to_cpup(prop++);
+		if ((phys < 0) | (phys >= dev->ports)) {
+			pr_err("%s: physical port index out of range\n",
+					port->name);
+			continue;
+		}
+
+		dev->portmap[phys].s = kstrdup(segment, GFP_KERNEL);
+		dev->portmap[phys].virt = be32_to_cpup(prop);
+		pr_debug("Found port: %s, physical: %d, virtual: %d\n",
+			segment, phys, dev->portmap[phys].virt);
+	}
+}
+#endif
+
 int
 register_switch(struct switch_dev *dev, struct net_device *netdev)
 {
@@ -1051,6 +1116,12 @@ register_switch(struct switch_dev *dev, struct net_device *netdev)
 				dev->ports, GFP_KERNEL);
 		if (!dev->portbuf)
 			return -ENOMEM;
+		dev->portmap = kzalloc(sizeof(struct switch_portmap) *
+				dev->ports, GFP_KERNEL);
+		if (!dev->portmap) {
+			kfree(dev->portbuf);
+			return -ENOMEM;
+		}
 	}
 	swconfig_defaults_init(dev);
 	mutex_init(&dev->sw_mutex);
@@ -1072,6 +1143,11 @@ register_switch(struct switch_dev *dev, struct net_device *netdev)
 		return -ENFILE;
 	}
 
+#ifdef CONFIG_OF
+	if (dev->ports)
+		of_switch_load_portmap(dev);
+#endif
+
 	/* fill device name */
 	snprintf(dev->devname, IFNAMSIZ, SWCONFIG_DEVNAME, i);
 
diff --git a/target/linux/generic/files/include/linux/switch.h b/target/linux/generic/files/include/linux/switch.h
index a381fd006c..dda4820a7a 100644
--- a/target/linux/generic/files/include/linux/switch.h
+++ b/target/linux/generic/files/include/linux/switch.h
@@ -105,6 +105,7 @@ struct switch_dev_ops {
 };
 
 struct switch_dev {
+	struct device_node *of_node;
 	const struct switch_dev_ops *ops;
 	/* will be automatically filled */
 	char devname[IFNAMSIZ];
@@ -125,6 +126,7 @@ struct switch_dev {
 
 	struct mutex sw_mutex;
 	struct switch_port *portbuf;
+	struct switch_portmap *portmap;
 	struct switch_port_link linkbuf;
 
 	char buf[128];
@@ -139,6 +141,11 @@ struct switch_port {
 	u32 flags;
 };
 
+struct switch_portmap {
+	u32 virt;
+	const char *s;
+};
+
 struct switch_val {
 	const struct switch_attr *attr;
 	int port_vlan;
diff --git a/target/linux/generic/files/include/uapi/linux/switch.h b/target/linux/generic/files/include/uapi/linux/switch.h
index 2a67da2e9d..ea449653fa 100644
--- a/target/linux/generic/files/include/uapi/linux/switch.h
+++ b/target/linux/generic/files/include/uapi/linux/switch.h
@@ -39,6 +39,7 @@ enum {
 	SWITCH_ATTR_NAME,
 	SWITCH_ATTR_VLANS,
 	SWITCH_ATTR_PORTS,
+	SWITCH_ATTR_PORTMAP,
 	SWITCH_ATTR_CPU_PORT,
 	/* attributes */
 	SWITCH_ATTR_OP_ID,
@@ -56,6 +57,14 @@ enum {
 	SWITCH_ATTR_MAX
 };
 
+enum {
+	/* port map */
+	SWITCH_PORTMAP_PORTS,
+	SWITCH_PORTMAP_SEGMENT,
+	SWITCH_PORTMAP_VIRT,
+	SWITCH_PORTMAP_MAX
+};
+
 /* commands */
 enum {
 	SWITCH_CMD_UNSPEC,