src/drivers/driver_nl80211_monitor.c - platform/external/wpa_supplicant_8 - Git at Google

 /*
  * Driver interaction with Linux nl80211/cfg80211 - AP monitor interface
  * Copyright (c) 2002-2014, Jouni Malinen <j@w1.fi>
  * Copyright (c) 2003-2004, Instant802 Networks, Inc.
  * Copyright (c) 2005-2006, Devicescape Software, Inc.
  * Copyright (c) 2007, Johannes Berg <johannes@sipsolutions.net>
  * 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 <netpacket/packet.h>
 #include <linux/filter.h>

 #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;
 }
	/*
	* Driver interaction with Linux nl80211/cfg80211 - AP monitor interface
	* Copyright (c) 2002-2014, Jouni Malinen <j@w1.fi>
	* Copyright (c) 2003-2004, Instant802 Networks, Inc.
	* Copyright (c) 2005-2006, Devicescape Software, Inc.
	* Copyright (c) 2007, Johannes Berg <johannes@sipsolutions.net>
	* 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 <netpacket/packet.h>
	#include <linux/filter.h>

	#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;
	}