Radiotap injection for any Mode

Index: wireless-dev/include/net/cfg80211.h
===================================================================
@@ -75,6 +75,44 @@ struct key_params {
 	u32 cipher;
 };
 
+
+/* Radiotap header iteration
+ *   implemented in net/wireless/radiotap.c
+ *   docs in Documentation/networking/radiotap-headers.txt
+ */
+/**
+ * struct ieee80211_radiotap_iterator - tracks walk thru present radiotap args
+ * @rtheader: pointer to the radiotap header we are walking through
+ * @max_length: length of radiotap header in cpu byte ordering
+ * @this_arg_index: IEEE80211_RADIOTAP_... index of current arg
+ * @this_arg: pointer to current radiotap arg
+ * @arg_index: internal next argument index
+ * @arg: internal next argument pointer
+ * @next_bitmap: internal pointer to next present u32
+ * @bitmap_shifter: internal shifter for curr u32 bitmap, b0 set == arg present
+ */
+
+struct ieee80211_radiotap_iterator {
+	struct ieee80211_radiotap_header *rtheader;
+	int max_length;
+	int this_arg_index;
+	u8 *this_arg;
+
+	int arg_index;
+	u8 *arg;
+	__le32 *next_bitmap;
+	u32 bitmap_shifter;
+};
+
+extern int ieee80211_radiotap_iterator_init(
+   struct ieee80211_radiotap_iterator *iterator,
+   struct ieee80211_radiotap_header *radiotap_header,
+   int max_length);
+
+extern int ieee80211_radiotap_iterator_next(
+   struct ieee80211_radiotap_iterator *iterator);
+
+
 /* from net/wireless.h */
 struct wiphy;
 

257 lines not on this page · raw message

Index: wireless-dev/include/linux/if_ether.h
===================================================================
@@ -98,6 +98,7 @@
 #define ETH_P_ECONET	0x0018		/* Acorn Econet			*/
 #define ETH_P_HDLC	0x0019		/* HDLC frames			*/
 #define ETH_P_ARCNET	0x001A		/* 1A for ArcNet :-)            */
