#include "main.h"

int parse_ieee80211(struct sk_buff *skb, struct packet_info *pinfo)
{
	struct ieee80211_llc_hdr *llc_hdr;
	struct ieee80211_qos_hdr *ieee80211_hdr;
	int ieee80211_hdr_len = -1;
	int llc_hdr_len;
	__le16 fc;

	if (!pskb_may_pull(skb, sizeof(fc))) {
		fprintf(stderr, "frame control field not present?\n");
		return -1;
	}

	ieee80211_hdr = (struct ieee80211_qos_hdr *) skb->data;
	fc = ieee80211_hdr->frame_control;

	if (!(ieee80211_is_data(fc) || ieee80211_is_data_qos(fc))) {
//		fprintf(stderr, "no data frame, ignore\n");
		return -1;
	}

	if (ieee80211_has_protected(fc)) {
//		fprintf(stderr, "protected, ignore\n");
		return -1;
	}

	ieee80211_hdr_len = 24;
	if (ieee80211_has_a4(fc)) 
		ieee80211_hdr_len += 6;

	if (ieee80211_is_data_qos(fc))
		ieee80211_hdr_len += 2;

//	fprintf(stderr, "ieee80211_hdr_len = %d\n", ieee80211_hdr_len);

	if (!pskb_may_pull(skb, ieee80211_hdr_len)) {
		fprintf(stderr, "ieee80211 header too short?\n");
		return -1;
	}

	llc_hdr = (struct ieee80211_llc_hdr *) (((u8 *) ieee80211_hdr) + ieee80211_hdr_len);
	llc_hdr_len = sizeof(struct ieee80211_llc_hdr);

	if (!pskb_may_pull(skb, ieee80211_hdr_len + llc_hdr_len)) {
//		fprintf(stderr, "packet not long enough for llc header\n");
		return -1;
	}

	if (ntohs(llc_hdr->ethernet_type) != ANALYSIS_PTYPE)
		return -1;

	memcpy(pinfo->mac_dst, ieee80211_hdr->addr1, ETH_ALEN);
	memcpy(pinfo->mac_src, ieee80211_hdr->addr2, ETH_ALEN);

	skb_pull(skb, ieee80211_hdr_len + llc_hdr_len);
	
	return 0;
}

/* build_ieee80211_header - builds an ieee80211/llc header in front of data
 * @data: ethernet header/payload
 * @head: headroom which can be used for header data
 *
 * Returns <0 on error or number of bytes used in the headroom
 */
int build_ieee80211(struct sk_buff *skb, struct packet_info *pinfo)
{
	struct ieee80211_qos_hdr *ieee80211_hdr;
	struct ieee80211_llc_hdr *llc_hdr;

	if (skb_headroom(skb) < sizeof(*ieee80211_hdr) + sizeof(*llc_hdr))
		return -1;

	llc_hdr = (struct ieee80211_llc_hdr *) skb_push(skb, sizeof(*llc_hdr));

	llc_hdr->dsap = 0xaa;
	llc_hdr->ssap = 0xaa;
	llc_hdr->control_field = 0x03;
	memset(&llc_hdr->organization_code, 0,
	       sizeof(llc_hdr->organization_code));
	/* leave llc_hdr->ether_type, it's at the right position already. */
	llc_hdr->ethernet_type = htons(ANALYSIS_PTYPE);

	ieee80211_hdr = (struct ieee80211_qos_hdr *) skb_push(skb, sizeof(*ieee80211_hdr));

	ieee80211_hdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_DATA | IEEE80211_STYPE_QOS_DATA);
	memcpy(ieee80211_hdr->addr1, pinfo->mac_dst, ETH_ALEN);
	memcpy(ieee80211_hdr->addr2, pinfo->mac_src, ETH_ALEN);
	memcpy(ieee80211_hdr->addr3, "\x02\x41\xbc\xf7\x3a\x18", ETH_ALEN); /* TODO: read from wireless config */
	ieee80211_hdr->seq_ctrl = cpu_to_le16(0); /* TODO */
//	ieee80211_hdr->qos_ctrl = cpu_to_le16(IEEE80211_QOS_CTL_ACK_POLICY_NOACK);
	ieee80211_hdr->qos_ctrl = cpu_to_le16(0);
	ieee80211_hdr->duration_id = cpu_to_le16(is_multicast_ether_addr(ieee80211_hdr->addr1) ? 0 : 44); /* TODO: calculate */
//	ieee80211_hdr->duration_id = 10000;

	return 0;
}

