/** @file wlan_mac_packet_types.c
 *  @brief Packet Constructors
 *
 *  This contains code for constructing a variety of different types of MPDUs.
 *
 *  @copyright Copyright 2013-2019, Mango Communications. All rights reserved.
 *          Distributed under the Mango Communications Reference Design License
 *              See LICENSE.txt included in the design archive or
 *              at http://mangocomm.com/802.11/license
 *
 *  This file is part of the Mango 802.11 Reference Design (https://mangocomm.com/802.11)
 */

#include "wlan_mac_high_sw_config.h"

// Xilinx SDK includes
#include "stdio.h"
#include "stdlib.h"
#include "string.h"
#include "xil_types.h"

// WLAN includes
#include "wlan_mac_high.h"
#include "wlan_mac_802_11_defs.h"
#include "wlan_mac_mgmt_tags.h"
#include "wlan_mac_network_info.h"
#include "wlan_mac_packet_types.h"
#include "wlan_mac_common.h"


int wlan_create_beacon_probe_resp_frame(u8* pkt,
										u8* addr1,
										u8* addr2,
										u8* addr3,
										u8 frame_control_1,
										network_info_t* network_info) {

	ht_capabilities* ht_capabilities_element;
	ht_information* ht_information_element;
	wmm_parameter_t* wmm_parameter;

	//void* pkt_buf,mac_header_80211_common* common, u16 beacon_interval, u16 capabilities, u8 ssid_len, u8* ssid, u8 chan

	u32 packetLen_bytes;
	mgmt_tag_template_t* mgmt_tag_template;

	u8  real_ssid_len = WLAN_MIN(strlen(network_info->bss_config.ssid), SSID_LEN_MAX);

	mac_header_80211* mac_header;
	mac_header = (mac_header_80211*)(pkt);

	mac_header->frame_control_1 = frame_control_1;
	mac_header->frame_control_2 = 0;

	//This field may be overwritten by CPU_LOW
	mac_header->duration_id = 0;

	mac_header->sequence_control = 0; //Will be filled in at dequeue

	memcpy(mac_header->address_1, addr1, MAC_ADDR_LEN);
	memcpy(mac_header->address_2, addr2, MAC_ADDR_LEN);
	memcpy(mac_header->address_3, addr3, MAC_ADDR_LEN);

	beacon_probe_frame* beacon_probe_mgmt_header;
	beacon_probe_mgmt_header = (beacon_probe_frame*)(pkt + sizeof(mac_header_80211));

	//This field may be overwritten by CPU_LOW
	beacon_probe_mgmt_header->timestamp = 0;

	beacon_probe_mgmt_header->beacon_interval = network_info->bss_config.beacon_interval;
	beacon_probe_mgmt_header->capabilities    = ((network_info->capabilities & BSS_CAPABILITIES_BEACON_MASK) | CAPABILITIES_SHORT_TIMESLOT);

	mgmt_tag_template = (mgmt_tag_template_t*)( pkt + sizeof(mac_header_80211) + sizeof(beacon_probe_frame));


	mgmt_tag_template->header.tag_element_id = MGMT_TAG_SSID;
	mgmt_tag_template->header.tag_length = real_ssid_len;
	memcpy((void *)(mgmt_tag_template->data),network_info->bss_config.ssid,real_ssid_len);
	mgmt_tag_template = (void*)mgmt_tag_template + ( mgmt_tag_template->header.tag_length + sizeof(mgmt_tag_header) ); //Advance tag template forward

	//Top bit is whether or not the rate is mandatory (basic). Bottom 7 bits is in units of "number of 500kbps"
	mgmt_tag_template->header.tag_element_id = MGMT_TAG_SUPPORTED_RATES;
	mgmt_tag_template->header.tag_length = 8;
	mgmt_tag_template->data[0] = RATE_BASIC | (0x0C); 	//6Mbps  (BPSK,   1/2)
	mgmt_tag_template->data[1] = (0x12);				//9Mbps  (BPSK,   3/4)
	mgmt_tag_template->data[2] = RATE_BASIC | (0x18); 	//12Mbps (QPSK,   1/2)
	mgmt_tag_template->data[3] = (0x24); 				//18Mbps (QPSK,   3/4)
	mgmt_tag_template->data[4] = RATE_BASIC | (0x30); 	//24Mbps (16-QAM, 1/2)
	mgmt_tag_template->data[5] = (0x48); 				//36Mbps (16-QAM, 3/4)
	mgmt_tag_template->data[6] = (0x60); 				//48Mbps  (64-QAM, 2/3)
	mgmt_tag_template->data[7] = (0x6C); 				//54Mbps  (64-QAM, 3/4)
	mgmt_tag_template = (void*)mgmt_tag_template + ( mgmt_tag_template->header.tag_length + sizeof(mgmt_tag_header) ); //Advance tag template forward

	if (network_info->bss_config.ht_capable) {
		//Insert HT Capabilities and HT Information tags
		mgmt_tag_template->header.tag_element_id = MGMT_TAG_HT_CAPABILITIES;
		mgmt_tag_template->header.tag_length = 26;

		ht_capabilities_element = (ht_capabilities*)mgmt_tag_template->data;
		ht_capabilities_element->ht_capabilities_info = 0x000c;
		ht_capabilities_element->a_mpdu_parameters = 0x00;
		ht_capabilities_element->rx_supported_mcs[0] = 0x000000ff;
		ht_capabilities_element->rx_supported_mcs[1] = 0x00000000;
		ht_capabilities_element->rx_supported_mcs[2] = 0x00000000;
		ht_capabilities_element->rx_supported_mcs[3] = 0x00000000;
		ht_capabilities_element->ht_extended_capabilities = 0x0000;
		ht_capabilities_element->tx_beamforming = 0x0000;
		ht_capabilities_element->ant_sel = 0x00;

		mgmt_tag_template = (void*)mgmt_tag_template + ( mgmt_tag_template->header.tag_length + sizeof(mgmt_tag_header) ); //Advance tag template forward

		mgmt_tag_template->header.tag_element_id = MGMT_TAG_HT_OPERATION;
		mgmt_tag_template->header.tag_length = 22;

		ht_information_element = (ht_information*)mgmt_tag_template->data;
		ht_information_element->channel = wlan_mac_high_bss_channel_spec_to_radio_chan(network_info->bss_config.chan_spec);
		ht_information_element->ht_info_subset_1 = 0x00;	//only HT20 currently supported
		ht_information_element->ht_info_subset_2 = 0x0004; //One or more STAs are not greenfield compatible
		ht_information_element->ht_info_subset_3 = 0x0000;
		ht_information_element->rx_supported_mcs[0] = 0x00000000;
		ht_information_element->rx_supported_mcs[1] = 0x00000000;
		ht_information_element->rx_supported_mcs[2] = 0x00000000;
		ht_information_element->rx_supported_mcs[3] = 0x00000000;

		mgmt_tag_template = (void*)mgmt_tag_template + ( mgmt_tag_template->header.tag_length + sizeof(mgmt_tag_header) ); //Advance tag template forward
	}

	mgmt_tag_template->header.tag_element_id = MGMT_TAG_ERP;
	mgmt_tag_template->header.tag_length = 1;
	mgmt_tag_template->data[0] = 0; //Non ERP Present - not set, don't use protection, no barker preamble mode
	mgmt_tag_template = (void*)mgmt_tag_template + ( mgmt_tag_template->header.tag_length + sizeof(mgmt_tag_header) ); //Advance tag template forward

	if (network_info->bss_config.ht_capable) {
		//Insert WMM tag
		mgmt_tag_template->header.tag_element_id = MGMT_TAG_VENDOR_SPECIFIC;
		mgmt_tag_template->header.tag_length = 24;

		wmm_parameter = (wmm_parameter_t*)mgmt_tag_template->data;
		wmm_parameter->oui[0] = 0x00;
		wmm_parameter->oui[1] = 0x50;
		wmm_parameter->oui[2] = 0xf2;
		wmm_parameter->vendor_specific_oui_type = 2;
		wmm_parameter->wme_subtype = 1;
		wmm_parameter->wme_version = 1;
		wmm_parameter->wme_qos_info = 0x08;
		wmm_parameter->reserved = 0;
		wmm_parameter->aci0	= Xil_Htonl(0x03a40000);
		wmm_parameter->aci1 = Xil_Htonl(0x27a40000);
		wmm_parameter->aci2 = Xil_Htonl(0x42430000);
		wmm_parameter->aci3 = Xil_Htonl(0x62320000);

		mgmt_tag_template = (void*)mgmt_tag_template + ( mgmt_tag_template->header.tag_length + sizeof(mgmt_tag_header) ); //Advance tag template forward
	}

	packetLen_bytes = ((u8*)mgmt_tag_template - pkt) + WLAN_PHY_FCS_NBYTES;

	return packetLen_bytes;

}



