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/version.h"
  21 #include "common/ieee802_11_defs.h"
  22 #include "wlantest.h"
  23 #include "wlantest_ctrl.h"
  24
  25
  26 static u8 * attr_get(u8 *buf, size_t buflen, enum wlantest_ctrl_attr attr,
  27                      size_t *len)
  28 {
  29         u8 *pos = buf;
  30
  31         while (pos + 8 <= buf + buflen) {
  32                 enum wlantest_ctrl_attr a;
  33                 size_t alen;
  34                 a = WPA_GET_BE32(pos);
  35                 pos += 4;
  36                 alen = WPA_GET_BE32(pos);
  37                 pos += 4;
  38                 if (pos + alen > buf + buflen) {
  39                         wpa_printf(MSG_DEBUG, "Invalid control message "
  40                                    "attribute");
  41                         return NULL;
  42                 }
  43                 if (a == attr) {
  44                         *len = alen;
  45                         return pos;
  46                 }
  47                 pos += alen;
  48         }
  49
  50         return NULL;
  51 }
  52
  53
  54 static u8 * attr_get_macaddr(u8 *buf, size_t buflen,
  55                              enum wlantest_ctrl_attr attr)
  56 {
  57         u8 *addr;
  58         size_t addr_len;
  59         addr = attr_get(buf, buflen, attr, &addr_len);
  60         if (addr && addr_len != ETH_ALEN)
  61                 addr = NULL;
  62         return addr;
  63 }
  64
  65
  66 static int attr_get_int(u8 *buf, size_t buflen, enum wlantest_ctrl_attr attr)
  67 {
  68         u8 *pos;
  69         size_t len;
  70         pos = attr_get(buf, buflen, attr, &len);
  71         if (pos == NULL || len != 4)
  72                 return -1;
  73         return WPA_GET_BE32(pos);
  74 }
  75
  76
  77 static u8 * attr_add_str(u8 *pos, u8 *end, enum wlantest_ctrl_attr attr,
  78                          const char *str)
  79 {
  80         size_t len = os_strlen(str);
  81
  82         if (pos == NULL || end - pos < 8 + len)
  83                 return NULL;
  84         WPA_PUT_BE32(pos, attr);
  85         pos += 4;
  86         WPA_PUT_BE32(pos, len);
  87         pos += 4;
  88         os_memcpy(pos, str, len);
  89         pos += len;
  90         return pos;
  91 }
  92
  93
  94 static u8 * attr_add_be32(u8 *pos, u8 *end, enum wlantest_ctrl_attr attr,
  95                           u32 val)
  96 {
  97         if (pos == NULL || end - pos < 12)
  98                 return NULL;
  99         WPA_PUT_BE32(pos, attr);
 100         pos += 4;
 101         WPA_PUT_BE32(pos, 4);
 102         pos += 4;
 103         WPA_PUT_BE32(pos, val);
 104         pos += 4;
 105         return pos;
 106 }
 107
 108
 109 static void ctrl_disconnect(struct wlantest *wt, int sock)
 110 {
 111         int i;
 112         wpa_printf(MSG_DEBUG, "Disconnect control interface connection %d",
 113                    sock);
 114         for (i = 0; i < MAX_CTRL_CONNECTIONS; i++) {
 115                 if (wt->ctrl_socks[i] == sock) {
 116                         close(wt->ctrl_socks[i]);
 117                         eloop_unregister_read_sock(wt->ctrl_socks[i]);
 118                         wt->ctrl_socks[i] = -1;
 119                         break;
 120                 }
 121         }
 122 }
 123
 124
 125 static void ctrl_send(struct wlantest *wt, int sock, const u8 *buf,
 126                       size_t len)
 127 {
 128         if (send(sock, buf, len, 0) < 0) {
 129                 wpa_printf(MSG_INFO, "send(ctrl): %s", strerror(errno));
 130                 ctrl_disconnect(wt, sock);
 131         }
 132 }
 133
 134
 135 static void ctrl_send_simple(struct wlantest *wt, int sock,
 136                              enum wlantest_ctrl_cmd cmd)
 137 {
 138         u8 buf[4];
 139         WPA_PUT_BE32(buf, cmd);
 140         ctrl_send(wt, sock, buf, sizeof(buf));
 141 }
 142
 143
 144 static void ctrl_list_bss(struct wlantest *wt, int sock)
 145 {
 146         u8 buf[WLANTEST_CTRL_MAX_RESP_LEN], *pos, *len;
 147         struct wlantest_bss *bss;
 148
 149         pos = buf;
 150         WPA_PUT_BE32(pos, WLANTEST_CTRL_SUCCESS);
 151         pos += 4;
 152         WPA_PUT_BE32(pos, WLANTEST_ATTR_BSSID);
 153         pos += 4;
 154         len = pos; /* to be filled */
 155         pos += 4;
 156
 157         dl_list_for_each(bss, &wt->bss, struct wlantest_bss, list) {
 158                 if (pos + ETH_ALEN > buf + WLANTEST_CTRL_MAX_RESP_LEN)
 159                         break;
 160                 os_memcpy(pos, bss->bssid, ETH_ALEN);
 161                 pos += ETH_ALEN;
 162         }
 163
 164         WPA_PUT_BE32(len, pos - len - 4);
 165         ctrl_send(wt, sock, buf, pos - buf);
 166 }
 167
 168
 169 static void ctrl_list_sta(struct wlantest *wt, int sock, u8 *cmd, size_t clen)
 170 {
 171         u8 buf[WLANTEST_CTRL_MAX_RESP_LEN], *pos, *len;
 172         u8 *bssid;
 173         size_t bssid_len;
 174         struct wlantest_bss *bss;
 175         struct wlantest_sta *sta;
 176
 177         bssid = attr_get(cmd, clen, WLANTEST_ATTR_BSSID, &bssid_len);
 178         if (bssid == NULL || bssid_len != ETH_ALEN) {
 179                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
 180                 return;
 181         }
 182
 183         bss = bss_find(wt, bssid);
 184         if (bss == NULL) {
 185                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
 186                 return;
 187         }
 188
 189         pos = buf;
 190         WPA_PUT_BE32(pos, WLANTEST_CTRL_SUCCESS);
 191         pos += 4;
 192         WPA_PUT_BE32(pos, WLANTEST_ATTR_STA_ADDR);
 193         pos += 4;
 194         len = pos; /* to be filled */
 195         pos += 4;
 196
 197         dl_list_for_each(sta, &bss->sta, struct wlantest_sta, list) {
 198                 if (pos + ETH_ALEN > buf + WLANTEST_CTRL_MAX_RESP_LEN)
 199                         break;
 200                 os_memcpy(pos, sta->addr, ETH_ALEN);
 201                 pos += ETH_ALEN;
 202         }
 203
 204         WPA_PUT_BE32(len, pos - len - 4);
 205         ctrl_send(wt, sock, buf, pos - buf);
 206 }
 207
 208
 209 static void ctrl_flush(struct wlantest *wt, int sock)
 210 {
 211         wpa_printf(MSG_DEBUG, "Drop all collected BSS data");
 212         bss_flush(wt);
 213         ctrl_send_simple(wt, sock, WLANTEST_CTRL_SUCCESS);
 214 }
 215
 216
 217 static void ctrl_clear_sta_counters(struct wlantest *wt, int sock, u8 *cmd,
 218                                     size_t clen)
 219 {
 220         u8 *addr;
 221         size_t addr_len;
 222         struct wlantest_bss *bss;
 223         struct wlantest_sta *sta;
 224
 225         addr = attr_get(cmd, clen, WLANTEST_ATTR_BSSID, &addr_len);
 226         if (addr == NULL || addr_len != ETH_ALEN) {
 227                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
 228                 return;
 229         }
 230
 231         bss = bss_find(wt, addr);
 232         if (bss == NULL) {
 233                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
 234                 return;
 235         }
 236
 237         addr = attr_get(cmd, clen, WLANTEST_ATTR_STA_ADDR, &addr_len);
 238         if (addr == NULL || addr_len != ETH_ALEN) {
 239                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
 240                 return;
 241         }
 242
 243         sta = sta_find(bss, addr);
 244         if (sta == NULL) {
 245                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
 246                 return;
 247         }
 248
 249         os_memset(sta->counters, 0, sizeof(sta->counters));
 250         ctrl_send_simple(wt, sock, WLANTEST_CTRL_SUCCESS);
 251 }
 252
 253
 254 static void ctrl_clear_bss_counters(struct wlantest *wt, int sock, u8 *cmd,
 255                                     size_t clen)
 256 {
 257         u8 *addr;
 258         size_t addr_len;
 259         struct wlantest_bss *bss;
 260
 261         addr = attr_get(cmd, clen, WLANTEST_ATTR_BSSID, &addr_len);
 262         if (addr == NULL || addr_len != ETH_ALEN) {
 263                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
 264                 return;
 265         }
 266
 267         bss = bss_find(wt, addr);
 268         if (bss == NULL) {
 269                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
 270                 return;
 271         }
 272
 273         os_memset(bss->counters, 0, sizeof(bss->counters));
 274         ctrl_send_simple(wt, sock, WLANTEST_CTRL_SUCCESS);
 275 }
 276
 277
 278 static void ctrl_get_sta_counter(struct wlantest *wt, int sock, u8 *cmd,
 279                                  size_t clen)
 280 {
 281         u8 *addr;
 282         size_t addr_len;
 283         struct wlantest_bss *bss;
 284         struct wlantest_sta *sta;
 285         u32 counter;
 286         u8 buf[4 + 12], *end, *pos;
 287
 288         addr = attr_get(cmd, clen, WLANTEST_ATTR_BSSID, &addr_len);
 289         if (addr == NULL || addr_len != ETH_ALEN) {
 290                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
 291                 return;
 292         }
 293
 294         bss = bss_find(wt, addr);
 295         if (bss == NULL) {
 296                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
 297                 return;
 298         }
 299
 300         addr = attr_get(cmd, clen, WLANTEST_ATTR_STA_ADDR, &addr_len);
 301         if (addr == NULL || addr_len != ETH_ALEN) {
 302                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
 303                 return;
 304         }
 305
 306         sta = sta_find(bss, addr);
 307         if (sta == NULL) {
 308                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
 309                 return;
 310         }
 311
 312         addr = attr_get(cmd, clen, WLANTEST_ATTR_STA_COUNTER, &addr_len);
 313         if (addr == NULL || addr_len != 4) {
 314                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
 315                 return;
 316         }
 317         counter = WPA_GET_BE32(addr);
 318         if (counter >= NUM_WLANTEST_STA_COUNTER) {
 319                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
 320                 return;
 321         }
 322
 323         pos = buf;
 324         end = buf + sizeof(buf);
 325         WPA_PUT_BE32(pos, WLANTEST_CTRL_SUCCESS);
 326         pos += 4;
 327         pos = attr_add_be32(pos, end, WLANTEST_ATTR_COUNTER,
 328                             sta->counters[counter]);
 329         ctrl_send(wt, sock, buf, pos - buf);
 330 }
 331
 332
 333 static void ctrl_get_bss_counter(struct wlantest *wt, int sock, u8 *cmd,
 334                                  size_t clen)
 335 {
 336         u8 *addr;
 337         size_t addr_len;
 338         struct wlantest_bss *bss;
 339         u32 counter;
 340         u8 buf[4 + 12], *end, *pos;
 341
 342         addr = attr_get(cmd, clen, WLANTEST_ATTR_BSSID, &addr_len);
 343         if (addr == NULL || addr_len != ETH_ALEN) {
 344                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
 345                 return;
 346         }
 347
 348         bss = bss_find(wt, addr);
 349         if (bss == NULL) {
 350                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
 351                 return;
 352         }
 353
 354         addr = attr_get(cmd, clen, WLANTEST_ATTR_BSS_COUNTER, &addr_len);
 355         if (addr == NULL || addr_len != 4) {
 356                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
 357                 return;
 358         }
 359         counter = WPA_GET_BE32(addr);
 360         if (counter >= NUM_WLANTEST_BSS_COUNTER) {
 361                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
 362                 return;
 363         }
 364
 365         pos = buf;
 366         end = buf + sizeof(buf);
 367         WPA_PUT_BE32(pos, WLANTEST_CTRL_SUCCESS);
 368         pos += 4;
 369         pos = attr_add_be32(pos, end, WLANTEST_ATTR_COUNTER,
 370                             bss->counters[counter]);
 371         ctrl_send(wt, sock, buf, pos - buf);
 372 }
 373
 374
 375 static void build_mgmt_hdr(struct ieee80211_mgmt *mgmt,
 376                            struct wlantest_bss *bss, struct wlantest_sta *sta,
 377                            int sender_ap, int stype)
 378 {
 379         os_memset(mgmt, 0, 24);
 380         mgmt->frame_control = IEEE80211_FC(WLAN_FC_TYPE_MGMT, stype);
 381         if (sender_ap) {
 382                 if (sta)
 383                         os_memcpy(mgmt->da, sta->addr, ETH_ALEN);
 384                 else
 385                         os_memset(mgmt->da, 0xff, ETH_ALEN);
 386                 os_memcpy(mgmt->sa, bss->bssid, ETH_ALEN);
 387         } else {
 388                 os_memcpy(mgmt->da, bss->bssid, ETH_ALEN);
 389                 os_memcpy(mgmt->sa, sta->addr, ETH_ALEN);
 390         }
 391         os_memcpy(mgmt->bssid, bss->bssid, ETH_ALEN);
 392 }
 393
 394
 395 static int ctrl_inject_auth(struct wlantest *wt, struct wlantest_bss *bss,
 396                             struct wlantest_sta *sta, int sender_ap,
 397                             enum wlantest_inject_protection prot)
 398 {
 399         struct ieee80211_mgmt mgmt;
 400
 401         if (prot != WLANTEST_INJECT_NORMAL &&
 402             prot != WLANTEST_INJECT_UNPROTECTED)
 403                 return -1; /* Authentication frame is never protected */
 404         if (sta == NULL)
 405                 return -1; /* No broadcast Authentication frames */
 406
 407         if (sender_ap)
 408                 wpa_printf(MSG_INFO, "INJECT: Auth " MACSTR " -> " MACSTR,
 409                            MAC2STR(bss->bssid), MAC2STR(sta->addr));
 410         else
 411                 wpa_printf(MSG_INFO, "INJECT: Auth " MACSTR " -> " MACSTR,
 412                            MAC2STR(sta->addr), MAC2STR(bss->bssid));
 413         build_mgmt_hdr(&mgmt, bss, sta, sender_ap, WLAN_FC_STYPE_AUTH);
 414
 415         mgmt.u.auth.auth_alg = host_to_le16(WLAN_AUTH_OPEN);
 416         mgmt.u.auth.auth_transaction = host_to_le16(1);
 417         mgmt.u.auth.status_code = host_to_le16(WLAN_STATUS_SUCCESS);
 418
 419         return wlantest_inject(wt, bss, sta, (u8 *) &mgmt, 24 + 6,
 420                                WLANTEST_INJECT_UNPROTECTED);
 421 }
 422
 423
 424 static int ctrl_inject_assocreq(struct wlantest *wt, struct wlantest_bss *bss,
 425                                 struct wlantest_sta *sta, int sender_ap,
 426                                 enum wlantest_inject_protection prot)
 427 {
 428         u8 *buf;
 429         struct ieee80211_mgmt *mgmt;
 430         int ret;
 431
 432         if (prot != WLANTEST_INJECT_NORMAL &&
 433             prot != WLANTEST_INJECT_UNPROTECTED)
 434                 return -1; /* Association Request frame is never protected */
 435         if (sta == NULL)
 436                 return -1; /* No broadcast Association Request frames */
 437         if (sender_ap)
 438                 return -1; /* No Association Request frame sent by AP */
 439         if (sta->assocreq_ies == NULL) {
 440                 wpa_printf(MSG_INFO, "INJECT: No previous (Re)Association "
 441                            "Request available for " MACSTR,
 442                            MAC2STR(sta->addr));
 443                 return -1;
 444         }
 445
 446         wpa_printf(MSG_INFO, "INJECT: AssocReq " MACSTR " -> " MACSTR,
 447                    MAC2STR(sta->addr), MAC2STR(bss->bssid));
 448         buf = os_malloc(sizeof(*mgmt) + sta->assocreq_ies_len);
 449         if (buf == NULL)
 450                 return -1;
 451         mgmt = (struct ieee80211_mgmt *) buf;
 452
 453         build_mgmt_hdr(mgmt, bss, sta, sender_ap, WLAN_FC_STYPE_ASSOC_REQ);
 454
 455         mgmt->u.assoc_req.capab_info = host_to_le16(sta->assocreq_capab_info);
 456         mgmt->u.assoc_req.listen_interval =
 457                 host_to_le16(sta->assocreq_listen_int);
 458         os_memcpy(mgmt->u.assoc_req.variable, sta->assocreq_ies,
 459                   sta->assocreq_ies_len);
 460
 461         ret = wlantest_inject(wt, bss, sta, buf,
 462                               24 + 4 + sta->assocreq_ies_len,
 463                               WLANTEST_INJECT_UNPROTECTED);
 464         os_free(buf);
 465         return ret;
 466 }
 467
 468
 469 static int ctrl_inject_reassocreq(struct wlantest *wt,
 470                                   struct wlantest_bss *bss,
 471                                   struct wlantest_sta *sta, int sender_ap,
 472                                   enum wlantest_inject_protection prot)
 473 {
 474         u8 *buf;
 475         struct ieee80211_mgmt *mgmt;
 476         int ret;
 477
 478         if (prot != WLANTEST_INJECT_NORMAL &&
 479             prot != WLANTEST_INJECT_UNPROTECTED)
 480                 return -1; /* Reassociation Request frame is never protected */
 481         if (sta == NULL)
 482                 return -1; /* No broadcast Reassociation Request frames */
 483         if (sender_ap)
 484                 return -1; /* No Reassociation Request frame sent by AP */
 485         if (sta->assocreq_ies == NULL) {
 486                 wpa_printf(MSG_INFO, "INJECT: No previous (Re)Association "
 487                            "Request available for " MACSTR,
 488                            MAC2STR(sta->addr));
 489                 return -1;
 490         }
 491
 492         wpa_printf(MSG_INFO, "INJECT: ReassocReq " MACSTR " -> " MACSTR,
 493                    MAC2STR(sta->addr), MAC2STR(bss->bssid));
 494         buf = os_malloc(sizeof(*mgmt) + sta->assocreq_ies_len);
 495         if (buf == NULL)
 496                 return -1;
 497         mgmt = (struct ieee80211_mgmt *) buf;
 498
 499         build_mgmt_hdr(mgmt, bss, sta, sender_ap, WLAN_FC_STYPE_REASSOC_REQ);
 500
 501         mgmt->u.reassoc_req.capab_info =
 502                 host_to_le16(sta->assocreq_capab_info);
 503         mgmt->u.reassoc_req.listen_interval =
 504                 host_to_le16(sta->assocreq_listen_int);
 505         os_memcpy(mgmt->u.reassoc_req.current_ap, bss->bssid, ETH_ALEN);
 506         os_memcpy(mgmt->u.reassoc_req.variable, sta->assocreq_ies,
 507                   sta->assocreq_ies_len);
 508
 509         ret = wlantest_inject(wt, bss, sta, buf,
 510                               24 + 10 + sta->assocreq_ies_len,
 511                               WLANTEST_INJECT_UNPROTECTED);
 512         os_free(buf);
 513         return ret;
 514 }
 515
 516
 517 static int ctrl_inject_deauth(struct wlantest *wt, struct wlantest_bss *bss,
 518                               struct wlantest_sta *sta, int sender_ap,
 519                               enum wlantest_inject_protection prot)
 520 {
 521         struct ieee80211_mgmt mgmt;
 522
 523         if (sender_ap) {
 524                 if (sta)
 525                         wpa_printf(MSG_INFO, "INJECT: Deauth " MACSTR " -> "
 526                                    MACSTR,
 527                                    MAC2STR(bss->bssid), MAC2STR(sta->addr));
 528                 else
 529                         wpa_printf(MSG_INFO, "INJECT: Deauth " MACSTR
 530                                    " -> broadcast", MAC2STR(bss->bssid));
 531         } else
 532                 wpa_printf(MSG_INFO, "INJECT: Deauth " MACSTR " -> " MACSTR,
 533                            MAC2STR(sta->addr), MAC2STR(bss->bssid));
 534         build_mgmt_hdr(&mgmt, bss, sta, sender_ap, WLAN_FC_STYPE_DEAUTH);
 535
 536         mgmt.u.deauth.reason_code = host_to_le16(WLAN_REASON_UNSPECIFIED);
 537
 538         return wlantest_inject(wt, bss, sta, (u8 *) &mgmt, 24 + 2, prot);
 539 }
 540
 541
 542 static int ctrl_inject_disassoc(struct wlantest *wt, struct wlantest_bss *bss,
 543                                 struct wlantest_sta *sta, int sender_ap,
 544                                 enum wlantest_inject_protection prot)
 545 {
 546         struct ieee80211_mgmt mgmt;
 547
 548         if (sender_ap) {
 549                 if (sta)
 550                         wpa_printf(MSG_INFO, "INJECT: Disassoc " MACSTR " -> "
 551                                    MACSTR,
 552                                    MAC2STR(bss->bssid), MAC2STR(sta->addr));
 553                 else
 554                         wpa_printf(MSG_INFO, "INJECT: Disassoc " MACSTR
 555                                    " -> broadcast", MAC2STR(bss->bssid));
 556         } else
 557                 wpa_printf(MSG_INFO, "INJECT: Disassoc " MACSTR " -> " MACSTR,
 558                            MAC2STR(sta->addr), MAC2STR(bss->bssid));
 559         build_mgmt_hdr(&mgmt, bss, sta, sender_ap, WLAN_FC_STYPE_DISASSOC);
 560
 561         mgmt.u.disassoc.reason_code = host_to_le16(WLAN_REASON_UNSPECIFIED);
 562
 563         return wlantest_inject(wt, bss, sta, (u8 *) &mgmt, 24 + 2, prot);
 564 }
 565
 566
 567 static int ctrl_inject_saqueryreq(struct wlantest *wt,
 568                                   struct wlantest_bss *bss,
 569                                   struct wlantest_sta *sta, int sender_ap,
 570                                   enum wlantest_inject_protection prot)
 571 {
 572         struct ieee80211_mgmt mgmt;
 573
 574         if (sta == NULL)
 575                 return -1; /* No broadcast SA Query frames */
 576
 577         if (sender_ap)
 578                 wpa_printf(MSG_INFO, "INJECT: SA Query Request " MACSTR " -> "
 579                            MACSTR, MAC2STR(bss->bssid), MAC2STR(sta->addr));
 580         else
 581                 wpa_printf(MSG_INFO, "INJECT: SA Query Request " MACSTR " -> "
 582                            MACSTR, MAC2STR(sta->addr), MAC2STR(bss->bssid));
 583         build_mgmt_hdr(&mgmt, bss, sta, sender_ap, WLAN_FC_STYPE_ACTION);
 584
 585         mgmt.u.action.category = WLAN_ACTION_SA_QUERY;
 586         mgmt.u.action.u.sa_query_req.action = WLAN_SA_QUERY_REQUEST;
 587         mgmt.u.action.u.sa_query_req.trans_id[0] = 0x12;
 588         mgmt.u.action.u.sa_query_req.trans_id[1] = 0x34;
 589         os_memcpy(sender_ap ? sta->ap_sa_query_tr : sta->sta_sa_query_tr,
 590                   mgmt.u.action.u.sa_query_req.trans_id,
 591                   WLAN_SA_QUERY_TR_ID_LEN);
 592         return wlantest_inject(wt, bss, sta, (u8 *) &mgmt, 24 + 4, prot);
 593 }
 594
 595
 596 static void ctrl_inject(struct wlantest *wt, int sock, u8 *cmd, size_t clen)
 597 {
 598         u8 *bssid, *sta_addr;
 599         struct wlantest_bss *bss;
 600         struct wlantest_sta *sta;
 601         int frame, sender_ap, prot;
 602         int ret = 0;
 603
 604         bssid = attr_get_macaddr(cmd, clen, WLANTEST_ATTR_BSSID);
 605         sta_addr = attr_get_macaddr(cmd, clen, WLANTEST_ATTR_STA_ADDR);
 606         frame = attr_get_int(cmd, clen, WLANTEST_ATTR_INJECT_FRAME);
 607         sender_ap = attr_get_int(cmd, clen, WLANTEST_ATTR_INJECT_SENDER_AP);
 608         if (sender_ap < 0)
 609                 sender_ap = 0;
 610         prot = attr_get_int(cmd, clen, WLANTEST_ATTR_INJECT_PROTECTION);
 611         if (bssid == NULL || sta_addr == NULL || frame < 0 || prot < 0) {
 612                 wpa_printf(MSG_INFO, "Invalid inject command parameters");
 613                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
 614                 return;
 615         }
 616
 617         bss = bss_find(wt, bssid);
 618         if (bss == NULL) {
 619                 wpa_printf(MSG_INFO, "BSS not found for inject command");
 620                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
 621                 return;
 622         }
 623
 624         if (is_broadcast_ether_addr(sta_addr)) {
 625                 if (!sender_ap) {
 626                         wpa_printf(MSG_INFO, "Invalid broadcast inject "
 627                                    "command without sender_ap set");
 628                         ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
 629                         return;
 630                 } sta = NULL;
 631         } else {
 632                 sta = sta_find(bss, sta_addr);
 633                 if (sta == NULL) {
 634                         wpa_printf(MSG_INFO, "Station not found for inject "
 635                                    "command");
 636                         ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
 637                         return;
 638                 }
 639         }
 640
 641         switch (frame) {
 642         case WLANTEST_FRAME_AUTH:
 643                 ret = ctrl_inject_auth(wt, bss, sta, sender_ap, prot);
 644                 break;
 645         case WLANTEST_FRAME_ASSOCREQ:
 646                 ret = ctrl_inject_assocreq(wt, bss, sta, sender_ap, prot);
 647                 break;
 648         case WLANTEST_FRAME_REASSOCREQ:
 649                 ret = ctrl_inject_reassocreq(wt, bss, sta, sender_ap, prot);
 650                 break;
 651         case WLANTEST_FRAME_DEAUTH:
 652                 ret = ctrl_inject_deauth(wt, bss, sta, sender_ap, prot);
 653                 break;
 654         case WLANTEST_FRAME_DISASSOC:
 655                 ret = ctrl_inject_disassoc(wt, bss, sta, sender_ap, prot);
 656                 break;
 657         case WLANTEST_FRAME_SAQUERYREQ:
 658                 ret = ctrl_inject_saqueryreq(wt, bss, sta, sender_ap, prot);
 659                 break;
 660         default:
 661                 wpa_printf(MSG_INFO, "Unsupported inject command frame %d",
 662                            frame);
 663                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
 664                 return;
 665         }
 666
 667         if (ret)
 668                 wpa_printf(MSG_INFO, "Failed to inject frame");
 669         else
 670                 wpa_printf(MSG_INFO, "Frame injected successfully");
 671         ctrl_send_simple(wt, sock, ret == 0 ? WLANTEST_CTRL_SUCCESS :
 672                          WLANTEST_CTRL_FAILURE);
 673 }
 674
 675
 676 static void ctrl_version(struct wlantest *wt, int sock)
 677 {
 678         u8 buf[WLANTEST_CTRL_MAX_RESP_LEN], *pos;
 679
 680         pos = buf;
 681         WPA_PUT_BE32(pos, WLANTEST_CTRL_SUCCESS);
 682         pos += 4;
 683         pos = attr_add_str(pos, buf + sizeof(buf), WLANTEST_ATTR_VERSION,
 684                            VERSION_STR);
 685         ctrl_send(wt, sock, buf, pos - buf);
 686 }
 687
 688
 689 static void ctrl_add_passphrase(struct wlantest *wt, int sock, u8 *cmd,
 690                                 size_t clen)
 691 {
 692         u8 *passphrase;
 693         size_t len;
 694         struct wlantest_passphrase *p, *pa;
 695         u8 *bssid;
 696
 697         passphrase = attr_get(cmd, clen, WLANTEST_ATTR_PASSPHRASE, &len);
 698         if (passphrase == NULL || len < 8 || len > 63) {
 699                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
 700                 return;
 701         }
 702
 703         p = os_zalloc(sizeof(*p));
 704         if (p == NULL) {
 705                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
 706                 return;
 707         }
 708         os_memcpy(p->passphrase, passphrase, len);
 709         wpa_printf(MSG_INFO, "Add passphrase '%s'", p->passphrase);
 710
 711         bssid = attr_get_macaddr(cmd, clen, WLANTEST_ATTR_BSSID);
 712         if (bssid) {
 713                 os_memcpy(p->bssid, bssid, ETH_ALEN);
 714                 wpa_printf(MSG_INFO, "Limit passphrase for BSSID " MACSTR,
 715                            MAC2STR(p->bssid));
 716         }
 717
 718         dl_list_for_each(pa, &wt->passphrase, struct wlantest_passphrase, list)
 719         {
 720                 if (os_strcmp(p->passphrase, pa->passphrase) == 0 &&
 721                     os_memcmp(p->bssid, pa->bssid, ETH_ALEN) == 0) {
 722                         wpa_printf(MSG_INFO, "Passphrase was already known");
 723                         os_free(p);
 724                         p = NULL;
 725                         break;
 726                 }
 727         }
 728
 729         if (p)
 730                 dl_list_add(&wt->passphrase, &p->list);
 731
 732         ctrl_send_simple(wt, sock, WLANTEST_CTRL_SUCCESS);
 733 }
 734
 735
 736 static void ctrl_read(int sock, void *eloop_ctx, void *sock_ctx)
 737 {
 738         struct wlantest *wt = eloop_ctx;
 739         u8 buf[WLANTEST_CTRL_MAX_CMD_LEN];
 740         int len;
 741         enum wlantest_ctrl_cmd cmd;
 742
 743         wpa_printf(MSG_EXCESSIVE, "New control interface message from %d",
 744                    sock);
 745         len = recv(sock, buf, sizeof(buf), 0);
 746         if (len < 0) {
 747                 wpa_printf(MSG_INFO, "recv(ctrl): %s", strerror(errno));
 748                 ctrl_disconnect(wt, sock);
 749                 return;
 750         }
 751         if (len == 0) {
 752                 ctrl_disconnect(wt, sock);
 753                 return;
 754         }
 755
 756         if (len < 4) {
 757                 wpa_printf(MSG_INFO, "Too short control interface command "
 758                            "from %d", sock);
 759                 ctrl_disconnect(wt, sock);
 760                 return;
 761         }
 762         cmd = WPA_GET_BE32(buf);
 763         wpa_printf(MSG_EXCESSIVE, "Control interface command %d from %d",
 764                    cmd, sock);
 765
 766         switch (cmd) {
 767         case WLANTEST_CTRL_PING:
 768                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_SUCCESS);
 769                 break;
 770         case WLANTEST_CTRL_TERMINATE:
 771                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_SUCCESS);
 772                 eloop_terminate();
 773                 break;
 774         case WLANTEST_CTRL_LIST_BSS:
 775                 ctrl_list_bss(wt, sock);
 776                 break;
 777         case WLANTEST_CTRL_LIST_STA:
 778                 ctrl_list_sta(wt, sock, buf + 4, len - 4);
 779                 break;
 780         case WLANTEST_CTRL_FLUSH:
 781                 ctrl_flush(wt, sock);
 782                 break;
 783         case WLANTEST_CTRL_CLEAR_STA_COUNTERS:
 784                 ctrl_clear_sta_counters(wt, sock, buf + 4, len - 4);
 785                 break;
 786         case WLANTEST_CTRL_CLEAR_BSS_COUNTERS:
 787                 ctrl_clear_bss_counters(wt, sock, buf + 4, len - 4);
 788                 break;
 789         case WLANTEST_CTRL_GET_STA_COUNTER:
 790                 ctrl_get_sta_counter(wt, sock, buf + 4, len - 4);
 791                 break;
 792         case WLANTEST_CTRL_GET_BSS_COUNTER:
 793                 ctrl_get_bss_counter(wt, sock, buf + 4, len - 4);
 794                 break;
 795         case WLANTEST_CTRL_INJECT:
 796                 ctrl_inject(wt, sock, buf + 4, len - 4);
 797                 break;
 798         case WLANTEST_CTRL_VERSION:
 799                 ctrl_version(wt, sock);
 800                 break;
 801         case WLANTEST_CTRL_ADD_PASSPHRASE:
 802                 ctrl_add_passphrase(wt, sock, buf + 4, len - 4);
 803                 break;
 804         default:
 805                 ctrl_send_simple(wt, sock, WLANTEST_CTRL_UNKNOWN_CMD);
 806                 break;
 807         }
 808 }
 809
 810
 811 static void ctrl_connect(int sock, void *eloop_ctx, void *sock_ctx)
 812 {
 813         struct wlantest *wt = eloop_ctx;
 814         int conn, i;
 815
 816         conn = accept(sock, NULL, NULL);
 817         if (conn < 0) {
 818                 wpa_printf(MSG_INFO, "accept(ctrl): %s", strerror(errno));
 819                 return;
 820         }
 821         wpa_printf(MSG_MSGDUMP, "New control interface connection %d", conn);
 822
 823         for (i = 0; i < MAX_CTRL_CONNECTIONS; i++) {
 824                 if (wt->ctrl_socks[i] < 0)
 825                         break;
 826         }
 827
 828         if (i == MAX_CTRL_CONNECTIONS) {
 829                 wpa_printf(MSG_INFO, "No room for new control connection");
 830                 close(conn);
 831                 return;
 832         }
 833
 834         wt->ctrl_socks[i] = conn;
 835         eloop_register_read_sock(conn, ctrl_read, wt, NULL);
 836 }
 837
 838
 839 int ctrl_init(struct wlantest *wt)
 840 {
 841         struct sockaddr_un addr;
 842
 843         wt->ctrl_sock = socket(AF_UNIX, SOCK_SEQPACKET, 0);
 844         if (wt->ctrl_sock < 0) {
 845                 wpa_printf(MSG_ERROR, "socket: %s", strerror(errno));
 846                 return -1;
 847         }
 848
 849         os_memset(&addr, 0, sizeof(addr));
 850         addr.sun_family = AF_UNIX;
 851         os_strlcpy(addr.sun_path + 1, WLANTEST_SOCK_NAME,
 852                    sizeof(addr.sun_path) - 1);
 853         if (bind(wt->ctrl_sock, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
 854                 wpa_printf(MSG_ERROR, "bind: %s", strerror(errno));
 855                 close(wt->ctrl_sock);
 856                 wt->ctrl_sock = -1;
 857                 return -1;
 858         }
 859
 860         if (listen(wt->ctrl_sock, 5) < 0) {
 861                 wpa_printf(MSG_ERROR, "listen: %s", strerror(errno));
 862                 close(wt->ctrl_sock);
 863                 wt->ctrl_sock = -1;
 864                 return -1;
 865         }
 866
 867         if (eloop_register_read_sock(wt->ctrl_sock, ctrl_connect, wt, NULL)) {
 868                 close(wt->ctrl_sock);
 869                 wt->ctrl_sock = -1;
 870                 return -1;
 871         }
 872
 873         return 0;
 874 }
 875
 876
 877 void ctrl_deinit(struct wlantest *wt)
 878 {
 879         int i;
 880
 881         if (wt->ctrl_sock < 0)
 882                 return;
 883
 884         for (i = 0; i < MAX_CTRL_CONNECTIONS; i++) {
 885                 if (wt->ctrl_socks[i] >= 0) {
 886                         close(wt->ctrl_socks[i]);
 887                         eloop_unregister_read_sock(wt->ctrl_socks[i]);
 888                         wt->ctrl_socks[i] = -1;
 889                 }
 890         }
 891
 892         eloop_unregister_read_sock(wt->ctrl_sock);
 893         close(wt->ctrl_sock);
 894         wt->ctrl_sock = -1;
 895 }