From: Mattias Forsblad <mattias.forsblad@gmail.com>

Support connecting Marvell dsa tagger to the switch driver frame2reg
function to decode received RMU frames.

(Note: during teardown or failed setup, tagger_data may be NULL when
a late FRAME2REG frame arrives.)

Signed-off-by: Luke Howard <lukeh@padl.com>
Signed-off-by: Mattias Forsblad <mattias.forsblad@gmail.com>
Signed-off-by: Andrew Lunn <andrew@lunn.ch>
---
 include/linux/dsa/mv88e6xxx.h |  8 ++++++++
 net/dsa/tag_dsa.c             | 43 ++++++++++++++++++++++++++++++++++++++-----
 2 files changed, 46 insertions(+), 5 deletions(-)

diff --git a/include/linux/dsa/mv88e6xxx.h b/include/linux/dsa/mv88e6xxx.h
index 8c3d45eca46bd..1e9460d6f7782 100644
--- a/include/linux/dsa/mv88e6xxx.h
+++ b/include/linux/dsa/mv88e6xxx.h
@@ -6,6 +6,14 @@
 #define _NET_DSA_TAG_MV88E6XXX_H
 
 #include <linux/if_vlan.h>
+#include <net/dsa.h>
+
+struct dsa_tagger_data {
+	/* DSA frame decoded to be from the RMU */
+	void (*rmu_frame2reg)(struct dsa_switch *ds,
+			      struct sk_buff *skb,
+			      u8 seqno);
+};
 
 #define MV88E6XXX_VID_STANDALONE	0
 #define MV88E6XXX_VID_BRIDGED		(VLAN_N_VID - 1)
diff --git a/net/dsa/tag_dsa.c b/net/dsa/tag_dsa.c
index d5ffee35fbb53..5a95873e87340 100644
--- a/net/dsa/tag_dsa.c
+++ b/net/dsa/tag_dsa.c
@@ -201,14 +201,17 @@ static struct sk_buff *dsa_xmit_ll(struct sk_buff *skb, struct net_device *dev,
 static struct sk_buff *dsa_rcv_ll(struct sk_buff *skb, struct net_device *dev,
 				  u8 extra)
 {
+	struct dsa_tagger_data *tagger_data;
 	bool trap = false, trunk = false;
 	int source_device, source_port;
+	struct dsa_switch *ds;
 	enum dsa_code code;
 	enum dsa_cmd cmd;
 	u8 *dsa_header;
 
 	/* The ethertype field is part of the DSA header. */
 	dsa_header = dsa_etype_header_pos_rx(skb);
+	source_device = dsa_header[0] & 0x1f;
 
 	cmd = dsa_header[0] >> 6;
 	switch (cmd) {
@@ -220,12 +223,20 @@ static struct sk_buff *dsa_rcv_ll(struct sk_buff *skb, struct net_device *dev,
 		code = (dsa_header[1] & 0x6) | ((dsa_header[2] >> 4) & 1);
 
 		switch (code) {
-		case DSA_CODE_FRAME2REG:
-			/* Remote management is not implemented yet,
-			 * drop.
-			 */
+		case DSA_CODE_FRAME2REG: {
+			u8 seqno = dsa_header[3];
+
+			ds = dsa_conduit_find_switch(dev, source_device);
+			if (!ds) {
+				kfree_skb(skb);
+				return NULL;
+			}
+			tagger_data = ds->tagger_data;
+			if (likely(tagger_data && tagger_data->rmu_frame2reg))
+				tagger_data->rmu_frame2reg(ds, skb, seqno);
 			kfree_skb(skb);
 			return NULL;
+		}
 		case DSA_CODE_ARP_MIRROR:
 		case DSA_CODE_POLICY_MIRROR:
 			/* Mark mirrored packets to notify any upper
@@ -256,7 +267,6 @@ static struct sk_buff *dsa_rcv_ll(struct sk_buff *skb, struct net_device *dev,
 		return NULL;
 	}
 
-	source_device = dsa_header[0] & 0x1f;
 	source_port = (dsa_header[1] >> 3) & 0x1f;
 
 	if (trunk) {
@@ -331,6 +341,25 @@ static struct sk_buff *dsa_rcv_ll(struct sk_buff *skb, struct net_device *dev,
 	return skb;
 }
 
+static int dsa_tag_connect(struct dsa_switch *ds)
+{
+	struct dsa_tagger_data *tagger_data;
+
+	tagger_data = kzalloc_obj(*tagger_data, GFP_KERNEL);
+	if (!tagger_data)
+		return -ENOMEM;
+
+	ds->tagger_data = tagger_data;
+
+	return 0;
+}
+
+static void dsa_tag_disconnect(struct dsa_switch *ds)
+{
+	kfree(ds->tagger_data);
+	ds->tagger_data = NULL;
+}
+
 #if IS_ENABLED(CONFIG_NET_DSA_TAG_DSA)
 
 static struct sk_buff *dsa_xmit(struct sk_buff *skb, struct net_device *dev)
@@ -353,6 +382,8 @@ static const struct dsa_device_ops dsa_netdev_ops = {
 	.proto	  = DSA_TAG_PROTO_DSA,
 	.xmit	  = dsa_xmit,
 	.rcv	  = dsa_rcv,
+	.connect  = dsa_tag_connect,
+	.disconnect = dsa_tag_disconnect,
 	.needed_headroom = DSA_HLEN,
 };
 
@@ -397,6 +428,8 @@ static const struct dsa_device_ops edsa_netdev_ops = {
 	.proto	  = DSA_TAG_PROTO_EDSA,
 	.xmit	  = edsa_xmit,
 	.rcv	  = edsa_rcv,
+	.connect  = dsa_tag_connect,
+	.disconnect = dsa_tag_disconnect,
 	.needed_headroom = EDSA_HLEN,
 };
 

-- 
2.43.0