int wlan_create_probe_req_frame(u8* pkt,
								u8* addr1,
								u8* addr2,
								u8* addr3,
								char* ssid){
	u32 packetLen_bytes;
	u8* txBufferPtr_u8;
	u8 real_ssid_len = WLAN_MIN(strlen(ssid), SSID_LEN_MAX);

	mac_header_80211* probe_req_80211_header;

	if( pkt == NULL ) return WLAN_FAILURE;

	probe_req_80211_header = (mac_header_80211*)(pkt);

	probe_req_80211_header->frame_control_1 = MAC_FRAME_CTRL1_SUBTYPE_PROBE_REQ;
	probe_req_80211_header->frame_control_2 = 0;

	//This field may be overwritten by CPU_LOW
	probe_req_80211_header->duration_id = 0;

	memcpy(probe_req_80211_header->address_1, addr1, MAC_ADDR_LEN);
	memcpy(probe_req_80211_header->address_2, addr2, MAC_ADDR_LEN);
	memcpy(probe_req_80211_header->address_3, addr3, MAC_ADDR_LEN);

	probe_req_80211_header->sequence_control = 0; //Will be filled in at dequeue

	txBufferPtr_u8 = (u8*)((u8*)(pkt) + sizeof(mac_header_80211));
	txBufferPtr_u8[0] = 0; //Tag 0: SSID parameter set
	txBufferPtr_u8[1] = real_ssid_len;
	memcpy((void *)(&(txBufferPtr_u8[2])),(void *)(&ssid[0]),real_ssid_len);

	txBufferPtr_u8+=(real_ssid_len+2); //Move up to next tag

	//http://my.safaribooksonline.com/book/networking/wireless/0596100523/4dot-802dot11-framing-in-detail/wireless802dot112-chp-4-sect-3
	//Top bit is whether or not the rate is mandatory (basic). Bottom 7 bits is in units of "number of 500kbps"
	txBufferPtr_u8[0] = 1; //Tag 1: Supported Rates
	txBufferPtr_u8[1] = 8; //tag length... doesn't include the tag itself and the tag length
	txBufferPtr_u8[2] = (0x0C); 				//6Mbps  (BPSK,   1/2)
	txBufferPtr_u8[3] = (0x12);				 	//9Mbps  (BPSK,   3/4)
	txBufferPtr_u8[4] = (0x18); 				//12Mbps (QPSK,   1/2)
	txBufferPtr_u8[5] = (0x24); 				//18Mbps (QPSK,   3/4)
	txBufferPtr_u8[6] = (0x30); 				//24Mbps (16-QAM, 1/2)
	txBufferPtr_u8[7] = (0x48); 				//36Mbps (16-QAM, 3/4)
	txBufferPtr_u8[8] = (0x60); 				//48Mbps  (64-QAM, 2/3)
	txBufferPtr_u8[9] = (0x6C); 				//54Mbps  (64-QAM, 3/4)
	txBufferPtr_u8+=(8+2); //Move up to next tag

	packetLen_bytes = (txBufferPtr_u8 - pkt) + WLAN_PHY_FCS_NBYTES;

	return packetLen_bytes;
}

