libeap/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             elems->ftie_len < sizeof(struct rsn_ftie))
 147                 return -1;
 148
 149         len = 2 * ETH_ALEN + 1 + 2 + 18 + 2 + elems->rsn_ie_len +
 150                 2 + 5 + 2 + elems->ftie_len;
 151
 152         buf = os_zalloc(len);
 153         if (buf == NULL)
 154                 return -1;
 155
 156         pos = buf;
 157         /* 1) TDLS initiator STA MAC address */
 158         os_memcpy(pos, elems->link_id + ETH_ALEN, ETH_ALEN);
 159         pos += ETH_ALEN;
 160         /* 2) TDLS responder STA MAC address */
 161         os_memcpy(pos, elems->link_id + 2 * ETH_ALEN, ETH_ALEN);
 162         pos += ETH_ALEN;
 163         /* 3) Transaction Sequence number */
 164         *pos++ = trans_seq;
 165         /* 4) Link Identifier IE */
 166         os_memcpy(pos, elems->link_id - 2, 2 + 18);
 167         pos += 2 + 18;
 168         /* 5) RSN IE */
 169         os_memcpy(pos, elems->rsn_ie - 2, 2 + elems->rsn_ie_len);
 170         pos += 2 + elems->rsn_ie_len;
 171         /* 6) Timeout Interval IE */
 172         os_memcpy(pos, elems->timeout_int - 2, 2 + 5);
 173         pos += 2 + 5;
 174         /* 7) FTIE, with the MIC field of the FTIE set to 0 */
 175         os_memcpy(pos, elems->ftie - 2, 2 + elems->ftie_len);
 176         pos += 2;
 177         tmp_ftie = (struct rsn_ftie *) pos;
 178         os_memset(tmp_ftie->mic, 0, 16);
 179         pos += elems->ftie_len;
 180
 181         wpa_hexdump(MSG_DEBUG, "TDLS: Data for FTIE MIC", buf, pos - buf);
 182         wpa_hexdump_key(MSG_DEBUG, "TDLS: KCK", tdls->tpk.kck, 16);
 183         ret = omac1_aes_128(tdls->tpk.kck, buf, pos - buf, mic);
 184         os_free(buf);
 185         if (ret)
 186                 return -1;
 187         wpa_hexdump(MSG_DEBUG, "TDLS: FTIE MIC", mic, 16);
 188         rx_ftie = (const struct rsn_ftie *) elems->ftie;
 189
 190         if (os_memcmp(mic, rx_ftie->mic, 16) == 0) {
 191                 add_note(wt, MSG_DEBUG, "TDLS: Valid MIC");
 192                 return 0;
 193         }
 194         add_note(wt, MSG_DEBUG, "TDLS: Invalid MIC");
 195         return -1;
 196 }
 197
 198
 199 static void rx_data_tdls_setup_request(struct wlantest *wt, const u8 *bssid,
 200                                        const u8 *sta_addr, const u8 *dst,
 201                                        const u8 *src,
 202                                        const u8 *data, size_t len)
 203 {
 204         struct ieee802_11_elems elems;
 205         struct wlantest_tdls *tdls;
 206         u8 linkid[3 * ETH_ALEN];
 207
 208         if (len < 3) {
 209                 add_note(wt, MSG_INFO, "Too short TDLS Setup Request " MACSTR
 210                          " -> " MACSTR, MAC2STR(src), MAC2STR(dst));
 211                 return;
 212         }
 213         wpa_printf(MSG_DEBUG, "TDLS Setup Request " MACSTR " -> "
 214                    MACSTR, MAC2STR(src), MAC2STR(dst));
 215
 216         if (ieee802_11_parse_elems(data + 3, len - 3, &elems, 1) ==
 217             ParseFailed || elems.link_id == NULL)
 218                 return;
 219         wpa_printf(MSG_DEBUG, "TDLS Link Identifier: BSSID " MACSTR
 220                    " initiator STA " MACSTR " responder STA " MACSTR,
 221                    MAC2STR(elems.link_id), MAC2STR(elems.link_id + ETH_ALEN),
 222                    MAC2STR(elems.link_id + 2 * ETH_ALEN));
 223         tdls = get_tdls(wt, elems.link_id, 1, bssid);
 224         if (tdls) {
 225                 tdls->counters[WLANTEST_TDLS_COUNTER_SETUP_REQ]++;
 226                 tdls->dialog_token = data[0];
 227                 if (elems.ftie && elems.ftie_len >= sizeof(struct rsn_ftie)) {
 228                         const struct rsn_ftie *f;
 229                         f = (const struct rsn_ftie *) elems.ftie;
 230                         os_memcpy(tdls->inonce, f->snonce, WPA_NONCE_LEN);
 231                 }
 232         }
 233
 234         /* Check whether reverse direction context exists already */
 235         os_memcpy(linkid, bssid, ETH_ALEN);
 236         os_memcpy(linkid + ETH_ALEN, dst, ETH_ALEN);
 237         os_memcpy(linkid + 2 * ETH_ALEN, src, ETH_ALEN);
 238         tdls = get_tdls(wt, linkid, 0, bssid);
 239         if (tdls)
 240                 add_note(wt, MSG_INFO, "Reverse direction TDLS context exists");
 241 }
 242
 243
 244 static void rx_data_tdls_setup_response_failure(struct wlantest *wt,
 245                                                 const u8 *bssid,
 246                                                 const u8 *sta_addr,
 247                                                 u8 dialog_token, u16 status)
 248 {
 249         struct wlantest_bss *bss;
 250         struct wlantest_tdls *tdls;
 251         struct wlantest_sta *sta;
 252
 253         if (status == WLAN_STATUS_SUCCESS) {
 254                 add_note(wt, MSG_INFO, "TDLS: Invalid TDLS Setup Response from "
 255                          MACSTR, MAC2STR(sta_addr));
 256                 return;
 257         }
 258
 259         bss = bss_find(wt, bssid);
 260         if (!bss)
 261                 return;
 262         sta = sta_find(bss, sta_addr);
 263         if (!sta)
 264                 return;
 265
 266         dl_list_for_each(tdls, &bss->tdls, struct wlantest_tdls, list) {
 267                 if (tdls->resp == sta) {
 268                         if (dialog_token != tdls->dialog_token) {
 269                                 add_note(wt, MSG_DEBUG, "TDLS: Dialog token "
 270                                          "mismatch in TDLS Setup Response "
 271                                          "(failure)");
 272                                 break;
 273                         }
 274                         add_note(wt, MSG_DEBUG, "TDLS: Found matching TDLS "
 275                                  "setup session based on dialog token");
 276                         tdls->counters[
 277                                 WLANTEST_TDLS_COUNTER_SETUP_RESP_FAIL]++;
 278                         break;
 279                 }
 280         }
 281 }
 282
 283
 284 static void rx_data_tdls_setup_response(struct wlantest *wt, const u8 *bssid,
 285                                         const u8 *sta_addr, const u8 *dst,
 286                                         const u8 *src,
 287                                         const u8 *data, size_t len)
 288 {
 289         u16 status;
 290         struct ieee802_11_elems elems;
 291         struct wlantest_tdls *tdls;
 292
 293         if (len < 3) {
 294                 add_note(wt, MSG_INFO, "Too short TDLS Setup Response " MACSTR
 295                          " -> " MACSTR, MAC2STR(src), MAC2STR(dst));
 296                 return;
 297         }
 298         status = WPA_GET_LE16(data);
 299         wpa_printf(MSG_DEBUG, "TDLS Setup Response " MACSTR " -> "
 300                    MACSTR " (status %d)",
 301                    MAC2STR(src), MAC2STR(dst), status);
 302         if (len < 5 && status == 0) {
 303                 add_note(wt, MSG_INFO, "Too short TDLS Setup Response " MACSTR
 304                          " -> " MACSTR, MAC2STR(src), MAC2STR(dst));
 305                 return;
 306         }
 307
 308         if (len < 5 ||
 309             ieee802_11_parse_elems(data + 5, len - 5, &elems, 1) ==
 310             ParseFailed || elems.link_id == NULL) {
 311                 /* Need to match TDLS link based on Dialog Token */
 312                 rx_data_tdls_setup_response_failure(wt, bssid, sta_addr,
 313                                                     data[2], status);
 314                 return;
 315         }
 316         wpa_printf(MSG_DEBUG, "TDLS Link Identifier: BSSID " MACSTR
 317                    " initiator STA " MACSTR " responder STA " MACSTR,
 318                    MAC2STR(elems.link_id), MAC2STR(elems.link_id + ETH_ALEN),
 319                    MAC2STR(elems.link_id + 2 * ETH_ALEN));
 320
 321         tdls = get_tdls(wt, elems.link_id, 1, bssid);
 322         if (!tdls) {
 323                 add_note(wt, MSG_INFO, "No match TDLS context found");
 324                 return;
 325         }
 326         if (status)
 327                 tdls->counters[WLANTEST_TDLS_COUNTER_SETUP_RESP_FAIL]++;
 328         else
 329                 tdls->counters[WLANTEST_TDLS_COUNTER_SETUP_RESP_OK]++;
 330
 331         if (status != WLAN_STATUS_SUCCESS)
 332                 return;
 333
 334         if (elems.ftie && elems.ftie_len >= sizeof(struct rsn_ftie)) {
 335                 const struct rsn_ftie *f;
 336                 f = (const struct rsn_ftie *) elems.ftie;
 337                 if (os_memcmp(tdls->inonce, f->snonce, WPA_NONCE_LEN) != 0) {
 338                         add_note(wt, MSG_INFO, "Mismatch in TDLS initiator "
 339                                  "nonce");
 340                 }
 341                 os_memcpy(tdls->rnonce, f->anonce, WPA_NONCE_LEN);
 342         }
 343
 344         if (tdls_derive_tpk(tdls, bssid, elems.ftie, elems.ftie_len) < 1)
 345                 return;
 346         if (tdls_verify_mic(wt, tdls, 2, &elems) == 0) {
 347                 tdls->dialog_token = data[2];
 348                 add_note(wt, MSG_DEBUG, "TDLS: Dialog Token for the link: %u",
 349                          tdls->dialog_token);
 350         }
 351 }
 352
 353
 354 static void rx_data_tdls_setup_confirm_failure(struct wlantest *wt,
 355                                                const u8 *bssid,
 356                                                const u8 *src,
 357                                                u8 dialog_token, u16 status)
 358 {
 359         struct wlantest_bss *bss;
 360         struct wlantest_tdls *tdls;
 361         struct wlantest_sta *sta;
 362
 363         if (status == WLAN_STATUS_SUCCESS) {
 364                 add_note(wt, MSG_INFO, "TDLS: Invalid TDLS Setup Confirm from "
 365                          MACSTR, MAC2STR(src));
 366                 return;
 367         }
 368
 369         bss = bss_find(wt, bssid);
 370         if (!bss)
 371                 return;
 372         sta = sta_find(bss, src);
 373         if (!sta)
 374                 return;
 375
 376         dl_list_for_each(tdls, &bss->tdls, struct wlantest_tdls, list) {
 377                 if (tdls->init == sta) {
 378                         if (dialog_token != tdls->dialog_token) {
 379                                 add_note(wt, MSG_DEBUG, "TDLS: Dialog token "
 380                                          "mismatch in TDLS Setup Confirm "
 381                                          "(failure)");
 382                                 break;
 383                         }
 384                         add_note(wt, MSG_DEBUG, "TDLS: Found matching TDLS "
 385                                  "setup session based on dialog token");
 386                         tdls->counters[
 387                                 WLANTEST_TDLS_COUNTER_SETUP_CONF_FAIL]++;
 388                         break;
 389                 }
 390         }
 391 }
 392
 393
 394 static void rx_data_tdls_setup_confirm(struct wlantest *wt, const u8 *bssid,
 395                                        const u8 *sta_addr, const u8 *dst,
 396                                        const u8 *src,
 397                                        const u8 *data, size_t len)
 398 {
 399         u16 status;
 400         struct ieee802_11_elems elems;
 401         struct wlantest_tdls *tdls;
 402         u8 link_id[3 * ETH_ALEN];
 403
 404         if (len < 3) {
 405                 add_note(wt, MSG_INFO, "Too short TDLS Setup Confirm " MACSTR
 406                          " -> " MACSTR, MAC2STR(src), MAC2STR(dst));
 407                 return;
 408         }
 409         status = WPA_GET_LE16(data);
 410         wpa_printf(MSG_DEBUG, "TDLS Setup Confirm " MACSTR " -> "
 411                    MACSTR " (status %d)",
 412                    MAC2STR(src), MAC2STR(dst), status);
 413
 414         if (ieee802_11_parse_elems(data + 3, len - 3, &elems, 1) ==
 415             ParseFailed || elems.link_id == NULL) {
 416                 /* Need to match TDLS link based on Dialog Token */
 417                 rx_data_tdls_setup_confirm_failure(wt, bssid, src,
 418                                                    data[2], status);
 419                 return;
 420         }
 421         wpa_printf(MSG_DEBUG, "TDLS Link Identifier: BSSID " MACSTR
 422                    " initiator STA " MACSTR " responder STA " MACSTR,
 423                    MAC2STR(elems.link_id), MAC2STR(elems.link_id + ETH_ALEN),
 424                    MAC2STR(elems.link_id + 2 * ETH_ALEN));
 425
 426         tdls = get_tdls(wt, elems.link_id, 1, bssid);
 427         if (tdls == NULL)
 428                 return;
 429         if (status)
 430                 tdls->counters[WLANTEST_TDLS_COUNTER_SETUP_CONF_FAIL]++;
 431         else
 432                 tdls->counters[WLANTEST_TDLS_COUNTER_SETUP_CONF_OK]++;
 433
 434         if (status != WLAN_STATUS_SUCCESS)
 435                 return;
 436
 437         if (elems.ftie && elems.ftie_len >= sizeof(struct rsn_ftie)) {
 438                 const struct rsn_ftie *f;
 439                 f = (const struct rsn_ftie *) elems.ftie;
 440                 if (os_memcmp(tdls->inonce, f->snonce, WPA_NONCE_LEN) != 0) {
 441                         add_note(wt, MSG_INFO, "Mismatch in TDLS initiator "
 442                                  "nonce");
 443                 }
 444                 if (os_memcmp(tdls->rnonce, f->anonce, WPA_NONCE_LEN) != 0) {
 445                         add_note(wt, MSG_INFO, "Mismatch in TDLS responder "
 446                                  "nonce");
 447                 }
 448         }
 449
 450         tdls->link_up = 1;
 451         if (tdls_derive_tpk(tdls, bssid, elems.ftie, elems.ftie_len) < 1) {
 452                 if (elems.ftie == NULL)
 453                         goto remove_reverse;
 454                 return;
 455         }
 456         if (tdls_verify_mic(wt, tdls, 3, &elems) == 0) {
 457                 tdls->dialog_token = data[2];
 458                 add_note(wt, MSG_DEBUG, "TDLS: Link up - Dialog Token: %u",
 459                          tdls->dialog_token);
 460         }
 461
 462 remove_reverse:
 463         /*
 464          * The TDLS link itself is bidirectional, but there is explicit
 465          * initiator/responder roles. Remove the other direction of the link
 466          * (if it exists) to make sure that the link counters are stored for
 467          * the current TDLS entery.
 468          */
 469         os_memcpy(link_id, elems.link_id, ETH_ALEN);
 470         os_memcpy(link_id + ETH_ALEN, elems.link_id + 2 * ETH_ALEN, ETH_ALEN);
 471         os_memcpy(link_id + 2 * ETH_ALEN, elems.link_id + ETH_ALEN, ETH_ALEN);
 472         tdls = get_tdls(wt, link_id, 0, bssid);
 473         if (tdls) {
 474                 add_note(wt, MSG_DEBUG, "TDLS: Remove reverse link entry");
 475                 tdls_deinit(tdls);
 476         }
 477 }
 478
 479
 480 static int tdls_verify_mic_teardown(struct wlantest *wt,
 481                                     struct wlantest_tdls *tdls, u8 trans_seq,
 482                                     const u8 *reason_code,
 483                                     struct ieee802_11_elems *elems)
 484 {
 485         u8 *buf, *pos;
 486         int len;
 487         u8 mic[16];
 488         int ret;
 489         const struct rsn_ftie *rx_ftie;
 490         struct rsn_ftie *tmp_ftie;
 491
 492         if (elems->link_id == NULL || elems->ftie == NULL ||
 493             elems->ftie_len < sizeof(struct rsn_ftie))
 494                 return -1;
 495
 496         len = 2 + 18 + 2 + 1 + 1 + 2 + elems->ftie_len;
 497
 498         buf = os_zalloc(len);
 499         if (buf == NULL)
 500                 return -1;
 501
 502         pos = buf;
 503         /* 1) Link Identifier IE */
 504         os_memcpy(pos, elems->link_id - 2, 2 + 18);
 505         pos += 2 + 18;
 506         /* 2) Reason Code */
 507         os_memcpy(pos, reason_code, 2);
 508         pos += 2;
 509         /* 3) Dialog token */
 510         *pos++ = tdls->dialog_token;
 511         /* 4) Transaction Sequence number */
 512         *pos++ = trans_seq;
 513         /* 5) FTIE, with the MIC field of the FTIE set to 0 */
 514         os_memcpy(pos, elems->ftie - 2, 2 + elems->ftie_len);
 515         pos += 2;
 516         tmp_ftie = (struct rsn_ftie *) pos;
 517         os_memset(tmp_ftie->mic, 0, 16);
 518         pos += elems->ftie_len;
 519
 520         wpa_hexdump(MSG_DEBUG, "TDLS: Data for FTIE MIC", buf, pos - buf);
 521         wpa_hexdump_key(MSG_DEBUG, "TDLS: KCK", tdls->tpk.kck, 16);
 522         ret = omac1_aes_128(tdls->tpk.kck, buf, pos - buf, mic);
 523         os_free(buf);
 524         if (ret)
 525                 return -1;
 526         wpa_hexdump(MSG_DEBUG, "TDLS: FTIE MIC", mic, 16);
 527         rx_ftie = (const struct rsn_ftie *) elems->ftie;
 528
 529         if (os_memcmp(mic, rx_ftie->mic, 16) == 0) {
 530                 add_note(wt, MSG_DEBUG, "TDLS: Valid MIC");
 531                 return 0;
 532         }
 533         add_note(wt, MSG_DEBUG, "TDLS: Invalid MIC");
 534         return -1;
 535 }
 536
 537
 538 static void rx_data_tdls_teardown(struct wlantest *wt, const u8 *bssid,
 539                                   const u8 *sta_addr, const u8 *dst,
 540                                   const u8 *src,
 541                                   const u8 *data, size_t len)
 542 {
 543         u16 reason;
 544         struct ieee802_11_elems elems;
 545         struct wlantest_tdls *tdls;
 546
 547         if (len < 2)
 548                 return;
 549         reason = WPA_GET_LE16(data);
 550         wpa_printf(MSG_DEBUG, "TDLS Teardown " MACSTR " -> "
 551                    MACSTR " (reason %d)",
 552                    MAC2STR(src), MAC2STR(dst), reason);
 553
 554         if (ieee802_11_parse_elems(data + 2, len - 2, &elems, 1) ==
 555             ParseFailed || elems.link_id == NULL)
 556                 return;
 557         wpa_printf(MSG_DEBUG, "TDLS Link Identifier: BSSID " MACSTR
 558                    " initiator STA " MACSTR " responder STA " MACSTR,
 559                    MAC2STR(elems.link_id), MAC2STR(elems.link_id + ETH_ALEN),
 560                    MAC2STR(elems.link_id + 2 * ETH_ALEN));
 561
 562         tdls = get_tdls(wt, elems.link_id, 1, bssid);
 563         if (tdls) {
 564                 if (tdls->link_up)
 565                         add_note(wt, MSG_DEBUG, "TDLS: Link down");
 566                 tdls->link_up = 0;
 567                 tdls->counters[WLANTEST_TDLS_COUNTER_TEARDOWN]++;
 568                 tdls_verify_mic_teardown(wt, tdls, 4, data, &elems);
 569         }
 570 }
 571
 572
 573 static void rx_data_tdls(struct wlantest *wt, const u8 *bssid,
 574                          const u8 *sta_addr, const u8 *dst, const u8 *src,
 575                          const u8 *data, size_t len)
 576 {
 577         /* data contains the payload of a TDLS Action frame */
 578         if (len < 2 || data[0] != WLAN_ACTION_TDLS) {
 579                 wpa_hexdump(MSG_DEBUG, "Unrecognized encapsulated TDLS frame",
 580                             data, len);
 581                 return;
 582         }
 583
 584         switch (data[1]) {
 585         case WLAN_TDLS_SETUP_REQUEST:
 586                 rx_data_tdls_setup_request(wt, bssid, sta_addr, dst, src,
 587                                            data + 2, len - 2);
 588                 break;
 589         case WLAN_TDLS_SETUP_RESPONSE:
 590                 rx_data_tdls_setup_response(wt, bssid, sta_addr, dst, src,
 591                                             data + 2, len - 2);
 592                 break;
 593         case WLAN_TDLS_SETUP_CONFIRM:
 594                 rx_data_tdls_setup_confirm(wt, bssid, sta_addr, dst, src,
 595                                            data + 2, len - 2);
 596                 break;
 597         case WLAN_TDLS_TEARDOWN:
 598                 rx_data_tdls_teardown(wt, bssid, sta_addr, dst, src, data + 2,
 599                                       len - 2);
 600                 break;
 601         case WLAN_TDLS_DISCOVERY_REQUEST:
 602                 wpa_printf(MSG_DEBUG, "TDLS Discovery Request " MACSTR " -> "
 603                            MACSTR, MAC2STR(src), MAC2STR(dst));
 604                 break;
 605         }
 606 }
 607
 608
 609 void rx_data_80211_encap(struct wlantest *wt, const u8 *bssid,
 610                          const u8 *sta_addr, const u8 *dst, const u8 *src,
 611                          const u8 *data, size_t len)
 612 {
 613         wpa_hexdump(MSG_EXCESSIVE, "802.11 data encap frame", data, len);
 614         if (len < 1)
 615                 return;
 616         if (data[0] == 0x02)
 617                 rx_data_tdls(wt, bssid, sta_addr, dst, src, data + 1, len - 1);
 618 }