From 722c7d1958d97345c8a52343fc4289daee6236d4 Mon Sep 17 00:00:00 2001
From: Jouni Malinen <jouni@codeaurora.org>
Date: Fri, 20 Sep 2019 23:54:05 +0300
Subject: [PATCH] wlantest: Process VLAN tagged Data frames

This allows Data frames to be fully processed for the case where VLAN
tags are used on the wireless link.

Signed-off-by: Jouni Malinen <jouni@codeaurora.org>
---
 src/utils/common.h |  3 +++
 wlantest/rx_data.c | 27 +++++++++++++++++++++++++++
 2 files changed, 30 insertions(+)

diff --git a/src/utils/common.h b/src/utils/common.h
index 17411451d..833469a52 100644
--- a/src/utils/common.h
+++ b/src/utils/common.h
@@ -344,6 +344,9 @@ static inline void WPA_PUT_LE64(u8 *a, u64 val)
 #ifndef ETH_P_OUI
 #define ETH_P_OUI 0x88B7
 #endif /* ETH_P_OUI */
+#ifndef ETH_P_8021Q
+#define ETH_P_8021Q 0x8100
+#endif /* ETH_P_8021Q */
 
 
 #ifdef __GNUC__
diff --git a/wlantest/rx_data.c b/wlantest/rx_data.c
index 28202d822..d6c7a54c7 100644
--- a/wlantest/rx_data.c
+++ b/wlantest/rx_data.c
@@ -52,6 +52,29 @@ static const char * data_stype(u16 stype)
 }
 
 
+static void rx_data_eth(struct wlantest *wt, const u8 *bssid,
+			const u8 *sta_addr, const u8 *dst, const u8 *src,
+			u16 ethertype, const u8 *data, size_t len, int prot,
+			const u8 *peer_addr);
+
+static void rx_data_vlan(struct wlantest *wt, 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)
+{
+	u16 tag;
+
+	if (len < 4)
+		return;
+	tag = WPA_GET_BE16(data);
+	wpa_printf(MSG_MSGDUMP, "VLAN tag: Priority=%u ID=%u",
+		   tag >> 12, tag & 0x0ffff);
+	/* ignore VLAN information and process the original frame */
+	rx_data_eth(wt, bssid, sta_addr, dst, src, WPA_GET_BE16(data + 2),
+		    data + 4, len - 4, prot, peer_addr);
+}
+
+
 static void rx_data_eth(struct wlantest *wt, const u8 *bssid,
 			const u8 *sta_addr, const u8 *dst, const u8 *src,
 			u16 ethertype, const u8 *data, size_t len, int prot,
@@ -68,6 +91,10 @@ static void rx_data_eth(struct wlantest *wt, const u8 *bssid,
 	case 0x890d:
 		rx_data_80211_encap(wt, bssid, sta_addr, dst, src, data, len);
 		break;
+	case ETH_P_8021Q:
+		rx_data_vlan(wt, bssid, sta_addr, dst, src, data, len, prot,
+			     peer_addr);
+		break;
 	}
 }