From 981cf85a410f21d0ecee5103e73717e497156950 Mon Sep 17 00:00:00 2001 From: Jouni Malinen Date: Sun, 26 Oct 2014 12:40:10 +0200 Subject: [PATCH] nl80211: Move AP monitor interface handling to a separate file This moves the old monitor interface design to driver_nl80211_monitor.c. Signed-off-by: Jouni Malinen --- src/drivers/driver_nl80211.c | 478 +------------------------- src/drivers/driver_nl80211.h | 6 + src/drivers/driver_nl80211_monitor.c | 491 +++++++++++++++++++++++++++ src/drivers/drivers.mak | 1 + src/drivers/drivers.mk | 1 + 5 files changed, 501 insertions(+), 476 deletions(-) create mode 100644 src/drivers/driver_nl80211_monitor.c diff --git a/src/drivers/driver_nl80211.c b/src/drivers/driver_nl80211.c index c5d9c74bc..4f7a415c1 100644 --- a/src/drivers/driver_nl80211.c +++ b/src/drivers/driver_nl80211.c @@ -20,7 +20,6 @@ #endif /* CONFIG_LIBNL3_ROUTE */ #include #include -#include #include #include "common.h" @@ -159,8 +158,6 @@ wpa_driver_nl80211_finish_drv_init(struct wpa_driver_nl80211_data *drv, static int wpa_driver_nl80211_mlme(struct wpa_driver_nl80211_data *drv, const u8 *addr, int cmd, u16 reason_code, int local_state_change); -static void nl80211_remove_monitor_interface( - struct wpa_driver_nl80211_data *drv); static int nl80211_send_frame_cmd(struct i802_bss *bss, unsigned int freq, unsigned int wait, const u8 *buf, size_t buf_len, u64 *cookie, @@ -7011,63 +7008,6 @@ wpa_driver_nl80211_get_hw_feature_data(void *priv, u16 *num_modes, u16 *flags) } -static int wpa_driver_nl80211_send_mntr(struct wpa_driver_nl80211_data *drv, - const void *data, size_t len, - int encrypt, int noack) -{ - __u8 rtap_hdr[] = { - 0x00, 0x00, /* radiotap version */ - 0x0e, 0x00, /* radiotap length */ - 0x02, 0xc0, 0x00, 0x00, /* bmap: flags, tx and rx flags */ - IEEE80211_RADIOTAP_F_FRAG, /* F_FRAG (fragment if required) */ - 0x00, /* padding */ - 0x00, 0x00, /* RX and TX flags to indicate that */ - 0x00, 0x00, /* this is the injected frame directly */ - }; - struct iovec iov[2] = { - { - .iov_base = &rtap_hdr, - .iov_len = sizeof(rtap_hdr), - }, - { - .iov_base = (void *) data, - .iov_len = len, - } - }; - struct msghdr msg = { - .msg_name = NULL, - .msg_namelen = 0, - .msg_iov = iov, - .msg_iovlen = 2, - .msg_control = NULL, - .msg_controllen = 0, - .msg_flags = 0, - }; - int res; - u16 txflags = 0; - - if (encrypt) - rtap_hdr[8] |= IEEE80211_RADIOTAP_F_WEP; - - if (drv->monitor_sock < 0) { - wpa_printf(MSG_DEBUG, "nl80211: No monitor socket available " - "for %s", __func__); - return -1; - } - - if (noack) - txflags |= IEEE80211_RADIOTAP_F_TX_NOACK; - WPA_PUT_LE16(&rtap_hdr[12], txflags); - - res = sendmsg(drv->monitor_sock, &msg, 0); - if (res < 0) { - wpa_printf(MSG_INFO, "nl80211: sendmsg: %s", strerror(errno)); - return -1; - } - return 0; -} - - static int wpa_driver_nl80211_send_frame(struct i802_bss *bss, const void *data, size_t len, int encrypt, int noack, @@ -7091,10 +7031,9 @@ static int wpa_driver_nl80211_send_frame(struct i802_bss *bss, } if (drv->use_monitor) { - wpa_printf(MSG_DEBUG, "nl80211: send_frame(freq=%u bss->freq=%u) -> send_mntr", + wpa_printf(MSG_DEBUG, "nl80211: send_frame(freq=%u bss->freq=%u) -> send_monitor", freq, bss->freq); - return wpa_driver_nl80211_send_mntr(drv, data, len, - encrypt, noack); + return nl80211_send_monitor(drv, data, len, encrypt, noack); } wpa_printf(MSG_DEBUG, "nl80211: send_frame -> send_frame_cmd"); @@ -8050,419 +7989,6 @@ int nl80211_create_iface(struct wpa_driver_nl80211_data *drv, } -static void handle_tx_callback(void *ctx, u8 *buf, size_t len, int ok) -{ - struct ieee80211_hdr *hdr; - u16 fc; - union wpa_event_data event; - - hdr = (struct ieee80211_hdr *) buf; - fc = le_to_host16(hdr->frame_control); - - os_memset(&event, 0, sizeof(event)); - event.tx_status.type = WLAN_FC_GET_TYPE(fc); - event.tx_status.stype = WLAN_FC_GET_STYPE(fc); - event.tx_status.dst = hdr->addr1; - event.tx_status.data = buf; - event.tx_status.data_len = len; - event.tx_status.ack = ok; - wpa_supplicant_event(ctx, EVENT_TX_STATUS, &event); -} - - -static void from_unknown_sta(struct wpa_driver_nl80211_data *drv, - u8 *buf, size_t len) -{ - struct ieee80211_hdr *hdr = (void *)buf; - u16 fc; - union wpa_event_data event; - - if (len < sizeof(*hdr)) - return; - - fc = le_to_host16(hdr->frame_control); - - os_memset(&event, 0, sizeof(event)); - event.rx_from_unknown.bssid = get_hdr_bssid(hdr, len); - event.rx_from_unknown.addr = hdr->addr2; - event.rx_from_unknown.wds = (fc & (WLAN_FC_FROMDS | WLAN_FC_TODS)) == - (WLAN_FC_FROMDS | WLAN_FC_TODS); - wpa_supplicant_event(drv->ctx, EVENT_RX_FROM_UNKNOWN, &event); -} - - -static void handle_frame(struct wpa_driver_nl80211_data *drv, - u8 *buf, size_t len, int datarate, int ssi_signal) -{ - struct ieee80211_hdr *hdr; - u16 fc; - union wpa_event_data event; - - hdr = (struct ieee80211_hdr *) buf; - fc = le_to_host16(hdr->frame_control); - - switch (WLAN_FC_GET_TYPE(fc)) { - case WLAN_FC_TYPE_MGMT: - os_memset(&event, 0, sizeof(event)); - event.rx_mgmt.frame = buf; - event.rx_mgmt.frame_len = len; - event.rx_mgmt.datarate = datarate; - event.rx_mgmt.ssi_signal = ssi_signal; - wpa_supplicant_event(drv->ctx, EVENT_RX_MGMT, &event); - break; - case WLAN_FC_TYPE_CTRL: - /* can only get here with PS-Poll frames */ - wpa_printf(MSG_DEBUG, "CTRL"); - from_unknown_sta(drv, buf, len); - break; - case WLAN_FC_TYPE_DATA: - from_unknown_sta(drv, buf, len); - break; - } -} - - -static void handle_monitor_read(int sock, void *eloop_ctx, void *sock_ctx) -{ - struct wpa_driver_nl80211_data *drv = eloop_ctx; - int len; - unsigned char buf[3000]; - struct ieee80211_radiotap_iterator iter; - int ret; - int datarate = 0, ssi_signal = 0; - int injected = 0, failed = 0, rxflags = 0; - - len = recv(sock, buf, sizeof(buf), 0); - if (len < 0) { - wpa_printf(MSG_ERROR, "nl80211: Monitor socket recv failed: %s", - strerror(errno)); - return; - } - - if (ieee80211_radiotap_iterator_init(&iter, (void *) buf, len, NULL)) { - wpa_printf(MSG_INFO, "nl80211: received invalid radiotap frame"); - return; - } - - while (1) { - ret = ieee80211_radiotap_iterator_next(&iter); - if (ret == -ENOENT) - break; - if (ret) { - wpa_printf(MSG_INFO, "nl80211: received invalid radiotap frame (%d)", - ret); - return; - } - switch (iter.this_arg_index) { - case IEEE80211_RADIOTAP_FLAGS: - if (*iter.this_arg & IEEE80211_RADIOTAP_F_FCS) - len -= 4; - break; - case IEEE80211_RADIOTAP_RX_FLAGS: - rxflags = 1; - break; - case IEEE80211_RADIOTAP_TX_FLAGS: - injected = 1; - failed = le_to_host16((*(uint16_t *) iter.this_arg)) & - IEEE80211_RADIOTAP_F_TX_FAIL; - break; - case IEEE80211_RADIOTAP_DATA_RETRIES: - break; - case IEEE80211_RADIOTAP_CHANNEL: - /* TODO: convert from freq/flags to channel number */ - break; - case IEEE80211_RADIOTAP_RATE: - datarate = *iter.this_arg * 5; - break; - case IEEE80211_RADIOTAP_DBM_ANTSIGNAL: - ssi_signal = (s8) *iter.this_arg; - break; - } - } - - if (rxflags && injected) - return; - - if (!injected) - handle_frame(drv, buf + iter._max_length, - len - iter._max_length, datarate, ssi_signal); - else - handle_tx_callback(drv->ctx, buf + iter._max_length, - len - iter._max_length, !failed); -} - - -/* - * we post-process the filter code later and rewrite - * this to the offset to the last instruction - */ -#define PASS 0xFF -#define FAIL 0xFE - -static struct sock_filter msock_filter_insns[] = { - /* - * do a little-endian load of the radiotap length field - */ - /* load lower byte into A */ - BPF_STMT(BPF_LD | BPF_B | BPF_ABS, 2), - /* put it into X (== index register) */ - BPF_STMT(BPF_MISC| BPF_TAX, 0), - /* load upper byte into A */ - BPF_STMT(BPF_LD | BPF_B | BPF_ABS, 3), - /* left-shift it by 8 */ - BPF_STMT(BPF_ALU | BPF_LSH | BPF_K, 8), - /* or with X */ - BPF_STMT(BPF_ALU | BPF_OR | BPF_X, 0), - /* put result into X */ - BPF_STMT(BPF_MISC| BPF_TAX, 0), - - /* - * Allow management frames through, this also gives us those - * management frames that we sent ourselves with status - */ - /* load the lower byte of the IEEE 802.11 frame control field */ - BPF_STMT(BPF_LD | BPF_B | BPF_IND, 0), - /* mask off frame type and version */ - BPF_STMT(BPF_ALU | BPF_AND | BPF_K, 0xF), - /* accept frame if it's both 0, fall through otherwise */ - BPF_JUMP(BPF_JMP | BPF_JEQ | BPF_K, 0, PASS, 0), - - /* - * TODO: add a bit to radiotap RX flags that indicates - * that the sending station is not associated, then - * add a filter here that filters on our DA and that flag - * to allow us to deauth frames to that bad station. - * - * For now allow all To DS data frames through. - */ - /* load the IEEE 802.11 frame control field */ - BPF_STMT(BPF_LD | BPF_H | BPF_IND, 0), - /* mask off frame type, version and DS status */ - BPF_STMT(BPF_ALU | BPF_AND | BPF_K, 0x0F03), - /* accept frame if version 0, type 2 and To DS, fall through otherwise - */ - BPF_JUMP(BPF_JMP | BPF_JEQ | BPF_K, 0x0801, PASS, 0), - -#if 0 - /* - * drop non-data frames - */ - /* load the lower byte of the frame control field */ - BPF_STMT(BPF_LD | BPF_B | BPF_IND, 0), - /* mask off QoS bit */ - BPF_STMT(BPF_ALU | BPF_AND | BPF_K, 0x0c), - /* drop non-data frames */ - BPF_JUMP(BPF_JMP | BPF_JEQ | BPF_K, 8, 0, FAIL), -#endif - /* load the upper byte of the frame control field */ - BPF_STMT(BPF_LD | BPF_B | BPF_IND, 1), - /* mask off toDS/fromDS */ - BPF_STMT(BPF_ALU | BPF_AND | BPF_K, 0x03), - /* accept WDS frames */ - BPF_JUMP(BPF_JMP | BPF_JEQ | BPF_K, 3, PASS, 0), - - /* - * add header length to index - */ - /* load the lower byte of the frame control field */ - BPF_STMT(BPF_LD | BPF_B | BPF_IND, 0), - /* mask off QoS bit */ - BPF_STMT(BPF_ALU | BPF_AND | BPF_K, 0x80), - /* right shift it by 6 to give 0 or 2 */ - BPF_STMT(BPF_ALU | BPF_RSH | BPF_K, 6), - /* add data frame header length */ - BPF_STMT(BPF_ALU | BPF_ADD | BPF_K, 24), - /* add index, was start of 802.11 header */ - BPF_STMT(BPF_ALU | BPF_ADD | BPF_X, 0), - /* move to index, now start of LL header */ - BPF_STMT(BPF_MISC | BPF_TAX, 0), - - /* - * Accept empty data frames, we use those for - * polling activity. - */ - BPF_STMT(BPF_LD | BPF_W | BPF_LEN, 0), - BPF_JUMP(BPF_JMP | BPF_JEQ | BPF_X, 0, PASS, 0), - - /* - * Accept EAPOL frames - */ - BPF_STMT(BPF_LD | BPF_W | BPF_IND, 0), - BPF_JUMP(BPF_JMP | BPF_JEQ | BPF_K, 0xAAAA0300, 0, FAIL), - BPF_STMT(BPF_LD | BPF_W | BPF_IND, 4), - BPF_JUMP(BPF_JMP | BPF_JEQ | BPF_K, 0x0000888E, PASS, FAIL), - - /* keep these last two statements or change the code below */ - /* return 0 == "DROP" */ - BPF_STMT(BPF_RET | BPF_K, 0), - /* return ~0 == "keep all" */ - BPF_STMT(BPF_RET | BPF_K, ~0), -}; - -static struct sock_fprog msock_filter = { - .len = ARRAY_SIZE(msock_filter_insns), - .filter = msock_filter_insns, -}; - - -static int add_monitor_filter(int s) -{ - int idx; - - /* rewrite all PASS/FAIL jump offsets */ - for (idx = 0; idx < msock_filter.len; idx++) { - struct sock_filter *insn = &msock_filter_insns[idx]; - - if (BPF_CLASS(insn->code) == BPF_JMP) { - if (insn->code == (BPF_JMP|BPF_JA)) { - if (insn->k == PASS) - insn->k = msock_filter.len - idx - 2; - else if (insn->k == FAIL) - insn->k = msock_filter.len - idx - 3; - } - - if (insn->jt == PASS) - insn->jt = msock_filter.len - idx - 2; - else if (insn->jt == FAIL) - insn->jt = msock_filter.len - idx - 3; - - if (insn->jf == PASS) - insn->jf = msock_filter.len - idx - 2; - else if (insn->jf == FAIL) - insn->jf = msock_filter.len - idx - 3; - } - } - - if (setsockopt(s, SOL_SOCKET, SO_ATTACH_FILTER, - &msock_filter, sizeof(msock_filter))) { - wpa_printf(MSG_ERROR, "nl80211: setsockopt(SO_ATTACH_FILTER) failed: %s", - strerror(errno)); - return -1; - } - - return 0; -} - - -static void nl80211_remove_monitor_interface( - struct wpa_driver_nl80211_data *drv) -{ - if (drv->monitor_refcount > 0) - drv->monitor_refcount--; - wpa_printf(MSG_DEBUG, "nl80211: Remove monitor interface: refcount=%d", - drv->monitor_refcount); - if (drv->monitor_refcount > 0) - return; - - if (drv->monitor_ifidx >= 0) { - nl80211_remove_iface(drv, drv->monitor_ifidx); - drv->monitor_ifidx = -1; - } - if (drv->monitor_sock >= 0) { - eloop_unregister_read_sock(drv->monitor_sock); - close(drv->monitor_sock); - drv->monitor_sock = -1; - } -} - - -static int -nl80211_create_monitor_interface(struct wpa_driver_nl80211_data *drv) -{ - char buf[IFNAMSIZ]; - struct sockaddr_ll ll; - int optval; - socklen_t optlen; - - if (drv->monitor_ifidx >= 0) { - drv->monitor_refcount++; - wpa_printf(MSG_DEBUG, "nl80211: Re-use existing monitor interface: refcount=%d", - drv->monitor_refcount); - return 0; - } - - if (os_strncmp(drv->first_bss->ifname, "p2p-", 4) == 0) { - /* - * P2P interface name is of the format p2p-%s-%d. For monitor - * interface name corresponding to P2P GO, replace "p2p-" with - * "mon-" to retain the same interface name length and to - * indicate that it is a monitor interface. - */ - snprintf(buf, IFNAMSIZ, "mon-%s", drv->first_bss->ifname + 4); - } else { - /* Non-P2P interface with AP functionality. */ - snprintf(buf, IFNAMSIZ, "mon.%s", drv->first_bss->ifname); - } - - buf[IFNAMSIZ - 1] = '\0'; - - drv->monitor_ifidx = - nl80211_create_iface(drv, buf, NL80211_IFTYPE_MONITOR, NULL, - 0, NULL, NULL, 0); - - if (drv->monitor_ifidx == -EOPNOTSUPP) { - /* - * This is backward compatibility for a few versions of - * the kernel only that didn't advertise the right - * attributes for the only driver that then supported - * AP mode w/o monitor -- ath6kl. - */ - wpa_printf(MSG_DEBUG, "nl80211: Driver does not support " - "monitor interface type - try to run without it"); - drv->device_ap_sme = 1; - } - - if (drv->monitor_ifidx < 0) - return -1; - - if (linux_set_iface_flags(drv->global->ioctl_sock, buf, 1)) - goto error; - - memset(&ll, 0, sizeof(ll)); - ll.sll_family = AF_PACKET; - ll.sll_ifindex = drv->monitor_ifidx; - drv->monitor_sock = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL)); - if (drv->monitor_sock < 0) { - wpa_printf(MSG_ERROR, "nl80211: socket[PF_PACKET,SOCK_RAW] failed: %s", - strerror(errno)); - goto error; - } - - if (add_monitor_filter(drv->monitor_sock)) { - wpa_printf(MSG_INFO, "Failed to set socket filter for monitor " - "interface; do filtering in user space"); - /* This works, but will cost in performance. */ - } - - if (bind(drv->monitor_sock, (struct sockaddr *) &ll, sizeof(ll)) < 0) { - wpa_printf(MSG_ERROR, "nl80211: monitor socket bind failed: %s", - strerror(errno)); - goto error; - } - - optlen = sizeof(optval); - optval = 20; - if (setsockopt - (drv->monitor_sock, SOL_SOCKET, SO_PRIORITY, &optval, optlen)) { - wpa_printf(MSG_ERROR, "nl80211: Failed to set socket priority: %s", - strerror(errno)); - goto error; - } - - if (eloop_register_read_sock(drv->monitor_sock, handle_monitor_read, - drv, NULL)) { - wpa_printf(MSG_INFO, "nl80211: Could not register monitor read socket"); - goto error; - } - - drv->monitor_refcount++; - return 0; - error: - nl80211_remove_monitor_interface(drv); - return -1; -} - - static int nl80211_setup_ap(struct i802_bss *bss) { struct wpa_driver_nl80211_data *drv = bss->drv; diff --git a/src/drivers/driver_nl80211.h b/src/drivers/driver_nl80211.h index 8af502724..ff66ba969 100644 --- a/src/drivers/driver_nl80211.h +++ b/src/drivers/driver_nl80211.h @@ -210,6 +210,12 @@ int wpa_driver_nl80211_set_mode(struct i802_bss *bss, enum nl80211_iftype nlmode); void wpa_driver_nl80211_scan_timeout(void *eloop_ctx, void *timeout_ctx); +int nl80211_create_monitor_interface(struct wpa_driver_nl80211_data *drv); +void nl80211_remove_monitor_interface(struct wpa_driver_nl80211_data *drv); +int nl80211_send_monitor(struct wpa_driver_nl80211_data *drv, + const void *data, size_t len, + int encrypt, int noack); + #ifdef ANDROID int android_nl_socket_set_nonblocking(struct nl_handle *handle); int android_genl_ctrl_resolve(struct nl_handle *handle, const char *name); diff --git a/src/drivers/driver_nl80211_monitor.c b/src/drivers/driver_nl80211_monitor.c new file mode 100644 index 000000000..45385da91 --- /dev/null +++ b/src/drivers/driver_nl80211_monitor.c @@ -0,0 +1,491 @@ +/* + * Driver interaction with Linux nl80211/cfg80211 - AP monitor interface + * Copyright (c) 2002-2014, Jouni Malinen + * Copyright (c) 2003-2004, Instant802 Networks, Inc. + * Copyright (c) 2005-2006, Devicescape Software, Inc. + * Copyright (c) 2007, Johannes Berg + * Copyright (c) 2009-2010, Atheros Communications + * + * This software may be distributed under the terms of the BSD license. + * See README for more details. + */ + +#include "includes.h" +#include +#include + +#include "utils/common.h" +#include "utils/eloop.h" +#include "common/ieee802_11_defs.h" +#include "common/ieee802_11_common.h" +#include "linux_ioctl.h" +#include "radiotap_iter.h" +#include "driver_nl80211.h" + + +static void handle_tx_callback(void *ctx, u8 *buf, size_t len, int ok) +{ + struct ieee80211_hdr *hdr; + u16 fc; + union wpa_event_data event; + + hdr = (struct ieee80211_hdr *) buf; + fc = le_to_host16(hdr->frame_control); + + os_memset(&event, 0, sizeof(event)); + event.tx_status.type = WLAN_FC_GET_TYPE(fc); + event.tx_status.stype = WLAN_FC_GET_STYPE(fc); + event.tx_status.dst = hdr->addr1; + event.tx_status.data = buf; + event.tx_status.data_len = len; + event.tx_status.ack = ok; + wpa_supplicant_event(ctx, EVENT_TX_STATUS, &event); +} + + +static void from_unknown_sta(struct wpa_driver_nl80211_data *drv, + u8 *buf, size_t len) +{ + struct ieee80211_hdr *hdr = (void *)buf; + u16 fc; + union wpa_event_data event; + + if (len < sizeof(*hdr)) + return; + + fc = le_to_host16(hdr->frame_control); + + os_memset(&event, 0, sizeof(event)); + event.rx_from_unknown.bssid = get_hdr_bssid(hdr, len); + event.rx_from_unknown.addr = hdr->addr2; + event.rx_from_unknown.wds = (fc & (WLAN_FC_FROMDS | WLAN_FC_TODS)) == + (WLAN_FC_FROMDS | WLAN_FC_TODS); + wpa_supplicant_event(drv->ctx, EVENT_RX_FROM_UNKNOWN, &event); +} + + +static void handle_frame(struct wpa_driver_nl80211_data *drv, + u8 *buf, size_t len, int datarate, int ssi_signal) +{ + struct ieee80211_hdr *hdr; + u16 fc; + union wpa_event_data event; + + hdr = (struct ieee80211_hdr *) buf; + fc = le_to_host16(hdr->frame_control); + + switch (WLAN_FC_GET_TYPE(fc)) { + case WLAN_FC_TYPE_MGMT: + os_memset(&event, 0, sizeof(event)); + event.rx_mgmt.frame = buf; + event.rx_mgmt.frame_len = len; + event.rx_mgmt.datarate = datarate; + event.rx_mgmt.ssi_signal = ssi_signal; + wpa_supplicant_event(drv->ctx, EVENT_RX_MGMT, &event); + break; + case WLAN_FC_TYPE_CTRL: + /* can only get here with PS-Poll frames */ + wpa_printf(MSG_DEBUG, "CTRL"); + from_unknown_sta(drv, buf, len); + break; + case WLAN_FC_TYPE_DATA: + from_unknown_sta(drv, buf, len); + break; + } +} + + +static void handle_monitor_read(int sock, void *eloop_ctx, void *sock_ctx) +{ + struct wpa_driver_nl80211_data *drv = eloop_ctx; + int len; + unsigned char buf[3000]; + struct ieee80211_radiotap_iterator iter; + int ret; + int datarate = 0, ssi_signal = 0; + int injected = 0, failed = 0, rxflags = 0; + + len = recv(sock, buf, sizeof(buf), 0); + if (len < 0) { + wpa_printf(MSG_ERROR, "nl80211: Monitor socket recv failed: %s", + strerror(errno)); + return; + } + + if (ieee80211_radiotap_iterator_init(&iter, (void *) buf, len, NULL)) { + wpa_printf(MSG_INFO, "nl80211: received invalid radiotap frame"); + return; + } + + while (1) { + ret = ieee80211_radiotap_iterator_next(&iter); + if (ret == -ENOENT) + break; + if (ret) { + wpa_printf(MSG_INFO, "nl80211: received invalid radiotap frame (%d)", + ret); + return; + } + switch (iter.this_arg_index) { + case IEEE80211_RADIOTAP_FLAGS: + if (*iter.this_arg & IEEE80211_RADIOTAP_F_FCS) + len -= 4; + break; + case IEEE80211_RADIOTAP_RX_FLAGS: + rxflags = 1; + break; + case IEEE80211_RADIOTAP_TX_FLAGS: + injected = 1; + failed = le_to_host16((*(uint16_t *) iter.this_arg)) & + IEEE80211_RADIOTAP_F_TX_FAIL; + break; + case IEEE80211_RADIOTAP_DATA_RETRIES: + break; + case IEEE80211_RADIOTAP_CHANNEL: + /* TODO: convert from freq/flags to channel number */ + break; + case IEEE80211_RADIOTAP_RATE: + datarate = *iter.this_arg * 5; + break; + case IEEE80211_RADIOTAP_DBM_ANTSIGNAL: + ssi_signal = (s8) *iter.this_arg; + break; + } + } + + if (rxflags && injected) + return; + + if (!injected) + handle_frame(drv, buf + iter._max_length, + len - iter._max_length, datarate, ssi_signal); + else + handle_tx_callback(drv->ctx, buf + iter._max_length, + len - iter._max_length, !failed); +} + + +/* + * we post-process the filter code later and rewrite + * this to the offset to the last instruction + */ +#define PASS 0xFF +#define FAIL 0xFE + +static struct sock_filter msock_filter_insns[] = { + /* + * do a little-endian load of the radiotap length field + */ + /* load lower byte into A */ + BPF_STMT(BPF_LD | BPF_B | BPF_ABS, 2), + /* put it into X (== index register) */ + BPF_STMT(BPF_MISC| BPF_TAX, 0), + /* load upper byte into A */ + BPF_STMT(BPF_LD | BPF_B | BPF_ABS, 3), + /* left-shift it by 8 */ + BPF_STMT(BPF_ALU | BPF_LSH | BPF_K, 8), + /* or with X */ + BPF_STMT(BPF_ALU | BPF_OR | BPF_X, 0), + /* put result into X */ + BPF_STMT(BPF_MISC| BPF_TAX, 0), + + /* + * Allow management frames through, this also gives us those + * management frames that we sent ourselves with status + */ + /* load the lower byte of the IEEE 802.11 frame control field */ + BPF_STMT(BPF_LD | BPF_B | BPF_IND, 0), + /* mask off frame type and version */ + BPF_STMT(BPF_ALU | BPF_AND | BPF_K, 0xF), + /* accept frame if it's both 0, fall through otherwise */ + BPF_JUMP(BPF_JMP | BPF_JEQ | BPF_K, 0, PASS, 0), + + /* + * TODO: add a bit to radiotap RX flags that indicates + * that the sending station is not associated, then + * add a filter here that filters on our DA and that flag + * to allow us to deauth frames to that bad station. + * + * For now allow all To DS data frames through. + */ + /* load the IEEE 802.11 frame control field */ + BPF_STMT(BPF_LD | BPF_H | BPF_IND, 0), + /* mask off frame type, version and DS status */ + BPF_STMT(BPF_ALU | BPF_AND | BPF_K, 0x0F03), + /* accept frame if version 0, type 2 and To DS, fall through otherwise + */ + BPF_JUMP(BPF_JMP | BPF_JEQ | BPF_K, 0x0801, PASS, 0), + +#if 0 + /* + * drop non-data frames + */ + /* load the lower byte of the frame control field */ + BPF_STMT(BPF_LD | BPF_B | BPF_IND, 0), + /* mask off QoS bit */ + BPF_STMT(BPF_ALU | BPF_AND | BPF_K, 0x0c), + /* drop non-data frames */ + BPF_JUMP(BPF_JMP | BPF_JEQ | BPF_K, 8, 0, FAIL), +#endif + /* load the upper byte of the frame control field */ + BPF_STMT(BPF_LD | BPF_B | BPF_IND, 1), + /* mask off toDS/fromDS */ + BPF_STMT(BPF_ALU | BPF_AND | BPF_K, 0x03), + /* accept WDS frames */ + BPF_JUMP(BPF_JMP | BPF_JEQ | BPF_K, 3, PASS, 0), + + /* + * add header length to index + */ + /* load the lower byte of the frame control field */ + BPF_STMT(BPF_LD | BPF_B | BPF_IND, 0), + /* mask off QoS bit */ + BPF_STMT(BPF_ALU | BPF_AND | BPF_K, 0x80), + /* right shift it by 6 to give 0 or 2 */ + BPF_STMT(BPF_ALU | BPF_RSH | BPF_K, 6), + /* add data frame header length */ + BPF_STMT(BPF_ALU | BPF_ADD | BPF_K, 24), + /* add index, was start of 802.11 header */ + BPF_STMT(BPF_ALU | BPF_ADD | BPF_X, 0), + /* move to index, now start of LL header */ + BPF_STMT(BPF_MISC | BPF_TAX, 0), + + /* + * Accept empty data frames, we use those for + * polling activity. + */ + BPF_STMT(BPF_LD | BPF_W | BPF_LEN, 0), + BPF_JUMP(BPF_JMP | BPF_JEQ | BPF_X, 0, PASS, 0), + + /* + * Accept EAPOL frames + */ + BPF_STMT(BPF_LD | BPF_W | BPF_IND, 0), + BPF_JUMP(BPF_JMP | BPF_JEQ | BPF_K, 0xAAAA0300, 0, FAIL), + BPF_STMT(BPF_LD | BPF_W | BPF_IND, 4), + BPF_JUMP(BPF_JMP | BPF_JEQ | BPF_K, 0x0000888E, PASS, FAIL), + + /* keep these last two statements or change the code below */ + /* return 0 == "DROP" */ + BPF_STMT(BPF_RET | BPF_K, 0), + /* return ~0 == "keep all" */ + BPF_STMT(BPF_RET | BPF_K, ~0), +}; + +static struct sock_fprog msock_filter = { + .len = ARRAY_SIZE(msock_filter_insns), + .filter = msock_filter_insns, +}; + + +static int add_monitor_filter(int s) +{ + int idx; + + /* rewrite all PASS/FAIL jump offsets */ + for (idx = 0; idx < msock_filter.len; idx++) { + struct sock_filter *insn = &msock_filter_insns[idx]; + + if (BPF_CLASS(insn->code) == BPF_JMP) { + if (insn->code == (BPF_JMP|BPF_JA)) { + if (insn->k == PASS) + insn->k = msock_filter.len - idx - 2; + else if (insn->k == FAIL) + insn->k = msock_filter.len - idx - 3; + } + + if (insn->jt == PASS) + insn->jt = msock_filter.len - idx - 2; + else if (insn->jt == FAIL) + insn->jt = msock_filter.len - idx - 3; + + if (insn->jf == PASS) + insn->jf = msock_filter.len - idx - 2; + else if (insn->jf == FAIL) + insn->jf = msock_filter.len - idx - 3; + } + } + + if (setsockopt(s, SOL_SOCKET, SO_ATTACH_FILTER, + &msock_filter, sizeof(msock_filter))) { + wpa_printf(MSG_ERROR, "nl80211: setsockopt(SO_ATTACH_FILTER) failed: %s", + strerror(errno)); + return -1; + } + + return 0; +} + + +void nl80211_remove_monitor_interface(struct wpa_driver_nl80211_data *drv) +{ + if (drv->monitor_refcount > 0) + drv->monitor_refcount--; + wpa_printf(MSG_DEBUG, "nl80211: Remove monitor interface: refcount=%d", + drv->monitor_refcount); + if (drv->monitor_refcount > 0) + return; + + if (drv->monitor_ifidx >= 0) { + nl80211_remove_iface(drv, drv->monitor_ifidx); + drv->monitor_ifidx = -1; + } + if (drv->monitor_sock >= 0) { + eloop_unregister_read_sock(drv->monitor_sock); + close(drv->monitor_sock); + drv->monitor_sock = -1; + } +} + + +int nl80211_create_monitor_interface(struct wpa_driver_nl80211_data *drv) +{ + char buf[IFNAMSIZ]; + struct sockaddr_ll ll; + int optval; + socklen_t optlen; + + if (drv->monitor_ifidx >= 0) { + drv->monitor_refcount++; + wpa_printf(MSG_DEBUG, "nl80211: Re-use existing monitor interface: refcount=%d", + drv->monitor_refcount); + return 0; + } + + if (os_strncmp(drv->first_bss->ifname, "p2p-", 4) == 0) { + /* + * P2P interface name is of the format p2p-%s-%d. For monitor + * interface name corresponding to P2P GO, replace "p2p-" with + * "mon-" to retain the same interface name length and to + * indicate that it is a monitor interface. + */ + snprintf(buf, IFNAMSIZ, "mon-%s", drv->first_bss->ifname + 4); + } else { + /* Non-P2P interface with AP functionality. */ + snprintf(buf, IFNAMSIZ, "mon.%s", drv->first_bss->ifname); + } + + buf[IFNAMSIZ - 1] = '\0'; + + drv->monitor_ifidx = + nl80211_create_iface(drv, buf, NL80211_IFTYPE_MONITOR, NULL, + 0, NULL, NULL, 0); + + if (drv->monitor_ifidx == -EOPNOTSUPP) { + /* + * This is backward compatibility for a few versions of + * the kernel only that didn't advertise the right + * attributes for the only driver that then supported + * AP mode w/o monitor -- ath6kl. + */ + wpa_printf(MSG_DEBUG, "nl80211: Driver does not support " + "monitor interface type - try to run without it"); + drv->device_ap_sme = 1; + } + + if (drv->monitor_ifidx < 0) + return -1; + + if (linux_set_iface_flags(drv->global->ioctl_sock, buf, 1)) + goto error; + + memset(&ll, 0, sizeof(ll)); + ll.sll_family = AF_PACKET; + ll.sll_ifindex = drv->monitor_ifidx; + drv->monitor_sock = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL)); + if (drv->monitor_sock < 0) { + wpa_printf(MSG_ERROR, "nl80211: socket[PF_PACKET,SOCK_RAW] failed: %s", + strerror(errno)); + goto error; + } + + if (add_monitor_filter(drv->monitor_sock)) { + wpa_printf(MSG_INFO, "Failed to set socket filter for monitor " + "interface; do filtering in user space"); + /* This works, but will cost in performance. */ + } + + if (bind(drv->monitor_sock, (struct sockaddr *) &ll, sizeof(ll)) < 0) { + wpa_printf(MSG_ERROR, "nl80211: monitor socket bind failed: %s", + strerror(errno)); + goto error; + } + + optlen = sizeof(optval); + optval = 20; + if (setsockopt + (drv->monitor_sock, SOL_SOCKET, SO_PRIORITY, &optval, optlen)) { + wpa_printf(MSG_ERROR, "nl80211: Failed to set socket priority: %s", + strerror(errno)); + goto error; + } + + if (eloop_register_read_sock(drv->monitor_sock, handle_monitor_read, + drv, NULL)) { + wpa_printf(MSG_INFO, "nl80211: Could not register monitor read socket"); + goto error; + } + + drv->monitor_refcount++; + return 0; + error: + nl80211_remove_monitor_interface(drv); + return -1; +} + + +int nl80211_send_monitor(struct wpa_driver_nl80211_data *drv, + const void *data, size_t len, + int encrypt, int noack) +{ + __u8 rtap_hdr[] = { + 0x00, 0x00, /* radiotap version */ + 0x0e, 0x00, /* radiotap length */ + 0x02, 0xc0, 0x00, 0x00, /* bmap: flags, tx and rx flags */ + IEEE80211_RADIOTAP_F_FRAG, /* F_FRAG (fragment if required) */ + 0x00, /* padding */ + 0x00, 0x00, /* RX and TX flags to indicate that */ + 0x00, 0x00, /* this is the injected frame directly */ + }; + struct iovec iov[2] = { + { + .iov_base = &rtap_hdr, + .iov_len = sizeof(rtap_hdr), + }, + { + .iov_base = (void *) data, + .iov_len = len, + } + }; + struct msghdr msg = { + .msg_name = NULL, + .msg_namelen = 0, + .msg_iov = iov, + .msg_iovlen = 2, + .msg_control = NULL, + .msg_controllen = 0, + .msg_flags = 0, + }; + int res; + u16 txflags = 0; + + if (encrypt) + rtap_hdr[8] |= IEEE80211_RADIOTAP_F_WEP; + + if (drv->monitor_sock < 0) { + wpa_printf(MSG_DEBUG, "nl80211: No monitor socket available " + "for %s", __func__); + return -1; + } + + if (noack) + txflags |= IEEE80211_RADIOTAP_F_TX_NOACK; + WPA_PUT_LE16(&rtap_hdr[12], txflags); + + res = sendmsg(drv->monitor_sock, &msg, 0); + if (res < 0) { + wpa_printf(MSG_INFO, "nl80211: sendmsg: %s", strerror(errno)); + return -1; + } + return 0; +} diff --git a/src/drivers/drivers.mak b/src/drivers/drivers.mak index 2048b2d94..e9ec93c64 100644 --- a/src/drivers/drivers.mak +++ b/src/drivers/drivers.mak @@ -25,6 +25,7 @@ endif ifdef CONFIG_DRIVER_NL80211 DRV_CFLAGS += -DCONFIG_DRIVER_NL80211 DRV_OBJS += ../src/drivers/driver_nl80211.o +DRV_OBJS += ../src/drivers/driver_nl80211_monitor.o DRV_OBJS += ../src/utils/radiotap.o NEED_SME=y NEED_AP_MLME=y diff --git a/src/drivers/drivers.mk b/src/drivers/drivers.mk index 762d21cac..fe048a64a 100644 --- a/src/drivers/drivers.mk +++ b/src/drivers/drivers.mk @@ -21,6 +21,7 @@ ifdef CONFIG_DRIVER_NL80211 DRV_CFLAGS += -DCONFIG_DRIVER_NL80211 DRV_OBJS += src/drivers/driver_nl80211.c DRV_OBJS += src/drivers/driver_nl80211_android.c +DRV_OBJS += src/drivers/driver_nl80211_monitor.c DRV_OBJS += src/utils/radiotap.c NEED_SME=y NEED_AP_MLME=y