+#define ETH_P_80211_RT	0x001B		/* Radiotap + 80211 encapsulation */
 
 /*
  *	This is an Ethernet frame header.

Index: wireless-dev/net/mac80211/ieee80211.c
===================================================================
@@ -24,6 +24,7 @@
 #include <linux/compiler.h>
 #include <linux/bitmap.h>
 #include <net/cfg80211.h>
+#include <asm/unaligned.h>
 
 #include "ieee80211_common.h"
 #include "ieee80211_i.h"

@@ -1118,7 +1119,135 @@ ieee80211_tx_h_ps_buf(struct ieee80211_t
 }
 
 
-static void inline
+/*
+ * deal with packet injection down monitor interface
+ * with Radiotap Header -- only called for monitor mode interface
+ */
+
+static ieee80211_txrx_result
+__ieee80211_parse_tx_radiotap(
+	struct ieee80211_txrx_data *tx,
+	struct sk_buff *skb, struct ieee80211_tx_control *control)
+{
+	/*
+	 * this is the moment to interpret and discard the radiotap header that
+	 * must be at the start of the packet injected in Monitor mode
+	 *
+	 * Need to take some care with endian-ness, since radiotap
+	 * args are always little-endian
+	 */
+	struct ieee80211_radiotap_iterator iterator;
+	struct ieee80211_radiotap_header *rthdr =
+		(struct ieee80211_radiotap_header *) skb->data;
+	struct ieee80211_hw_mode *mode = tx->local->hw.conf.mode;
+	int ret = ieee80211_radiotap_iterator_init(&iterator, rthdr, skb->len);
+
+	/*
+	 * default control situation for all injected packets
+	 * FIXME: this does not suit all usage cases, expand to allow control
+	 */
+	control->retry_limit = 1; /* no retry */
+	control->key_idx = -1; /* no encryption key */
+	control->flags &= ~(IEEE80211_TXCTL_USE_RTS_CTS |
+			    IEEE80211_TXCTL_USE_CTS_PROTECT);
+	control->flags |= IEEE80211_TXCTL_DO_NOT_ENCRYPT |
+			  IEEE80211_TXCTL_NO_ACK;
+	control->antenna_sel_tx = 0; /* default to default antenna */
+
+	/*
+	 * for every radiotap entry that is present
+	 * (ieee80211_radiotap_iterator_next returns -ENOENT when no more
+	 * entries present, or -EINVAL on error)
+	 */
+	while (!ret) {
+		int i, target_rate;
+
+		ret = ieee80211_radiotap_iterator_next(&iterator);
+
+		if (ret)
+			continue;
+
+		/* see if this argument is something we can use */
+		switch (iterator.this_arg_index) {
+		/*
+ 		 * You must take care when dereferencing iterator.this_arg
+ 		 * for multibyte types... the pointer is not aligned.  Use
+ 		 * get_unaligned((type *)iterator.this_arg) to dereference
+ 		 * iterator.this_arg for type "type" safely on all arches.
+		*/
+		case IEEE80211_RADIOTAP_RATE:
+			/*
+			 * radiotap rate u8 is in 500kbps units eg, 0x02=1Mbps
+			 * ieee80211 rate int is in 100kbps units eg, 0x0a=1Mbps
+			 */
+			target_rate = (*iterator.this_arg) * 5;
+			for (i = 0; i < mode->num_rates; i++) {
+				struct ieee80211_rate *r = &mode->rates[i];
+
+				if (r->rate > target_rate)
+					continue;
+
+				control->rate = r;
+
+				if (r->flags & IEEE80211_RATE_PREAMBLE2)
+					control->tx_rate = r->val2;
+				else
+					control->tx_rate = r->val;
+
+				/* end on exact match */
+				if (r->rate == target_rate)
+					i = mode->num_rates;
+			}
+			break;
+
+		case IEEE80211_RADIOTAP_ANTENNA:
+			/*
+			 * radiotap uses 0 for 1st ant, mac80211 is 1 for
+			 * 1st ant
+			 */
+			control->antenna_sel_tx = (*iterator.this_arg) + 1;
+			break;
+
+		case IEEE80211_RADIOTAP_DBM_TX_POWER:
+			control->power_level = *iterator.this_arg;
+			break;
+
+		case IEEE80211_RADIOTAP_FLAGS:
+			if (*iterator.this_arg & IEEE80211_RADIOTAP_F_FCS) {
+				/*
+				 * this indicates that the skb we have been
+				 * handed has the 32-bit FCS CRC at the end...
+				 * we should react to that by snipping it off
+				 * because it will be recomputed and added
+				 * on transmission
+				 */
+				if (skb->len < (iterator.max_length + FCS_LEN))
+					return TXRX_DROP;
+
+				skb_trim(skb, skb->len - FCS_LEN);
+			}
+			break;
+
+		default:
+			break;
+		}
+	}
+
+	if (ret != -ENOENT) /* ie, if we didn't simply run out of fields */
+		return TXRX_DROP;
+
+	/*
+	 * remove the radiotap header
+	 * iterator->max_length was sanity-checked against
+	 * skb->len by iterator init
+	 */
+	skb_pull(skb, iterator.max_length);
+
+	return TXRX_CONTINUE;
+}
+
+
+static ieee80211_txrx_result inline
 __ieee80211_tx_prepare(struct ieee80211_txrx_data *tx,
 		       struct sk_buff *skb,
 		       struct net_device *dev,

@@ -1126,6 +1255,8 @@ __ieee80211_tx_prepare(struct ieee80211_
 {
 	struct ieee80211_local *local = wdev_priv(dev->ieee80211_ptr);
 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
+	ieee80211_txrx_result res = TXRX_CONTINUE;
+
 	int hdrlen;
 
 	memset(tx, 0, sizeof(*tx));

@@ -1135,7 +1266,38 @@ __ieee80211_tx_prepare(struct ieee80211_
 	tx->sdata = IEEE80211_DEV_TO_SUB_IF(dev);
 	tx->sta = sta_info_get(local, hdr->addr1);
 	tx->fc = le16_to_cpu(hdr->frame_control);
+
+	/*
+	 * set defaults for things that can be set by
+	 * injected radiotap headers
+	 */
 	control->power_level = local->hw.conf.power_level;
+	control->antenna_sel_tx = local->hw.conf.antenna_sel_tx;
+	if (local->sta_antenna_sel != STA_ANTENNA_SEL_AUTO && tx->sta)
+		control->antenna_sel_tx = tx->sta->antenna_sel_tx;
+
+	/*
+	 * if the skb came in as in encapsulated injection packet, process
+	 * and then remove the radiotap header
+	 */
+	if (skb->protocol == ETH_P_80211_RT) {
+
+		printk("__ieee80211_tx_prepare sees ETH_P_80211_RT\n");
+
+		if (__ieee80211_parse_tx_radiotap(tx, skb, control) ==
+								TXRX_DROP) {
+			printk("  drop\n");
+			return TXRX_DROP;
+		}
+		/*
+		 * we removed the radiotap header after this point,
+		 * we filled control with what we could use
+		 * set to the actual ieee header now
+		 */
+		hdr = (struct ieee80211_hdr *) skb->data;
+		res = TXRX_QUEUED; /* indication it was injected packet */
+	}
+
 	tx->u.tx.control = control;
 	tx->u.tx.unicast = !is_multicast_ether_addr(hdr->addr1);
 	if (is_multicast_ether_addr(hdr->addr1))

@@ -1152,9 +1314,6 @@ __ieee80211_tx_prepare(struct ieee80211_
 		control->flags |= IEEE80211_TXCTL_CLEAR_DST_MASK;
 		tx->sta->clear_dst_mask = 0;
 	}
-	control->antenna_sel_tx = local->hw.conf.antenna_sel_tx;
-	if (local->sta_antenna_sel != STA_ANTENNA_SEL_AUTO && tx->sta)
-		control->antenna_sel_tx = tx->sta->antenna_sel_tx;
 	hdrlen = ieee80211_get_hdrlen(tx->fc);
 	if (skb->len > hdrlen + sizeof(rfc1042_header) + 2) {
 		u8 *pos = &skb->data[hdrlen + sizeof(rfc1042_header)];

@@ -1162,6 +1321,7 @@ __ieee80211_tx_prepare(struct ieee80211_
 	}
 	control->flags |= IEEE80211_TXCTL_FIRST_FRAGMENT;
 
+	return res;
 }
 
 static int inline is_ieee80211_device(struct net_device *dev,

@@ -1274,7 +1434,7 @@ static int ieee80211_tx(struct net_devic
 	struct sta_info *sta;
 	ieee80211_tx_handler *handler;
 	struct ieee80211_txrx_data tx;
-	ieee80211_txrx_result res = TXRX_DROP;
+	ieee80211_txrx_result res = TXRX_DROP, res_prepare;
 	int ret, i;
 
 	WARN_ON(__ieee80211_queue_pending(local, control->queue));

@@ -1284,15 +1444,26 @@ static int ieee80211_tx(struct net_devic
 		return 0;
 	}
 
-	__ieee80211_tx_prepare(&tx, skb, dev, control);
+	res_prepare = __ieee80211_tx_prepare(&tx, skb, dev, control);
+
+	if (res_prepare == TXRX_DROP) {
+		dev_kfree_skb(skb);
+		return 0;
+	}
+
 	sta = tx.sta;
 	tx.u.tx.mgmt_interface = mgmt;
 	tx.u.tx.mode = local->hw.conf.mode;
 
-	for (handler = local->tx_handlers; *handler != NULL; handler++) {
-		res = (*handler)(&tx);
-		if (res != TXRX_CONTINUE)
-			break;
+	if (res_prepare == TXRX_QUEUED) { /* if it was an injected packet */
+		res = TXRX_CONTINUE;
+	} else {
+		for (handler = local->tx_handlers; *handler != NULL;
+		     handler++) {
+			res = (*handler)(&tx);
+			if (res != TXRX_CONTINUE)
+				break;
+		}
 	}
 
 	skb = tx.skb; /* handlers are allowed to change skb */

@@ -1493,6 +1664,76 @@ static int ieee80211_master_start_xmit(s
 	return ret;
 }
 
+/*
+ * snips off the injection encapsulation ethernet header when the packet
+ * has been identified as coming into ieee80211_subif_start_xmit
+ * having the ETH_P_80211_RT type
+ */
+static int
+ieee80211_subif_tx_rt_encap_remove(struct sk_buff *skb, struct net_device *dev,
+    struct ieee80211_sub_if_data *sdata, struct ieee80211_local *local)
+{
+	struct ieee80211_tx_packet_data *pkt_data;
+	struct ieee80211_radiotap_header *prthdr;
+	u16 len;
+
+	/*
+	 * snip off the encapsulating ethernet type header
+	 * caller ieee80211_subif_start_xmit confirmed we have at least
+	 * ETH_HLEN in the skb
+	 */
+	skb_pull(skb, ETH_HLEN);
+	prthdr = (struct ieee80211_radiotap_header *)skb->data;
+	/*
+	 * there must be a radiotap header at the
+	 * start in this case
+	 */
+	if (unlikely(skb->len < sizeof(*prthdr))) {
+		printk(KERN_DEBUG "%s: short skb (len=%d)\n",
+			dev->name, skb->len);
+		return -EINVAL;
+	}
+	/*
+	 * only version 0 is supported... must check here because
+	 * we are interested in the length inside in a moment
+	 */
+	if (unlikely(prthdr->it_version))
+		return -EINVAL;
+
+	skb->dev = local->mdev;
+	pkt_data = (struct ieee80211_tx_packet_data *)skb->cb;
+	memset(pkt_data, 0, sizeof(*pkt_data));
+	/* needed because we set skb device to master */
+	pkt_data->ifindex = sdata->dev->ifindex;
+	pkt_data->mgmt_iface = 0;
+	pkt_data->do_not_encrypt = 1;
+	/*
+	 * fix up the skb pointers accounting for the radiotap
+	 * header still being in there.  We are being given
+	 * a precooked IEEE80211 header so no need for
+	 * normal processing
+	 */
+	len = le16_to_cpu(get_unaligned(&prthdr->it_len));
+	/*
+	 * detect possible insantity with the reported rt length
+	 * vs the actual skb length
+	 */
+	if (unlikely(skb->len < (sizeof(*prthdr) + len))) {
+		printk(KERN_DEBUG "%s: short skb (len=%d)\n",
+			dev->name, skb->len);
+		return -EINVAL;
+	}
+	skb_set_mac_header(skb, len);
+	skb_set_network_header(skb, len + sizeof(struct ieee80211_hdr));
+	skb_set_transport_header(skb, len + sizeof(struct ieee80211_hdr));
+	/*
+	 * mark the SKB as having had the encapsulation so we can recognize
+	 * that this is an injected packet with radiotap subsequently
+	 */
+	skb->protocol = ETH_P_80211_RT;
+
+	return 0;
+}
 
 /**
  * ieee80211_subif_start_xmit - netif start_xmit function for Ethernet-type

@@ -1537,6 +1778,28 @@ static int ieee80211_subif_start_xmit(st
 	/* convert Ethernet header to proper 802.11 header (based on
 	 * operation mode) */
 	ethertype = (skb->data[12] << 8) | skb->data[13];
+
+	/*
+	 * Case where an injected radiotap + 80211 packet is encapsulated
+	 * in a ETH_P_80211_RT frame
+	 */
+	if (ethertype == ETH_P_80211_RT) {
+		ret = 0; /* we should free the skb on fail */
+		/*
+		 * strip the encapsulating ethernet header and mark skb's
+		 * protocol as being ETH_P_80211_RT
+		 */
+		if (ieee80211_subif_tx_rt_encap_remove(skb, dev, sdata, local))
+			goto fail;
+		/*
+		 * pass the remaining radiotap header + 80211 header + payload
+		 * up to the next stage
+		 */
+		dev_queue_xmit(skb);
+
+		return 0;
+	}
+
 	/* TODO: handling for 802.1x authorized/unauthorized port */
 	fc = IEEE80211_FTYPE_DATA | IEEE80211_STYPE_DATA;