src/drivers/driver_nl80211_monitor.c

   1 /*
   2  * Driver interaction with Linux nl80211/cfg80211 - AP monitor interface
   3  * Copyright (c) 2002-2014, Jouni Malinen <j@w1.fi>
   4  * Copyright (c) 2003-2004, Instant802 Networks, Inc.
   5  * Copyright (c) 2005-2006, Devicescape Software, Inc.
   6  * Copyright (c) 2007, Johannes Berg <johannes@sipsolutions.net>
   7  * Copyright (c) 2009-2010, Atheros Communications
   8  *
   9  * This software may be distributed under the terms of the BSD license.
  10  * See README for more details.
  11  */
  12
  13 #include "includes.h"
  14 #include <netpacket/packet.h>
  15 #include <linux/filter.h>
  16
  17 #include "utils/common.h"
  18 #include "utils/eloop.h"
  19 #include "common/ieee802_11_defs.h"
  20 #include "common/ieee802_11_common.h"
  21 #include "linux_ioctl.h"
  22 #include "radiotap_iter.h"
  23 #include "driver_nl80211.h"
  24
  25
  26 static void handle_tx_callback(void *ctx, u8 *buf, size_t len, int ok)
  27 {
  28         struct ieee80211_hdr *hdr;
  29         u16 fc;
  30         union wpa_event_data event;
  31
  32         hdr = (struct ieee80211_hdr *) buf;
  33         fc = le_to_host16(hdr->frame_control);
  34
  35         os_memset(&event, 0, sizeof(event));
  36         event.tx_status.type = WLAN_FC_GET_TYPE(fc);
  37         event.tx_status.stype = WLAN_FC_GET_STYPE(fc);
  38         event.tx_status.dst = hdr->addr1;
  39         event.tx_status.data = buf;
  40         event.tx_status.data_len = len;
  41         event.tx_status.ack = ok;
  42         wpa_supplicant_event(ctx, EVENT_TX_STATUS, &event);
  43 }
  44
  45
  46 static void from_unknown_sta(struct wpa_driver_nl80211_data *drv,
  47                              u8 *buf, size_t len)
  48 {
  49         struct ieee80211_hdr *hdr = (void *)buf;
  50         u16 fc;
  51         union wpa_event_data event;
  52
  53         if (len < sizeof(*hdr))
  54                 return;
  55
  56         fc = le_to_host16(hdr->frame_control);
  57
  58         os_memset(&event, 0, sizeof(event));
  59         event.rx_from_unknown.bssid = get_hdr_bssid(hdr, len);
  60         event.rx_from_unknown.addr = hdr->addr2;
  61         event.rx_from_unknown.wds = (fc & (WLAN_FC_FROMDS | WLAN_FC_TODS)) ==
  62                 (WLAN_FC_FROMDS | WLAN_FC_TODS);
  63         wpa_supplicant_event(drv->ctx, EVENT_RX_FROM_UNKNOWN, &event);
  64 }
  65
  66
  67 static void handle_frame(struct wpa_driver_nl80211_data *drv,
  68                          u8 *buf, size_t len, int datarate, int ssi_signal)
  69 {
  70         struct ieee80211_hdr *hdr;
  71         u16 fc;
  72         union wpa_event_data event;
  73
  74         hdr = (struct ieee80211_hdr *) buf;
  75         fc = le_to_host16(hdr->frame_control);
  76
  77         switch (WLAN_FC_GET_TYPE(fc)) {
  78         case WLAN_FC_TYPE_MGMT:
  79                 os_memset(&event, 0, sizeof(event));
  80                 event.rx_mgmt.frame = buf;
  81                 event.rx_mgmt.frame_len = len;
  82                 event.rx_mgmt.datarate = datarate;
  83                 event.rx_mgmt.ssi_signal = ssi_signal;
  84                 wpa_supplicant_event(drv->ctx, EVENT_RX_MGMT, &event);
  85                 break;
  86         case WLAN_FC_TYPE_CTRL:
  87                 /* can only get here with PS-Poll frames */
  88                 wpa_printf(MSG_DEBUG, "CTRL");
  89                 from_unknown_sta(drv, buf, len);
  90                 break;
  91         case WLAN_FC_TYPE_DATA:
  92                 from_unknown_sta(drv, buf, len);
  93                 break;
  94         }
  95 }
  96
  97
  98 static void handle_monitor_read(int sock, void *eloop_ctx, void *sock_ctx)
  99 {
 100         struct wpa_driver_nl80211_data *drv = eloop_ctx;
 101         int len;
 102         unsigned char buf[3000];
 103         struct ieee80211_radiotap_iterator iter;
 104         int ret;
 105         int datarate = 0, ssi_signal = 0;
 106         int injected = 0, failed = 0, rxflags = 0;
 107
 108         len = recv(sock, buf, sizeof(buf), 0);
 109         if (len < 0) {
 110                 wpa_printf(MSG_ERROR, "nl80211: Monitor socket recv failed: %s",
 111                            strerror(errno));
 112                 return;
 113         }
 114
 115         if (ieee80211_radiotap_iterator_init(&iter, (void *) buf, len, NULL)) {
 116                 wpa_printf(MSG_INFO, "nl80211: received invalid radiotap frame");
 117                 return;
 118         }
 119
 120         while (1) {
 121                 ret = ieee80211_radiotap_iterator_next(&iter);
 122                 if (ret == -ENOENT)
 123                         break;
 124                 if (ret) {
 125                         wpa_printf(MSG_INFO, "nl80211: received invalid radiotap frame (%d)",
 126                                    ret);
 127                         return;
 128                 }
 129                 switch (iter.this_arg_index) {
 130                 case IEEE80211_RADIOTAP_FLAGS:
 131                         if (*iter.this_arg & IEEE80211_RADIOTAP_F_FCS)
 132                                 len -= 4;
 133                         break;
 134                 case IEEE80211_RADIOTAP_RX_FLAGS:
 135                         rxflags = 1;
 136                         break;
 137                 case IEEE80211_RADIOTAP_TX_FLAGS:
 138                         injected = 1;
 139                         failed = le_to_host16((*(le16 *) iter.this_arg)) &
 140                                         IEEE80211_RADIOTAP_F_TX_FAIL;
 141                         break;
 142                 case IEEE80211_RADIOTAP_DATA_RETRIES:
 143                         break;
 144                 case IEEE80211_RADIOTAP_CHANNEL:
 145                         /* TODO: convert from freq/flags to channel number */
 146                         break;
 147                 case IEEE80211_RADIOTAP_RATE:
 148                         datarate = *iter.this_arg * 5;
 149                         break;
 150                 case IEEE80211_RADIOTAP_DBM_ANTSIGNAL:
 151                         ssi_signal = (s8) *iter.this_arg;
 152                         break;
 153                 }
 154         }
 155
 156         if (rxflags && injected)
 157                 return;
 158
 159         if (!injected)
 160                 handle_frame(drv, buf + iter._max_length,
 161                              len - iter._max_length, datarate, ssi_signal);
 162         else
 163                 handle_tx_callback(drv->ctx, buf + iter._max_length,
 164                                    len - iter._max_length, !failed);
 165 }
 166
 167
 168 /*
 169  * we post-process the filter code later and rewrite
 170  * this to the offset to the last instruction
 171  */
 172 #define PASS    0xFF
 173 #define FAIL    0xFE
 174
 175 static struct sock_filter msock_filter_insns[] = {
 176         /*
 177          * do a little-endian load of the radiotap length field
 178          */
 179         /* load lower byte into A */
 180         BPF_STMT(BPF_LD  | BPF_B | BPF_ABS, 2),
 181         /* put it into X (== index register) */
 182         BPF_STMT(BPF_MISC| BPF_TAX, 0),
 183         /* load upper byte into A */
 184         BPF_STMT(BPF_LD  | BPF_B | BPF_ABS, 3),
 185         /* left-shift it by 8 */
 186         BPF_STMT(BPF_ALU | BPF_LSH | BPF_K, 8),
 187         /* or with X */
 188         BPF_STMT(BPF_ALU | BPF_OR | BPF_X, 0),
 189         /* put result into X */
 190         BPF_STMT(BPF_MISC| BPF_TAX, 0),
 191
 192         /*
 193          * Allow management frames through, this also gives us those
 194          * management frames that we sent ourselves with status
 195          */
 196         /* load the lower byte of the IEEE 802.11 frame control field */
 197         BPF_STMT(BPF_LD  | BPF_B | BPF_IND, 0),
 198         /* mask off frame type and version */
 199         BPF_STMT(BPF_ALU | BPF_AND | BPF_K, 0xF),
 200         /* accept frame if it's both 0, fall through otherwise */
 201         BPF_JUMP(BPF_JMP | BPF_JEQ | BPF_K, 0, PASS, 0),
 202
 203         /*
 204          * TODO: add a bit to radiotap RX flags that indicates
 205          * that the sending station is not associated, then
 206          * add a filter here that filters on our DA and that flag
 207          * to allow us to deauth frames to that bad station.
 208          *
 209          * For now allow all To DS data frames through.
 210          */
 211         /* load the IEEE 802.11 frame control field */
 212         BPF_STMT(BPF_LD  | BPF_H | BPF_IND, 0),
 213         /* mask off frame type, version and DS status */
 214         BPF_STMT(BPF_ALU | BPF_AND | BPF_K, 0x0F03),
 215         /* accept frame if version 0, type 2 and To DS, fall through otherwise
 216          */
 217         BPF_JUMP(BPF_JMP | BPF_JEQ | BPF_K, 0x0801, PASS, 0),
 218
 219 #if 0
 220         /*
 221          * drop non-data frames
 222          */
 223         /* load the lower byte of the frame control field */
 224         BPF_STMT(BPF_LD   | BPF_B | BPF_IND, 0),
 225         /* mask off QoS bit */
 226         BPF_STMT(BPF_ALU  | BPF_AND | BPF_K, 0x0c),
 227         /* drop non-data frames */
 228         BPF_JUMP(BPF_JMP  | BPF_JEQ | BPF_K, 8, 0, FAIL),
 229 #endif
 230         /* load the upper byte of the frame control field */
 231         BPF_STMT(BPF_LD   | BPF_B | BPF_IND, 1),
 232         /* mask off toDS/fromDS */
 233         BPF_STMT(BPF_ALU  | BPF_AND | BPF_K, 0x03),
 234         /* accept WDS frames */
 235         BPF_JUMP(BPF_JMP  | BPF_JEQ | BPF_K, 3, PASS, 0),
 236
 237         /*
 238          * add header length to index
 239          */
 240         /* load the lower byte of the frame control field */
 241         BPF_STMT(BPF_LD   | BPF_B | BPF_IND, 0),
 242         /* mask off QoS bit */
 243         BPF_STMT(BPF_ALU  | BPF_AND | BPF_K, 0x80),
 244         /* right shift it by 6 to give 0 or 2 */
 245         BPF_STMT(BPF_ALU  | BPF_RSH | BPF_K, 6),
 246         /* add data frame header length */
 247         BPF_STMT(BPF_ALU  | BPF_ADD | BPF_K, 24),
 248         /* add index, was start of 802.11 header */
 249         BPF_STMT(BPF_ALU  | BPF_ADD | BPF_X, 0),
 250         /* move to index, now start of LL header */
 251         BPF_STMT(BPF_MISC | BPF_TAX, 0),
 252
 253         /*
 254          * Accept empty data frames, we use those for
 255          * polling activity.
 256          */
 257         BPF_STMT(BPF_LD  | BPF_W | BPF_LEN, 0),
 258         BPF_JUMP(BPF_JMP | BPF_JEQ | BPF_X, 0, PASS, 0),
 259
 260         /*
 261          * Accept EAPOL frames
 262          */
 263         BPF_STMT(BPF_LD  | BPF_W | BPF_IND, 0),
 264         BPF_JUMP(BPF_JMP | BPF_JEQ | BPF_K, 0xAAAA0300, 0, FAIL),
 265         BPF_STMT(BPF_LD  | BPF_W | BPF_IND, 4),
 266         BPF_JUMP(BPF_JMP | BPF_JEQ | BPF_K, 0x0000888E, PASS, FAIL),
 267
 268         /* keep these last two statements or change the code below */
 269         /* return 0 == "DROP" */
 270         BPF_STMT(BPF_RET | BPF_K, 0),
 271         /* return ~0 == "keep all" */
 272         BPF_STMT(BPF_RET | BPF_K, ~0),
 273 };
 274
 275 static struct sock_fprog msock_filter = {
 276         .len = ARRAY_SIZE(msock_filter_insns),
 277         .filter = msock_filter_insns,
 278 };
 279
 280
 281 static int add_monitor_filter(int s)
 282 {
 283         int idx;
 284
 285         /* rewrite all PASS/FAIL jump offsets */
 286         for (idx = 0; idx < msock_filter.len; idx++) {
 287                 struct sock_filter *insn = &msock_filter_insns[idx];
 288
 289                 if (BPF_CLASS(insn->code) == BPF_JMP) {
 290                         if (insn->code == (BPF_JMP|BPF_JA)) {
 291                                 if (insn->k == PASS)
 292                                         insn->k = msock_filter.len - idx - 2;
 293                                 else if (insn->k == FAIL)
 294                                         insn->k = msock_filter.len - idx - 3;
 295                         }
 296
 297                         if (insn->jt == PASS)
 298                                 insn->jt = msock_filter.len - idx - 2;
 299                         else if (insn->jt == FAIL)
 300                                 insn->jt = msock_filter.len - idx - 3;
 301
 302                         if (insn->jf == PASS)
 303                                 insn->jf = msock_filter.len - idx - 2;
 304                         else if (insn->jf == FAIL)
 305                                 insn->jf = msock_filter.len - idx - 3;
 306                 }
 307         }
 308
 309         if (setsockopt(s, SOL_SOCKET, SO_ATTACH_FILTER,
 310                        &msock_filter, sizeof(msock_filter))) {
 311                 wpa_printf(MSG_ERROR, "nl80211: setsockopt(SO_ATTACH_FILTER) failed: %s",
 312                            strerror(errno));
 313                 return -1;
 314         }
 315
 316         return 0;
 317 }
 318
 319
 320 void nl80211_remove_monitor_interface(struct wpa_driver_nl80211_data *drv)
 321 {
 322         if (drv->monitor_refcount > 0)
 323                 drv->monitor_refcount--;
 324         wpa_printf(MSG_DEBUG, "nl80211: Remove monitor interface: refcount=%d",
 325                    drv->monitor_refcount);
 326         if (drv->monitor_refcount > 0)
 327                 return;
 328
 329         if (drv->monitor_ifidx >= 0) {
 330                 nl80211_remove_iface(drv, drv->monitor_ifidx);
 331                 drv->monitor_ifidx = -1;
 332         }
 333         if (drv->monitor_sock >= 0) {
 334                 eloop_unregister_read_sock(drv->monitor_sock);
 335                 close(drv->monitor_sock);
 336                 drv->monitor_sock = -1;
 337         }
 338 }
 339
 340
 341 int nl80211_create_monitor_interface(struct wpa_driver_nl80211_data *drv)
 342 {
 343         char buf[IFNAMSIZ];
 344         struct sockaddr_ll ll;
 345         int optval;
 346         socklen_t optlen;
 347
 348         if (drv->monitor_ifidx >= 0) {
 349                 drv->monitor_refcount++;
 350                 wpa_printf(MSG_DEBUG, "nl80211: Re-use existing monitor interface: refcount=%d",
 351                            drv->monitor_refcount);
 352                 return 0;
 353         }
 354
 355         if (os_strncmp(drv->first_bss->ifname, "p2p-", 4) == 0) {
 356                 /*
 357                  * P2P interface name is of the format p2p-%s-%d. For monitor
 358                  * interface name corresponding to P2P GO, replace "p2p-" with
 359                  * "mon-" to retain the same interface name length and to
 360                  * indicate that it is a monitor interface.
 361                  */
 362                 snprintf(buf, IFNAMSIZ, "mon-%s", drv->first_bss->ifname + 4);
 363         } else {
 364                 int ret;
 365
 366                 /* Non-P2P interface with AP functionality. */
 367                 ret = os_snprintf(buf, IFNAMSIZ, "mon.%s",
 368                                   drv->first_bss->ifname);
 369                 if (ret >= (int) sizeof(buf))
 370                         wpa_printf(MSG_DEBUG,
 371                                    "nl80211: Monitor interface name has been truncated to %s",
 372                                    buf);
 373                 else if (ret < 0)
 374                         return ret;
 375         }
 376
 377         buf[IFNAMSIZ - 1] = '\0';
 378
 379         drv->monitor_ifidx =
 380                 nl80211_create_iface(drv, buf, NL80211_IFTYPE_MONITOR, NULL,
 381                                      0, NULL, NULL, 0);
 382
 383         if (drv->monitor_ifidx == -EOPNOTSUPP) {
 384                 /*
 385                  * This is backward compatibility for a few versions of
 386                  * the kernel only that didn't advertise the right
 387                  * attributes for the only driver that then supported
 388                  * AP mode w/o monitor -- ath6kl.
 389                  */
 390                 wpa_printf(MSG_DEBUG, "nl80211: Driver does not support "
 391                            "monitor interface type - try to run without it");
 392                 drv->device_ap_sme = 1;
 393         }
 394
 395         if (drv->monitor_ifidx < 0)
 396                 return -1;
 397
 398         if (linux_set_iface_flags(drv->global->ioctl_sock, buf, 1))
 399                 goto error;
 400
 401         memset(&ll, 0, sizeof(ll));
 402         ll.sll_family = AF_PACKET;
 403         ll.sll_ifindex = drv->monitor_ifidx;
 404         drv->monitor_sock = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
 405         if (drv->monitor_sock < 0) {
 406                 wpa_printf(MSG_ERROR, "nl80211: socket[PF_PACKET,SOCK_RAW] failed: %s",
 407                            strerror(errno));
 408                 goto error;
 409         }
 410
 411         if (add_monitor_filter(drv->monitor_sock)) {
 412                 wpa_printf(MSG_INFO, "Failed to set socket filter for monitor "
 413                            "interface; do filtering in user space");
 414                 /* This works, but will cost in performance. */
 415         }
 416
 417         if (bind(drv->monitor_sock, (struct sockaddr *) &ll, sizeof(ll)) < 0) {
 418                 wpa_printf(MSG_ERROR, "nl80211: monitor socket bind failed: %s",
 419                            strerror(errno));
 420                 goto error;
 421         }
 422
 423         optlen = sizeof(optval);
 424         optval = 20;
 425         if (setsockopt
 426             (drv->monitor_sock, SOL_SOCKET, SO_PRIORITY, &optval, optlen)) {
 427                 wpa_printf(MSG_ERROR, "nl80211: Failed to set socket priority: %s",
 428                            strerror(errno));
 429                 goto error;
 430         }
 431
 432         if (eloop_register_read_sock(drv->monitor_sock, handle_monitor_read,
 433                                      drv, NULL)) {
 434                 wpa_printf(MSG_INFO, "nl80211: Could not register monitor read socket");
 435                 goto error;
 436         }
 437
 438         drv->monitor_refcount++;
 439         return 0;
 440  error:
 441         nl80211_remove_monitor_interface(drv);
 442         return -1;
 443 }
 444
 445
 446 int nl80211_send_monitor(struct wpa_driver_nl80211_data *drv,
 447                          const void *data, size_t len,
 448                          int encrypt, int noack)
 449 {
 450         __u8 rtap_hdr[] = {
 451                 0x00, 0x00, /* radiotap version */
 452                 0x0e, 0x00, /* radiotap length */
 453                 0x02, 0xc0, 0x00, 0x00, /* bmap: flags, tx and rx flags */
 454                 IEEE80211_RADIOTAP_F_FRAG, /* F_FRAG (fragment if required) */
 455                 0x00,       /* padding */
 456                 0x00, 0x00, /* RX and TX flags to indicate that */
 457                 0x00, 0x00, /* this is the injected frame directly */
 458         };
 459         struct iovec iov[2] = {
 460                 {
 461                         .iov_base = &rtap_hdr,
 462                         .iov_len = sizeof(rtap_hdr),
 463                 },
 464                 {
 465                         .iov_base = (void *) data,
 466                         .iov_len = len,
 467                 }
 468         };
 469         struct msghdr msg = {
 470                 .msg_name = NULL,
 471                 .msg_namelen = 0,
 472                 .msg_iov = iov,
 473                 .msg_iovlen = 2,
 474                 .msg_control = NULL,
 475                 .msg_controllen = 0,
 476                 .msg_flags = 0,
 477         };
 478         int res;
 479         u16 txflags = 0;
 480
 481         if (encrypt)
 482                 rtap_hdr[8] |= IEEE80211_RADIOTAP_F_WEP;
 483
 484         if (drv->monitor_sock < 0) {
 485                 wpa_printf(MSG_DEBUG, "nl80211: No monitor socket available "
 486                            "for %s", __func__);
 487                 return -1;
 488         }
 489
 490         if (noack)
 491                 txflags |= IEEE80211_RADIOTAP_F_TX_NOACK;
 492         WPA_PUT_LE16(&rtap_hdr[12], txflags);
 493
 494         res = sendmsg(drv->monitor_sock, &msg, 0);
 495         if (res < 0) {
 496                 wpa_printf(MSG_INFO, "nl80211: sendmsg: %s", strerror(errno));
 497                 return -1;
 498         }
 499         return 0;
 500 }