wlantest/ctrl.c

   1 /*
   2  * wlantest control interface
   3  * Copyright (c) 2010-2013, 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         ctrl_send_simple(wt, sock, WLANTEST_CTRL_SUCCESS);
 294 }
 295
 296
 297 static void ctrl_clear_bss_counters(struct wlantest *wt, int sock, u8 *cmd,
 298                                     size_t clen)
 299 {
 300         struct wlantest_bss *bss;
 301
 302         bss = ctrl_get_bss(wt, sock, cmd, clen);
 303         if (bss == NULL) {
 304                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
 305                 return;
 306         }
 307
 308         os_memset(bss->counters, 0, sizeof(bss->counters));
 309         ctrl_send_simple(wt, sock, WLANTEST_CTRL_SUCCESS);
 310 }
 311
 312
 313 static void ctrl_clear_tdls_counters(struct wlantest *wt, int sock, u8 *cmd,
 314                                      size_t clen)
 315 {
 316         struct wlantest_bss *bss;
 317         struct wlantest_sta *sta;
 318         struct wlantest_sta *sta2;
 319         struct wlantest_tdls *tdls;
 320
 321         bss = ctrl_get_bss(wt, sock, cmd, clen);
 322         sta = ctrl_get_sta(wt, sock, cmd, clen, bss);
 323         sta2 = ctrl_get_sta2(wt, sock, cmd, clen, bss);
 324         if (sta == NULL || sta2 == NULL) {
 325                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
 326                 return;
 327         }
 328
 329         dl_list_for_each(tdls, &bss->tdls, struct wlantest_tdls, list) {
 330                 if ((tdls->init == sta && tdls->resp == sta2) ||
 331                     (tdls->init == sta2 && tdls->resp == sta))
 332                         os_memset(tdls->counters, 0, sizeof(tdls->counters));
 333         }
 334         ctrl_send_simple(wt, sock, WLANTEST_CTRL_SUCCESS);
 335 }
 336
 337
 338 static void ctrl_get_sta_counter(struct wlantest *wt, int sock, u8 *cmd,
 339                                  size_t clen)
 340 {
 341         u8 *addr;
 342         size_t addr_len;
 343         struct wlantest_bss *bss;
 344         struct wlantest_sta *sta;
 345         u32 counter;
 346         u8 buf[4 + 12], *end, *pos;
 347
 348         bss = ctrl_get_bss(wt, sock, cmd, clen);
 349         sta = ctrl_get_sta(wt, sock, cmd, clen, bss);
 350         if (sta == NULL)
 351                 return;
 352
 353         addr = attr_get(cmd, clen, WLANTEST_ATTR_STA_COUNTER, &addr_len);
 354         if (addr == NULL || addr_len != 4) {
 355                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
 356                 return;
 357         }
 358         counter = WPA_GET_BE32(addr);
 359         if (counter >= NUM_WLANTEST_STA_COUNTER) {
 360                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
 361                 return;
 362         }
 363
 364         pos = buf;
 365         end = buf + sizeof(buf);
 366         WPA_PUT_BE32(pos, WLANTEST_CTRL_SUCCESS);
 367         pos += 4;
 368         pos = attr_add_be32(pos, end, WLANTEST_ATTR_COUNTER,
 369                             sta->counters[counter]);
 370         ctrl_send(wt, sock, buf, pos - buf);
 371 }
 372
 373
 374 static void ctrl_get_bss_counter(struct wlantest *wt, int sock, u8 *cmd,
 375                                  size_t clen)
 376 {
 377         u8 *addr;
 378         size_t addr_len;
 379         struct wlantest_bss *bss;
 380         u32 counter;
 381         u8 buf[4 + 12], *end, *pos;
 382
 383         bss = ctrl_get_bss(wt, sock, cmd, clen);
 384         if (bss == NULL)
 385                 return;
 386
 387         addr = attr_get(cmd, clen, WLANTEST_ATTR_BSS_COUNTER, &addr_len);
 388         if (addr == NULL || addr_len != 4) {
 389                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
 390                 return;
 391         }
 392         counter = WPA_GET_BE32(addr);
 393         if (counter >= NUM_WLANTEST_BSS_COUNTER) {
 394                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
 395                 return;
 396         }
 397
 398         pos = buf;
 399         end = buf + sizeof(buf);
 400         WPA_PUT_BE32(pos, WLANTEST_CTRL_SUCCESS);
 401         pos += 4;
 402         pos = attr_add_be32(pos, end, WLANTEST_ATTR_COUNTER,
 403                             bss->counters[counter]);
 404         ctrl_send(wt, sock, buf, pos - buf);
 405 }
 406
 407
 408 static void ctrl_get_tdls_counter(struct wlantest *wt, int sock, u8 *cmd,
 409                                   size_t clen)
 410 {
 411         u8 *addr;
 412         size_t addr_len;
 413         struct wlantest_bss *bss;
 414         struct wlantest_sta *sta;
 415         struct wlantest_sta *sta2;
 416         struct wlantest_tdls *tdls;
 417         u32 counter;
 418         u8 buf[4 + 12], *end, *pos;
 419         int found = 0;
 420
 421         bss = ctrl_get_bss(wt, sock, cmd, clen);
 422         sta = ctrl_get_sta(wt, sock, cmd, clen, bss);
 423         sta2 = ctrl_get_sta2(wt, sock, cmd, clen, bss);
 424         if (sta == NULL || sta2 == NULL) {
 425                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
 426                 return;
 427         }
 428
 429         addr = attr_get(cmd, clen, WLANTEST_ATTR_TDLS_COUNTER, &addr_len);
 430         if (addr == NULL || addr_len != 4) {
 431                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
 432                 return;
 433         }
 434         counter = WPA_GET_BE32(addr);
 435         if (counter >= NUM_WLANTEST_TDLS_COUNTER) {
 436                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
 437                 return;
 438         }
 439
 440         dl_list_for_each(tdls, &bss->tdls, struct wlantest_tdls, list) {
 441                 if (tdls->init == sta && tdls->resp == sta2) {
 442                         found = 1;
 443                         break;
 444                 }
 445         }
 446
 447         if (!found) {
 448                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
 449                 return;
 450         }
 451
 452         pos = buf;
 453         end = buf + sizeof(buf);
 454         WPA_PUT_BE32(pos, WLANTEST_CTRL_SUCCESS);
 455         pos += 4;
 456         pos = attr_add_be32(pos, end, WLANTEST_ATTR_COUNTER,
 457                             tdls->counters[counter]);
 458         ctrl_send(wt, sock, buf, pos - buf);
 459 }
 460
 461
 462 static void build_mgmt_hdr(struct ieee80211_mgmt *mgmt,
 463                            struct wlantest_bss *bss, struct wlantest_sta *sta,
 464                            int sender_ap, int stype)
 465 {
 466         os_memset(mgmt, 0, 24);
 467         mgmt->frame_control = IEEE80211_FC(WLAN_FC_TYPE_MGMT, stype);
 468         if (sender_ap) {
 469                 if (sta)
 470                         os_memcpy(mgmt->da, sta->addr, ETH_ALEN);
 471                 else
 472                         os_memset(mgmt->da, 0xff, ETH_ALEN);
 473                 os_memcpy(mgmt->sa, bss->bssid, ETH_ALEN);
 474         } else {
 475                 os_memcpy(mgmt->da, bss->bssid, ETH_ALEN);
 476                 os_memcpy(mgmt->sa, sta->addr, ETH_ALEN);
 477         }
 478         os_memcpy(mgmt->bssid, bss->bssid, ETH_ALEN);
 479 }
 480
 481
 482 static int ctrl_inject_auth(struct wlantest *wt, struct wlantest_bss *bss,
 483                             struct wlantest_sta *sta, int sender_ap,
 484                             enum wlantest_inject_protection prot)
 485 {
 486         struct ieee80211_mgmt mgmt;
 487
 488         if (prot != WLANTEST_INJECT_NORMAL &&
 489             prot != WLANTEST_INJECT_UNPROTECTED)
 490                 return -1; /* Authentication frame is never protected */
 491         if (sta == NULL)
 492                 return -1; /* No broadcast Authentication frames */
 493
 494         if (sender_ap)
 495                 wpa_printf(MSG_INFO, "INJECT: Auth " MACSTR " -> " MACSTR,
 496                            MAC2STR(bss->bssid), MAC2STR(sta->addr));
 497         else
 498                 wpa_printf(MSG_INFO, "INJECT: Auth " MACSTR " -> " MACSTR,
 499                            MAC2STR(sta->addr), MAC2STR(bss->bssid));
 500         build_mgmt_hdr(&mgmt, bss, sta, sender_ap, WLAN_FC_STYPE_AUTH);
 501
 502         mgmt.u.auth.auth_alg = host_to_le16(WLAN_AUTH_OPEN);
 503         mgmt.u.auth.auth_transaction = host_to_le16(1);
 504         mgmt.u.auth.status_code = host_to_le16(WLAN_STATUS_SUCCESS);
 505
 506         return wlantest_inject(wt, bss, sta, (u8 *) &mgmt, 24 + 6,
 507                                WLANTEST_INJECT_UNPROTECTED);
 508 }
 509
 510
 511 static int ctrl_inject_assocreq(struct wlantest *wt, struct wlantest_bss *bss,
 512                                 struct wlantest_sta *sta, int sender_ap,
 513                                 enum wlantest_inject_protection prot)
 514 {
 515         u8 *buf;
 516         struct ieee80211_mgmt *mgmt;
 517         int ret;
 518
 519         if (prot != WLANTEST_INJECT_NORMAL &&
 520             prot != WLANTEST_INJECT_UNPROTECTED)
 521                 return -1; /* Association Request frame is never protected */
 522         if (sta == NULL)
 523                 return -1; /* No broadcast Association Request frames */
 524         if (sender_ap)
 525                 return -1; /* No Association Request frame sent by AP */
 526         if (sta->assocreq_ies == NULL) {
 527                 wpa_printf(MSG_INFO, "INJECT: No previous (Re)Association "
 528                            "Request available for " MACSTR,
 529                            MAC2STR(sta->addr));
 530                 return -1;
 531         }
 532
 533         wpa_printf(MSG_INFO, "INJECT: AssocReq " MACSTR " -> " MACSTR,
 534                    MAC2STR(sta->addr), MAC2STR(bss->bssid));
 535         buf = os_malloc(sizeof(*mgmt) + sta->assocreq_ies_len);
 536         if (buf == NULL)
 537                 return -1;
 538         mgmt = (struct ieee80211_mgmt *) buf;
 539
 540         build_mgmt_hdr(mgmt, bss, sta, sender_ap, WLAN_FC_STYPE_ASSOC_REQ);
 541
 542         mgmt->u.assoc_req.capab_info = host_to_le16(sta->assocreq_capab_info);
 543         mgmt->u.assoc_req.listen_interval =
 544                 host_to_le16(sta->assocreq_listen_int);
 545         os_memcpy(mgmt->u.assoc_req.variable, sta->assocreq_ies,
 546                   sta->assocreq_ies_len);
 547
 548         ret = wlantest_inject(wt, bss, sta, buf,
 549                               24 + 4 + sta->assocreq_ies_len,
 550                               WLANTEST_INJECT_UNPROTECTED);
 551         os_free(buf);
 552         return ret;
 553 }
 554
 555
 556 static int ctrl_inject_reassocreq(struct wlantest *wt,
 557                                   struct wlantest_bss *bss,
 558                                   struct wlantest_sta *sta, int sender_ap,
 559                                   enum wlantest_inject_protection prot)
 560 {
 561         u8 *buf;
 562         struct ieee80211_mgmt *mgmt;
 563         int ret;
 564
 565         if (prot != WLANTEST_INJECT_NORMAL &&
 566             prot != WLANTEST_INJECT_UNPROTECTED)
 567                 return -1; /* Reassociation Request frame is never protected */
 568         if (sta == NULL)
 569                 return -1; /* No broadcast Reassociation Request frames */
 570         if (sender_ap)
 571                 return -1; /* No Reassociation Request frame sent by AP */
 572         if (sta->assocreq_ies == NULL) {
 573                 wpa_printf(MSG_INFO, "INJECT: No previous (Re)Association "
 574                            "Request available for " MACSTR,
 575                            MAC2STR(sta->addr));
 576                 return -1;
 577         }
 578
 579         wpa_printf(MSG_INFO, "INJECT: ReassocReq " MACSTR " -> " MACSTR,
 580                    MAC2STR(sta->addr), MAC2STR(bss->bssid));
 581         buf = os_malloc(sizeof(*mgmt) + sta->assocreq_ies_len);
 582         if (buf == NULL)
 583                 return -1;
 584         mgmt = (struct ieee80211_mgmt *) buf;
 585
 586         build_mgmt_hdr(mgmt, bss, sta, sender_ap, WLAN_FC_STYPE_REASSOC_REQ);
 587
 588         mgmt->u.reassoc_req.capab_info =
 589                 host_to_le16(sta->assocreq_capab_info);
 590         mgmt->u.reassoc_req.listen_interval =
 591                 host_to_le16(sta->assocreq_listen_int);
 592         os_memcpy(mgmt->u.reassoc_req.current_ap, bss->bssid, ETH_ALEN);
 593         os_memcpy(mgmt->u.reassoc_req.variable, sta->assocreq_ies,
 594                   sta->assocreq_ies_len);
 595
 596         ret = wlantest_inject(wt, bss, sta, buf,
 597                               24 + 10 + sta->assocreq_ies_len,
 598                               WLANTEST_INJECT_UNPROTECTED);
 599         os_free(buf);
 600         return ret;
 601 }
 602
 603
 604 static int ctrl_inject_deauth(struct wlantest *wt, struct wlantest_bss *bss,
 605                               struct wlantest_sta *sta, int sender_ap,
 606                               enum wlantest_inject_protection prot)
 607 {
 608         struct ieee80211_mgmt mgmt;
 609
 610         if (sender_ap) {
 611                 if (sta)
 612                         wpa_printf(MSG_INFO, "INJECT: Deauth " MACSTR " -> "
 613                                    MACSTR,
 614                                    MAC2STR(bss->bssid), MAC2STR(sta->addr));
 615                 else
 616                         wpa_printf(MSG_INFO, "INJECT: Deauth " MACSTR
 617                                    " -> broadcast", MAC2STR(bss->bssid));
 618         } else
 619                 wpa_printf(MSG_INFO, "INJECT: Deauth " MACSTR " -> " MACSTR,
 620                            MAC2STR(sta->addr), MAC2STR(bss->bssid));
 621         build_mgmt_hdr(&mgmt, bss, sta, sender_ap, WLAN_FC_STYPE_DEAUTH);
 622
 623         mgmt.u.deauth.reason_code = host_to_le16(WLAN_REASON_UNSPECIFIED);
 624
 625         return wlantest_inject(wt, bss, sta, (u8 *) &mgmt, 24 + 2, prot);
 626 }
 627
 628
 629 static int ctrl_inject_disassoc(struct wlantest *wt, struct wlantest_bss *bss,
 630                                 struct wlantest_sta *sta, int sender_ap,
 631                                 enum wlantest_inject_protection prot)
 632 {
 633         struct ieee80211_mgmt mgmt;
 634
 635         if (sender_ap) {
 636                 if (sta)
 637                         wpa_printf(MSG_INFO, "INJECT: Disassoc " MACSTR " -> "
 638                                    MACSTR,
 639                                    MAC2STR(bss->bssid), MAC2STR(sta->addr));
 640                 else
 641                         wpa_printf(MSG_INFO, "INJECT: Disassoc " MACSTR
 642                                    " -> broadcast", MAC2STR(bss->bssid));
 643         } else
 644                 wpa_printf(MSG_INFO, "INJECT: Disassoc " MACSTR " -> " MACSTR,
 645                            MAC2STR(sta->addr), MAC2STR(bss->bssid));
 646         build_mgmt_hdr(&mgmt, bss, sta, sender_ap, WLAN_FC_STYPE_DISASSOC);
 647
 648         mgmt.u.disassoc.reason_code = host_to_le16(WLAN_REASON_UNSPECIFIED);
 649
 650         return wlantest_inject(wt, bss, sta, (u8 *) &mgmt, 24 + 2, prot);
 651 }
 652
 653
 654 static int ctrl_inject_saqueryreq(struct wlantest *wt,
 655                                   struct wlantest_bss *bss,
 656                                   struct wlantest_sta *sta, int sender_ap,
 657                                   enum wlantest_inject_protection prot)
 658 {
 659         struct ieee80211_mgmt mgmt;
 660
 661         if (sta == NULL)
 662                 return -1; /* No broadcast SA Query frames */
 663
 664         if (sender_ap)
 665                 wpa_printf(MSG_INFO, "INJECT: SA Query Request " MACSTR " -> "
 666                            MACSTR, MAC2STR(bss->bssid), MAC2STR(sta->addr));
 667         else
 668                 wpa_printf(MSG_INFO, "INJECT: SA Query Request " MACSTR " -> "
 669                            MACSTR, MAC2STR(sta->addr), MAC2STR(bss->bssid));
 670         build_mgmt_hdr(&mgmt, bss, sta, sender_ap, WLAN_FC_STYPE_ACTION);
 671
 672         mgmt.u.action.category = WLAN_ACTION_SA_QUERY;
 673         mgmt.u.action.u.sa_query_req.action = WLAN_SA_QUERY_REQUEST;
 674         mgmt.u.action.u.sa_query_req.trans_id[0] = 0x12;
 675         mgmt.u.action.u.sa_query_req.trans_id[1] = 0x34;
 676         os_memcpy(sender_ap ? sta->ap_sa_query_tr : sta->sta_sa_query_tr,
 677                   mgmt.u.action.u.sa_query_req.trans_id,
 678                   WLAN_SA_QUERY_TR_ID_LEN);
 679         return wlantest_inject(wt, bss, sta, (u8 *) &mgmt, 24 + 4, prot);
 680 }
 681
 682
 683 static void ctrl_inject(struct wlantest *wt, int sock, u8 *cmd, size_t clen)
 684 {
 685         u8 *bssid, *sta_addr;
 686         struct wlantest_bss *bss;
 687         struct wlantest_sta *sta;
 688         int frame, sender_ap, prot;
 689         int ret = 0;
 690
 691         bssid = attr_get_macaddr(cmd, clen, WLANTEST_ATTR_BSSID);
 692         sta_addr = attr_get_macaddr(cmd, clen, WLANTEST_ATTR_STA_ADDR);
 693         frame = attr_get_int(cmd, clen, WLANTEST_ATTR_INJECT_FRAME);
 694         sender_ap = attr_get_int(cmd, clen, WLANTEST_ATTR_INJECT_SENDER_AP);
 695         if (sender_ap < 0)
 696                 sender_ap = 0;
 697         prot = attr_get_int(cmd, clen, WLANTEST_ATTR_INJECT_PROTECTION);
 698         if (bssid == NULL || sta_addr == NULL || frame < 0 || prot < 0) {
 699                 wpa_printf(MSG_INFO, "Invalid inject command parameters");
 700                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
 701                 return;
 702         }
 703
 704         bss = bss_find(wt, bssid);
 705         if (bss == NULL) {
 706                 wpa_printf(MSG_INFO, "BSS not found for inject command");
 707                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
 708                 return;
 709         }
 710
 711         if (is_broadcast_ether_addr(sta_addr)) {
 712                 if (!sender_ap) {
 713                         wpa_printf(MSG_INFO, "Invalid broadcast inject "
 714                                    "command without sender_ap set");
 715                         ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
 716                         return;
 717                 } sta = NULL;
 718         } else {
 719                 sta = sta_find(bss, sta_addr);
 720                 if (sta == NULL) {
 721                         wpa_printf(MSG_INFO, "Station not found for inject "
 722                                    "command");
 723                         ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
 724                         return;
 725                 }
 726         }
 727
 728         switch (frame) {
 729         case WLANTEST_FRAME_AUTH:
 730                 ret = ctrl_inject_auth(wt, bss, sta, sender_ap, prot);
 731                 break;
 732         case WLANTEST_FRAME_ASSOCREQ:
 733                 ret = ctrl_inject_assocreq(wt, bss, sta, sender_ap, prot);
 734                 break;
 735         case WLANTEST_FRAME_REASSOCREQ:
 736                 ret = ctrl_inject_reassocreq(wt, bss, sta, sender_ap, prot);
 737                 break;
 738         case WLANTEST_FRAME_DEAUTH:
 739                 ret = ctrl_inject_deauth(wt, bss, sta, sender_ap, prot);
 740                 break;
 741         case WLANTEST_FRAME_DISASSOC:
 742                 ret = ctrl_inject_disassoc(wt, bss, sta, sender_ap, prot);
 743                 break;
 744         case WLANTEST_FRAME_SAQUERYREQ:
 745                 ret = ctrl_inject_saqueryreq(wt, bss, sta, sender_ap, prot);
 746                 break;
 747         default:
 748                 wpa_printf(MSG_INFO, "Unsupported inject command frame %d",
 749                            frame);
 750                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
 751                 return;
 752         }
 753
 754         if (ret)
 755                 wpa_printf(MSG_INFO, "Failed to inject frame");
 756         else
 757                 wpa_printf(MSG_INFO, "Frame injected successfully");
 758         ctrl_send_simple(wt, sock, ret == 0 ? WLANTEST_CTRL_SUCCESS :
 759                          WLANTEST_CTRL_FAILURE);
 760 }
 761
 762
 763 static void ctrl_version(struct wlantest *wt, int sock)
 764 {
 765         u8 buf[WLANTEST_CTRL_MAX_RESP_LEN], *pos;
 766
 767         pos = buf;
 768         WPA_PUT_BE32(pos, WLANTEST_CTRL_SUCCESS);
 769         pos += 4;
 770         pos = attr_add_str(pos, buf + sizeof(buf), WLANTEST_ATTR_VERSION,
 771                            VERSION_STR);
 772         ctrl_send(wt, sock, buf, pos - buf);
 773 }
 774
 775
 776 static void ctrl_add_passphrase(struct wlantest *wt, int sock, u8 *cmd,
 777                                 size_t clen)
 778 {
 779         u8 *passphrase;
 780         size_t len;
 781         struct wlantest_passphrase *p, *pa;
 782         u8 *bssid;
 783
 784         passphrase = attr_get(cmd, clen, WLANTEST_ATTR_PASSPHRASE, &len);
 785         if (passphrase == NULL) {
 786                 u8 *wepkey;
 787                 char *key;
 788                 enum wlantest_ctrl_cmd res;
 789
 790                 wepkey = attr_get(cmd, clen, WLANTEST_ATTR_WEPKEY, &len);
 791                 if (wepkey == NULL) {
 792                         ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
 793                         return;
 794                 }
 795                 key = os_zalloc(len + 1);
 796                 if (key == NULL) {
 797                         ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
 798                         return;
 799                 }
 800                 os_memcpy(key, wepkey, len);
 801                 if (add_wep(wt, key) < 0)
 802                         res = WLANTEST_CTRL_FAILURE;
 803                 else
 804                         res = WLANTEST_CTRL_SUCCESS;
 805                 os_free(key);
 806                 ctrl_send_simple(wt, sock, res);
 807                 return;
 808         }
 809
 810         if (len < 8 || len > 63) {
 811                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
 812                 return;
 813         }
 814
 815         p = os_zalloc(sizeof(*p));
 816         if (p == NULL) {
 817                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
 818                 return;
 819         }
 820         os_memcpy(p->passphrase, passphrase, len);
 821         wpa_printf(MSG_INFO, "Add passphrase '%s'", p->passphrase);
 822
 823         bssid = attr_get_macaddr(cmd, clen, WLANTEST_ATTR_BSSID);
 824         if (bssid) {
 825                 os_memcpy(p->bssid, bssid, ETH_ALEN);
 826                 wpa_printf(MSG_INFO, "Limit passphrase for BSSID " MACSTR,
 827                            MAC2STR(p->bssid));
 828         }
 829
 830         dl_list_for_each(pa, &wt->passphrase, struct wlantest_passphrase, list)
 831         {
 832                 if (os_strcmp(p->passphrase, pa->passphrase) == 0 &&
 833                     os_memcmp(p->bssid, pa->bssid, ETH_ALEN) == 0) {
 834                         wpa_printf(MSG_INFO, "Passphrase was already known");
 835                         os_free(p);
 836                         p = NULL;
 837                         break;
 838                 }
 839         }
 840
 841         if (p) {
 842                 struct wlantest_bss *bss;
 843                 dl_list_add(&wt->passphrase, &p->list);
 844                 dl_list_for_each(bss, &wt->bss, struct wlantest_bss, list) {
 845                         if (bssid &&
 846                             os_memcmp(p->bssid, bss->bssid, ETH_ALEN) != 0)
 847                                 continue;
 848                         bss_add_pmk_from_passphrase(bss, p->passphrase);
 849                 }
 850         }
 851
 852         ctrl_send_simple(wt, sock, WLANTEST_CTRL_SUCCESS);
 853 }
 854
 855
 856 static void info_print_proto(char *buf, size_t len, int proto)
 857 {
 858         char *pos, *end;
 859
 860         if (proto == 0) {
 861                 os_snprintf(buf, len, "OPEN");
 862                 return;
 863         }
 864
 865         pos = buf;
 866         end = buf + len;
 867
 868         if (proto & WPA_PROTO_WPA)
 869                 pos += os_snprintf(pos, end - pos, "%sWPA",
 870                                    pos == buf ? "" : " ");
 871         if (proto & WPA_PROTO_RSN)
 872                 pos += os_snprintf(pos, end - pos, "%sWPA2",
 873                                    pos == buf ? "" : " ");
 874 }
 875
 876
 877 static void info_print_cipher(char *buf, size_t len, int cipher)
 878 {
 879         char *pos, *end;
 880
 881         if (cipher == 0) {
 882                 os_snprintf(buf, len, "N/A");
 883                 return;
 884         }
 885
 886         pos = buf;
 887         end = buf + len;
 888
 889         if (cipher & WPA_CIPHER_NONE)
 890                 pos += os_snprintf(pos, end - pos, "%sNONE",
 891                                    pos == buf ? "" : " ");
 892         if (cipher & WPA_CIPHER_WEP40)
 893                 pos += os_snprintf(pos, end - pos, "%sWEP40",
 894                                    pos == buf ? "" : " ");
 895         if (cipher & WPA_CIPHER_WEP104)
 896                 pos += os_snprintf(pos, end - pos, "%sWEP104",
 897                                    pos == buf ? "" : " ");
 898         if (cipher & WPA_CIPHER_TKIP)
 899                 pos += os_snprintf(pos, end - pos, "%sTKIP",
 900                                    pos == buf ? "" : " ");
 901         if (cipher & WPA_CIPHER_CCMP)
 902                 pos += os_snprintf(pos, end - pos, "%sCCMP",
 903                                    pos == buf ? "" : " ");
 904         if (cipher & WPA_CIPHER_AES_128_CMAC)
 905                 pos += os_snprintf(pos, end - pos, "%sBIP",
 906                                    pos == buf ? "" : " ");
 907 }
 908
 909
 910 static void info_print_key_mgmt(char *buf, size_t len, int key_mgmt)
 911 {
 912         char *pos, *end;
 913
 914         if (key_mgmt == 0) {
 915                 os_snprintf(buf, len, "N/A");
 916                 return;
 917         }
 918
 919         pos = buf;
 920         end = buf + len;
 921
 922         if (key_mgmt & WPA_KEY_MGMT_IEEE8021X)
 923                 pos += os_snprintf(pos, end - pos, "%sEAP",
 924                                    pos == buf ? "" : " ");
 925         if (key_mgmt & WPA_KEY_MGMT_PSK)
 926                 pos += os_snprintf(pos, end - pos, "%sPSK",
 927                                    pos == buf ? "" : " ");
 928         if (key_mgmt & WPA_KEY_MGMT_WPA_NONE)
 929                 pos += os_snprintf(pos, end - pos, "%sWPA-NONE",
 930                                    pos == buf ? "" : " ");
 931         if (key_mgmt & WPA_KEY_MGMT_FT_IEEE8021X)
 932                 pos += os_snprintf(pos, end - pos, "%sFT-EAP",
 933                                    pos == buf ? "" : " ");
 934         if (key_mgmt & WPA_KEY_MGMT_FT_PSK)
 935                 pos += os_snprintf(pos, end - pos, "%sFT-PSK",
 936                                    pos == buf ? "" : " ");
 937         if (key_mgmt & WPA_KEY_MGMT_IEEE8021X_SHA256)
 938                 pos += os_snprintf(pos, end - pos, "%sEAP-SHA256",
 939                                    pos == buf ? "" : " ");
 940         if (key_mgmt & WPA_KEY_MGMT_PSK_SHA256)
 941                 pos += os_snprintf(pos, end - pos, "%sPSK-SHA256",
 942                                    pos == buf ? "" : " ");
 943 }
 944
 945
 946 static void info_print_rsn_capab(char *buf, size_t len, int capab)
 947 {
 948         char *pos, *end;
 949
 950         pos = buf;
 951         end = buf + len;
 952
 953         if (capab & WPA_CAPABILITY_PREAUTH)
 954                 pos += os_snprintf(pos, end - pos, "%sPREAUTH",
 955                                    pos == buf ? "" : " ");
 956         if (capab & WPA_CAPABILITY_NO_PAIRWISE)
 957                 pos += os_snprintf(pos, end - pos, "%sNO_PAIRWISE",
 958                                    pos == buf ? "" : " ");
 959         if (capab & WPA_CAPABILITY_MFPR)
 960                 pos += os_snprintf(pos, end - pos, "%sMFPR",
 961                                    pos == buf ? "" : " ");
 962         if (capab & WPA_CAPABILITY_MFPC)
 963                 pos += os_snprintf(pos, end - pos, "%sMFPC",
 964                                    pos == buf ? "" : " ");
 965         if (capab & WPA_CAPABILITY_PEERKEY_ENABLED)
 966                 pos += os_snprintf(pos, end - pos, "%sPEERKEY",
 967                                    pos == buf ? "" : " ");
 968 }
 969
 970
 971 static void info_print_state(char *buf, size_t len, int state)
 972 {
 973         switch (state) {
 974         case STATE1:
 975                 os_strlcpy(buf, "NOT-AUTH", len);
 976                 break;
 977         case STATE2:
 978                 os_strlcpy(buf, "AUTH", len);
 979                 break;
 980         case STATE3:
 981                 os_strlcpy(buf, "AUTH+ASSOC", len);
 982                 break;
 983         }
 984 }
 985
 986
 987 static void info_print_gtk(char *buf, size_t len, struct wlantest_sta *sta)
 988 {
 989         size_t pos;
 990
 991         pos = os_snprintf(buf, len, "IDX=%d,GTK=", sta->gtk_idx);
 992         wpa_snprintf_hex(buf + pos, len - pos, sta->gtk, sta->gtk_len);
 993 }
 994
 995
 996 static void ctrl_info_sta(struct wlantest *wt, int sock, u8 *cmd, size_t clen)
 997 {
 998         u8 *addr;
 999         size_t addr_len;
1000         struct wlantest_bss *bss;
1001         struct wlantest_sta *sta;
1002         enum wlantest_sta_info info;
1003         u8 buf[4 + 108], *end, *pos;
1004         char resp[100];
1005
1006         bss = ctrl_get_bss(wt, sock, cmd, clen);
1007         sta = ctrl_get_sta(wt, sock, cmd, clen, bss);
1008         if (sta == NULL)
1009                 return;
1010
1011         addr = attr_get(cmd, clen, WLANTEST_ATTR_STA_INFO, &addr_len);
1012         if (addr == NULL || addr_len != 4) {
1013                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
1014                 return;
1015         }
1016         info = WPA_GET_BE32(addr);
1017
1018         resp[0] = '\0';
1019         switch (info) {
1020         case WLANTEST_STA_INFO_PROTO:
1021                 info_print_proto(resp, sizeof(resp), sta->proto);
1022                 break;
1023         case WLANTEST_STA_INFO_PAIRWISE:
1024                 info_print_cipher(resp, sizeof(resp), sta->pairwise_cipher);
1025                 break;
1026         case WLANTEST_STA_INFO_KEY_MGMT:
1027                 info_print_key_mgmt(resp, sizeof(resp), sta->key_mgmt);
1028                 break;
1029         case WLANTEST_STA_INFO_RSN_CAPAB:
1030                 info_print_rsn_capab(resp, sizeof(resp), sta->rsn_capab);
1031                 break;
1032         case WLANTEST_STA_INFO_STATE:
1033                 info_print_state(resp, sizeof(resp), sta->state);
1034                 break;
1035         case WLANTEST_STA_INFO_GTK:
1036                 info_print_gtk(resp, sizeof(resp), sta);
1037                 break;
1038         default:
1039                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
1040                 return;
1041         }
1042
1043         pos = buf;
1044         end = buf + sizeof(buf);
1045         WPA_PUT_BE32(pos, WLANTEST_CTRL_SUCCESS);
1046         pos += 4;
1047         pos = attr_add_str(pos, end, WLANTEST_ATTR_INFO, resp);
1048         ctrl_send(wt, sock, buf, pos - buf);
1049 }
1050
1051
1052 static void ctrl_info_bss(struct wlantest *wt, int sock, u8 *cmd, size_t clen)
1053 {
1054         u8 *addr;
1055         size_t addr_len;
1056         struct wlantest_bss *bss;
1057         enum wlantest_bss_info info;
1058         u8 buf[4 + 108], *end, *pos;
1059         char resp[100];
1060
1061         bss = ctrl_get_bss(wt, sock, cmd, clen);
1062         if (bss == NULL)
1063                 return;
1064
1065         addr = attr_get(cmd, clen, WLANTEST_ATTR_BSS_INFO, &addr_len);
1066         if (addr == NULL || addr_len != 4) {
1067                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
1068                 return;
1069         }
1070         info = WPA_GET_BE32(addr);
1071
1072         resp[0] = '\0';
1073         switch (info) {
1074         case WLANTEST_BSS_INFO_PROTO:
1075                 info_print_proto(resp, sizeof(resp), bss->proto);
1076                 break;
1077         case WLANTEST_BSS_INFO_PAIRWISE:
1078                 info_print_cipher(resp, sizeof(resp), bss->pairwise_cipher);
1079                 break;
1080         case WLANTEST_BSS_INFO_GROUP:
1081                 info_print_cipher(resp, sizeof(resp), bss->group_cipher);
1082                 break;
1083         case WLANTEST_BSS_INFO_GROUP_MGMT:
1084                 info_print_cipher(resp, sizeof(resp), bss->mgmt_group_cipher);
1085                 break;
1086         case WLANTEST_BSS_INFO_KEY_MGMT:
1087                 info_print_key_mgmt(resp, sizeof(resp), bss->key_mgmt);
1088                 break;
1089         case WLANTEST_BSS_INFO_RSN_CAPAB:
1090                 info_print_rsn_capab(resp, sizeof(resp), bss->rsn_capab);
1091                 break;
1092         default:
1093                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
1094                 return;
1095         }
1096
1097         pos = buf;
1098         end = buf + sizeof(buf);
1099         WPA_PUT_BE32(pos, WLANTEST_CTRL_SUCCESS);
1100         pos += 4;
1101         pos = attr_add_str(pos, end, WLANTEST_ATTR_INFO, resp);
1102         ctrl_send(wt, sock, buf, pos - buf);
1103 }
1104
1105
1106 static void ctrl_send_(struct wlantest *wt, int sock, u8 *cmd, size_t clen)
1107 {
1108         struct wlantest_bss *bss;
1109         struct wlantest_sta *sta;
1110         u8 *bssid, *sta_addr;
1111         int prot;
1112         u8 *frame;
1113         size_t frame_len;
1114         int ret = 0;
1115         struct ieee80211_hdr *hdr;
1116         u16 fc;
1117
1118         frame = attr_get(cmd, clen, WLANTEST_ATTR_FRAME, &frame_len);
1119         prot = attr_get_int(cmd, clen, WLANTEST_ATTR_INJECT_PROTECTION);
1120         if (frame == NULL || frame_len < 24 || prot < 0) {
1121                 wpa_printf(MSG_INFO, "Invalid send command parameters");
1122                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
1123                 return;
1124         }
1125
1126         hdr = (struct ieee80211_hdr *) frame;
1127         fc = le_to_host16(hdr->frame_control);
1128         switch (WLAN_FC_GET_TYPE(fc)) {
1129         case WLAN_FC_TYPE_MGMT:
1130                 bssid = hdr->addr3;
1131                 if (os_memcmp(hdr->addr2, hdr->addr3, ETH_ALEN) == 0)
1132                         sta_addr = hdr->addr1;
1133                 else
1134                         sta_addr = hdr->addr2;
1135                 break;
1136         case WLAN_FC_TYPE_DATA:
1137                 switch (fc & (WLAN_FC_TODS | WLAN_FC_FROMDS)) {
1138                 case 0:
1139                         bssid = hdr->addr3;
1140                         sta_addr = hdr->addr2;
1141                         break;
1142                 case WLAN_FC_TODS:
1143                         bssid = hdr->addr1;
1144                         sta_addr = hdr->addr2;
1145                         break;
1146                 case WLAN_FC_FROMDS:
1147                         bssid = hdr->addr2;
1148                         sta_addr = hdr->addr1;
1149                         break;
1150                 default:
1151                         wpa_printf(MSG_INFO, "Unsupported inject frame");
1152                         ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
1153                         return;
1154                 }
1155                 break;
1156         default:
1157                 wpa_printf(MSG_INFO, "Unsupported inject frame");
1158                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
1159                 return;
1160         }
1161
1162         bss = bss_find(wt, bssid);
1163         if (bss == NULL && prot != WLANTEST_INJECT_UNPROTECTED) {
1164                 wpa_printf(MSG_INFO, "Unknown BSSID");
1165                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
1166                 return;
1167         }
1168
1169         if (bss)
1170                 sta = sta_find(bss, sta_addr);
1171         else
1172                 sta = NULL;
1173         if (sta == NULL && prot != WLANTEST_INJECT_UNPROTECTED) {
1174                 wpa_printf(MSG_INFO, "Unknown STA address");
1175                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
1176                 return;
1177         }
1178
1179         ret = wlantest_inject(wt, bss, sta, frame, frame_len, prot);
1180
1181         if (ret)
1182                 wpa_printf(MSG_INFO, "Failed to inject frame");
1183         else
1184                 wpa_printf(MSG_INFO, "Frame injected successfully");
1185         ctrl_send_simple(wt, sock, ret == 0 ? WLANTEST_CTRL_SUCCESS :
1186                          WLANTEST_CTRL_FAILURE);
1187 }
1188
1189
1190 static void ctrl_relog(struct wlantest *wt, int sock)
1191 {
1192         int res = wlantest_relog(wt);
1193         ctrl_send_simple(wt, sock, res ? WLANTEST_CTRL_FAILURE :
1194                          WLANTEST_CTRL_SUCCESS);
1195 }
1196
1197
1198 static void ctrl_read(int sock, void *eloop_ctx, void *sock_ctx)
1199 {
1200         struct wlantest *wt = eloop_ctx;
1201         u8 buf[WLANTEST_CTRL_MAX_CMD_LEN];
1202         int len;
1203         enum wlantest_ctrl_cmd cmd;
1204
1205         wpa_printf(MSG_EXCESSIVE, "New control interface message from %d",
1206                    sock);
1207         len = recv(sock, buf, sizeof(buf), 0);
1208         if (len < 0) {
1209                 wpa_printf(MSG_INFO, "recv(ctrl): %s", strerror(errno));
1210                 ctrl_disconnect(wt, sock);
1211                 return;
1212         }
1213         if (len == 0) {
1214                 ctrl_disconnect(wt, sock);
1215                 return;
1216         }
1217
1218         if (len < 4) {
1219                 wpa_printf(MSG_INFO, "Too short control interface command "
1220                            "from %d", sock);
1221                 ctrl_disconnect(wt, sock);
1222                 return;
1223         }
1224         cmd = WPA_GET_BE32(buf);
1225         wpa_printf(MSG_EXCESSIVE, "Control interface command %d from %d",
1226                    cmd, sock);
1227
1228         switch (cmd) {
1229         case WLANTEST_CTRL_PING:
1230                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_SUCCESS);
1231                 break;
1232         case WLANTEST_CTRL_TERMINATE:
1233                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_SUCCESS);
1234                 eloop_terminate();
1235                 break;
1236         case WLANTEST_CTRL_LIST_BSS:
1237                 ctrl_list_bss(wt, sock);
1238                 break;
1239         case WLANTEST_CTRL_LIST_STA:
1240                 ctrl_list_sta(wt, sock, buf + 4, len - 4);
1241                 break;
1242         case WLANTEST_CTRL_FLUSH:
1243                 ctrl_flush(wt, sock);
1244                 break;
1245         case WLANTEST_CTRL_CLEAR_STA_COUNTERS:
1246                 ctrl_clear_sta_counters(wt, sock, buf + 4, len - 4);
1247                 break;
1248         case WLANTEST_CTRL_CLEAR_BSS_COUNTERS:
1249                 ctrl_clear_bss_counters(wt, sock, buf + 4, len - 4);
1250                 break;
1251         case WLANTEST_CTRL_CLEAR_TDLS_COUNTERS:
1252                 ctrl_clear_tdls_counters(wt, sock, buf + 4, len - 4);
1253                 break;
1254         case WLANTEST_CTRL_GET_STA_COUNTER:
1255                 ctrl_get_sta_counter(wt, sock, buf + 4, len - 4);
1256                 break;
1257         case WLANTEST_CTRL_GET_BSS_COUNTER:
1258                 ctrl_get_bss_counter(wt, sock, buf + 4, len - 4);
1259                 break;
1260         case WLANTEST_CTRL_GET_TDLS_COUNTER:
1261                 ctrl_get_tdls_counter(wt, sock, buf + 4, len - 4);
1262                 break;
1263         case WLANTEST_CTRL_INJECT:
1264                 ctrl_inject(wt, sock, buf + 4, len - 4);
1265                 break;
1266         case WLANTEST_CTRL_VERSION:
1267                 ctrl_version(wt, sock);
1268                 break;
1269         case WLANTEST_CTRL_ADD_PASSPHRASE:
1270                 ctrl_add_passphrase(wt, sock, buf + 4, len - 4);
1271                 break;
1272         case WLANTEST_CTRL_INFO_STA:
1273                 ctrl_info_sta(wt, sock, buf + 4, len - 4);
1274                 break;
1275         case WLANTEST_CTRL_INFO_BSS:
1276                 ctrl_info_bss(wt, sock, buf + 4, len - 4);
1277                 break;
1278         case WLANTEST_CTRL_SEND:
1279                 ctrl_send_(wt, sock, buf + 4, len - 4);
1280                 break;
1281         case WLANTEST_CTRL_RELOG:
1282                 ctrl_relog(wt, sock);
1283                 break;
1284         default:
1285                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_UNKNOWN_CMD);
1286                 break;
1287         }
1288 }
1289
1290
1291 static void ctrl_connect(int sock, void *eloop_ctx, void *sock_ctx)
1292 {
1293         struct wlantest *wt = eloop_ctx;
1294         int conn, i;
1295
1296         conn = accept(sock, NULL, NULL);
1297         if (conn < 0) {
1298                 wpa_printf(MSG_INFO, "accept(ctrl): %s", strerror(errno));
1299                 return;
1300         }
1301         wpa_printf(MSG_MSGDUMP, "New control interface connection %d", conn);
1302
1303         for (i = 0; i < MAX_CTRL_CONNECTIONS; i++) {
1304                 if (wt->ctrl_socks[i] < 0)
1305                         break;
1306         }
1307
1308         if (i == MAX_CTRL_CONNECTIONS) {
1309                 wpa_printf(MSG_INFO, "No room for new control connection");
1310                 close(conn);
1311                 return;
1312         }
1313
1314         wt->ctrl_socks[i] = conn;
1315         eloop_register_read_sock(conn, ctrl_read, wt, NULL);
1316 }
1317
1318
1319 int ctrl_init(struct wlantest *wt)
1320 {
1321         struct sockaddr_un addr;
1322
1323         wt->ctrl_sock = socket(AF_UNIX, SOCK_SEQPACKET, 0);
1324         if (wt->ctrl_sock < 0) {
1325                 wpa_printf(MSG_ERROR, "socket: %s", strerror(errno));
1326                 return -1;
1327         }
1328
1329         os_memset(&addr, 0, sizeof(addr));
1330         addr.sun_family = AF_UNIX;
1331         os_strlcpy(addr.sun_path + 1, WLANTEST_SOCK_NAME,
1332                    sizeof(addr.sun_path) - 1);
1333         if (bind(wt->ctrl_sock, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
1334                 wpa_printf(MSG_ERROR, "bind: %s", strerror(errno));
1335                 close(wt->ctrl_sock);
1336                 wt->ctrl_sock = -1;
1337                 return -1;
1338         }
1339
1340         if (listen(wt->ctrl_sock, 5) < 0) {
1341                 wpa_printf(MSG_ERROR, "listen: %s", strerror(errno));
1342                 close(wt->ctrl_sock);
1343                 wt->ctrl_sock = -1;
1344                 return -1;
1345         }
1346
1347         if (eloop_register_read_sock(wt->ctrl_sock, ctrl_connect, wt, NULL)) {
1348                 close(wt->ctrl_sock);
1349                 wt->ctrl_sock = -1;
1350                 return -1;
1351         }
1352
1353         return 0;
1354 }
1355
1356
1357 void ctrl_deinit(struct wlantest *wt)
1358 {
1359         int i;
1360
1361         if (wt->ctrl_sock < 0)
1362                 return;
1363
1364         for (i = 0; i < MAX_CTRL_CONNECTIONS; i++) {
1365                 if (wt->ctrl_socks[i] >= 0) {
1366                         close(wt->ctrl_socks[i]);
1367                         eloop_unregister_read_sock(wt->ctrl_socks[i]);
1368                         wt->ctrl_socks[i] = -1;
1369                 }
1370         }
1371
1372         eloop_unregister_read_sock(wt->ctrl_sock);
1373         close(wt->ctrl_sock);
1374         wt->ctrl_sock = -1;
1375 }