Merge branch 'sparx5-mcast'

Casper Andersson says:

====================
net: sparx5: Add multicast support

Add multicast support to Sparx5.
====================

Signed-off-by: David S. Miller <davem@davemloft.net>

6 files changed, 221 insertions, 11 deletions

diff --git a/drivers/net/ethernet/microchip/sparx5/Makefile b/drivers/net/ethernet/microchip/sparx5/Makefile
index e9dd348a6ebb..4402c3ed1dc5 100644
--- a/drivers/net/ethernet/microchip/sparx5/Makefile
+++ b/drivers/net/ethernet/microchip/sparx5/Makefile
@@ -8,4 +8,4 @@ obj-$(CONFIG_SPARX5_SWITCH) += sparx5-switch.o
 sparx5-switch-objs  := sparx5_main.o sparx5_packet.o \
  sparx5_netdev.o sparx5_phylink.o sparx5_port.o sparx5_mactable.o sparx5_vlan.o \
  sparx5_switchdev.o sparx5_calendar.o sparx5_ethtool.o sparx5_fdma.o \
- sparx5_ptp.o
+ sparx5_ptp.o sparx5_pgid.o
diff --git a/drivers/net/ethernet/microchip/sparx5/sparx5_mactable.c b/drivers/net/ethernet/microchip/sparx5/sparx5_mactable.c
index 82b1b3c9a065..35abb3d0ce19 100644
--- a/drivers/net/ethernet/microchip/sparx5/sparx5_mactable.c
+++ b/drivers/net/ethernet/microchip/sparx5/sparx5_mactable.c
@@ -186,11 +186,11 @@ bool sparx5_mact_getnext(struct sparx5 *sparx5,
 	return ret == 0;
 }
 
