[PATCH net-next 0/5] Multicast improvement in Ocelot and Felix drivers

[PATCH net-next 0/5] Multicast improvement in Ocelot and Felix drivers

[Vladimir Oltean]

From: Vladimir Oltean <vladimir.oltean@nxp.com>

This series makes some basic multicast forwarding functionality work for
Felix DSA and for Ocelot switchdev. IGMP/MLD snooping in Felix is still
missing, and there are other improvements to be made in the general area
of multicast address filtering towards the CPU, but let's get these
hardware-specific fixes out of the way first.

Vladimir Oltean (5):
  net: mscc: ocelot: fix encoding destination ports into multicast IPv4
    address
  net: mscc: ocelot: make the NPI port a proper target for FDB and MDB
  net: dsa: felix: call port mdb operations from ocelot
  net: mscc: ocelot: introduce macros for iterating over PGIDs
  net: mscc: ocelot: support IPv4, IPv6 and plain Ethernet mdb entries

 drivers/net/dsa/ocelot/felix.c         |  26 +++++
 drivers/net/ethernet/mscc/ocelot.c     | 137 ++++++++++++++++++-------
 drivers/net/ethernet/mscc/ocelot.h     |   6 +-
 drivers/net/ethernet/mscc/ocelot_net.c |  28 ++++-
 include/soc/mscc/ocelot.h              |  19 ++++
 5 files changed, 175 insertions(+), 41 deletions(-)

-- 
2.25.1

[PATCH net-next 1/5] net: mscc: ocelot: fix encoding destination ports into multicast IPv4 address

[Vladimir Oltean]

From: Vladimir Oltean <vladimir.oltean@nxp.com>

The ocelot hardware designers have made some hacks to support multicast
IPv4 and IPv6 addresses. Normally, the MAC table matches on MAC
addresses and the destination ports are selected through the DEST_IDX
field of the respective MAC table entry. The DEST_IDX points to a Port
Group ID (PGID) which contains the bit mask of ports that frames should
be forwarded to. But there aren't a lot of PGIDs (only 80 or so) and
there are clearly many more IP multicast addresses than that, so it
doesn't scale to use this PGID mechanism, so something else was done.
Since the first portion of the MAC address is known, the hack they did
was to use a single PGID for _flooding_ unknown IPv4 multicast
(PGID_MCIPV4 == 62), but for known IP multicast, embed the destination
ports into the first 3 bytes of the MAC address recorded in the MAC
table.

The VSC7514 datasheet explains it like this:

    3.9.1.5 IPv4 Multicast Entries

    MAC table entries with the ENTRY_TYPE = 2 settings are interpreted
    as IPv4 multicast entries.
    IPv4 multicasts entries match IPv4 frames, which are classified to
    the specified VID, and which have DMAC = 0x01005Exxxxxx, where
    xxxxxx is the lower 24 bits of the MAC address in the entry.
    Instead of a lookup in the destination mask table (PGID), the
    destination set is programmed as part of the entry MAC address. This
    is shown in the following table.

    Table 78: IPv4 Multicast Destination Mask

        Destination Ports            Record Bit Field
        ---------------------------------------------
        Ports 10-0                   MAC[34-24]

    Example: All IPv4 multicast frames in VLAN 12 with MAC 01005E112233 are
    to be forwarded to ports 3, 8, and 9. This is done by inserting the
    following entry in the MAC table entry:
    VALID = 1
    VID = 12
    MAC = 0x000308112233
    ENTRY_TYPE = 2
    DEST_IDX = 0

But this procedure is not at all what's going on in the driver. In fact,
the code that embeds the ports into the MAC address looks like it hasn't
actually been tested. This patch applies the procedure described in the
datasheet.

Since there are many other fixes to be made around multicast forwarding
until it works properly, there is no real reason for this patch to be
backported to stable trees, or considered a real fix of something that
should have worked.

Signed-off-by: Vladimir Oltean <vladimir.oltean@nxp.com>
---
 drivers/net/ethernet/mscc/ocelot.c | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/drivers/net/ethernet/mscc/ocelot.c b/drivers/net/ethernet/mscc/ocelot.c
