realtek: Store and Restore MC memberships for port enable/disable

We need to store and restore MC memberships in HW when a port joins or
leaves a bridge as well as when it is enabled or disabled, as these
properties should not change in these situations.

Signed-off-by: Sebastian Gottschall <s.gottschall@dd-wrt.com>
Signed-off-by: Birger Koblitz <git@birger-koblitz.de>

2 files changed, 86 insertions, 55 deletions

diff --git a/target/linux/realtek/files-5.10/drivers/net/dsa/rtl83xx/dsa.c b/target/linux/realtek/files-5.10/drivers/net/dsa/rtl83xx/dsa.c
index e81f7f6405..5ea21b1e6f 100644
--- a/target/linux/realtek/files-5.10/drivers/net/dsa/rtl83xx/dsa.c
+++ b/target/linux/realtek/files-5.10/drivers/net/dsa/rtl83xx/dsa.c
@@ -933,6 +933,86 @@ static int rtl83xx_get_sset_count(struct dsa_switch *ds, int port, int sset)
 	return ARRAY_SIZE(rtl83xx_mib);
 }
 
+static int rtl83xx_mc_group_alloc(struct rtl838x_switch_priv *priv, int port)
+{
+	int mc_group = find_first_zero_bit(priv->mc_group_bm, MAX_MC_GROUPS - 1);
+	u64 portmask;
+
+	if (mc_group >= MAX_MC_GROUPS - 1)
+		return -1;
+
+	if (priv->is_lagmember[port]) {
+		pr_info("%s: %d is lag slave. ignore\n", __func__, port);
+		return 0;
+	}
+
+	set_bit(mc_group, priv->mc_group_bm);
+	mc_group++;  // We cannot use group 0, as this is used for lookup miss flooding
+	portmask = BIT_ULL(port) | BIT_ULL(priv->cpu_port); 
+	priv->r->write_mcast_pmask(mc_group, portmask);
+
+	return mc_group;
+}
+
+static u64 rtl83xx_mc_group_add_port(struct rtl838x_switch_priv *priv, int mc_group, int port)
+{
+	u64 portmask = priv->r->read_mcast_pmask(mc_group);
+
+	pr_debug("%s: %d\n", __func__, port);
+	if (priv->is_lagmember[port]) {
+		pr_info("%s: %d is lag slave. ignore\n", __func__, port);
+		return portmask;
+	}
+	portmask |= BIT_ULL(port);
+	priv->r->write_mcast_pmask(mc_group, portmask);
+
+	return portmask;
+}
+
+static u64 rtl83xx_mc_group_del_port(struct rtl838x_switch_priv *priv, int mc_group, int port)
+{
+	u64 portmask = priv->r->read_mcast_pmask(mc_group);
+
+	pr_debug("%s: %d\n", __func__, port);
+	if (priv->is_lagmember[port]) {
+		pr_info("%s: %d is lag slave. ignore\n", __func__, port);
+		return portmask;
+	}
+	priv->r->write_mcast_pmask(mc_group, portmask);
+	if (portmask == BIT_ULL(priv->cpu_port)) {
+		portmask &= ~BIT_ULL(priv->cpu_port);
+		priv->r->write_mcast_pmask(mc_group, portmask);
+		clear_bit(mc_group, priv->mc_group_bm);
+	}
+
+	return portmask;
+}
+
+static void store_mcgroups(struct rtl838x_switch_priv *priv, int port)
+{
+	int mc_group;
+
+	for (mc_group = 0; mc_group < MAX_MC_GROUPS; mc_group++) {
+		u64 portmask = priv->r->read_mcast_pmask(mc_group);
+		if (portmask & BIT_ULL(port)) {
+			priv->mc_group_saves[mc_group] = port;
+			rtl83xx_mc_group_del_port(priv, mc_group, port);
+		}
+	}
+}
+
+static void load_mcgroups(struct rtl838x_switch_priv *priv, int port)
+{
+	int mc_group;
+
+	for (mc_group = 0; mc_group < MAX_MC_GROUPS; mc_group++) {
+		if (priv->mc_group_saves[mc_group] == port) {
+			rtl83xx_mc_group_add_port(priv, mc_group, port);
+			priv->mc_group_saves[mc_group] = -1;
+		}
+	}
+}
+
 static int rtl83xx_port_enable(struct dsa_switch *ds, int port,
 				struct phy_device *phydev)
 {
@@ -954,6 +1034,8 @@ static int rtl83xx_port_enable(struct dsa_switch *ds, int port,
 	/* add port to switch mask of CPU_PORT */
 	priv->r->traffic_enable(priv->cpu_port, port);
 
+	load_mcgroups(priv, port);
+
 	if (priv->is_lagmember[port]) {
 		pr_debug("%s: %d is lag slave. ignore\n", __func__, port);
 		return 0;
@@ -988,6 +1070,7 @@ static void rtl83xx_port_disable(struct dsa_switch *ds, int port)
 	// BUG: This does not work on RTL931X
 	/* remove port from switch mask of CPU_PORT */
 	priv->r->traffic_disable(priv->cpu_port, port);
+	store_mcgroups(priv, port);
 
 	/* remove all other ports in the same bridge from switch mask of port */
 	v = priv->r->traffic_get(port);
@@ -1087,6 +1170,7 @@ static int rtl83xx_port_bridge_join(struct dsa_switch *ds, int port,
 			port_bitmap |= BIT_ULL(i);
 		}
 	}
+	load_mcgroups(priv, port);
 
 	/* Add all other ports to this port matrix. */
 	if (priv->ports[port].enable) {
@@ -1127,6 +1211,7 @@ static void rtl83xx_port_bridge_leave(struct dsa_switch *ds, int port,
 			port_bitmap &= ~BIT_ULL(i);
 		}
 	}
+	store_mcgroups(priv, port);
 
 	/* Add all other ports to this port matrix. */
 	if (priv->ports[port].enable) {
@@ -1653,61 +1738,6 @@ static int rtl83xx_port_mdb_prepare(struct dsa_switch *ds, int port,
 	return 0;
 }
 
-static int rtl83xx_mc_group_alloc(struct rtl838x_switch_priv *priv, int port)
-{
-	int mc_group = find_first_zero_bit(priv->mc_group_bm, MAX_MC_GROUPS - 1);
-	u64 portmask;
-
-	if (mc_group >= MAX_MC_GROUPS - 1)
-		return -1;
-
-	pr_debug("Using MC group %d\n", mc_group);
-
-	if (priv->is_lagmember[port]) {
-		pr_debug("%s: %d is lag slave. ignore\n", __func__, port);
-		return 0;
-	}
-
-	set_bit(mc_group, priv->mc_group_bm);
-	mc_group++;  // We cannot use group 0, as this is used for lookup miss flooding
-	portmask = BIT_ULL(port);
-	priv->r->write_mcast_pmask(mc_group, portmask);
-
-	return mc_group;
-}
-
-static u64 rtl83xx_mc_group_add_port(struct rtl838x_switch_priv *priv, int mc_group, int port)
-{
-	u64 portmask = priv->r->read_mcast_pmask(mc_group);
-
-	if (priv->is_lagmember[port]) {
-		pr_debug("%s: %d is lag slave. ignore\n", __func__, port);
-		return portmask;
-	}
-
-	portmask |= BIT_ULL(port);
-	priv->r->write_mcast_pmask(mc_group, portmask);
-
-	return portmask;
-}
-
-static u64 rtl83xx_mc_group_del_port(struct rtl838x_switch_priv *priv, int mc_group, int port)
-{
-	u64 portmask = priv->r->read_mcast_pmask(mc_group);
-
-	if (priv->is_lagmember[port]) {
-		pr_debug("%s: %d is lag slave. ignore\n", __func__, port);
-		return portmask;
-	}
-
-	portmask &= ~BIT_ULL(port);
-	priv->r->write_mcast_pmask(mc_group, portmask);
-	if (!portmask)
-		clear_bit(mc_group, priv->mc_group_bm);
-
-	return portmask;
-}
-
 static void rtl83xx_port_mdb_add(struct dsa_switch *ds, int port,
 			const struct switchdev_obj_port_mdb *mdb)
 {
diff --git a/target/linux/realtek/files-5.10/drivers/net/dsa/rtl83xx/rtl838x.h b/target/linux/realtek/files-5.10/drivers/net/dsa/rtl83xx/rtl838x.h
index 1b671264dc..e41f81b834 100644
--- a/target/linux/realtek/files-5.10/drivers/net/dsa/rtl83xx/rtl838x.h
+++ b/target/linux/realtek/files-5.10/drivers/net/dsa/rtl83xx/rtl838x.h
@@ -1044,6 +1044,7 @@ struct rtl838x_switch_priv {
 	struct notifier_block fib_nb;
 	bool eee_enabled;
 	unsigned long int mc_group_bm[MAX_MC_GROUPS >> 5];
+	int mc_group_saves[MAX_MC_GROUPS];
 	int n_pie_blocks;
 	struct rhashtable tc_ht;
 	unsigned long int pie_use_bm[MAX_PIE_ENTRIES >> 5];