int wlan_create_auth_frame(u8* pkt,
						   u8* addr1,
						   u8* addr2,
						   u8* addr3,
						   u16 auth_algorithm,
						   u16 auth_seq,
						   u16 status_code){
	u32 packetLen_bytes;
	mac_header_80211* auth_80211_header;

	if(pkt == NULL) return WLAN_FAILURE;

	auth_80211_header = (mac_header_80211*)(pkt);

	auth_80211_header->frame_control_1 = MAC_FRAME_CTRL1_SUBTYPE_AUTH;
	auth_80211_header->frame_control_2 = 0;

	//duration can be filled in by CPU_LOW
	auth_80211_header->duration_id = 0;
	memcpy(auth_80211_header->address_1, addr1, MAC_ADDR_LEN);
	memcpy(auth_80211_header->address_2, addr2, MAC_ADDR_LEN);
	memcpy(auth_80211_header->address_3, addr3, MAC_ADDR_LEN);

	auth_80211_header->sequence_control = 0; //Will be filled in at dequeue

	authentication_frame* auth_mgmt_header;
	auth_mgmt_header = (authentication_frame*)(pkt + sizeof(mac_header_80211));
	auth_mgmt_header->auth_algorithm = auth_algorithm;
	auth_mgmt_header->auth_sequence = auth_seq;
	auth_mgmt_header->status_code = status_code;

	packetLen_bytes = sizeof(mac_header_80211) + sizeof(authentication_frame) + WLAN_PHY_FCS_NBYTES;

	return packetLen_bytes;
}



