Try #5: Radiotap on Monitor Mode interfaces for rx and tx

index 577dbe3..c598c9d 100644
@@ -275,7 +275,8 @@ EXPORT_SYMBOL(ieee80211_get_hdrlen);
 
 int ieee80211_get_hdrlen_from_skb(const struct sk_buff *skb)
 {
-	const struct ieee80211_hdr *hdr = (const struct ieee80211_hdr *) skb->data;
+	const struct ieee80211_hdr *hdr =
+		(const struct ieee80211_hdr *) skb->data;
 	int hdrlen;
 
 	if (unlikely(skb->len < 10))

@@ -489,7 +490,8 @@ ieee80211_tx_h_fragment(struct ieee80211_txrx_data *tx)
 		fhdr = (struct ieee80211_hdr *) skb_put(frag, hdrlen);
 		memcpy(fhdr, first->data, hdrlen);
 		if (i == num_fragm - 2)
-			fhdr->frame_control &= cpu_to_le16(~IEEE80211_FCTL_MOREFRAGS);
+			fhdr->frame_control &=
+				cpu_to_le16(~IEEE80211_FCTL_MOREFRAGS);
 		fhdr->seq_ctrl = cpu_to_le16(i + 1);
 		copylen = left > per_fragm ? per_fragm : left;
 		memcpy(skb_put(frag, copylen), pos, copylen);

@@ -545,7 +547,8 @@ void ieee80211_tx_set_iswep(struct ieee80211_txrx_data *tx)
 		for (i = 0; i < tx->u.tx.num_extra_frag; i++) {
 			fhdr = (struct ieee80211_hdr *)
 				tx->u.tx.extra_frag[i]->data;
-			fhdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_PROTECTED);
+			fhdr->frame_control |=
+				cpu_to_le16(IEEE80211_FCTL_PROTECTED);
 		}
 	}
 }

