[PATCH] mac80211: Monitor mode radiotap-based packet injection

#include <linux/compiler.h>

#include <linux/bitmap.h>

#include <net/cfg80211.h>

#include <asm/unaligned.h>

#include "ieee80211_common.h"

#include "ieee80211_i.h"

}

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 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,

{

	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));

	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_parse_tx_radiotap(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))

		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)];

	}

	control->flags |= IEEE80211_TXCTL_FIRST_FRAGMENT;

	return res;

}

static int inline is_ieee80211_device(struct net_device *dev,

	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));

		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++) {

	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 */

		goto fail;

	}

	if (unlikely(sdata->type == IEEE80211_IF_TYPE_MNTR)) {

		struct ieee80211_radiotap_header *prthdr =

			(struct ieee80211_radiotap_header *)skb->data;

		u16 len;

		/*

		 * there must be a radiotap header at the

		 * start in this case

		 */

		if (unlikely(prthdr->it_version)) {

			/* only version 0 is supported  */

			ret = 0;

			goto fail;

		}

		skb->dev = local->mdev;

		pkt_data = (struct ieee80211_tx_packet_data *)skb->cb;

		memset(pkt_data, 0, sizeof(*pkt_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

		 */

		len = le16_to_cpu(get_unaligned(&prthdr->it_len));

		skb_set_mac_header(skb, len);

		skb_set_network_header(skb, len + sizeof(hdr));

		skb_set_transport_header(skb, len + sizeof(hdr));

		/*

		 * pass the radiotap header up to

		 * the next stage intact

		 */

		dev_queue_xmit(skb);

		return 0;

	}

	nh_pos = skb_network_header(skb) - skb->data;

	h_pos = skb_transport_header(skb) - skb->data;