index 52b180280d2f..922c3e855c3a 100644
--- a/drivers/net/ethernet/mscc/ocelot.c
+++ b/drivers/net/ethernet/mscc/ocelot.c
@@ -973,14 +973,14 @@ int ocelot_port_obj_add_mdb(struct net_device *dev,
 	addr[0] = 0;
 
 	if (!new) {
-		addr[2] = mc->ports << 0;
-		addr[1] = mc->ports << 8;
+		addr[1] = mc->ports >> 8;
+		addr[2] = mc->ports & 0xff;
 		ocelot_mact_forget(ocelot, addr, vid);
 	}
 
 	mc->ports |= BIT(port);
-	addr[2] = mc->ports << 0;
-	addr[1] = mc->ports << 8;
+	addr[1] = mc->ports >> 8;
+	addr[2] = mc->ports & 0xff;
 
 	return ocelot_mact_learn(ocelot, 0, addr, vid, ENTRYTYPE_MACv4);
 }
@@ -1005,9 +1005,9 @@ int ocelot_port_obj_del_mdb(struct net_device *dev,
 		return -ENOENT;
 
 	memcpy(addr, mc->addr, ETH_ALEN);
-	addr[2] = mc->ports << 0;
-	addr[1] = mc->ports << 8;
 	addr[0] = 0;
+	addr[1] = mc->ports >> 8;
+	addr[2] = mc->ports & 0xff;
 	ocelot_mact_forget(ocelot, addr, vid);
 
 	mc->ports &= ~BIT(port);
@@ -1017,8 +1017,8 @@ int ocelot_port_obj_del_mdb(struct net_device *dev,
 		return 0;
 	}
 
-	addr[2] = mc->ports << 0;
-	addr[1] = mc->ports << 8;
+	addr[1] = mc->ports >> 8;
+	addr[2] = mc->ports & 0xff;
 
 	return ocelot_mact_learn(ocelot, 0, addr, vid, ENTRYTYPE_MACv4);
 }
-- 
2.25.1

[PATCH net-next 2/5] net: mscc: ocelot: make the NPI port a proper target for FDB and MDB

[Vladimir Oltean]

From: Vladimir Oltean <vladimir.oltean@nxp.com>

When used in DSA mode (as seen in Felix), the DEST_IDX in the MAC table
should point to the PGID for the CPU port (PGID_CPU) and not for the
Ethernet port where the CPU queues are redirected to (also known as Node
Processor Interface - NPI).

Because for Felix this distinction shouldn't really matter (from DSA
perspective, the NPI port _is_ the CPU port), make the ocelot library
act upon the CPU port when NPI mode is enabled. This has no effect for
the mscc_ocelot driver for VSC7514, because that does not use NPI (and
ocelot->npi is -1).

Signed-off-by: Vladimir Oltean <vladimir.oltean@nxp.com>
---
 drivers/net/ethernet/mscc/ocelot.c | 12 +++++++++++-
 1 file changed, 11 insertions(+), 1 deletion(-)

