libeap/wlantest/ctrl.c

   1 /*
   2  * wlantest control interface
   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 #include <sys/un.h>
  11
  12 #include "utils/common.h"
  13 #include "utils/eloop.h"
  14 #include "common/defs.h"
  15 #include "common/version.h"
  16 #include "common/ieee802_11_defs.h"
  17 #include "wlantest.h"
  18 #include "wlantest_ctrl.h"
  19
  20
  21 static u8 * attr_get(u8 *buf, size_t buflen, enum wlantest_ctrl_attr attr,
  22                      size_t *len)
  23 {
  24         u8 *pos = buf;
  25
  26         while (pos + 8 <= buf + buflen) {
  27                 enum wlantest_ctrl_attr a;
  28                 size_t alen;
  29                 a = WPA_GET_BE32(pos);
  30                 pos += 4;
  31                 alen = WPA_GET_BE32(pos);
  32                 pos += 4;
  33                 if (pos + alen > buf + buflen) {
  34                         wpa_printf(MSG_DEBUG, "Invalid control message "
  35                                    "attribute");
  36                         return NULL;
  37                 }
  38                 if (a == attr) {
  39                         *len = alen;
  40                         return pos;
  41                 }
  42                 pos += alen;
  43         }
  44
  45         return NULL;
  46 }
  47
  48
  49 static u8 * attr_get_macaddr(u8 *buf, size_t buflen,
  50                              enum wlantest_ctrl_attr attr)
  51 {
  52         u8 *addr;
  53         size_t addr_len;
  54         addr = attr_get(buf, buflen, attr, &addr_len);
  55         if (addr && addr_len != ETH_ALEN)
  56                 addr = NULL;
  57         return addr;
  58 }
  59
  60
  61 static int attr_get_int(u8 *buf, size_t buflen, enum wlantest_ctrl_attr attr)
  62 {
  63         u8 *pos;
  64         size_t len;
  65         pos = attr_get(buf, buflen, attr, &len);
  66         if (pos == NULL || len != 4)
  67                 return -1;
  68         return WPA_GET_BE32(pos);
  69 }
  70
  71
  72 static u8 * attr_add_str(u8 *pos, u8 *end, enum wlantest_ctrl_attr attr,
  73                          const char *str)
  74 {
  75         size_t len = os_strlen(str);
  76
  77         if (pos == NULL || end - pos < 8 + len)
  78                 return NULL;
  79         WPA_PUT_BE32(pos, attr);
  80         pos += 4;
  81         WPA_PUT_BE32(pos, len);
  82         pos += 4;
  83         os_memcpy(pos, str, len);
  84         pos += len;
  85         return pos;
  86 }
  87
  88
  89 static u8 * attr_add_be32(u8 *pos, u8 *end, enum wlantest_ctrl_attr attr,
  90                           u32 val)
  91 {
  92         if (pos == NULL || end - pos < 12)
  93                 return NULL;
  94         WPA_PUT_BE32(pos, attr);
  95         pos += 4;
  96         WPA_PUT_BE32(pos, 4);
  97         pos += 4;
  98         WPA_PUT_BE32(pos, val);
  99         pos += 4;
 100         return pos;
 101 }
 102
 103
 104 static void ctrl_disconnect(struct wlantest *wt, int sock)
 105 {
 106         int i;
 107         wpa_printf(MSG_DEBUG, "Disconnect control interface connection %d",
 108                    sock);
 109         for (i = 0; i < MAX_CTRL_CONNECTIONS; i++) {
 110                 if (wt->ctrl_socks[i] == sock) {
 111                         close(wt->ctrl_socks[i]);
 112                         eloop_unregister_read_sock(wt->ctrl_socks[i]);
 113                         wt->ctrl_socks[i] = -1;
 114                         break;
 115                 }
 116         }
 117 }
 118
 119
 120 static void ctrl_send(struct wlantest *wt, int sock, const u8 *buf,
 121                       size_t len)
 122 {
 123         if (send(sock, buf, len, 0) < 0) {
 124                 wpa_printf(MSG_INFO, "send(ctrl): %s", strerror(errno));
 125                 ctrl_disconnect(wt, sock);
 126         }
 127 }
 128
 129
 130 static void ctrl_send_simple(struct wlantest *wt, int sock,
 131                              enum wlantest_ctrl_cmd cmd)
 132 {
 133         u8 buf[4];
 134         WPA_PUT_BE32(buf, cmd);
 135         ctrl_send(wt, sock, buf, sizeof(buf));
 136 }
 137
 138
 139 static struct wlantest_bss * ctrl_get_bss(struct wlantest *wt, int sock,
 140                                           u8 *cmd, size_t clen)
 141 {
 142         struct wlantest_bss *bss;
 143         u8 *pos;
 144         size_t len;
 145
 146         pos = attr_get(cmd, clen, WLANTEST_ATTR_BSSID, &len);
 147         if (pos == NULL || len != ETH_ALEN) {
 148                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
 149                 return NULL;
 150         }
 151
 152         bss = bss_find(wt, pos);
 153         if (bss == NULL) {
 154                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
 155                 return NULL;
 156         }
 157
 158         return bss;
 159 }
 160
 161
 162 static struct wlantest_sta * ctrl_get_sta(struct wlantest *wt, int sock,
 163                                           u8 *cmd, size_t clen,
 164                                           struct wlantest_bss *bss)
 165 {
 166         struct wlantest_sta *sta;
 167         u8 *pos;
 168         size_t len;
 169
 170         if (bss == NULL)
 171                 return NULL;
 172
 173         pos = attr_get(cmd, clen, WLANTEST_ATTR_STA_ADDR, &len);
 174         if (pos == NULL || len != ETH_ALEN) {
 175                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
 176                 return NULL;
 177         }
 178
 179         sta = sta_find(bss, pos);
 180         if (sta == NULL) {
 181                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
 182                 return NULL;
 183         }
 184
 185         return sta;
 186 }
 187
 188
 189 static struct wlantest_sta * ctrl_get_sta2(struct wlantest *wt, int sock,
 190                                            u8 *cmd, size_t clen,
 191                                            struct wlantest_bss *bss)
 192 {
 193         struct wlantest_sta *sta;
 194         u8 *pos;
 195         size_t len;
 196
 197         if (bss == NULL)
 198                 return NULL;
 199
 200         pos = attr_get(cmd, clen, WLANTEST_ATTR_STA2_ADDR, &len);
 201         if (pos == NULL || len != ETH_ALEN) {
 202                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
 203                 return NULL;
 204         }
 205
 206         sta = sta_find(bss, pos);
 207         if (sta == NULL) {
 208                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
 209                 return NULL;
 210         }
 211
 212         return sta;
 213 }
 214
 215
 216 static void ctrl_list_bss(struct wlantest *wt, int sock)
 217 {
 218         u8 buf[WLANTEST_CTRL_MAX_RESP_LEN], *pos, *len;
 219         struct wlantest_bss *bss;
 220
 221         pos = buf;
 222         WPA_PUT_BE32(pos, WLANTEST_CTRL_SUCCESS);
 223         pos += 4;
 224         WPA_PUT_BE32(pos, WLANTEST_ATTR_BSSID);
 225         pos += 4;
 226         len = pos; /* to be filled */
 227         pos += 4;
 228
 229         dl_list_for_each(bss, &wt->bss, struct wlantest_bss, list) {
 230                 if (pos + ETH_ALEN > buf + WLANTEST_CTRL_MAX_RESP_LEN)
 231                         break;
 232                 os_memcpy(pos, bss->bssid, ETH_ALEN);
 233                 pos += ETH_ALEN;
 234         }
 235
 236         WPA_PUT_BE32(len, pos - len - 4);
 237         ctrl_send(wt, sock, buf, pos - buf);
 238 }
 239
 240
 241 static void ctrl_list_sta(struct wlantest *wt, int sock, u8 *cmd, size_t clen)
 242 {
 243         u8 buf[WLANTEST_CTRL_MAX_RESP_LEN], *pos, *len;
 244         struct wlantest_bss *bss;
 245         struct wlantest_sta *sta;
 246
 247         bss = ctrl_get_bss(wt, sock, cmd, clen);
 248         if (bss == NULL)
 249                 return;
 250
 251         pos = buf;
 252         WPA_PUT_BE32(pos, WLANTEST_CTRL_SUCCESS);
 253         pos += 4;
 254         WPA_PUT_BE32(pos, WLANTEST_ATTR_STA_ADDR);
 255         pos += 4;
 256         len = pos; /* to be filled */
 257         pos += 4;
 258
 259         dl_list_for_each(sta, &bss->sta, struct wlantest_sta, list) {
 260                 if (pos + ETH_ALEN > buf + WLANTEST_CTRL_MAX_RESP_LEN)
 261                         break;
 262                 os_memcpy(pos, sta->addr, ETH_ALEN);
 263                 pos += ETH_ALEN;
 264         }
 265
 266         WPA_PUT_BE32(len, pos - len - 4);
 267         ctrl_send(wt, sock, buf, pos - buf);
 268 }
 269
 270
 271 static void ctrl_flush(struct wlantest *wt, int sock)
 272 {
 273         wpa_printf(MSG_DEBUG, "Drop all collected BSS data");
 274         bss_flush(wt);
 275         ctrl_send_simple(wt, sock, WLANTEST_CTRL_SUCCESS);
 276 }
 277
 278
 279 static void ctrl_clear_sta_counters(struct wlantest *wt, int sock, u8 *cmd,
 280                                     size_t clen)
 281 {
 282         struct wlantest_bss *bss;
 283         struct wlantest_sta *sta;
 284
 285         bss = ctrl_get_bss(wt, sock, cmd, clen);
 286         sta = ctrl_get_sta(wt, sock, cmd, clen, bss);
 287         if (sta == NULL) {
 288                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
 289                 return;
 290         }
 291
 292         os_memset(sta->counters, 0, sizeof(sta->counters));
 293         os_memset(sta->tx_tid, 0, sizeof(sta->tx_tid));
 294         os_memset(sta->rx_tid, 0, sizeof(sta->rx_tid));
 295         ctrl_send_simple(wt, sock, WLANTEST_CTRL_SUCCESS);
 296 }
 297
 298
 299 static void ctrl_clear_bss_counters(struct wlantest *wt, int sock, u8 *cmd,
 300                                     size_t clen)
 301 {
 302         struct wlantest_bss *bss;
 303
 304         bss = ctrl_get_bss(wt, sock, cmd, clen);
 305         if (bss == NULL) {
 306                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
 307                 return;
 308         }
 309
 310         os_memset(bss->counters, 0, sizeof(bss->counters));
 311         ctrl_send_simple(wt, sock, WLANTEST_CTRL_SUCCESS);
 312 }
 313
 314
 315 static void ctrl_clear_tdls_counters(struct wlantest *wt, int sock, u8 *cmd,
 316                                      size_t clen)
 317 {
 318         struct wlantest_bss *bss;
 319         struct wlantest_sta *sta;
 320         struct wlantest_sta *sta2;
 321         struct wlantest_tdls *tdls;
 322
 323         bss = ctrl_get_bss(wt, sock, cmd, clen);
 324         sta = ctrl_get_sta(wt, sock, cmd, clen, bss);
 325         sta2 = ctrl_get_sta2(wt, sock, cmd, clen, bss);
 326         if (sta == NULL || sta2 == NULL) {
 327                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
 328                 return;
 329         }
 330
 331         dl_list_for_each(tdls, &bss->tdls, struct wlantest_tdls, list) {
 332                 if ((tdls->init == sta && tdls->resp == sta2) ||
 333                     (tdls->init == sta2 && tdls->resp == sta))
 334                         os_memset(tdls->counters, 0, sizeof(tdls->counters));
 335         }
 336         ctrl_send_simple(wt, sock, WLANTEST_CTRL_SUCCESS);
 337 }
 338
 339
 340 static void ctrl_get_sta_counter(struct wlantest *wt, int sock, u8 *cmd,
 341                                  size_t clen)
 342 {
 343         u8 *addr;
 344         size_t addr_len;
 345         struct wlantest_bss *bss;
 346         struct wlantest_sta *sta;
 347         u32 counter;
 348         u8 buf[4 + 12], *end, *pos;
 349
 350         bss = ctrl_get_bss(wt, sock, cmd, clen);
 351         sta = ctrl_get_sta(wt, sock, cmd, clen, bss);
 352         if (sta == NULL)
 353                 return;
 354
 355         addr = attr_get(cmd, clen, WLANTEST_ATTR_STA_COUNTER, &addr_len);
 356         if (addr == NULL || addr_len != 4) {
 357                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
 358                 return;
 359         }
 360         counter = WPA_GET_BE32(addr);
 361         if (counter >= NUM_WLANTEST_STA_COUNTER) {
 362                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
 363                 return;
 364         }
 365
 366         pos = buf;
 367         end = buf + sizeof(buf);
 368         WPA_PUT_BE32(pos, WLANTEST_CTRL_SUCCESS);
 369         pos += 4;
 370         pos = attr_add_be32(pos, end, WLANTEST_ATTR_COUNTER,
 371                             sta->counters[counter]);
 372         ctrl_send(wt, sock, buf, pos - buf);
 373 }
 374
 375
 376 static void ctrl_get_bss_counter(struct wlantest *wt, int sock, u8 *cmd,
 377                                  size_t clen)
 378 {
 379         u8 *addr;
 380         size_t addr_len;
 381         struct wlantest_bss *bss;
 382         u32 counter;
 383         u8 buf[4 + 12], *end, *pos;
 384
 385         bss = ctrl_get_bss(wt, sock, cmd, clen);
 386         if (bss == NULL)
 387                 return;
 388
 389         addr = attr_get(cmd, clen, WLANTEST_ATTR_BSS_COUNTER, &addr_len);
 390         if (addr == NULL || addr_len != 4) {
 391                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
 392                 return;
 393         }
 394         counter = WPA_GET_BE32(addr);
 395         if (counter >= NUM_WLANTEST_BSS_COUNTER) {
 396                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
 397                 return;
 398         }
 399
 400         pos = buf;
 401         end = buf + sizeof(buf);
 402         WPA_PUT_BE32(pos, WLANTEST_CTRL_SUCCESS);
 403         pos += 4;
 404         pos = attr_add_be32(pos, end, WLANTEST_ATTR_COUNTER,
 405                             bss->counters[counter]);
 406         ctrl_send(wt, sock, buf, pos - buf);
 407 }
 408
 409
 410 static void ctrl_get_tdls_counter(struct wlantest *wt, int sock, u8 *cmd,
 411                                   size_t clen)
 412 {
 413         u8 *addr;
 414         size_t addr_len;
 415         struct wlantest_bss *bss;
 416         struct wlantest_sta *sta;
 417         struct wlantest_sta *sta2;
 418         struct wlantest_tdls *tdls;
 419         u32 counter;
 420         u8 buf[4 + 12], *end, *pos;
 421         int found = 0;
 422
 423         bss = ctrl_get_bss(wt, sock, cmd, clen);
 424         sta = ctrl_get_sta(wt, sock, cmd, clen, bss);
 425         sta2 = ctrl_get_sta2(wt, sock, cmd, clen, bss);
 426         if (sta == NULL || sta2 == NULL) {
 427                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
 428                 return;
 429         }
 430
 431         addr = attr_get(cmd, clen, WLANTEST_ATTR_TDLS_COUNTER, &addr_len);
 432         if (addr == NULL || addr_len != 4) {
 433                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
 434                 return;
 435         }
 436         counter = WPA_GET_BE32(addr);
 437         if (counter >= NUM_WLANTEST_TDLS_COUNTER) {
 438                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
 439                 return;
 440         }
 441
 442         dl_list_for_each(tdls, &bss->tdls, struct wlantest_tdls, list) {
 443                 if (tdls->init == sta && tdls->resp == sta2) {
 444                         found = 1;
 445                         break;
 446                 }
 447         }
 448
 449         if (!found) {
 450                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
 451                 return;
 452         }
 453
 454         pos = buf;
 455         end = buf + sizeof(buf);
 456         WPA_PUT_BE32(pos, WLANTEST_CTRL_SUCCESS);
 457         pos += 4;
 458         pos = attr_add_be32(pos, end, WLANTEST_ATTR_COUNTER,
 459                             tdls->counters[counter]);
 460         ctrl_send(wt, sock, buf, pos - buf);
 461 }
 462
 463
 464 static void build_mgmt_hdr(struct ieee80211_mgmt *mgmt,
 465                            struct wlantest_bss *bss, struct wlantest_sta *sta,
 466                            int sender_ap, int stype)
 467 {
 468         os_memset(mgmt, 0, 24);
 469         mgmt->frame_control = IEEE80211_FC(WLAN_FC_TYPE_MGMT, stype);
 470         if (sender_ap) {
 471                 if (sta)
 472                         os_memcpy(mgmt->da, sta->addr, ETH_ALEN);
 473                 else
 474                         os_memset(mgmt->da, 0xff, ETH_ALEN);
 475                 os_memcpy(mgmt->sa, bss->bssid, ETH_ALEN);
 476         } else {
 477                 os_memcpy(mgmt->da, bss->bssid, ETH_ALEN);
 478                 os_memcpy(mgmt->sa, sta->addr, ETH_ALEN);
 479         }
 480         os_memcpy(mgmt->bssid, bss->bssid, ETH_ALEN);
 481 }
 482
 483
 484 static int ctrl_inject_auth(struct wlantest *wt, struct wlantest_bss *bss,
 485                             struct wlantest_sta *sta, int sender_ap,
 486                             enum wlantest_inject_protection prot)
 487 {
 488         struct ieee80211_mgmt mgmt;
 489
 490         if (prot != WLANTEST_INJECT_NORMAL &&
 491             prot != WLANTEST_INJECT_UNPROTECTED)
 492                 return -1; /* Authentication frame is never protected */
 493         if (sta == NULL)
 494                 return -1; /* No broadcast Authentication frames */
 495
 496         if (sender_ap)
 497                 wpa_printf(MSG_INFO, "INJECT: Auth " MACSTR " -> " MACSTR,
 498                            MAC2STR(bss->bssid), MAC2STR(sta->addr));
 499         else
 500                 wpa_printf(MSG_INFO, "INJECT: Auth " MACSTR " -> " MACSTR,
 501                            MAC2STR(sta->addr), MAC2STR(bss->bssid));
 502         build_mgmt_hdr(&mgmt, bss, sta, sender_ap, WLAN_FC_STYPE_AUTH);
 503
 504         mgmt.u.auth.auth_alg = host_to_le16(WLAN_AUTH_OPEN);
 505         mgmt.u.auth.auth_transaction = host_to_le16(1);
 506         mgmt.u.auth.status_code = host_to_le16(WLAN_STATUS_SUCCESS);
 507
 508         return wlantest_inject(wt, bss, sta, (u8 *) &mgmt, 24 + 6,
 509                                WLANTEST_INJECT_UNPROTECTED);
 510 }
 511
 512
 513 static int ctrl_inject_assocreq(struct wlantest *wt, struct wlantest_bss *bss,
 514                                 struct wlantest_sta *sta, int sender_ap,
 515                                 enum wlantest_inject_protection prot)
 516 {
 517         u8 *buf;
 518         struct ieee80211_mgmt *mgmt;
 519         int ret;
 520
 521         if (prot != WLANTEST_INJECT_NORMAL &&
 522             prot != WLANTEST_INJECT_UNPROTECTED)
 523                 return -1; /* Association Request frame is never protected */
 524         if (sta == NULL)
 525                 return -1; /* No broadcast Association Request frames */
 526         if (sender_ap)
 527                 return -1; /* No Association Request frame sent by AP */
 528         if (sta->assocreq_ies == NULL) {
 529                 wpa_printf(MSG_INFO, "INJECT: No previous (Re)Association "
 530                            "Request available for " MACSTR,
 531                            MAC2STR(sta->addr));
 532                 return -1;
 533         }
 534
 535         wpa_printf(MSG_INFO, "INJECT: AssocReq " MACSTR " -> " MACSTR,
 536                    MAC2STR(sta->addr), MAC2STR(bss->bssid));
 537         buf = os_malloc(sizeof(*mgmt) + sta->assocreq_ies_len);
 538         if (buf == NULL)
 539                 return -1;
 540         mgmt = (struct ieee80211_mgmt *) buf;
 541
 542         build_mgmt_hdr(mgmt, bss, sta, sender_ap, WLAN_FC_STYPE_ASSOC_REQ);
 543
 544         mgmt->u.assoc_req.capab_info = host_to_le16(sta->assocreq_capab_info);
 545         mgmt->u.assoc_req.listen_interval =
 546                 host_to_le16(sta->assocreq_listen_int);
 547         os_memcpy(mgmt->u.assoc_req.variable, sta->assocreq_ies,
 548                   sta->assocreq_ies_len);
 549
 550         ret = wlantest_inject(wt, bss, sta, buf,
 551                               24 + 4 + sta->assocreq_ies_len,
 552                               WLANTEST_INJECT_UNPROTECTED);
 553         os_free(buf);
 554         return ret;
 555 }
 556
 557
 558 static int ctrl_inject_reassocreq(struct wlantest *wt,
 559                                   struct wlantest_bss *bss,
 560                                   struct wlantest_sta *sta, int sender_ap,
 561                                   enum wlantest_inject_protection prot)
 562 {
 563         u8 *buf;
 564         struct ieee80211_mgmt *mgmt;
 565         int ret;
 566
 567         if (prot != WLANTEST_INJECT_NORMAL &&
 568             prot != WLANTEST_INJECT_UNPROTECTED)
 569                 return -1; /* Reassociation Request frame is never protected */
 570         if (sta == NULL)
 571                 return -1; /* No broadcast Reassociation Request frames */
 572         if (sender_ap)
 573                 return -1; /* No Reassociation Request frame sent by AP */
 574         if (sta->assocreq_ies == NULL) {
 575                 wpa_printf(MSG_INFO, "INJECT: No previous (Re)Association "
 576                            "Request available for " MACSTR,
 577                            MAC2STR(sta->addr));
 578                 return -1;
 579         }
 580
 581         wpa_printf(MSG_INFO, "INJECT: ReassocReq " MACSTR " -> " MACSTR,
 582                    MAC2STR(sta->addr), MAC2STR(bss->bssid));
 583         buf = os_malloc(sizeof(*mgmt) + sta->assocreq_ies_len);
 584         if (buf == NULL)
 585                 return -1;
 586         mgmt = (struct ieee80211_mgmt *) buf;
 587
 588         build_mgmt_hdr(mgmt, bss, sta, sender_ap, WLAN_FC_STYPE_REASSOC_REQ);
 589
 590         mgmt->u.reassoc_req.capab_info =
 591                 host_to_le16(sta->assocreq_capab_info);
 592         mgmt->u.reassoc_req.listen_interval =
 593                 host_to_le16(sta->assocreq_listen_int);
 594         os_memcpy(mgmt->u.reassoc_req.current_ap, bss->bssid, ETH_ALEN);
 595         os_memcpy(mgmt->u.reassoc_req.variable, sta->assocreq_ies,
 596                   sta->assocreq_ies_len);
 597
 598         ret = wlantest_inject(wt, bss, sta, buf,
 599                               24 + 10 + sta->assocreq_ies_len,
 600                               WLANTEST_INJECT_UNPROTECTED);
 601         os_free(buf);
 602         return ret;
 603 }
 604
 605
 606 static int ctrl_inject_deauth(struct wlantest *wt, struct wlantest_bss *bss,
 607                               struct wlantest_sta *sta, int sender_ap,
 608                               enum wlantest_inject_protection prot)
 609 {
 610         struct ieee80211_mgmt mgmt;
 611
 612         if (sender_ap) {
 613                 if (sta)
 614                         wpa_printf(MSG_INFO, "INJECT: Deauth " MACSTR " -> "
 615                                    MACSTR,
 616                                    MAC2STR(bss->bssid), MAC2STR(sta->addr));
 617                 else
 618                         wpa_printf(MSG_INFO, "INJECT: Deauth " MACSTR
 619                                    " -> broadcast", MAC2STR(bss->bssid));
 620         } else
 621                 wpa_printf(MSG_INFO, "INJECT: Deauth " MACSTR " -> " MACSTR,
 622                            MAC2STR(sta->addr), MAC2STR(bss->bssid));
 623         build_mgmt_hdr(&mgmt, bss, sta, sender_ap, WLAN_FC_STYPE_DEAUTH);
 624
 625         mgmt.u.deauth.reason_code = host_to_le16(WLAN_REASON_UNSPECIFIED);
 626
 627         return wlantest_inject(wt, bss, sta, (u8 *) &mgmt, 24 + 2, prot);
 628 }
 629
 630
 631 static int ctrl_inject_disassoc(struct wlantest *wt, struct wlantest_bss *bss,
 632                                 struct wlantest_sta *sta, int sender_ap,
 633                                 enum wlantest_inject_protection prot)
 634 {
 635         struct ieee80211_mgmt mgmt;
 636
 637         if (sender_ap) {
 638                 if (sta)
 639                         wpa_printf(MSG_INFO, "INJECT: Disassoc " MACSTR " -> "
 640                                    MACSTR,
 641                                    MAC2STR(bss->bssid), MAC2STR(sta->addr));
 642                 else
 643                         wpa_printf(MSG_INFO, "INJECT: Disassoc " MACSTR
 644                                    " -> broadcast", MAC2STR(bss->bssid));
 645         } else
 646                 wpa_printf(MSG_INFO, "INJECT: Disassoc " MACSTR " -> " MACSTR,
 647                            MAC2STR(sta->addr), MAC2STR(bss->bssid));
 648         build_mgmt_hdr(&mgmt, bss, sta, sender_ap, WLAN_FC_STYPE_DISASSOC);
 649
 650         mgmt.u.disassoc.reason_code = host_to_le16(WLAN_REASON_UNSPECIFIED);
 651
 652         return wlantest_inject(wt, bss, sta, (u8 *) &mgmt, 24 + 2, prot);
 653 }
 654
 655
 656 static int ctrl_inject_saqueryreq(struct wlantest *wt,
 657                                   struct wlantest_bss *bss,
 658                                   struct wlantest_sta *sta, int sender_ap,
 659                                   enum wlantest_inject_protection prot)
 660 {
 661         struct ieee80211_mgmt mgmt;
 662
 663         if (sta == NULL)
 664                 return -1; /* No broadcast SA Query frames */
 665
 666         if (sender_ap)
 667                 wpa_printf(MSG_INFO, "INJECT: SA Query Request " MACSTR " -> "
 668                            MACSTR, MAC2STR(bss->bssid), MAC2STR(sta->addr));
 669         else
 670                 wpa_printf(MSG_INFO, "INJECT: SA Query Request " MACSTR " -> "
 671                            MACSTR, MAC2STR(sta->addr), MAC2STR(bss->bssid));
 672         build_mgmt_hdr(&mgmt, bss, sta, sender_ap, WLAN_FC_STYPE_ACTION);
 673
 674         mgmt.u.action.category = WLAN_ACTION_SA_QUERY;
 675         mgmt.u.action.u.sa_query_req.action = WLAN_SA_QUERY_REQUEST;
 676         mgmt.u.action.u.sa_query_req.trans_id[0] = 0x12;
 677         mgmt.u.action.u.sa_query_req.trans_id[1] = 0x34;
 678         os_memcpy(sender_ap ? sta->ap_sa_query_tr : sta->sta_sa_query_tr,
 679                   mgmt.u.action.u.sa_query_req.trans_id,
 680                   WLAN_SA_QUERY_TR_ID_LEN);
 681         return wlantest_inject(wt, bss, sta, (u8 *) &mgmt, 24 + 4, prot);
 682 }
 683
 684
 685 static void ctrl_inject(struct wlantest *wt, int sock, u8 *cmd, size_t clen)
 686 {
 687         u8 *bssid, *sta_addr;
 688         struct wlantest_bss *bss;
 689         struct wlantest_sta *sta;
 690         int frame, sender_ap, prot;
 691         int ret = 0;
 692
 693         bssid = attr_get_macaddr(cmd, clen, WLANTEST_ATTR_BSSID);
 694         sta_addr = attr_get_macaddr(cmd, clen, WLANTEST_ATTR_STA_ADDR);
 695         frame = attr_get_int(cmd, clen, WLANTEST_ATTR_INJECT_FRAME);
 696         sender_ap = attr_get_int(cmd, clen, WLANTEST_ATTR_INJECT_SENDER_AP);
 697         if (sender_ap < 0)
 698                 sender_ap = 0;
 699         prot = attr_get_int(cmd, clen, WLANTEST_ATTR_INJECT_PROTECTION);
 700         if (bssid == NULL || sta_addr == NULL || frame < 0 || prot < 0) {
 701                 wpa_printf(MSG_INFO, "Invalid inject command parameters");
 702                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
 703                 return;
 704         }
 705
 706         bss = bss_find(wt, bssid);
 707         if (bss == NULL) {
 708                 wpa_printf(MSG_INFO, "BSS not found for inject command");
 709                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
 710                 return;
 711         }
 712
 713         if (is_broadcast_ether_addr(sta_addr)) {
 714                 if (!sender_ap) {
 715                         wpa_printf(MSG_INFO, "Invalid broadcast inject "
 716                                    "command without sender_ap set");
 717                         ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
 718                         return;
 719                 } sta = NULL;
 720         } else {
 721                 sta = sta_find(bss, sta_addr);
 722                 if (sta == NULL) {
 723                         wpa_printf(MSG_INFO, "Station not found for inject "
 724                                    "command");
 725                         ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
 726                         return;
 727                 }
 728         }
 729
 730         switch (frame) {
 731         case WLANTEST_FRAME_AUTH:
 732                 ret = ctrl_inject_auth(wt, bss, sta, sender_ap, prot);
 733                 break;
 734         case WLANTEST_FRAME_ASSOCREQ:
 735                 ret = ctrl_inject_assocreq(wt, bss, sta, sender_ap, prot);
 736                 break;
 737         case WLANTEST_FRAME_REASSOCREQ:
 738                 ret = ctrl_inject_reassocreq(wt, bss, sta, sender_ap, prot);
 739                 break;
 740         case WLANTEST_FRAME_DEAUTH:
 741                 ret = ctrl_inject_deauth(wt, bss, sta, sender_ap, prot);
 742                 break;
 743         case WLANTEST_FRAME_DISASSOC:
 744                 ret = ctrl_inject_disassoc(wt, bss, sta, sender_ap, prot);
 745                 break;
 746         case WLANTEST_FRAME_SAQUERYREQ:
 747                 ret = ctrl_inject_saqueryreq(wt, bss, sta, sender_ap, prot);
 748                 break;
 749         default:
 750                 wpa_printf(MSG_INFO, "Unsupported inject command frame %d",
 751                            frame);
 752                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
 753                 return;
 754         }
 755
 756         if (ret)
 757                 wpa_printf(MSG_INFO, "Failed to inject frame");
 758         else
 759                 wpa_printf(MSG_INFO, "Frame injected successfully");
 760         ctrl_send_simple(wt, sock, ret == 0 ? WLANTEST_CTRL_SUCCESS :
 761                          WLANTEST_CTRL_FAILURE);
 762 }
 763
 764
 765 static void ctrl_version(struct wlantest *wt, int sock)
 766 {
 767         u8 buf[WLANTEST_CTRL_MAX_RESP_LEN], *pos;
 768
 769         pos = buf;
 770         WPA_PUT_BE32(pos, WLANTEST_CTRL_SUCCESS);
 771         pos += 4;
 772         pos = attr_add_str(pos, buf + sizeof(buf), WLANTEST_ATTR_VERSION,
 773                            VERSION_STR);
 774         ctrl_send(wt, sock, buf, pos - buf);
 775 }
 776
 777
 778 static void ctrl_add_passphrase(struct wlantest *wt, int sock, u8 *cmd,
 779                                 size_t clen)
 780 {
 781         u8 *passphrase;
 782         size_t len;
 783         struct wlantest_passphrase *p, *pa;
 784         u8 *bssid;
 785
 786         passphrase = attr_get(cmd, clen, WLANTEST_ATTR_PASSPHRASE, &len);
 787         if (passphrase == NULL) {
 788                 u8 *wepkey;
 789                 char *key;
 790                 enum wlantest_ctrl_cmd res;
 791
 792                 wepkey = attr_get(cmd, clen, WLANTEST_ATTR_WEPKEY, &len);
 793                 if (wepkey == NULL) {
 794                         ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
 795                         return;
 796                 }
 797                 key = os_zalloc(len + 1);
 798                 if (key == NULL) {
 799                         ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
 800                         return;
 801                 }
 802                 os_memcpy(key, wepkey, len);
 803                 if (add_wep(wt, key) < 0)
 804                         res = WLANTEST_CTRL_FAILURE;
 805                 else
 806                         res = WLANTEST_CTRL_SUCCESS;
 807                 os_free(key);
 808                 ctrl_send_simple(wt, sock, res);
 809                 return;
 810         }
 811
 812         if (len < 8 || len > 63) {
 813                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
 814                 return;
 815         }
 816
 817         p = os_zalloc(sizeof(*p));
 818         if (p == NULL) {
 819                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
 820                 return;
 821         }
 822         os_memcpy(p->passphrase, passphrase, len);
 823         wpa_printf(MSG_INFO, "Add passphrase '%s'", p->passphrase);
 824
 825         bssid = attr_get_macaddr(cmd, clen, WLANTEST_ATTR_BSSID);
 826         if (bssid) {
 827                 os_memcpy(p->bssid, bssid, ETH_ALEN);
 828                 wpa_printf(MSG_INFO, "Limit passphrase for BSSID " MACSTR,
 829                            MAC2STR(p->bssid));
 830         }
 831
 832         dl_list_for_each(pa, &wt->passphrase, struct wlantest_passphrase, list)
 833         {
 834                 if (os_strcmp(p->passphrase, pa->passphrase) == 0 &&
 835                     os_memcmp(p->bssid, pa->bssid, ETH_ALEN) == 0) {
 836                         wpa_printf(MSG_INFO, "Passphrase was already known");
 837                         os_free(p);
 838                         p = NULL;
 839                         break;
 840                 }
 841         }
 842
 843         if (p) {
 844                 struct wlantest_bss *bss;
 845                 dl_list_add(&wt->passphrase, &p->list);
 846                 dl_list_for_each(bss, &wt->bss, struct wlantest_bss, list) {
 847                         if (bssid &&
 848                             os_memcmp(p->bssid, bss->bssid, ETH_ALEN) != 0)
 849                                 continue;
 850                         bss_add_pmk_from_passphrase(bss, p->passphrase);
 851                 }
 852         }
 853
 854         ctrl_send_simple(wt, sock, WLANTEST_CTRL_SUCCESS);
 855 }
 856
 857
 858 static void info_print_proto(char *buf, size_t len, int proto)
 859 {
 860         char *pos, *end;
 861
 862         if (proto == 0) {
 863                 os_snprintf(buf, len, "OPEN");
 864                 return;
 865         }
 866
 867         pos = buf;
 868         end = buf + len;
 869
 870         if (proto & WPA_PROTO_WPA)
 871                 pos += os_snprintf(pos, end - pos, "%sWPA",
 872                                    pos == buf ? "" : " ");
 873         if (proto & WPA_PROTO_RSN)
 874                 pos += os_snprintf(pos, end - pos, "%sWPA2",
 875                                    pos == buf ? "" : " ");
 876 }
 877
 878
 879 static void info_print_cipher(char *buf, size_t len, int cipher)
 880 {
 881         char *pos, *end;
 882
 883         if (cipher == 0) {
 884                 os_snprintf(buf, len, "N/A");
 885                 return;
 886         }
 887
 888         pos = buf;
 889         end = buf + len;
 890
 891         if (cipher & WPA_CIPHER_NONE)
 892                 pos += os_snprintf(pos, end - pos, "%sNONE",
 893                                    pos == buf ? "" : " ");
 894         if (cipher & WPA_CIPHER_WEP40)
 895                 pos += os_snprintf(pos, end - pos, "%sWEP40",
 896                                    pos == buf ? "" : " ");
 897         if (cipher & WPA_CIPHER_WEP104)
 898                 pos += os_snprintf(pos, end - pos, "%sWEP104",
 899                                    pos == buf ? "" : " ");
 900         if (cipher & WPA_CIPHER_TKIP)
 901                 pos += os_snprintf(pos, end - pos, "%sTKIP",
 902                                    pos == buf ? "" : " ");
 903         if (cipher & WPA_CIPHER_CCMP)
 904                 pos += os_snprintf(pos, end - pos, "%sCCMP",
 905                                    pos == buf ? "" : " ");
 906         if (cipher & WPA_CIPHER_AES_128_CMAC)
 907                 pos += os_snprintf(pos, end - pos, "%sBIP",
 908                                    pos == buf ? "" : " ");
 909         if (cipher & WPA_CIPHER_BIP_GMAC_128)
 910                 pos += os_snprintf(pos, end - pos, "%sBIP-GMAC-128",
 911                                    pos == buf ? "" : " ");
 912         if (cipher & WPA_CIPHER_BIP_GMAC_256)
 913                 pos += os_snprintf(pos, end - pos, "%sBIP-GMAC-256",
 914                                    pos == buf ? "" : " ");
 915         if (cipher & WPA_CIPHER_BIP_CMAC_256)
 916                 pos += os_snprintf(pos, end - pos, "%sBIP-CMAC-256",
 917                                    pos == buf ? "" : " ");
 918 }
 919
 920
 921 static void info_print_key_mgmt(char *buf, size_t len, int key_mgmt)
 922 {
 923         char *pos, *end;
 924
 925         if (key_mgmt == 0) {
 926                 os_snprintf(buf, len, "N/A");
 927                 return;
 928         }
 929
 930         pos = buf;
 931         end = buf + len;
 932
 933         if (key_mgmt & WPA_KEY_MGMT_IEEE8021X)
 934                 pos += os_snprintf(pos, end - pos, "%sEAP",
 935                                    pos == buf ? "" : " ");
 936         if (key_mgmt & WPA_KEY_MGMT_PSK)
 937                 pos += os_snprintf(pos, end - pos, "%sPSK",
 938                                    pos == buf ? "" : " ");
 939         if (key_mgmt & WPA_KEY_MGMT_WPA_NONE)
 940                 pos += os_snprintf(pos, end - pos, "%sWPA-NONE",
 941                                    pos == buf ? "" : " ");
 942         if (key_mgmt & WPA_KEY_MGMT_FT_IEEE8021X)
 943                 pos += os_snprintf(pos, end - pos, "%sFT-EAP",
 944                                    pos == buf ? "" : " ");
 945         if (key_mgmt & WPA_KEY_MGMT_FT_PSK)
 946                 pos += os_snprintf(pos, end - pos, "%sFT-PSK",
 947                                    pos == buf ? "" : " ");
 948         if (key_mgmt & WPA_KEY_MGMT_IEEE8021X_SHA256)
 949                 pos += os_snprintf(pos, end - pos, "%sEAP-SHA256",
 950                                    pos == buf ? "" : " ");
 951         if (key_mgmt & WPA_KEY_MGMT_PSK_SHA256)
 952                 pos += os_snprintf(pos, end - pos, "%sPSK-SHA256",
 953                                    pos == buf ? "" : " ");
 954         if (key_mgmt & WPA_KEY_MGMT_IEEE8021X_SUITE_B)
 955                 pos += os_snprintf(pos, end - pos, "%sEAP-SUITE-B",
 956                                    pos == buf ? "" : " ");
 957         if (key_mgmt & WPA_KEY_MGMT_IEEE8021X_SUITE_B_192)
 958                 pos += os_snprintf(pos, end - pos, "%sEAP-SUITE-B-192",
 959                                    pos == buf ? "" : " ");
 960 }
 961
 962
 963 static void info_print_rsn_capab(char *buf, size_t len, int capab)
 964 {
 965         char *pos, *end;
 966
 967         pos = buf;
 968         end = buf + len;
 969
 970         if (capab & WPA_CAPABILITY_PREAUTH)
 971                 pos += os_snprintf(pos, end - pos, "%sPREAUTH",
 972                                    pos == buf ? "" : " ");
 973         if (capab & WPA_CAPABILITY_NO_PAIRWISE)
 974                 pos += os_snprintf(pos, end - pos, "%sNO_PAIRWISE",
 975                                    pos == buf ? "" : " ");
 976         if (capab & WPA_CAPABILITY_MFPR)
 977                 pos += os_snprintf(pos, end - pos, "%sMFPR",
 978                                    pos == buf ? "" : " ");
 979         if (capab & WPA_CAPABILITY_MFPC)
 980                 pos += os_snprintf(pos, end - pos, "%sMFPC",
 981                                    pos == buf ? "" : " ");
 982         if (capab & WPA_CAPABILITY_PEERKEY_ENABLED)
 983                 pos += os_snprintf(pos, end - pos, "%sPEERKEY",
 984                                    pos == buf ? "" : " ");
 985 }
 986
 987
 988 static void info_print_state(char *buf, size_t len, int state)
 989 {
 990         switch (state) {
 991         case STATE1:
 992                 os_strlcpy(buf, "NOT-AUTH", len);
 993                 break;
 994         case STATE2:
 995                 os_strlcpy(buf, "AUTH", len);
 996                 break;
 997         case STATE3:
 998                 os_strlcpy(buf, "AUTH+ASSOC", len);
 999                 break;
1000         }
1001 }
1002
1003
1004 static void info_print_gtk(char *buf, size_t len, struct wlantest_sta *sta)
1005 {
1006         size_t pos;
1007
1008         pos = os_snprintf(buf, len, "IDX=%d,GTK=", sta->gtk_idx);
1009         wpa_snprintf_hex(buf + pos, len - pos, sta->gtk, sta->gtk_len);
1010 }
1011
1012
1013 static void ctrl_info_sta(struct wlantest *wt, int sock, u8 *cmd, size_t clen)
1014 {
1015         u8 *addr;
1016         size_t addr_len;
1017         struct wlantest_bss *bss;
1018         struct wlantest_sta *sta;
1019         enum wlantest_sta_info info;
1020         u8 buf[4 + 108], *end, *pos;
1021         char resp[100];
1022
1023         bss = ctrl_get_bss(wt, sock, cmd, clen);
1024         sta = ctrl_get_sta(wt, sock, cmd, clen, bss);
1025         if (sta == NULL)
1026                 return;
1027
1028         addr = attr_get(cmd, clen, WLANTEST_ATTR_STA_INFO, &addr_len);
1029         if (addr == NULL || addr_len != 4) {
1030                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
1031                 return;
1032         }
1033         info = WPA_GET_BE32(addr);
1034
1035         resp[0] = '\0';
1036         switch (info) {
1037         case WLANTEST_STA_INFO_PROTO:
1038                 info_print_proto(resp, sizeof(resp), sta->proto);
1039                 break;
1040         case WLANTEST_STA_INFO_PAIRWISE:
1041                 info_print_cipher(resp, sizeof(resp), sta->pairwise_cipher);
1042                 break;
1043         case WLANTEST_STA_INFO_KEY_MGMT:
1044                 info_print_key_mgmt(resp, sizeof(resp), sta->key_mgmt);
1045                 break;
1046         case WLANTEST_STA_INFO_RSN_CAPAB:
1047                 info_print_rsn_capab(resp, sizeof(resp), sta->rsn_capab);
1048                 break;
1049         case WLANTEST_STA_INFO_STATE:
1050                 info_print_state(resp, sizeof(resp), sta->state);
1051                 break;
1052         case WLANTEST_STA_INFO_GTK:
1053                 info_print_gtk(resp, sizeof(resp), sta);
1054                 break;
1055         default:
1056                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
1057                 return;
1058         }
1059
1060         pos = buf;
1061         end = buf + sizeof(buf);
1062         WPA_PUT_BE32(pos, WLANTEST_CTRL_SUCCESS);
1063         pos += 4;
1064         pos = attr_add_str(pos, end, WLANTEST_ATTR_INFO, resp);
1065         ctrl_send(wt, sock, buf, pos - buf);
1066 }
1067
1068
1069 static void ctrl_info_bss(struct wlantest *wt, int sock, u8 *cmd, size_t clen)
1070 {
1071         u8 *addr;
1072         size_t addr_len;
1073         struct wlantest_bss *bss;
1074         enum wlantest_bss_info info;
1075         u8 buf[4 + 108], *end, *pos;
1076         char resp[100];
1077
1078         bss = ctrl_get_bss(wt, sock, cmd, clen);
1079         if (bss == NULL)
1080                 return;
1081
1082         addr = attr_get(cmd, clen, WLANTEST_ATTR_BSS_INFO, &addr_len);
1083         if (addr == NULL || addr_len != 4) {
1084                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
1085                 return;
1086         }
1087         info = WPA_GET_BE32(addr);
1088
1089         resp[0] = '\0';
1090         switch (info) {
1091         case WLANTEST_BSS_INFO_PROTO:
1092                 info_print_proto(resp, sizeof(resp), bss->proto);
1093                 break;
1094         case WLANTEST_BSS_INFO_PAIRWISE:
1095                 info_print_cipher(resp, sizeof(resp), bss->pairwise_cipher);
1096                 break;
1097         case WLANTEST_BSS_INFO_GROUP:
1098                 info_print_cipher(resp, sizeof(resp), bss->group_cipher);
1099                 break;
1100         case WLANTEST_BSS_INFO_GROUP_MGMT:
1101                 info_print_cipher(resp, sizeof(resp), bss->mgmt_group_cipher);
1102                 break;
1103         case WLANTEST_BSS_INFO_KEY_MGMT:
1104                 info_print_key_mgmt(resp, sizeof(resp), bss->key_mgmt);
1105                 break;
1106         case WLANTEST_BSS_INFO_RSN_CAPAB:
1107                 info_print_rsn_capab(resp, sizeof(resp), bss->rsn_capab);
1108                 break;
1109         default:
1110                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
1111                 return;
1112         }
1113
1114         pos = buf;
1115         end = buf + sizeof(buf);
1116         WPA_PUT_BE32(pos, WLANTEST_CTRL_SUCCESS);
1117         pos += 4;
1118         pos = attr_add_str(pos, end, WLANTEST_ATTR_INFO, resp);
1119         ctrl_send(wt, sock, buf, pos - buf);
1120 }
1121
1122
1123 static void ctrl_send_(struct wlantest *wt, int sock, u8 *cmd, size_t clen)
1124 {
1125         struct wlantest_bss *bss;
1126         struct wlantest_sta *sta;
1127         u8 *bssid, *sta_addr;
1128         int prot;
1129         u8 *frame;
1130         size_t frame_len;
1131         int ret = 0;
1132         struct ieee80211_hdr *hdr;
1133         u16 fc;
1134
1135         frame = attr_get(cmd, clen, WLANTEST_ATTR_FRAME, &frame_len);
1136         prot = attr_get_int(cmd, clen, WLANTEST_ATTR_INJECT_PROTECTION);
1137         if (frame == NULL || frame_len < 24 || prot < 0) {
1138                 wpa_printf(MSG_INFO, "Invalid send command parameters");
1139                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
1140                 return;
1141         }
1142
1143         hdr = (struct ieee80211_hdr *) frame;
1144         fc = le_to_host16(hdr->frame_control);
1145         switch (WLAN_FC_GET_TYPE(fc)) {
1146         case WLAN_FC_TYPE_MGMT:
1147                 bssid = hdr->addr3;
1148                 if (os_memcmp(hdr->addr2, hdr->addr3, ETH_ALEN) == 0)
1149                         sta_addr = hdr->addr1;
1150                 else
1151                         sta_addr = hdr->addr2;
1152                 break;
1153         case WLAN_FC_TYPE_DATA:
1154                 switch (fc & (WLAN_FC_TODS | WLAN_FC_FROMDS)) {
1155                 case 0:
1156                         bssid = hdr->addr3;
1157                         sta_addr = hdr->addr2;
1158                         break;
1159                 case WLAN_FC_TODS:
1160                         bssid = hdr->addr1;
1161                         sta_addr = hdr->addr2;
1162                         break;
1163                 case WLAN_FC_FROMDS:
1164                         bssid = hdr->addr2;
1165                         sta_addr = hdr->addr1;
1166                         break;
1167                 default:
1168                         wpa_printf(MSG_INFO, "Unsupported inject frame");
1169                         ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
1170                         return;
1171                 }
1172                 break;
1173         default:
1174                 wpa_printf(MSG_INFO, "Unsupported inject frame");
1175                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
1176                 return;
1177         }
1178
1179         bss = bss_find(wt, bssid);
1180         if (bss == NULL && prot != WLANTEST_INJECT_UNPROTECTED) {
1181                 wpa_printf(MSG_INFO, "Unknown BSSID");
1182                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
1183                 return;
1184         }
1185
1186         if (bss)
1187                 sta = sta_find(bss, sta_addr);
1188         else
1189                 sta = NULL;
1190         if (sta == NULL && prot != WLANTEST_INJECT_UNPROTECTED) {
1191                 wpa_printf(MSG_INFO, "Unknown STA address");
1192                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
1193                 return;
1194         }
1195
1196         ret = wlantest_inject(wt, bss, sta, frame, frame_len, prot);
1197
1198         if (ret)
1199                 wpa_printf(MSG_INFO, "Failed to inject frame");
1200         else
1201                 wpa_printf(MSG_INFO, "Frame injected successfully");
1202         ctrl_send_simple(wt, sock, ret == 0 ? WLANTEST_CTRL_SUCCESS :
1203                          WLANTEST_CTRL_FAILURE);
1204 }
1205
1206
1207 static void ctrl_relog(struct wlantest *wt, int sock)
1208 {
1209         int res = wlantest_relog(wt);
1210         ctrl_send_simple(wt, sock, res ? WLANTEST_CTRL_FAILURE :
1211                          WLANTEST_CTRL_SUCCESS);
1212 }
1213
1214
1215 static void ctrl_get_tx_tid(struct wlantest *wt, int sock, u8 *cmd, size_t clen)
1216 {
1217         u8 *addr;
1218         size_t addr_len;
1219         struct wlantest_bss *bss;
1220         struct wlantest_sta *sta;
1221         u32 counter;
1222         u8 buf[4 + 12], *end, *pos;
1223
1224         bss = ctrl_get_bss(wt, sock, cmd, clen);
1225         sta = ctrl_get_sta(wt, sock, cmd, clen, bss);
1226         if (sta == NULL)
1227                 return;
1228
1229         addr = attr_get(cmd, clen, WLANTEST_ATTR_TID, &addr_len);
1230         if (addr == NULL || addr_len != 4) {
1231                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
1232                 return;
1233         }
1234         counter = WPA_GET_BE32(addr);
1235         if (counter >= 16 + 1) {
1236                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
1237                 return;
1238         }
1239
1240         pos = buf;
1241         end = buf + sizeof(buf);
1242         WPA_PUT_BE32(pos, WLANTEST_CTRL_SUCCESS);
1243         pos += 4;
1244         pos = attr_add_be32(pos, end, WLANTEST_ATTR_COUNTER,
1245                             sta->tx_tid[counter]);
1246         ctrl_send(wt, sock, buf, pos - buf);
1247 }
1248
1249
1250 static void ctrl_get_rx_tid(struct wlantest *wt, int sock, u8 *cmd, size_t clen)
1251 {
1252         u8 *addr;
1253         size_t addr_len;
1254         struct wlantest_bss *bss;
1255         struct wlantest_sta *sta;
1256         u32 counter;
1257         u8 buf[4 + 12], *end, *pos;
1258
1259         bss = ctrl_get_bss(wt, sock, cmd, clen);
1260         sta = ctrl_get_sta(wt, sock, cmd, clen, bss);
1261         if (sta == NULL)
1262                 return;
1263
1264         addr = attr_get(cmd, clen, WLANTEST_ATTR_TID, &addr_len);
1265         if (addr == NULL || addr_len != 4) {
1266                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
1267                 return;
1268         }
1269         counter = WPA_GET_BE32(addr);
1270         if (counter >= 16 + 1) {
1271                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
1272                 return;
1273         }
1274
1275         pos = buf;
1276         end = buf + sizeof(buf);
1277         WPA_PUT_BE32(pos, WLANTEST_CTRL_SUCCESS);
1278         pos += 4;
1279         pos = attr_add_be32(pos, end, WLANTEST_ATTR_COUNTER,
1280                             sta->rx_tid[counter]);
1281         ctrl_send(wt, sock, buf, pos - buf);
1282 }
1283
1284
1285 static void ctrl_read(int sock, void *eloop_ctx, void *sock_ctx)
1286 {
1287         struct wlantest *wt = eloop_ctx;
1288         u8 buf[WLANTEST_CTRL_MAX_CMD_LEN];
1289         int len;
1290         enum wlantest_ctrl_cmd cmd;
1291
1292         wpa_printf(MSG_EXCESSIVE, "New control interface message from %d",
1293                    sock);
1294         len = recv(sock, buf, sizeof(buf), 0);
1295         if (len < 0) {
1296                 wpa_printf(MSG_INFO, "recv(ctrl): %s", strerror(errno));
1297                 ctrl_disconnect(wt, sock);
1298                 return;
1299         }
1300         if (len == 0) {
1301                 ctrl_disconnect(wt, sock);
1302                 return;
1303         }
1304
1305         if (len < 4) {
1306                 wpa_printf(MSG_INFO, "Too short control interface command "
1307                            "from %d", sock);
1308                 ctrl_disconnect(wt, sock);
1309                 return;
1310         }
1311         cmd = WPA_GET_BE32(buf);
1312         wpa_printf(MSG_EXCESSIVE, "Control interface command %d from %d",
1313                    cmd, sock);
1314
1315         switch (cmd) {
1316         case WLANTEST_CTRL_PING:
1317                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_SUCCESS);
1318                 break;
1319         case WLANTEST_CTRL_TERMINATE:
1320                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_SUCCESS);
1321                 eloop_terminate();
1322                 break;
1323         case WLANTEST_CTRL_LIST_BSS:
1324                 ctrl_list_bss(wt, sock);
1325                 break;
1326         case WLANTEST_CTRL_LIST_STA:
1327                 ctrl_list_sta(wt, sock, buf + 4, len - 4);
1328                 break;
1329         case WLANTEST_CTRL_FLUSH:
1330                 ctrl_flush(wt, sock);
1331                 break;
1332         case WLANTEST_CTRL_CLEAR_STA_COUNTERS:
1333                 ctrl_clear_sta_counters(wt, sock, buf + 4, len - 4);
1334                 break;
1335         case WLANTEST_CTRL_CLEAR_BSS_COUNTERS:
1336                 ctrl_clear_bss_counters(wt, sock, buf + 4, len - 4);
1337                 break;
1338         case WLANTEST_CTRL_CLEAR_TDLS_COUNTERS:
1339                 ctrl_clear_tdls_counters(wt, sock, buf + 4, len - 4);
1340                 break;
1341         case WLANTEST_CTRL_GET_STA_COUNTER:
1342                 ctrl_get_sta_counter(wt, sock, buf + 4, len - 4);
1343                 break;
1344         case WLANTEST_CTRL_GET_BSS_COUNTER:
1345                 ctrl_get_bss_counter(wt, sock, buf + 4, len - 4);
1346                 break;
1347         case WLANTEST_CTRL_GET_TDLS_COUNTER:
1348                 ctrl_get_tdls_counter(wt, sock, buf + 4, len - 4);
1349                 break;
1350         case WLANTEST_CTRL_INJECT:
1351                 ctrl_inject(wt, sock, buf + 4, len - 4);
1352                 break;
1353         case WLANTEST_CTRL_VERSION:
1354                 ctrl_version(wt, sock);
1355                 break;
1356         case WLANTEST_CTRL_ADD_PASSPHRASE:
1357                 ctrl_add_passphrase(wt, sock, buf + 4, len - 4);
1358                 break;
1359         case WLANTEST_CTRL_INFO_STA:
1360                 ctrl_info_sta(wt, sock, buf + 4, len - 4);
1361                 break;
1362         case WLANTEST_CTRL_INFO_BSS:
1363                 ctrl_info_bss(wt, sock, buf + 4, len - 4);
1364                 break;
1365         case WLANTEST_CTRL_SEND:
1366                 ctrl_send_(wt, sock, buf + 4, len - 4);
1367                 break;
1368         case WLANTEST_CTRL_RELOG:
1369                 ctrl_relog(wt, sock);
1370                 break;
1371         case WLANTEST_CTRL_GET_TX_TID:
1372                 ctrl_get_tx_tid(wt, sock, buf + 4, len - 4);
1373                 break;
1374         case WLANTEST_CTRL_GET_RX_TID:
1375                 ctrl_get_rx_tid(wt, sock, buf + 4, len - 4);
1376                 break;
1377         default:
1378                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_UNKNOWN_CMD);
1379                 break;
1380         }
1381 }
1382
1383
1384 static void ctrl_connect(int sock, void *eloop_ctx, void *sock_ctx)
1385 {
1386         struct wlantest *wt = eloop_ctx;
1387         int conn, i;
1388
1389         conn = accept(sock, NULL, NULL);
1390         if (conn < 0) {
1391                 wpa_printf(MSG_INFO, "accept(ctrl): %s", strerror(errno));
1392                 return;
1393         }
1394         wpa_printf(MSG_MSGDUMP, "New control interface connection %d", conn);
1395
1396         for (i = 0; i < MAX_CTRL_CONNECTIONS; i++) {
1397                 if (wt->ctrl_socks[i] < 0)
1398                         break;
1399         }
1400
1401         if (i == MAX_CTRL_CONNECTIONS) {
1402                 wpa_printf(MSG_INFO, "No room for new control connection");
1403                 close(conn);
1404                 return;
1405         }
1406
1407         wt->ctrl_socks[i] = conn;
1408         eloop_register_read_sock(conn, ctrl_read, wt, NULL);
1409 }
1410
1411
1412 int ctrl_init(struct wlantest *wt)
1413 {
1414         struct sockaddr_un addr;
1415
1416         wt->ctrl_sock = socket(AF_UNIX, SOCK_SEQPACKET, 0);
1417         if (wt->ctrl_sock < 0) {
1418                 wpa_printf(MSG_ERROR, "socket: %s", strerror(errno));
1419                 return -1;
1420         }
1421
1422         os_memset(&addr, 0, sizeof(addr));
1423         addr.sun_family = AF_UNIX;
1424         os_strlcpy(addr.sun_path + 1, WLANTEST_SOCK_NAME,
1425                    sizeof(addr.sun_path) - 1);
1426         if (bind(wt->ctrl_sock, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
1427                 wpa_printf(MSG_ERROR, "bind: %s", strerror(errno));
1428                 close(wt->ctrl_sock);
1429                 wt->ctrl_sock = -1;
1430                 return -1;
1431         }
1432
1433         if (listen(wt->ctrl_sock, 5) < 0) {
1434                 wpa_printf(MSG_ERROR, "listen: %s", strerror(errno));
1435                 close(wt->ctrl_sock);
1436                 wt->ctrl_sock = -1;
1437                 return -1;
1438         }
1439
1440         if (eloop_register_read_sock(wt->ctrl_sock, ctrl_connect, wt, NULL)) {
1441                 close(wt->ctrl_sock);
1442                 wt->ctrl_sock = -1;
1443                 return -1;
1444         }
1445
1446         return 0;
1447 }
1448
1449
1450 void ctrl_deinit(struct wlantest *wt)
1451 {
1452         int i;
1453
1454         if (wt->ctrl_sock < 0)
1455                 return;
1456
1457         for (i = 0; i < MAX_CTRL_CONNECTIONS; i++) {
1458                 if (wt->ctrl_socks[i] >= 0) {
1459                         close(wt->ctrl_socks[i]);
1460                         eloop_unregister_read_sock(wt->ctrl_socks[i]);
1461                         wt->ctrl_socks[i] = -1;
1462                 }
1463         }
1464
1465         eloop_unregister_read_sock(wt->ctrl_sock);
1466         close(wt->ctrl_sock);
1467         wt->ctrl_sock = -1;
1468 }