macsec: Add configuration parameters for hostapd

[thirdparty/hostap.git] / wlantest / rx_data.c

diff --git a/wlantest/rx_data.c b/wlantest/rx_data.c
index 384e4dc7e6926df3f120814efef3cbb077c58431..bafc33fd95e0579d06e9c57ee5cf12a87836aa8b 100644 (file)
--- a/wlantest/rx_data.c
+++ b/wlantest/rx_data.c
@@ -146,6 +146,7 @@ static void rx_data_bss_prot_group(struct wlantest *wt,
        u8 *decrypted = NULL;
        size_t dlen;
        u8 pn[6];
+       int replay = 0;
 
        bss = bss_get(wt, hdr->addr2);
        if (bss == NULL)
@@ -211,6 +212,7 @@ static void rx_data_bss_prot_group(struct wlantest *wt,
                         " Retry" : "");
                wpa_hexdump(MSG_INFO, "RX PN", pn, 6);
                wpa_hexdump(MSG_INFO, "RSC", bss->rsc[keyid], 6);
+               replay = 1;
        }
 
 skip_replay_det:
@@ -233,7 +235,8 @@ skip_replay_det:
        if (decrypted) {
                rx_data_process(wt, bss->bssid, NULL, dst, src, decrypted,
                                dlen, 1, NULL);
-               os_memcpy(bss->rsc[keyid], pn, 6);
+               if (!replay)
+                       os_memcpy(bss->rsc[keyid], pn, 6);
                write_pcap_decrypted(wt, (const u8 *) hdr, hdrlen,
                                     decrypted, dlen);
        } else
@@ -247,7 +250,7 @@ static void rx_data_bss_prot(struct wlantest *wt,
                             const u8 *qos, const u8 *dst, const u8 *src,
                             const u8 *data, size_t len)
 {
-       struct wlantest_bss *bss;
+       struct wlantest_bss *bss, *bss2;
        struct wlantest_sta *sta, *sta2;
        int keyid;
        u16 fc = le_to_host16(hdr->frame_control);
@@ -259,6 +262,7 @@ static void rx_data_bss_prot(struct wlantest *wt,
        const u8 *tk = NULL;
        int ptk_iter_done = 0;
        int try_ptk_iter = 0;
+       int replay = 0;
 
        if (hdr->addr1[0] & 0x01) {
                rx_data_bss_prot_group(wt, hdr, hdrlen, qos, dst, src,
@@ -271,9 +275,20 @@ static void rx_data_bss_prot(struct wlantest *wt,
                bss = bss_find(wt, hdr->addr1);
                if (bss) {
                        sta = sta_find(bss, hdr->addr2);
-                       if (sta)
+                       if (sta) {
                                sta->counters[
                                        WLANTEST_STA_COUNTER_PROT_DATA_TX]++;
+                       }
+                       if (!sta || !sta->ptk_set) {
+                               bss2 = bss_find(wt, hdr->addr2);
+                               if (bss2) {
+                                       sta2 = sta_find(bss2, hdr->addr1);
+                                       if (sta2 && (!sta || sta2->ptk_set)) {
+                                               bss = bss2;
+                                               sta = sta2;
+                                       }
+                               }
+                       }
                } else {
                        bss = bss_find(wt, hdr->addr2);
                        if (!bss)
@@ -389,6 +404,12 @@ static void rx_data_bss_prot(struct wlantest *wt,
                        rsc = tdls->rsc_init[tid];
                else
                        rsc = tdls->rsc_resp[tid];
+       } else if ((fc & (WLAN_FC_TODS | WLAN_FC_FROMDS)) ==
+                  (WLAN_FC_TODS | WLAN_FC_FROMDS)) {
+               if (os_memcmp(sta->addr, hdr->addr2, ETH_ALEN) == 0)
+                       rsc = sta->rsc_tods[tid];
+               else
+                       rsc = sta->rsc_fromds[tid];
        } else if (fc & WLAN_FC_TODS)
                rsc = sta->rsc_tods[tid];
        else
@@ -413,6 +434,7 @@ static void rx_data_bss_prot(struct wlantest *wt,
                         " Retry" : "");
                wpa_hexdump(MSG_INFO, "RX PN", pn, 6);
                wpa_hexdump(MSG_INFO, "RSC", rsc, 6);
+               replay = 1;
        }
 
 skip_replay_det:
@@ -421,7 +443,7 @@ skip_replay_det:
                        decrypted = ccmp_256_decrypt(tk, hdr, data, len, &dlen);
                else if (sta->pairwise_cipher == WPA_CIPHER_GCMP ||
                         sta->pairwise_cipher == WPA_CIPHER_GCMP_256)
-                       decrypted = gcmp_decrypt(tk, sta->tk_len, hdr, data,
+                       decrypted = gcmp_decrypt(tk, sta->ptk.tk_len, hdr, data,
                                                 len, &dlen);
                else
                        decrypted = ccmp_decrypt(tk, hdr, data, len, &dlen);
@@ -435,7 +457,7 @@ skip_replay_det:
                                                     len, &dlen);
                else if (sta->pairwise_cipher == WPA_CIPHER_GCMP ||
                         sta->pairwise_cipher == WPA_CIPHER_GCMP_256)
-                       decrypted = gcmp_decrypt(sta->ptk.tk, sta->tk_len,
+                       decrypted = gcmp_decrypt(sta->ptk.tk, sta->ptk.tk_len,
                                                 hdr, data, len, &dlen);
                else
                        decrypted = ccmp_decrypt(sta->ptk.tk, hdr, data, len,
@@ -457,7 +479,8 @@ skip_replay_det:
                const u8 *peer_addr = NULL;
                if (!(fc & (WLAN_FC_FROMDS | WLAN_FC_TODS)))
                        peer_addr = hdr->addr1;
-               os_memcpy(rsc, pn, 6);
+               if (!replay)
+                       os_memcpy(rsc, pn, 6);
                rx_data_process(wt, bss->bssid, sta->addr, dst, src, decrypted,
                                dlen, 1, peer_addr);
                write_pcap_decrypted(wt, (const u8 *) hdr, hdrlen,