wlantest/ctrl.c

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