wlantest/rx_data.c

   1 /*
   2  * Received Data frame processing
   3  * Copyright (c) 2010, Jouni Malinen <j@w1.fi>
   4  *
   5  * This software may be distributed under the terms of the BSD license.
   6  * See README for more details.
   7  */
   8
   9 #include "utils/includes.h"
  10 #include <linux/if_ether.h>
  11
  12 #include "utils/common.h"
  13 #include "common/defs.h"
  14 #include "common/ieee802_11_defs.h"
  15 #include "wlantest.h"
  16
  17
  18 static const char * data_stype(u16 stype)
  19 {
  20         switch (stype) {
  21         case WLAN_FC_STYPE_DATA:
  22                 return "DATA";
  23         case WLAN_FC_STYPE_DATA_CFACK:
  24                 return "DATA-CFACK";
  25         case WLAN_FC_STYPE_DATA_CFPOLL:
  26                 return "DATA-CFPOLL";
  27         case WLAN_FC_STYPE_DATA_CFACKPOLL:
  28                 return "DATA-CFACKPOLL";
  29         case WLAN_FC_STYPE_NULLFUNC:
  30                 return "NULLFUNC";
  31         case WLAN_FC_STYPE_CFACK:
  32                 return "CFACK";
  33         case WLAN_FC_STYPE_CFPOLL:
  34                 return "CFPOLL";
  35         case WLAN_FC_STYPE_CFACKPOLL:
  36                 return "CFACKPOLL";
  37         case WLAN_FC_STYPE_QOS_DATA:
  38                 return "QOSDATA";
  39         case WLAN_FC_STYPE_QOS_DATA_CFACK:
  40                 return "QOSDATA-CFACK";
  41         case WLAN_FC_STYPE_QOS_DATA_CFPOLL:
  42                 return "QOSDATA-CFPOLL";
  43         case WLAN_FC_STYPE_QOS_DATA_CFACKPOLL:
  44                 return "QOSDATA-CFACKPOLL";
  45         case WLAN_FC_STYPE_QOS_NULL:
  46                 return "QOS-NULL";
  47         case WLAN_FC_STYPE_QOS_CFPOLL:
  48                 return "QOS-CFPOLL";
  49         case WLAN_FC_STYPE_QOS_CFACKPOLL:
  50                 return "QOS-CFACKPOLL";
  51         }
  52         return "??";
  53 }
  54
  55
  56 static void rx_data_eth(struct wlantest *wt, const u8 *bssid,
  57                         const u8 *sta_addr, const u8 *dst, const u8 *src,
  58                         u16 ethertype, const u8 *data, size_t len, int prot,
  59                         const u8 *peer_addr)
  60 {
  61         switch (ethertype) {
  62         case ETH_P_PAE:
  63                 rx_data_eapol(wt, dst, src, data, len, prot);
  64                 break;
  65         case ETH_P_IP:
  66                 rx_data_ip(wt, bssid, sta_addr, dst, src, data, len,
  67                            peer_addr);
  68                 break;
  69         case 0x890d:
  70                 rx_data_80211_encap(wt, bssid, sta_addr, dst, src, data, len);
  71                 break;
  72         }
  73 }
  74
  75
  76 static void rx_data_process(struct wlantest *wt, const u8 *bssid,
  77                             const u8 *sta_addr,
  78                             const u8 *dst, const u8 *src,
  79                             const u8 *data, size_t len, int prot,
  80                             const u8 *peer_addr)
  81 {
  82         if (len == 0)
  83                 return;
  84
  85         if (len >= 8 && os_memcmp(data, "\xaa\xaa\x03\x00\x00\x00", 6) == 0) {
  86                 rx_data_eth(wt, bssid, sta_addr, dst, src,
  87                             WPA_GET_BE16(data + 6), data + 8, len - 8, prot,
  88                             peer_addr);
  89                 return;
  90         }
  91
  92         wpa_hexdump(MSG_DEBUG, "Unrecognized LLC", data, len > 8 ? 8 : len);
  93 }
  94
  95
  96 static u8 * try_all_ptk(struct wlantest *wt, int pairwise_cipher,
  97                         const struct ieee80211_hdr *hdr,
  98                         const u8 *data, size_t data_len, size_t *decrypted_len)
  99 {
 100         struct wlantest_ptk *ptk;
 101         u8 *decrypted;
 102         int prev_level = wpa_debug_level;
 103
 104         wpa_debug_level = MSG_WARNING;
 105         dl_list_for_each(ptk, &wt->ptk, struct wlantest_ptk, list) {
 106                 decrypted = NULL;
 107                 if ((pairwise_cipher == WPA_CIPHER_CCMP ||
 108                      pairwise_cipher == 0) && ptk->ptk_len == 48) {
 109                         decrypted = ccmp_decrypt(ptk->ptk.tk1, hdr, data,
 110                                                  data_len, decrypted_len);
 111                 }
 112                 if ((pairwise_cipher == WPA_CIPHER_TKIP ||
 113                      pairwise_cipher == 0) && ptk->ptk_len == 64) {
 114                         decrypted = tkip_decrypt(ptk->ptk.tk1, hdr, data,
 115                                                  data_len, decrypted_len);
 116                 }
 117                 if (decrypted) {
 118                         wpa_debug_level = prev_level;
 119                         add_note(wt, MSG_DEBUG, "Found PTK match from list of all known PTKs");
 120                         return decrypted;
 121                 }
 122         }
 123         wpa_debug_level = prev_level;
 124
 125         return NULL;
 126 }
 127
 128
 129 static void rx_data_bss_prot_group(struct wlantest *wt,
 130                                    const struct ieee80211_hdr *hdr,
 131                                    const u8 *qos, const u8 *dst, const u8 *src,
 132                                    const u8 *data, size_t len)
 133 {
 134         struct wlantest_bss *bss;
 135         int keyid;
 136         u8 *decrypted;
 137         size_t dlen;
 138         u8 pn[6];
 139
 140         bss = bss_get(wt, hdr->addr2);
 141         if (bss == NULL)
 142                 return;
 143         if (len < 4) {
 144                 add_note(wt, MSG_INFO, "Too short group addressed data frame");
 145                 return;
 146         }
 147
 148         if (bss->group_cipher & (WPA_CIPHER_TKIP | WPA_CIPHER_CCMP) &&
 149             !(data[3] & 0x20)) {
 150                 add_note(wt, MSG_INFO, "Expected TKIP/CCMP frame from "
 151                          MACSTR " did not have ExtIV bit set to 1",
 152                          MAC2STR(bss->bssid));
 153                 return;
 154         }
 155
 156         if (bss->group_cipher == WPA_CIPHER_TKIP) {
 157                 if (data[3] & 0x1f) {
 158                         add_note(wt, MSG_INFO, "TKIP frame from " MACSTR
 159                                  " used non-zero reserved bit",
 160                                  MAC2STR(bss->bssid));
 161                 }
 162                 if (data[1] != ((data[0] | 0x20) & 0x7f)) {
 163                         add_note(wt, MSG_INFO, "TKIP frame from " MACSTR
 164                                  " used incorrect WEPSeed[1] (was 0x%x, "
 165                                  "expected 0x%x)",
 166                                  MAC2STR(bss->bssid), data[1],
 167                                  (data[0] | 0x20) & 0x7f);
 168                 }
 169         } else if (bss->group_cipher == WPA_CIPHER_CCMP) {
 170                 if (data[2] != 0 || (data[3] & 0x1f) != 0) {
 171                         add_note(wt, MSG_INFO, "CCMP frame from " MACSTR
 172                                  " used non-zero reserved bit",
 173                                  MAC2STR(bss->bssid));
 174                 }
 175         }
 176
 177         keyid = data[3] >> 6;
 178         if (bss->gtk_len[keyid] == 0 && bss->group_cipher != WPA_CIPHER_WEP40)
 179         {
 180                 add_note(wt, MSG_MSGDUMP, "No GTK known to decrypt the frame "
 181                          "(A2=" MACSTR " KeyID=%d)",
 182                          MAC2STR(hdr->addr2), keyid);
 183                 return;
 184         }
 185
 186         if (bss->group_cipher == WPA_CIPHER_TKIP)
 187                 tkip_get_pn(pn, data);
 188         else if (bss->group_cipher == WPA_CIPHER_WEP40)
 189                 goto skip_replay_det;
 190         else
 191                 ccmp_get_pn(pn, data);
 192         if (os_memcmp(pn, bss->rsc[keyid], 6) <= 0) {
 193                 u16 seq_ctrl = le_to_host16(hdr->seq_ctrl);
 194                 add_note(wt, MSG_INFO, "CCMP/TKIP replay detected: A1=" MACSTR
 195                          " A2=" MACSTR " A3=" MACSTR " seq=%u frag=%u",
 196                          MAC2STR(hdr->addr1), MAC2STR(hdr->addr2),
 197                          MAC2STR(hdr->addr3),
 198                          WLAN_GET_SEQ_SEQ(seq_ctrl),
 199                          WLAN_GET_SEQ_FRAG(seq_ctrl));
 200                 wpa_hexdump(MSG_INFO, "RX PN", pn, 6);
 201                 wpa_hexdump(MSG_INFO, "RSC", bss->rsc[keyid], 6);
 202         }
 203
 204 skip_replay_det:
 205         if (bss->group_cipher == WPA_CIPHER_TKIP)
 206                 decrypted = tkip_decrypt(bss->gtk[keyid], hdr, data, len,
 207                                          &dlen);
 208         else if (bss->group_cipher == WPA_CIPHER_WEP40)
 209                 decrypted = wep_decrypt(wt, hdr, data, len, &dlen);
 210         else
 211                 decrypted = ccmp_decrypt(bss->gtk[keyid], hdr, data, len,
 212                                          &dlen);
 213         if (decrypted) {
 214                 rx_data_process(wt, bss->bssid, NULL, dst, src, decrypted,
 215                                 dlen, 1, NULL);
 216                 os_memcpy(bss->rsc[keyid], pn, 6);
 217                 write_pcap_decrypted(wt, (const u8 *) hdr, 24 + (qos ? 2 : 0),
 218                                      decrypted, dlen);
 219         } else
 220                 add_note(wt, MSG_DEBUG, "Failed to decrypt frame");
 221         os_free(decrypted);
 222 }
 223
 224
 225 static void rx_data_bss_prot(struct wlantest *wt,
 226                              const struct ieee80211_hdr *hdr, const u8 *qos,
 227                              const u8 *dst, const u8 *src, const u8 *data,
 228                              size_t len)
 229 {
 230         struct wlantest_bss *bss;
 231         struct wlantest_sta *sta, *sta2;
 232         int keyid;
 233         u16 fc = le_to_host16(hdr->frame_control);
 234         u8 *decrypted;
 235         size_t dlen;
 236         int tid;
 237         u8 pn[6], *rsc;
 238         struct wlantest_tdls *tdls = NULL, *found;
 239         const u8 *tk = NULL;
 240         int ptk_iter_done = 0;
 241         int try_ptk_iter = 0;
 242
 243         if (hdr->addr1[0] & 0x01) {
 244                 rx_data_bss_prot_group(wt, hdr, qos, dst, src, data, len);
 245                 return;
 246         }
 247
 248         if (fc & WLAN_FC_TODS) {
 249                 bss = bss_get(wt, hdr->addr1);
 250                 if (bss == NULL)
 251                         return;
 252                 sta = sta_get(bss, hdr->addr2);
 253                 if (sta)
 254                         sta->counters[WLANTEST_STA_COUNTER_PROT_DATA_TX]++;
 255         } else if (fc & WLAN_FC_FROMDS) {
 256                 bss = bss_get(wt, hdr->addr2);
 257                 if (bss == NULL)
 258                         return;
 259                 sta = sta_get(bss, hdr->addr1);
 260         } else {
 261                 bss = bss_get(wt, hdr->addr3);
 262                 if (bss == NULL)
 263                         return;
 264                 sta = sta_find(bss, hdr->addr2);
 265                 sta2 = sta_find(bss, hdr->addr1);
 266                 if (sta == NULL || sta2 == NULL)
 267                         return;
 268                 found = NULL;
 269                 dl_list_for_each(tdls, &bss->tdls, struct wlantest_tdls, list)
 270                 {
 271                         if ((tdls->init == sta && tdls->resp == sta2) ||
 272                             (tdls->init == sta2 && tdls->resp == sta)) {
 273                                 found = tdls;
 274                                 if (tdls->link_up)
 275                                         break;
 276                         }
 277                 }
 278                 if (found) {
 279                         if (!found->link_up)
 280                                 add_note(wt, MSG_DEBUG,
 281                                          "TDLS: Link not up, but Data "
 282                                          "frame seen");
 283                         tk = found->tpk.tk;
 284                         tdls = found;
 285                 }
 286         }
 287         if ((sta == NULL ||
 288              (!sta->ptk_set && sta->pairwise_cipher != WPA_CIPHER_WEP40)) &&
 289             tk == NULL) {
 290                 add_note(wt, MSG_MSGDUMP, "No PTK known to decrypt the frame");
 291                 if (dl_list_empty(&wt->ptk))
 292                         return;
 293                 try_ptk_iter = 1;
 294         }
 295
 296         if (len < 4) {
 297                 add_note(wt, MSG_INFO, "Too short encrypted data frame");
 298                 return;
 299         }
 300
 301         if (sta == NULL)
 302                 return;
 303         if (sta->pairwise_cipher & (WPA_CIPHER_TKIP | WPA_CIPHER_CCMP) &&
 304             !(data[3] & 0x20)) {
 305                 add_note(wt, MSG_INFO, "Expected TKIP/CCMP frame from "
 306                          MACSTR " did not have ExtIV bit set to 1",
 307                          MAC2STR(src));
 308                 return;
 309         }
 310
 311         if (tk == NULL && sta->pairwise_cipher == WPA_CIPHER_TKIP) {
 312                 if (data[3] & 0x1f) {
 313                         add_note(wt, MSG_INFO, "TKIP frame from " MACSTR
 314                                  " used non-zero reserved bit",
 315                                  MAC2STR(hdr->addr2));
 316                 }
 317                 if (data[1] != ((data[0] | 0x20) & 0x7f)) {
 318                         add_note(wt, MSG_INFO, "TKIP frame from " MACSTR
 319                                  " used incorrect WEPSeed[1] (was 0x%x, "
 320                                  "expected 0x%x)",
 321                                  MAC2STR(hdr->addr2), data[1],
 322                                  (data[0] | 0x20) & 0x7f);
 323                 }
 324         } else if (tk || sta->pairwise_cipher == WPA_CIPHER_CCMP) {
 325                 if (data[2] != 0 || (data[3] & 0x1f) != 0) {
 326                         add_note(wt, MSG_INFO, "CCMP frame from " MACSTR
 327                                  " used non-zero reserved bit",
 328                                  MAC2STR(hdr->addr2));
 329                 }
 330         }
 331
 332         keyid = data[3] >> 6;
 333         if (keyid != 0) {
 334                 add_note(wt, MSG_INFO, "Unexpected non-zero KeyID %d in "
 335                          "individually addressed Data frame from " MACSTR,
 336                          keyid, MAC2STR(hdr->addr2));
 337         }
 338
 339         if (qos) {
 340                 tid = qos[0] & 0x0f;
 341                 if (fc & WLAN_FC_TODS)
 342                         sta->tx_tid[tid]++;
 343                 else
 344                         sta->rx_tid[tid]++;
 345         } else {
 346                 tid = 0;
 347                 if (fc & WLAN_FC_TODS)
 348                         sta->tx_tid[16]++;
 349                 else
 350                         sta->rx_tid[16]++;
 351         }
 352         if (tk) {
 353                 if (os_memcmp(hdr->addr2, tdls->init->addr, ETH_ALEN) == 0)
 354                         rsc = tdls->rsc_init[tid];
 355                 else
 356                         rsc = tdls->rsc_resp[tid];
 357         } else if (fc & WLAN_FC_TODS)
 358                 rsc = sta->rsc_tods[tid];
 359         else
 360                 rsc = sta->rsc_fromds[tid];
 361
 362
 363         if (tk == NULL && sta->pairwise_cipher == WPA_CIPHER_TKIP)
 364                 tkip_get_pn(pn, data);
 365         else if (sta->pairwise_cipher == WPA_CIPHER_WEP40)
 366                 goto skip_replay_det;
 367         else
 368                 ccmp_get_pn(pn, data);
 369         if (os_memcmp(pn, rsc, 6) <= 0) {
 370                 u16 seq_ctrl = le_to_host16(hdr->seq_ctrl);
 371                 add_note(wt, MSG_INFO, "CCMP/TKIP replay detected: A1=" MACSTR
 372                          " A2=" MACSTR " A3=" MACSTR " seq=%u frag=%u",
 373                          MAC2STR(hdr->addr1), MAC2STR(hdr->addr2),
 374                          MAC2STR(hdr->addr3),
 375                          WLAN_GET_SEQ_SEQ(seq_ctrl),
 376                          WLAN_GET_SEQ_FRAG(seq_ctrl));
 377                 wpa_hexdump(MSG_INFO, "RX PN", pn, 6);
 378                 wpa_hexdump(MSG_INFO, "RSC", rsc, 6);
 379         }
 380
 381 skip_replay_det:
 382         if (tk)
 383                 decrypted = ccmp_decrypt(tk, hdr, data, len, &dlen);
 384         else if (sta->pairwise_cipher == WPA_CIPHER_TKIP)
 385                 decrypted = tkip_decrypt(sta->ptk.tk1, hdr, data, len, &dlen);
 386         else if (sta->pairwise_cipher == WPA_CIPHER_WEP40)
 387                 decrypted = wep_decrypt(wt, hdr, data, len, &dlen);
 388         else if (sta->ptk_set)
 389                 decrypted = ccmp_decrypt(sta->ptk.tk1, hdr, data, len, &dlen);
 390         else {
 391                 decrypted = try_all_ptk(wt, sta->pairwise_cipher, hdr, data,
 392                                         len, &dlen);
 393                 ptk_iter_done = 1;
 394         }
 395         if (!decrypted && !ptk_iter_done) {
 396                 decrypted = try_all_ptk(wt, sta->pairwise_cipher, hdr, data,
 397                                         len, &dlen);
 398                 if (decrypted) {
 399                         add_note(wt, MSG_DEBUG, "Current PTK did not work, but found a match from all known PTKs");
 400                 }
 401         }
 402         if (decrypted) {
 403                 u16 fc = le_to_host16(hdr->frame_control);
 404                 const u8 *peer_addr = NULL;
 405                 if (!(fc & (WLAN_FC_FROMDS | WLAN_FC_TODS)))
 406                         peer_addr = hdr->addr1;
 407                 os_memcpy(rsc, pn, 6);
 408                 rx_data_process(wt, bss->bssid, sta->addr, dst, src, decrypted,
 409                                 dlen, 1, peer_addr);
 410                 write_pcap_decrypted(wt, (const u8 *) hdr, 24 + (qos ? 2 : 0),
 411                                      decrypted, dlen);
 412         } else if (!try_ptk_iter)
 413                 add_note(wt, MSG_DEBUG, "Failed to decrypt frame");
 414         os_free(decrypted);
 415 }
 416
 417
 418 static void rx_data_bss(struct wlantest *wt, const struct ieee80211_hdr *hdr,
 419                         const u8 *qos, const u8 *dst, const u8 *src,
 420                         const u8 *data, size_t len)
 421 {
 422         u16 fc = le_to_host16(hdr->frame_control);
 423         int prot = !!(fc & WLAN_FC_ISWEP);
 424
 425         if (qos) {
 426                 u8 ack = (qos[0] & 0x60) >> 5;
 427                 wpa_printf(MSG_MSGDUMP, "BSS DATA: " MACSTR " -> " MACSTR
 428                            " len=%u%s tid=%u%s%s",
 429                            MAC2STR(src), MAC2STR(dst), (unsigned int) len,
 430                            prot ? " Prot" : "", qos[0] & 0x0f,
 431                            (qos[0] & 0x10) ? " EOSP" : "",
 432                            ack == 0 ? "" :
 433                            (ack == 1 ? " NoAck" :
 434                             (ack == 2 ? " NoExpAck" : " BA")));
 435         } else {
 436                 wpa_printf(MSG_MSGDUMP, "BSS DATA: " MACSTR " -> " MACSTR
 437                            " len=%u%s",
 438                            MAC2STR(src), MAC2STR(dst), (unsigned int) len,
 439                            prot ? " Prot" : "");
 440         }
 441
 442         if (prot)
 443                 rx_data_bss_prot(wt, hdr, qos, dst, src, data, len);
 444         else {
 445                 const u8 *bssid, *sta_addr, *peer_addr;
 446                 struct wlantest_bss *bss;
 447
 448                 if (fc & WLAN_FC_TODS) {
 449                         bssid = hdr->addr1;
 450                         sta_addr = hdr->addr2;
 451                         peer_addr = NULL;
 452                 } else if (fc & WLAN_FC_FROMDS) {
 453                         bssid = hdr->addr2;
 454                         sta_addr = hdr->addr1;
 455                         peer_addr = NULL;
 456                 } else {
 457                         bssid = hdr->addr3;
 458                         sta_addr = hdr->addr2;
 459                         peer_addr = hdr->addr1;
 460                 }
 461
 462                 bss = bss_get(wt, bssid);
 463                 if (bss) {
 464                         struct wlantest_sta *sta = sta_get(bss, sta_addr);
 465
 466                         if (sta) {
 467                                 if (qos) {
 468                                         int tid = qos[0] & 0x0f;
 469                                         if (fc & WLAN_FC_TODS)
 470                                                 sta->tx_tid[tid]++;
 471                                         else
 472                                                 sta->rx_tid[tid]++;
 473                                 } else {
 474                                         if (fc & WLAN_FC_TODS)
 475                                                 sta->tx_tid[16]++;
 476                                         else
 477                                                 sta->rx_tid[16]++;
 478                                 }
 479                         }
 480                 }
 481
 482                 rx_data_process(wt, bssid, sta_addr, dst, src, data, len, 0,
 483                                 peer_addr);
 484         }
 485 }
 486
 487
 488 static struct wlantest_tdls * get_tdls(struct wlantest *wt, const u8 *bssid,
 489                                        const u8 *sta1_addr,
 490                                        const u8 *sta2_addr)
 491 {
 492         struct wlantest_bss *bss;
 493         struct wlantest_sta *sta1, *sta2;
 494         struct wlantest_tdls *tdls, *found = NULL;
 495
 496         bss = bss_find(wt, bssid);
 497         if (bss == NULL)
 498                 return NULL;
 499         sta1 = sta_find(bss, sta1_addr);
 500         if (sta1 == NULL)
 501                 return NULL;
 502         sta2 = sta_find(bss, sta2_addr);
 503         if (sta2 == NULL)
 504                 return NULL;
 505
 506         dl_list_for_each(tdls, &bss->tdls, struct wlantest_tdls, list) {
 507                 if ((tdls->init == sta1 && tdls->resp == sta2) ||
 508                     (tdls->init == sta2 && tdls->resp == sta1)) {
 509                         found = tdls;
 510                         if (tdls->link_up)
 511                                 break;
 512                 }
 513         }
 514
 515         return found;
 516 }
 517
 518
 519 static void add_direct_link(struct wlantest *wt, const u8 *bssid,
 520                             const u8 *sta1_addr, const u8 *sta2_addr)
 521 {
 522         struct wlantest_tdls *tdls;
 523
 524         tdls = get_tdls(wt, bssid, sta1_addr, sta2_addr);
 525         if (tdls == NULL)
 526                 return;
 527
 528         if (tdls->link_up)
 529                 tdls->counters[WLANTEST_TDLS_COUNTER_VALID_DIRECT_LINK]++;
 530         else
 531                 tdls->counters[WLANTEST_TDLS_COUNTER_INVALID_DIRECT_LINK]++;
 532 }
 533
 534
 535 static void add_ap_path(struct wlantest *wt, const u8 *bssid,
 536                         const u8 *sta1_addr, const u8 *sta2_addr)
 537 {
 538         struct wlantest_tdls *tdls;
 539
 540         tdls = get_tdls(wt, bssid, sta1_addr, sta2_addr);
 541         if (tdls == NULL)
 542                 return;
 543
 544         if (tdls->link_up)
 545                 tdls->counters[WLANTEST_TDLS_COUNTER_INVALID_AP_PATH]++;
 546         else
 547                 tdls->counters[WLANTEST_TDLS_COUNTER_VALID_AP_PATH]++;
 548 }
 549
 550
 551 void rx_data(struct wlantest *wt, const u8 *data, size_t len)
 552 {
 553         const struct ieee80211_hdr *hdr;
 554         u16 fc, stype;
 555         size_t hdrlen;
 556         const u8 *qos = NULL;
 557
 558         if (len < 24)
 559                 return;
 560
 561         hdr = (const struct ieee80211_hdr *) data;
 562         fc = le_to_host16(hdr->frame_control);
 563         stype = WLAN_FC_GET_STYPE(fc);
 564         hdrlen = 24;
 565         if ((fc & (WLAN_FC_TODS | WLAN_FC_FROMDS)) ==
 566             (WLAN_FC_TODS | WLAN_FC_FROMDS))
 567                 hdrlen += ETH_ALEN;
 568         if (stype & 0x08) {
 569                 qos = data + hdrlen;
 570                 hdrlen += 2;
 571         }
 572         if (len < hdrlen)
 573                 return;
 574         wt->rx_data++;
 575
 576         switch (fc & (WLAN_FC_TODS | WLAN_FC_FROMDS)) {
 577         case 0:
 578                 wpa_printf(MSG_EXCESSIVE, "DATA %s%s%s IBSS DA=" MACSTR " SA="
 579                            MACSTR " BSSID=" MACSTR,
 580                            data_stype(WLAN_FC_GET_STYPE(fc)),
 581                            fc & WLAN_FC_PWRMGT ? " PwrMgt" : "",
 582                            fc & WLAN_FC_ISWEP ? " Prot" : "",
 583                            MAC2STR(hdr->addr1), MAC2STR(hdr->addr2),
 584                            MAC2STR(hdr->addr3));
 585                 add_direct_link(wt, hdr->addr3, hdr->addr1, hdr->addr2);
 586                 rx_data_bss(wt, hdr, qos, hdr->addr1, hdr->addr2,
 587                             data + hdrlen, len - hdrlen);
 588                 break;
 589         case WLAN_FC_FROMDS:
 590                 wpa_printf(MSG_EXCESSIVE, "DATA %s%s%s FromDS DA=" MACSTR
 591                            " BSSID=" MACSTR " SA=" MACSTR,
 592                            data_stype(WLAN_FC_GET_STYPE(fc)),
 593                            fc & WLAN_FC_PWRMGT ? " PwrMgt" : "",
 594                            fc & WLAN_FC_ISWEP ? " Prot" : "",
 595                            MAC2STR(hdr->addr1), MAC2STR(hdr->addr2),
 596                            MAC2STR(hdr->addr3));
 597                 add_ap_path(wt, hdr->addr2, hdr->addr1, hdr->addr3);
 598                 rx_data_bss(wt, hdr, qos, hdr->addr1, hdr->addr3,
 599                             data + hdrlen, len - hdrlen);
 600                 break;
 601         case WLAN_FC_TODS:
 602                 wpa_printf(MSG_EXCESSIVE, "DATA %s%s%s ToDS BSSID=" MACSTR
 603                            " SA=" MACSTR " DA=" MACSTR,
 604                            data_stype(WLAN_FC_GET_STYPE(fc)),
 605                            fc & WLAN_FC_PWRMGT ? " PwrMgt" : "",
 606                            fc & WLAN_FC_ISWEP ? " Prot" : "",
 607                            MAC2STR(hdr->addr1), MAC2STR(hdr->addr2),
 608                            MAC2STR(hdr->addr3));
 609                 add_ap_path(wt, hdr->addr1, hdr->addr3, hdr->addr2);
 610                 rx_data_bss(wt, hdr, qos, hdr->addr3, hdr->addr2,
 611                             data + hdrlen, len - hdrlen);
 612                 break;
 613         case WLAN_FC_TODS | WLAN_FC_FROMDS:
 614                 wpa_printf(MSG_EXCESSIVE, "DATA %s%s%s WDS RA=" MACSTR " TA="
 615                            MACSTR " DA=" MACSTR " SA=" MACSTR,
 616                            data_stype(WLAN_FC_GET_STYPE(fc)),
 617                            fc & WLAN_FC_PWRMGT ? " PwrMgt" : "",
 618                            fc & WLAN_FC_ISWEP ? " Prot" : "",
 619                            MAC2STR(hdr->addr1), MAC2STR(hdr->addr2),
 620                            MAC2STR(hdr->addr3),
 621                            MAC2STR((const u8 *) (hdr + 1)));
 622                 break;
 623         }
 624 }