From e90ededb4bfaf442ad4aaebe636945d19f0923c3 Mon Sep 17 00:00:00 2001
From: Jouni Malinen <jouni@codeaurora.org>
Date: Tue, 9 Mar 2021 20:20:24 +0200
Subject: [PATCH] wlantest: Skip Mesh Control field from the beginning of
 payload

This allows correct processing of Data frames with Mesh Control field by
finding the LLC/SNAP header after that field.

Signed-off-by: Jouni Malinen <jouni@codeaurora.org>
---
 wlantest/bss.c      |  2 ++
 wlantest/rx_data.c  | 47 +++++++++++++++++++++++++++++++++++++--------
 wlantest/wlantest.h |  1 +
 3 files changed, 42 insertions(+), 8 deletions(-)

diff --git a/wlantest/bss.c b/wlantest/bss.c
index 0e5b60386..1834aecba 100644
--- a/wlantest/bss.c
+++ b/wlantest/bss.c
@@ -227,6 +227,8 @@ void bss_update(struct wlantest *wt, struct wlantest_bss *bss,
 	if (elems->mdie)
 		os_memcpy(bss->mdid, elems->mdie, 2);
 
+	bss->mesh = elems->mesh_id != NULL;
+
 	if (!update)
 		return;
 
diff --git a/wlantest/rx_data.c b/wlantest/rx_data.c
index 428900f24..8cb2d3718 100644
--- a/wlantest/rx_data.c
+++ b/wlantest/rx_data.c
@@ -99,15 +99,46 @@ static void rx_data_eth(struct wlantest *wt, const u8 *bssid,
 }
 
 
-static void rx_data_process(struct wlantest *wt, const u8 *bssid,
+static void rx_data_process(struct wlantest *wt, struct wlantest_bss *bss,
+			    const u8 *bssid,
 			    const u8 *sta_addr,
 			    const u8 *dst, const u8 *src,
 			    const u8 *data, size_t len, int prot,
-			    const u8 *peer_addr)
+			    const u8 *peer_addr, const u8 *qos)
 {
 	if (len == 0)
 		return;
 
+	if (bss && bss->mesh && qos && !(qos[0] & BIT(7)) &&
+	    (qos[1] & BIT(0))) {
+		u8 addr_ext_mode;
+		size_t mesh_control_len = 6;
+
+		/* Skip Mesh Control field if this is not an A-MSDU */
+		if (len < mesh_control_len) {
+			wpa_printf(MSG_DEBUG,
+				   "Not enough room for Mesh Control field");
+			return;
+		}
+
+		addr_ext_mode = data[0] & 0x03;
+		if (addr_ext_mode == 3) {
+			wpa_printf(MSG_DEBUG,
+				   "Reserved Mesh Control :: Address Extension Mode");
+			return;
+		}
+
+		mesh_control_len += addr_ext_mode * ETH_ALEN;
+		if (len < mesh_control_len) {
+			wpa_printf(MSG_DEBUG,
+				   "Not enough room for Mesh Address Extension");
+			return;
+		}
+
+		len -= mesh_control_len;
+		data += mesh_control_len;
+	}
+
 	if (len >= 8 && os_memcmp(data, "\xaa\xaa\x03\x00\x00\x00", 6) == 0) {
 		rx_data_eth(wt, bssid, sta_addr, dst, src,
 			    WPA_GET_BE16(data + 6), data + 8, len - 8, prot,
@@ -310,8 +341,8 @@ skip_replay_det:
 				 bss->gtk_len[keyid]);
 		add_note(wt, MSG_EXCESSIVE, "GTK[%d] %s", keyid, gtk);
 	process:
-		rx_data_process(wt, bss->bssid, NULL, dst, src, decrypted,
-				dlen, 1, NULL);
+		rx_data_process(wt, bss, bss->bssid, NULL, dst, src, decrypted,
+				dlen, 1, NULL, qos);
 		if (!replay)
 			os_memcpy(bss->rsc[keyid], pn, 6);
 		write_pcap_decrypted(wt, (const u8 *) hdr, hdrlen,
@@ -622,8 +653,8 @@ check_zero_tk:
 			peer_addr = hdr->addr1;
 		if (!replay && rsc)
 			os_memcpy(rsc, pn, 6);
-		rx_data_process(wt, bss->bssid, sta->addr, dst, src, decrypted,
-				dlen, 1, peer_addr);
+		rx_data_process(wt, bss, bss->bssid, sta->addr, dst, src,
+				decrypted, dlen, 1, peer_addr, qos);
 		write_pcap_decrypted(wt, (const u8 *) hdr, hdrlen,
 				     decrypted, dlen);
 	} else if (sta->tptk_set) {
@@ -726,8 +757,8 @@ static void rx_data_bss(struct wlantest *wt, const struct ieee80211_hdr *hdr,
 			}
 		}
 
-		rx_data_process(wt, bssid, sta_addr, dst, src, data, len, 0,
-				peer_addr);
+		rx_data_process(wt, bss, bssid, sta_addr, dst, src, data, len,
+				0, peer_addr, qos);
 	}
 }
 
diff --git a/wlantest/wlantest.h b/wlantest/wlantest.h
index 658a3a06d..af29f578f 100644
--- a/wlantest/wlantest.h
+++ b/wlantest/wlantest.h
@@ -170,6 +170,7 @@ struct wlantest_bss {
 	u8 r0kh_id[FT_R0KH_ID_MAX_LEN];
 	size_t r0kh_id_len;
 	u8 r1kh_id[FT_R1KH_ID_LEN];
+	bool mesh;
 };
 
 struct wlantest_radius {