realtek: Add support for Layer 2 Multicast

Adds support for Layer 2 multicast by implementing the DSA port_mdb_*
callbacks. The Kernel bridge listens to IGMP/MLD messages trapped to
the CPU-port, and calls the Multicast Forwarding Database updates.
The updates manage the L2 forwarding entries and the multicast
port-maps.

Signed-off-by: Birger Koblitz <git@birger-koblitz.de>

5 files changed, 1024 insertions, 300 deletions

diff --git a/target/linux/realtek/files-5.4/drivers/net/dsa/rtl83xx/dsa.c b/target/linux/realtek/files-5.4/drivers/net/dsa/rtl83xx/dsa.c
index 40130c287a..c5f243c55a 100644
--- a/target/linux/realtek/files-5.4/drivers/net/dsa/rtl83xx/dsa.c
+++ b/target/linux/realtek/files-5.4/drivers/net/dsa/rtl83xx/dsa.c
@@ -26,50 +26,6 @@ static void rtl83xx_init_stats(struct rtl838x_switch_priv *priv)
 	mutex_unlock(&priv->reg_mutex);
 }
 
-static void rtl83xx_write_cam(int idx, u32 *r)
-{
-	u32 cmd = BIT(16) /* Execute cmd */
-		| BIT(15) /* Read */
-		| BIT(13) /* Table type 0b01 */
-		| (idx & 0x3f);
-
-	sw_w32(r[0], RTL838X_TBL_ACCESS_L2_DATA(0));
-	sw_w32(r[1], RTL838X_TBL_ACCESS_L2_DATA(1));
-	sw_w32(r[2], RTL838X_TBL_ACCESS_L2_DATA(2));
-
-	sw_w32(cmd, RTL838X_TBL_ACCESS_L2_CTRL);
-	do { }  while (sw_r32(RTL838X_TBL_ACCESS_L2_CTRL) & BIT(16));
-}
-
-static u64 rtl83xx_hash_key(struct rtl838x_switch_priv *priv, u64 mac, u32 vid)
-{
-	switch (priv->family_id) {
-	case RTL8380_FAMILY_ID:
-		return rtl838x_hash(priv, mac << 12 | vid);
-	case RTL8390_FAMILY_ID:
-		return rtl839x_hash(priv, mac << 12 | vid);
-	case RTL9300_FAMILY_ID:
-		return rtl930x_hash(priv, ((u64)vid) << 48 | mac);
-	default:
-		pr_err("Hash not implemented\n");
-	}
-	return 0;
-}
-
-static void rtl83xx_write_hash(int idx, u32 *r)
-{
-	u32 cmd = BIT(16) /* Execute cmd */
-		| 0 << 15 /* Write */
-		| 0 << 13 /* Table type 0b00 */
-		| (idx & 0x1fff);
-
-	sw_w32(0, RTL838X_TBL_ACCESS_L2_DATA(0));
-	sw_w32(0, RTL838X_TBL_ACCESS_L2_DATA(1));
-	sw_w32(0, RTL838X_TBL_ACCESS_L2_DATA(2));
-	sw_w32(cmd, RTL838X_TBL_ACCESS_L2_CTRL);
-	do { }  while (sw_r32(RTL838X_TBL_ACCESS_L2_CTRL) & BIT(16));
-}
-
 static void rtl83xx_enable_phy_polling(struct rtl838x_switch_priv *priv)
 {
 	int i;
@@ -79,7 +35,7 @@ static void rtl83xx_enable_phy_polling(struct rtl838x_switch_priv *priv)
 	/* Enable all ports with a PHY, including the SFP-ports */
 	for (i = 0; i < priv->cpu_port; i++) {
 		if (priv->ports[i].phy)
-			v |= BIT(i);
+			v |= BIT_ULL(i);
 	}
 
 	pr_debug("%s: %16llx\n", __func__, v);
@@ -204,7 +160,7 @@ static int rtl83xx_setup(struct dsa_switch *ds)
 	 */
 	for (i = 0; i < priv->cpu_port; i++) {
 		if (priv->ports[i].phy) {
-			priv->r->set_port_reg_be(BIT_ULL(priv->cpu_port) | BIT(i),
+			priv->r->set_port_reg_be(BIT_ULL(priv->cpu_port) | BIT_ULL(i),
 					      priv->r->port_iso_ctrl(i));
 			port_bitmap |= BIT_ULL(i);
 		}
@@ -250,8 +206,8 @@ static int rtl930x_setup(struct dsa_switch *ds)
 
 	for (i = 0; i < priv->cpu_port; i++) {
 		if (priv->ports[i].phy) {
-			priv->r->traffic_set(i, BIT(priv->cpu_port) | BIT(i));
-			port_bitmap |= 1ULL << i;
+			priv->r->traffic_set(i, BIT_ULL(priv->cpu_port) | BIT_ULL(i));
+			port_bitmap |= BIT_ULL(i);
 		}
 	}
 	priv->r->traffic_set(priv->cpu_port, port_bitmap);
@@ -276,7 +232,7 @@ static void rtl83xx_phylink_validate(struct dsa_switch *ds, int port,
 	struct rtl838x_switch_priv *priv = ds->priv;
 	__ETHTOOL_DECLARE_LINK_MODE_MASK(mask) = { 0, };
 
-	pr_debug("In %s port %d", __func__, port);
+	pr_debug("In %s port %d, state is %d", __func__, port, state->interface);
 
 	if (!phy_interface_mode_is_rgmii(state->interface) &&
 	    state->interface != PHY_INTERFACE_MODE_NA &&
@@ -313,6 +269,10 @@ static void rtl83xx_phylink_validate(struct dsa_switch *ds, int port,
 	if (port >= 24 && port <= 27 && priv->family_id == RTL8380_FAMILY_ID)
 		phylink_set(mask, 1000baseX_Full);
 
+	/* On the RTL839x family of SoCs, ports 48 to 51 are SFP ports */
+	if (port >=48 && port <= 51 && priv->family_id == RTL8390_FAMILY_ID)
+		phylink_set(mask, 1000baseX_Full);
+
 	phylink_set(mask, 10baseT_Half);
 	phylink_set(mask, 10baseT_Full);
 	phylink_set(mask, 100baseT_Half);
@@ -345,7 +305,7 @@ static int rtl83xx_phylink_mac_link_state(struct dsa_switch *ds, int port,
 	link = priv->r->get_port_reg_le(priv->r->mac_link_sts);
 	if (link & BIT_ULL(port))
 		state->link = 1;
-	pr_debug("%s: link state: %llx\n", __func__, link & BIT_ULL(port));
+	pr_debug("%s: link state port %d: %llx\n", __func__, port, link & BIT_ULL(port));
 
 	state->duplex = 0;
 	if (priv->r->get_port_reg_le(priv->r->mac_link_dup_sts) & BIT_ULL(port))
@@ -364,7 +324,8 @@ static int rtl83xx_phylink_mac_link_state(struct dsa_switch *ds, int port,
 		state->speed = SPEED_1000;
 		break;
 	case 3:
-		if (port == 24 || port == 26) /* Internal serdes */
+		if (priv->family_id == RTL9300_FAMILY_ID
+			&& (port == 24 || port == 26)) /* Internal serdes */
 			state->speed = SPEED_2500;
 		else
 			state->speed = SPEED_100; /* Is in fact 500Mbit */
@@ -378,7 +339,6 @@ static int rtl83xx_phylink_mac_link_state(struct dsa_switch *ds, int port,
 	return 1;
 }
 
-
 static void rtl83xx_config_interface(int port, phy_interface_t interface)
 {
 	u32 old, int_shift, sds_shift;
@@ -583,8 +543,11 @@ static int rtl83xx_port_enable(struct dsa_switch *ds, int port,
 	v |= priv->ports[port].pm;
 	priv->r->traffic_set(port, v);
 
-	sw_w32_mask(0, BIT(port), RTL930X_L2_PORT_SABLK_CTRL);
-	sw_w32_mask(0, BIT(port), RTL930X_L2_PORT_DABLK_CTRL);
+	// TODO: Figure out if this is necessary
+	if (priv->family_id == RTL9300_FAMILY_ID) {
+		sw_w32_mask(0, BIT(port), RTL930X_L2_PORT_SABLK_CTRL);
+		sw_w32_mask(0, BIT(port), RTL930X_L2_PORT_DABLK_CTRL);
+	}
 
 	return 0;
 }
@@ -689,7 +652,7 @@ static int rtl83xx_port_bridge_join(struct dsa_switch *ds, int port,
 					struct net_device *bridge)
 {
 	struct rtl838x_switch_priv *priv = ds->priv;
-	u64 port_bitmap = 1ULL << priv->cpu_port, v;
+	u64 port_bitmap = BIT_ULL(priv->cpu_port), v;
 	int i;
 
 	pr_debug("%s %x: %d %llx", __func__, (u32)priv, port, port_bitmap);
@@ -705,8 +668,8 @@ static int rtl83xx_port_bridge_join(struct dsa_switch *ds, int port,
 			if (priv->ports[i].enable)
 				priv->r->traffic_enable(i, port);
 
-			priv->ports[i].pm |= 1ULL << port;
-			port_bitmap |= 1ULL << i;
+			priv->ports[i].pm |= BIT_ULL(port);
+			port_bitmap |= BIT_ULL(i);
 		}
 	}
 
@@ -727,7 +690,7 @@ static void rtl83xx_port_bridge_leave(struct dsa_switch *ds, int port,
 					struct net_device *bridge)
 {
 	struct rtl838x_switch_priv *priv = ds->priv;
-	u64 port_bitmap = 1ULL << priv->cpu_port, v;
+	u64 port_bitmap = BIT_ULL(priv->cpu_port), v;
 	int i;
 
 	pr_debug("%s %x: %d", __func__, (u32)priv, port);
@@ -745,7 +708,7 @@ static void rtl83xx_port_bridge_leave(struct dsa_switch *ds, int port,
 			if (priv->ports[i].enable)
 				priv->r->traffic_disable(i, port);
 
-			priv->ports[i].pm |= 1ULL << port;
+			priv->ports[i].pm |= BIT_ULL(port);
 			port_bitmap &= ~BIT_ULL(i);
 		}
 	}
@@ -897,17 +860,16 @@ static int rtl83xx_vlan_prepare(struct dsa_switch *ds, int port,
 	struct rtl838x_vlan_info info;
 	struct rtl838x_switch_priv *priv = ds->priv;
 
-	pr_info("%s: port %d\n", __func__, port);
+	priv->r->vlan_tables_read(0, &info);
 
-	mutex_lock(&priv->reg_mutex);
+	pr_debug("VLAN 0: Tagged ports %llx, untag %llx, profile %d, MC# %d, UC# %d, FID %x\n",
+		info.tagged_ports, info.untagged_ports, info.profile_id,
+		info.hash_mc_fid, info.hash_uc_fid, info.fid);
 
-	priv->r->vlan_profile_dump(1);
 	priv->r->vlan_tables_read(1, &info);
-
-	pr_info("Tagged ports %llx, untag %llx, prof %x, MC# %d, UC# %d, FID %x\n",
+	pr_debug("VLAN 1: Tagged ports %llx, untag %llx, profile %d, MC# %d, UC# %d, FID %x\n",
 		info.tagged_ports, info.untagged_ports, info.profile_id,
 		info.hash_mc_fid, info.hash_uc_fid, info.fid);
-
 	priv->r->vlan_set_untagged(1, info.untagged_ports);
 	pr_debug("SET: Untagged ports, VLAN %d: %llx\n", 1, info.untagged_ports);
 
@@ -925,7 +887,7 @@ static void rtl83xx_vlan_add(struct dsa_switch *ds, int port,
 	struct rtl838x_switch_priv *priv = ds->priv;
 	int v;
 
-	pr_info("%s port %d, vid_end %d, vid_end %d, flags %x\n", __func__,
+	pr_debug("%s port %d, vid_end %d, vid_end %d, flags %x\n", __func__,
 		port, vlan->vid_begin, vlan->vid_end, vlan->flags);
 
 	if (vlan->vid_begin > 4095 || vlan->vid_end > 4095) {
@@ -970,10 +932,10 @@ static void rtl83xx_vlan_add(struct dsa_switch *ds, int port,
 			info.untagged_ports |= BIT_ULL(port);
 
 		priv->r->vlan_set_untagged(v, info.untagged_ports);
-		pr_info("Untagged ports, VLAN %d: %llx\n", v, info.untagged_ports);
+		pr_debug("Untagged ports, VLAN %d: %llx\n", v, info.untagged_ports);
 
 		priv->r->vlan_set_tagged(v, &info);
-		pr_info("Tagged ports, VLAN %d: %llx\n", v, info.tagged_ports);
+		pr_debug("Tagged ports, VLAN %d: %llx\n", v, info.tagged_ports);
 	}
 
 	mutex_unlock(&priv->reg_mutex);
@@ -1024,59 +986,136 @@ static int rtl83xx_vlan_del(struct dsa_switch *ds, int port,
 	return 0;
 }
 
-static int rtl83xx_port_fdb_add(struct dsa_switch *ds, int port,
-				const unsigned char *addr, u16 vid)
+static void dump_l2_entry(struct rtl838x_l2_entry *e)
 {
-	struct rtl838x_switch_priv *priv = ds->priv;
-	u64 mac = ether_addr_to_u64(addr);
-	u32 key = rtl83xx_hash_key(priv, mac, vid);
-	struct rtl838x_l2_entry e;
-	u32 r[3];
+	pr_info("MAC: %02x:%02x:%02x:%02x:%02x:%02x vid: %d, rvid: %d, port: %d, valid: %d\n",
+		e->mac[0], e->mac[1], e->mac[2], e->mac[3], e->mac[4], e->mac[5],
+		e->vid, e->rvid, e->port, e->valid);
+
+	if (e->type != L2_MULTICAST) {
+		pr_info("Type: %d, is_static: %d, is_ip_mc: %d, is_ipv6_mc: %d, block_da: %d\n",
+			e->type, e->is_static, e->is_ip_mc, e->is_ipv6_mc, e->block_da);
+		pr_info("  block_sa: %d, susp: %d, nh: %d, age: %d, is_trunk: %d, trunk: %d\n",
+		e->block_sa, e->suspended, e->next_hop, e->age, e->is_trunk, e->trunk);
+	}
+	if (e->type == L2_MULTICAST)
+		pr_info("  L2_MULTICAST mc_portmask_index: %d\n", e->mc_portmask_index);
+	if (e->is_ip_mc || e->is_ipv6_mc)
+		pr_info("  mc_portmask_index: %d, mc_gip: %d, mc_sip: %d\n",
+			e->mc_portmask_index, e->mc_gip, e->mc_sip);
+	pr_info("  stack_dev: %d\n", e->stack_dev);
+	if (e->next_hop)
+		pr_info("  nh_route_id: %d\n", e->nh_route_id);
+}
+
+static void rtl83xx_setup_l2_uc_entry(struct rtl838x_l2_entry *e, int port, int vid, u64 mac)
+{
+	e->is_ip_mc = e->is_ipv6_mc  = false;
+	e->valid = true;
+	e->age = 3;
+	e->port = port,
+	e->vid = vid;
+	u64_to_ether_addr(mac, e->mac);
+}
+
+static void rtl83xx_setup_l2_mc_entry(struct rtl838x_switch_priv *priv,
+				      struct rtl838x_l2_entry *e, int vid, u64 mac, int mc_group)
+{
+	e->is_ip_mc = e->is_ipv6_mc  = false;
+	e->valid = true;
+	e->mc_portmask_index = mc_group;
+	e->type = L2_MULTICAST;
+	e->rvid = e->vid = vid;
+	pr_debug("%s: vid: %d, rvid: %d\n", __func__, e->vid, e->rvid);
+	u64_to_ether_addr(mac, e->mac);
+}
+
+/*
+ * Uses the seed to identify a hash bucket in the L2 using the derived hash key and then loops
+ * over the entries in the bucket until either a matching entry is found or an empty slot
+ * Returns the filled in rtl838x_l2_entry and the index in the bucket when an entry was found
+ * when an empty slot was found and must exist is false, the index of the slot is returned
+ * when no slots are available returns -1
+ */
+static int rtl83xx_find_l2_hash_entry(struct rtl838x_switch_priv *priv, u64 seed,
+				     bool must_exist, struct rtl838x_l2_entry *e)
+{
+	int i, idx = -1;
+	u32 key = priv->r->l2_hash_key(priv, seed);
 	u64 entry;
-	int idx = -1, err = 0, i;
 
-	mutex_lock(&priv->reg_mutex);
-	for (i = 0; i < 4; i++) {
-		entry = priv->r->read_l2_entry_using_hash(key, i, &e);
-		if (!e.valid) {
-			idx = (key << 2) | i;
-			break;
-		}
-		if ((entry & 0x0fffffffffffffffULL) == ((mac << 12) | vid)) {
-			idx = (key << 2) | i;
+	pr_debug("%s: using key %x, for seed %016llx\n", __func__, key, seed);
+	// Loop over all entries in the hash-bucket and over the second block on 93xx SoCs
+	for (i = 0; i < priv->l2_bucket_size; i++) {
+		entry = priv->r->read_l2_entry_using_hash(key, i, e);
+		pr_debug("valid %d, mac %016llx\n", e->valid, ether_addr_to_u64(&e->mac[0]));
+		if (must_exist && !e->valid)
+			continue;
+		if (!e->valid || ((entry & 0x0fffffffffffffffULL) == seed)) {
+			idx = i > 3 ? ((key >> 14) & 0xffff) | i >> 1 : ((key << 2) | i) & 0xffff;
 			break;
 		}
 	}
-	if (idx >= 0) {
-		r[0] = 3 << 17 | port << 12; // Aging and  port
-		r[0] |= vid;
-		r[1] = mac >> 16;
-		r[2] = (mac & 0xffff) << 12; /* rvid = 0 */
-		rtl83xx_write_hash(idx, r);
-		goto out;
-	}
 
-	/* Hash buckets full, try CAM */
+	return idx;
+}
+
+/*
+ * Uses the seed to identify an entry in the CAM by looping over all its entries
+ * Returns the filled in rtl838x_l2_entry and the index in the CAM when an entry was found
+ * when an empty slot was found the index of the slot is returned
+ * when no slots are available returns -1
+ */
+static int rtl83xx_find_l2_cam_entry(struct rtl838x_switch_priv *priv, u64 seed,
+				     bool must_exist, struct rtl838x_l2_entry *e)
+{
+	int i, idx = -1;
+	u64 entry;
+
 	for (i = 0; i < 64; i++) {
-		entry = priv->r->read_cam(i, &e);
-		if (!e.valid) {
+		entry = priv->r->read_cam(i, e);
+		if (!must_exist && !e->valid) {
 			if (idx < 0) /* First empty entry? */
 				idx = i;
 			break;
-		} else if ((entry & 0x0fffffffffffffffULL) == ((mac << 12) | vid)) {
+		} else if ((entry & 0x0fffffffffffffffULL) == seed) {
 			pr_debug("Found entry in CAM\n");
 			idx = i;
 			break;
 		}
 	}
+	return idx;
+}
+
+static int rtl83xx_port_fdb_add(struct dsa_switch *ds, int port,
+				const unsigned char *addr, u16 vid)
+{
+	struct rtl838x_switch_priv *priv = ds->priv;
+	u64 mac = ether_addr_to_u64(addr);
+	struct rtl838x_l2_entry e;
+	int err = 0, idx;
+	u64 seed = priv->r->l2_hash_seed(mac, vid);
+
+	mutex_lock(&priv->reg_mutex);
+
+	idx = rtl83xx_find_l2_hash_entry(priv, seed, false, &e);
+
+	// Found an existing or empty entry
+	if (idx >= 0) {
+		rtl83xx_setup_l2_uc_entry(&e, port, vid, mac);
+		priv->r->write_l2_entry_using_hash(idx >> 2, idx & 0x3, &e);
+		goto out;
+	}
+
+	// Hash buckets full, try CAM
+	rtl83xx_find_l2_cam_entry(priv, seed, false, &e);
+
 	if (idx >= 0) {
-		r[0] = 3 << 17 | port << 12; // Aging
-		r[0] |= vid;
-		r[1] = mac >> 16;
-		r[2] = (mac & 0xffff) << 12; /* rvid = 0 */
-		rtl83xx_write_cam(idx, r);
+		rtl83xx_setup_l2_uc_entry(&e, port, vid, mac);
+		priv->r->write_cam(idx, &e);
 		goto out;
 	}
+
 	err = -ENOTSUPP;
 out:
 	mutex_unlock(&priv->reg_mutex);
@@ -1088,41 +1127,29 @@ static int rtl83xx_port_fdb_del(struct dsa_switch *ds, int port,
 {
 	struct rtl838x_switch_priv *priv = ds->priv;
 	u64 mac = ether_addr_to_u64(addr);
-	u32 key = rtl83xx_hash_key(priv, mac, vid);
 	struct rtl838x_l2_entry e;
-	u32 r[3];
-	u64 entry;
-	int idx = -1, err = 0, i;
+	int err = 0, idx;
+	u64 seed = priv->r->l2_hash_seed(mac, vid);
 
-	pr_debug("In %s, mac %llx, vid: %d, key: %x08x\n", __func__, mac, vid, key);
+	pr_info("In %s, mac %llx, vid: %d\n", __func__, mac, vid);
 	mutex_lock(&priv->reg_mutex);
-	for (i = 0; i < 4; i++) {
-		entry = priv->r->read_l2_entry_using_hash(key, i, &e);
-		if (!e.valid)
-			continue;
-		if ((entry & 0x0fffffffffffffffULL) == ((mac << 12) | vid)) {
-			idx = (key << 2) | i;
-			break;
-		}
-	}
 
+	idx = rtl83xx_find_l2_hash_entry(priv, seed, true, &e);
+
+	pr_info("Found entry index %d, key %d and bucket %d\n", idx, idx >> 2, idx & 3);
 	if (idx >= 0) {
-		r[0] = r[1] = r[2] = 0;
-		rtl83xx_write_hash(idx, r);
+		e.valid = false;
+		dump_l2_entry(&e);
+		priv->r->write_l2_entry_using_hash(idx >> 2, idx & 0x3, &e);
 		goto out;
 	}
 
 	/* Check CAM for spillover from hash buckets */
-	for (i = 0; i < 64; i++) {
-		entry = priv->r->read_cam(i, &e);
-		if ((entry & 0x0fffffffffffffffULL) == ((mac << 12) | vid)) {
-			idx = i;
-			break;
-		}
-	}
+	rtl83xx_find_l2_cam_entry(priv, seed, true, &e);
+
 	if (idx >= 0) {
-		r[0] = r[1] = r[2] = 0;
-		rtl83xx_write_cam(idx, r);
+		e.valid = false;
+		priv->r->write_cam(idx, &e);
 		goto out;
 	}
 	err = -ENOENT;
@@ -1137,8 +1164,7 @@ static int rtl83xx_port_fdb_dump(struct dsa_switch *ds, int port,
 	struct rtl838x_l2_entry e;
 	struct rtl838x_switch_priv *priv = ds->priv;
 	int i;
-	u32 fid;
-	u32 pkey;
+	u32 fid, pkey;
 	u64 mac;
 
 	mutex_lock(&priv->reg_mutex);
@@ -1150,13 +1176,25 @@ static int rtl83xx_port_fdb_dump(struct dsa_switch *ds, int port,
 			continue;
 
 		if (e.port == port) {
-			fid = (i & 0x3ff) | (e.rvid & ~0x3ff);
+			fid = ((i >> 2) & 0x3ff) | (e.rvid & ~0x3ff);
 			mac = ether_addr_to_u64(&e.mac[0]);
-			pkey = rtl838x_hash(priv, mac << 12 | fid);
+			pkey = priv->r->l2_hash_key(priv, priv->r->l2_hash_seed(mac, fid));
 			fid = (pkey & 0x3ff) | (fid & ~0x3ff);
-			pr_debug("-> mac %016llx, fid: %d\n", mac, fid);
+			pr_info("-> index %d, key %x, bucket %d, dmac %016llx, fid: %x rvid: %x\n",
+				i, i >> 2, i & 0x3, mac, fid, e.rvid);
+			dump_l2_entry(&e);
+			u64 seed = priv->r->l2_hash_seed(mac, e.rvid);
+			u32 key = priv->r->l2_hash_key(priv, seed);
+			pr_info("seed: %016llx, key based on rvid: %08x\n", seed, key);
 			cb(e.mac, e.vid, e.is_static, data);
 		}
+		if (e.type == L2_MULTICAST) {
+			u64 portmask = priv->r->read_mcast_pmask(e.mc_portmask_index);
+			if (portmask & BIT_ULL(port)) {
+				dump_l2_entry(&e);
+				pr_info("  PM: %016llx\n", portmask);
+			}
+		}
 	}
 
 	for (i = 0; i < 64; i++) {
@@ -1173,6 +1211,164 @@ static int rtl83xx_port_fdb_dump(struct dsa_switch *ds, int port,
 	return 0;
 }
 
+static int rtl83xx_port_mdb_prepare(struct dsa_switch *ds, int port,
+					const struct switchdev_obj_port_mdb *mdb)
+{
+	struct rtl838x_switch_priv *priv = ds->priv;
+
+	if (priv->id >= 0x9300)
+		return -EOPNOTSUPP;
+
+	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);
+	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);
+
+	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);
+
+	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)
+{
+	struct rtl838x_switch_priv *priv = ds->priv;
+	u64 mac = ether_addr_to_u64(mdb->addr);
+	struct rtl838x_l2_entry e;
+	int err = 0, idx;
+	int vid = mdb->vid;
+	u64 seed = priv->r->l2_hash_seed(mac, vid);
+	int mc_group;
+
+	pr_debug("In %s port %d, mac %llx, vid: %d\n", __func__, port, mac, vid);
+	mutex_lock(&priv->reg_mutex);
+
+	idx = rtl83xx_find_l2_hash_entry(priv, seed, false, &e);
+
+	// Found an existing or empty entry
+	if (idx >= 0) {
+		if (e.valid) {
+			pr_debug("Found an existing entry %016llx, mc_group %d\n",
+				ether_addr_to_u64(e.mac), e.mc_portmask_index);
+			rtl83xx_mc_group_add_port(priv, e.mc_portmask_index, port);
+		} else {
+			pr_debug("New entry for seed %016llx\n", seed);
+			mc_group = rtl83xx_mc_group_alloc(priv, port);
+			if (mc_group < 0) {
+				err = -ENOTSUPP;
+				goto out;
+			}
+			rtl83xx_setup_l2_mc_entry(priv, &e, vid, mac, mc_group);
+			priv->r->write_l2_entry_using_hash(idx >> 2, idx & 0x3, &e);
+		}
+		goto out;
+	}
+
+	// Hash buckets full, try CAM
+	rtl83xx_find_l2_cam_entry(priv, seed, false, &e);
+
+	if (idx >= 0) {
+		if (e.valid) {
+			pr_debug("Found existing CAM entry %016llx, mc_group %d\n",
+				 ether_addr_to_u64(e.mac), e.mc_portmask_index);
+			rtl83xx_mc_group_add_port(priv, e.mc_portmask_index, port);
+		} else {
+			pr_debug("New entry\n");
+			mc_group = rtl83xx_mc_group_alloc(priv, port);
+			if (mc_group < 0) {
+				err = -ENOTSUPP;
+				goto out;
+			}
+			rtl83xx_setup_l2_mc_entry(priv, &e, vid, mac, mc_group);
+			priv->r->write_cam(idx, &e);
+		}
+		goto out;
+	}
+
+	err = -ENOTSUPP;
+out:
+	mutex_unlock(&priv->reg_mutex);
+	if (err)
+		dev_err(ds->dev, "failed to add MDB entry\n");
+}
+
+int rtl83xx_port_mdb_del(struct dsa_switch *ds, int port,
+			const struct switchdev_obj_port_mdb *mdb)
+{
+	struct rtl838x_switch_priv *priv = ds->priv;
+	u64 mac = ether_addr_to_u64(mdb->addr);
+	struct rtl838x_l2_entry e;
+	int err = 0, idx;
+	int vid = mdb->vid;
+	u64 seed = priv->r->l2_hash_seed(mac, vid);
+	u64 portmask;
+
+	pr_debug("In %s, port %d, mac %llx, vid: %d\n", __func__, port, mac, vid);
+	mutex_lock(&priv->reg_mutex);
+
+	idx = rtl83xx_find_l2_hash_entry(priv, seed, true, &e);
+
+	pr_debug("Found entry index %d, key %d and bucket %d\n", idx, idx >> 2, idx & 3);
+	if (idx >= 0) {
+		portmask = rtl83xx_mc_group_del_port(priv, e.mc_portmask_index, port);
+		if (!portmask) {
+			e.valid = false;
+			// dump_l2_entry(&e);
+			priv->r->write_l2_entry_using_hash(idx >> 2, idx & 0x3, &e);
+		}
+		goto out;
+	}
+
+	/* Check CAM for spillover from hash buckets */
+	rtl83xx_find_l2_cam_entry(priv, seed, true, &e);
+
+	if (idx >= 0) {
+		portmask = rtl83xx_mc_group_del_port(priv, e.mc_portmask_index, port);
+		if (!portmask) {
+			e.valid = false;
+			// dump_l2_entry(&e);
+			priv->r->write_cam(idx, &e);
+		}
+		goto out;
+	}
+	// TODO: Re-enable with a newer kernel: err = -ENOENT;
+out:
+	mutex_unlock(&priv->reg_mutex);
+	return err;
+}
+
 static int rtl83xx_port_mirror_add(struct dsa_switch *ds, int port,
 				   struct dsa_mall_mirror_tc_entry *mirror,
 				   bool ingress)
@@ -1339,6 +1535,10 @@ const struct dsa_switch_ops rtl83xx_switch_ops = {
 	.port_fdb_del		= rtl83xx_port_fdb_del,
 	.port_fdb_dump		= rtl83xx_port_fdb_dump,
 
+	.port_mdb_prepare	= rtl83xx_port_mdb_prepare,
+	.port_mdb_add		= rtl83xx_port_mdb_add,
+	.port_mdb_del		= rtl83xx_port_mdb_del,
+
 	.port_mirror_add	= rtl83xx_port_mirror_add,
 	.port_mirror_del	= rtl83xx_port_mirror_del,
 };
@@ -1380,4 +1580,9 @@ const struct dsa_switch_ops rtl930x_switch_ops = {
 	.port_fdb_add		= rtl83xx_port_fdb_add,
 	.port_fdb_del		= rtl83xx_port_fdb_del,
 	.port_fdb_dump		= rtl83xx_port_fdb_dump,
+
+	.port_mdb_prepare	= rtl83xx_port_mdb_prepare,
+	.port_mdb_add		= rtl83xx_port_mdb_add,
+	.port_mdb_del		= rtl83xx_port_mdb_del,
+
 };
diff --git a/target/linux/realtek/files-5.4/drivers/net/dsa/rtl83xx/rtl838x.c b/target/linux/realtek/files-5.4/drivers/net/dsa/rtl83xx/rtl838x.c
index 9ecd189d91..dfd773c5e6 100644
--- a/target/linux/realtek/files-5.4/drivers/net/dsa/rtl83xx/rtl838x.c
+++ b/target/linux/realtek/files-5.4/drivers/net/dsa/rtl83xx/rtl838x.c
@@ -102,6 +102,40 @@ static void rtl838x_vlan_fwd_on_inner(int port, bool is_set)
 		sw_w32_mask(0, BIT(port), RTL838X_VLAN_PORT_FWD);
 }
 
+static u64 rtl838x_l2_hash_seed(u64 mac, u32 vid)
+{
+	return mac << 12 | vid;
+}
+
+/*
+ * Applies the same hash algorithm as the one used currently by the ASIC to the seed
+ * and returns a key into the L2 hash table
+ */
+static u32 rtl838x_l2_hash_key(struct rtl838x_switch_priv *priv, u64 seed)
+{
+	u32 h1, h2, h3, h;
+
+	if (sw_r32(priv->r->l2_ctrl_0) & 1) {
+		h1 = (seed >> 11) & 0x7ff;
+		h1 = ((h1 & 0x1f) << 6) | ((h1 >> 5) & 0x3f);
+
+		h2 = (seed >> 33) & 0x7ff;
+		h2 = ((h2 & 0x3f) << 5) | ((h2 >> 6) & 0x1f);
+
+		h3 = (seed >> 44) & 0x7ff;
+		h3 = ((h3 & 0x7f) << 4) | ((h3 >> 7) & 0xf);
+
+		h = h1 ^ h2 ^ h3 ^ ((seed >> 55) & 0x1ff);
+		h ^= ((seed >> 22) & 0x7ff) ^ (seed & 0x7ff);
+	} else {
+		h = ((seed >> 55) & 0x1ff) ^ ((seed >> 44) & 0x7ff)
+			^ ((seed >> 33) & 0x7ff) ^ ((seed >> 22) & 0x7ff)
+			^ ((seed >> 11) & 0x7ff) ^ (seed & 0x7ff);
+	}
+
+	return h;
+}
+
 static inline int rtl838x_mac_force_mode_ctrl(int p)
 {
 	return RTL838X_MAC_FORCE_MODE_CTRL + (p << 2);
@@ -132,84 +166,202 @@ inline static int rtl838x_trk_mbr_ctr(int group)
 	return RTL838X_TRK_MBR_CTR + (group << 2);
 }
 
-static u64 rtl838x_read_l2_entry_using_hash(u32 hash, u32 position, struct rtl838x_l2_entry *e)
+/*
+ * Fills an L2 entry structure from the SoC registers
+ */
+static void rtl838x_fill_l2_entry(u32 r[], struct rtl838x_l2_entry *e)
+{
+	/* Table contains different entry types, we need to identify the right one:
+	 * Check for MC entries, first
+	 * In contrast to the RTL93xx SoCs, there is no valid bit, use heuristics to
+	 * identify valid entries
+	 */
+	e->is_ip_mc = !!(r[0] & BIT(22));
+	e->is_ipv6_mc = !!(r[0] & BIT(21));
+	e->type = L2_INVALID;
+
+	if (!e->is_ip_mc && !e->is_ipv6_mc) {
+		e->mac[0] = (r[1] >> 20);
+		e->mac[1] = (r[1] >> 12);
+		e->mac[2] = (r[1] >> 4);
+		e->mac[3] = (r[1] & 0xf) << 4 | (r[2] >> 28);
+		e->mac[4] = (r[2] >> 20);
+		e->mac[5] = (r[2] >> 12);
+
+		e->rvid = r[2] & 0xfff;
+		e->vid = r[0] & 0xfff;
+
+		/* Is it a unicast entry? check multicast bit */
+		if (!(e->mac[0] & 1)) {
+			e->is_static = !!((r[0] >> 19) & 1);
+			e->port = (r[0] >> 12) & 0x1f;
+			e->block_da = !!(r[1] & BIT(30));
+			e->block_sa = !!(r[1] & BIT(31));
+			e->suspended = !!(r[1] & BIT(29));
+			e->next_hop = !!(r[1] & BIT(28));
+			if (e->next_hop) {
+				pr_info("Found next hop entry, need to read extra data\n");
+				e->nh_vlan_target = !!(r[0] & BIT(9));
+				e->nh_route_id = r[0] & 0x1ff;
+			}
+			e->age = (r[0] >> 17) & 0x3;
+			e->valid = true;
+			
+			/* A valid entry has one of mutli-cast, aging, sa/da-blocking,
+			 * next-hop or static entry bit set */
+			if (!(r[0] & 0x007c0000) && !(r[1] & 0xd0000000))
+				e->valid = false;
+			else
+				e->type = L2_UNICAST;
+		} else { // L2 multicast
+			pr_info("Got L2 MC entry: %08x %08x %08x\n", r[0], r[1], r[2]);
+			e->valid = true;
+			e->type = L2_MULTICAST;
+			e->mc_portmask_index = (r[0] >> 12) & 0x1ff;
+		}
+	} else { // IPv4 and IPv6 multicast
+		e->valid = true;
+		e->mc_portmask_index = (r[0] >> 12) & 0x1ff;
+		e->mc_gip = r[1];
+		e->mc_sip = r[2];
+		e->rvid = r[0] & 0xfff;
+	}
+	if (e->is_ip_mc)
+		e->type = IP4_MULTICAST;
+	if (e->is_ipv6_mc)
+		e->type = IP6_MULTICAST;
+}
+
+/*
+ * Fills the 3 SoC table registers r[] with the information of in the rtl838x_l2_entry
+ */
+static void rtl838x_fill_l2_row(u32 r[], struct rtl838x_l2_entry *e)
+{
+	u64 mac = ether_addr_to_u64(e->mac);
+
+	if (!e->valid) {
+		r[0] = r[1] = r[2] = 0;
+		return;
+	}
+
+	r[0] = e->is_ip_mc ? BIT(22) : 0;
+	r[0] |= e->is_ipv6_mc ? BIT(21) : 0;
+
+	if (!e->is_ip_mc && !e->is_ipv6_mc) {
+		r[1] = mac >> 20;
+		r[2] = (mac & 0xfffff) << 12;
+
+		/* Is it a unicast entry? check multicast bit */
+		if (!(e->mac[0] & 1)) {
+			r[0] |= e->is_static ? BIT(19) : 0;
+			r[0] |= (e->port & 0x3f) << 12;
+			r[0] |= e->vid;
+			r[1] |= e->block_da ? BIT(30) : 0;
+			r[1] |= e->block_sa ? BIT(31) : 0;
+			r[1] |= e->suspended ? BIT(29) : 0;
+			r[2] |= e->rvid & 0xfff;
+			if (e->next_hop) {
+				r[1] |= BIT(28);
+				r[0] |= e->nh_vlan_target ? BIT(9) : 0;
+				r[0] |= e->nh_route_id &0x1ff;
+			}
+			r[0] |= (e->age & 0x3) << 17;
+		} else { // L2 Multicast
+			r[0] |= (e->mc_portmask_index & 0x1ff) << 12;
+			r[2] |= e->rvid & 0xfff;
+			r[0] |= e->vid & 0xfff;
+			pr_info("FILL MC: %08x %08x %08x\n", r[0], r[1], r[2]);
+		}
+	} else { // IPv4 and IPv6 multicast
+		r[1] = e->mc_gip;
+		r[2] = e->mc_sip;
+		r[0] |= e->rvid;
+	}
+}
+
+/*
+ * Read an L2 UC or MC entry out of a hash bucket of the L2 forwarding table
+ * hash is the id of the bucket and pos is the position of the entry in that bucket
+ * The data read from the SoC is filled into rtl838x_l2_entry
+ */
+static u64 rtl838x_read_l2_entry_using_hash(u32 hash, u32 pos, struct rtl838x_l2_entry *e)
 {
 	u64 entry;
 	u32 r[3];
+	struct table_reg *q = rtl_table_get(RTL8380_TBL_L2, 0); // Access L2 Table 0
+	u32 idx = (0 << 14) | (hash << 2) | pos; // Search SRAM, with hash and at pos in bucket
+	int i;
 
-	/* Search in SRAM, with hash and at position in hash bucket (0-3) */
-	u32 idx = (0 << 14) | (hash << 2) | position;
-
-	u32 cmd = BIT(16) /* Execute cmd */
-		| BIT(15) /* Read */
-		| 0 << 13 /* Table type 0b00 */
-		| (idx & 0x1fff);
-
-	sw_w32(cmd, RTL838X_TBL_ACCESS_L2_CTRL);
-	do { }  while (sw_r32(RTL838X_TBL_ACCESS_L2_CTRL) & BIT(16));
-	r[0] = sw_r32(RTL838X_TBL_ACCESS_L2_DATA(0));
-	r[1] = sw_r32(RTL838X_TBL_ACCESS_L2_DATA(1));
-	r[2] = sw_r32(RTL838X_TBL_ACCESS_L2_DATA(2));
-
-	e->mac[0] = (r[1] >> 20);
-	e->mac[1] = (r[1] >> 12);
-	e->mac[2] = (r[1] >> 4);
-	e->mac[3] = (r[1] & 0xf) << 4 | (r[2] >> 28);
-	e->mac[4] = (r[2] >> 20);
-	e->mac[5] = (r[2] >> 12);
-	e->is_static = !!((r[0] >> 19) & 1);
-	e->vid = r[0] & 0xfff;
-	e->rvid = r[2] & 0xfff;
-	e->port = (r[0] >> 12) & 0x1f;
-
-	e->valid = true;
-	if (!(r[0] >> 17)) /* Check for invalid entry */
-		e->valid = false;
-
-	if (e->valid)
-		pr_debug("Found in Hash: R1 %x R2 %x R3 %x\n", r[0], r[1], r[2]);
+	rtl_table_read(q, idx);
+	for (i= 0; i < 3; i++)
+		r[i] = sw_r32(rtl_table_data(q, i));
+
+	rtl_table_release(q);
+
+	rtl838x_fill_l2_entry(r, e);
+	if (!e->valid)
+		return 0;
 
 	entry = (((u64) r[1]) << 32) | (r[2] & 0xfffff000) | (r[0] & 0xfff);
 	return entry;
 }
 
+static void rtl838x_write_l2_entry_using_hash(u32 hash, u32 pos, struct rtl838x_l2_entry *e)
+{
+	u32 r[3];
+	struct table_reg *q = rtl_table_get(RTL8380_TBL_L2, 0);
+	int i;
+
+	u32 idx = (0 << 14) | (hash << 2) | pos; // Access SRAM, with hash and at pos in bucket
+
+	rtl838x_fill_l2_row(r, e);
+
+	for (i= 0; i < 3; i++)
+		sw_w32(r[i], rtl_table_data(q, i));
+
+	rtl_table_write(q, idx);
+	rtl_table_release(q);
+}
+
 static u64 rtl838x_read_cam(int idx, struct rtl838x_l2_entry *e)
 {
 	u64 entry;
 	u32 r[3];
+	struct table_reg *q = rtl_table_get(RTL8380_TBL_L2, 1); // Access L2 Table 1
+	int i;
+
+	rtl_table_read(q, idx);
+	for (i= 0; i < 3; i++)
+		r[i] = sw_r32(rtl_table_data(q, i));
+
+	rtl_table_release(q);
 
-	u32 cmd = BIT(16) /* Execute cmd */
-		| BIT(15) /* Read */
-		| BIT(13) /* Table type 0b01 */
-		| (idx & 0x3f);
-	sw_w32(cmd, RTL838X_TBL_ACCESS_L2_CTRL);
-	do { }  while (sw_r32(RTL838X_TBL_ACCESS_L2_CTRL) & BIT(16));
-	r[0] = sw_r32(RTL838X_TBL_ACCESS_L2_DATA(0));
-	r[1] = sw_r32(RTL838X_TBL_ACCESS_L2_DATA(1));
-	r[2] = sw_r32(RTL838X_TBL_ACCESS_L2_DATA(2));
-
-	e->mac[0] = (r[1] >> 20);
-	e->mac[1] = (r[1] >> 12);
-	e->mac[2] = (r[1] >> 4);
-	e->mac[3] = (r[1] & 0xf) << 4 | (r[2] >> 28);
-	e->mac[4] = (r[2] >> 20);
-	e->mac[5] = (r[2] >> 12);
-	e->is_static = !!((r[0] >> 19) & 1);
-	e->vid = r[0] & 0xfff;
-	e->rvid = r[2] & 0xfff;
-	e->port = (r[0] >> 12) & 0x1f;
-
-	e->valid = true;
-	if (!(r[0] >> 17)) /* Check for invalid entry */
-		e->valid = false;
-
-	if (e->valid)
-		pr_debug("Found in CAM: R1 %x R2 %x R3 %x\n", r[0], r[1], r[2]);
+	rtl838x_fill_l2_entry(r, e);
+	if (!e->valid)
+		return 0;
 
+	pr_debug("Found in CAM: R1 %x R2 %x R3 %x\n", r[0], r[1], r[2]);
+
+	// Return MAC with concatenated VID ac concatenated ID
 	entry = (((u64) r[1]) << 32) | (r[2] & 0xfffff000) | (r[0] & 0xfff);
 	return entry;
 }
 
+static void rtl838x_write_cam(int idx, struct rtl838x_l2_entry *e)
+{
+	u32 r[3];
+	struct table_reg *q = rtl_table_get(RTL8380_TBL_L2, 1); // Access L2 Table 1
+	int i;
+
+	rtl838x_fill_l2_row(r, e);
+
+	for (i= 0; i < 3; i++)
+		sw_w32(r[i], rtl_table_data(q, i));
+
+	rtl_table_write(q, idx);
+	rtl_table_release(q);
+}
+
 static u64 rtl838x_read_mcast_pmask(int idx)
 {
 	u32 portmask;
@@ -431,7 +583,9 @@ const struct rtl838x_reg rtl838x_reg = {
 	.mac_rx_pause_sts = RTL838X_MAC_RX_PAUSE_STS,
 	.mac_tx_pause_sts = RTL838X_MAC_TX_PAUSE_STS,
 	.read_l2_entry_using_hash = rtl838x_read_l2_entry_using_hash,
+	.write_l2_entry_using_hash = rtl838x_write_l2_entry_using_hash,
 	.read_cam = rtl838x_read_cam,
+	.write_cam = rtl838x_write_cam,
 	.vlan_port_egr_filter = RTL838X_VLAN_PORT_EGR_FLTR,
 	.vlan_port_igr_filter = RTL838X_VLAN_PORT_IGR_FLTR(0),
 	.vlan_port_pb = RTL838X_VLAN_PORT_PB_VLAN,
@@ -442,6 +596,8 @@ const struct rtl838x_reg rtl838x_reg = {
 	.init_eee = rtl838x_init_eee,
 	.port_eee_set = rtl838x_port_eee_set,
 	.eee_port_ability = rtl838x_eee_port_ability,
+	.l2_hash_seed = rtl838x_l2_hash_seed, 
+	.l2_hash_key = rtl838x_l2_hash_key,
 	.read_mcast_pmask = rtl838x_read_mcast_pmask,
 	.write_mcast_pmask = rtl838x_write_mcast_pmask,
 };
@@ -659,34 +815,6 @@ void rtl8380_get_version(struct rtl838x_switch_priv *priv)
 	}
 }
 
-/*
- * Applies the same hash algorithm as the one used currently by the ASIC
- */
-u32 rtl838x_hash(struct rtl838x_switch_priv *priv, u64 seed)
-{
-	u32 h1, h2, h3, h;
-
-	if (sw_r32(priv->r->l2_ctrl_0) & 1) {
-		h1 = (seed >> 11) & 0x7ff;
-		h1 = ((h1 & 0x1f) << 6) | ((h1 >> 5) & 0x3f);
-
-		h2 = (seed >> 33) & 0x7ff;
-		h2 = ((h2 & 0x3f) << 5) | ((h2 >> 6) & 0x1f);
-
-		h3 = (seed >> 44) & 0x7ff;
-		h3 = ((h3 & 0x7f) << 4) | ((h3 >> 7) & 0xf);
-
-		h = h1 ^ h2 ^ h3 ^ ((seed >> 55) & 0x1ff);
-		h ^= ((seed >> 22) & 0x7ff) ^ (seed & 0x7ff);
-	} else {
-		h = ((seed >> 55) & 0x1ff) ^ ((seed >> 44) & 0x7ff)
-			^ ((seed >> 33) & 0x7ff) ^ ((seed >> 22) & 0x7ff)
-			^ ((seed >> 11) & 0x7ff) ^ (seed & 0x7ff);
-	}
-
-	return h;
-}
-
 void rtl838x_vlan_profile_dump(int profile)
 {
 	u32 p;
diff --git a/target/linux/realtek/files-5.4/drivers/net/dsa/rtl83xx/rtl838x.h b/target/linux/realtek/files-5.4/drivers/net/dsa/rtl83xx/rtl838x.h
index 5990b6453a..b2097363b9 100644
--- a/target/linux/realtek/files-5.4/drivers/net/dsa/rtl83xx/rtl838x.h
+++ b/target/linux/realtek/files-5.4/drivers/net/dsa/rtl83xx/rtl838x.h
@@ -341,6 +341,7 @@
 #define MAX_VLANS 4096
 #define MAX_LAGS 16
 #define MAX_PRIOS 8
+#define RTL930X_PORT_IGNORE 0x3f
 #define MAX_MC_GROUPS 512
 #define UNKNOWN_MC_PMASK (MAX_MC_GROUPS - 1)
 
@@ -398,8 +399,27 @@ struct rtl838x_l2_entry {
 	bool next_hop;
 	int age;
 	u8 trunk;
-	u8 stackDev;
+	bool is_trunk;
+	u8 stack_dev;
 	u16 mc_portmask_index;
+	u32 mc_gip;
+	u32 mc_sip;
+	u16 mc_mac_index;
+	u16 nh_route_id;
+	bool nh_vlan_target;  // Only RTL83xx: VLAN used for next hop
+};
+
+struct rtl838x_nexthop {
+	u16 id;		// ID in HW Nexthop table
+	u32 ip;		// IP Addres of nexthop
+	u32 dev_id;
+	u16 port;
+	u16 vid;
+	u16 fid;
+	u64 mac;
+	u16 mac_id;
+	u16 l2_id;	// Index of this next hop forwarding entry in L2 FIB table
+	u16 if_id;
 };
 
 struct rtl838x_switch_priv;
@@ -451,7 +471,9 @@ struct rtl838x_reg {
 	int mac_rx_pause_sts;
 	int mac_tx_pause_sts;
 	u64 (*read_l2_entry_using_hash)(u32 hash, u32 position, struct rtl838x_l2_entry *e);
+	void (*write_l2_entry_using_hash)(u32 hash, u32 pos, struct rtl838x_l2_entry *e);
 	u64 (*read_cam)(int idx, struct rtl838x_l2_entry *e);
+	void (*write_cam)(int idx, struct rtl838x_l2_entry *e);
 	int vlan_port_egr_filter;
 	int vlan_port_igr_filter;
 	int vlan_port_pb;
@@ -464,6 +486,8 @@ struct rtl838x_reg {
 	void (*port_eee_set)(struct rtl838x_switch_priv *priv, int port, bool enable);
 	int (*eee_port_ability)(struct rtl838x_switch_priv *priv,
 				struct ethtool_eee *e, int port);
+	u64 (*l2_hash_seed)(u64 mac, u32 vid);
+	u32 (*l2_hash_key)(struct rtl838x_switch_priv *priv, u64 seed);
 	u64 (*read_mcast_pmask)(int idx);
 	void (*write_mcast_pmask)(int idx, u64 portmask);
 	void (*vlan_fwd_on_inner)(int port, bool is_set);
@@ -494,6 +518,7 @@ struct rtl838x_switch_priv {
 	struct net_device *lag_devs[MAX_LAGS];
 	struct notifier_block nb;
 	bool eee_enabled;
+	unsigned long int mc_group_bm[MAX_MC_GROUPS >> 5];
 };
 
 void rtl838x_dbgfs_init(struct rtl838x_switch_priv *priv);
diff --git a/target/linux/realtek/files-5.4/drivers/net/dsa/rtl83xx/rtl839x.c b/target/linux/realtek/files-5.4/drivers/net/dsa/rtl83xx/rtl839x.c
index bf2e3d3980..74472461a1 100644
--- a/target/linux/realtek/files-5.4/drivers/net/dsa/rtl83xx/rtl839x.c
+++ b/target/linux/realtek/files-5.4/drivers/net/dsa/rtl83xx/rtl839x.c
@@ -125,6 +125,45 @@ static void rtl839x_vlan_fwd_on_inner(int port, bool is_set)
 		rtl839x_mask_port_reg_be(0ULL, BIT_ULL(port), RTL839X_VLAN_PORT_FWD);
 }
 
+/*
+ * Hash seed is vid (actually rvid) concatenated with the MAC address
+ */
+static u64 rtl839x_l2_hash_seed(u64 mac, u32 vid)
+{
+	u64 v = vid;
+
+	v <<= 48;
+	v |= mac;
+
+	return v;
+}
+
+/*
+ * Applies the same hash algorithm as the one used currently by the ASIC to the seed
+ * and returns a key into the L2 hash table
+ */
+static u32 rtl839x_l2_hash_key(struct rtl838x_switch_priv *priv, u64 seed)
+{
+	u32 h1, h2, h;
+
+	if (sw_r32(priv->r->l2_ctrl_0) & 1) {
+		h1 = (u32) (((seed >> 60) & 0x3f) ^ ((seed >> 54) & 0x3f)
+				^ ((seed >> 36) & 0x3f) ^ ((seed >> 30) & 0x3f)
+				^ ((seed >> 12) & 0x3f) ^ ((seed >> 6) & 0x3f));
+		h2 = (u32) (((seed >> 48) & 0x3f) ^ ((seed >> 42) & 0x3f)
+				^ ((seed >> 24) & 0x3f) ^ ((seed >> 18) & 0x3f)
+				^ (seed & 0x3f));
+		h = (h1 << 6) | h2;
+	} else {
+		h = (seed >> 60)
+			^ ((((seed >> 48) & 0x3f) << 6) | ((seed >> 54) & 0x3f))
+			^ ((seed >> 36) & 0xfff) ^ ((seed >> 24) & 0xfff)
+			^ ((seed >> 12) & 0xfff) ^ (seed & 0xfff);
+	}
+
+	return h;
+}
+
 static inline int rtl839x_mac_force_mode_ctrl(int p)
 {
 	return RTL839X_MAC_FORCE_MODE_CTRL + (p << 2);
@@ -205,55 +244,131 @@ static void rtl839x_fill_l2_entry(u32 r[], struct rtl838x_l2_entry *e)
 	}
 }
 
-static u64 rtl839x_read_l2_entry_using_hash(u32 hash, u32 position, struct rtl838x_l2_entry *e)
+/*
+ * Fills the 3 SoC table registers r[] with the information of in the rtl838x_l2_entry
+ */
+static void rtl839x_fill_l2_row(u32 r[], struct rtl838x_l2_entry *e)
 {
-	u64 entry;
-	u32 r[3];
+	if (!e->valid) {
+		r[0] = r[1] = r[2] = 0;
+		return;
+	}
+
+	r[2] = e->is_ip_mc ? BIT(31) : 0;
+	r[2] |= e->is_ipv6_mc ? BIT(30) : 0;
+
+	if (!e->is_ip_mc  && !e->is_ipv6_mc) {
+		r[0] = ((u32)e->mac[0]) << 12;
+		r[0] |= ((u32)e->mac[1]) << 4;
+		r[0] |= ((u32)e->mac[2]) >> 4;
+		r[1] = ((u32)e->mac[2]) << 28;
+		r[1] |= ((u32)e->mac[3]) << 20;
+		r[1] |= ((u32)e->mac[4]) << 12;
+		r[1] |= ((u32)e->mac[5]) << 4;
+
+		if (!(e->mac[0] & 1)) { // Not multicast
+			r[2] |= e->is_static ? BIT(18) : 0;
+			r[2] |= e->vid << 4;
+			r[0] |= ((u32)e->rvid) << 20;
+			r[2] |= e->port << 24;
+			r[2] |= e->block_da ? BIT(19) : 0;
+			r[2] |= e->block_sa ? BIT(20) : 0;
+			r[2] |= e->suspended ? BIT(17) : 0;
+			if (e->next_hop) {
+				r[2] |= BIT(16);
+				r[2] |= e->nh_vlan_target ? BIT(15) : 0;
+				r[2] |= (e->nh_route_id & 0x7ff) << 4;
+			}
+			r[2] |= ((u32)e->age) << 21;
+		} else {  // L2 Multicast
+			r[0] |= ((u32)e->rvid) << 20;
+			r[2] |= ((u32)e->mc_portmask_index) << 6;
+			pr_debug("Write L2 MC entry: %08x %08x %08x\n", r[0], r[1], r[2]);
+		}
+	} else { // IPv4 or IPv6 MC entry
+		r[0] = ((u32)e->rvid) << 20;
+		r[2] |= ((u32)e->mc_portmask_index) << 6;
+		r[1] = e->mc_gip;
+	}
+}
 
-	/* Search in SRAM, with hash and at position in hash bucket (0-3) */
-	u32 idx = (0 << 14) | (hash << 2) | position;
+/*
+ * Read an L2 UC or MC entry out of a hash bucket of the L2 forwarding table
+ * hash is the id of the bucket and pos is the position of the entry in that bucket
+ * The data read from the SoC is filled into rtl838x_l2_entry
+ */
+static u64 rtl839x_read_l2_entry_using_hash(u32 hash, u32 pos, struct rtl838x_l2_entry *e)
+{
+	u32 r[3];
+	struct table_reg *q = rtl_table_get(RTL8390_TBL_L2, 0);
+	u32 idx = (0 << 14) | (hash << 2) | pos; // Search SRAM, with hash and at pos in bucket
+	int i;
 
-	u32 cmd = 1 << 17 /* Execute cmd */
-		| 0 << 16 /* Read */
-		| 0 << 14 /* Table type 0b00 */
-		| (idx & 0x3fff);
+	rtl_table_read(q, idx);
+	for (i= 0; i < 3; i++)
+		r[i] = sw_r32(rtl_table_data(q, i));
 
-	sw_w32(cmd, RTL839X_TBL_ACCESS_L2_CTRL);
-	do { }  while (sw_r32(RTL839X_TBL_ACCESS_L2_CTRL) & (1 << 17));
-	r[0] = sw_r32(RTL839X_TBL_ACCESS_L2_DATA(0));
-	r[1] = sw_r32(RTL839X_TBL_ACCESS_L2_DATA(1));
-	r[2] = sw_r32(RTL839X_TBL_ACCESS_L2_DATA(2));
+	rtl_table_release(q);
 
 	rtl839x_fill_l2_entry(r, e);
+	if (!e->valid)
+		return 0;
 
-	entry = (((u64) r[0]) << 12) | ((r[1] & 0xfffffff0) << 12) | ((r[2] >> 4) & 0xfff);
-	return entry;
+	return rtl839x_l2_hash_seed(ether_addr_to_u64(&e->mac[0]), e->rvid);
+}
+
+static void rtl839x_write_l2_entry_using_hash(u32 hash, u32 pos, struct rtl838x_l2_entry *e)
+{
+	u32 r[3];
+	struct table_reg *q = rtl_table_get(RTL8390_TBL_L2, 0);
+	int i;
+
+	u32 idx = (0 << 14) | (hash << 2) | pos; // Access SRAM, with hash and at pos in bucket
+
+	rtl839x_fill_l2_row(r, e);
+
+	for (i= 0; i < 3; i++)
+		sw_w32(r[i], rtl_table_data(q, i));
+
+	rtl_table_write(q, idx);
+	rtl_table_release(q);
 }
 
 static u64 rtl839x_read_cam(int idx, struct rtl838x_l2_entry *e)
 {
-	u64 entry;
 	u32 r[3];
+	struct table_reg *q = rtl_table_get(RTL8390_TBL_L2, 1); // Access L2 Table 1
+	int i;
 
-	u32 cmd = 1 << 17 /* Execute cmd */
-		| 0 << 16 /* Read */
-		| 1 << 14 /* Table type 0b01 */
-		| (idx & 0x3f);
-	sw_w32(cmd, RTL839X_TBL_ACCESS_L2_CTRL);
-	do { }  while (sw_r32(RTL839X_TBL_ACCESS_L2_CTRL) & (1 << 17));
-	r[0] = sw_r32(RTL839X_TBL_ACCESS_L2_DATA(0));
-	r[1] = sw_r32(RTL839X_TBL_ACCESS_L2_DATA(1));
-	r[2] = sw_r32(RTL839X_TBL_ACCESS_L2_DATA(2));
+	rtl_table_read(q, idx);
+	for (i= 0; i < 3; i++)
+		r[i] = sw_r32(rtl_table_data(q, i));
 
+	rtl_table_release(q);
 
 	rtl839x_fill_l2_entry(r, e);
-	if (e->valid)
-		pr_info("Found in CAM: R1 %x R2 %x R3 %x\n", r[0], r[1], r[2]);
-	else
+	if (!e->valid)
 		return 0;
 
-	entry = (((u64) r[0]) << 12) | ((r[1] & 0xfffffff0) << 12) | ((r[2] >> 4) & 0xfff);
-	return entry;
+	pr_debug("Found in CAM: R1 %x R2 %x R3 %x\n", r[0], r[1], r[2]);
+
+	// Return MAC with concatenated VID ac concatenated ID
+	return rtl839x_l2_hash_seed(ether_addr_to_u64(&e->mac[0]), e->rvid);
+}
+
+static void rtl839x_write_cam(int idx, struct rtl838x_l2_entry *e)
+{
+	u32 r[3];
+	struct table_reg *q = rtl_table_get(RTL8390_TBL_L2, 1); // Access L2 Table 1
+	int i;
+
+	rtl839x_fill_l2_row(r, e);
+
+	for (i= 0; i < 3; i++)
+		sw_w32(r[i], rtl_table_data(q, i));
+
+	rtl_table_write(q, idx);
+	rtl_table_release(q);
 }
 
 static u64 rtl839x_read_mcast_pmask(int idx)
@@ -326,7 +441,7 @@ void rtl839x_traffic_enable(int source, int dest)
 
 void rtl839x_traffic_disable(int source, int dest)
 {
-	rtl839x_mask_port_reg_be(BIT(dest), 0, rtl839x_port_iso_ctrl(source));
+	rtl839x_mask_port_reg_be(BIT_ULL(dest), 0, rtl839x_port_iso_ctrl(source));
 }
 
 irqreturn_t rtl839x_switch_irq(int irq, void *dev_id)
@@ -341,10 +456,10 @@ irqreturn_t rtl839x_switch_irq(int irq, void *dev_id)
 	rtl839x_set_port_reg_le(ports, RTL839X_ISR_PORT_LINK_STS_CHG);
 	pr_debug("RTL8390 Link change: status: %x, ports %llx\n", status, ports);
 
-	for (i = 0; i < 52; i++) {
-		if (ports & (1ULL << i)) {
+	for (i = 0; i < RTL839X_CPU_PORT; i++) {
+		if (ports & BIT_ULL(i)) {
 			link = rtl839x_get_port_reg_le(RTL839X_MAC_LINK_STS);
-			if (link & (1ULL << i))
+			if (link & BIT_ULL(i))
 				dsa_port_phylink_mac_change(ds, i, true);
 			else
 				dsa_port_phylink_mac_change(ds, i, false);
@@ -372,7 +487,6 @@ int rtl8390_sds_power(int mac, int val)
 	return 0;
 }
 
-
 int rtl839x_read_phy(u32 port, u32 page, u32 reg, u32 *val)
 {
 	u32 v;
@@ -506,28 +620,6 @@ void rtl8390_get_version(struct rtl838x_switch_priv *priv)
 	priv->version = RTL8390_VERSION_A;
 }
 
-u32 rtl839x_hash(struct rtl838x_switch_priv *priv, u64 seed)
-{
-	u32 h1, h2, h;
-
-	if (sw_r32(priv->r->l2_ctrl_0) & 1) {
-		h1 = (u32) (((seed >> 60) & 0x3f) ^ ((seed >> 54) & 0x3f)
-				^ ((seed >> 36) & 0x3f) ^ ((seed >> 30) & 0x3f)
-				^ ((seed >> 12) & 0x3f) ^ ((seed >> 6) & 0x3f));
-		h2 = (u32) (((seed >> 48) & 0x3f) ^ ((seed >> 42) & 0x3f)
-				^ ((seed >> 24) & 0x3f) ^ ((seed >> 18) & 0x3f)
-				^ (seed & 0x3f));
-		h = (h1 << 6) | h2;
-	} else {
-		h = (seed >> 60)
-			^ ((((seed >> 48) & 0x3f) << 6) | ((seed >> 54) & 0x3f))
-			^ ((seed >> 36) & 0xfff) ^ ((seed >> 24) & 0xfff)
-			^ ((seed >> 12) & 0xfff) ^ (seed & 0xfff);
-	}
-
-	return h;
-}
-
 void rtl839x_vlan_profile_dump(int profile)
 {
 	u32 p[2];
@@ -695,7 +787,9 @@ const struct rtl838x_reg rtl839x_reg = {
 	.mac_rx_pause_sts = RTL839X_MAC_RX_PAUSE_STS,
 	.mac_tx_pause_sts = RTL839X_MAC_TX_PAUSE_STS,
 	.read_l2_entry_using_hash = rtl839x_read_l2_entry_using_hash,
+	.write_l2_entry_using_hash = rtl839x_write_l2_entry_using_hash,
 	.read_cam = rtl839x_read_cam,
+	.write_cam = rtl839x_write_cam,
 	.vlan_port_egr_filter = RTL839X_VLAN_PORT_EGR_FLTR(0),
 	.vlan_port_igr_filter = RTL839X_VLAN_PORT_IGR_FLTR(0),
 	.vlan_port_pb = RTL839X_VLAN_PORT_PB_VLAN,
@@ -706,6 +800,8 @@ const struct rtl838x_reg rtl839x_reg = {
 	.init_eee = rtl839x_init_eee,
 	.port_eee_set = rtl839x_port_eee_set,
 	.eee_port_ability = rtl839x_eee_port_ability,
+	.l2_hash_seed = rtl839x_l2_hash_seed, 
+	.l2_hash_key = rtl839x_l2_hash_key,
 	.read_mcast_pmask = rtl839x_read_mcast_pmask,
 	.write_mcast_pmask = rtl839x_write_mcast_pmask,
 };
diff --git a/target/linux/realtek/files-5.4/drivers/net/dsa/rtl83xx/rtl930x.c b/target/linux/realtek/files-5.4/drivers/net/dsa/rtl83xx/rtl930x.c
index a80a6d89b2..820c78165a 100644
--- a/target/linux/realtek/files-5.4/drivers/net/dsa/rtl83xx/rtl930x.c
+++ b/target/linux/realtek/files-5.4/drivers/net/dsa/rtl83xx/rtl930x.c
@@ -207,12 +207,74 @@ static inline int rtl930x_mac_link_spd_sts(int p)
 	return RTL930X_MAC_LINK_SPD_STS(p);
 }
 
+static u64 rtl930x_l2_hash_seed(u64 mac, u32 vid)
+{
+	u64 v = vid;
+
+	v <<= 48;
+	v |= mac;
+
+	return v;
+}
+
+/*
+ * Calculate both the block 0 and the block 1 hash by applyingthe same hash
+ * algorithm as the one used currently by the ASIC to the seed, and return
+ * both hashes in the lower and higher word of the return value since only 12 bit of
+ * the hash are significant
+ */
+static u32 rtl930x_l2_hash_key(struct rtl838x_switch_priv *priv, u64 seed)
+{
+	u32 k0, k1, h1, h2, h;
+
+	k0 = (u32) (((seed >> 55) & 0x1f) ^ ((seed >> 44) & 0x7ff)
+		^ ((seed >> 33) & 0x7ff) ^ ((seed >> 22) & 0x7ff)
+		^ ((seed >> 11) & 0x7ff) ^ (seed & 0x7ff));
+
+	h1 = (seed >> 11) & 0x7ff;
+	h1 = ((h1 & 0x1f) << 6) | ((h1 >> 5) & 0x3f);
+
+	h2 = (seed >> 33) & 0x7ff;
+	h2 = ((h2 & 0x3f) << 5)| ((h2 >> 6) & 0x3f);
+
+	k1 = (u32) (((seed << 55) & 0x1f) ^ ((seed >> 44) & 0x7ff) ^ h2
+		    ^ ((seed >> 22) & 0x7ff) ^ h1
+		    ^ (seed & 0x7ff));
+
+	// Algorithm choice for block 0
+	if (sw_r32(RTL930X_L2_CTRL) & BIT(0))
+		h = k1;
+	else
+		h = k0;
+
+	/* Algorithm choice for block 1
+	 * Since k0 and k1 are < 2048, adding 2048 will offset the hash into the second
+	 * half of hash-space
+	 * 2048 is in fact the hash-table size 16384 divided by 4 hashes per bucket
+	 * divided by 2 to divide the hash space in 2
+	 */
+	if (sw_r32(RTL930X_L2_CTRL) & BIT(1))
+		h |= (k1 + 2048) << 16;
+	else
+		h |= (k0 + 2048) << 16;
+
+	return h;
+}
+
+/*
+ * Fills an L2 entry structure from the SoC registers
+ */
 static void rtl930x_fill_l2_entry(u32 r[], struct rtl838x_l2_entry *e)
 {
+	pr_debug("In %s valid?\n", __func__);
 	e->valid = !!(r[2] & BIT(31));
 	if (!e->valid)
 		return;
 
+	pr_debug("In %s is valid\n", __func__);
+	e->is_ip_mc = false;
+	e->is_ipv6_mc = false;
+
 	// TODO: Is there not a function to copy directly MAC memory?
 	e->mac[0] = (r[0] >> 24);
 	e->mac[1] = (r[0] >> 16);
@@ -221,61 +283,164 @@ static void rtl930x_fill_l2_entry(u32 r[], struct rtl838x_l2_entry *e)
 	e->mac[4] = (r[1] >> 24);
 	e->mac[5] = (r[1] >> 16);
 
+	e->next_hop = !!(r[2] & BIT(12));
+	e->rvid = r[1] & 0xfff;
+
 	/* Is it a unicast entry? check multicast bit */
 	if (!(e->mac[0] & 1)) {
 		e->type = L2_UNICAST;
 		e->is_static = !!(r[2] & BIT(14));
-		e->vid = r[2] & 0xfff;
-		e->rvid = r[1] & 0xfff;
 		e->port = (r[2] >> 20) & 0x3ff;
 		// Check for trunk port
 		if (r[2] & BIT(30)) {
-			e->stackDev = (e->port >> 9) & 1;
+			e->is_trunk = true;
+			e->stack_dev = (e->port >> 9) & 1;
 			e->trunk = e->port & 0x3f;
 		} else {
-			e->stackDev = (e->port >> 6) & 0xf;
+			e->is_trunk = false;
+			e->stack_dev = (e->port >> 6) & 0xf;
 			e->port = e->port & 0x3f;
 		}
 
 		e->block_da = !!(r[2] & BIT(15));
 		e->block_sa = !!(r[2] & BIT(16));
 		e->suspended = !!(r[2] & BIT(13));
-		e->next_hop = !!(r[2] & BIT(12));
 		e->age = (r[2] >> 17) & 3;
 		e->valid = true;
-
+		// the UC_VID field in hardware is used for the VID or for the route id
+		if (e->next_hop) {
+			e->nh_route_id = r[2] & 0xfff;
+			e->vid = 0;
+		} else {
+			e->vid = r[2] & 0xfff;
+			e->nh_route_id = 0;
+		}
 	} else {
 		e->valid = true;
 		e->type = L2_MULTICAST;
-		e->mc_portmask_index = (r[2]>>6) & 0xfff;
+		e->mc_portmask_index = (r[2] >> 16) & 0x3ff;
 	}
 }
 
-static u64 rtl930x_read_l2_entry_using_hash(u32 hash, u32 position, struct rtl838x_l2_entry *e)
+/*
+ * Fills the 3 SoC table registers r[] with the information of in the rtl838x_l2_entry
+ */
+static void rtl930x_fill_l2_row(u32 r[], struct rtl838x_l2_entry *e)
+{
+	u32 port;
+
+	if (!e->valid) {
+		r[0] = r[1] = r[2] = 0;
+		return;
+	}
+
+	r[2] = BIT(31);	// Set valid bit
+
+	r[0] = ((u32)e->mac[0]) << 24 | ((u32)e->mac[1]) << 16 
+		| ((u32)e->mac[2]) << 8 | ((u32)e->mac[3]);
+	r[1] = ((u32)e->mac[4]) << 24 | ((u32)e->mac[5]) << 16;
+
+	r[2] |= e->next_hop ? BIT(12) : 0;
+
+	if (e->type == L2_UNICAST) {
+		r[2] |= e->is_static ? BIT(14) : 0;
+		r[1] |= e->rvid & 0xfff;
+		r[2] |= (e->port & 0x3ff) << 20;
+		if (e->is_trunk) {
+			r[2] |= BIT(30);
+			port = e->stack_dev << 9 | (e->port & 0x3f);
+		} else {
+			port = (e->stack_dev & 0xf) << 6;
+			port |= e->port & 0x3f;
+		}
+		r[2] |= port << 20;
+		r[2] |= e->block_da ? BIT(15) : 0;
+		r[2] |= e->block_sa ? BIT(17) : 0;
+		r[2] |= e->suspended ? BIT(13) : 0;
+		r[2] |= (e->age & 0x3) << 17;
+		// the UC_VID field in hardware is used for the VID or for the route id
+		if (e->next_hop)
+			r[2] |= e->nh_route_id & 0xfff;
+		else
+			r[2] |= e->vid & 0xfff;
+	} else { // L2_MULTICAST
+		r[2] |= (e->mc_portmask_index & 0x3ff) << 16;
+		r[2] |= e->mc_mac_index & 0x7ff;
+	}
+}
+
+/*
+ * Read an L2 UC or MC entry out of a hash bucket of the L2 forwarding table
+ * hash is the id of the bucket and pos is the position of the entry in that bucket
+ * The data read from the SoC is filled into rtl838x_l2_entry
+ */
+static u64 rtl930x_read_l2_entry_using_hash(u32 hash, u32 pos, struct rtl838x_l2_entry *e)
 {
-	u64 entry;
 	u32 r[3];
 	struct table_reg *q = rtl_table_get(RTL9300_TBL_L2, 0);
-	u32 idx = (0 << 14) | (hash << 2) | position;
+	u32 idx;
 	int i;
+	u64 mac;
+	u64 seed;
+
+	pr_debug("%s: hash %08x, pos: %d\n", __func__, hash, pos);
+
+	/* On the RTL93xx, 2 different hash algorithms are used making it a total of
+	 * 8 buckets that need to be searched, 4 for each hash-half
+	 * Use second hash space when bucket is between 4 and 8 */
+	if (pos >= 4) {
+		pos -= 4;
+		hash >>= 16;
+	} else {
+		hash &= 0xffff;
+	}
+
+	idx = (0 << 14) | (hash << 2) | pos; // Search SRAM, with hash and at pos in bucket
+	pr_debug("%s: NOW hash %08x, pos: %d\n", __func__, hash, pos);
 
 	rtl_table_read(q, idx);
-	for (i= 0; i < 3; i++)
+	for (i = 0; i < 3; i++)
 		r[i] = sw_r32(rtl_table_data(q, i));
 
 	rtl_table_release(q);
 
 	rtl930x_fill_l2_entry(r, e);
+
+	pr_debug("%s: valid: %d, nh: %d\n", __func__, e->valid, e->next_hop);
 	if (!e->valid)
 		return 0;
 
-	entry = ((u64)r[0] << 32) | (r[1] & 0xffff0000) | e->vid;
-	return entry;
+	mac = ((u64)e->mac[0]) << 40 | ((u64)e->mac[1]) << 32 | ((u64)e->mac[2]) << 24
+		| ((u64)e->mac[3]) << 16 | ((u64)e->mac[4]) << 8 | ((u64)e->mac[5]);
+
+	seed = rtl930x_l2_hash_seed(mac, e->rvid);
+	pr_debug("%s: mac %016llx, seed %016llx\n", __func__, mac, seed);
+	// return vid with concatenated mac as unique id
+	return seed;
+}
+
+static void rtl930x_write_l2_entry_using_hash(u32 hash, u32 pos, struct rtl838x_l2_entry *e)
+{
+	u32 r[3];
+	struct table_reg *q = rtl_table_get(RTL9300_TBL_L2, 0);
+	u32 idx = (0 << 14) | (hash << 2) | pos; // Access SRAM, with hash and at pos in bucket
+	int i;
+
+	pr_info("%s: hash %d, pos %d\n", __func__, hash, pos);
+	pr_info("%s: index %d -> mac %02x:%02x:%02x:%02x:%02x:%02x\n", __func__, idx,
+		e->mac[0], e->mac[1], e->mac[2], e->mac[3],e->mac[4],e->mac[5]);
+
+	rtl930x_fill_l2_row(r, e);
+
+	for (i= 0; i < 3; i++)
+		sw_w32(r[i], rtl_table_data(q, i));
+
+	rtl_table_write(q, idx);
+	rtl_table_release(q);
 }
 
 static u64 rtl930x_read_cam(int idx, struct rtl838x_l2_entry *e)
 {
-	u64 entry;
 	u32 r[3];
 	struct table_reg *q = rtl_table_get(RTL9300_TBL_L2, 1);
 	int i;
@@ -290,9 +455,113 @@ static u64 rtl930x_read_cam(int idx, struct rtl838x_l2_entry *e)
 	if (!e->valid)
 		return 0;
 
-	entry = ((u64)r[0] << 32) | (r[1] & 0xffff0000) | e->vid;
+	// return mac with concatenated vid as unique id
+	return ((u64)r[0] << 28) | ((r[1] & 0xffff0000) >> 4) | e->vid;
+}
 
-	return entry;
+static void rtl930x_write_cam(int idx, struct rtl838x_l2_entry *e)
+{
+	u32 r[3];
+	struct table_reg *q = rtl_table_get(RTL9300_TBL_L2, 1); // Access L2 Table 1
+	int i;
+
+	rtl930x_fill_l2_row(r, e);
+
+	for (i= 0; i < 3; i++)
+		sw_w32(r[i], rtl_table_data(q, i));
+
+	rtl_table_write(q, idx);
+	rtl_table_release(q);
+}
+
+static void dump_l2_entry(struct rtl838x_l2_entry *e)
+{
+	pr_info("MAC: %02x:%02x:%02x:%02x:%02x:%02x vid: %d, rvid: %d, port: %d, valid: %d\n",
+		e->mac[0], e->mac[1], e->mac[2], e->mac[3], e->mac[4], e->mac[5],
+		e->vid, e->rvid, e->port, e->valid);
+	pr_info("Type: %d, is_static: %d, is_ip_mc: %d, is_ipv6_mc: %d, block_da: %d\n",
+		e->type, e->is_static, e->is_ip_mc, e->is_ipv6_mc, e->block_da);
+	pr_info("  block_sa: %d, suspended: %d, next_hop: %d, age: %d, is_trunk: %d, trunk: %d\n",
+		e->block_sa, e->suspended, e->next_hop, e->age, e->is_trunk, e->trunk);
+	if (e->is_ip_mc || e->is_ipv6_mc)
+		pr_info("  mc_portmask_index: %d, mc_gip: %d, mc_sip: %d\n",
+			e->mc_portmask_index, e->mc_gip, e->mc_sip);
+	pr_info("  stac_dev: %d, nh_route_id: %d, port: %d, dev_id\n",
+		e->stack_dev, e->nh_route_id, e->port);
+}
+
+/*
+ * Add an L2 nexthop entry for the L3 routing system in the SoC
+ * Use VID and MAC in rtl838x_l2_entry to identify either a free slot in the L2 hash table
+ * or mark an existing entry as a nexthop by setting it's nexthop bit
+ * Called from the L3 layer
+ * The index in the L2 hash table is filled into nh->l2_id;
+ */
+static int rtl930x_l2_nexthop_add(struct rtl838x_switch_priv *priv, struct rtl838x_nexthop *nh)
+{
+	struct rtl838x_l2_entry e;
+	u64 seed = rtl930x_l2_hash_seed(nh->mac, nh->vid);
+	u32 key = rtl930x_l2_hash_key(priv, seed);
+	int i, idx = -1;
+	u64 entry;
+
+	pr_info("%s searching for %08llx vid %d with key %d, seed: %016llx\n",
+		__func__, nh->mac, nh->vid, key, seed);
+	
+	e.type = L2_UNICAST;
+	e.rvid = nh->fid; // Verify its the forwarding ID!!! l2_entry.un.unicast.fid
+	u64_to_ether_addr(nh->mac, &e.mac[0]);
+	e.port = RTL930X_PORT_IGNORE;
+
+	// Loop over all entries in the hash-bucket and over the second block on 93xx SoCs
+	for (i = 0; i < priv->l2_bucket_size; i++) {
+		entry = rtl930x_read_l2_entry_using_hash(key, i, &e);
+		pr_info("%s i: %d, entry %016llx, seed %016llx\n", __func__, i, entry, seed);
+		if (e.valid && e.next_hop)
+			continue;
+		if (!e.valid || ((entry & 0x0fffffffffffffffULL) == seed)) {
+			idx = i > 3 ? ((key >> 14) & 0xffff) | i >> 1
+					: ((key << 2) | i) & 0xffff;
+			break;
+		}
+	}
+
+	pr_info("%s: found idx %d and i %d\n", __func__, idx, i);
+
+	if (idx < 0) {
+		pr_err("%s: No more L2 forwarding entries available\n", __func__);
+		return -1;
+	}
+
+	// Found an existing or empty entry, make it a nexthop entry
+	pr_info("%s BEFORE -> key %d, pos: %d, index: %d\n", __func__, key, i, idx);
+	dump_l2_entry(&e);
+	nh->l2_id = idx;
+
+	// Found an existing (e->valid is true) or empty entry, make it a nexthop entry
+	if (e.valid) {
+		nh->port = e.port;
+		nh->fid = e.rvid;
+		nh->vid = e.vid;
+		nh->dev_id = e.stack_dev;
+	} else {
+		e.valid = true;
+		e.is_static = false;
+		e.vid = nh->vid;
+		e.rvid = nh->fid;
+		e.port = RTL930X_PORT_IGNORE;
+		u64_to_ether_addr(nh->mac, &e.mac[0]);
+	}
+	e.next_hop = true;
+	// For nexthop entries, the vid field in the table is used to denote the dest mac_id
+	e.nh_route_id = nh->mac_id;
+	pr_info("%s AFTER\n", __func__);
+	dump_l2_entry(&e);
+
+	rtl930x_write_l2_entry_using_hash(idx >> 2, idx & 0x3, &e);
+
+	// _dal_longan_l2_nexthop_add
+	return 0;
 }
 
 static u64 rtl930x_read_mcast_pmask(int idx)
@@ -485,7 +754,6 @@ int rtl930x_read_phy(u32 port, u32 page, u32 reg, u32 *val)
 	u32 v;
 	int err = 0;
 
-//	pr_info("In %s\n", __func__);
 	if (port > 63 || page > 4095 || reg > 31)
 		return -ENOTSUPP;
 
@@ -531,12 +799,12 @@ int rtl930x_write_mmd_phy(u32 port, u32 devnum, u32 regnum, u32 val)
 	// Set MMD device number and register to write to
 	sw_w32(devnum << 16 | (regnum & 0xffff), RTL930X_SMI_ACCESS_PHY_CTRL_3);
 
-	v = BIT(2)| BIT(1)| BIT(0); // WRITE | MMD-access | EXEC
+	v = BIT(2) | BIT(1) | BIT(0); // WRITE | MMD-access | EXEC
 	sw_w32(v, RTL930X_SMI_ACCESS_PHY_CTRL_1);
 
 	do {
 		v = sw_r32(RTL930X_SMI_ACCESS_PHY_CTRL_1);
-	} while ( v & BIT(0));
+	} while (v & BIT(0));
 
 	pr_debug("%s: port %d, regnum: %x, val: %x (err %d)\n", __func__, port, regnum, val, err);
 	mutex_unlock(&smi_lock);
@@ -559,12 +827,12 @@ int rtl930x_read_mmd_phy(u32 port, u32 devnum, u32 regnum, u32 *val)
 	// Set MMD device number and register to write to
 	sw_w32(devnum << 16 | (regnum & 0xffff), RTL930X_SMI_ACCESS_PHY_CTRL_3);
 
-	v = BIT(1)| BIT(0); // MMD-access | EXEC
+	v = BIT(1) | BIT(0); // MMD-access | EXEC
 	sw_w32(v, RTL930X_SMI_ACCESS_PHY_CTRL_1);
 
 	do {
 		v = sw_r32(RTL930X_SMI_ACCESS_PHY_CTRL_1);
-	} while ( v & 0x1);
+	} while (v & BIT(0));
 	// There is no error-checking via BIT 25 of v, as it does not seem to be set correctly
 	*val = (sw_r32(RTL930X_SMI_ACCESS_PHY_CTRL_2) & 0xffff);
 	pr_debug("%s: port %d, regnum: %x, val: %x (err %d)\n", __func__, port, regnum, *val, err);
@@ -574,7 +842,6 @@ int rtl930x_read_mmd_phy(u32 port, u32 devnum, u32 regnum, u32 *val)
 	return err;
 }
 
-
 /*
  * Calculate both the block 0 and the block 1 hash, and return in
  * lower and higher word of the return value since only 12 bit of
@@ -617,6 +884,7 @@ u32 rtl930x_hash(struct rtl838x_switch_priv *priv, u64 seed)
 
 	return h;
 }
+
 /*
  * Enables or disables the EEE/EEEP capability of a port
  */
@@ -754,7 +1022,9 @@ const struct rtl838x_reg rtl930x_reg = {
 	.mac_rx_pause_sts = RTL930X_MAC_RX_PAUSE_STS,
 	.mac_tx_pause_sts = RTL930X_MAC_TX_PAUSE_STS,
 	.read_l2_entry_using_hash = rtl930x_read_l2_entry_using_hash,
+	.write_l2_entry_using_hash = rtl930x_write_l2_entry_using_hash,
 	.read_cam = rtl930x_read_cam,
+	.write_cam = rtl930x_write_cam,
 	.vlan_port_egr_filter = RTL930X_VLAN_PORT_EGR_FLTR,
 	.vlan_port_igr_filter = RTL930X_VLAN_PORT_IGR_FLTR(0),
 	.vlan_port_pb = RTL930X_VLAN_PORT_PB_VLAN,