[PATCH 14/20] net: dsa: xilinx: program MAC frame filter and per-port nibbles

[Nagadheeraj Rottela]

The switch fabric identifies a frame's destination port by matching
its MAC against a 48-bit address in TSN_SW_MAC_LSB/TSN_SW_MAC_MSB,
with a 16-bit mask covering bytes 4 and 5. Until the filter is
programmed, every frame misses the classifier.

Take the DSA conduit's MAC as a 44-bit prefix, wildcarding the low
nibble of byte 5, and program both registers. Each of the three
ports also owns a 4-bit MAC nibble field in the Switch Port State
Control register at +0x004C. The field holds the low nibble of
byte 5 of that port's MAC and covers the wildcarded bits, so the
fabric can still tell the ports apart.

Drive all of this from a netdev notifier. On the conduit's
NETDEV_REGISTER, derive the prefix, program the frame filter, and
set the CPU port nibbles for all ports. Handle NETDEV_REGISTER for
each swpN to set its nibble.

Reject port_set_mac_address whenever the requested address does not
match the conduit's prefix, or its low nibble collides with the CPU
port or another user port. The nibble is the only thing that tells
ports apart once the top 44 bits are shared.

Also route CPU-originated bridge control frames (STP, LLDP) to a
single port by nibble match instead of flooding both, since the
classifier can now tell one port's source MAC from the other's.

Co-developed-by: Srinivas Neeli <srinivas.neeli@xxxxxxx>
Signed-off-by: Srinivas Neeli <srinivas.neeli@xxxxxxx>
Signed-off-by: Nagadheeraj Rottela <nagadheeraj.rottela@xxxxxxx>
---
 drivers/net/dsa/xilinx/xilinx_tsn.c | 232 +++++++++++++++++++++++++++-
 drivers/net/dsa/xilinx/xilinx_tsn.h |  39 ++++-
 2 files changed, 268 insertions(+), 3 deletions(-)

diff --git a/drivers/net/dsa/xilinx/xilinx_tsn.c b/drivers/net/dsa/xilinx/xilinx_tsn.c
index 86aba2e8b6e9..9826f006b078 100644
--- a/drivers/net/dsa/xilinx/xilinx_tsn.c
+++ b/drivers/net/dsa/xilinx/xilinx_tsn.c
@@ -5,12 +5,14 @@
 
 #include <linux/bitfield.h>
 #include <linux/clk.h>
+#include <linux/etherdevice.h>
 #include <linux/if_bridge.h>
 #include <linux/io.h>
 #include <linux/iopoll.h>
 #include <linux/kernel.h>
 #include <linux/mdio.h>
 #include <linux/module.h>
+#include <linux/netdevice.h>
 #include <linux/of.h>
 #include <linux/of_mdio.h>
 #include <linux/phy.h>
@@ -33,6 +35,74 @@ static u32 sw_ior(struct xlnx_tsn *sw, u32 off)
 	return ioread32(sw->sw_base + off);
 }
 
