wlantest/rx_data.c

   1 /*
   2  * Received Data frame processing
   3  * Copyright (c) 2010, Jouni Malinen <j@w1.fi>
   4  *
   5  * This program is free software; you can redistribute it and/or modify
   6  * it under the terms of the GNU General Public License version 2 as
   7  * published by the Free Software Foundation.
   8  *
   9  * Alternatively, this software may be distributed under the terms of BSD
  10  * license.
  11  *
  12  * See README and COPYING for more details.
  13  */
  14
  15 #include "utils/includes.h"
  16
  17 #include "utils/common.h"
  18 #include "crypto/aes_wrap.h"
  19 #include "crypto/crypto.h"
  20 #include "common/defs.h"
  21 #include "common/ieee802_11_defs.h"
  22 #include "common/eapol_common.h"
  23 #include "common/wpa_common.h"
  24 #include "rsn_supp/wpa_ie.h"
  25 #include "wlantest.h"
  26
  27
  28 static const char * data_stype(u16 stype)
  29 {
  30         switch (stype) {
  31         case WLAN_FC_STYPE_DATA:
  32                 return "DATA";
  33         case WLAN_FC_STYPE_DATA_CFACK:
  34                 return "DATA-CFACK";
  35         case WLAN_FC_STYPE_DATA_CFPOLL:
  36                 return "DATA-CFPOLL";
  37         case WLAN_FC_STYPE_DATA_CFACKPOLL:
  38                 return "DATA-CFACKPOLL";
  39         case WLAN_FC_STYPE_NULLFUNC:
  40                 return "NULLFUNC";
  41         case WLAN_FC_STYPE_CFACK:
  42                 return "CFACK";
  43         case WLAN_FC_STYPE_CFPOLL:
  44                 return "CFPOLL";
  45         case WLAN_FC_STYPE_CFACKPOLL:
  46                 return "CFACKPOLL";
  47         case WLAN_FC_STYPE_QOS_DATA:
  48                 return "QOSDATA";
  49         case WLAN_FC_STYPE_QOS_DATA_CFACK:
  50                 return "QOSDATA-CFACK";
  51         case WLAN_FC_STYPE_QOS_DATA_CFPOLL:
  52                 return "QOSDATA-CFPOLL";
  53         case WLAN_FC_STYPE_QOS_DATA_CFACKPOLL:
  54                 return "QOSDATA-CFACKPOLL";
  55         case WLAN_FC_STYPE_QOS_NULL:
  56                 return "QOS-NULL";
  57         case WLAN_FC_STYPE_QOS_CFPOLL:
  58                 return "QOS-CFPOLL";
  59         case WLAN_FC_STYPE_QOS_CFACKPOLL:
  60                 return "QOS-CFACKPOLL";
  61         }
  62         return "??";
  63 }
  64
  65
  66 static int check_mic(const u8 *kck, int ver, const u8 *data, size_t len)
  67 {
  68         u8 *buf;
  69         int ret = -1;
  70         struct ieee802_1x_hdr *hdr;
  71         struct wpa_eapol_key *key;
  72         u8 rx_mic[16];
  73
  74         buf = os_malloc(len);
  75         if (buf == NULL)
  76                 return -1;
  77         os_memcpy(buf, data, len);
  78         hdr = (struct ieee802_1x_hdr *) buf;
  79         key = (struct wpa_eapol_key *) (hdr + 1);
  80
  81         os_memcpy(rx_mic, key->key_mic, 16);
  82         os_memset(key->key_mic, 0, 16);
  83
  84         if (wpa_eapol_key_mic(kck, ver, buf, len, key->key_mic) == 0 &&
  85             os_memcmp(rx_mic, key->key_mic, 16) == 0)
  86                 ret = 0;
  87
  88         os_free(buf);
  89
  90         return ret;
  91 }
  92
  93
  94 static void rx_data_eapol_key_1_of_4(struct wlantest *wt, const u8 *dst,
  95                                      const u8 *src, const u8 *data, size_t len)
  96 {
  97         struct wlantest_bss *bss;
  98         struct wlantest_sta *sta;
  99         const struct ieee802_1x_hdr *eapol;
 100         const struct wpa_eapol_key *hdr;
 101
 102         wpa_printf(MSG_DEBUG, "EAPOL-Key 1/4 " MACSTR " -> " MACSTR,
 103                    MAC2STR(src), MAC2STR(dst));
 104         bss = bss_get(wt, src);
 105         if (bss == NULL)
 106                 return;
 107         sta = sta_get(bss, dst);
 108         if (sta == NULL)
 109                 return;
 110
 111         eapol = (const struct ieee802_1x_hdr *) data;
 112         hdr = (const struct wpa_eapol_key *) (eapol + 1);
 113         os_memcpy(sta->anonce, hdr->key_nonce, WPA_NONCE_LEN);
 114 }
 115
 116
 117 static int try_pmk(struct wlantest_bss *bss, struct wlantest_sta *sta,
 118                    u16 ver, const u8 *data, size_t len,
 119                    struct wlantest_pmk *pmk)
 120 {
 121         struct wpa_ptk ptk;
 122         size_t ptk_len = sta->pairwise_cipher == WPA_CIPHER_TKIP ? 64 : 48;
 123         wpa_pmk_to_ptk(pmk->pmk, sizeof(pmk->pmk),
 124                        "Pairwise key expansion",
 125                        bss->bssid, sta->addr, sta->anonce, sta->snonce,
 126                        (u8 *) &ptk, ptk_len,
 127                        wpa_key_mgmt_sha256(sta->key_mgmt));
 128         if (check_mic(ptk.kck, ver, data, len) < 0)
 129                 return -1;
 130
 131         wpa_printf(MSG_INFO, "Derived PTK for STA " MACSTR " BSSID " MACSTR,
 132                    MAC2STR(sta->addr), MAC2STR(bss->bssid));
 133         os_memcpy(&sta->ptk, &ptk, sizeof(ptk));
 134         wpa_hexdump(MSG_DEBUG, "PTK:KCK", sta->ptk.kck, 16);
 135         wpa_hexdump(MSG_DEBUG, "PTK:KEK", sta->ptk.kek, 16);
 136         wpa_hexdump(MSG_DEBUG, "PTK:TK1", sta->ptk.tk1, 16);
 137         if (ptk_len > 48)
 138                 wpa_hexdump(MSG_DEBUG, "PTK:TK2", sta->ptk.u.tk2, 16);
 139         sta->ptk_set = 1;
 140         os_memset(sta->rsc_tods, 0, sizeof(sta->rsc_tods));
 141         os_memset(sta->rsc_fromds, 0, sizeof(sta->rsc_fromds));
 142         return 0;
 143 }
 144
 145
 146 static void derive_ptk(struct wlantest *wt, struct wlantest_bss *bss,
 147                        struct wlantest_sta *sta, u16 ver,
 148                        const u8 *data, size_t len)
 149 {
 150         struct wlantest_pmk *pmk;
 151
 152         dl_list_for_each(pmk, &bss->pmk, struct wlantest_pmk, list) {
 153                 if (try_pmk(bss, sta, ver, data, len, pmk) == 0)
 154                         return;
 155         }
 156
 157         dl_list_for_each(pmk, &wt->pmk, struct wlantest_pmk, list) {
 158                 if (try_pmk(bss, sta, ver, data, len, pmk) == 0)
 159                         return;
 160         }
 161 }
 162
 163
 164 static void rx_data_eapol_key_2_of_4(struct wlantest *wt, const u8 *dst,
 165                                      const u8 *src, const u8 *data, size_t len)
 166 {
 167         struct wlantest_bss *bss;
 168         struct wlantest_sta *sta;
 169         const struct ieee802_1x_hdr *eapol;
 170         const struct wpa_eapol_key *hdr;
 171         const u8 *key_data;
 172         u16 key_info, key_data_len;
 173         struct wpa_eapol_ie_parse ie;
 174
 175         wpa_printf(MSG_DEBUG, "EAPOL-Key 2/4 " MACSTR " -> " MACSTR,
 176                    MAC2STR(src), MAC2STR(dst));
 177         bss = bss_get(wt, dst);
 178         if (bss == NULL)
 179                 return;
 180         sta = sta_get(bss, src);
 181         if (sta == NULL)
 182                 return;
 183
 184         eapol = (const struct ieee802_1x_hdr *) data;
 185         hdr = (const struct wpa_eapol_key *) (eapol + 1);
 186         os_memcpy(sta->snonce, hdr->key_nonce, WPA_NONCE_LEN);
 187         key_info = WPA_GET_BE16(hdr->key_info);
 188         key_data_len = WPA_GET_BE16(hdr->key_data_length);
 189         derive_ptk(wt, bss, sta, key_info & WPA_KEY_INFO_TYPE_MASK, data, len);
 190
 191         if (!sta->ptk_set) {
 192                 wpa_printf(MSG_DEBUG, "No PTK known to process EAPOL-Key 2/4");
 193                 return;
 194         }
 195
 196         if (check_mic(sta->ptk.kck, key_info & WPA_KEY_INFO_TYPE_MASK,
 197                       data, len) < 0) {
 198                 wpa_printf(MSG_INFO, "Mismatch in EAPOL-Key 2/4 MIC");
 199                 return;
 200         }
 201         wpa_printf(MSG_DEBUG, "Valid MIC found in EAPOL-Key 2/4");
 202
 203         key_data = (const u8 *) (hdr + 1);
 204
 205         if (wpa_supplicant_parse_ies(key_data, key_data_len, &ie) < 0) {
 206                 wpa_printf(MSG_INFO, "Failed to parse EAPOL-Key Key Data");
 207                 return;
 208         }
 209
 210         if (ie.wpa_ie) {
 211                 wpa_hexdump(MSG_MSGDUMP, "EAPOL-Key Key Data - WPA IE",
 212                             ie.wpa_ie, ie.wpa_ie_len);
 213                 if (os_memcmp(ie.wpa_ie, sta->rsnie, ie.wpa_ie_len) != 0) {
 214                         wpa_printf(MSG_INFO, "Mismatch in WPA IE between "
 215                                    "EAPOL-Key 2/4 and (Re)Association "
 216                                    "Request from " MACSTR, MAC2STR(sta->addr));
 217                         wpa_hexdump(MSG_INFO, "WPA IE in EAPOL-Key",
 218                                     ie.wpa_ie, ie.wpa_ie_len);
 219                         wpa_hexdump(MSG_INFO, "WPA IE in (Re)Association "
 220                                     "Request",
 221                                     sta->rsnie,
 222                                     sta->rsnie[0] ? 2 + sta->rsnie[1] : 0);
 223                 }
 224         }
 225
 226         if (ie.rsn_ie) {
 227                 wpa_hexdump(MSG_MSGDUMP, "EAPOL-Key Key Data - RSN IE",
 228                             ie.rsn_ie, ie.rsn_ie_len);
 229                 if (os_memcmp(ie.rsn_ie, sta->rsnie, ie.rsn_ie_len) != 0) {
 230                         wpa_printf(MSG_INFO, "Mismatch in WPA IE between "
 231                                    "EAPOL-Key 2/4 and (Re)Association "
 232                                    "Request from " MACSTR, MAC2STR(sta->addr));
 233                         wpa_hexdump(MSG_INFO, "WPA IE in EAPOL-Key",
 234                                     ie.rsn_ie, ie.rsn_ie_len);
 235                         wpa_hexdump(MSG_INFO, "WPA IE in (Re)Association "
 236                                     "Request",
 237                                     sta->rsnie,
 238                                     sta->rsnie[0] ? 2 + sta->rsnie[1] : 0);
 239                 }
 240         }
 241 }
 242
 243
 244 static u8 * decrypt_eapol_key_data_rc4(const u8 *kek,
 245                                        const struct wpa_eapol_key *hdr,
 246                                        size_t *len)
 247 {
 248         u8 ek[32], *buf;
 249         u16 keydatalen = WPA_GET_BE16(hdr->key_data_length);
 250
 251         buf = os_malloc(keydatalen);
 252         if (buf == NULL)
 253                 return NULL;
 254
 255         os_memcpy(ek, hdr->key_iv, 16);
 256         os_memcpy(ek + 16, kek, 16);
 257         os_memcpy(buf, hdr + 1, keydatalen);
 258         if (rc4_skip(ek, 32, 256, buf, keydatalen)) {
 259                 wpa_printf(MSG_INFO, "RC4 failed");
 260                 os_free(buf);
 261                 return NULL;
 262         }
 263
 264         *len = keydatalen;
 265         return buf;
 266 }
 267
 268
 269 static u8 * decrypt_eapol_key_data_aes(const u8 *kek,
 270                                        const struct wpa_eapol_key *hdr,
 271                                        size_t *len)
 272 {
 273         u8 *buf;
 274         u16 keydatalen = WPA_GET_BE16(hdr->key_data_length);
 275
 276         if (keydatalen % 8) {
 277                 wpa_printf(MSG_INFO, "Unsupported AES-WRAP len %d",
 278                            keydatalen);
 279                 return NULL;
 280         }
 281         keydatalen -= 8; /* AES-WRAP adds 8 bytes */
 282         buf = os_malloc(keydatalen);
 283         if (buf == NULL)
 284                 return NULL;
 285         if (aes_unwrap(kek, keydatalen / 8, (u8 *) (hdr + 1), buf)) {
 286                 os_free(buf);
 287                 wpa_printf(MSG_INFO, "AES unwrap failed - "
 288                            "could not decrypt EAPOL-Key key data");
 289                 return NULL;
 290         }
 291
 292         *len = keydatalen;
 293         return buf;
 294 }
 295
 296
 297 static u8 * decrypt_eapol_key_data(const u8 *kek, u16 ver,
 298                                    const struct wpa_eapol_key *hdr,
 299                                    size_t *len)
 300 {
 301         switch (ver) {
 302         case WPA_KEY_INFO_TYPE_HMAC_MD5_RC4:
 303                 return decrypt_eapol_key_data_rc4(kek, hdr, len);
 304         case WPA_KEY_INFO_TYPE_HMAC_SHA1_AES:
 305         case WPA_KEY_INFO_TYPE_AES_128_CMAC:
 306                 return decrypt_eapol_key_data_aes(kek, hdr, len);
 307         default:
 308                 wpa_printf(MSG_INFO, "Unsupported EAPOL-Key Key Descriptor "
 309                            "Version %u", ver);
 310                 return NULL;
 311         }
 312 }
 313
 314
 315 static void learn_kde_keys(struct wlantest_bss *bss, u8 *buf, size_t len,
 316                            const u8 *rsc)
 317 {
 318         struct wpa_eapol_ie_parse ie;
 319
 320         if (wpa_supplicant_parse_ies(buf, len, &ie) < 0) {
 321                 wpa_printf(MSG_INFO, "Failed to parse EAPOL-Key Key Data");
 322                 return;
 323         }
 324
 325         if (ie.wpa_ie) {
 326                 wpa_hexdump(MSG_MSGDUMP, "EAPOL-Key Key Data - WPA IE",
 327                             ie.wpa_ie, ie.wpa_ie_len);
 328         }
 329
 330         if (ie.rsn_ie) {
 331                 wpa_hexdump(MSG_MSGDUMP, "EAPOL-Key Key Data - RSN IE",
 332                             ie.rsn_ie, ie.rsn_ie_len);
 333         }
 334
 335         if (ie.gtk) {
 336                 wpa_hexdump(MSG_MSGDUMP, "EAPOL-Key Key Data - GTK KDE",
 337                             ie.gtk, ie.gtk_len);
 338                 if (ie.gtk_len >= 2 && ie.gtk_len <= 2 + 32) {
 339                         int id;
 340                         id = ie.gtk[0] & 0x03;
 341                         wpa_printf(MSG_DEBUG, "GTK KeyID=%u tx=%u",
 342                                    id, !!(ie.gtk[0] & 0x04));
 343                         if ((ie.gtk[0] & 0xf8) || ie.gtk[1])
 344                                 wpa_printf(MSG_INFO, "GTK KDE: Reserved field "
 345                                            "set: %02x %02x",
 346                                            ie.gtk[0], ie.gtk[1]);
 347                         wpa_hexdump(MSG_DEBUG, "GTK", ie.gtk + 2,
 348                                     ie.gtk_len - 2);
 349                         bss->gtk_len[id] = ie.gtk_len - 2;
 350                         os_memcpy(bss->gtk[id], ie.gtk + 2, ie.gtk_len - 2);
 351                         bss->rsc[id][0] = rsc[5];
 352                         bss->rsc[id][1] = rsc[4];
 353                         bss->rsc[id][2] = rsc[3];
 354                         bss->rsc[id][3] = rsc[2];
 355                         bss->rsc[id][4] = rsc[1];
 356                         bss->rsc[id][5] = rsc[0];
 357                         wpa_hexdump(MSG_DEBUG, "RSC", bss->rsc[id], 6);
 358                 } else {
 359                         wpa_printf(MSG_INFO, "Invalid GTK KDE length %u",
 360                                    (unsigned) ie.gtk_len);
 361                 }
 362         }
 363
 364         if (ie.igtk) {
 365                 wpa_hexdump(MSG_MSGDUMP, "EAPOL-Key Key Data - IGTK KDE",
 366                             ie.igtk, ie.igtk_len);
 367                 if (ie.igtk_len == 24) {
 368                         u16 id;
 369                         id = WPA_GET_LE16(ie.igtk);
 370                         if (id > 5) {
 371                                 wpa_printf(MSG_INFO, "Unexpected IGTK KeyID "
 372                                            "%u", id);
 373                         } else {
 374                                 wpa_printf(MSG_DEBUG, "IGTK KeyID %u", id);
 375                                 wpa_hexdump(MSG_DEBUG, "IPN", ie.igtk + 2, 6);
 376                                 wpa_hexdump(MSG_DEBUG, "IGTK", ie.igtk + 8,
 377                                             16);
 378                                 os_memcpy(bss->igtk[id], ie.igtk + 8, 16);
 379                                 bss->igtk_set[id] = 1;
 380                         }
 381                 } else {
 382                         wpa_printf(MSG_INFO, "Invalid IGTK KDE length %u",
 383                                    (unsigned) ie.igtk_len);
 384                 }
 385         }
 386 }
 387
 388
 389 static void rx_data_eapol_key_3_of_4(struct wlantest *wt, const u8 *dst,
 390                                      const u8 *src, const u8 *data, size_t len)
 391 {
 392         struct wlantest_bss *bss;
 393         struct wlantest_sta *sta;
 394         const struct ieee802_1x_hdr *eapol;
 395         const struct wpa_eapol_key *hdr;
 396         const u8 *key_data;
 397         int recalc = 0;
 398         u16 key_info, ver;
 399         u8 *decrypted;
 400         size_t decrypted_len = 0;
 401         struct wpa_eapol_ie_parse ie;
 402
 403         wpa_printf(MSG_DEBUG, "EAPOL-Key 3/4 " MACSTR " -> " MACSTR,
 404                    MAC2STR(src), MAC2STR(dst));
 405         bss = bss_get(wt, src);
 406         if (bss == NULL)
 407                 return;
 408         sta = sta_get(bss, dst);
 409         if (sta == NULL)
 410                 return;
 411
 412         eapol = (const struct ieee802_1x_hdr *) data;
 413         hdr = (const struct wpa_eapol_key *) (eapol + 1);
 414         key_info = WPA_GET_BE16(hdr->key_info);
 415
 416         if (os_memcmp(sta->anonce, hdr->key_nonce, WPA_NONCE_LEN) != 0) {
 417                 wpa_printf(MSG_INFO, "EAPOL-Key ANonce mismatch between 1/4 "
 418                            "and 3/4");
 419                 recalc = 1;
 420         }
 421         os_memcpy(sta->anonce, hdr->key_nonce, WPA_NONCE_LEN);
 422         if (recalc) {
 423                 derive_ptk(wt, bss, sta, key_info & WPA_KEY_INFO_TYPE_MASK,
 424                            data, len);
 425         }
 426
 427         if (!sta->ptk_set) {
 428                 wpa_printf(MSG_DEBUG, "No PTK known to process EAPOL-Key 3/4");
 429                 return;
 430         }
 431
 432         if (check_mic(sta->ptk.kck, key_info & WPA_KEY_INFO_TYPE_MASK,
 433                       data, len) < 0) {
 434                 wpa_printf(MSG_INFO, "Mismatch in EAPOL-Key 3/4 MIC");
 435                 return;
 436         }
 437         wpa_printf(MSG_DEBUG, "Valid MIC found in EAPOL-Key 3/4");
 438
 439         key_data = (const u8 *) (hdr + 1);
 440         /* TODO: handle WPA without EncrKeyData bit */
 441         if (!(key_info & WPA_KEY_INFO_ENCR_KEY_DATA)) {
 442                 wpa_printf(MSG_INFO, "EAPOL-Key 3/4 without EncrKeyData bit");
 443                 return;
 444         }
 445         ver = key_info & WPA_KEY_INFO_TYPE_MASK;
 446         decrypted = decrypt_eapol_key_data(sta->ptk.kek, ver, hdr,
 447                                            &decrypted_len);
 448         if (decrypted == NULL) {
 449                 wpa_printf(MSG_INFO, "Failed to decrypt EAPOL-Key Key Data");
 450                 return;
 451         }
 452         wpa_hexdump(MSG_DEBUG, "Decrypted EAPOL-Key Key Data",
 453                     decrypted, decrypted_len);
 454         if (wt->write_pcap_dumper) {
 455                 /* Fill in a dummy Data frame header */
 456                 u8 buf[24 + 8 + sizeof(*eapol) + sizeof(*hdr)];
 457                 struct ieee80211_hdr *h;
 458                 struct wpa_eapol_key *k;
 459                 u8 *pos;
 460                 size_t plain_len;
 461
 462                 plain_len = decrypted_len;
 463                 pos = decrypted;
 464                 while (pos + 1 < decrypted + decrypted_len) {
 465                         if (pos[0] == 0xdd && pos[1] == 0x00) {
 466                                 /* Remove padding */
 467                                 plain_len = pos - decrypted;
 468                                 break;
 469                         }
 470                         pos += 2 + pos[1];
 471                 }
 472
 473                 os_memset(buf, 0, sizeof(buf));
 474                 h = (struct ieee80211_hdr *) buf;
 475                 h->frame_control = host_to_le16(0x0208);
 476                 os_memcpy(h->addr1, dst, ETH_ALEN);
 477                 os_memcpy(h->addr2, src, ETH_ALEN);
 478                 os_memcpy(h->addr3, src, ETH_ALEN);
 479                 pos = (u8 *) (h + 1);
 480                 os_memcpy(pos, "\xaa\xaa\x03\x00\x00\x00\x88\x8e", 8);
 481                 pos += 8;
 482                 os_memcpy(pos, eapol, sizeof(*eapol));
 483                 pos += sizeof(*eapol);
 484                 os_memcpy(pos, hdr, sizeof(*hdr));
 485                 k = (struct wpa_eapol_key *) pos;
 486                 WPA_PUT_BE16(k->key_info,
 487                              key_info & ~WPA_KEY_INFO_ENCR_KEY_DATA);
 488                 WPA_PUT_BE16(k->key_data_length, plain_len);
 489                 write_pcap_decrypted(wt, buf, sizeof(buf),
 490                                      decrypted, plain_len);
 491         }
 492
 493         if (wpa_supplicant_parse_ies(decrypted, decrypted_len, &ie) < 0) {
 494                 wpa_printf(MSG_INFO, "Failed to parse EAPOL-Key Key Data");
 495                 os_free(decrypted);
 496                 return;
 497         }
 498
 499         if ((ie.wpa_ie &&
 500              os_memcmp(ie.wpa_ie, bss->wpaie, ie.wpa_ie_len) != 0) ||
 501             (ie.wpa_ie == NULL && bss->wpaie[0])) {
 502                 wpa_printf(MSG_INFO, "Mismatch in WPA IE between "
 503                            "EAPOL-Key 3/4 and Beacon/Probe Response "
 504                            "from " MACSTR, MAC2STR(bss->bssid));
 505                 wpa_hexdump(MSG_INFO, "WPA IE in EAPOL-Key",
 506                             ie.wpa_ie, ie.wpa_ie_len);
 507                 wpa_hexdump(MSG_INFO, "WPA IE in Beacon/Probe "
 508                             "Response",
 509                             bss->wpaie,
 510                             bss->wpaie[0] ? 2 + bss->wpaie[1] : 0);
 511         }
 512
 513         if ((ie.rsn_ie &&
 514              os_memcmp(ie.rsn_ie, bss->rsnie, ie.rsn_ie_len) != 0) ||
 515             (ie.rsn_ie == NULL && bss->rsnie[0])) {
 516                 wpa_printf(MSG_INFO, "Mismatch in RSN IE between "
 517                            "EAPOL-Key 3/4 and Beacon/Probe Response "
 518                            "from " MACSTR, MAC2STR(bss->bssid));
 519                 wpa_hexdump(MSG_INFO, "RSN IE in EAPOL-Key",
 520                             ie.rsn_ie, ie.rsn_ie_len);
 521                 wpa_hexdump(MSG_INFO, "RSN IE in (Re)Association "
 522                             "Request",
 523                             bss->rsnie,
 524                             bss->rsnie[0] ? 2 + bss->rsnie[1] : 0);
 525         }
 526
 527         learn_kde_keys(bss, decrypted, decrypted_len, hdr->key_rsc);
 528         os_free(decrypted);
 529 }
 530
 531
 532 static void rx_data_eapol_key_4_of_4(struct wlantest *wt, const u8 *dst,
 533                                      const u8 *src, const u8 *data, size_t len)
 534 {
 535         struct wlantest_bss *bss;
 536         struct wlantest_sta *sta;
 537         const struct ieee802_1x_hdr *eapol;
 538         const struct wpa_eapol_key *hdr;
 539         u16 key_info;
 540
 541         wpa_printf(MSG_DEBUG, "EAPOL-Key 4/4 " MACSTR " -> " MACSTR,
 542                    MAC2STR(src), MAC2STR(dst));
 543         bss = bss_get(wt, dst);
 544         if (bss == NULL)
 545                 return;
 546         sta = sta_get(bss, src);
 547         if (sta == NULL)
 548                 return;
 549
 550         eapol = (const struct ieee802_1x_hdr *) data;
 551         hdr = (const struct wpa_eapol_key *) (eapol + 1);
 552         key_info = WPA_GET_BE16(hdr->key_info);
 553
 554         if (!sta->ptk_set) {
 555                 wpa_printf(MSG_DEBUG, "No PTK known to process EAPOL-Key 4/4");
 556                 return;
 557         }
 558
 559         if (sta->ptk_set &&
 560             check_mic(sta->ptk.kck, key_info & WPA_KEY_INFO_TYPE_MASK,
 561                       data, len) < 0) {
 562                 wpa_printf(MSG_INFO, "Mismatch in EAPOL-Key 4/4 MIC");
 563                 return;
 564         }
 565         wpa_printf(MSG_DEBUG, "Valid MIC found in EAPOL-Key 4/4");
 566 }
 567
 568
 569 static void rx_data_eapol_key_1_of_2(struct wlantest *wt, const u8 *dst,
 570                                      const u8 *src, const u8 *data, size_t len)
 571 {
 572         struct wlantest_bss *bss;
 573         struct wlantest_sta *sta;
 574         const struct ieee802_1x_hdr *eapol;
 575         const struct wpa_eapol_key *hdr;
 576         const u8 *key_data;
 577         u16 key_info, ver;
 578         u8 *decrypted;
 579         size_t decrypted_len = 0;
 580
 581         wpa_printf(MSG_DEBUG, "EAPOL-Key 1/2 " MACSTR " -> " MACSTR,
 582                    MAC2STR(src), MAC2STR(dst));
 583         bss = bss_get(wt, src);
 584         if (bss == NULL)
 585                 return;
 586         sta = sta_get(bss, dst);
 587         if (sta == NULL)
 588                 return;
 589
 590         eapol = (const struct ieee802_1x_hdr *) data;
 591         hdr = (const struct wpa_eapol_key *) (eapol + 1);
 592         key_info = WPA_GET_BE16(hdr->key_info);
 593
 594         if (!sta->ptk_set) {
 595                 wpa_printf(MSG_DEBUG, "No PTK known to process EAPOL-Key 1/2");
 596                 return;
 597         }
 598
 599         if (sta->ptk_set &&
 600             check_mic(sta->ptk.kck, key_info & WPA_KEY_INFO_TYPE_MASK,
 601                       data, len) < 0) {
 602                 wpa_printf(MSG_INFO, "Mismatch in EAPOL-Key 1/2 MIC");
 603                 return;
 604         }
 605         wpa_printf(MSG_DEBUG, "Valid MIC found in EAPOL-Key 1/2");
 606
 607         key_data = (const u8 *) (hdr + 1);
 608         /* TODO: handle WPA without EncrKeyData bit */
 609         if (!(key_info & WPA_KEY_INFO_ENCR_KEY_DATA)) {
 610                 wpa_printf(MSG_INFO, "EAPOL-Key 1/2 without EncrKeyData bit");
 611                 return;
 612         }
 613         ver = key_info & WPA_KEY_INFO_TYPE_MASK;
 614         decrypted = decrypt_eapol_key_data(sta->ptk.kek, ver, hdr,
 615                                            &decrypted_len);
 616         if (decrypted == NULL) {
 617                 wpa_printf(MSG_INFO, "Failed to decrypt EAPOL-Key Key Data");
 618                 return;
 619         }
 620         wpa_hexdump(MSG_DEBUG, "Decrypted EAPOL-Key Key Data",
 621                     decrypted, decrypted_len);
 622         if (wt->write_pcap_dumper) {
 623                 /* Fill in a dummy Data frame header */
 624                 u8 buf[24 + 8 + sizeof(*eapol) + sizeof(*hdr)];
 625                 struct ieee80211_hdr *h;
 626                 struct wpa_eapol_key *k;
 627                 u8 *pos;
 628                 size_t plain_len;
 629
 630                 plain_len = decrypted_len;
 631                 pos = decrypted;
 632                 while (pos + 1 < decrypted + decrypted_len) {
 633                         if (pos[0] == 0xdd && pos[1] == 0x00) {
 634                                 /* Remove padding */
 635                                 plain_len = pos - decrypted;
 636                                 break;
 637                         }
 638                         pos += 2 + pos[1];
 639                 }
 640
 641                 os_memset(buf, 0, sizeof(buf));
 642                 h = (struct ieee80211_hdr *) buf;
 643                 h->frame_control = host_to_le16(0x0208);
 644                 os_memcpy(h->addr1, dst, ETH_ALEN);
 645                 os_memcpy(h->addr2, src, ETH_ALEN);
 646                 os_memcpy(h->addr3, src, ETH_ALEN);
 647                 pos = (u8 *) (h + 1);
 648                 os_memcpy(pos, "\xaa\xaa\x03\x00\x00\x00\x88\x8e", 8);
 649                 pos += 8;
 650                 os_memcpy(pos, eapol, sizeof(*eapol));
 651                 pos += sizeof(*eapol);
 652                 os_memcpy(pos, hdr, sizeof(*hdr));
 653                 k = (struct wpa_eapol_key *) pos;
 654                 WPA_PUT_BE16(k->key_info,
 655                              key_info & ~WPA_KEY_INFO_ENCR_KEY_DATA);
 656                 WPA_PUT_BE16(k->key_data_length, plain_len);
 657                 write_pcap_decrypted(wt, buf, sizeof(buf),
 658                                      decrypted, plain_len);
 659         }
 660         learn_kde_keys(bss, decrypted, decrypted_len, hdr->key_rsc);
 661         os_free(decrypted);
 662 }
 663
 664
 665 static void rx_data_eapol_key_2_of_2(struct wlantest *wt, const u8 *dst,
 666                                      const u8 *src, const u8 *data, size_t len)
 667 {
 668         struct wlantest_bss *bss;
 669         struct wlantest_sta *sta;
 670         const struct ieee802_1x_hdr *eapol;
 671         const struct wpa_eapol_key *hdr;
 672         u16 key_info;
 673
 674         wpa_printf(MSG_DEBUG, "EAPOL-Key 2/2 " MACSTR " -> " MACSTR,
 675                    MAC2STR(src), MAC2STR(dst));
 676         bss = bss_get(wt, dst);
 677         if (bss == NULL)
 678                 return;
 679         sta = sta_get(bss, src);
 680         if (sta == NULL)
 681                 return;
 682
 683         eapol = (const struct ieee802_1x_hdr *) data;
 684         hdr = (const struct wpa_eapol_key *) (eapol + 1);
 685         key_info = WPA_GET_BE16(hdr->key_info);
 686
 687         if (!sta->ptk_set) {
 688                 wpa_printf(MSG_DEBUG, "No PTK known to process EAPOL-Key 2/2");
 689                 return;
 690         }
 691
 692         if (sta->ptk_set &&
 693             check_mic(sta->ptk.kck, key_info & WPA_KEY_INFO_TYPE_MASK,
 694                       data, len) < 0) {
 695                 wpa_printf(MSG_INFO, "Mismatch in EAPOL-Key 2/2 MIC");
 696                 return;
 697         }
 698         wpa_printf(MSG_DEBUG, "Valid MIC found in EAPOL-Key 2/2");
 699 }
 700
 701
 702 static void rx_data_eapol_key(struct wlantest *wt, const u8 *dst,
 703                               const u8 *src, const u8 *data, size_t len,
 704                               int prot)
 705 {
 706         const struct ieee802_1x_hdr *eapol;
 707         const struct wpa_eapol_key *hdr;
 708         const u8 *key_data;
 709         u16 key_info, key_length, ver, key_data_length;
 710
 711         eapol = (const struct ieee802_1x_hdr *) data;
 712         hdr = (const struct wpa_eapol_key *) (eapol + 1);
 713
 714         wpa_hexdump(MSG_MSGDUMP, "EAPOL-Key",
 715                     (const u8 *) hdr, len - sizeof(*eapol));
 716         if (len < sizeof(*hdr)) {
 717                 wpa_printf(MSG_INFO, "Too short EAPOL-Key frame from " MACSTR,
 718                            MAC2STR(src));
 719                 return;
 720         }
 721
 722         if (hdr->type == EAPOL_KEY_TYPE_RC4) {
 723                 /* TODO: EAPOL-Key RC4 for WEP */
 724                 return;
 725         }
 726
 727         if (hdr->type != EAPOL_KEY_TYPE_RSN &&
 728             hdr->type != EAPOL_KEY_TYPE_WPA) {
 729                 wpa_printf(MSG_DEBUG, "Unsupported EAPOL-Key type %u",
 730                            hdr->type);
 731                 return;
 732         }
 733
 734         key_info = WPA_GET_BE16(hdr->key_info);
 735         key_length = WPA_GET_BE16(hdr->key_length);
 736         key_data_length = WPA_GET_BE16(hdr->key_data_length);
 737         key_data = (const u8 *) (hdr + 1);
 738         if (key_data + key_data_length > data + len) {
 739                 wpa_printf(MSG_INFO, "Truncated EAPOL-Key from " MACSTR,
 740                            MAC2STR(src));
 741                 return;
 742         }
 743         if (key_data + key_data_length < data + len) {
 744                 wpa_hexdump(MSG_DEBUG, "Extra data after EAPOL-Key Key Data "
 745                             "field", key_data + key_data_length,
 746                         data + len - key_data - key_data_length);
 747         }
 748
 749
 750         ver = key_info & WPA_KEY_INFO_TYPE_MASK;
 751         wpa_printf(MSG_DEBUG, "EAPOL-Key ver=%u %c idx=%u%s%s%s%s%s%s%s%s "
 752                    "datalen=%u",
 753                    ver, key_info & WPA_KEY_INFO_KEY_TYPE ? 'P' : 'G',
 754                    (key_info & WPA_KEY_INFO_KEY_INDEX_MASK) >>
 755                    WPA_KEY_INFO_KEY_INDEX_SHIFT,
 756                    (key_info & WPA_KEY_INFO_INSTALL) ? " Install" : "",
 757                    (key_info & WPA_KEY_INFO_ACK) ? " ACK" : "",
 758                    (key_info & WPA_KEY_INFO_MIC) ? " MIC" : "",
 759                    (key_info & WPA_KEY_INFO_SECURE) ? " Secure" : "",
 760                    (key_info & WPA_KEY_INFO_ERROR) ? " Error" : "",
 761                    (key_info & WPA_KEY_INFO_REQUEST) ? " Request" : "",
 762                    (key_info & WPA_KEY_INFO_ENCR_KEY_DATA) ? " Encr" : "",
 763                    (key_info & WPA_KEY_INFO_SMK_MESSAGE) ? " SMK" : "",
 764                    key_data_length);
 765
 766         if (ver != WPA_KEY_INFO_TYPE_HMAC_MD5_RC4 &&
 767             ver != WPA_KEY_INFO_TYPE_HMAC_SHA1_AES &&
 768             ver != WPA_KEY_INFO_TYPE_AES_128_CMAC) {
 769                 wpa_printf(MSG_DEBUG, "Unsupported EAPOL-Key Key Descriptor "
 770                            "Version %u", ver);
 771                 return;
 772         }
 773
 774         wpa_hexdump(MSG_MSGDUMP, "EAPOL-Key Replay Counter",
 775                     hdr->replay_counter, WPA_REPLAY_COUNTER_LEN);
 776         wpa_hexdump(MSG_MSGDUMP, "EAPOL-Key Key Nonce",
 777                     hdr->key_nonce, WPA_NONCE_LEN);
 778         wpa_hexdump(MSG_MSGDUMP, "EAPOL-Key Key IV",
 779                     hdr->key_iv, 16);
 780         wpa_hexdump(MSG_MSGDUMP, "EAPOL-Key RSC",
 781                     hdr->key_rsc, WPA_KEY_RSC_LEN);
 782         wpa_hexdump(MSG_MSGDUMP, "EAPOL-Key Key MIC",
 783                     hdr->key_mic, 16);
 784         wpa_hexdump(MSG_MSGDUMP, "EAPOL-Key Key Data",
 785                     key_data, key_data_length);
 786
 787         if (key_info & (WPA_KEY_INFO_ERROR | WPA_KEY_INFO_REQUEST))
 788                 return;
 789
 790         if (key_info & WPA_KEY_INFO_SMK_MESSAGE)
 791                 return;
 792
 793         if (key_info & WPA_KEY_INFO_KEY_TYPE) {
 794                 /* 4-Way Handshake */
 795                 switch (key_info & (WPA_KEY_INFO_SECURE |
 796                                     WPA_KEY_INFO_MIC |
 797                                     WPA_KEY_INFO_ACK |
 798                                     WPA_KEY_INFO_INSTALL)) {
 799                 case WPA_KEY_INFO_ACK:
 800                         rx_data_eapol_key_1_of_4(wt, dst, src, data, len);
 801                         break;
 802                 case WPA_KEY_INFO_MIC:
 803                         rx_data_eapol_key_2_of_4(wt, dst, src, data, len);
 804                         break;
 805                 case WPA_KEY_INFO_SECURE | WPA_KEY_INFO_MIC |
 806                         WPA_KEY_INFO_ACK | WPA_KEY_INFO_INSTALL:
 807                         rx_data_eapol_key_3_of_4(wt, dst, src, data, len);
 808                         break;
 809                 case WPA_KEY_INFO_SECURE | WPA_KEY_INFO_MIC:
 810                         rx_data_eapol_key_4_of_4(wt, dst, src, data, len);
 811                         break;
 812                 default:
 813                         wpa_printf(MSG_DEBUG, "Unsupported EAPOL-Key frame");
 814                         break;
 815                 }
 816         } else {
 817                 /* Group Key Handshake */
 818                 switch (key_info & (WPA_KEY_INFO_SECURE |
 819                                     WPA_KEY_INFO_MIC |
 820                                     WPA_KEY_INFO_ACK)) {
 821                 case WPA_KEY_INFO_SECURE | WPA_KEY_INFO_MIC |
 822                         WPA_KEY_INFO_ACK:
 823                         rx_data_eapol_key_1_of_2(wt, dst, src, data, len);
 824                         break;
 825                 case WPA_KEY_INFO_SECURE | WPA_KEY_INFO_MIC:
 826                         rx_data_eapol_key_2_of_2(wt, dst, src, data, len);
 827                         break;
 828                 default:
 829                         wpa_printf(MSG_DEBUG, "Unsupported EAPOL-Key frame");
 830                         break;
 831                 }
 832         }
 833 }
 834
 835
 836 static void rx_data_eapol(struct wlantest *wt, const u8 *dst, const u8 *src,
 837                           const u8 *data, size_t len, int prot)
 838 {
 839         const struct ieee802_1x_hdr *hdr;
 840         u16 length;
 841         const u8 *p;
 842
 843         wpa_hexdump(MSG_EXCESSIVE, "EAPOL", data, len);
 844         if (len < sizeof(*hdr)) {
 845                 wpa_printf(MSG_INFO, "Too short EAPOL frame from " MACSTR,
 846                            MAC2STR(src));
 847                 return;
 848         }
 849
 850         hdr = (const struct ieee802_1x_hdr *) data;
 851         length = be_to_host16(hdr->length);
 852         wpa_printf(MSG_DEBUG, "RX EAPOL: " MACSTR " -> " MACSTR "%s ver=%u "
 853                    "type=%u len=%u",
 854                    MAC2STR(src), MAC2STR(dst), prot ? " Prot" : "",
 855                    hdr->version, hdr->type, length);
 856         if (sizeof(*hdr) + length > len) {
 857                 wpa_printf(MSG_INFO, "Truncated EAPOL frame from " MACSTR,
 858                            MAC2STR(src));
 859                 return;
 860         }
 861
 862         if (sizeof(*hdr) + length < len) {
 863                 wpa_printf(MSG_INFO, "EAPOL frame with %d extra bytes",
 864                            (int) (len - sizeof(*hdr) - length));
 865         }
 866         p = (const u8 *) (hdr + 1);
 867
 868         switch (hdr->type) {
 869         case IEEE802_1X_TYPE_EAP_PACKET:
 870                 wpa_hexdump(MSG_MSGDUMP, "EAPOL - EAP packet", p, length);
 871                 break;
 872         case IEEE802_1X_TYPE_EAPOL_START:
 873                 wpa_hexdump(MSG_MSGDUMP, "EAPOL-Start", p, length);
 874                 break;
 875         case IEEE802_1X_TYPE_EAPOL_LOGOFF:
 876                 wpa_hexdump(MSG_MSGDUMP, "EAPOL-Logoff", p, length);
 877                 break;
 878         case IEEE802_1X_TYPE_EAPOL_KEY:
 879                 rx_data_eapol_key(wt, dst, src, data, sizeof(*hdr) + length,
 880                                   prot);
 881                 break;
 882         case IEEE802_1X_TYPE_EAPOL_ENCAPSULATED_ASF_ALERT:
 883                 wpa_hexdump(MSG_MSGDUMP, "EAPOL - Encapsulated ASF alert",
 884                             p, length);
 885                 break;
 886         default:
 887                 wpa_hexdump(MSG_MSGDUMP, "Unknown EAPOL payload", p, length);
 888                 break;
 889         }
 890 }
 891
 892
 893 static void rx_data_eth(struct wlantest *wt, const u8 *dst, const u8 *src,
 894                         u16 ethertype, const u8 *data, size_t len, int prot)
 895 {
 896         if (ethertype == ETH_P_PAE)
 897                 rx_data_eapol(wt, dst, src, data, len, prot);
 898 }
 899
 900
 901 static void rx_data_process(struct wlantest *wt, const u8 *dst, const u8 *src,
 902                             const u8 *data, size_t len, int prot)
 903 {
 904         if (len == 0)
 905                 return;
 906
 907         if (len >= 8 && os_memcmp(data, "\xaa\xaa\x03\x00\x00\x00", 6) == 0) {
 908                 rx_data_eth(wt, dst, src, WPA_GET_BE16(data + 6),
 909                             data + 8, len - 8, prot);
 910                 return;
 911         }
 912
 913         wpa_hexdump(MSG_DEBUG, "Unrecognized LLC", data, len > 8 ? 8 : len);
 914 }
 915
 916
 917 static void rx_data_bss_prot_group(struct wlantest *wt,
 918                                    const struct ieee80211_hdr *hdr,
 919                                    const u8 *qos, const u8 *dst, const u8 *src,
 920                                    const u8 *data, size_t len)
 921 {
 922         struct wlantest_bss *bss;
 923         int keyid;
 924         u8 *decrypted;
 925         size_t dlen;
 926         u8 pn[6];
 927
 928         bss = bss_get(wt, hdr->addr2);
 929         if (bss == NULL)
 930                 return;
 931         if (len < 4) {
 932                 wpa_printf(MSG_INFO, "Too short group addressed data frame");
 933                 return;
 934         }
 935
 936         keyid = data[3] >> 6;
 937         if (bss->gtk_len[keyid] == 0) {
 938                 wpa_printf(MSG_MSGDUMP, "No GTK known to decrypt the frame "
 939                            "(A2=" MACSTR " KeyID=%d)",
 940                            MAC2STR(hdr->addr2), keyid);
 941                 return;
 942         }
 943
 944         /* TODO: different replay protection for TKIP */
 945         ccmp_get_pn(pn, data);
 946         if (os_memcmp(pn, bss->rsc[keyid], 6) <= 0) {
 947                 wpa_printf(MSG_INFO, "CCMP/TKIP replay detected: SA=" MACSTR,
 948                            MAC2STR(hdr->addr2));
 949                 wpa_hexdump(MSG_INFO, "RX PN", pn, 6);
 950                 wpa_hexdump(MSG_INFO, "RSC", bss->rsc[keyid], 6);
 951         }
 952
 953         if (bss->group_cipher == WPA_CIPHER_TKIP)
 954                 decrypted = tkip_decrypt(bss->gtk[keyid], hdr, data, len,
 955                                          &dlen);
 956         else
 957                 decrypted = ccmp_decrypt(bss->gtk[keyid], hdr, data, len,
 958                                          &dlen);
 959         if (decrypted) {
 960                 rx_data_process(wt, dst, src, decrypted, dlen, 1);
 961                 os_memcpy(bss->rsc[keyid], pn, 6);
 962                 write_pcap_decrypted(wt, (const u8 *) hdr, 24 + (qos ? 2 : 0),
 963                                      decrypted, dlen);
 964         }
 965         os_free(decrypted);
 966 }
 967
 968
 969 static void rx_data_bss_prot(struct wlantest *wt,
 970                              const struct ieee80211_hdr *hdr, const u8 *qos,
 971                              const u8 *dst, const u8 *src, const u8 *data,
 972                              size_t len)
 973 {
 974         struct wlantest_bss *bss;
 975         struct wlantest_sta *sta;
 976         int keyid;
 977         u16 fc = le_to_host16(hdr->frame_control);
 978         u8 *decrypted;
 979         size_t dlen;
 980         int tid;
 981         u8 pn[6], *rsc;
 982
 983         if (hdr->addr1[0] & 0x01) {
 984                 rx_data_bss_prot_group(wt, hdr, qos, dst, src, data, len);
 985                 return;
 986         }
 987
 988         if (fc & WLAN_FC_TODS) {
 989                 bss = bss_get(wt, hdr->addr1);
 990                 if (bss == NULL)
 991                         return;
 992                 sta = sta_get(bss, hdr->addr2);
 993         } else {
 994                 bss = bss_get(wt, hdr->addr2);
 995                 if (bss == NULL)
 996                         return;
 997                 sta = sta_get(bss, hdr->addr1);
 998         }
 999         if (sta == NULL || !sta->ptk_set) {
1000                 wpa_printf(MSG_MSGDUMP, "No PTK known to decrypt the frame");
1001                 return;
1002         }
1003
1004         if (len < 4) {
1005                 wpa_printf(MSG_INFO, "Too short encrypted data frame");
1006                 return;
1007         }
1008
1009         keyid = data[3] >> 6;
1010         if (keyid != 0) {
1011                 wpa_printf(MSG_INFO, "Unexpected non-zero KeyID %d in "
1012                            "individually addressed Data frame from " MACSTR,
1013                            keyid, MAC2STR(hdr->addr2));
1014         }
1015
1016         if (qos)
1017                 tid = qos[0] & 0x0f;
1018         else
1019                 tid = 0;
1020         if (fc & WLAN_FC_TODS)
1021                 rsc = sta->rsc_tods[tid];
1022         else
1023                 rsc = sta->rsc_fromds[tid];
1024
1025
1026         ccmp_get_pn(pn, data);
1027         if (os_memcmp(pn, rsc, 6) <= 0) {
1028                 wpa_printf(MSG_INFO, "CCMP/TKIP replay detected: SA=" MACSTR,
1029                            MAC2STR(hdr->addr2));
1030                 wpa_hexdump(MSG_INFO, "RX PN", pn, 6);
1031                 wpa_hexdump(MSG_INFO, "RSC", rsc, 6);
1032         }
1033
1034         if (sta->pairwise_cipher == WPA_CIPHER_TKIP)
1035                 decrypted = tkip_decrypt(sta->ptk.tk1, hdr, data, len, &dlen);
1036         else
1037                 decrypted = ccmp_decrypt(sta->ptk.tk1, hdr, data, len, &dlen);
1038         if (decrypted) {
1039                 rx_data_process(wt, dst, src, decrypted, dlen, 1);
1040                 os_memcpy(rsc, pn, 6);
1041                 write_pcap_decrypted(wt, (const u8 *) hdr, 24 + (qos ? 2 : 0),
1042                                      decrypted, dlen);
1043         }
1044         os_free(decrypted);
1045 }
1046
1047
1048 static void rx_data_bss(struct wlantest *wt, const struct ieee80211_hdr *hdr,
1049                         const u8 *qos, const u8 *dst, const u8 *src,
1050                         const u8 *data, size_t len)
1051 {
1052         u16 fc = le_to_host16(hdr->frame_control);
1053         int prot = !!(fc & WLAN_FC_ISWEP);
1054
1055         if (qos) {
1056                 u8 ack = (qos[0] & 0x60) >> 5;
1057                 wpa_printf(MSG_MSGDUMP, "BSS DATA: " MACSTR " -> " MACSTR
1058                            " len=%u%s tid=%u%s%s",
1059                            MAC2STR(src), MAC2STR(dst), (unsigned int) len,
1060                            prot ? " Prot" : "", qos[0] & 0x0f,
1061                            (qos[0] & 0x10) ? " EOSP" : "",
1062                            ack == 0 ? "" :
1063                            (ack == 1 ? " NoAck" :
1064                             (ack == 2 ? " NoExpAck" : " BA")));
1065         } else {
1066                 wpa_printf(MSG_MSGDUMP, "BSS DATA: " MACSTR " -> " MACSTR
1067                            " len=%u%s",
1068                            MAC2STR(src), MAC2STR(dst), (unsigned int) len,
1069                            prot ? " Prot" : "");
1070         }
1071
1072         if (prot)
1073                 rx_data_bss_prot(wt, hdr, qos, dst, src, data, len);
1074         else
1075                 rx_data_process(wt, dst, src, data, len, 0);
1076 }
1077
1078
1079 void rx_data(struct wlantest *wt, const u8 *data, size_t len)
1080 {
1081         const struct ieee80211_hdr *hdr;
1082         u16 fc, stype;
1083         size_t hdrlen;
1084         const u8 *qos = NULL;
1085
1086         if (len < 24)
1087                 return;
1088
1089         hdr = (const struct ieee80211_hdr *) data;
1090         fc = le_to_host16(hdr->frame_control);
1091         stype = WLAN_FC_GET_STYPE(fc);
1092         hdrlen = 24;
1093         if ((fc & (WLAN_FC_TODS | WLAN_FC_FROMDS)) ==
1094             (WLAN_FC_TODS | WLAN_FC_FROMDS))
1095                 hdrlen += ETH_ALEN;
1096         if (stype & 0x08) {
1097                 qos = data + hdrlen;
1098                 hdrlen += 2;
1099         }
1100         if (len < hdrlen)
1101                 return;
1102         wt->rx_data++;
1103
1104         switch (fc & (WLAN_FC_TODS | WLAN_FC_FROMDS)) {
1105         case 0:
1106                 wpa_printf(MSG_EXCESSIVE, "DATA %s%s%s IBSS DA=" MACSTR " SA="
1107                            MACSTR " BSSID=" MACSTR,
1108                            data_stype(WLAN_FC_GET_STYPE(fc)),
1109                            fc & WLAN_FC_PWRMGT ? " PwrMgt" : "",
1110                            fc & WLAN_FC_ISWEP ? " Prot" : "",
1111                            MAC2STR(hdr->addr1), MAC2STR(hdr->addr2),
1112                            MAC2STR(hdr->addr3));
1113                 break;
1114         case WLAN_FC_FROMDS:
1115                 wpa_printf(MSG_EXCESSIVE, "DATA %s%s%s FromDS DA=" MACSTR
1116                            " BSSID=" MACSTR " SA=" MACSTR,
1117                            data_stype(WLAN_FC_GET_STYPE(fc)),
1118                            fc & WLAN_FC_PWRMGT ? " PwrMgt" : "",
1119                            fc & WLAN_FC_ISWEP ? " Prot" : "",
1120                            MAC2STR(hdr->addr1), MAC2STR(hdr->addr2),
1121                            MAC2STR(hdr->addr3));
1122                 rx_data_bss(wt, hdr, qos, hdr->addr1, hdr->addr2,
1123                             data + hdrlen, len - hdrlen);
1124                 break;
1125         case WLAN_FC_TODS:
1126                 wpa_printf(MSG_EXCESSIVE, "DATA %s%s%s ToDS BSSID=" MACSTR
1127                            " SA=" MACSTR " DA=" MACSTR,
1128                            data_stype(WLAN_FC_GET_STYPE(fc)),
1129                            fc & WLAN_FC_PWRMGT ? " PwrMgt" : "",
1130                            fc & WLAN_FC_ISWEP ? " Prot" : "",
1131                            MAC2STR(hdr->addr1), MAC2STR(hdr->addr2),
1132                            MAC2STR(hdr->addr3));
1133                 rx_data_bss(wt, hdr, qos, hdr->addr3, hdr->addr2,
1134                             data + hdrlen, len - hdrlen);
1135                 break;
1136         case WLAN_FC_TODS | WLAN_FC_FROMDS:
1137                 wpa_printf(MSG_EXCESSIVE, "DATA %s%s%s WDS RA=" MACSTR " TA="
1138                            MACSTR " DA=" MACSTR " SA=" MACSTR,
1139                            data_stype(WLAN_FC_GET_STYPE(fc)),
1140                            fc & WLAN_FC_PWRMGT ? " PwrMgt" : "",
1141                            fc & WLAN_FC_ISWEP ? " Prot" : "",
1142                            MAC2STR(hdr->addr1), MAC2STR(hdr->addr2),
1143                            MAC2STR(hdr->addr3),
1144                            MAC2STR((const u8 *) (hdr + 1)));
1145                 break;
1146         }
1147 }