diff --git a/drivers/net/ethernet/mscc/ocelot.c b/drivers/net/ethernet/mscc/ocelot.c
index 922c3e855c3a..4aadb65a6af8 100644
--- a/drivers/net/ethernet/mscc/ocelot.c
+++ b/drivers/net/ethernet/mscc/ocelot.c
@@ -535,6 +535,10 @@ int ocelot_fdb_add(struct ocelot *ocelot, int port,
 		   const unsigned char *addr, u16 vid)
 {
 	struct ocelot_port *ocelot_port = ocelot->ports[port];
+	int pgid = port;
+
+	if (port == ocelot->npi)
+		pgid = PGID_CPU;
 
 	if (!vid) {
 		if (!ocelot_port->vlan_aware)
@@ -550,7 +554,7 @@ int ocelot_fdb_add(struct ocelot *ocelot, int port,
 			return -EINVAL;
 	}
 
-	return ocelot_mact_learn(ocelot, port, addr, vid, ENTRYTYPE_LOCKED);
+	return ocelot_mact_learn(ocelot, pgid, addr, vid, ENTRYTYPE_LOCKED);
 }
 EXPORT_SYMBOL(ocelot_fdb_add);
 
@@ -953,6 +957,9 @@ int ocelot_port_obj_add_mdb(struct net_device *dev,
 	u16 vid = mdb->vid;
 	bool new = false;
 
+	if (port == ocelot->npi)
+		port = ocelot->num_phys_ports;
+
 	if (!vid)
 		vid = ocelot_port->pvid;
 
@@ -997,6 +1004,9 @@ int ocelot_port_obj_del_mdb(struct net_device *dev,
 	int port = priv->chip_port;
 	u16 vid = mdb->vid;
 
+	if (port == ocelot->npi)
+		port = ocelot->num_phys_ports;
+
 	if (!vid)
 		vid = ocelot_port->pvid;
 
-- 
2.25.1

[PATCH net-next 3/5] net: dsa: felix: call port mdb operations from ocelot

[Vladimir Oltean]

From: Vladimir Oltean <vladimir.oltean@nxp.com>

This adds the mdb hooks in felix and exports the mdb functions from
ocelot.

Signed-off-by: Vladimir Oltean <vladimir.oltean@nxp.com>
---
 drivers/net/dsa/ocelot/felix.c         | 26 ++++++++++++++++++++++++++
 drivers/net/ethernet/mscc/ocelot.c     | 23 ++++++++---------------
 drivers/net/ethernet/mscc/ocelot.h     |  5 -----
 drivers/net/ethernet/mscc/ocelot_net.c | 26 ++++++++++++++++++++++++++
 include/soc/mscc/ocelot.h              |  4 ++++
 5 files changed, 64 insertions(+), 20 deletions(-)

diff --git a/drivers/net/dsa/ocelot/felix.c b/drivers/net/dsa/ocelot/felix.c
index 66648986e6e3..25046777c993 100644
--- a/drivers/net/dsa/ocelot/felix.c
+++ b/drivers/net/dsa/ocelot/felix.c
@@ -59,6 +59,29 @@ static int felix_fdb_del(struct dsa_switch *ds, int port,
 	return ocelot_fdb_del(ocelot, port, addr, vid);
 }
 
+/* This callback needs to be present */
+static int felix_mdb_prepare(struct dsa_switch *ds, int port,
+			     const struct switchdev_obj_port_mdb *mdb)
+{
+	return 0;
+}
+
+static void felix_mdb_add(struct dsa_switch *ds, int port,
+			  const struct switchdev_obj_port_mdb *mdb)
+{
+	struct ocelot *ocelot = ds->priv;
+
+	ocelot_port_mdb_add(ocelot, port, mdb);
+}
+
+static int felix_mdb_del(struct dsa_switch *ds, int port,
+			 const struct switchdev_obj_port_mdb *mdb)
+{
+	struct ocelot *ocelot = ds->priv;
+
+	return ocelot_port_mdb_del(ocelot, port, mdb);
+}
+
 static void felix_bridge_stp_state_set(struct dsa_switch *ds, int port,
 				       u8 state)
 {
@@ -771,6 +794,9 @@ static const struct dsa_switch_ops felix_switch_ops = {
 	.port_fdb_dump		= felix_fdb_dump,
 	.port_fdb_add		= felix_fdb_add,
 	.port_fdb_del		= felix_fdb_del,
+	.port_mdb_prepare	= felix_mdb_prepare,
+	.port_mdb_add		= felix_mdb_add,
+	.port_mdb_del		= felix_mdb_del,
 	.port_bridge_join	= felix_bridge_join,
 	.port_bridge_leave	= felix_bridge_leave,
 	.port_stp_state_set	= felix_bridge_stp_state_set,
diff --git a/drivers/net/ethernet/mscc/ocelot.c b/drivers/net/ethernet/mscc/ocelot.c
index 4aadb65a6af8..468eaf5916e5 100644
--- a/drivers/net/ethernet/mscc/ocelot.c
+++ b/drivers/net/ethernet/mscc/ocelot.c
@@ -944,16 +944,12 @@ static struct ocelot_multicast *ocelot_multicast_get(struct ocelot *ocelot,
 	return NULL;
 }
 
-int ocelot_port_obj_add_mdb(struct net_device *dev,
-			    const struct switchdev_obj_port_mdb *mdb,
-			    struct switchdev_trans *trans)
+int ocelot_port_mdb_add(struct ocelot *ocelot, int port,
+			const struct switchdev_obj_port_mdb *mdb)
 {
-	struct ocelot_port_private *priv = netdev_priv(dev);
-	struct ocelot_port *ocelot_port = &priv->port;
-	struct ocelot *ocelot = ocelot_port->ocelot;
+	struct ocelot_port *ocelot_port = ocelot->ports[port];
 	unsigned char addr[ETH_ALEN];
 	struct ocelot_multicast *mc;
-	int port = priv->chip_port;
 	u16 vid = mdb->vid;
 	bool new = false;
 
@@ -991,17 +987,14 @@ int ocelot_port_obj_add_mdb(struct net_device *dev,
 
 	return ocelot_mact_learn(ocelot, 0, addr, vid, ENTRYTYPE_MACv4);
 }
-EXPORT_SYMBOL(ocelot_port_obj_add_mdb);
+EXPORT_SYMBOL(ocelot_port_mdb_add);
 
-int ocelot_port_obj_del_mdb(struct net_device *dev,
-			    const struct switchdev_obj_port_mdb *mdb)
+int ocelot_port_mdb_del(struct ocelot *ocelot, int port,
+			const struct switchdev_obj_port_mdb *mdb)
 {
-	struct ocelot_port_private *priv = netdev_priv(dev);
-	struct ocelot_port *ocelot_port = &priv->port;
-	struct ocelot *ocelot = ocelot_port->ocelot;
+	struct ocelot_port *ocelot_port = ocelot->ports[port];
 	unsigned char addr[ETH_ALEN];
 	struct ocelot_multicast *mc;
-	int port = priv->chip_port;
 	u16 vid = mdb->vid;
 
 	if (port == ocelot->npi)
@@ -1032,7 +1025,7 @@ int ocelot_port_obj_del_mdb(struct net_device *dev,
 
 	return ocelot_mact_learn(ocelot, 0, addr, vid, ENTRYTYPE_MACv4);
 }
-EXPORT_SYMBOL(ocelot_port_obj_del_mdb);
+EXPORT_SYMBOL(ocelot_port_mdb_del);
 
 int ocelot_port_bridge_join(struct ocelot *ocelot, int port,
 			    struct net_device *bridge)
diff --git a/drivers/net/ethernet/mscc/ocelot.h b/drivers/net/ethernet/mscc/ocelot.h
index 0c23734a87be..be4a41646e5e 100644
--- a/drivers/net/ethernet/mscc/ocelot.h
+++ b/drivers/net/ethernet/mscc/ocelot.h
@@ -97,11 +97,6 @@ int ocelot_port_lag_join(struct ocelot *ocelot, int port,
 			 struct net_device *bond);
 void ocelot_port_lag_leave(struct ocelot *ocelot, int port,
 			   struct net_device *bond);
-int ocelot_port_obj_del_mdb(struct net_device *dev,
-			    const struct switchdev_obj_port_mdb *mdb);
-int ocelot_port_obj_add_mdb(struct net_device *dev,
-			    const struct switchdev_obj_port_mdb *mdb,
-			    struct switchdev_trans *trans);
 
 u32 ocelot_port_readl(struct ocelot_port *port, u32 reg);
 void ocelot_port_writel(struct ocelot_port *port, u32 val, u32 reg);
diff --git a/drivers/net/ethernet/mscc/ocelot_net.c b/drivers/net/ethernet/mscc/ocelot_net.c
index 80cb1873e9d9..1bad146a0105 100644
--- a/drivers/net/ethernet/mscc/ocelot_net.c
+++ b/drivers/net/ethernet/mscc/ocelot_net.c
@@ -795,6 +795,32 @@ static int ocelot_port_vlan_del_vlan(struct net_device *dev,
 	return 0;
 }
 
+static int ocelot_port_obj_add_mdb(struct net_device *dev,
+				   const struct switchdev_obj_port_mdb *mdb,
+				   struct switchdev_trans *trans)
+{
+	struct ocelot_port_private *priv = netdev_priv(dev);
+	struct ocelot_port *ocelot_port = &priv->port;
+	struct ocelot *ocelot = ocelot_port->ocelot;
+	int port = priv->chip_port;
+
+	if (switchdev_trans_ph_prepare(trans))
+		return 0;
+
+	return ocelot_port_mdb_add(ocelot, port, mdb);
+}
+
+static int ocelot_port_obj_del_mdb(struct net_device *dev,
+				   const struct switchdev_obj_port_mdb *mdb)
+{
+	struct ocelot_port_private *priv = netdev_priv(dev);
+	struct ocelot_port *ocelot_port = &priv->port;
+	struct ocelot *ocelot = ocelot_port->ocelot;
+	int port = priv->chip_port;
+
+	return ocelot_port_mdb_del(ocelot, port, mdb);
+}
+
 static int ocelot_port_obj_add(struct net_device *dev,
 			       const struct switchdev_obj *obj,
 			       struct switchdev_trans *trans,
diff --git a/include/soc/mscc/ocelot.h b/include/soc/mscc/ocelot.h
index fa2c3904049e..80415b63ccfa 100644
--- a/include/soc/mscc/ocelot.h
+++ b/include/soc/mscc/ocelot.h
@@ -641,5 +641,9 @@ int ocelot_cls_flower_destroy(struct ocelot *ocelot, int port,
 			      struct flow_cls_offload *f, bool ingress);
 int ocelot_cls_flower_stats(struct ocelot *ocelot, int port,
 			    struct flow_cls_offload *f, bool ingress);
+int ocelot_port_mdb_add(struct ocelot *ocelot, int port,
+			const struct switchdev_obj_port_mdb *mdb);
+int ocelot_port_mdb_del(struct ocelot *ocelot, int port,
+			const struct switchdev_obj_port_mdb *mdb);
 
 #endif
-- 
2.25.1

[PATCH net-next 4/5] net: mscc: ocelot: introduce macros for iterating over PGIDs

[Vladimir Oltean]

From: Vladimir Oltean <vladimir.oltean@nxp.com>

The current iterators are impossible to understand at first glance
without switching back and forth between the definitions and their
actual use in the for loops.

So introduce some convenience names to help readability.

Signed-off-by: Vladimir Oltean <vladimir.oltean@nxp.com>
---
 drivers/net/ethernet/mscc/ocelot.c     |  8 ++++----
 drivers/net/ethernet/mscc/ocelot_net.c |  2 +-
 include/soc/mscc/ocelot.h              | 15 +++++++++++++++
 3 files changed, 20 insertions(+), 5 deletions(-)

diff --git a/drivers/net/ethernet/mscc/ocelot.c b/drivers/net/ethernet/mscc/ocelot.c
index 468eaf5916e5..b6254c20f2f0 100644
--- a/drivers/net/ethernet/mscc/ocelot.c
+++ b/drivers/net/ethernet/mscc/ocelot.c
@@ -1064,10 +1064,10 @@ static void ocelot_set_aggr_pgids(struct ocelot *ocelot)
 	int i, port, lag;
 
 	/* Reset destination and aggregation PGIDS */
-	for (port = 0; port < ocelot->num_phys_ports; port++)
+	for_each_unicast_dest_pgid(ocelot, port)
 		ocelot_write_rix(ocelot, BIT(port), ANA_PGID_PGID, port);
 
-	for (i = PGID_AGGR; i < PGID_SRC; i++)
+	for_each_aggr_pgid(ocelot, i)
 		ocelot_write_rix(ocelot, GENMASK(ocelot->num_phys_ports - 1, 0),
 				 ANA_PGID_PGID, i);
 
@@ -1089,7 +1089,7 @@ static void ocelot_set_aggr_pgids(struct ocelot *ocelot)
 			aggr_count++;
 		}
 
-		for (i = PGID_AGGR; i < PGID_SRC; i++) {
+		for_each_aggr_pgid(ocelot, i) {
 			u32 ac;
 
 			ac = ocelot_read_rix(ocelot, ANA_PGID_PGID, i);
@@ -1451,7 +1451,7 @@ int ocelot_init(struct ocelot *ocelot)
 	}
 
 	/* Allow broadcast MAC frames. */
-	for (i = ocelot->num_phys_ports + 1; i < PGID_CPU; i++) {
+	for_each_nonreserved_multicast_dest_pgid(ocelot, i) {
 		u32 val = ANA_PGID_PGID_PGID(GENMASK(ocelot->num_phys_ports - 1, 0));
 
 		ocelot_write_rix(ocelot, val, ANA_PGID_PGID, i);
diff --git a/drivers/net/ethernet/mscc/ocelot_net.c b/drivers/net/ethernet/mscc/ocelot_net.c
index 1bad146a0105..702b42543fb7 100644
--- a/drivers/net/ethernet/mscc/ocelot_net.c
+++ b/drivers/net/ethernet/mscc/ocelot_net.c
@@ -422,7 +422,7 @@ static void ocelot_set_rx_mode(struct net_device *dev)
 	 * forwarded to the CPU port.
 	 */
 	val = GENMASK(ocelot->num_phys_ports - 1, 0);
-	for (i = ocelot->num_phys_ports + 1; i < PGID_CPU; i++)
+	for_each_nonreserved_multicast_dest_pgid(ocelot, i)
 		ocelot_write_rix(ocelot, val, ANA_PGID_PGID, i);
 
 	__dev_mc_sync(dev, ocelot_mc_sync, ocelot_mc_unsync);
diff --git a/include/soc/mscc/ocelot.h b/include/soc/mscc/ocelot.h
index 80415b63ccfa..e050f8121ba2 100644
--- a/include/soc/mscc/ocelot.h
+++ b/include/soc/mscc/ocelot.h
@@ -65,6 +65,21 @@
 #define PGID_MCIPV4			62
 #define PGID_MCIPV6			63
 
+#define for_each_unicast_dest_pgid(ocelot, pgid)		\
+	for ((pgid) = 0;					\
+	     (pgid) < (ocelot)->num_phys_ports;			\
+	     (pgid)++)
+
+#define for_each_nonreserved_multicast_dest_pgid(ocelot, pgid)	\
+	for ((pgid) = (ocelot)->num_phys_ports + 1;		\
+	     (pgid) < PGID_CPU;					\
+	     (pgid)++)
+
+#define for_each_aggr_pgid(ocelot, pgid)			\
+	for ((pgid) = PGID_AGGR;				\
+	     (pgid) < PGID_SRC;					\
+	     (pgid)++)
+
 /* Aggregation PGIDs, one per Link Aggregation Code */
 #define PGID_AGGR			64
 
-- 
2.25.1

[PATCH net-next 5/5] net: mscc: ocelot: support IPv4, IPv6 and plain Ethernet mdb entries

[Vladimir Oltean]

From: Vladimir Oltean <vladimir.oltean@nxp.com>

The current procedure for installing a multicast address is hardcoded
for IPv4. But, in the ocelot hardware, there are 3 different procedures
for IPv4, IPv6 and for regular L2 multicast.

For IPv6 (33-33-xx-xx-xx-xx), it's the same as for IPv4
(01-00-5e-xx-xx-xx), except that the destination port mask is stuffed
into first 2 bytes of the MAC address except into first 3 bytes.

For plain Ethernet multicast, there's no port-in-address stuffing going
on, instead the DEST_IDX (pointer to PGID) is used there, just as for
unicast. So we have to use one of the nonreserved multicast PGIDs that
the hardware has allocated for this purpose.

This patch classifies the type of multicast address based on its first
bytes, then redirects to one of the 3 different hardware procedures.

Note that this gives us a really better way of redirecting PTP frames
sent at 01-1b-19-00-00-00 to the CPU. Previously, Yangbo Lu tried to add
a trapping rule for PTP EtherType but got a lot of pushback:

https://patchwork.ozlabs.org/project/netdev/patch/20190813025214.18601-5-yangbo.lu@nxp.com/

But right now, that isn't needed at all. The application stack (ptp4l)
does this for the PTP multicast addresses it's interested in (which are
configurable, and include 01-1b-19-00-00-00):

	memset(&mreq, 0, sizeof(mreq));
	mreq.mr_ifindex = index;
	mreq.mr_type = PACKET_MR_MULTICAST;
	mreq.mr_alen = MAC_LEN;
	memcpy(mreq.mr_address, addr1, MAC_LEN);

	err1 = setsockopt(fd, SOL_PACKET, PACKET_ADD_MEMBERSHIP, &mreq,
			  sizeof(mreq));

Into the kernel, this translates into a dev_mc_add on the switch network
interfaces, and our drivers know that it means they should translate it
into a host MDB address (make the CPU port be the destination).
Previously, this was broken because all mdb addresses were treated as
IPv4 (which 01-1b-19-00-00-00 obviously is not).

Signed-off-by: Vladimir Oltean <vladimir.oltean@nxp.com>
---
 drivers/net/ethernet/mscc/ocelot.c | 94 +++++++++++++++++++++++++-----
 drivers/net/ethernet/mscc/ocelot.h |  1 +
 2 files changed, 80 insertions(+), 15 deletions(-)

diff --git a/drivers/net/ethernet/mscc/ocelot.c b/drivers/net/ethernet/mscc/ocelot.c
index b6254c20f2f0..e815aad8d85e 100644
--- a/drivers/net/ethernet/mscc/ocelot.c
+++ b/drivers/net/ethernet/mscc/ocelot.c
@@ -944,10 +944,68 @@ static struct ocelot_multicast *ocelot_multicast_get(struct ocelot *ocelot,
 	return NULL;
 }
 
+static enum macaccess_entry_type ocelot_classify_mdb(const unsigned char *addr)
+{
+	if (addr[0] == 0x01 && addr[1] == 0x00 && addr[2] == 0x5e)
+		return ENTRYTYPE_MACv4;
+	if (addr[0] == 0x33 && addr[1] == 0x33)
+		return ENTRYTYPE_MACv6;
+	return ENTRYTYPE_NORMAL;
+}
+
+static int ocelot_mdb_get_pgid(struct ocelot *ocelot,
+			       enum macaccess_entry_type entry_type)
+{
+	int pgid;
+
+	/* According to VSC7514 datasheet 3.9.1.5 IPv4 Multicast Entries and
+	 * 3.9.1.6 IPv6 Multicast Entries, "Instead of a lookup in the
+	 * destination mask table (PGID), the destination set is programmed as
+	 * part of the entry MAC address.", and the DEST_IDX is set to 0.
+	 */
+	if (entry_type == ENTRYTYPE_MACv4 ||
+	    entry_type == ENTRYTYPE_MACv6)
+		return 0;
+
+	for_each_nonreserved_multicast_dest_pgid(ocelot, pgid) {
+		struct ocelot_multicast *mc;
+		bool used = false;
+
+		list_for_each_entry(mc, &ocelot->multicast, list) {
+			if (mc->pgid == pgid) {
+				used = true;
+				break;
+			}
+		}
+
+		if (!used)
+			return pgid;
+	}
+
+	return -1;
+}
+
+static void ocelot_encode_ports_to_mdb(unsigned char *addr,
+				       struct ocelot_multicast *mc,
+				       enum macaccess_entry_type entry_type)
+{
+	memcpy(addr, mc->addr, ETH_ALEN);
+
+	if (entry_type == ENTRYTYPE_MACv4) {
+		addr[0] = 0;
+		addr[1] = mc->ports >> 8;
+		addr[2] = mc->ports & 0xff;
+	} else if (entry_type == ENTRYTYPE_MACv6) {
+		addr[0] = mc->ports >> 8;
+		addr[1] = mc->ports & 0xff;
+	}
+}
+
 int ocelot_port_mdb_add(struct ocelot *ocelot, int port,
 			const struct switchdev_obj_port_mdb *mdb)
 {
 	struct ocelot_port *ocelot_port = ocelot->ports[port];
+	enum macaccess_entry_type entry_type;
 	unsigned char addr[ETH_ALEN];
 	struct ocelot_multicast *mc;
 	u16 vid = mdb->vid;
@@ -959,33 +1017,40 @@ int ocelot_port_mdb_add(struct ocelot *ocelot, int port,
 	if (!vid)
 		vid = ocelot_port->pvid;
 
+	entry_type = ocelot_classify_mdb(mdb->addr);
+
 	mc = ocelot_multicast_get(ocelot, mdb->addr, vid);
 	if (!mc) {
+		int pgid = ocelot_mdb_get_pgid(ocelot, entry_type);
+
+		if (pgid < 0) {
+			dev_err(ocelot->dev,
+				"No more PGIDs available for mdb %pM vid %d\n",
+				mdb->addr, vid);
+			return -ENOSPC;
+		}
+
 		mc = devm_kzalloc(ocelot->dev, sizeof(*mc), GFP_KERNEL);
 		if (!mc)
 			return -ENOMEM;
 
 		memcpy(mc->addr, mdb->addr, ETH_ALEN);
 		mc->vid = vid;
+		mc->pgid = pgid;
 
 		list_add_tail(&mc->list, &ocelot->multicast);
 		new = true;
 	}
 
-	memcpy(addr, mc->addr, ETH_ALEN);
-	addr[0] = 0;
-
 	if (!new) {
-		addr[1] = mc->ports >> 8;
-		addr[2] = mc->ports & 0xff;
+		ocelot_encode_ports_to_mdb(addr, mc, entry_type);
 		ocelot_mact_forget(ocelot, addr, vid);
 	}
 
 	mc->ports |= BIT(port);
-	addr[1] = mc->ports >> 8;
-	addr[2] = mc->ports & 0xff;
+	ocelot_encode_ports_to_mdb(addr, mc, entry_type);
 
-	return ocelot_mact_learn(ocelot, 0, addr, vid, ENTRYTYPE_MACv4);
+	return ocelot_mact_learn(ocelot, mc->pgid, addr, vid, entry_type);
 }
 EXPORT_SYMBOL(ocelot_port_mdb_add);
 
@@ -993,6 +1058,7 @@ int ocelot_port_mdb_del(struct ocelot *ocelot, int port,
 			const struct switchdev_obj_port_mdb *mdb)
 {
 	struct ocelot_port *ocelot_port = ocelot->ports[port];
+	enum macaccess_entry_type entry_type;
 	unsigned char addr[ETH_ALEN];
 	struct ocelot_multicast *mc;
 	u16 vid = mdb->vid;
@@ -1007,10 +1073,9 @@ int ocelot_port_mdb_del(struct ocelot *ocelot, int port,
 	if (!mc)
 		return -ENOENT;
 
-	memcpy(addr, mc->addr, ETH_ALEN);
-	addr[0] = 0;
-	addr[1] = mc->ports >> 8;
-	addr[2] = mc->ports & 0xff;
+	entry_type = ocelot_classify_mdb(mdb->addr);
+
+	ocelot_encode_ports_to_mdb(addr, mc, entry_type);
 	ocelot_mact_forget(ocelot, addr, vid);
 
 	mc->ports &= ~BIT(port);
@@ -1020,10 +1085,9 @@ int ocelot_port_mdb_del(struct ocelot *ocelot, int port,
 		return 0;
 	}
 
-	addr[1] = mc->ports >> 8;
-	addr[2] = mc->ports & 0xff;
+	ocelot_encode_ports_to_mdb(addr, mc, entry_type);
 
-	return ocelot_mact_learn(ocelot, 0, addr, vid, ENTRYTYPE_MACv4);
+	return ocelot_mact_learn(ocelot, mc->pgid, addr, vid, entry_type);
 }
 EXPORT_SYMBOL(ocelot_port_mdb_del);
 
diff --git a/drivers/net/ethernet/mscc/ocelot.h b/drivers/net/ethernet/mscc/ocelot.h
index be4a41646e5e..394362e23c47 100644
--- a/drivers/net/ethernet/mscc/ocelot.h
+++ b/drivers/net/ethernet/mscc/ocelot.h
@@ -46,6 +46,7 @@ struct ocelot_multicast {
 	unsigned char addr[ETH_ALEN];
 	u16 vid;
 	u16 ports;
+	int pgid;
 };
 
 struct ocelot_port_tc {
-- 
2.25.1

Re: [PATCH net-next 0/5] Multicast improvement in Ocelot and Felix drivers

[David Miller]

From: Vladimir Oltean <olteanv@gmail.com>
Date: Sun, 21 Jun 2020 14:45:58 +0300

> From: Vladimir Oltean <vladimir.oltean@nxp.com>
> 
> This series makes some basic multicast forwarding functionality work for
> Felix DSA and for Ocelot switchdev. IGMP/MLD snooping in Felix is still
> missing, and there are other improvements to be made in the general area
> of multicast address filtering towards the CPU, but let's get these
> hardware-specific fixes out of the way first.

Series applied to net-next, thank you.