From 838fcae7f51ced6d011f7f0e4d705f8072426a16 Mon Sep 17 00:00:00 2001
From: Felix Fietkau <nbd@nbd.name>
Date: Tue, 23 Nov 2021 11:49:04 +0100
Subject: [PATCH] mt76: mt7615: fix decap offload corner case with 4-addr VLAN frames
Git-commit: 838fcae7f51ced6d011f7f0e4d705f8072426a16
Patch-mainline: v5.17-rc1
References: bsc#1209980

With 4-address mode VLAN frames, an internal header translation step fails,
leaving behind an extra 2-byte length field that must be reomved by the driver.
Add a check for this condition to fix receiving such packets

Signed-off-by: Felix Fietkau <nbd@nbd.name>
Acked-by: Takashi Iwai <tiwai@suse.de>

---
 .../net/wireless/mediatek/mt76/mt7615/mac.c   | 23 +++++++++++++++++--
 1 file changed, 21 insertions(+), 2 deletions(-)

diff --git a/drivers/net/wireless/mediatek/mt76/mt7615/mac.c b/drivers/net/wireless/mediatek/mt76/mt7615/mac.c
index e2b99b6163e1..ec25e5a95d44 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7615/mac.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7615/mac.c
@@ -600,10 +600,29 @@ static int mt7615_mac_fill_rx(struct mt7615_dev *dev, struct sk_buff *skb)
 			return -EINVAL;
 		hdr_trans = false;
 	} else {
+		int pad_start = 0;
+
 		skb_pull(skb, hdr_gap);
 		if (!hdr_trans && status->amsdu) {
-			memmove(skb->data + 2, skb->data,
-				ieee80211_get_hdrlen_from_skb(skb));
+			pad_start = ieee80211_get_hdrlen_from_skb(skb);
+		} else if (hdr_trans && (rxd2 & MT_RXD2_NORMAL_HDR_TRANS_ERROR)) {
+			/*
+			 * When header translation failure is indicated,
+			 * the hardware will insert an extra 2-byte field
+			 * containing the data length after the protocol
+			 * type field.
+			 */
+			pad_start = 12;
+			if (get_unaligned_be16(skb->data + pad_start) == ETH_P_8021Q)
+				pad_start += 4;
+
+			if (get_unaligned_be16(skb->data + pad_start) !=
+			    skb->len - pad_start - 2)
+				pad_start = 0;
+		}
+
+		if (pad_start) {
+			memmove(skb->data + 2, skb->data, pad_start);
 			skb_pull(skb, 2);
 		}
 	}
-- 
2.35.3