libeap/wlantest/rx_data.c

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