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/ieee802_11_defs.h"
  21 #include "common/eapol_common.h"
  22 #include "common/wpa_common.h"
  23 #include "wlantest.h"
  24
  25
  26 static const char * data_stype(u16 stype)
  27 {
  28         switch (stype) {
  29         case WLAN_FC_STYPE_DATA:
  30                 return "DATA";
  31         case WLAN_FC_STYPE_DATA_CFACK:
  32                 return "DATA-CFACK";
  33         case WLAN_FC_STYPE_DATA_CFPOLL:
  34                 return "DATA-CFPOLL";
  35         case WLAN_FC_STYPE_DATA_CFACKPOLL:
  36                 return "DATA-CFACKPOLL";
  37         case WLAN_FC_STYPE_NULLFUNC:
  38                 return "NULLFUNC";
  39         case WLAN_FC_STYPE_CFACK:
  40                 return "CFACK";
  41         case WLAN_FC_STYPE_CFPOLL:
  42                 return "CFPOLL";
  43         case WLAN_FC_STYPE_CFACKPOLL:
  44                 return "CFACKPOLL";
  45         case WLAN_FC_STYPE_QOS_DATA:
  46                 return "QOSDATA";
  47         case WLAN_FC_STYPE_QOS_DATA_CFACK:
  48                 return "QOSDATA-CFACK";
  49         case WLAN_FC_STYPE_QOS_DATA_CFPOLL:
  50                 return "QOSDATA-CFPOLL";
  51         case WLAN_FC_STYPE_QOS_DATA_CFACKPOLL:
  52                 return "QOSDATA-CFACKPOLL";
  53         case WLAN_FC_STYPE_QOS_NULL:
  54                 return "QOS-NULL";
  55         case WLAN_FC_STYPE_QOS_CFPOLL:
  56                 return "QOS-CFPOLL";
  57         case WLAN_FC_STYPE_QOS_CFACKPOLL:
  58                 return "QOS-CFACKPOLL";
  59         }
  60         return "??";
  61 }
  62
  63
  64 static int check_mic(const u8 *kck, int ver, const u8 *data, size_t len)
  65 {
  66         u8 *buf;
  67         int ret = -1;
  68         struct ieee802_1x_hdr *hdr;
  69         struct wpa_eapol_key *key;
  70         u8 rx_mic[16];
  71
  72         buf = os_malloc(len);
  73         if (buf == NULL)
  74                 return -1;
  75         os_memcpy(buf, data, len);
  76         hdr = (struct ieee802_1x_hdr *) buf;
  77         key = (struct wpa_eapol_key *) (hdr + 1);
  78
  79         os_memcpy(rx_mic, key->key_mic, 16);
  80         os_memset(key->key_mic, 0, 16);
  81
  82         if (wpa_eapol_key_mic(kck, ver, buf, len, key->key_mic) == 0 &&
  83             os_memcmp(rx_mic, key->key_mic, 16) == 0)
  84                 ret = 0;
  85
  86         os_free(buf);
  87
  88         return ret;
  89 }
  90
  91
  92 static void rx_data_eapol_key_1_of_4(struct wlantest *wt, const u8 *dst,
  93                                      const u8 *src, const u8 *data, size_t len)
  94 {
  95         struct wlantest_bss *bss;
  96         struct wlantest_sta *sta;
  97         const struct ieee802_1x_hdr *eapol;
  98         const struct wpa_eapol_key *hdr;
  99
 100         wpa_printf(MSG_DEBUG, "EAPOL-Key 1/4 " MACSTR " -> " MACSTR,
 101                    MAC2STR(src), MAC2STR(dst));
 102         bss = bss_get(wt, src);
 103         if (bss == NULL)
 104                 return;
 105         sta = sta_get(bss, dst);
 106         if (sta == NULL)
 107                 return;
 108
 109         eapol = (const struct ieee802_1x_hdr *) data;
 110         hdr = (const struct wpa_eapol_key *) (eapol + 1);
 111         os_memcpy(sta->anonce, hdr->key_nonce, WPA_NONCE_LEN);
 112 }
 113
 114
 115 static int try_pmk(struct wlantest_bss *bss, struct wlantest_sta *sta,
 116                    u16 ver, const u8 *data, size_t len,
 117                    struct wlantest_pmk *pmk)
 118 {
 119         struct wpa_ptk ptk;
 120         size_t ptk_len = 48; /* FIX: 64 for TKIP */
 121         wpa_pmk_to_ptk(pmk->pmk, sizeof(pmk->pmk),
 122                        "Pairwise key expansion",
 123                        bss->bssid, sta->addr, sta->anonce, sta->snonce,
 124                        (u8 *) &ptk, ptk_len,
 125                        0 /* FIX: SHA256 based on AKM */);
 126         if (check_mic(ptk.kck, ver,
 127                       data, len) < 0)
 128                 return -1;
 129
 130         wpa_printf(MSG_INFO, "Derived PTK for STA " MACSTR " BSSID " MACSTR
 131                    ")", MAC2STR(sta->addr), MAC2STR(bss->bssid));
 132         os_memcpy(&sta->ptk, &ptk, sizeof(ptk));
 133         sta->ptk_set = 1;
 134         return 0;
 135 }
 136
 137
 138 static void derive_ptk(struct wlantest *wt, struct wlantest_bss *bss,
 139                        struct wlantest_sta *sta, u16 ver,
 140                        const u8 *data, size_t len)
 141 {
 142         struct wlantest_pmk *pmk;
 143
 144         dl_list_for_each(pmk, &bss->pmk, struct wlantest_pmk, list) {
 145                 if (try_pmk(bss, sta, ver, data, len, pmk) == 0)
 146                         return;
 147         }
 148
 149         dl_list_for_each(pmk, &wt->pmk, struct wlantest_pmk, list) {
 150                 if (try_pmk(bss, sta, ver, data, len, pmk) == 0)
 151                         return;
 152         }
 153 }
 154
 155
 156 static void rx_data_eapol_key_2_of_4(struct wlantest *wt, const u8 *dst,
 157                                      const u8 *src, const u8 *data, size_t len)
 158 {
 159         struct wlantest_bss *bss;
 160         struct wlantest_sta *sta;
 161         const struct ieee802_1x_hdr *eapol;
 162         const struct wpa_eapol_key *hdr;
 163         u16 key_info;
 164
 165         wpa_printf(MSG_DEBUG, "EAPOL-Key 2/4 " MACSTR " -> " MACSTR,
 166                    MAC2STR(src), MAC2STR(dst));
 167         bss = bss_get(wt, dst);
 168         if (bss == NULL)
 169                 return;
 170         sta = sta_get(bss, src);
 171         if (sta == NULL)
 172                 return;
 173
 174         eapol = (const struct ieee802_1x_hdr *) data;
 175         hdr = (const struct wpa_eapol_key *) (eapol + 1);
 176         os_memcpy(sta->snonce, hdr->key_nonce, WPA_NONCE_LEN);
 177         key_info = WPA_GET_BE16(hdr->key_info);
 178         derive_ptk(wt, bss, sta, key_info & WPA_KEY_INFO_TYPE_MASK, data, len);
 179 }
 180
 181
 182 static u8 * decrypt_eapol_key_data_rc4(const u8 *kek,
 183                                        const struct wpa_eapol_key *hdr,
 184                                        size_t *len)
 185 {
 186         u8 ek[32], *buf;
 187         u16 keydatalen = WPA_GET_BE16(hdr->key_data_length);
 188
 189         buf = os_malloc(keydatalen);
 190         if (buf == NULL)
 191                 return NULL;
 192
 193         os_memcpy(ek, hdr->key_iv, 16);
 194         os_memcpy(ek + 16, kek, 16);
 195         os_memcpy(buf, hdr + 1, keydatalen);
 196         if (rc4_skip(ek, 32, 256, buf, keydatalen)) {
 197                 wpa_printf(MSG_INFO, "RC4 failed");
 198                 os_free(buf);
 199                 return NULL;
 200         }
 201
 202         *len = keydatalen;
 203         return buf;
 204 }
 205
 206
 207 static u8 * decrypt_eapol_key_data_aes(const u8 *kek,
 208                                        const struct wpa_eapol_key *hdr,
 209                                        size_t *len)
 210 {
 211         u8 *buf;
 212         u16 keydatalen = WPA_GET_BE16(hdr->key_data_length);
 213
 214         if (keydatalen % 8) {
 215                 wpa_printf(MSG_INFO, "Unsupported AES-WRAP len %d",
 216                            keydatalen);
 217                 return NULL;
 218         }
 219         keydatalen -= 8; /* AES-WRAP adds 8 bytes */
 220         buf = os_malloc(keydatalen);
 221         if (buf == NULL)
 222                 return NULL;
 223         if (aes_unwrap(kek, keydatalen / 8, (u8 *) (hdr + 1), buf)) {
 224                 os_free(buf);
 225                 wpa_printf(MSG_INFO, "AES unwrap failed - "
 226                            "could not decrypt EAPOL-Key key data");
 227                 return NULL;
 228         }
 229
 230         *len = keydatalen;
 231         return buf;
 232 }
 233
 234
 235 static u8 * decrypt_eapol_key_data(const u8 *kek, u16 ver,
 236                                    const struct wpa_eapol_key *hdr,
 237                                    size_t *len)
 238 {
 239         switch (ver) {
 240         case WPA_KEY_INFO_TYPE_HMAC_MD5_RC4:
 241                 return decrypt_eapol_key_data_rc4(kek, hdr, len);
 242         case WPA_KEY_INFO_TYPE_HMAC_SHA1_AES:
 243         case WPA_KEY_INFO_TYPE_AES_128_CMAC:
 244                 return decrypt_eapol_key_data_aes(kek, hdr, len);
 245         default:
 246                 wpa_printf(MSG_INFO, "Unsupported EAPOL-Key Key Descriptor "
 247                            "Version %u", ver);
 248                 return NULL;
 249         }
 250 }
 251
 252
 253 static void rx_data_eapol_key_3_of_4(struct wlantest *wt, const u8 *dst,
 254                                      const u8 *src, const u8 *data, size_t len)
 255 {
 256         struct wlantest_bss *bss;
 257         struct wlantest_sta *sta;
 258         const struct ieee802_1x_hdr *eapol;
 259         const struct wpa_eapol_key *hdr;
 260         const u8 *key_data;
 261         int recalc = 0;
 262         u16 key_info, ver, key_data_len;
 263         u8 *decrypted;
 264         size_t decrypted_len = 0;
 265
 266         wpa_printf(MSG_DEBUG, "EAPOL-Key 3/4 " MACSTR " -> " MACSTR,
 267                    MAC2STR(src), MAC2STR(dst));
 268         bss = bss_get(wt, src);
 269         if (bss == NULL)
 270                 return;
 271         sta = sta_get(bss, dst);
 272         if (sta == NULL)
 273                 return;
 274
 275         eapol = (const struct ieee802_1x_hdr *) data;
 276         hdr = (const struct wpa_eapol_key *) (eapol + 1);
 277         key_info = WPA_GET_BE16(hdr->key_info);
 278         key_data_len = WPA_GET_BE16(hdr->key_data_length);
 279
 280         if (os_memcmp(sta->anonce, hdr->key_nonce, WPA_NONCE_LEN) != 0) {
 281                 wpa_printf(MSG_INFO, "EAPOL-Key ANonce mismatch between 1/4 "
 282                            "and 3/4");
 283                 recalc = 1;
 284         }
 285         os_memcpy(sta->anonce, hdr->key_nonce, WPA_NONCE_LEN);
 286         if (recalc) {
 287                 derive_ptk(wt, bss, sta, key_info & WPA_KEY_INFO_TYPE_MASK,
 288                            data, len);
 289         }
 290
 291         if (!sta->ptk_set) {
 292                 wpa_printf(MSG_DEBUG, "No PTK known to process EAPOL-Key 3/4");
 293                 return;
 294         }
 295
 296         if (check_mic(sta->ptk.kck, key_info & WPA_KEY_INFO_TYPE_MASK,
 297                       data, len) < 0) {
 298                 wpa_printf(MSG_INFO, "Mismatch in EAPOL-Key 3/4 MIC");
 299                 return;
 300         }
 301         wpa_printf(MSG_DEBUG, "Valid MIC found in EAPOL-Key 3/4");
 302
 303         key_data = (const u8 *) (hdr + 1);
 304         /* TODO: handle WPA without EncrKeyData bit */
 305         if (!(key_info & WPA_KEY_INFO_ENCR_KEY_DATA)) {
 306                 wpa_printf(MSG_INFO, "EAPOL-Key 3/4 without EncrKeyData bit");
 307                 return;
 308         }
 309         ver = key_info & WPA_KEY_INFO_TYPE_MASK;
 310         decrypted = decrypt_eapol_key_data(sta->ptk.kek, ver, hdr,
 311                                            &decrypted_len);
 312         if (decrypted == NULL) {
 313                 wpa_printf(MSG_INFO, "Failed to decrypt EAPOL-Key Key Data");
 314                 return;
 315         }
 316         wpa_hexdump(MSG_DEBUG, "Decrypted EAPOL-Key Key Data",
 317                     decrypted, decrypted_len);
 318         /* TODO: parse KDEs and store GTK, IGTK */
 319         os_free(decrypted);
 320 }
 321
 322
 323 static void rx_data_eapol_key_4_of_4(struct wlantest *wt, const u8 *dst,
 324                                      const u8 *src, const u8 *data, size_t len)
 325 {
 326         struct wlantest_bss *bss;
 327         struct wlantest_sta *sta;
 328         const struct ieee802_1x_hdr *eapol;
 329         const struct wpa_eapol_key *hdr;
 330         u16 key_info;
 331
 332         wpa_printf(MSG_DEBUG, "EAPOL-Key 4/4 " MACSTR " -> " MACSTR,
 333                    MAC2STR(src), MAC2STR(dst));
 334         bss = bss_get(wt, dst);
 335         if (bss == NULL)
 336                 return;
 337         sta = sta_get(bss, src);
 338         if (sta == NULL)
 339                 return;
 340
 341         eapol = (const struct ieee802_1x_hdr *) data;
 342         hdr = (const struct wpa_eapol_key *) (eapol + 1);
 343         key_info = WPA_GET_BE16(hdr->key_info);
 344
 345         if (!sta->ptk_set) {
 346                 wpa_printf(MSG_DEBUG, "No PTK known to process EAPOL-Key 4/4");
 347                 return;
 348         }
 349
 350         if (sta->ptk_set &&
 351             check_mic(sta->ptk.kck, key_info & WPA_KEY_INFO_TYPE_MASK,
 352                       data, len) < 0) {
 353                 wpa_printf(MSG_INFO, "Mismatch in EAPOL-Key 4/4 MIC");
 354                 return;
 355         }
 356         wpa_printf(MSG_DEBUG, "Valid MIC found in EAPOL-Key 4/4");
 357 }
 358
 359
 360 static void rx_data_eapol_key_1_of_2(struct wlantest *wt, const u8 *dst,
 361                                      const u8 *src, const u8 *data, size_t len)
 362 {
 363         wpa_printf(MSG_DEBUG, "EAPOL-Key 1/2 " MACSTR " -> " MACSTR,
 364                    MAC2STR(src), MAC2STR(dst));
 365 }
 366
 367
 368 static void rx_data_eapol_key_2_of_2(struct wlantest *wt, const u8 *dst,
 369                                      const u8 *src, const u8 *data, size_t len)
 370 {
 371         wpa_printf(MSG_DEBUG, "EAPOL-Key 2/2 " MACSTR " -> " MACSTR,
 372                    MAC2STR(src), MAC2STR(dst));
 373 }
 374
 375
 376 static void rx_data_eapol_key(struct wlantest *wt, const u8 *dst,
 377                               const u8 *src, const u8 *data, size_t len,
 378                               int prot)
 379 {
 380         const struct ieee802_1x_hdr *eapol;
 381         const struct wpa_eapol_key *hdr;
 382         const u8 *key_data;
 383         u16 key_info, key_length, ver, key_data_length;
 384
 385         eapol = (const struct ieee802_1x_hdr *) data;
 386         hdr = (const struct wpa_eapol_key *) (eapol + 1);
 387
 388         wpa_hexdump(MSG_MSGDUMP, "EAPOL-Key",
 389                     (const u8 *) hdr, len - sizeof(*eapol));
 390         if (len < sizeof(*hdr)) {
 391                 wpa_printf(MSG_INFO, "Too short EAPOL-Key frame from " MACSTR,
 392                            MAC2STR(src));
 393                 return;
 394         }
 395
 396         if (hdr->type == EAPOL_KEY_TYPE_RC4) {
 397                 /* TODO: EAPOL-Key RC4 for WEP */
 398                 return;
 399         }
 400
 401         if (hdr->type != EAPOL_KEY_TYPE_RSN &&
 402             hdr->type != EAPOL_KEY_TYPE_WPA) {
 403                 wpa_printf(MSG_DEBUG, "Unsupported EAPOL-Key type %u",
 404                            hdr->type);
 405                 return;
 406         }
 407
 408         key_info = WPA_GET_BE16(hdr->key_info);
 409         key_length = WPA_GET_BE16(hdr->key_length);
 410         key_data_length = WPA_GET_BE16(hdr->key_data_length);
 411         key_data = (const u8 *) (hdr + 1);
 412         if (key_data + key_data_length > data + len) {
 413                 wpa_printf(MSG_INFO, "Truncated EAPOL-Key from " MACSTR,
 414                            MAC2STR(src));
 415                 return;
 416         }
 417         if (key_data + key_data_length < data + len) {
 418                 wpa_hexdump(MSG_DEBUG, "Extra data after EAPOL-Key Key Data "
 419                             "field", key_data + key_data_length,
 420                         data + len - key_data - key_data_length);
 421         }
 422
 423
 424         ver = key_info & WPA_KEY_INFO_TYPE_MASK;
 425         wpa_printf(MSG_DEBUG, "EAPOL-Key ver=%u %c idx=%u%s%s%s%s%s%s%s%s "
 426                    "datalen=%u",
 427                    ver, key_info & WPA_KEY_INFO_KEY_TYPE ? 'P' : 'G',
 428                    (key_info & WPA_KEY_INFO_KEY_INDEX_MASK) >>
 429                    WPA_KEY_INFO_KEY_INDEX_SHIFT,
 430                    (key_info & WPA_KEY_INFO_INSTALL) ? " Install" : "",
 431                    (key_info & WPA_KEY_INFO_ACK) ? " ACK" : "",
 432                    (key_info & WPA_KEY_INFO_MIC) ? " MIC" : "",
 433                    (key_info & WPA_KEY_INFO_SECURE) ? " Secure" : "",
 434                    (key_info & WPA_KEY_INFO_ERROR) ? " Error" : "",
 435                    (key_info & WPA_KEY_INFO_REQUEST) ? " Request" : "",
 436                    (key_info & WPA_KEY_INFO_ENCR_KEY_DATA) ? " Encr" : "",
 437                    (key_info & WPA_KEY_INFO_SMK_MESSAGE) ? " SMK" : "",
 438                    key_data_length);
 439
 440         if (ver != WPA_KEY_INFO_TYPE_HMAC_MD5_RC4 &&
 441             ver != WPA_KEY_INFO_TYPE_HMAC_SHA1_AES &&
 442             ver != WPA_KEY_INFO_TYPE_AES_128_CMAC) {
 443                 wpa_printf(MSG_DEBUG, "Unsupported EAPOL-Key Key Descriptor "
 444                            "Version %u", ver);
 445                 return;
 446         }
 447
 448         wpa_hexdump(MSG_MSGDUMP, "EAPOL-Key Replay Counter",
 449                     hdr->replay_counter, WPA_REPLAY_COUNTER_LEN);
 450         wpa_hexdump(MSG_MSGDUMP, "EAPOL-Key Key Nonce",
 451                     hdr->key_nonce, WPA_NONCE_LEN);
 452         wpa_hexdump(MSG_MSGDUMP, "EAPOL-Key Key IV",
 453                     hdr->key_iv, 16);
 454         wpa_hexdump(MSG_MSGDUMP, "EAPOL-Key RSC",
 455                     hdr->key_nonce, WPA_KEY_RSC_LEN);
 456         wpa_hexdump(MSG_MSGDUMP, "EAPOL-Key Key MIC",
 457                     hdr->key_mic, 16);
 458         wpa_hexdump(MSG_MSGDUMP, "EAPOL-Key Key Data",
 459                     key_data, key_data_length);
 460
 461         if (key_info & (WPA_KEY_INFO_ERROR | WPA_KEY_INFO_REQUEST))
 462                 return;
 463
 464         if (key_info & WPA_KEY_INFO_SMK_MESSAGE)
 465                 return;
 466
 467         if (key_info & WPA_KEY_INFO_KEY_TYPE) {
 468                 /* 4-Way Handshake */
 469                 switch (key_info & (WPA_KEY_INFO_SECURE |
 470                                     WPA_KEY_INFO_MIC |
 471                                     WPA_KEY_INFO_ACK |
 472                                     WPA_KEY_INFO_INSTALL)) {
 473                 case WPA_KEY_INFO_ACK:
 474                         rx_data_eapol_key_1_of_4(wt, dst, src, data, len);
 475                         break;
 476                 case WPA_KEY_INFO_MIC:
 477                         rx_data_eapol_key_2_of_4(wt, dst, src, data, len);
 478                         break;
 479                 case WPA_KEY_INFO_SECURE | WPA_KEY_INFO_MIC |
 480                         WPA_KEY_INFO_ACK | WPA_KEY_INFO_INSTALL:
 481                         rx_data_eapol_key_3_of_4(wt, dst, src, data, len);
 482                         break;
 483                 case WPA_KEY_INFO_SECURE | WPA_KEY_INFO_MIC:
 484                         rx_data_eapol_key_4_of_4(wt, dst, src, data, len);
 485                         break;
 486                 default:
 487                         wpa_printf(MSG_DEBUG, "Unsupported EAPOL-Key frame");
 488                         break;
 489                 }
 490         } else {
 491                 /* Group Key Handshake */
 492                 switch (key_info & (WPA_KEY_INFO_SECURE |
 493                                     WPA_KEY_INFO_MIC |
 494                                     WPA_KEY_INFO_ACK)) {
 495                 case WPA_KEY_INFO_SECURE | WPA_KEY_INFO_MIC |
 496                         WPA_KEY_INFO_ACK:
 497                         rx_data_eapol_key_1_of_2(wt, dst, src, data, len);
 498                         break;
 499                 case WPA_KEY_INFO_SECURE | WPA_KEY_INFO_MIC:
 500                         rx_data_eapol_key_2_of_2(wt, dst, src, data, len);
 501                         break;
 502                 default:
 503                         wpa_printf(MSG_DEBUG, "Unsupported EAPOL-Key frame");
 504                         break;
 505                 }
 506         }
 507 }
 508
 509
 510 static void rx_data_eapol(struct wlantest *wt, const u8 *dst, const u8 *src,
 511                           const u8 *data, size_t len, int prot)
 512 {
 513         const struct ieee802_1x_hdr *hdr;
 514         u16 length;
 515         const u8 *p;
 516
 517         wpa_hexdump(MSG_EXCESSIVE, "EAPOL", data, len);
 518         if (len < sizeof(*hdr)) {
 519                 wpa_printf(MSG_INFO, "Too short EAPOL frame from " MACSTR,
 520                            MAC2STR(src));
 521                 return;
 522         }
 523
 524         hdr = (const struct ieee802_1x_hdr *) data;
 525         length = be_to_host16(hdr->length);
 526         wpa_printf(MSG_DEBUG, "RX EAPOL: " MACSTR " -> " MACSTR "%s ver=%u "
 527                    "type=%u len=%u",
 528                    MAC2STR(src), MAC2STR(dst), prot ? " Prot" : "",
 529                    hdr->version, hdr->type, length);
 530         if (sizeof(*hdr) + length > len) {
 531                 wpa_printf(MSG_INFO, "Truncated EAPOL frame from " MACSTR,
 532                            MAC2STR(src));
 533                 return;
 534         }
 535
 536         if (sizeof(*hdr) + length < len) {
 537                 wpa_printf(MSG_INFO, "EAPOL frame with %d extra bytes",
 538                            (int) (len - sizeof(*hdr) - length));
 539         }
 540         p = (const u8 *) (hdr + 1);
 541
 542         switch (hdr->type) {
 543         case IEEE802_1X_TYPE_EAP_PACKET:
 544                 wpa_hexdump(MSG_MSGDUMP, "EAPOL - EAP packet", p, length);
 545                 break;
 546         case IEEE802_1X_TYPE_EAPOL_START:
 547                 wpa_hexdump(MSG_MSGDUMP, "EAPOL-Start", p, length);
 548                 break;
 549         case IEEE802_1X_TYPE_EAPOL_LOGOFF:
 550                 wpa_hexdump(MSG_MSGDUMP, "EAPOL-Logoff", p, length);
 551                 break;
 552         case IEEE802_1X_TYPE_EAPOL_KEY:
 553                 rx_data_eapol_key(wt, dst, src, data, sizeof(*hdr) + length,
 554                                   prot);
 555                 break;
 556         case IEEE802_1X_TYPE_EAPOL_ENCAPSULATED_ASF_ALERT:
 557                 wpa_hexdump(MSG_MSGDUMP, "EAPOL - Encapsulated ASF alert",
 558                             p, length);
 559                 break;
 560         default:
 561                 wpa_hexdump(MSG_MSGDUMP, "Unknown EAPOL payload", p, length);
 562                 break;
 563         }
 564 }
 565
 566
 567 static void rx_data_eth(struct wlantest *wt, const u8 *dst, const u8 *src,
 568                         u16 ethertype, const u8 *data, size_t len, int prot)
 569 {
 570         if (ethertype == ETH_P_PAE)
 571                 rx_data_eapol(wt, dst, src, data, len, prot);
 572 }
 573
 574
 575 static void rx_data_process(struct wlantest *wt, const u8 *dst, const u8 *src,
 576                             const u8 *data, size_t len, int prot)
 577 {
 578         if (len == 0)
 579                 return;
 580
 581         if (len >= 8 && os_memcmp(data, "\xaa\xaa\x03\x00\x00\x00", 6) == 0) {
 582                 rx_data_eth(wt, dst, src, WPA_GET_BE16(data + 6),
 583                             data + 8, len - 8, prot);
 584                 return;
 585         }
 586
 587         wpa_hexdump(MSG_DEBUG, "Unrecognized LLC", data, len > 8 ? 8 : len);
 588 }
 589
 590
 591 static void rx_data_bss_prot(struct wlantest *wt,
 592                              const struct ieee80211_hdr *hdr, const u8 *qos,
 593                              const u8 *dst, const u8 *src, const u8 *data,
 594                              size_t len)
 595 {
 596         /* TODO: Try to decrypt and if success, call rx_data_process() with
 597          * prot = 1 */
 598 }
 599
 600
 601 static void rx_data_bss(struct wlantest *wt, const struct ieee80211_hdr *hdr,
 602                         const u8 *qos, const u8 *dst, const u8 *src,
 603                         const u8 *data, size_t len)
 604 {
 605         u16 fc = le_to_host16(hdr->frame_control);
 606         int prot = !!(fc & WLAN_FC_ISWEP);
 607
 608         if (qos) {
 609                 u8 ack = (qos[0] & 0x60) >> 5;
 610                 wpa_printf(MSG_MSGDUMP, "BSS DATA: " MACSTR " -> " MACSTR
 611                            " len=%u%s tid=%u%s%s",
 612                            MAC2STR(src), MAC2STR(dst), (unsigned int) len,
 613                            prot ? " Prot" : "", qos[0] & 0x0f,
 614                            (qos[0] & 0x10) ? " EOSP" : "",
 615                            ack == 0 ? "" :
 616                            (ack == 1 ? " NoAck" :
 617                             (ack == 2 ? " NoExpAck" : " BA")));
 618         } else {
 619                 wpa_printf(MSG_MSGDUMP, "BSS DATA: " MACSTR " -> " MACSTR
 620                            " len=%u%s",
 621                            MAC2STR(src), MAC2STR(dst), (unsigned int) len,
 622                            prot ? " Prot" : "");
 623         }
 624
 625         if (prot)
 626                 rx_data_bss_prot(wt, hdr, qos, dst, src, data, len);
 627         else
 628                 rx_data_process(wt, dst, src, data, len, 0);
 629 }
 630
 631
 632 void rx_data(struct wlantest *wt, const u8 *data, size_t len)
 633 {
 634         const struct ieee80211_hdr *hdr;
 635         u16 fc, stype;
 636         size_t hdrlen;
 637         const u8 *qos = NULL;
 638
 639         if (len < 24)
 640                 return;
 641
 642         hdr = (const struct ieee80211_hdr *) data;
 643         fc = le_to_host16(hdr->frame_control);
 644         stype = WLAN_FC_GET_STYPE(fc);
 645         hdrlen = 24;
 646         if ((fc & (WLAN_FC_TODS | WLAN_FC_FROMDS)) ==
 647             (WLAN_FC_TODS | WLAN_FC_FROMDS))
 648                 hdrlen += ETH_ALEN;
 649         if (stype & 0x08) {
 650                 qos = data + hdrlen;
 651                 hdrlen += 2;
 652         }
 653         if (len < hdrlen)
 654                 return;
 655         wt->rx_data++;
 656
 657         switch (fc & (WLAN_FC_TODS | WLAN_FC_FROMDS)) {
 658         case 0:
 659                 wpa_printf(MSG_EXCESSIVE, "DATA %s%s%s IBSS DA=" MACSTR " SA="
 660                            MACSTR " BSSID=" MACSTR,
 661                            data_stype(WLAN_FC_GET_STYPE(fc)),
 662                            fc & WLAN_FC_PWRMGT ? " PwrMgt" : "",
 663                            fc & WLAN_FC_ISWEP ? " Prot" : "",
 664                            MAC2STR(hdr->addr1), MAC2STR(hdr->addr2),
 665                            MAC2STR(hdr->addr3));
 666                 break;
 667         case WLAN_FC_FROMDS:
 668                 wpa_printf(MSG_EXCESSIVE, "DATA %s%s%s FromDS DA=" MACSTR
 669                            " BSSID=" MACSTR " SA=" MACSTR,
 670                            data_stype(WLAN_FC_GET_STYPE(fc)),
 671                            fc & WLAN_FC_PWRMGT ? " PwrMgt" : "",
 672                            fc & WLAN_FC_ISWEP ? " Prot" : "",
 673                            MAC2STR(hdr->addr1), MAC2STR(hdr->addr2),
 674                            MAC2STR(hdr->addr3));
 675                 rx_data_bss(wt, hdr, qos, hdr->addr1, hdr->addr2,
 676                             data + hdrlen, len - hdrlen);
 677                 break;
 678         case WLAN_FC_TODS:
 679                 wpa_printf(MSG_EXCESSIVE, "DATA %s%s%s ToDS BSSID=" MACSTR
 680                            " SA=" MACSTR " DA=" MACSTR,
 681                            data_stype(WLAN_FC_GET_STYPE(fc)),
 682                            fc & WLAN_FC_PWRMGT ? " PwrMgt" : "",
 683                            fc & WLAN_FC_ISWEP ? " Prot" : "",
 684                            MAC2STR(hdr->addr1), MAC2STR(hdr->addr2),
 685                            MAC2STR(hdr->addr3));
 686                 rx_data_bss(wt, hdr, qos, hdr->addr3, hdr->addr2,
 687                             data + hdrlen, len - hdrlen);
 688                 break;
 689         case WLAN_FC_TODS | WLAN_FC_FROMDS:
 690                 wpa_printf(MSG_EXCESSIVE, "DATA %s%s%s WDS RA=" MACSTR " TA="
 691                            MACSTR " DA=" MACSTR " SA=" MACSTR,
 692                            data_stype(WLAN_FC_GET_STYPE(fc)),
 693                            fc & WLAN_FC_PWRMGT ? " PwrMgt" : "",
 694                            fc & WLAN_FC_ISWEP ? " Prot" : "",
 695                            MAC2STR(hdr->addr1), MAC2STR(hdr->addr2),
 696                            MAC2STR(hdr->addr3),
 697                            MAC2STR((const u8 *) (hdr + 1)));
 698                 break;
 699         }
 700 }