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