[net-next PATCH] net: dsa: qca8k: add support for port_change_master

[Christian Marangi <ansuelsmth@gmail.com>]

Add support for port_change_master to permit assigning an alternative
CPU port if the switch have both CPU port connected or create a LAG on
both CPU port and assign the LAG as DSA master.

On port change master request, we check if the master is a LAG.
With LAG we compose the cpu_port_mask with the CPU port in the LAG, if
master is a simple dsa_port, we derive the index.

Finally we apply the new cpu_port_mask to the LOOKUP MEMBER to permit
the port to receive packet by the new CPU port setup for the port and
we reenable the target port previously disabled.

Signed-off-by: Christian Marangi <ansuelsmth@gmail.com>
---
 drivers/net/dsa/qca/qca8k-8xxx.c | 54 ++++++++++++++++++++++++++++++++
 drivers/net/dsa/qca/qca8k.h      |  1 +
 2 files changed, 55 insertions(+)

diff --git a/drivers/net/dsa/qca/qca8k-8xxx.c b/drivers/net/dsa/qca/qca8k-8xxx.c
index dee7b6579916..435b69c1c552 100644
--- a/drivers/net/dsa/qca/qca8k-8xxx.c
+++ b/drivers/net/dsa/qca/qca8k-8xxx.c
@@ -1713,6 +1713,59 @@ qca8k_get_tag_protocol(struct dsa_switch *ds, int port,
 	return DSA_TAG_PROTO_QCA;
 }
 
+static int qca8k_port_change_master(struct dsa_switch *ds, int port,
+				    struct net_device *master,
+				    struct netlink_ext_ack *extack)
+{
+	struct qca8k_priv *priv = ds->priv;
+	u32 val, cpu_port_mask = 0;
+	struct dsa_port *dp;
+	int ret;
+
+	/* With LAG of CPU port, compose the mask for LOOKUP MEMBER */
+	if (netif_is_lag_master(master)) {
+		struct dsa_lag *lag;
+		int id;
+
+		id = dsa_lag_id(ds->dst, master);
+		lag = dsa_lag_by_id(ds->dst, id);
+
+		dsa_lag_foreach_port(dp, ds->dst, lag)
+			if (dsa_port_is_cpu(dp))
+				cpu_port_mask |= BIT(dp->index);
+	} else {
+		dp = dsa_port_from_netdev(master);
+		cpu_port_mask |= BIT(dp->index);
+	}
+
+	/* Disable port */
+	qca8k_port_set_status(priv, port, 0);
+
+	/* Connect it to new cpu port */
+	ret = qca8k_read(priv, QCA8K_PORT_LOOKUP_CTRL(port), &val);
+	if (ret)
+		return ret;
+
+	/* Reset connected CPU port in LOOKUP MEMBER */
+	val &= QCA8K_PORT_LOOKUP_USER_MEMBER;
+	/* Assign the new CPU port in LOOKUP MEMBER */
+	val |= cpu_port_mask;
+
+	ret = qca8k_rmw(priv, QCA8K_PORT_LOOKUP_CTRL(port),
+			QCA8K_PORT_LOOKUP_MEMBER,
+			val);
+	if (ret)
+		return ret;
+
+	/* Fast Age the port to flush FDB table */
+	qca8k_port_fast_age(ds, port);
+
+	/* Reenable port */
+	qca8k_port_set_status(priv, port, 1);
+
+	return 0;
+}
+
 static void
 qca8k_master_change(struct dsa_switch *ds, const struct net_device *master,
 		    bool operational)
@@ -1996,6 +2049,7 @@ static const struct dsa_switch_ops qca8k_switch_ops = {
 	.get_phy_flags		= qca8k_get_phy_flags,
 	.port_lag_join		= qca8k_port_lag_join,
 	.port_lag_leave		= qca8k_port_lag_leave,
+	.port_change_master	= qca8k_port_change_master,
 	.master_state_change	= qca8k_master_change,
 	.connect_tag_protocol	= qca8k_connect_tag_protocol,
 };
diff --git a/drivers/net/dsa/qca/qca8k.h b/drivers/net/dsa/qca/qca8k.h
index c5cc8a172d65..424f851db881 100644
--- a/drivers/net/dsa/qca/qca8k.h
+++ b/drivers/net/dsa/qca/qca8k.h
@@ -250,6 +250,7 @@
 #define   QCA8K_GLOBAL_FW_CTRL1_MC_DP_MASK		GENMASK(14, 8)
 #define   QCA8K_GLOBAL_FW_CTRL1_UC_DP_MASK		GENMASK(6, 0)
 #define QCA8K_PORT_LOOKUP_CTRL(_i)			(0x660 + (_i) * 0xc)
+#define   QCA8K_PORT_LOOKUP_USER_MEMBER			GENMASK(5, 1)
 #define   QCA8K_PORT_LOOKUP_MEMBER			GENMASK(6, 0)
 #define   QCA8K_PORT_LOOKUP_VLAN_MODE_MASK		GENMASK(9, 8)
 #define   QCA8K_PORT_LOOKUP_VLAN_MODE(x)		FIELD_PREP(QCA8K_PORT_LOOKUP_VLAN_MODE_MASK, x)
-- 
2.40.1