[PATCH net-next v3 8/9] net: dsa: lan9645x: add mdb management

["Jens Emil Schulz Østergaard" <jensemil.schulzostergaard@microchip.com>]

Add support for dsa mdb callbacks. L2 multicast and IP multicast is
handled differently. IP multicast stores the port group mask inline in
the mac table. L2 multicast points to a PGID index, which encodes the
port group mask.

Reviewed-by: Steen Hegelund <Steen.Hegelund@microchip.com>
Signed-off-by: Jens Emil Schulz Østergaard <jensemil.schulzostergaard@microchip.com>
---
Changes in v3:
- avoid mdb add/del dealloc when mac table writes fail
- dealloc mdb entries on deinit

Changes in v2:
- New file: selftests required implementation of the mdb callbacks.
---
 drivers/net/dsa/microchip/lan9645x/Makefile        |   1 +
 drivers/net/dsa/microchip/lan9645x/lan9645x_mac.c  |  14 +
 drivers/net/dsa/microchip/lan9645x/lan9645x_main.c |  61 ++++
 drivers/net/dsa/microchip/lan9645x/lan9645x_main.h |  21 ++
 drivers/net/dsa/microchip/lan9645x/lan9645x_mdb.c  | 383 +++++++++++++++++++++
 drivers/net/dsa/microchip/lan9645x/lan9645x_port.c |   8 +
 include/linux/dsa/lan9645x.h                       |  11 +
 net/dsa/tag_lan9645x.c                             |  19 +-
 8 files changed, 516 insertions(+), 2 deletions(-)

diff --git a/drivers/net/dsa/microchip/lan9645x/Makefile b/drivers/net/dsa/microchip/lan9645x/Makefile
index 70815edca5b9..2413d11fe849 100644
--- a/drivers/net/dsa/microchip/lan9645x/Makefile
+++ b/drivers/net/dsa/microchip/lan9645x/Makefile
@@ -4,6 +4,7 @@ obj-$(CONFIG_NET_DSA_MICROCHIP_LAN9645X) += mchp-lan9645x.o
 mchp-lan9645x-objs := \
 	lan9645x_mac.o \
 	lan9645x_main.o \
+	lan9645x_mdb.o \
 	lan9645x_npi.o \
 	lan9645x_phylink.o \
 	lan9645x_port.o \
diff --git a/drivers/net/dsa/microchip/lan9645x/lan9645x_mac.c b/drivers/net/dsa/microchip/lan9645x/lan9645x_mac.c
index f516979225ae..0bd97cc8e42c 100644
--- a/drivers/net/dsa/microchip/lan9645x/lan9645x_mac.c
+++ b/drivers/net/dsa/microchip/lan9645x/lan9645x_mac.c
@@ -215,6 +215,20 @@ int lan9645x_mact_learn(struct lan9645x *lan9645x, int port,
 	return err;
 }
 
+int lan9645x_mact_learn_cpu_copy(struct lan9645x *lan9645x, int port,
+				 const unsigned char *addr, u16 vid,
+				 enum macaccess_entry_type type, bool cpu_copy)
+{
+	int err;
+
+	mutex_lock(&lan9645x->mact_lock);
+	err = __lan9645x_mact_learn_cpu_copy(lan9645x, port, addr, vid, type,
+					     cpu_copy);
+	mutex_unlock(&lan9645x->mact_lock);
+
+	return err;
+}
+
 int lan9645x_mact_flush(struct lan9645x *lan9645x, int port)
 {
 	int err;
diff --git a/drivers/net/dsa/microchip/lan9645x/lan9645x_main.c b/drivers/net/dsa/microchip/lan9645x/lan9645x_main.c
index 764f4d6c0571..8f63729ff55d 100644
--- a/drivers/net/dsa/microchip/lan9645x/lan9645x_main.c
+++ b/drivers/net/dsa/microchip/lan9645x/lan9645x_main.c
@@ -72,6 +72,7 @@ static void lan9645x_teardown(struct dsa_switch *ds)
 	destroy_workqueue(lan9645x->owq);
 	lan9645x_npi_port_deinit(lan9645x, lan9645x->npi);
 	lan9645x_mac_deinit(lan9645x);
+	lan9645x_mdb_deinit(lan9645x);
 	mutex_destroy(&lan9645x->fwd_domain_lock);
 }
 
