wlantest/rx_tdls.c

   1 /*
   2  * Received Data frame processing for TDLS packets
   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
  11 #include "utils/common.h"
  12 #include "crypto/sha256.h"
  13 #include "crypto/crypto.h"
  14 #include "crypto/aes_wrap.h"
  15 #include "common/ieee802_11_defs.h"
  16 #include "common/ieee802_11_common.h"
  17 #include "wlantest.h"
  18
  19
  20 static struct wlantest_tdls * get_tdls(struct wlantest *wt, const u8 *linkid,
  21                                        int create_new, const u8 *bssid)
  22 {
  23         struct wlantest_bss *bss;
  24         struct wlantest_sta *init, *resp;
  25         struct wlantest_tdls *tdls;
  26
  27         bss = bss_find(wt, linkid);
  28         if (bss == NULL && bssid) {
  29                 bss = bss_find(wt, bssid);
  30                 if (bss)
  31                         add_note(wt, MSG_INFO, "TDLS: Incorrect BSSID " MACSTR
  32                                  " in LinkId?! (init=" MACSTR " resp="
  33                                  MACSTR ")",
  34                                  MAC2STR(linkid), MAC2STR(linkid + ETH_ALEN),
  35                                  MAC2STR(linkid + 2 * ETH_ALEN));
  36         }
  37         if (bss == NULL)
  38                 return NULL;
  39
  40         init = sta_find(bss, linkid + ETH_ALEN);
  41         if (init == NULL)
  42                 return NULL;
  43
  44         resp = sta_find(bss, linkid + 2 * ETH_ALEN);
  45         if (resp == NULL)
  46                 return NULL;
  47
  48         dl_list_for_each(tdls, &bss->tdls, struct wlantest_tdls, list) {
  49                 if (tdls->init == init && tdls->resp == resp)
  50                         return tdls;
  51         }
  52
  53         if (!create_new)
  54                 return NULL;
  55
  56         add_note(wt, MSG_DEBUG, "Add new TDLS link context: initiator " MACSTR
  57                  " responder " MACSTR " BSSID " MACSTR,
  58                  MAC2STR(linkid + ETH_ALEN),
  59                  MAC2STR(linkid + 2 * ETH_ALEN),
  60                  MAC2STR(bssid));
  61
  62         tdls = os_zalloc(sizeof(*tdls));
  63         if (tdls == NULL)
  64                 return NULL;
  65         tdls->init = init;
  66         tdls->resp = resp;
  67         dl_list_add(&bss->tdls, &tdls->list);
  68         return tdls;
  69 }
  70
  71
  72 static int tdls_derive_tpk(struct wlantest_tdls *tdls, const u8 *bssid,
  73                            const u8 *ftie, u8 ftie_len)
  74 {
  75         const struct rsn_ftie *f;
  76         u8 key_input[SHA256_MAC_LEN];
  77         const u8 *nonce[2];
  78         size_t len[2];
  79         u8 data[3 * ETH_ALEN];
  80
  81         if (ftie == NULL || ftie_len < sizeof(struct rsn_ftie))
  82                 return 0;
  83
  84         f = (const struct rsn_ftie *) ftie;
  85         wpa_hexdump(MSG_DEBUG, "TDLS ANonce", f->anonce, WPA_NONCE_LEN);
  86         wpa_hexdump(MSG_DEBUG, "TDLS SNonce", f->snonce, WPA_NONCE_LEN);
  87
  88         /*
  89          * IEEE Std 802.11z-2010 8.5.9.1:
  90          * TPK-Key-Input = SHA-256(min(SNonce, ANonce) || max(SNonce, ANonce))
  91          */
  92         len[0] = WPA_NONCE_LEN;
  93         len[1] = WPA_NONCE_LEN;
  94         if (os_memcmp(f->anonce, f->snonce, WPA_NONCE_LEN) < 0) {
  95                 nonce[0] = f->anonce;
  96                 nonce[1] = f->snonce;
  97         } else {
  98                 nonce[0] = f->snonce;
  99                 nonce[1] = f->anonce;
 100         }
 101         sha256_vector(2, nonce, len, key_input);
 102         wpa_hexdump_key(MSG_DEBUG, "TDLS: TPK-Key-Input",
 103                         key_input, SHA256_MAC_LEN);
 104
 105         /*
 106          * TPK-Key-Data = KDF-N_KEY(TPK-Key-Input, "TDLS PMK",
 107          *      min(MAC_I, MAC_R) || max(MAC_I, MAC_R) || BSSID || N_KEY)
 108          * TODO: is N_KEY really included in KDF Context and if so, in which
 109          * presentation format (little endian 16-bit?) is it used? It gets
 110          * added by the KDF anyway..
 111          */
 112
 113         if (os_memcmp(tdls->init->addr, tdls->resp->addr, ETH_ALEN) < 0) {
 114                 os_memcpy(data, tdls->init->addr, ETH_ALEN);
 115                 os_memcpy(data + ETH_ALEN, tdls->resp->addr, ETH_ALEN);
 116         } else {
 117                 os_memcpy(data, tdls->resp->addr, ETH_ALEN);
 118                 os_memcpy(data + ETH_ALEN, tdls->init->addr, ETH_ALEN);
 119         }
 120         os_memcpy(data + 2 * ETH_ALEN, bssid, ETH_ALEN);
 121         wpa_hexdump(MSG_DEBUG, "TDLS: KDF Context", data, sizeof(data));
 122
 123         sha256_prf(key_input, SHA256_MAC_LEN, "TDLS PMK", data, sizeof(data),
 124                    (u8 *) &tdls->tpk, sizeof(tdls->tpk));
 125         wpa_hexdump_key(MSG_DEBUG, "TDLS: TPK-KCK",
 126                         tdls->tpk.kck, sizeof(tdls->tpk.kck));
 127         wpa_hexdump_key(MSG_DEBUG, "TDLS: TPK-TK",
 128                         tdls->tpk.tk, sizeof(tdls->tpk.tk));
 129
 130         return 1;
 131 }
 132
 133
 134 static int tdls_verify_mic(struct wlantest *wt, struct wlantest_tdls *tdls,
 135                            u8 trans_seq, struct ieee802_11_elems *elems)
 136 {
 137         u8 *buf, *pos;
 138         int len;
 139         u8 mic[16];
 140         int ret;
 141         const struct rsn_ftie *rx_ftie;
 142         struct rsn_ftie *tmp_ftie;
 143
 144         if (elems->link_id == NULL || elems->rsn_ie == NULL ||
 145             elems->timeout_int == NULL || elems->ftie == NULL)
 146                 return -1;
 147
 148         len = 2 * ETH_ALEN + 1 + 2 + 18 + 2 + elems->rsn_ie_len +
 149                 2 + elems->timeout_int_len + 2 + elems->ftie_len;
 150
 151         buf = os_zalloc(len);
 152         if (buf == NULL)
 153                 return -1;
 154
 155         pos = buf;
 156         /* 1) TDLS initiator STA MAC address */
 157         os_memcpy(pos, elems->link_id + ETH_ALEN, ETH_ALEN);
 158         pos += ETH_ALEN;
 159         /* 2) TDLS responder STA MAC address */
 160         os_memcpy(pos, elems->link_id + 2 * ETH_ALEN, ETH_ALEN);
 161         pos += ETH_ALEN;
 162         /* 3) Transaction Sequence number */
 163         *pos++ = trans_seq;
 164         /* 4) Link Identifier IE */
 165         os_memcpy(pos, elems->link_id - 2, 2 + 18);
 166         pos += 2 + 18;
 167         /* 5) RSN IE */
 168         os_memcpy(pos, elems->rsn_ie - 2, 2 + elems->rsn_ie_len);
 169         pos += 2 + elems->rsn_ie_len;
 170         /* 6) Timeout Interval IE */
 171         os_memcpy(pos, elems->timeout_int - 2, 2 + elems->timeout_int_len);
 172         pos += 2 + elems->timeout_int_len;
 173         /* 7) FTIE, with the MIC field of the FTIE set to 0 */
 174         os_memcpy(pos, elems->ftie - 2, 2 + elems->ftie_len);
 175         pos += 2;
 176         tmp_ftie = (struct rsn_ftie *) pos;
 177         os_memset(tmp_ftie->mic, 0, 16);
 178         pos += elems->ftie_len;
 179
 180         wpa_hexdump(MSG_DEBUG, "TDLS: Data for FTIE MIC", buf, pos - buf);
 181         wpa_hexdump_key(MSG_DEBUG, "TDLS: KCK", tdls->tpk.kck, 16);
 182         ret = omac1_aes_128(tdls->tpk.kck, buf, pos - buf, mic);
 183         os_free(buf);
 184         if (ret)
 185                 return -1;
 186         wpa_hexdump(MSG_DEBUG, "TDLS: FTIE MIC", mic, 16);
 187         rx_ftie = (const struct rsn_ftie *) elems->ftie;
 188
 189         if (os_memcmp(mic, rx_ftie->mic, 16) == 0) {
 190                 add_note(wt, MSG_DEBUG, "TDLS: Valid MIC");
 191                 return 0;
 192         }
 193         add_note(wt, MSG_DEBUG, "TDLS: Invalid MIC");
 194         return -1;
 195 }
 196
 197
 198 static void rx_data_tdls_setup_request(struct wlantest *wt, const u8 *bssid,
 199                                        const u8 *sta_addr, const u8 *dst,
 200                                        const u8 *src,
 201                                        const u8 *data, size_t len)
 202 {
 203         struct ieee802_11_elems elems;
 204         struct wlantest_tdls *tdls;
 205         u8 linkid[3 * ETH_ALEN];
 206
 207         if (len < 3) {
 208                 add_note(wt, MSG_INFO, "Too short TDLS Setup Request " MACSTR
 209                          " -> " MACSTR, MAC2STR(src), MAC2STR(dst));
 210                 return;
 211         }
 212         wpa_printf(MSG_DEBUG, "TDLS Setup Request " MACSTR " -> "
 213                    MACSTR, MAC2STR(src), MAC2STR(dst));
 214
 215         if (ieee802_11_parse_elems(data + 3, len - 3, &elems, 1) ==
 216             ParseFailed || elems.link_id == NULL)
 217                 return;
 218         wpa_printf(MSG_DEBUG, "TDLS Link Identifier: BSSID " MACSTR
 219                    " initiator STA " MACSTR " responder STA " MACSTR,
 220                    MAC2STR(elems.link_id), MAC2STR(elems.link_id + ETH_ALEN),
 221                    MAC2STR(elems.link_id + 2 * ETH_ALEN));
 222         tdls = get_tdls(wt, elems.link_id, 1, bssid);
 223         if (tdls) {
 224                 tdls->counters[WLANTEST_TDLS_COUNTER_SETUP_REQ]++;
 225                 tdls->dialog_token = data[0];
 226                 if (elems.ftie && elems.ftie_len >= sizeof(struct rsn_ftie)) {
 227                         const struct rsn_ftie *f;
 228                         f = (const struct rsn_ftie *) elems.ftie;
 229                         os_memcpy(tdls->inonce, f->snonce, WPA_NONCE_LEN);
 230                 }
 231         }
 232
 233         /* Check whether reverse direction context exists already */
 234         os_memcpy(linkid, bssid, ETH_ALEN);
 235         os_memcpy(linkid + ETH_ALEN, dst, ETH_ALEN);
 236         os_memcpy(linkid + 2 * ETH_ALEN, src, ETH_ALEN);
 237         tdls = get_tdls(wt, linkid, 0, bssid);
 238         if (tdls)
 239                 add_note(wt, MSG_INFO, "Reverse direction TDLS context exists");
 240 }
 241
 242
 243 static void rx_data_tdls_setup_response_failure(struct wlantest *wt,
 244                                                 const u8 *bssid,
 245                                                 const u8 *sta_addr,
 246                                                 u8 dialog_token, u16 status)
 247 {
 248         struct wlantest_bss *bss;
 249         struct wlantest_tdls *tdls;
 250         struct wlantest_sta *sta;
 251
 252         if (status == WLAN_STATUS_SUCCESS) {
 253                 add_note(wt, MSG_INFO, "TDLS: Invalid TDLS Setup Response from "
 254                          MACSTR, MAC2STR(sta_addr));
 255                 return;
 256         }
 257
 258         bss = bss_find(wt, bssid);
 259         if (!bss)
 260                 return;
 261         sta = sta_find(bss, sta_addr);
 262         if (!sta)
 263                 return;
 264
 265         dl_list_for_each(tdls, &bss->tdls, struct wlantest_tdls, list) {
 266                 if (tdls->resp == sta) {
 267                         if (dialog_token != tdls->dialog_token) {
 268                                 add_note(wt, MSG_DEBUG, "TDLS: Dialog token "
 269                                          "mismatch in TDLS Setup Response "
 270                                          "(failure)");
 271                                 break;
 272                         }
 273                         add_note(wt, MSG_DEBUG, "TDLS: Found matching TDLS "
 274                                  "setup session based on dialog token");
 275                         tdls->counters[
 276                                 WLANTEST_TDLS_COUNTER_SETUP_RESP_FAIL]++;
 277                         break;
 278                 }
 279         }
 280 }
 281
 282
 283 static void rx_data_tdls_setup_response(struct wlantest *wt, const u8 *bssid,
 284                                         const u8 *sta_addr, const u8 *dst,
 285                                         const u8 *src,
 286                                         const u8 *data, size_t len)
 287 {
 288         u16 status;
 289         struct ieee802_11_elems elems;
 290         struct wlantest_tdls *tdls;
 291
 292         if (len < 3) {
 293                 add_note(wt, MSG_INFO, "Too short TDLS Setup Response " MACSTR
 294                          " -> " MACSTR, MAC2STR(src), MAC2STR(dst));
 295                 return;
 296         }
 297         status = WPA_GET_LE16(data);
 298         wpa_printf(MSG_DEBUG, "TDLS Setup Response " MACSTR " -> "
 299                    MACSTR " (status %d)",
 300                    MAC2STR(src), MAC2STR(dst), status);
 301         if (len < 5 && status == 0) {
 302                 add_note(wt, MSG_INFO, "Too short TDLS Setup Response " MACSTR
 303                          " -> " MACSTR, MAC2STR(src), MAC2STR(dst));
 304                 return;
 305         }
 306
 307         if (len < 5 ||
 308             ieee802_11_parse_elems(data + 5, len - 5, &elems, 1) ==
 309             ParseFailed || elems.link_id == NULL) {
 310                 /* Need to match TDLS link based on Dialog Token */
 311                 rx_data_tdls_setup_response_failure(wt, bssid, sta_addr,
 312                                                     data[2], status);
 313                 return;
 314         }
 315         wpa_printf(MSG_DEBUG, "TDLS Link Identifier: BSSID " MACSTR
 316                    " initiator STA " MACSTR " responder STA " MACSTR,
 317                    MAC2STR(elems.link_id), MAC2STR(elems.link_id + ETH_ALEN),
 318                    MAC2STR(elems.link_id + 2 * ETH_ALEN));
 319
 320         tdls = get_tdls(wt, elems.link_id, 1, bssid);
 321         if (!tdls) {
 322                 add_note(wt, MSG_INFO, "No match TDLS context found");
 323                 return;
 324         }
 325         if (status)
 326                 tdls->counters[WLANTEST_TDLS_COUNTER_SETUP_RESP_FAIL]++;
 327         else
 328                 tdls->counters[WLANTEST_TDLS_COUNTER_SETUP_RESP_OK]++;
 329
 330         if (status != WLAN_STATUS_SUCCESS)
 331                 return;
 332
 333         if (elems.ftie && elems.ftie_len >= sizeof(struct rsn_ftie)) {
 334                 const struct rsn_ftie *f;
 335                 f = (const struct rsn_ftie *) elems.ftie;
 336                 if (os_memcmp(tdls->inonce, f->snonce, WPA_NONCE_LEN) != 0) {
 337                         add_note(wt, MSG_INFO, "Mismatch in TDLS initiator "
 338                                  "nonce");
 339                 }
 340                 os_memcpy(tdls->rnonce, f->anonce, WPA_NONCE_LEN);
 341         }
 342
 343         if (tdls_derive_tpk(tdls, bssid, elems.ftie, elems.ftie_len) < 1)
 344                 return;
 345         if (tdls_verify_mic(wt, tdls, 2, &elems) == 0) {
 346                 tdls->dialog_token = data[2];
 347                 add_note(wt, MSG_DEBUG, "TDLS: Dialog Token for the link: %u",
 348                          tdls->dialog_token);
 349         }
 350 }
 351
 352
 353 static void rx_data_tdls_setup_confirm_failure(struct wlantest *wt,
 354                                                const u8 *bssid,
 355                                                const u8 *src,
 356                                                u8 dialog_token, u16 status)
 357 {
 358         struct wlantest_bss *bss;
 359         struct wlantest_tdls *tdls;
 360         struct wlantest_sta *sta;
 361
 362         if (status == WLAN_STATUS_SUCCESS) {
 363                 add_note(wt, MSG_INFO, "TDLS: Invalid TDLS Setup Confirm from "
 364                          MACSTR, MAC2STR(src));
 365                 return;
 366         }
 367
 368         bss = bss_find(wt, bssid);
 369         if (!bss)
 370                 return;
 371         sta = sta_find(bss, src);
 372         if (!sta)
 373                 return;
 374
 375         dl_list_for_each(tdls, &bss->tdls, struct wlantest_tdls, list) {
 376                 if (tdls->init == sta) {
 377                         if (dialog_token != tdls->dialog_token) {
 378                                 add_note(wt, MSG_DEBUG, "TDLS: Dialog token "
 379                                          "mismatch in TDLS Setup Confirm "
 380                                          "(failure)");
 381                                 break;
 382                         }
 383                         add_note(wt, MSG_DEBUG, "TDLS: Found matching TDLS "
 384                                  "setup session based on dialog token");
 385                         tdls->counters[
 386                                 WLANTEST_TDLS_COUNTER_SETUP_CONF_FAIL]++;
 387                         break;
 388                 }
 389         }
 390 }
 391
 392
 393 static void rx_data_tdls_setup_confirm(struct wlantest *wt, const u8 *bssid,
 394                                        const u8 *sta_addr, const u8 *dst,
 395                                        const u8 *src,
 396                                        const u8 *data, size_t len)
 397 {
 398         u16 status;
 399         struct ieee802_11_elems elems;
 400         struct wlantest_tdls *tdls;
 401         u8 link_id[3 * ETH_ALEN];
 402
 403         if (len < 3) {
 404                 add_note(wt, MSG_INFO, "Too short TDLS Setup Confirm " MACSTR
 405                          " -> " MACSTR, MAC2STR(src), MAC2STR(dst));
 406                 return;
 407         }
 408         status = WPA_GET_LE16(data);
 409         wpa_printf(MSG_DEBUG, "TDLS Setup Confirm " MACSTR " -> "
 410                    MACSTR " (status %d)",
 411                    MAC2STR(src), MAC2STR(dst), status);
 412
 413         if (ieee802_11_parse_elems(data + 3, len - 3, &elems, 1) ==
 414             ParseFailed || elems.link_id == NULL) {
 415                 /* Need to match TDLS link based on Dialog Token */
 416                 rx_data_tdls_setup_confirm_failure(wt, bssid, src,
 417                                                    data[2], status);
 418                 return;
 419         }
 420         wpa_printf(MSG_DEBUG, "TDLS Link Identifier: BSSID " MACSTR
 421                    " initiator STA " MACSTR " responder STA " MACSTR,
 422                    MAC2STR(elems.link_id), MAC2STR(elems.link_id + ETH_ALEN),
 423                    MAC2STR(elems.link_id + 2 * ETH_ALEN));
 424
 425         tdls = get_tdls(wt, elems.link_id, 1, bssid);
 426         if (tdls == NULL)
 427                 return;
 428         if (status)
 429                 tdls->counters[WLANTEST_TDLS_COUNTER_SETUP_CONF_FAIL]++;
 430         else
 431                 tdls->counters[WLANTEST_TDLS_COUNTER_SETUP_CONF_OK]++;
 432
 433         if (status != WLAN_STATUS_SUCCESS)
 434                 return;
 435
 436         if (elems.ftie && elems.ftie_len >= sizeof(struct rsn_ftie)) {
 437                 const struct rsn_ftie *f;
 438                 f = (const struct rsn_ftie *) elems.ftie;
 439                 if (os_memcmp(tdls->inonce, f->snonce, WPA_NONCE_LEN) != 0) {
 440                         add_note(wt, MSG_INFO, "Mismatch in TDLS initiator "
 441                                  "nonce");
 442                 }
 443                 if (os_memcmp(tdls->rnonce, f->anonce, WPA_NONCE_LEN) != 0) {
 444                         add_note(wt, MSG_INFO, "Mismatch in TDLS responder "
 445                                  "nonce");
 446                 }
 447         }
 448
 449         tdls->link_up = 1;
 450         if (tdls_derive_tpk(tdls, bssid, elems.ftie, elems.ftie_len) < 1) {
 451                 if (elems.ftie == NULL)
 452                         goto remove_reverse;
 453                 return;
 454         }
 455         if (tdls_verify_mic(wt, tdls, 3, &elems) == 0) {
 456                 tdls->dialog_token = data[2];
 457                 add_note(wt, MSG_DEBUG, "TDLS: Link up - Dialog Token: %u",
 458                          tdls->dialog_token);
 459         }
 460
 461 remove_reverse:
 462         /*
 463          * The TDLS link itself is bidirectional, but there is explicit
 464          * initiator/responder roles. Remove the other direction of the link
 465          * (if it exists) to make sure that the link counters are stored for
 466          * the current TDLS entery.
 467          */
 468         os_memcpy(link_id, elems.link_id, ETH_ALEN);
 469         os_memcpy(link_id + ETH_ALEN, elems.link_id + 2 * ETH_ALEN, ETH_ALEN);
 470         os_memcpy(link_id + 2 * ETH_ALEN, elems.link_id + ETH_ALEN, ETH_ALEN);
 471         tdls = get_tdls(wt, link_id, 0, bssid);
 472         if (tdls) {
 473                 add_note(wt, MSG_DEBUG, "TDLS: Remove reverse link entry");
 474                 tdls_deinit(tdls);
 475         }
 476 }
 477
 478
 479 static int tdls_verify_mic_teardown(struct wlantest *wt,
 480                                     struct wlantest_tdls *tdls, u8 trans_seq,
 481                                     const u8 *reason_code,
 482                                     struct ieee802_11_elems *elems)
 483 {
 484         u8 *buf, *pos;
 485         int len;
 486         u8 mic[16];
 487         int ret;
 488         const struct rsn_ftie *rx_ftie;
 489         struct rsn_ftie *tmp_ftie;
 490
 491         if (elems->link_id == NULL || elems->ftie == NULL)
 492                 return -1;
 493
 494         len = 2 + 18 + 2 + 1 + 1 + 2 + elems->ftie_len;
 495
 496         buf = os_zalloc(len);
 497         if (buf == NULL)
 498                 return -1;
 499
 500         pos = buf;
 501         /* 1) Link Identifier IE */
 502         os_memcpy(pos, elems->link_id - 2, 2 + 18);
 503         pos += 2 + 18;
 504         /* 2) Reason Code */
 505         os_memcpy(pos, reason_code, 2);
 506         pos += 2;
 507         /* 3) Dialog token */
 508         *pos++ = tdls->dialog_token;
 509         /* 4) Transaction Sequence number */
 510         *pos++ = trans_seq;
 511         /* 5) FTIE, with the MIC field of the FTIE set to 0 */
 512         os_memcpy(pos, elems->ftie - 2, 2 + elems->ftie_len);
 513         pos += 2;
 514         tmp_ftie = (struct rsn_ftie *) pos;
 515         os_memset(tmp_ftie->mic, 0, 16);
 516         pos += elems->ftie_len;
 517
 518         wpa_hexdump(MSG_DEBUG, "TDLS: Data for FTIE MIC", buf, pos - buf);
 519         wpa_hexdump_key(MSG_DEBUG, "TDLS: KCK", tdls->tpk.kck, 16);
 520         ret = omac1_aes_128(tdls->tpk.kck, buf, pos - buf, mic);
 521         os_free(buf);
 522         if (ret)
 523                 return -1;
 524         wpa_hexdump(MSG_DEBUG, "TDLS: FTIE MIC", mic, 16);
 525         rx_ftie = (const struct rsn_ftie *) elems->ftie;
 526
 527         if (os_memcmp(mic, rx_ftie->mic, 16) == 0) {
 528                 add_note(wt, MSG_DEBUG, "TDLS: Valid MIC");
 529                 return 0;
 530         }
 531         add_note(wt, MSG_DEBUG, "TDLS: Invalid MIC");
 532         return -1;
 533 }
 534
 535
 536 static void rx_data_tdls_teardown(struct wlantest *wt, const u8 *bssid,
 537                                   const u8 *sta_addr, const u8 *dst,
 538                                   const u8 *src,
 539                                   const u8 *data, size_t len)
 540 {
 541         u16 reason;
 542         struct ieee802_11_elems elems;
 543         struct wlantest_tdls *tdls;
 544
 545         if (len < 2)
 546                 return;
 547         reason = WPA_GET_LE16(data);
 548         wpa_printf(MSG_DEBUG, "TDLS Teardown " MACSTR " -> "
 549                    MACSTR " (reason %d)",
 550                    MAC2STR(src), MAC2STR(dst), reason);
 551
 552         if (ieee802_11_parse_elems(data + 2, len - 2, &elems, 1) ==
 553             ParseFailed || elems.link_id == NULL)
 554                 return;
 555         wpa_printf(MSG_DEBUG, "TDLS Link Identifier: BSSID " MACSTR
 556                    " initiator STA " MACSTR " responder STA " MACSTR,
 557                    MAC2STR(elems.link_id), MAC2STR(elems.link_id + ETH_ALEN),
 558                    MAC2STR(elems.link_id + 2 * ETH_ALEN));
 559
 560         tdls = get_tdls(wt, elems.link_id, 1, bssid);
 561         if (tdls) {
 562                 if (tdls->link_up)
 563                         add_note(wt, MSG_DEBUG, "TDLS: Link down");
 564                 tdls->link_up = 0;
 565                 tdls->counters[WLANTEST_TDLS_COUNTER_TEARDOWN]++;
 566                 tdls_verify_mic_teardown(wt, tdls, 4, data, &elems);
 567         }
 568 }
 569
 570
 571 static void rx_data_tdls(struct wlantest *wt, const u8 *bssid,
 572                          const u8 *sta_addr, const u8 *dst, const u8 *src,
 573                          const u8 *data, size_t len)
 574 {
 575         /* data contains the payload of a TDLS Action frame */
 576         if (len < 2 || data[0] != WLAN_ACTION_TDLS) {
 577                 wpa_hexdump(MSG_DEBUG, "Unrecognized encapsulated TDLS frame",
 578                             data, len);
 579                 return;
 580         }
 581
 582         switch (data[1]) {
 583         case WLAN_TDLS_SETUP_REQUEST:
 584                 rx_data_tdls_setup_request(wt, bssid, sta_addr, dst, src,
 585                                            data + 2, len - 2);
 586                 break;
 587         case WLAN_TDLS_SETUP_RESPONSE:
 588                 rx_data_tdls_setup_response(wt, bssid, sta_addr, dst, src,
 589                                             data + 2, len - 2);
 590                 break;
 591         case WLAN_TDLS_SETUP_CONFIRM:
 592                 rx_data_tdls_setup_confirm(wt, bssid, sta_addr, dst, src,
 593                                            data + 2, len - 2);
 594                 break;
 595         case WLAN_TDLS_TEARDOWN:
 596                 rx_data_tdls_teardown(wt, bssid, sta_addr, dst, src, data + 2,
 597                                       len - 2);
 598                 break;
 599         case WLAN_TDLS_DISCOVERY_REQUEST:
 600                 wpa_printf(MSG_DEBUG, "TDLS Discovery Request " MACSTR " -> "
 601                            MACSTR, MAC2STR(src), MAC2STR(dst));
 602                 break;
 603         }
 604 }
 605
 606
 607 void rx_data_80211_encap(struct wlantest *wt, const u8 *bssid,
 608                          const u8 *sta_addr, const u8 *dst, const u8 *src,
 609                          const u8 *data, size_t len)
 610 {
 611         wpa_hexdump(MSG_EXCESSIVE, "802.11 data encap frame", data, len);
 612         if (len < 1)
 613                 return;
 614         if (data[0] == 0x02)
 615                 rx_data_tdls(wt, bssid, sta_addr, dst, src, data + 1, len - 1);
 616 }