int wlan_create_deauth_disassoc_frame(u8* pkt,
									  u8* addr1,
									  u8* addr2,
									  u8* addr3,
									  u8 frame_control_1,
									  u16 reason_code){
	u32 packetLen_bytes;
	mac_header_80211* deauth_80211_header;

	if( pkt == NULL ) return WLAN_FAILURE;

	deauth_80211_header = (mac_header_80211*)(pkt);

	deauth_80211_header->frame_control_1 = frame_control_1;
	deauth_80211_header->frame_control_2 = 0;

	//duration can be filled in by CPU_LOW
	deauth_80211_header->duration_id = 0;
	memcpy(deauth_80211_header->address_1, addr1, MAC_ADDR_LEN);
	memcpy(deauth_80211_header->address_2, addr2, MAC_ADDR_LEN);
	memcpy(deauth_80211_header->address_3, addr3, MAC_ADDR_LEN);

	deauth_80211_header->sequence_control = 0; //Will be filled in at dequeue

	deauthentication_frame* deauth_mgmt_header;
	deauth_mgmt_header = (deauthentication_frame*)(pkt + sizeof(mac_header_80211));
	deauth_mgmt_header->reason_code = reason_code;

	packetLen_bytes = sizeof(mac_header_80211) + sizeof(deauthentication_frame) + WLAN_PHY_FCS_NBYTES;

	return packetLen_bytes;
}



