#include "main.h"

/* parse_radiotap - parses the radiotap header
 *
 * Returns the length of the radiotap header, or <0 on error.
 */
int parse_radiotap(struct sk_buff *skb, struct packet_info *pinfo __unused)
{
	struct ieee80211_radiotap_iterator iterator;
	struct ieee80211_radiotap_header *rthdr;
	int found_signal_strength = 0;
	int rthdr_len;
	int ret;

	if (!pskb_may_pull(skb, sizeof(*rthdr)))
		return -1;

	rthdr = (struct ieee80211_radiotap_header *) skb->data;
	rthdr_len = ieee80211_get_radiotap_len((u8 *)rthdr);
	if (rthdr_len < 0)
		return -1;

	if (!pskb_may_pull(skb, rthdr_len))
		return -1;

	ret = ieee80211_radiotap_iterator_init(&iterator, rthdr, rthdr_len, NULL);

	while (!ret) {
		ret = ieee80211_radiotap_iterator_next(&iterator);

		if (ret)
			continue;

		switch (iterator.this_arg_index) {
		case IEEE80211_RADIOTAP_RATE:
			pinfo->bitrate = (u8) *iterator.this_arg;
			break;
		case IEEE80211_RADIOTAP_MCS:
			pinfo->mcs = ((u8 *)iterator.this_arg)[2];
			break;
		case IEEE80211_RADIOTAP_DBM_ANTSIGNAL:
			found_signal_strength = 1;
			break;
		default:
			break;
		}
	}

	/* we did not stop because we parsed all fields */
	if (ret != -ENOENT)
		return ret;

	/* NOTE: yes, it would have been easier to just check rthdr->it_present
	 * for the flag. */
	if (!found_signal_strength)
		return -1;

	skb_pull(skb, rthdr_len);

	return 0;
}

struct radiotap_builder {
	struct ieee80211_radiotap_header *rthdr;
	int last_field;
	u8 *pos;
};

/* out of net/radiotap.c */
extern const struct radiotap_align_size rtap_namespace_sizes[];

void build_radiotap_field(struct radiotap_builder *b, u8 field, u8 *val)
{
	assert(field > b->last_field);

	b->rthdr->it_present |= cpu_to_le32(1 << field);
	b->last_field = field;
	
	while (((unsigned long)b->pos) % rtap_namespace_sizes[field].align)
		b->pos++;

	memcpy(b->pos, val, rtap_namespace_sizes[field].size);
	b->pos += rtap_namespace_sizes[field].size;
	b->rthdr->it_len = cpu_to_le16(b->pos - ((u8 *) b->rthdr));
}

/* build_radiotap_header - builds a radiotap header in front of data
 * @data: already built data (i.e. ieee80211 header)
 * @head: headroom which can be used for radiotap data
 *
 * Returns <0 on error or number of bytes used in the headroom
 */
int build_radiotap(struct sk_buff *skb, struct packet_info *pinfo)
{
	struct radiotap_builder b;
	u8 buf[128];
	__le16 tx_flags;
	u8 retries;
	u8 mcs_info[3];
	size_t rthdr_len;


	memset(&b, 0, sizeof(b));
	memset(buf, 0, sizeof(buf));
	b.rthdr = (struct ieee80211_radiotap_header *) buf;
	b.pos = (u8 *)(b.rthdr + 1);

	if (pinfo->bitrate)
		build_radiotap_field(&b, IEEE80211_RADIOTAP_RATE, &pinfo->bitrate);

//	tx_flags = IEEE80211_RADIOTAP_F_TX_NOACK;	/* TX_FLAGS */
	tx_flags = 0;
	build_radiotap_field(&b, IEEE80211_RADIOTAP_TX_FLAGS, (u8 *)&tx_flags);
	retries = 0;
	build_radiotap_field(&b, IEEE80211_RADIOTAP_DATA_RETRIES, &retries);

	if (!pinfo->bitrate) {
		mcs_info[0] =   IEEE80211_RADIOTAP_MCS_HAVE_BW \
			      | IEEE80211_RADIOTAP_MCS_HAVE_MCS \
			      | IEEE80211_RADIOTAP_MCS_HAVE_GI \
			      | IEEE80211_RADIOTAP_MCS_HAVE_STBC;
		mcs_info[1] = 0;
		mcs_info[2] = pinfo->mcs;
		build_radiotap_field(&b, IEEE80211_RADIOTAP_MCS, mcs_info);
	}

	rthdr_len = ieee80211_get_radiotap_len((u8 *)b.rthdr);
	if (skb_headroom(skb) < rthdr_len)
		return -1;

	skb_push(skb, rthdr_len);

	memcpy(skb->data, b.rthdr, rthdr_len);

	return 0;
}
