net: mscc: ocelot: support IPv4, IPv6 and plain Ethernet mdb entries

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>
Signed-off-by: David S. Miller <davem@davemloft.net>

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 {