[RFC PATCH net-next] net: dsa: tag_qca: Check for upstream VLAN tag

[Matthew Hagan <mnhagan88@gmail.com>]

The qca_tag_rcv function unconditionally expects a QCA tag to be present
between source MAC and EtherType. However if an upstream switch is used,
this may create a special case where VLAN tags are subsequently inserted
between the source MAC and the QCA tag. Thus when qca_tag_rcv is called,
it will attempt to read the 802.1q TPID as a QCA tag. This results in
complication since the TPID will pass the QCA tag version checking on bits
14 and 15, but the resulting packet after trimming the TPID will be
unusable.

The tested case is a Meraki MX65 which features two QCA8337 switches with
their CPU ports attached to a BCM58625 switch ports 4 and 5 respectively.
In this case a VLAN tag with VID 0 is added by the upstream BCM switch
when the port is unconfigured and packets with this VLAN tag or without
will be accepted at the BCM's CPU port. However, it is arguably possible
that other switches may be configured to drop VLAN untagged traffic at
their respective CPU port. Thus where packets are VLAN untagged, the
default VLAN tag, added by the upstream switch, should be maintained. Where
inbound packets are already VLAN tagged when arriving at the QCA switch, we
should replace the default VLAN tag, added by the upstream port, with the
correct VLAN tag.

This patch introduces:
  1 - A check for a VLAN tag before EtherType. If found, skip past this to
      find the QCA tag.
  2 - Check for a second VLAN tag after the QCA tag if one was found in 1.
      If found, remove both the initial VLAN tag and the QCA tag. If not
      found, remove only the QCA tag to maintain the VLAN tag added by the
      upstream switch.

Signed-off-by: Matthew Hagan <mnhagan88@gmail.com>
---
 net/dsa/tag_qca.c | 41 +++++++++++++++++++++++++++++++----------
 1 file changed, 31 insertions(+), 10 deletions(-)

diff --git a/net/dsa/tag_qca.c b/net/dsa/tag_qca.c
index 88181b52f480..e5273a27bf8a 100644
--- a/net/dsa/tag_qca.c
+++ b/net/dsa/tag_qca.c
@@ -52,18 +52,27 @@ static struct sk_buff *qca_tag_rcv(struct sk_buff *skb, struct net_device *dev,
 				   struct packet_type *pt)
 {
 	u8 ver;
-	u16  hdr;
-	int port;
-	__be16 *phdr;
+	u16 hdr, vlan_hdr;
+	int port, vlan_offset = 0, vlan_skip = 0;
+	__be16 *phdr, *vlan_phdr;
 
 	if (unlikely(!pskb_may_pull(skb, QCA_HDR_LEN)))
 		return NULL;
 
-	/* The QCA header is added by the switch between src addr and Ethertype
-	 * At this point, skb->data points to ethertype so header should be
-	 * right before
+	/* The QCA header is added by the switch between src addr and
+	 * Ethertype. Normally at this point, skb->data points to ethertype so the
+	 * header should be right before. However if a VLAN tag has subsequently
+	 * been added upstream, we need to skip past it to find the QCA header.
 	 */
-	phdr = (__be16 *)(skb->data - 2);
+	vlan_phdr = (__be16 *)(skb->data - 2);
+	vlan_hdr = ntohs(*vlan_phdr);
+
+	/* Check for VLAN tag before QCA tag */
+	if (!(vlan_hdr ^ ETH_P_8021Q))
+		vlan_offset = VLAN_HLEN;
+
+	/* Look for QCA tag at the correct location */
+	phdr = (__be16 *)(skb->data - 2 + vlan_offset);
 	hdr = ntohs(*phdr);
 
 	/* Make sure the version is correct */
@@ -71,10 +80,22 @@ static struct sk_buff *qca_tag_rcv(struct sk_buff *skb, struct net_device *dev,
 	if (unlikely(ver != QCA_HDR_VERSION))
 		return NULL;
 
+	/* Check for second VLAN tag after QCA tag if one was found prior */
+	if (!!(vlan_offset)) {
+		vlan_phdr = (__be16 *)(skb->data + 4);
+		vlan_hdr = ntohs(*vlan_phdr);
+		if (!!(vlan_hdr ^ ETH_P_8021Q)) {
+		/* Do not remove existing tag in case a tag is required */
+			vlan_offset = 0;
+			vlan_skip = VLAN_HLEN;
+		}
+	}
+
 	/* Remove QCA tag and recalculate checksum */
-	skb_pull_rcsum(skb, QCA_HDR_LEN);
-	memmove(skb->data - ETH_HLEN, skb->data - ETH_HLEN - QCA_HDR_LEN,
-		ETH_HLEN - QCA_HDR_LEN);
+	skb_pull_rcsum(skb, QCA_HDR_LEN + vlan_offset);
+	memmove(skb->data - ETH_HLEN,
+		skb->data - ETH_HLEN - QCA_HDR_LEN - vlan_offset,
+		ETH_HLEN - QCA_HDR_LEN + vlan_skip);
 
 	/* Get source port information */
 	port = (hdr & QCA_HDR_RECV_SOURCE_PORT_MASK);
-- 
2.26.3