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