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>

diff --git a/wlantest/bss.c b/wlantest/bss.c
index 0e5b60386f452cae0c0af966dfed1e3b3563c953..1834aecba199a58d1f139951c111bdbe7466fece 100644 (file)
--- 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 428900f24c40548490e965ea4e94dcda2b6a415a..8cb2d37187eb6b992863375b16e3bef99342d365 100644 (file)
--- 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 658a3a06d43b9c501691e5e714e18506e6bb3f4f..af29f578f0e55264397e2cb157f5bac20d06f854 100644 (file)
--- 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 {