@@ -161,6 +162,7 @@ static int lan9645x_setup(struct dsa_switch *ds)
 	if (err)
 		return err;
 	lan9645x_mac_init(lan9645x);
+	lan9645x_mdb_init(lan9645x);
 
 	/* Link Aggregation Mode: NETDEV_LAG_HASH_L2 */
 	lan_wr(ANA_AGGR_CFG_AC_SMAC_ENA |
@@ -680,6 +682,61 @@ static int lan9645x_fdb_del(struct dsa_switch *ds, int port,
 	return err;
 }
 
+static int lan9645x_mdb_add(struct dsa_switch *ds, int port,
+			    const struct switchdev_obj_port_mdb *mdb,
+			    struct dsa_db db)
+{
+	struct net_device *bridge_dev = lan9645x_db2bridge(db);
+	struct lan9645x *lan9645x = ds->priv;
+
+	dev_dbg(lan9645x->dev, "port=%d addr=%pM vid=%u\n", port, mdb->addr,
+		mdb->vid);
+
+	if (IS_ERR(bridge_dev))
+		return PTR_ERR(bridge_dev);
+
+	if (dsa_is_cpu_port(ds, port) && !bridge_dev &&
+	    dsa_mdb_present_in_other_db(ds, port, mdb, db))
+		return 0;
+
+	if (port == lan9645x->npi)
+		port = CPU_PORT;
+
+	return lan9645x_mdb_port_add(lan9645x, port, mdb, bridge_dev);
+}
+
+static int lan9645x_mdb_del(struct dsa_switch *ds, int port,
+			    const struct switchdev_obj_port_mdb *mdb,
+			    struct dsa_db db)
+{
+	struct net_device *bridge_dev = lan9645x_db2bridge(db);
+	struct lan9645x *lan9645x = ds->priv;
+	int err;
+
+	dev_dbg(lan9645x->dev, "port=%d addr=%pM vid=%u\n", port, mdb->addr,
+		mdb->vid);
+
+	if (IS_ERR(bridge_dev))
+		return PTR_ERR(bridge_dev);
+
+	if (dsa_is_cpu_port(ds, port) && !bridge_dev &&
+	    dsa_mdb_present_in_other_db(ds, port, mdb, db))
+		return 0;
+
+	if (port == lan9645x->npi)
+		port = CPU_PORT;
+
+	err = lan9645x_mdb_port_del(lan9645x, port, mdb, bridge_dev);
+	if (err == -ENOENT) {
+		dev_dbg(lan9645x->dev,
+			"mdb not found port=%d addr=%pM vid=%u\n", port,
+			mdb->addr, mdb->vid);
+		return 0;
+	}
+
+	return err;
+}
+
 static const struct dsa_switch_ops lan9645x_switch_ops = {
 	.get_tag_protocol		= lan9645x_get_tag_protocol,
 
@@ -713,6 +770,10 @@ static const struct dsa_switch_ops lan9645x_switch_ops = {
 	.port_fdb_dump			= lan9645x_fdb_dump,
 	.port_fdb_add			= lan9645x_fdb_add,
 	.port_fdb_del			= lan9645x_fdb_del,
+
+	/* Multicast database */
+	.port_mdb_add			= lan9645x_mdb_add,
+	.port_mdb_del			= lan9645x_mdb_del,
 };
 
 static int lan9645x_request_target_regmaps(struct lan9645x *lan9645x)
diff --git a/drivers/net/dsa/microchip/lan9645x/lan9645x_main.h b/drivers/net/dsa/microchip/lan9645x/lan9645x_main.h
index a5e64218d783..1c3c9899ed07 100644
--- a/drivers/net/dsa/microchip/lan9645x/lan9645x_main.h
+++ b/drivers/net/dsa/microchip/lan9645x/lan9645x_main.h
@@ -218,6 +218,14 @@ struct lan9645x {
 	/* VLAN entries */
 	struct lan9645x_vlan vlans[VLAN_N_VID];
 
+	/* Multicast Forwarding Database */
+	struct list_head mdb_entries;
+	struct list_head pgid_entries;
+	/* lock for mdb_entries and pgid_entries. Must be taken before mact_lock
+	 * if both are taken.
+	 */
+	struct mutex mdb_lock;
+
 	int num_port_dis;
 	bool dd_dis;
 	bool tsn_dis;
@@ -422,5 +430,18 @@ int lan9645x_mact_entry_del(struct lan9645x *lan9645x, int pgid,
 			    const unsigned char *mac, u16 vid);
 int lan9645x_mact_entry_add(struct lan9645x *lan9645x, int pgid,
 			    const unsigned char *mac, u16 vid);
+int lan9645x_mact_learn_cpu_copy(struct lan9645x *lan9645x, int port,
+				 const unsigned char *addr, u16 vid,
+				 enum macaccess_entry_type type, bool cpu_copy);
+
+/* Multicast Database lan9645x_mdb.c */
+int lan9645x_mdb_port_add(struct lan9645x *lan9645x, int port,
+			  const struct switchdev_obj_port_mdb *mdb,
+			  struct net_device *bridge);
+int lan9645x_mdb_port_del(struct lan9645x *lan9645x, int port,
+			  const struct switchdev_obj_port_mdb *mdb,
+			  struct net_device *bridge);
+void lan9645x_mdb_init(struct lan9645x *lan9645x);
+void lan9645x_mdb_deinit(struct lan9645x *lan9645x);
 
 #endif /* __LAN9645X_MAIN_H__ */
diff --git a/drivers/net/dsa/microchip/lan9645x/lan9645x_mdb.c b/drivers/net/dsa/microchip/lan9645x/lan9645x_mdb.c
new file mode 100644
index 000000000000..24e02aa1fd6f
--- /dev/null
+++ b/drivers/net/dsa/microchip/lan9645x/lan9645x_mdb.c
@@ -0,0 +1,383 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright (C) 2026 Microchip Technology Inc.
+ */
+
+#include "lan9645x_main.h"
+
+/* HW ignores dest_idx for IPv4/IPv6 types, so we use this dummy index */
+#define IP_ENTRY_PGID		0
+#define PGID_INDEX(pgid)	((pgid) ? (pgid)->index : IP_ENTRY_PGID)
+
+struct lan9645x_pgid_entry {
+	struct list_head list;
+	int index;
+	refcount_t refcount;
+	u16 ports;
+};
+
+struct lan9645x_mdb_entry {
+	struct list_head list;
+	unsigned char mac[ETH_ALEN];
+	u16 vid;
+	u16 ports;
+	struct lan9645x_pgid_entry *pgid;
+};
+
+void lan9645x_mdb_init(struct lan9645x *lan9645x)
+{
+	INIT_LIST_HEAD(&lan9645x->mdb_entries);
+	INIT_LIST_HEAD(&lan9645x->pgid_entries);
+	mutex_init(&lan9645x->mdb_lock);
+
+	/* Use CPU queues to communicate frame classification to the CPU */
+	lan_rmw(ANA_CPUQ_CFG_CPUQ_IGMP_SET(LAN9645X_CPUQ_IGMP) |
+		ANA_CPUQ_CFG_CPUQ_MLD_SET(LAN9645X_CPUQ_MLD) |
+		ANA_CPUQ_CFG_CPUQ_IPMC_CTRL_SET(LAN9645X_CPUQ_IPMC_CTRL),
+		ANA_CPUQ_CFG_CPUQ_IGMP |
+		ANA_CPUQ_CFG_CPUQ_MLD |
+		ANA_CPUQ_CFG_CPUQ_IPMC_CTRL,
+		lan9645x, ANA_CPUQ_CFG);
+}
+
+static enum macaccess_entry_type lan9645x_mdb_classify(const unsigned char *mac)
+{
+	if (ether_addr_is_ipv4_mcast(mac))
+		return ENTRYTYPE_MACV4;
+	if (ether_addr_is_ipv6_mcast(mac))
+		return ENTRYTYPE_MACV6;
+	return ENTRYTYPE_LOCKED;
+}
+
+static struct lan9645x_mdb_entry *
+lan9645x_mdb_entry_lookup(struct lan9645x *lan9645x, const unsigned char *mac,
+			  u16 vid)
+{
+	struct lan9645x_mdb_entry *mdb;
+
+	list_for_each_entry(mdb, &lan9645x->mdb_entries, list) {
+		if (ether_addr_equal(mdb->mac, mac) && mdb->vid == vid)
+			return mdb;
+	}
+
+	return NULL;
+}
+
+static struct lan9645x_mdb_entry *
+lan9645x_mdb_entry_alloc(struct lan9645x *lan9645x,
+			 const unsigned char addr[ETH_ALEN], u16 vid)
+{
+	struct lan9645x_mdb_entry *mdb_entry;
+
+	mdb_entry = kzalloc_obj(*mdb_entry);
+	if (!mdb_entry)
+		return ERR_PTR(-ENOMEM);
+
+	ether_addr_copy(mdb_entry->mac, addr);
+	mdb_entry->vid = vid;
+
+	list_add_tail(&mdb_entry->list, &lan9645x->mdb_entries);
+
+	dev_dbg(lan9645x->dev, "vid=%u addr=%pM\n", mdb_entry->vid,
+		mdb_entry->mac);
+
+	return mdb_entry;
+}
+
+static void lan9645x_mdb_encode_mac(unsigned char *dst, unsigned char *mac,
+				    u16 ports, enum macaccess_entry_type type)
+{
+	ether_addr_copy(dst, mac);
+
+	/* The HW encodes the portmask in the high bits of the mac for ip
+	 * multicast entries, to save on the limited PGID resources.
+	 *
+	 * IPv4 Multicast DMAC: 0x01005Exxxxxx
+	 * IPv6 Multicast DMAC: 0x3333xxxxxxxx
+	 *
+	 * which gives us 24 or 16 bits to encode the portmask.
+	 */
+	if (type == ENTRYTYPE_MACV4) {
+		dst[0] = 0;
+		dst[1] = ports >> 8;
+		dst[2] = ports & 0xff;
+	} else if (type == ENTRYTYPE_MACV6) {
+		dst[0] = ports >> 8;
+		dst[1] = ports & 0xff;
+	}
+}
+
+static void lan9645x_pgid_entry_put(struct lan9645x *lan9645x,
+				    struct lan9645x_pgid_entry *pgid_entry)
+{
+	if (!pgid_entry)
+		return;
+
+	if (!refcount_dec_and_test(&pgid_entry->refcount))
+		return;
+
+	dev_dbg(lan9645x->dev, "pgid=%d ports=0x%x", pgid_entry->index,
+		pgid_entry->ports);
+	/* We leave the PGID written in HW, as no entry is pointing to it. */
+	list_del(&pgid_entry->list);
+	kfree(pgid_entry);
+}
+
+static void lan9645x_mdb_entry_dealloc(struct lan9645x *lan9645x,
+				       struct lan9645x_mdb_entry *mdb_entry)
+{
+	dev_dbg(lan9645x->dev, "vid=%u addr=%pM\n", mdb_entry->vid,
+		mdb_entry->mac);
+	list_del(&mdb_entry->list);
+	lan9645x_pgid_entry_put(lan9645x, mdb_entry->pgid);
+	kfree(mdb_entry);
+}
+
+static struct lan9645x_pgid_entry *
+lan9645x_mdb_pgid_entry_lookup(struct lan9645x *lan9645x, u16 ports)
+{
+	struct lan9645x_pgid_entry *pgid_entry;
+
+	list_for_each_entry(pgid_entry, &lan9645x->pgid_entries, list) {
+		if (pgid_entry->ports == ports &&
+		    refcount_inc_not_zero(&pgid_entry->refcount))
+			return pgid_entry;
+	}
+
+	return NULL;
+}
+
+static struct lan9645x_pgid_entry *
+lan9645x_pgid_entry_alloc(struct lan9645x *lan9645x, int index, u16 ports)
+{
+	struct lan9645x_pgid_entry *pgid_entry;
+
+	pgid_entry = kzalloc_obj(*pgid_entry);
+	if (!pgid_entry)
+		return ERR_PTR(-ENOMEM);
+
+	pgid_entry->ports = ports;
+	pgid_entry->index = index;
+	refcount_set(&pgid_entry->refcount, 1);
+
+	list_add_tail(&pgid_entry->list, &lan9645x->pgid_entries);
+
+	dev_dbg(lan9645x->dev, "index=%d ports=0x%x", pgid_entry->index,
+		pgid_entry->ports);
+
+	lan_rmw(ANA_PGID_PGID_SET(pgid_entry->ports),
+		ANA_PGID_PGID, lan9645x,
+		ANA_PGID(pgid_entry->index));
+
+	return pgid_entry;
+}
+
+static struct lan9645x_pgid_entry *
+lan9645x_mdb_pgid_entry_create(struct lan9645x *lan9645x, u16 ports)
+{
+	struct lan9645x_pgid_entry *pgid_entry = NULL;
+	int index;
+
+	for (index = PGID_GP_START; index < PGID_GP_END; index++) {
+		bool used = false;
+
+		list_for_each_entry(pgid_entry, &lan9645x->pgid_entries, list) {
+			if (pgid_entry->index == index) {
+				used = true;
+				break;
+			}
+		}
+
+		if (!used)
+			return lan9645x_pgid_entry_alloc(lan9645x, index,
+							 ports);
+	}
+
+	return ERR_PTR(-ENOSPC);
+}
+
+static struct lan9645x_pgid_entry *
+lan9645x_mdb_pgid_entry_get(struct lan9645x *lan9645x, u16 ports,
+			    enum macaccess_entry_type type)
+{
+	struct lan9645x_pgid_entry *pgid_entry;
+	u16 pgid_ports;
+
+	if (type == ENTRYTYPE_MACV4 || type == ENTRYTYPE_MACV6 || !ports)
+		return NULL;
+
+	/* CPU_PORT forwarding is handled by cpu_copy flag on mac table entry.
+	 * So we can strip CPU_PORT here to allow better PGID sharing.
+	 */
+	pgid_ports = ports & ~BIT(CPU_PORT);
+
+	pgid_entry = lan9645x_mdb_pgid_entry_lookup(lan9645x, pgid_ports);
+	if (!pgid_entry)
+		return lan9645x_mdb_pgid_entry_create(lan9645x, pgid_ports);
+
+	return pgid_entry;
+}
+
+static int lan9645x_mdb_update_dest(struct lan9645x *lan9645x,
+				    struct lan9645x_mdb_entry *mdb_entry,
+				    enum macaccess_entry_type type,
+				    struct lan9645x_pgid_entry *new_pgid,
+				    u16 new_ports)
+{
+	struct lan9645x_pgid_entry *old_pgid;
+	unsigned char mac[ETH_ALEN];
+	int err, pgid_index;
+	bool cpu_copy;
+
+	old_pgid = mdb_entry->pgid;
+	lan9645x_mdb_encode_mac(mac, mdb_entry->mac, new_ports, type);
+	cpu_copy = !!(new_ports & BIT(CPU_PORT));
+	pgid_index = PGID_INDEX(new_pgid);
+
+	err = lan9645x_mact_learn_cpu_copy(lan9645x, pgid_index, mac,
+					   mdb_entry->vid, type, cpu_copy);
+	if (err) {
+		lan9645x_pgid_entry_put(lan9645x, new_pgid);
+		return err;
+	}
+	mdb_entry->pgid = new_pgid;
+	mdb_entry->ports = new_ports;
+	lan9645x_pgid_entry_put(lan9645x, old_pgid);
+	return 0;
+}
+
+static int __lan9645x_mdb_add(struct lan9645x *lan9645x, int chip_port,
+			      const unsigned char addr[ETH_ALEN], u16 vid,
+			      enum macaccess_entry_type type)
+{
+	struct lan9645x_pgid_entry *new_pgid;
+	struct lan9645x_mdb_entry *mdb_entry;
+	u16 new_ports;
+
+	mdb_entry = lan9645x_mdb_entry_lookup(lan9645x, addr, vid);
+	if (!mdb_entry) {
+		mdb_entry = lan9645x_mdb_entry_alloc(lan9645x, addr, vid);
+		if (IS_ERR(mdb_entry))
+			return PTR_ERR(mdb_entry);
+	}
+
+	if (mdb_entry->ports & BIT(chip_port))
+		return 0;
+
+	new_ports = mdb_entry->ports | BIT(chip_port);
+
+	/* Update PGID ptr for non-IP entries (L2 multicast) */
+	new_pgid = lan9645x_mdb_pgid_entry_get(lan9645x, new_ports, type);
+	if (IS_ERR(new_pgid)) {
+		/* Out of PGIDs or mem. Continue forwarding to old port
+		 * group, or remove if fresh mdb_entry.
+		 */
+		if (!mdb_entry->ports)
+			lan9645x_mdb_entry_dealloc(lan9645x, mdb_entry);
+
+		return PTR_ERR(new_pgid);
+	}
+
+	return lan9645x_mdb_update_dest(lan9645x, mdb_entry, type, new_pgid,
+					new_ports);
+}
+
+static int __lan9645x_mdb_del(struct lan9645x *lan9645x, int chip_port,
+			      const unsigned char addr[ETH_ALEN], u16 vid,
+			      enum macaccess_entry_type type)
+{
+	struct lan9645x_pgid_entry *new_pgid;
+	struct lan9645x_mdb_entry *mdb_entry;
+	u16 new_ports;
+	int err;
+
+	mdb_entry = lan9645x_mdb_entry_lookup(lan9645x, addr, vid);
+	if (!mdb_entry)
+		return -ENOENT;
+
+	if (!(mdb_entry->ports & BIT(chip_port)))
+		return 0;
+
+	new_ports = mdb_entry->ports & ~BIT(chip_port);
+
+	if (!new_ports) {
+		err = lan9645x_mact_forget(lan9645x, mdb_entry->mac,
+					   mdb_entry->vid, type);
+		if (err)
+			return err;
+		lan9645x_mdb_entry_dealloc(lan9645x, mdb_entry);
+		return 0;
+	}
+
+	/* Update PGID ptr for non-IP entries (L2 multicast) */
+	new_pgid = lan9645x_mdb_pgid_entry_get(lan9645x, new_ports, type);
+	if (IS_ERR(new_pgid))
+		/* Continue forwarding to old port group. */
+		return PTR_ERR(new_pgid);
+
+	return lan9645x_mdb_update_dest(lan9645x, mdb_entry, type, new_pgid,
+					new_ports);
+}
+
+static int lan9645x_mdb_add(struct lan9645x *lan9645x, int chip_port,
+			    const unsigned char addr[ETH_ALEN], u16 vid,
+			    enum macaccess_entry_type type)
+{
+	int err;
+
+	mutex_lock(&lan9645x->mdb_lock);
+	err = __lan9645x_mdb_add(lan9645x, chip_port, addr, vid, type);
+	mutex_unlock(&lan9645x->mdb_lock);
+	return err;
+}
+
+static int lan9645x_mdb_del(struct lan9645x *lan9645x, int chip_port,
+			    const unsigned char addr[ETH_ALEN], u16 vid,
+			    enum macaccess_entry_type type)
+{
+	int err;
+
+	mutex_lock(&lan9645x->mdb_lock);
+	err = __lan9645x_mdb_del(lan9645x, chip_port, addr, vid, type);
+	mutex_unlock(&lan9645x->mdb_lock);
+	return err;
+}
+
+int lan9645x_mdb_port_add(struct lan9645x *lan9645x, int port,
+			  const struct switchdev_obj_port_mdb *mdb,
+			  struct net_device *bridge)
+{
+	enum macaccess_entry_type type;
+	u16 vid = mdb->vid;
+
+	type = lan9645x_mdb_classify(mdb->addr);
+
+	if (!vid)
+		vid = lan9645x_vlan_unaware_pvid(!!bridge);
+
+	return lan9645x_mdb_add(lan9645x, port, mdb->addr, vid, type);
+}
+
+int lan9645x_mdb_port_del(struct lan9645x *lan9645x, int port,
+			  const struct switchdev_obj_port_mdb *mdb,
+			  struct net_device *bridge)
+{
+	enum macaccess_entry_type type;
+	u16 vid = mdb->vid;
+
+	type = lan9645x_mdb_classify(mdb->addr);
+
+	if (!vid)
+		vid = lan9645x_vlan_unaware_pvid(!!bridge);
+
+	return lan9645x_mdb_del(lan9645x, port, mdb->addr, vid, type);
+}
+
+void lan9645x_mdb_deinit(struct lan9645x *lan9645x)
+{
+	struct lan9645x_mdb_entry *mdb, *tmp;
+
+	list_for_each_entry_safe(mdb, tmp, &lan9645x->mdb_entries, list)
+		lan9645x_mdb_entry_dealloc(lan9645x, mdb);
+
+	mutex_destroy(&lan9645x->mdb_lock);
+}
diff --git a/drivers/net/dsa/microchip/lan9645x/lan9645x_port.c b/drivers/net/dsa/microchip/lan9645x/lan9645x_port.c
index 661cd00465e2..5dee92940f89 100644
--- a/drivers/net/dsa/microchip/lan9645x/lan9645x_port.c
+++ b/drivers/net/dsa/microchip/lan9645x/lan9645x_port.c
@@ -189,6 +189,14 @@ int lan9645x_port_setup(struct dsa_switch *ds, int port)
 		ANA_PORT_CFG_PORTID_VAL,
 		lan9645x, ANA_PORT_CFG(p->chip_port));
 
+	lan_rmw(ANA_CPU_FWD_CFG_IGMP_REDIR_ENA_SET(true) |
+		ANA_CPU_FWD_CFG_MLD_REDIR_ENA_SET(true) |
+		ANA_CPU_FWD_CFG_IPMC_CTRL_COPY_ENA_SET(true),
+		ANA_CPU_FWD_CFG_IGMP_REDIR_ENA |
+		ANA_CPU_FWD_CFG_MLD_REDIR_ENA |
+		ANA_CPU_FWD_CFG_IPMC_CTRL_COPY_ENA,
+		lan9645x, ANA_CPU_FWD_CFG(p->chip_port));
+
 	if (p->chip_port != lan9645x->npi)
 		lan9645x_vlan_set_hostmode(p);
 
diff --git a/include/linux/dsa/lan9645x.h b/include/linux/dsa/lan9645x.h
index 34c18bf975d0..ac0e70c704a5 100644
--- a/include/linux/dsa/lan9645x.h
+++ b/include/linux/dsa/lan9645x.h
@@ -131,4 +131,15 @@
 #define IFH_DUPL_DISC_ENA_SZ		1
 #define IFH_RCT_AVAIL_SZ		1
 
+/* Chip has 8 cpu queues. The cpu queues used by a frame is passed as a mask in
+ * the IFH on extraction. We use this to avoid classifying IGMP and MLD frames
+ * in the tag driver.
+ */
+enum {
+	LAN9645X_CPUQ_DEF = 0,
+	LAN9645X_CPUQ_IGMP = 1,
+	LAN9645X_CPUQ_MLD = 2,
+	LAN9645X_CPUQ_IPMC_CTRL = 3,
+};
+
 #endif /* _NET_DSA_TAG_LAN9645X_H_ */
diff --git a/net/dsa/tag_lan9645x.c b/net/dsa/tag_lan9645x.c
index 6dae3b9ec240..86d67426bf04 100644
--- a/net/dsa/tag_lan9645x.c
+++ b/net/dsa/tag_lan9645x.c
@@ -153,6 +153,20 @@ static void lan9645x_xmit_get_vlan_info(struct sk_buff *skb,
 					     LAN9645X_IFH_TAG_TYPE_C;
 }
 
+static void lan9645x_offload_fwd_mark(struct sk_buff *skb, u32 cpuq)
+{
+	u32 cpu_redir;
+
+	/* IGMP/MLD are trapped to CPU, and must be forwarded by the stack */
+	cpu_redir = BIT(LAN9645X_CPUQ_IGMP) | BIT(LAN9645X_CPUQ_MLD);
+	if (cpuq & cpu_redir) {
+		skb->offload_fwd_mark = 0;
+		return;
+	}
+
+	return dsa_default_offload_fwd_mark(skb);
+}
+
 static struct sk_buff *lan9645x_xmit(struct sk_buff *skb,
 				     struct net_device *ndev)
 {
@@ -193,7 +207,7 @@ static struct sk_buff *lan9645x_xmit(struct sk_buff *skb,
 static struct sk_buff *lan9645x_rcv(struct sk_buff *skb,
 				    struct net_device *ndev)
 {
-	u32 src_port, qos_class, vlan_tci, tag_type, popcnt, etype_ofs;
+	u32 src_port, qos_class, vlan_tci, tag_type, popcnt, etype_ofs, cpuq;
 	struct dsa_port *dp;
 	u32 ifh_gap_len = 0;
 	u16 vlan_tpid;
@@ -216,6 +230,7 @@ static struct sk_buff *lan9645x_rcv(struct sk_buff *skb,
 	tag_type = lan9645x_ifh_get(ifh, IFH_TAG_TYPE, IFH_TAG_TYPE_SZ);
 	vlan_tci = lan9645x_ifh_get(ifh, IFH_TCI, IFH_TCI_SZ);
 	qos_class = lan9645x_ifh_get(ifh, IFH_QOS_CLASS, IFH_QOS_CLASS_SZ);
+	cpuq = lan9645x_ifh_get(ifh, IFH_CPUQ, IFH_CPUQ_SZ);
 
 	/* Set skb->data at start of real header
 	 *
@@ -252,7 +267,7 @@ static struct sk_buff *lan9645x_rcv(struct sk_buff *skb,
 		return NULL;
 	}
 
-	dsa_default_offload_fwd_mark(skb);
+	lan9645x_offload_fwd_mark(skb, cpuq);
 
 	skb->priority = qos_class;
 

-- 
2.52.0