+/* Cache the conduit MAC with byte 5's low nibble zeroed out. The
+ * frame-filter mask covers those 4 bits. The per-port MAC-nibble
+ * fields in +0x004C supply the actual values.
+ */
+static void xlnx_tsn_derive_prefix(struct xlnx_tsn *sw)
+{
+	memcpy(sw->mac_prefix, sw->conduit->dev_addr, ETH_ALEN);
+	sw->mac_prefix[5] &= ~TSN_SW_MAC_NIBBLE_WILDCARD;
+}
+
+static void xlnx_tsn_program_frame_filter(struct xlnx_tsn *sw)
+{
+	const u8 *p = sw->mac_prefix;
+	u32 lsb, msb;
+
+	lsb = ((u32)p[2] << 24) | ((u32)p[3] << 16) |
+	      ((u32)p[4] << 8) | p[5];
+	msb = FIELD_PREP(TSN_SW_MAC_MSB_MASK_MASK, TSN_SW_MAC_NIBBLE_WILDCARD) |
+	      FIELD_PREP(TSN_SW_MAC_MSB_ADDR_MASK,
+			 ((u32)p[0] << 8) | p[1]);
+
+	sw_iow(sw, TSN_SW_MAC_LSB_OFFSET, lsb);
+	sw_iow(sw, TSN_SW_MAC_MSB_OFFSET, msb);
+}
+
+/* True when addr's upper 44 bits match the cached prefix. The
+ * prefix has byte 5's low nibble already cleared, so byte 5 of addr
+ * is masked the same way before comparison.
+ */
+static bool xlnx_tsn_prefix_matches(struct xlnx_tsn *sw, const u8 *addr)
+{
+	if (memcmp(addr, sw->mac_prefix, ETH_ALEN - 1) != 0)
+		return false;
+
+	return (addr[5] & ~TSN_SW_MAC_NIBBLE_WILDCARD) == sw->mac_prefix[5];
+}
+
+static int xlnx_tsn_set_port_mac_nibble(struct xlnx_tsn *sw, int port,
+					u8 nibble)
+{
+	u32 mask, new_field, reg;
+
+	nibble &= TSN_SW_MAC_NIBBLE_WILDCARD;
+
+	switch (port) {
+	case XLNX_TSN_CPU_PORT:
+		mask = EP_PORT_MAC_NIBBLE_MASK;
+		new_field = FIELD_PREP(EP_PORT_MAC_NIBBLE_MASK, nibble);
+		break;
+	case XLNX_TSN_PORT_MAC1:
+		mask = MAC1_PORT_MAC_NIBBLE_MASK;
+		new_field = FIELD_PREP(MAC1_PORT_MAC_NIBBLE_MASK, nibble);
+		break;
+	case XLNX_TSN_PORT_MAC2:
+		mask = MAC2_PORT_MAC_NIBBLE_MASK;
+		new_field = FIELD_PREP(MAC2_PORT_MAC_NIBBLE_MASK, nibble);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	reg = sw_ior(sw, TSN_PORT_STATE_CTRL_OFFSET);
+	reg = (reg & ~mask) | new_field;
+	sw_iow(sw, TSN_PORT_STATE_CTRL_OFFSET, reg);
+
+	return 0;
+}
+
 static int xlnx_tsn_switch_status_ready(struct xlnx_tsn *sw)
 {
 	u32 reg;
@@ -287,6 +357,112 @@ static int xlnx_tsn_mdio_register_all(struct xlnx_tsn *sw)
 	return ret;
 }
 
+/* Build a per-port MAC from the shared prefix. */
+static void xlnx_tsn_synth_port_mac(struct xlnx_tsn *sw, int port,
+				    u8 *out)
+{
+	u8 ep_nibble = sw->conduit->dev_addr[5] & TSN_SW_MAC_NIBBLE_WILDCARD;
+
+	memcpy(out, sw->mac_prefix, ETH_ALEN);
+	out[5] |= (ep_nibble + port) & TSN_SW_MAC_NIBBLE_WILDCARD;
+}
+
+static int xlnx_tsn_user_port_index(struct xlnx_tsn *sw,
+				    const struct net_device *dev)
+{
+	struct dsa_port *dp;
+
+	dsa_switch_for_each_user_port(dp, &sw->ds)
+		if (dp->user == dev)
+			return dp->index;
+
+	return -1;
+}
+
+static int xlnx_tsn_handle_user_register(struct xlnx_tsn *sw,
+					 struct net_device *dev, int port)
+{
+	u8 want[ETH_ALEN];
+	u8 nibble;
+
+	if (!xlnx_tsn_prefix_matches(sw, dev->dev_addr)) {
+		xlnx_tsn_synth_port_mac(sw, port, want);
+		dev_warn(sw->dev,
+			 "port %d: MAC %pM does not match conduit prefix; overriding to %pM\n",
+			 port, dev->dev_addr, want);
+		dev_addr_mod(dev, 0, want, ETH_ALEN);
+		nibble = want[5] & TSN_SW_MAC_NIBBLE_WILDCARD;
+	} else if (ether_addr_equal(dev->dev_addr, sw->conduit->dev_addr)) {
+		/* Either DSA inherited the conduit MAC, or DT gave port@N
+		 * the same address explicitly. Either way, assign a unique
+		 * per-port nibble.
+		 */
+		xlnx_tsn_synth_port_mac(sw, port, want);
+		dev_addr_mod(dev, 0, want, ETH_ALEN);
+		nibble = want[5] & TSN_SW_MAC_NIBBLE_WILDCARD;
+	} else {
+		nibble = dev->dev_addr[5] & TSN_SW_MAC_NIBBLE_WILDCARD;
+	}
+
+	return xlnx_tsn_set_port_mac_nibble(sw, port, nibble);
+}
+
+static void xlnx_tsn_handle_conduit_changeaddr(struct xlnx_tsn *sw)
+{
+	struct dsa_port *dp;
+
+	xlnx_tsn_derive_prefix(sw);
+	xlnx_tsn_program_frame_filter(sw);
+	xlnx_tsn_set_port_mac_nibble(sw, XLNX_TSN_CPU_PORT,
+				     sw->conduit->dev_addr[5]);
+
+	dsa_switch_for_each_user_port(dp, &sw->ds) {
+		u8 want[ETH_ALEN];
+
+		if (!dp->user)
+			continue;
+
+		xlnx_tsn_synth_port_mac(sw, dp->index, want);
+		dev_addr_mod(dp->user, 0, want, ETH_ALEN);
+		call_netdevice_notifiers(NETDEV_CHANGEADDR, dp->user);
+	}
+}
+
+static int xlnx_tsn_netdev_event(struct notifier_block *nb,
+				 unsigned long event, void *ptr)
+{
+	struct xlnx_tsn *sw = container_of(nb, struct xlnx_tsn, nb);
+	struct net_device *dev = netdev_notifier_info_to_dev(ptr);
+	int port;
+
+	switch (event) {
+	case NETDEV_REGISTER:
+		if (dev == sw->conduit) {
+			xlnx_tsn_handle_conduit_changeaddr(sw);
+		} else {
+			port = xlnx_tsn_user_port_index(sw, dev);
+			if (port < 0)
+				return NOTIFY_DONE;
+
+			xlnx_tsn_handle_user_register(sw, dev, port);
+		}
+		break;
+	case NETDEV_CHANGEADDR:
+		if (dev == sw->conduit) {
+			xlnx_tsn_handle_conduit_changeaddr(sw);
+		} else {
+			port = xlnx_tsn_user_port_index(sw, dev);
+			if (port < 0)
+				return NOTIFY_DONE;
+
+			xlnx_tsn_set_port_mac_nibble(sw, port, dev->dev_addr[5]);
+		}
+		break;
+	}
+
+	return NOTIFY_DONE;
+}
+
 static enum dsa_tag_protocol xlnx_tsn_get_tag_protocol(struct dsa_switch *ds,
 						       int port,
 						       enum dsa_tag_protocol mp)
@@ -294,6 +470,30 @@ static enum dsa_tag_protocol xlnx_tsn_get_tag_protocol(struct dsa_switch *ds,
 	return DSA_TAG_PROTO_XLNX_TSN;
 }
 
+static int xlnx_tsn_port_set_mac_address(struct dsa_switch *ds, int port,
+					 const unsigned char *addr)
+{
+	u8 nibble = addr[5] & TSN_SW_MAC_NIBBLE_WILDCARD;
+	struct xlnx_tsn *sw = ds->priv;
+	struct dsa_port *dp;
+
+	if (!xlnx_tsn_prefix_matches(sw, addr))
+		return -EINVAL;
+
+	if (nibble == (sw->conduit->dev_addr[5] & TSN_SW_MAC_NIBBLE_WILDCARD))
+		return -EADDRINUSE;
+
+	dsa_switch_for_each_user_port(dp, ds) {
+		if (dp->index == port || !dp->user)
+			continue;
+
+		if ((dp->user->dev_addr[5] & TSN_SW_MAC_NIBBLE_WILDCARD) == nibble)
+			return -EADDRINUSE;
+	}
+
+	return 0;
+}
+
 static void xlnx_tsn_port_stp_state_set(struct dsa_switch *ds, int port,
 					u8 state)
 {
@@ -426,8 +626,10 @@ static const struct phylink_mac_ops xlnx_tsn_phylink_mac_ops = {
 
 static int xlnx_tsn_setup(struct dsa_switch *ds)
 {
+	struct dsa_port *cpu_dp = dsa_to_port(ds, XLNX_TSN_CPU_PORT);
 	struct xlnx_tsn *sw = ds->priv;
 	struct dsa_port *dp;
+	u32 mgmt;
 	int ret;
 
 	if (!dsa_is_user_port(ds, XLNX_TSN_PORT_MAC1) ||
@@ -435,6 +637,19 @@ static int xlnx_tsn_setup(struct dsa_switch *ds)
 		return dev_err_probe(sw->dev, -EINVAL,
 				     "both MAC1 and MAC2 must be enabled as switch ports\n");
 
+	if (!cpu_dp || !cpu_dp->conduit)
+		return -ENODEV;
+
+	sw->conduit = cpu_dp->conduit;
+
+	/* Route CPU-originated bridge-group control frames (STP, LLDP) to
+	 * the single wire port whose MAC-nibble field matches the frame's
+	 * source-MAC low nibble, instead of flooding to both.
+	 */
+	mgmt = sw_ior(sw, TSN_SW_MGMT_QUEUING_OFFSET);
+	mgmt |= TSN_SW_MGMT_QUEUING_EP_SA_EGRESS;
+	sw_iow(sw, TSN_SW_MGMT_QUEUING_OFFSET, mgmt);
+
 	/* CPU port stays in FORWARDING so host traffic always flows.
 	 * User ports start in DISABLED and transition from there under
 	 * bridge STP control.
@@ -451,7 +666,20 @@ static int xlnx_tsn_setup(struct dsa_switch *ds)
 			return ret;
 	}
 
-	return xlnx_tsn_mdio_register_all(sw);
+	ret = xlnx_tsn_mdio_register_all(sw);
+	if (ret)
+		return ret;
+
+	sw->nb.notifier_call = xlnx_tsn_netdev_event;
+	ret = register_netdevice_notifier(&sw->nb);
+	if (ret)
+		goto err_mdio;
+
+	return 0;
+
+err_mdio:
+	xlnx_tsn_mdio_unregister_all(sw);
+	return ret;
 }
 
 static void xlnx_tsn_teardown(struct dsa_switch *ds)
@@ -459,6 +687,7 @@ static void xlnx_tsn_teardown(struct dsa_switch *ds)
 	struct xlnx_tsn *sw = ds->priv;
 	struct dsa_port *dp;
 
+	unregister_netdevice_notifier(&sw->nb);
 	xlnx_tsn_mdio_unregister_all(sw);
 
 	dsa_switch_for_each_user_port(dp, ds)
@@ -471,6 +700,7 @@ static const struct dsa_switch_ops xlnx_tsn_switch_ops = {
 	.get_tag_protocol	= xlnx_tsn_get_tag_protocol,
 	.setup			= xlnx_tsn_setup,
 	.teardown		= xlnx_tsn_teardown,
+	.port_set_mac_address	= xlnx_tsn_port_set_mac_address,
 	.port_stp_state_set	= xlnx_tsn_port_stp_state_set,
 	.phylink_get_caps	= xlnx_tsn_phylink_get_caps,
 };
diff --git a/drivers/net/dsa/xilinx/xilinx_tsn.h b/drivers/net/dsa/xilinx/xilinx_tsn.h
index 6286caff7a1c..a228a7bebd53 100644
--- a/drivers/net/dsa/xilinx/xilinx_tsn.h
+++ b/drivers/net/dsa/xilinx/xilinx_tsn.h
@@ -7,7 +7,9 @@
 
 #include <linux/bitfield.h>
 #include <linux/bits.h>
+#include <linux/if_ether.h>
 #include <linux/io.h>
+#include <linux/notifier.h>
 #include <linux/types.h>
 #include <net/dsa.h>
 
@@ -16,12 +18,29 @@
 #define XLNX_TSN_PORT_MAC1	1
 #define XLNX_TSN_PORT_MAC2	2
 
+/* Unicast Frame Filter: the switch accepts an incoming frame when
+ * its destination MAC matches this 48-bit address under the 16-bit
+ * mask in the MSB register's upper half. A set mask bit acts as a
+ * wildcard for the corresponding bit of bytes 4..5.
+ */
+#define TSN_SW_MAC_LSB_OFFSET		0x0000c
+#define TSN_SW_MAC_MSB_OFFSET		0x00010
+#define TSN_SW_MAC_MSB_ADDR_MASK	GENMASK(15, 0)
+#define TSN_SW_MAC_MSB_MASK_MASK	GENMASK(31, 16)
+
+/* Mask value that covers the low nibble of byte 5, leaving a
+ * 44-bit prefix common to all switch-port MACs. Those 4 bits are
+ * filled in by the per-port MAC-nibble fields in the Switch Port
+ * State Control register below.
+ */
+#define TSN_SW_MAC_NIBBLE_WILDCARD	0x000f
+
 #define TSN_SW_STATUS_OFFSET		0x00000
 /* Poll this before changing port state. */
 #define TSN_SW_STATUS_READY		BIT(0)
 
-/* Switch Port State Control register: packs per-port STP state and
- * change-commit bits into one 32-bit word.
+/* Switch Port State Control register: packs per-port STP state,
+ * change-commit bits, and MAC-nibble fields into one 32-bit word.
  */
 #define TSN_PORT_STATE_CTRL_OFFSET	0x0004c
 
@@ -31,6 +50,12 @@
 #define MAC1_PORT_STATUS_MASK		GENMASK(11, 9)
 #define MAC2_PORT_STATUS_CHG_BIT	BIT(16)
 #define MAC2_PORT_STATUS_MASK		GENMASK(19, 17)
+#define EP_PORT_MAC_NIBBLE_MASK		GENMASK(7, 4)
+#define MAC1_PORT_MAC_NIBBLE_MASK	GENMASK(15, 12)
+#define MAC2_PORT_MAC_NIBBLE_MASK	GENMASK(23, 20)
+
+#define TSN_SW_MGMT_QUEUING_OFFSET		0x00054
+#define TSN_SW_MGMT_QUEUING_EP_SA_EGRESS	BIT(4)
 
 /* readl_poll_timeout() parameters (in microseconds): poll until
  * the port-state change-commit bit self-clears.
@@ -100,6 +125,13 @@ struct xlnx_tsn_mac {
  * @ds: DSA switch
  * @dev: backing device
  * @sw_base: switch fabric register window
+ * @conduit: DSA conduit netdev (EP MAC), used as the source of the
+ *	     shared 44-bit frame-filter prefix
+ * @mac_prefix: conduit MAC with byte 5's low nibble cleared to zero,
+ *		forming the 44-bit prefix common to all switch-port MACs
+ * @nb: netdev notifier that handles NETDEV_REGISTER on each swpN
+ *	to set its final MAC, and NETDEV_CHANGEADDR on the conduit
+ *	to refresh the shared prefix
  * @mac: per-MAC state, indexed by user-port number (index 0 unused;
  *	 MAC1 at [1], MAC2 at [2])
  */
@@ -107,6 +139,9 @@ struct xlnx_tsn {
 	struct dsa_switch ds;
 	struct device *dev;
 	void __iomem *sw_base;
+	struct net_device *conduit;
+	u8 mac_prefix[ETH_ALEN];
+	struct notifier_block nb;
 	struct xlnx_tsn_mac mac[XLNX_TSN_NUM_PORTS];
 };
 
-- 
2.34.1