-static int sparx5_mact_lookup(struct sparx5 *sparx5,
-			      const unsigned char mac[ETH_ALEN],
-			      u16 vid)
+bool sparx5_mact_find(struct sparx5 *sparx5,
+		      const unsigned char mac[ETH_ALEN], u16 vid, u32 *pcfg2)
 {
 	int ret;
+	u32 cfg2;
 
 	mutex_lock(&sparx5->lock);
 
@@ -202,16 +202,29 @@ static int sparx5_mact_lookup(struct sparx5 *sparx5,
 		sparx5, LRN_COMMON_ACCESS_CTRL);
 
 	ret = sparx5_mact_wait_for_completion(sparx5);
-	if (ret)
-		goto out;
-
-	ret = LRN_MAC_ACCESS_CFG_2_MAC_ENTRY_VLD_GET
-		(spx5_rd(sparx5, LRN_MAC_ACCESS_CFG_2));
+	if (ret == 0) {
+		cfg2 = spx5_rd(sparx5, LRN_MAC_ACCESS_CFG_2);
+		if (LRN_MAC_ACCESS_CFG_2_MAC_ENTRY_VLD_GET(cfg2))
+			*pcfg2 = cfg2;
+		else
+			ret = -ENOENT;
+	}
 
-out:
 	mutex_unlock(&sparx5->lock);
 
-	return ret;
+	return ret == 0;
+}
+
+static int sparx5_mact_lookup(struct sparx5 *sparx5,
+			      const unsigned char mac[ETH_ALEN],
+			      u16 vid)
+{
+	u32 pcfg2;
+
+	if (sparx5_mact_find(sparx5, mac, vid, &pcfg2))
+		return 1;
+
+	return 0;
 }
 
 int sparx5_mact_forget(struct sparx5 *sparx5,
diff --git a/drivers/net/ethernet/microchip/sparx5/sparx5_main.c b/drivers/net/ethernet/microchip/sparx5/sparx5_main.c
index 5f7c7030ce03..01be7bd84181 100644
--- a/drivers/net/ethernet/microchip/sparx5/sparx5_main.c
+++ b/drivers/net/ethernet/microchip/sparx5/sparx5_main.c
@@ -626,6 +626,9 @@ static int sparx5_start(struct sparx5 *sparx5)
 	/* Init MAC table, ageing */
 	sparx5_mact_init(sparx5);
 
+	/* Init PGID table arbitrator */
+	sparx5_pgid_init(sparx5);
+
 	/* Setup VLANs */
 	sparx5_vlan_init(sparx5);
 
diff --git a/drivers/net/ethernet/microchip/sparx5/sparx5_main.h b/drivers/net/ethernet/microchip/sparx5/sparx5_main.h
index df68a0891029..7a04b8f2a546 100644
--- a/drivers/net/ethernet/microchip/sparx5/sparx5_main.h
+++ b/drivers/net/ethernet/microchip/sparx5/sparx5_main.h
@@ -66,6 +66,12 @@ enum sparx5_vlan_port_type {
 #define PGID_BCAST	       (PGID_BASE + 6)
 #define PGID_CPU	       (PGID_BASE + 7)
 
+#define PGID_TABLE_SIZE	       3290
+
+#define PGID_MCAST_START 65
+#define PGID_GLAG_START 833
+#define PGID_GLAG_END 1088
+
 #define IFH_LEN                9 /* 36 bytes */
 #define NULL_VID               0
 #define SPX5_MACT_PULL_DELAY   (2 * HZ)
@@ -271,6 +277,8 @@ struct sparx5 {
 	struct mutex ptp_lock; /* lock for ptp interface state */
 	u16 ptp_skbs;
 	int ptp_irq;
+	/* PGID allocation map */
+	u8 pgid_map[PGID_TABLE_SIZE];
 };
 
 /* sparx5_switchdev.c */
@@ -302,6 +310,8 @@ int sparx5_mact_learn(struct sparx5 *sparx5, int port,
 		      const unsigned char mac[ETH_ALEN], u16 vid);
 bool sparx5_mact_getnext(struct sparx5 *sparx5,
 			 unsigned char mac[ETH_ALEN], u16 *vid, u32 *pcfg2);
+bool sparx5_mact_find(struct sparx5 *sparx5,
+		      const unsigned char mac[ETH_ALEN], u16 vid, u32 *pcfg2);
 int sparx5_mact_forget(struct sparx5 *sparx5,
 		       const unsigned char mac[ETH_ALEN], u16 vid);
 int sparx5_add_mact_entry(struct sparx5 *sparx5,
@@ -359,6 +369,19 @@ void sparx5_ptp_txtstamp_release(struct sparx5_port *port,
 				 struct sk_buff *skb);
 irqreturn_t sparx5_ptp_irq_handler(int irq, void *args);
 
+/* sparx5_pgid.c */
+enum sparx5_pgid_type {
+	SPX5_PGID_FREE,
+	SPX5_PGID_RESERVED,
+	SPX5_PGID_MULTICAST,
+	SPX5_PGID_GLAG
+};
+
+void sparx5_pgid_init(struct sparx5 *spx5);
+int sparx5_pgid_alloc_glag(struct sparx5 *spx5, u16 *idx);
+int sparx5_pgid_alloc_mcast(struct sparx5 *spx5, u16 *idx);
+int sparx5_pgid_free(struct sparx5 *spx5, u16 idx);
+
 /* Clock period in picoseconds */
 static inline u32 sparx5_clk_period(enum sparx5_core_clockfreq cclock)
 {
diff --git a/drivers/net/ethernet/microchip/sparx5/sparx5_pgid.c b/drivers/net/ethernet/microchip/sparx5/sparx5_pgid.c
new file mode 100644
index 000000000000..90366fcb9958
--- /dev/null
+++ b/drivers/net/ethernet/microchip/sparx5/sparx5_pgid.c
@@ -0,0 +1,60 @@
+// SPDX-License-Identifier: GPL-2.0+
+#include "sparx5_main.h"
+
+void sparx5_pgid_init(struct sparx5 *spx5)
+{
+	int i;
+
+	for (i = 0; i < PGID_TABLE_SIZE; i++)
+		spx5->pgid_map[i] = SPX5_PGID_FREE;
+
+	/* Reserved for unicast, flood control, broadcast, and CPU.
+	 * These cannot be freed.
+	 */
+	for (i = 0; i <= PGID_CPU; i++)
+		spx5->pgid_map[i] = SPX5_PGID_RESERVED;
+}
+
+int sparx5_pgid_alloc_glag(struct sparx5 *spx5, u16 *idx)
+{
+	int i;
+
+	for (i = PGID_GLAG_START; i <= PGID_GLAG_END; i++)
+		if (spx5->pgid_map[i] == SPX5_PGID_FREE) {
+			spx5->pgid_map[i] = SPX5_PGID_GLAG;
+			*idx = i;
+			return 0;
+		}
+
+	return -EBUSY;
+}
+
+int sparx5_pgid_alloc_mcast(struct sparx5 *spx5, u16 *idx)
+{
+	int i;
+
+	for (i = PGID_MCAST_START; i < PGID_TABLE_SIZE; i++) {
+		if (i == PGID_GLAG_START)
+			i = PGID_GLAG_END + 1;
+
+		if (spx5->pgid_map[i] == SPX5_PGID_FREE) {
+			spx5->pgid_map[i] = SPX5_PGID_MULTICAST;
+			*idx = i;
+			return 0;
+		}
+	}
+
+	return -EBUSY;
+}
+
+int sparx5_pgid_free(struct sparx5 *spx5, u16 idx)
+{
+	if (idx <= PGID_CPU || idx >= PGID_TABLE_SIZE)
+		return -EINVAL;
+
+	if (spx5->pgid_map[idx] == SPX5_PGID_FREE)
+		return -EINVAL;
+
+	spx5->pgid_map[idx] = SPX5_PGID_FREE;
+	return 0;
+}
diff --git a/drivers/net/ethernet/microchip/sparx5/sparx5_switchdev.c b/drivers/net/ethernet/microchip/sparx5/sparx5_switchdev.c
index 8b69c72ff807..2d8e0b81c839 100644
--- a/drivers/net/ethernet/microchip/sparx5/sparx5_switchdev.c
+++ b/drivers/net/ethernet/microchip/sparx5/sparx5_switchdev.c
@@ -386,6 +386,109 @@ static int sparx5_handle_port_vlan_add(struct net_device *dev,
 				  v->flags & BRIDGE_VLAN_INFO_UNTAGGED);
 }
 
+static int sparx5_handle_port_mdb_add(struct net_device *dev,
+				      struct notifier_block *nb,
+				      const struct switchdev_obj_port_mdb *v)
+{
+	struct sparx5_port *port = netdev_priv(dev);
+	struct sparx5 *spx5 = port->sparx5;
+	u16 pgid_idx, vid;
+	u32 mact_entry;
+	int res, err;
+
+	/* When VLAN unaware the vlan value is not parsed and we receive vid 0.
+	 * Fall back to bridge vid 1.
+	 */
+	if (!br_vlan_enabled(spx5->hw_bridge_dev))
+		vid = 1;
+	else
+		vid = v->vid;
+
+	res = sparx5_mact_find(spx5, v->addr, vid, &mact_entry);
+
+	if (res) {
+		pgid_idx = LRN_MAC_ACCESS_CFG_2_MAC_ENTRY_ADDR_GET(mact_entry);
+
+		/* MC_IDX has an offset of 65 in the PGID table. */
+		pgid_idx += PGID_MCAST_START;
+		sparx5_pgid_update_mask(port, pgid_idx, true);
+	} else {
+		err = sparx5_pgid_alloc_mcast(spx5, &pgid_idx);
+		if (err) {
+			netdev_warn(dev, "multicast pgid table full\n");
+			return err;
+		}
+		sparx5_pgid_update_mask(port, pgid_idx, true);
+		err = sparx5_mact_learn(spx5, pgid_idx, v->addr, vid);
+		if (err) {
+			netdev_warn(dev, "could not learn mac address %pM\n", v->addr);
+			sparx5_pgid_update_mask(port, pgid_idx, false);
+			return err;
+		}
+	}
+
+	return 0;
+}
+
+static int sparx5_mdb_del_entry(struct net_device *dev,
+				struct sparx5 *spx5,
+				const unsigned char mac[ETH_ALEN],
+				const u16 vid,
+				u16 pgid_idx)
+{
+	int err;
+
+	err = sparx5_mact_forget(spx5, mac, vid);
+	if (err) {
+		netdev_warn(dev, "could not forget mac address %pM", mac);
+		return err;
+	}
+	err = sparx5_pgid_free(spx5, pgid_idx);
+	if (err) {
+		netdev_err(dev, "attempted to free already freed pgid\n");
+		return err;
+	}
+	return 0;
+}
+
+static int sparx5_handle_port_mdb_del(struct net_device *dev,
+				      struct notifier_block *nb,
+				      const struct switchdev_obj_port_mdb *v)
+{
+	struct sparx5_port *port = netdev_priv(dev);
+	struct sparx5 *spx5 = port->sparx5;
+	u16 pgid_idx, vid;
+	u32 mact_entry, res, pgid_entry[3];
+	int err;
+
+	if (!br_vlan_enabled(spx5->hw_bridge_dev))
+		vid = 1;
+	else
+		vid = v->vid;
+
+	res = sparx5_mact_find(spx5, v->addr, vid, &mact_entry);
+
+	if (res) {
+		pgid_idx = LRN_MAC_ACCESS_CFG_2_MAC_ENTRY_ADDR_GET(mact_entry);
+
+		/* MC_IDX has an offset of 65 in the PGID table. */
+		pgid_idx += PGID_MCAST_START;
+		sparx5_pgid_update_mask(port, pgid_idx, false);
+
+		pgid_entry[0] = spx5_rd(spx5, ANA_AC_PGID_CFG(pgid_idx));
+		pgid_entry[1] = spx5_rd(spx5, ANA_AC_PGID_CFG1(pgid_idx));
+		pgid_entry[2] = spx5_rd(spx5, ANA_AC_PGID_CFG2(pgid_idx));
+		if (pgid_entry[0] == 0 && pgid_entry[1] == 0 && pgid_entry[2] == 0) {
+			/* No ports are in MC group. Remove entry */
+			err = sparx5_mdb_del_entry(dev, spx5, v->addr, vid, pgid_idx);
+			if (err)
+				return err;
+		}
+	}
+
+	return 0;
+}
+
 static int sparx5_handle_port_obj_add(struct net_device *dev,
 				      struct notifier_block *nb,
 				      struct switchdev_notifier_port_obj_info *info)
@@ -398,6 +501,10 @@ static int sparx5_handle_port_obj_add(struct net_device *dev,
 		err = sparx5_handle_port_vlan_add(dev, nb,
 						  SWITCHDEV_OBJ_PORT_VLAN(obj));
 		break;
+	case SWITCHDEV_OBJ_ID_PORT_MDB:
+		err = sparx5_handle_port_mdb_add(dev, nb,
+						 SWITCHDEV_OBJ_PORT_MDB(obj));
+		break;
 	default:
 		err = -EOPNOTSUPP;
 		break;
@@ -446,6 +553,10 @@ static int sparx5_handle_port_obj_del(struct net_device *dev,
 		err = sparx5_handle_port_vlan_del(dev, nb,
 						  SWITCHDEV_OBJ_PORT_VLAN(obj)->vid);
 		break;
+	case SWITCHDEV_OBJ_ID_PORT_MDB:
+		err = sparx5_handle_port_mdb_del(dev, nb,
+						 SWITCHDEV_OBJ_PORT_MDB(obj));
+		break;
 	default:
 		err = -EOPNOTSUPP;
 		break;