int wlan_create_reassoc_assoc_req_frame(u8* pkt,
										u8* addr1,
										u8* addr2,
										u8* addr3,
										u8 frame_control_1,
										network_info_t* network_info){
	u32 packetLen_bytes;

	ht_capabilities* ht_capabilities_element;
	ht_information* ht_information_element;

	u8  real_ssid_len = WLAN_MIN(strlen(network_info->bss_config.ssid), SSID_LEN_MAX);
	mgmt_tag_template_t* mgmt_tag_template;

	if( pkt == NULL ) return WLAN_FAILURE;

	mac_header_80211* assoc_80211_header;
	assoc_80211_header = (mac_header_80211*)(pkt);

	assoc_80211_header->frame_control_1 = frame_control_1;
	assoc_80211_header->frame_control_2 = 0;
	//duration can be filled in by CPU_LOW
	assoc_80211_header->duration_id = 0;

	memcpy(assoc_80211_header->address_1, addr1, MAC_ADDR_LEN);
	memcpy(assoc_80211_header->address_2, addr2, MAC_ADDR_LEN);
	memcpy(assoc_80211_header->address_3, addr3, MAC_ADDR_LEN);

	assoc_80211_header->sequence_control = 0; //Will be filled in at dequeue

	association_request_frame* association_req_mgmt_header;
	association_req_mgmt_header = (association_request_frame*)(pkt + sizeof(mac_header_80211));
	association_req_mgmt_header->capabilities = (CAPABILITIES_ESS | CAPABILITIES_SHORT_TIMESLOT | CAPABILITIES_SHORT_PREAMBLE);

	//This value is defined in 802.11-2012 in 8.4.1.6 and tells the AP how many beacon intervals
	//this station intends to doze before waking. In our implementation we hardcode this to 1
	//to represent that we will be awake at every beacon target time.
	association_req_mgmt_header->listen_interval = 0x0001;

	mgmt_tag_template = (mgmt_tag_template_t *)( pkt + sizeof(mac_header_80211) + sizeof(association_request_frame) );


	mgmt_tag_template->header.tag_element_id = MGMT_TAG_SSID;
	mgmt_tag_template->header.tag_length = real_ssid_len;
	memcpy((void *)(mgmt_tag_template->data),network_info->bss_config.ssid,real_ssid_len);
	mgmt_tag_template = (void*)mgmt_tag_template + ( mgmt_tag_template->header.tag_length + sizeof(mgmt_tag_header) ); //Advance tag template forward

	//Top bit is whether or not the rate is mandatory (basic). Bottom 7 bits is in units of "number of 500kbps"
	//Note: these parameters are spoofed. The 802.11 Reference Design does not support the 802.11b rates (with the exception
	//of Rx of DSSS 1Mbps). However, most commercial APs will decline a STA from joining if they don't advertise support
	//for the nominal set of 802.11b basic rates.
	mgmt_tag_template->header.tag_element_id = MGMT_TAG_SUPPORTED_RATES;
	mgmt_tag_template->header.tag_length = 8;
	mgmt_tag_template->data[0] = RATE_BASIC | (0x02); 	//1Mbps
	mgmt_tag_template->data[1] = RATE_BASIC | (0x04);	//2Mbps
	mgmt_tag_template->data[2] = RATE_BASIC | (0x0B);	//5.5Mbps
	mgmt_tag_template->data[3] = RATE_BASIC | (0x16);	//11Mbps
	mgmt_tag_template->data[4] = (0x24); 				//18Mbps
	mgmt_tag_template->data[5] = (0x30); 				//24Mbps
	mgmt_tag_template->data[6] = (0x48); 				//36Mbps
	mgmt_tag_template->data[7] = (0x6C); 				//54Mbps
	mgmt_tag_template = (void*)mgmt_tag_template + ( mgmt_tag_template->header.tag_length + sizeof(mgmt_tag_header) ); //Advance tag template forward

	mgmt_tag_template->header.tag_element_id = MGMT_TAG_EXTENDED_SUPPORTED_RATES;
	mgmt_tag_template->header.tag_length = 4;
	mgmt_tag_template->data[0] = (0x0c); 				//6Mbps
	mgmt_tag_template->data[1] = (0x12);					//9Mbps
	mgmt_tag_template->data[2] = (0x18);					//12Mbps
	mgmt_tag_template->data[3] = (0x60);					//48Mbps
	mgmt_tag_template = (void*)mgmt_tag_template + ( mgmt_tag_template->header.tag_length + sizeof(mgmt_tag_header) ); //Advance tag template forward

	if (network_info->bss_config.ht_capable) {
		//Note: This is the only place in the code where a STA decides whether or not to advertise that it is
		// capable of HT rates. If it is joining a non-HT capable AP, it will omit these tags and pretend that
		// it is only capable of transmitting and receiving the non-HT rates.

		//Insert HT Capabilities and HT Information tags
		mgmt_tag_template->header.tag_element_id = MGMT_TAG_HT_CAPABILITIES;
		mgmt_tag_template->header.tag_length = 26;

		ht_capabilities_element = (ht_capabilities*)mgmt_tag_template->data;
		ht_capabilities_element->ht_capabilities_info = 0x000c;
		ht_capabilities_element->a_mpdu_parameters = 0x00;
		ht_capabilities_element->rx_supported_mcs[0] = 0x000000ff;
		ht_capabilities_element->rx_supported_mcs[1] = 0x00000000;
		ht_capabilities_element->rx_supported_mcs[2] = 0x00000000;
		ht_capabilities_element->rx_supported_mcs[3] = 0x00000000;
		ht_capabilities_element->ht_extended_capabilities = 0x0000;
		ht_capabilities_element->tx_beamforming = 0x0000;
		ht_capabilities_element->ant_sel = 0x00;

		mgmt_tag_template = (void*)mgmt_tag_template + ( mgmt_tag_template->header.tag_length + sizeof(mgmt_tag_header) ); //Advance tag template forward

		mgmt_tag_template->header.tag_element_id = MGMT_TAG_HT_OPERATION;
		mgmt_tag_template->header.tag_length = 22;

		ht_information_element = (ht_information*)mgmt_tag_template->data;
		ht_information_element->channel = wlan_mac_high_bss_channel_spec_to_radio_chan(network_info->bss_config.chan_spec);
		ht_information_element->ht_info_subset_1 = 0x00;
		ht_information_element->ht_info_subset_2 = 0x0004; //One or more STAs are not greenfield compatible
		ht_information_element->ht_info_subset_3 = 0x0000;
		ht_information_element->rx_supported_mcs[0] = 0x00000000;
		ht_information_element->rx_supported_mcs[1] = 0x00000000;
		ht_information_element->rx_supported_mcs[2] = 0x00000000;
		ht_information_element->rx_supported_mcs[3] = 0x00000000;

		mgmt_tag_template = (void*)mgmt_tag_template + ( mgmt_tag_template->header.tag_length + sizeof(mgmt_tag_header) ); //Advance tag template forward
	}
	packetLen_bytes = ((u8*)mgmt_tag_template - pkt) + WLAN_PHY_FCS_NBYTES;

	return packetLen_bytes;
}