@@ -861,7 +864,8 @@ ieee80211_tx_h_check_assoc(struct ieee80211_txrx_data *tx)
 	if (likely(tx->u.tx.unicast)) {
 		if (unlikely(!(sta_flags & WLAN_STA_ASSOC) &&
 			     tx->sdata->type != IEEE80211_IF_TYPE_IBSS &&
-			     (tx->fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_DATA)) {
+			     (tx->fc & IEEE80211_FCTL_FTYPE)
+==			      IEEE80211_FTYPE_DATA)) {
 #ifdef CONFIG_MAC80211_VERBOSE_DEBUG
 			printk(KERN_DEBUG "%s: dropped data frame to not "
 			       "associated station " MAC_FMT "\n",

@@ -871,7 +875,8 @@ ieee80211_tx_h_check_assoc(struct ieee80211_txrx_data *tx)
 			return TXRX_DROP;
 		}
 	} else {
-		if (unlikely((tx->fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_DATA &&
+		if (unlikely((tx->fc & IEEE80211_FCTL_FTYPE) ==
+		             IEEE80211_FTYPE_DATA &&
 			     tx->local->num_sta == 0 &&
 			     !tx->local->allow_broadcast_always &&
 			     tx->sdata->type != IEEE80211_IF_TYPE_IBSS)) {

@@ -982,7 +987,8 @@ ieee80211_tx_h_unicast_ps_buf(struct ieee80211_txrx_data *tx)
 
 	if (unlikely(!sta ||
 		     ((tx->fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT &&
-		      (tx->fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_PROBE_RESP)))
+		      (tx->fc & IEEE80211_FCTL_STYPE) ==
+		       IEEE80211_STYPE_PROBE_RESP)))
 		return TXRX_CONTINUE;
 
 	if (unlikely((sta->flags & WLAN_STA_PS) && !sta->pspoll)) {

@@ -1012,7 +1018,8 @@ ieee80211_tx_h_unicast_ps_buf(struct ieee80211_txrx_data *tx)
 				tx->local->ops->set_tim(local_to_hw(tx->local),
 						       sta->aid, 1);
 			if (tx->sdata->bss)
-				bss_tim_set(tx->local, tx->sdata->bss, sta->aid);
+				bss_tim_set(tx->local, tx->sdata->bss,
+				            sta->aid);
 		}
 		pkt_data = (struct ieee80211_tx_packet_data *)tx->skb->cb;
 		pkt_data->jiffies = jiffies;

@@ -1146,7 +1153,8 @@ static int __ieee80211_tx(struct ieee80211_local *local, struct sk_buff *skb,
 	int ret, i;
 
 	if (skb) {
-		ieee80211_dump_frame(local->mdev->name, "TX to low-level driver", skb);
+		ieee80211_dump_frame(local->mdev->name,
+		                     "TX to low-level driver", skb);
 		ret = local->ops->tx(local_to_hw(local), skb, control);
 		if (ret)
 			return IEEE80211_TX_AGAIN;

@@ -1170,7 +1178,8 @@ static int __ieee80211_tx(struct ieee80211_local *local, struct sk_buff *skb,
 						IEEE80211_TXCTL_RATE_CTRL_PROBE;
 				else
 					control->flags &=
-						~IEEE80211_TXCTL_RATE_CTRL_PROBE;
+						~IEEE80211_TXCTL_RATE_CTRL_PROBE
+					;
 			}
 
 			ieee80211_dump_frame(local->mdev->name,

@@ -1783,7 +1792,8 @@ struct sk_buff * ieee80211_beacon_get(struct ieee80211_hw *hw, int if_id,
 		rate = rate_control_get_rate(local, local->mdev, skb, &extra);
 		if (!rate) {
 			if (net_ratelimit()) {
-				printk(KERN_DEBUG "%s: ieee80211_beacon_get: no rate "
+				printk(KERN_DEBUG
+				       "%s: ieee80211_beacon_get: no rate "
 				       "found\n", local->mdev->name);
 			}
 			dev_kfree_skb(skb);

@@ -1835,7 +1845,8 @@ EXPORT_SYMBOL(ieee80211_rts_duration);
 
 __le16 ieee80211_ctstoself_duration(struct ieee80211_hw *hw,
 				    size_t frame_len,
-				    const struct ieee80211_tx_control *frame_txctl)
+				    const struct ieee80211_tx_control
+					*frame_txctl)
 {
 	struct ieee80211_local *local = hw_to_local(hw);
 	struct ieee80211_rate *rate;

@@ -1885,7 +1896,8 @@ void ieee80211_ctstoself_get(struct ieee80211_hw *hw,
 
 	fctl = IEEE80211_FTYPE_CTL | IEEE80211_STYPE_CTS;
 	cts->frame_control = cpu_to_le16(fctl);
-	cts->duration = ieee80211_ctstoself_duration(hw, frame_len, frame_txctl);
+	cts->duration = ieee80211_ctstoself_duration(hw, frame_len,
+			                             frame_txctl);
 	memcpy(cts->ra, hdr->addr1, sizeof(cts->ra));
 }
 EXPORT_SYMBOL(ieee80211_ctstoself_get);

@@ -2942,10 +2954,12 @@ ieee80211_rx_h_ps_poll(struct ieee80211_txrx_data *rx)
 		/* Use MoreData flag to indicate whether there are more
 		 * buffered frames for this STA */
 		if (no_pending_pkts) {
-			hdr->frame_control &= cpu_to_le16(~IEEE80211_FCTL_MOREDATA);
+			hdr->frame_control &=
+				cpu_to_le16(~IEEE80211_FCTL_MOREDATA);
 			rx->sta->flags &= ~WLAN_STA_TIM;
 		} else
-			hdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_MOREDATA);
+			hdr->frame_control |=
+				cpu_to_le16(IEEE80211_FCTL_MOREDATA);
 
 		dev_queue_xmit(skb);
 

@@ -2954,7 +2968,8 @@ ieee80211_rx_h_ps_poll(struct ieee80211_txrx_data *rx)
 				rx->local->ops->set_tim(local_to_hw(rx->local),
 						       rx->sta->aid, 0);
 			if (rx->sdata->bss)
-				bss_tim_clear(rx->local, rx->sdata->bss, rx->sta->aid);
+				bss_tim_clear(rx->local, rx->sdata->bss,
+					rx->sta->aid);
 		}
 #ifdef CONFIG_MAC80211_VERBOSE_PS_DEBUG
 	} else if (!rx->u.rx.sent_ps_buffered) {

@@ -3040,7 +3055,8 @@ ieee80211_reassemble_find(struct ieee80211_sub_if_data *sdata,
 		f_hdr = (struct ieee80211_hdr *) entry->skb_list.next->data;
 		f_fc = le16_to_cpu(f_hdr->frame_control);
 
-		if ((fc & IEEE80211_FCTL_FTYPE) != (f_fc & IEEE80211_FCTL_FTYPE) ||
+		if ((fc & IEEE80211_FCTL_FTYPE) !=
+		    (f_fc & IEEE80211_FCTL_FTYPE) ||
 		    compare_ether_addr(hdr->addr1, f_hdr->addr1) != 0 ||
 		    compare_ether_addr(hdr->addr2, f_hdr->addr2) != 0)
 			continue;

@@ -3231,7 +3247,8 @@ ieee80211_rx_h_check(struct ieee80211_txrx_data *rx)
 	 */
 	if (unlikely(((rx->fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_DATA ||
 		      ((rx->fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_CTL &&
-		       (rx->fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_PSPOLL)) &&
+		       (rx->fc & IEEE80211_FCTL_STYPE) ==
+		        IEEE80211_STYPE_PSPOLL)) &&
 		     rx->sdata->type != IEEE80211_IF_TYPE_IBSS &&
 		     (!rx->sta || !(rx->sta->flags & WLAN_STA_ASSOC)))) {
 		if ((!(rx->fc & IEEE80211_FCTL_FROMDS) &&

@@ -3445,7 +3462,8 @@ ieee80211_rx_h_802_1x_pae(struct ieee80211_txrx_data *rx)
 
 	if (unlikely(rx->sdata->ieee802_1x &&
 		     (rx->fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_DATA &&
-		     (rx->fc & IEEE80211_FCTL_STYPE) != IEEE80211_STYPE_NULLFUNC &&
+		     (rx->fc & IEEE80211_FCTL_STYPE) !=
+		      IEEE80211_STYPE_NULLFUNC &&
 		     (!rx->sta || !(rx->sta->flags & WLAN_STA_AUTHORIZED)) &&
 		     !ieee80211_is_eapol(rx->skb))) {
 #ifdef CONFIG_MAC80211_DEBUG

@@ -3472,7 +3490,8 @@ ieee80211_rx_h_drop_unencrypted(struct ieee80211_txrx_data *rx)
 	/* Drop unencrypted frames if key is set. */
 	if (unlikely(!(rx->fc & IEEE80211_FCTL_PROTECTED) &&
 		     (rx->fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_DATA &&
-		     (rx->fc & IEEE80211_FCTL_STYPE) != IEEE80211_STYPE_NULLFUNC &&
+		     (rx->fc & IEEE80211_FCTL_STYPE) !=
+		      IEEE80211_STYPE_NULLFUNC &&
 		     (rx->key || rx->sdata->drop_unencrypted) &&
 		     (rx->sdata->eapol == 0 ||
 		      !ieee80211_is_eapol(rx->skb)))) {

@@ -3537,7 +3556,8 @@ ieee80211_rx_h_passive_scan(struct ieee80211_txrx_data *rx)
 			local->scan.rx_beacon++;
 			/* Need to trim FCS here because it is normally
 			 * removed only after this passive scan handler. */
-			if ((rx->local->hw.flags & IEEE80211_HW_RX_INCLUDES_FCS) &&
+			if ((rx->local->hw.flags &
+			     IEEE80211_HW_RX_INCLUDES_FCS) &&
 			    rx->skb->len > FCS_LEN)
 				skb_trim(rx->skb, rx->skb->len - FCS_LEN);
 

@@ -3800,8 +3820,9 @@ void __ieee80211_rx(struct ieee80211_hw *hw, struct sk_buff *skb,
 						continue;
 					rx.u.rx.ra_match = 0;
 				} else if (!multicast &&
-					   compare_ether_addr(sdata->dev->dev_addr,
-							      hdr->addr1) != 0) {
+					   compare_ether_addr(
+						sdata->dev->dev_addr,
+						hdr->addr1) != 0) {
 					if (!sdata->promisc)
 						continue;
 					rx.u.rx.ra_match = 0;

@@ -3816,22 +3837,27 @@ void __ieee80211_rx(struct ieee80211_hw *hw, struct sk_buff *skb,
 						continue;
 					rx.u.rx.ra_match = 0;
 				} else if (!multicast &&
-					   compare_ether_addr(sdata->dev->dev_addr,
-							      hdr->addr1) != 0) {
+					   compare_ether_addr(
+						sdata->dev->dev_addr,
+						hdr->addr1) != 0) {
 					if (!sdata->promisc)
 						continue;
 					rx.u.rx.ra_match = 0;
 				} else if (!sta)
 					sta = rx.sta =
-						ieee80211_ibss_add_sta(local->mdev,
-								       skb, bssid,
-								       hdr->addr2);
-						/* FIXME: call with sdata->dev */
+						ieee80211_ibss_add_sta(
+							local->mdev,
+							skb, bssid,
+							hdr->addr2);
+						/* FIXME: call with
+						 * sdata->dev
+						 */
 				break;
 			case IEEE80211_IF_TYPE_AP:
 				if (!bssid) {
-					if (compare_ether_addr(sdata->dev->dev_addr,
-							       hdr->addr1) != 0)
+					if (compare_ether_addr(
+						sdata->dev->dev_addr,
+						hdr->addr1) != 0)
 						continue;
 				} else if (!ieee80211_bssid_match(bssid,
 							sdata->dev->dev_addr)) {

@@ -3847,7 +3873,8 @@ void __ieee80211_rx(struct ieee80211_hw *hw, struct sk_buff *skb,
 				break;
 			case IEEE80211_IF_TYPE_WDS:
 				if (bssid ||
-				    (rx.fc & IEEE80211_FCTL_FTYPE) != IEEE80211_FTYPE_DATA)
+				    (rx.fc & IEEE80211_FCTL_FTYPE) !=
+				     IEEE80211_FTYPE_DATA)
 					continue;
 				if (compare_ether_addr(sdata->u.wds.remote_addr,
 						       hdr->addr2) != 0)

@@ -3859,9 +3886,11 @@ void __ieee80211_rx(struct ieee80211_hw *hw, struct sk_buff *skb,
 				skb_new = skb_copy(skb, GFP_ATOMIC);
 				if (!skb_new) {
 					if (net_ratelimit())
-						printk(KERN_DEBUG "%s: failed to copy "
+						printk(KERN_DEBUG
+						       "%s: failed to copy "
 						       "multicast frame for %s",
-						       local->mdev->name, prev->dev->name);
+						       local->mdev->name,
+						       prev->dev->name);
 					continue;
 				}
 				rx.skb = skb_new;

@@ -4149,9 +4178,12 @@ static void ieee80211_remove_tx_extra(struct ieee80211_local *local,
 	pkt_data = (struct ieee80211_tx_packet_data *)skb->cb;
 	pkt_data->ifindex = control->ifindex;
 	pkt_data->mgmt_iface = (control->type == IEEE80211_IF_TYPE_MGMT);
-	pkt_data->req_tx_status = !!(control->flags & IEEE80211_TXCTL_REQ_TX_STATUS);
-	pkt_data->do_not_encrypt = !!(control->flags & IEEE80211_TXCTL_DO_NOT_ENCRYPT);
-	pkt_data->requeue = !!(control->flags & IEEE80211_TXCTL_REQUEUE);
+	pkt_data->req_tx_status = !!(control->flags &
+				     IEEE80211_TXCTL_REQ_TX_STATUS);
+	pkt_data->do_not_encrypt = !!(control->flags &
+				      IEEE80211_TXCTL_DO_NOT_ENCRYPT);
+	pkt_data->requeue = !!(control->flags &
+			       IEEE80211_TXCTL_REQUEUE);
 	pkt_data->queue = control->queue;
 
 	hdrlen = ieee80211_get_hdrlen_from_skb(skb);

@@ -4223,7 +4255,8 @@ void ieee80211_tx_status(struct ieee80211_hw *hw, struct sk_buff *skb,
 				 * that this TX packet failed because of that.
 				 */
 				status->excessive_retries = 0;
-				status->flags |= IEEE80211_TX_STATUS_TX_FILTERED;
+				status->flags |=
+					IEEE80211_TX_STATUS_TX_FILTERED;
 			}
 			sta_info_put(sta);
 		}

@@ -4254,9 +4287,11 @@ void ieee80211_tx_status(struct ieee80211_hw *hw, struct sk_buff *skb,
 							  &status->control);
 				skb_queue_tail(&sta->tx_filtered, skb);
 			} else if (!(sta->flags & WLAN_STA_PS) &&
-				   !(status->control.flags & IEEE80211_TXCTL_REQUEUE)) {
+				   !(status->control.flags &
+				     IEEE80211_TXCTL_REQUEUE)) {
 				/* Software retry the packet once */
-				status->control.flags |= IEEE80211_TXCTL_REQUEUE;
+				status->control.flags |=
+					IEEE80211_TXCTL_REQUEUE;
 				ieee80211_remove_tx_extra(local, sta->key,
 							  skb,
 							  &status->control);

index 916b21b..27cffdc 100644
@@ -518,6 +518,9 @@ struct ieee80211_hw {
 	 * normal operation. */
 #define IEEE80211_HW_MONITOR_DURING_OPER (1<<9)
 
+	/* Driver supports radiotap. */
+#define IEEE80211_HW_RADIOTAP_SUPPORTED (1<<10)
+
 	/* please fill this gap when adding new flags */
 
 	/* calculate Michael MIC for an MSDU when doing hwcrypto */

118 lines not on this page · raw message

index 3e0b4fa..51197b1 100644
@@ -199,7 +199,7 @@ void ieee80211_if_set_type(struct net_device *dev, int type)
 		break;
 	}
 	case IEEE80211_IF_TYPE_MNTR:
-		dev->type = ARPHRD_IEEE80211_PRISM;
+		dev->type = ARPHRD_IEEE80211_RADIOTAP;
 		break;
 	default:
 		printk(KERN_WARNING "%s: %s: Unknown interface type 0x%x",

index fb33b90..fe15612 100644
@@ -1054,7 +1054,180 @@ ieee80211_tx_h_ps_buf(struct ieee80211_txrx_data *tx)
 }
 
 
-static void inline
+/* deal with packet injection down monitor interface
+ * with Radiotap Header -- only called for monitor mode interface
+ */
+
+static ieee80211_txrx_result
+__ieee80211_convert_radiotap_to_control_and_remove(
+	struct ieee80211_txrx_data *tx,
+	struct sk_buff *skb, struct ieee80211_tx_control *control)
+{
+	/* this is the moment to interpret the radiotap header that
+	 * must be at the start of the packet injected in Monitor
+	 * mode into control and then discard the radiotap header
+	 *
+	 * Need to take some care with endian-ness since radiotap
+	 * is apparently a little-endian struct, including the args
+	 *
+	 * There is also some pervacious arg padding, so that args
+	 * of a given length must begin at a boundary of that length
+	 */
+
+	struct ieee80211_radiotap_header *rthdr =
+		(struct ieee80211_radiotap_header *) skb->data;
+
+	/* small length lookup table for all radiotap types we heard of
+	 * starting from b0 in the bitmap, so we can walk the payload
+	 * area of the radiotap header
+	 */
+
+	static const u8 radiotap_entry_sizes[] = {
+		8, /* IEEE80211_RADIOTAP_TSFT */
+		1, /* IEEE80211_RADIOTAP_FLAGS */
+		1, /* IEEE80211_RADIOTAP_RATE */
+		4, /* IEEE80211_RADIOTAP_CHANNEL */
+		2, /* IEEE80211_RADIOTAP_FHSS */
+		1, /* IEEE80211_RADIOTAP_DBM_ANTSIGNAL */
+		1, /* IEEE80211_RADIOTAP_DBM_ANTNOISE */
+		2, /* IEEE80211_RADIOTAP_LOCK_QUALITY */
+		2, /* IEEE80211_RADIOTAP_TX_ATTENUATION */
+		2, /* IEEE80211_RADIOTAP_DB_TX_ATTENUATION */
+		1, /* IEEE80211_RADIOTAP_DBM_TX_POWER */
+		1, /* IEEE80211_RADIOTAP_ANTENNA */
+		1, /* IEEE80211_RADIOTAP_DB_ANTSIGNAL */
+		1  /* IEEE80211_RADIOTAP_DB_ANTNOISE */
+		/* add more here as they are defined */
+	};
+	int tap_index = 0;
+	u8 *tap_arg = skb->data + sizeof(struct ieee80211_radiotap_header);
+	u32 *curr_arg_bitmap = &rthdr->it_present;
+	u32 arg_bitmap = le32_to_cpu(*curr_arg_bitmap);
+
+	if (rthdr->it_version)
+		return TXRX_DROP; /* version byte as magic */
+
+	/* sanity check for skb length and radiotap length field */
+	if (skb->len < (le16_to_cpu(rthdr->it_len) +
+	    sizeof(struct ieee80211_hdr)))
+		return TXRX_DROP;
+
+	/* find payload start allowing for extended bitmap(s) */
+
+	if (le32_to_cpu(rthdr->it_present) & 0x80000000) {
+		while (le32_to_cpu(*((u32 *)tap_arg)) & 0x80000000)
+			tap_arg += sizeof(u32);
+		tap_arg += sizeof(u32);
+	}
+
+	/* default control situation for all injected packets */
+
+	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 we can at
+	 * least skip (by knowing the length)...
+	 */
+
+	while (tap_index < sizeof(radiotap_entry_sizes)) {
+		int i, target_rate;
+
+		if (!(arg_bitmap & 1))
+			goto next_entry;
+
+		/* arg is present, account for alignment padding
+		 *  8-bit args can be at any alignment
+		 * 16-bit args must start on 16-bit boundary
+		 * 32-bit args must start on 32-bit boundary
+		 * 64-bit args must start on 64-bit boundary
+		 */
+
+		if (((int)tap_arg) & (radiotap_entry_sizes[tap_index]-1))
+			tap_arg += radiotap_entry_sizes[tap_index]-
+				   (((int)tap_arg) &
+				    (radiotap_entry_sizes[tap_index]-1));
+
+		/* see if this argument is something that interests us */
+
+		switch (tap_index) {
+
+		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 = (*tap_arg) * 5;
+			for (i = 0; i < tx->local->num_curr_rates; i++) {
+				struct ieee80211_rate *r =
+					&tx->local->curr_rates[i];
+
+				if (r->rate <= target_rate) {
+					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 = tx->local->num_curr_rates;
+				}
+			}
+			break;
+
+		case IEEE80211_RADIOTAP_ANTENNA:
+			/* radiotap uses 0 for 1st ant,
+			 * mac80211 is 1 for 1st ant
+			 * absence of IEEE80211_RADIOTAP_ANTENNA
+			 * gives default/diversity
+			 */
+			control->antenna_sel_tx = (*tap_arg) + 1;
+			break;
+
+		case IEEE80211_RADIOTAP_DBM_TX_POWER:
+			control->power_level = *tap_arg;
+			break;
+
+		default:
+			break;
+		}
+
+		tap_arg += radiotap_entry_sizes[tap_index];
+
+	next_entry:
+
+		tap_index++;
+		if (unlikely((tap_index & 31) == 0)) {
+			/* completed current u32 bitmap */
+			if (arg_bitmap & 1) { /* b31 was set, there is more */
+				/* move to next u32 bitmap */
+				curr_arg_bitmap++;
+				arg_bitmap = le32_to_cpu(*curr_arg_bitmap);
+			} else {
+				/* no more bitmaps: end */
+				tap_index = sizeof(radiotap_entry_sizes);
+			}
+		} else { /* just try the next bit */
+			arg_bitmap >>= 1;
+		}
+	}
+
+	/* remove the radiotap header */
+	skb_pull(skb, le16_to_cpu(rthdr->it_len));
+
+	return TXRX_CONTINUE;
+}
+
+
+static ieee80211_txrx_result inline
 __ieee80211_tx_prepare(struct ieee80211_txrx_data *tx,
 		       struct sk_buff *skb,
 		       struct net_device *dev,

@@ -1062,6 +1235,9 @@ __ieee80211_tx_prepare(struct ieee80211_txrx_data *tx,
 {
 	struct ieee80211_local *local = wdev_priv(dev->ieee80211_ptr);
 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
+	struct ieee80211_sub_if_data *sdata;
+	ieee80211_txrx_result res = TXRX_CONTINUE;
+
 	int hdrlen;
 
 	memset(tx, 0, sizeof(*tx));

@@ -1071,7 +1247,32 @@ __ieee80211_tx_prepare(struct ieee80211_txrx_data *tx,
 	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;
+
+	/* process and remove the injection radiotap header */
+
+	sdata = IEEE80211_DEV_TO_SUB_IF(dev);
+	if (unlikely(sdata->type == IEEE80211_IF_TYPE_MNTR)) {
+		if (__ieee80211_convert_radiotap_to_control_and_remove(
+		    tx, skb, control) == TXRX_DROP) {
+			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 monitor packet */
+	}
+
 	tx->u.tx.control = control;
 	tx->u.tx.unicast = !is_multicast_ether_addr(hdr->addr1);
 	if (is_multicast_ether_addr(hdr->addr1))

@@ -1088,9 +1289,6 @@ __ieee80211_tx_prepare(struct ieee80211_txrx_data *tx,
 		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)];

@@ -1098,6 +1296,7 @@ __ieee80211_tx_prepare(struct ieee80211_txrx_data *tx,
 	}
 	control->flags |= IEEE80211_TXCTL_FIRST_FRAGMENT;
 
+	return res;
 }
 
 static int inline is_ieee80211_device(struct net_device *dev,

@@ -1205,7 +1404,7 @@ static int ieee80211_tx(struct net_device *dev, struct sk_buff *skb,
 	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));

@@ -1215,14 +1414,24 @@ static int ieee80211_tx(struct net_device *dev, struct sk_buff *skb,
 		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;
 
-	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 */

@@ -1456,6 +1665,51 @@ static int ieee80211_subif_start_xmit(struct sk_buff *skb,
 		goto fail;
 	}
 
+	if (unlikely(sdata->type == IEEE80211_IF_TYPE_MNTR)) {
+		struct ieee80211_radiotap_header * prthdr =
+			(struct ieee80211_radiotap_header *)skb->data;
+
+		/* there must be a radiotap header at the
+		 * start in this case
+		 */
+
+		if (unlikely(prthdr->it_version)) {
+			/* radiotap version used as magic */
+			ret = 0;
+			goto fail;
+		}
+
+		skb->dev = local->mdev;
+
+		pkt_data = (struct ieee80211_tx_packet_data *)skb->cb;
+		memset(pkt_data, 0, sizeof(struct ieee80211_tx_packet_data));
+		pkt_data->ifindex = sdata->dev->ifindex;
+		pkt_data->mgmt_iface = 0;
+		pkt_data->do_not_encrypt = 1;
+
+		/* above needed because we set skb device to master */
+
+		/* fix up the pointers accounting for the radiotap
+		 * header still being in there.  We are being given
+		 * a precooked IEEE80211 header so no need for
+		 * normal processing
+		 */
+
+		skb->mac.raw = skb->data+prthdr->it_len;
+		skb->nh.raw = skb->data+prthdr->it_len+
+			sizeof(struct ieee80211_hdr);
+		skb->h.raw = skb->data+prthdr->it_len+
+			sizeof(struct ieee80211_hdr);
+
+		/* pass the radiotap header up to
+		 * the next stage intact
+		 */
+
+		dev_queue_xmit(skb);
+
+		return 0;
+	}
+
 	nh_pos = skb->nh.raw - skb->data;
 	h_pos = skb->h.raw - skb->data;