[PATCH] mac80211: show transmitted frames on monitor interfaces

This patch makes mac80211 show transmitted frames on monitor interfaces,
including radiotap headers that indicate some transmission parameters.
The shown parameters will need to be expanded, but this should work as
a basis to work from.

Signed-off-by: Johannes Berg <johannes@sipsolutions.net>
Signed-off-by: Jiri Benc <jbenc@suse.cz>
Signed-off-by: John W. Linville <linville@tuxdriver.com>

2 files changed, 118 insertions, 23 deletions

diff --git a/net/mac80211/ieee80211.c b/net/mac80211/ieee80211.c
index 8b57eaa2a271..85f23fd866ad 100644
--- a/net/mac80211/ieee80211.c
+++ b/net/mac80211/ieee80211.c
@@ -57,6 +57,17 @@ static const unsigned char eapol_header[] =
 	{ 0xaa, 0xaa, 0x03, 0x00, 0x00, 0x00, 0x88, 0x8e };
 
 
+/*
+ * For seeing transmitted packets on monitor interfaces
+ * we have a radiotap header too.
+ */
+struct ieee80211_tx_status_rtap_hdr {
+	struct ieee80211_radiotap_header hdr;
+	__le16 tx_flags;
+	u8 data_retries;
+} __attribute__ ((packed));
+
+
 static inline void ieee80211_include_sequence(struct ieee80211_sub_if_data *sdata,
 					      struct ieee80211_hdr *hdr)
 {
@@ -529,7 +540,7 @@ ieee80211_tx_h_fragment(struct ieee80211_txrx_data *tx)
 		/* reserve enough extra head and tail room for possible
 		 * encryption */
 		frag = frags[i] =
-			dev_alloc_skb(tx->local->hw.extra_tx_headroom +
+			dev_alloc_skb(tx->local->tx_headroom +
 				      frag_threshold +
 				      IEEE80211_ENCRYPT_HEADROOM +
 				      IEEE80211_ENCRYPT_TAILROOM);
@@ -538,8 +549,8 @@ ieee80211_tx_h_fragment(struct ieee80211_txrx_data *tx)
 		/* Make sure that all fragments use the same priority so
 		 * that they end up using the same TX queue */
 		frag->priority = first->priority;
-		skb_reserve(frag, tx->local->hw.extra_tx_headroom +
-			IEEE80211_ENCRYPT_HEADROOM);
+		skb_reserve(frag, tx->local->tx_headroom +
+				  IEEE80211_ENCRYPT_HEADROOM);
 		fhdr = (struct ieee80211_hdr *) skb_put(frag, hdrlen);
 		memcpy(fhdr, first->data, hdrlen);
 		if (i == num_fragm - 2)
@@ -1636,8 +1647,7 @@ static int ieee80211_master_start_xmit(struct sk_buff *skb,
 	}
 	osdata = IEEE80211_DEV_TO_SUB_IF(odev);
 
-	headroom = osdata->local->hw.extra_tx_headroom +
-		IEEE80211_ENCRYPT_HEADROOM;
+	headroom = osdata->local->tx_headroom + IEEE80211_ENCRYPT_HEADROOM;
 	if (skb_headroom(skb) < headroom) {
 		if (pskb_expand_head(skb, headroom, 0, GFP_ATOMIC)) {
 			dev_kfree_skb(skb);
@@ -1833,7 +1843,7 @@ static int ieee80211_subif_start_xmit(struct sk_buff *skb,
 	 * build in headroom in __dev_alloc_skb() (linux/skbuff.h) and
 	 * alloc_skb() (net/core/skbuff.c)
 	 */
-	head_need = hdrlen + encaps_len + local->hw.extra_tx_headroom;
+	head_need = hdrlen + encaps_len + local->tx_headroom;
 	head_need -= skb_headroom(skb);
 
 	/* We are going to modify skb data, so make a copy of it if happens to
@@ -1920,9 +1930,9 @@ ieee80211_mgmt_start_xmit(struct sk_buff *skb, struct net_device *dev)
 		return 0;
 	}
 
-	if (skb_headroom(skb) < sdata->local->hw.extra_tx_headroom) {
-		if (pskb_expand_head(skb,
-		    sdata->local->hw.extra_tx_headroom, 0, GFP_ATOMIC)) {
+	if (skb_headroom(skb) < sdata->local->tx_headroom) {
+		if (pskb_expand_head(skb, sdata->local->tx_headroom,
+				     0, GFP_ATOMIC)) {
 			dev_kfree_skb(skb);
 			return 0;
 		}
@@ -2061,12 +2071,12 @@ struct sk_buff * ieee80211_beacon_get(struct ieee80211_hw *hw, int if_id,
 	bh_len = ap->beacon_head_len;
 	bt_len = ap->beacon_tail_len;
 
-	skb = dev_alloc_skb(local->hw.extra_tx_headroom +
+	skb = dev_alloc_skb(local->tx_headroom +
 		bh_len + bt_len + 256 /* maximum TIM len */);
 	if (!skb)
 		return NULL;
 
-	skb_reserve(skb, local->hw.extra_tx_headroom);
+	skb_reserve(skb, local->tx_headroom);
 	memcpy(skb_put(skb, bh_len), b_head, bh_len);
 
 	ieee80211_include_sequence(sdata, (struct ieee80211_hdr *)skb->data);
@@ -4498,6 +4508,9 @@ void ieee80211_tx_status(struct ieee80211_hw *hw, struct sk_buff *skb,
 	struct ieee80211_local *local = hw_to_local(hw);
 	u16 frag, type;
 	u32 msg_type;
+	struct ieee80211_tx_status_rtap_hdr *rthdr;
+	struct ieee80211_sub_if_data *sdata;
+	int monitors;
 
 	if (!status) {
 		printk(KERN_ERR
@@ -4609,27 +4622,100 @@ void ieee80211_tx_status(struct ieee80211_hw *hw, struct sk_buff *skb,
 			local->dot11FailedCount++;
 	}
 
-	if (!(status->control.flags & IEEE80211_TXCTL_REQ_TX_STATUS)
-	    || unlikely(!local->apdev)) {
+	msg_type = (status->flags & IEEE80211_TX_STATUS_ACK) ?
+		ieee80211_msg_tx_callback_ack : ieee80211_msg_tx_callback_fail;
+
+	/* this was a transmitted frame, but now we want to reuse it */
+	skb_orphan(skb);
+
+	if ((status->control.flags & IEEE80211_TXCTL_REQ_TX_STATUS) &&
+	    local->apdev) {
+		if (local->monitors) {
+			skb2 = skb_clone(skb, GFP_ATOMIC);
+		} else {
+			skb2 = skb;
+			skb = NULL;
+		}
+
+		if (skb2)
+			/* Send frame to hostapd */
+			ieee80211_rx_mgmt(local, skb2, NULL, msg_type);
+
+		if (!skb)
+			return;
+	}
+
+	if (!local->monitors) {
 		dev_kfree_skb(skb);
 		return;
 	}
 
-	msg_type = (status->flags & IEEE80211_TX_STATUS_ACK) ?
-		ieee80211_msg_tx_callback_ack : ieee80211_msg_tx_callback_fail;
+	/* send frame to monitor interfaces now */
 
-	/* skb was the original skb used for TX. Clone it and give the clone
-	 * to netif_rx(). Free original skb. */
-	skb2 = skb_copy(skb, GFP_ATOMIC);
-	if (!skb2) {
+	if (skb_headroom(skb) < sizeof(*rthdr)) {
+		printk(KERN_ERR "ieee80211_tx_status: headroom too small\n");
 		dev_kfree_skb(skb);
 		return;
 	}
-	dev_kfree_skb(skb);
-	skb = skb2;
 
-	/* Send frame to hostapd */
-	ieee80211_rx_mgmt(local, skb, NULL, msg_type);
+	rthdr = (struct ieee80211_tx_status_rtap_hdr*)
+				skb_push(skb, sizeof(*rthdr));
+
+	memset(rthdr, 0, sizeof(*rthdr));
+	rthdr->hdr.it_len = cpu_to_le16(sizeof(*rthdr));
+	rthdr->hdr.it_present =
+		cpu_to_le32((1 << IEEE80211_RADIOTAP_TX_FLAGS) |
+			    (1 << IEEE80211_RADIOTAP_DATA_RETRIES));
+
+	if (!(status->flags & IEEE80211_TX_STATUS_ACK) &&
+	    !is_multicast_ether_addr(hdr->addr1))
+		rthdr->tx_flags |= cpu_to_le16(IEEE80211_RADIOTAP_F_TX_FAIL);
+
+	if ((status->control.flags & IEEE80211_TXCTL_USE_RTS_CTS) &&
+	    (status->control.flags & IEEE80211_TXCTL_USE_CTS_PROTECT))
+		rthdr->tx_flags |= cpu_to_le16(IEEE80211_RADIOTAP_F_TX_CTS);
+	else if (status->control.flags & IEEE80211_TXCTL_USE_RTS_CTS)
+		rthdr->tx_flags |= cpu_to_le16(IEEE80211_RADIOTAP_F_TX_RTS);
+
+	rthdr->data_retries = status->retry_count;
+
+	read_lock(&local->sub_if_lock);
+	monitors = local->monitors;
+	list_for_each_entry(sdata, &local->sub_if_list, list) {
+		/*
+		 * Using the monitors counter is possibly racy, but
+		 * if the value is wrong we simply either clone the skb
+		 * once too much or forget sending it to one monitor iface
+		 * The latter case isn't nice but fixing the race is much
+		 * more complicated.
+		 */
+		if (!monitors || !skb)
+			goto out;
+
+		if (sdata->type == IEEE80211_IF_TYPE_MNTR) {
+			if (!netif_running(sdata->dev))
+				continue;
+			monitors--;
+			if (monitors)
+				skb2 = skb_clone(skb, GFP_KERNEL);
+			else
+				skb2 = NULL;
+			skb->dev = sdata->dev;
+			/* XXX: is this sufficient for BPF? */
+			skb_set_mac_header(skb, 0);
+			skb->ip_summed = CHECKSUM_UNNECESSARY;
+			skb->pkt_type = PACKET_OTHERHOST;
+			skb->protocol = htons(ETH_P_802_2);
+			memset(skb->cb, 0, sizeof(skb->cb));
+			netif_rx(skb);
+			skb = skb2;
+			break;
+		}
+	}
+ out:
+	read_unlock(&local->sub_if_lock);
+	if (skb)
+		dev_kfree_skb(skb);
 }
 EXPORT_SYMBOL(ieee80211_tx_status);
 
@@ -4926,6 +5012,14 @@ int ieee80211_register_hw(struct ieee80211_hw *hw)
 		goto fail_workqueue;
 	}
 
+	/*
+	 * The hardware needs headroom for sending the frame,
+	 * and we need some headroom for passing the frame to monitor
+	 * interfaces, but never both at the same time.
+	 */
+	local->tx_headroom = max(local->hw.extra_tx_headroom,
+				 sizeof(struct ieee80211_tx_status_rtap_hdr));
+
 	debugfs_hw_add(local);
 
 	local->hw.conf.beacon_int = 1000;
diff --git a/net/mac80211/ieee80211_i.h b/net/mac80211/ieee80211_i.h
index af4d14d0b969..5a91e179efa0 100644
--- a/net/mac80211/ieee80211_i.h
+++ b/net/mac80211/ieee80211_i.h
@@ -392,6 +392,7 @@ struct ieee80211_local {
 	int monitors;
 	struct iw_statistics wstats;
 	u8 wstats_flags;
+	int tx_headroom; /* required headroom for hardware/radiotap */
 
 	enum {
 		IEEE80211_DEV_UNINITIALIZED = 0,