int wlan_create_association_response_frame(u8* pkt,
										   u8* addr1,
										   u8* addr2,
										   u8* addr3,
										   u16 status,
										   u16 AID,
										   network_info_t* network_info) {
	u32 packetLen_bytes;

	ht_capabilities* ht_capabilities_element;
	ht_information* ht_information_element;
	wmm_parameter_t* wmm_parameter;

	mgmt_tag_template_t* mgmt_tag_template;

	mac_header_80211* assoc_80211_header;

	if(pkt == NULL) return WLAN_FAILURE;

	assoc_80211_header = (mac_header_80211*)(pkt);

	assoc_80211_header->frame_control_1 = MAC_FRAME_CTRL1_SUBTYPE_ASSOC_RESP;
	assoc_80211_header->frame_control_2 = 0;
	//duration can be filled in by CPU_LOW
	assoc_80211_header->duration_id = 0;

	memcpy(assoc_80211_header->address_1, addr1, MAC_ADDR_LEN);
	memcpy(assoc_80211_header->address_2, addr2, MAC_ADDR_LEN);
	memcpy(assoc_80211_header->address_3, addr3, MAC_ADDR_LEN);

	assoc_80211_header->sequence_control = 0; //Will be filled in at dequeue

	association_response_frame* association_resp_mgmt_header;
	association_resp_mgmt_header = (association_response_frame*)(pkt + sizeof(mac_header_80211));
	association_resp_mgmt_header->capabilities = (CAPABILITIES_ESS | CAPABILITIES_SHORT_TIMESLOT);

	association_resp_mgmt_header->status_code = status;
	association_resp_mgmt_header->association_id = 0xC000 | AID;

	mgmt_tag_template = (mgmt_tag_template_t *)( pkt + sizeof(mac_header_80211) + sizeof(association_response_frame) );

	mgmt_tag_template->header.tag_element_id = MGMT_TAG_SUPPORTED_RATES;
	mgmt_tag_template->header.tag_length = 8;
	mgmt_tag_template->data[0] = RATE_BASIC | (0x0C);   //6Mbps  (BPSK,    1/2)
	mgmt_tag_template->data[1] = (0x12);                //9Mbps  (BPSK,    3/4)
	mgmt_tag_template->data[2] = RATE_BASIC | (0x18);   //12Mbps (QPSK,    1/2)
	mgmt_tag_template->data[3] = (0x24);                //18Mbps (QPSK,    3/4)
	mgmt_tag_template->data[4] = RATE_BASIC | (0x30);   //24Mbps (16-QAM,  1/2)
	mgmt_tag_template->data[5] = (0x48);                //36Mbps (16-QAM,  3/4)
	mgmt_tag_template->data[6] = (0x60);                //48Mbps (64-QAM,  2/3)
	mgmt_tag_template->data[7] = (0x6C);                //54Mbps (64-QAM,  3/4)
	mgmt_tag_template = (void*)mgmt_tag_template + ( mgmt_tag_template->header.tag_length + sizeof(mgmt_tag_header) ); //Advance tag template forward

	if (network_info->bss_config.ht_capable) {
		//Insert HT Capabilities and HT Information tags
		mgmt_tag_template->header.tag_element_id = MGMT_TAG_HT_CAPABILITIES;
		mgmt_tag_template->header.tag_length = 26;

		ht_capabilities_element = (ht_capabilities*)mgmt_tag_template->data;
		ht_capabilities_element->ht_capabilities_info = 0x000c;
		ht_capabilities_element->a_mpdu_parameters = 0x00;
		ht_capabilities_element->rx_supported_mcs[0] = 0x000000ff;
		ht_capabilities_element->rx_supported_mcs[1] = 0x00000000;
		ht_capabilities_element->rx_supported_mcs[2] = 0x00000000;
		ht_capabilities_element->rx_supported_mcs[3] = 0x00000000;
		ht_capabilities_element->ht_extended_capabilities = 0x0000;
		ht_capabilities_element->tx_beamforming = 0x0000;
		ht_capabilities_element->ant_sel = 0x00;

		mgmt_tag_template = (void*)mgmt_tag_template + ( mgmt_tag_template->header.tag_length + sizeof(mgmt_tag_header) ); //Advance tag template forward

		mgmt_tag_template->header.tag_element_id = MGMT_TAG_HT_OPERATION;
		mgmt_tag_template->header.tag_length = 22;

		ht_information_element = (ht_information*)mgmt_tag_template->data;
		ht_information_element->channel = wlan_mac_high_bss_channel_spec_to_radio_chan(network_info->bss_config.chan_spec);
		ht_information_element->ht_info_subset_1 = 0x00;
		ht_information_element->ht_info_subset_2 = 0x0004; //One or more STAs are not greenfield compatible
		ht_information_element->ht_info_subset_3 = 0x0000;
		ht_information_element->rx_supported_mcs[0] = 0x00000000;
		ht_information_element->rx_supported_mcs[1] = 0x00000000;
		ht_information_element->rx_supported_mcs[2] = 0x00000000;
		ht_information_element->rx_supported_mcs[3] = 0x00000000;

		mgmt_tag_template = (void*)mgmt_tag_template + ( mgmt_tag_template->header.tag_length + sizeof(mgmt_tag_header) ); //Advance tag template forward

		//Insert WMM tag
		mgmt_tag_template->header.tag_element_id = MGMT_TAG_VENDOR_SPECIFIC;
		mgmt_tag_template->header.tag_length = 24;

		wmm_parameter = (wmm_parameter_t*)mgmt_tag_template->data;
		wmm_parameter->oui[0] = 0x00;
		wmm_parameter->oui[1] = 0x50;
		wmm_parameter->oui[2] = 0xf2;
		wmm_parameter->vendor_specific_oui_type = 2;
		wmm_parameter->wme_subtype = 1;
		wmm_parameter->wme_version = 1;
		wmm_parameter->wme_qos_info = 0x08;
		wmm_parameter->reserved = 0;
		wmm_parameter->aci0	= Xil_Htonl(0x03a40000);
		wmm_parameter->aci1 = Xil_Htonl(0x27a40000);
		wmm_parameter->aci2 = Xil_Htonl(0x42430000);
		wmm_parameter->aci3 = Xil_Htonl(0x62320000);

		mgmt_tag_template = (void*)mgmt_tag_template + ( mgmt_tag_template->header.tag_length + sizeof(mgmt_tag_header) ); //Advance tag template forward
	}

	packetLen_bytes = ((u8*)mgmt_tag_template - pkt) + WLAN_PHY_FCS_NBYTES;

	return packetLen_bytes;
}



int wlan_create_data_frame_header(u8* pkt,
								  u8* addr1,
								  u8* addr2,
								  u8* addr3,
								  u8 frame_control_2) {

	if(pkt == NULL) return WLAN_FAILURE;

	mac_header_80211* data_80211_header;
	data_80211_header = (mac_header_80211*)(pkt);

	data_80211_header->frame_control_1 = MAC_FRAME_CTRL1_SUBTYPE_DATA;
	data_80211_header->frame_control_2 = frame_control_2;

	data_80211_header->duration_id = 0;

	memcpy(data_80211_header->address_1, addr1, MAC_ADDR_LEN);
	memcpy(data_80211_header->address_2, addr2, MAC_ADDR_LEN);
	memcpy(data_80211_header->address_3, addr3, MAC_ADDR_LEN);

	data_80211_header->sequence_control = 0; //Will be filled in at dequeue

	return (sizeof(mac_header_80211) + WLAN_PHY_FCS_NBYTES) ;
}