/*
 *************************************************************************
 * Ralink Tech Inc.
 * 5F., No.36, Taiyuan St., Jhubei City,
 * Hsinchu County 302,
 * Taiwan, R.O.C.
 *
 * (c) Copyright 2002-2007, Ralink Technology, Inc.
 *
 * This program is free software; you can redistribute it and/or modify  *
 * it under the terms of the GNU General Public License as published by  *
 * the Free Software Foundation; either version 2 of the License, or     *
 * (at your option) any later version.                                   *
 *                                                                       *
 * This program is distributed in the hope that it will be useful,       *
 * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 * GNU General Public License for more details.                          *
 *                                                                       *
 * You should have received a copy of the GNU General Public License     *
 * along with this program; if not, write to the                         *
 * Free Software Foundation, Inc.,                                       *
 * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 *                                                                       *
 *************************************************************************

	Module Name:
	wpa.c

	Abstract:

	Revision History:
	Who			When			What
	--------	----------		----------------------------------------------
	Jan	Lee		03-07-22		Initial
	Paul Lin	03-11-28		Modify for supplicant
*/
#include "../rt_config.h"

#define		WPARSNIE	0xdd
#define		WPA2RSNIE	0x30

//extern UCHAR BIT8[];
UCHAR	CipherWpaPskTkip[] = {
		0xDD, 0x16,				// RSN IE
		0x00, 0x50, 0xf2, 0x01,	// oui
		0x01, 0x00,				// Version
		0x00, 0x50, 0xf2, 0x02,	// Multicast
		0x01, 0x00,				// Number of unicast
		0x00, 0x50, 0xf2, 0x02,	// unicast
		0x01, 0x00,				// number of authentication method
		0x00, 0x50, 0xf2, 0x02	// authentication
		};
UCHAR	CipherWpaPskTkipLen = (sizeof(CipherWpaPskTkip) / sizeof(UCHAR));

UCHAR	CipherWpaPskAes[] = {
		0xDD, 0x16, 			// RSN IE
		0x00, 0x50, 0xf2, 0x01,	// oui
		0x01, 0x00,				// Version
		0x00, 0x50, 0xf2, 0x04,	// Multicast
		0x01, 0x00,				// Number of unicast
		0x00, 0x50, 0xf2, 0x04,	// unicast
		0x01, 0x00,				// number of authentication method
		0x00, 0x50, 0xf2, 0x02	// authentication
		};
UCHAR	CipherWpaPskAesLen = (sizeof(CipherWpaPskAes) / sizeof(UCHAR));

UCHAR	CipherSuiteCiscoCCKM[] = {
		0xDD, 0x16,				// RSN IE
		0x00, 0x50, 0xf2, 0x01, // oui
		0x01, 0x00,				// Version
		0x00, 0x40, 0x96, 0x01, // Multicast
		0x01, 0x00,				// Number of uicast
		0x00, 0x40, 0x96, 0x01, // unicast
		0x01, 0x00,				// number of authentication method
		0x00, 0x40, 0x96, 0x00  // Authentication
		};
UCHAR	CipherSuiteCiscoCCKMLen = (sizeof(CipherSuiteCiscoCCKM) / sizeof(UCHAR));

UCHAR	CipherSuiteCiscoCCKM24[] = {
		0xDD, 0x18,				// RSN IE
		0x00, 0x50, 0xf2, 0x01, // oui
		0x01, 0x00,				// Version
		0x00, 0x40, 0x96, 0x01, // Multicast
		0x01, 0x00,				// Number of uicast
		0x00, 0x40, 0x96, 0x01, // unicast
		0x01, 0x00,				// number of authentication method
		0x00, 0x40, 0x96, 0x00,
		0x28, 0x00// Authentication
		};

UCHAR	CipherSuiteCiscoCCKM24Len = (sizeof(CipherSuiteCiscoCCKM24) / sizeof(UCHAR));

UCHAR	CipherSuiteCCXTkip[] = {
		0xDD, 0x16,				// RSN IE
		0x00, 0x50, 0xf2, 0x01,	// oui
		0x01, 0x00,				// Version
		0x00, 0x50, 0xf2, 0x02,	// Multicast
		0x01, 0x00,				// Number of unicast
		0x00, 0x50, 0xf2, 0x02,	// unicast
		0x01, 0x00,				// number of authentication method
		0x00, 0x50, 0xf2, 0x01	// authentication
		};
UCHAR	CipherSuiteCCXTkipLen = (sizeof(CipherSuiteCCXTkip) / sizeof(UCHAR));

UCHAR	CCX_LLC_HDR[] = {0xAA, 0xAA, 0x03, 0x00, 0x40, 0x96, 0x00, 0x02};
UCHAR	LLC_NORMAL[] = {0xAA, 0xAA, 0x03, 0x00, 0x00, 0x00};

UCHAR	EAPOL_FRAME[] = {0x88, 0x8E};

BOOLEAN CheckRSNIE(
	IN  PRTMP_ADAPTER   pAd,
	IN  PUCHAR          pData,
	IN  UCHAR           DataLen,
	OUT	UCHAR			*Offset);

void inc_byte_array(UCHAR *counter, int len);

/*
	========================================================================

	Routine Description:
		Classify WPA EAP message type

	Arguments:
		EAPType		Value of EAP message type
		MsgType		Internal Message definition for MLME state machine

	Return Value:
		TRUE		Found appropriate message type
		FALSE		No appropriate message type

	IRQL = DISPATCH_LEVEL

	Note:
		All these constants are defined in wpa.h
		For supplicant, there is only EAPOL Key message avaliable

	========================================================================
*/
BOOLEAN	WpaMsgTypeSubst(
	IN	UCHAR	EAPType,
	OUT	INT		*MsgType)
{
	switch (EAPType)
	{
		case EAPPacket:
			*MsgType = MT2_EAPPacket;
			break;
		case EAPOLStart:
			*MsgType = MT2_EAPOLStart;
			break;
		case EAPOLLogoff:
			*MsgType = MT2_EAPOLLogoff;
			break;
		case EAPOLKey:
			*MsgType = MT2_EAPOLKey;
			break;
		case EAPOLASFAlert:
			*MsgType = MT2_EAPOLASFAlert;
			break;
		default:
			return FALSE;
	}
	return TRUE;
}

/*
	==========================================================================
	Description:
		association	state machine init,	including state	transition and timer init
	Parameters:
		S -	pointer	to the association state machine
	==========================================================================
 */
VOID WpaPskStateMachineInit(
	IN	PRTMP_ADAPTER	pAd,
	IN	STATE_MACHINE *S,
	OUT	STATE_MACHINE_FUNC Trans[])
{
	StateMachineInit(S,	Trans, MAX_WPA_PSK_STATE, MAX_WPA_PSK_MSG, (STATE_MACHINE_FUNC)Drop, WPA_PSK_IDLE, WPA_MACHINE_BASE);
	StateMachineSetAction(S, WPA_PSK_IDLE, MT2_EAPOLKey, (STATE_MACHINE_FUNC)WpaEAPOLKeyAction);
}

/*
	==========================================================================
	Description:
		This is	state machine function.
		When receiving EAPOL packets which is  for 802.1x key management.
		Use	both in	WPA, and WPAPSK	case.
		In this	function, further dispatch to different	functions according	to the received	packet.	 3 categories are :
		  1.  normal 4-way pairwisekey and 2-way groupkey handshake
		  2.  MIC error	(Countermeasures attack)  report packet	from STA.
		  3.  Request for pairwise/group key update	from STA
	Return:
	==========================================================================
*/
VOID WpaEAPOLKeyAction(
	IN	PRTMP_ADAPTER	pAd,
	IN	MLME_QUEUE_ELEM	*Elem)

{
	INT             MsgType = EAPOL_MSG_INVALID;
	PKEY_DESCRIPTER pKeyDesc;
	PHEADER_802_11  pHeader; //red
	UCHAR           ZeroReplay[LEN_KEY_DESC_REPLAY];
	UCHAR EapolVr;
	KEY_INFO		peerKeyInfo;

	DBGPRINT(RT_DEBUG_TRACE, ("-----> WpaEAPOLKeyAction\n"));

	// Get 802.11 header first
	pHeader = (PHEADER_802_11) Elem->Msg;

	// Get EAPoL-Key Descriptor
	pKeyDesc = (PKEY_DESCRIPTER) &Elem->Msg[(LENGTH_802_11 + LENGTH_802_1_H + LENGTH_EAPOL_H)];

	NdisZeroMemory((PUCHAR)&peerKeyInfo, sizeof(peerKeyInfo));
	NdisMoveMemory((PUCHAR)&peerKeyInfo, (PUCHAR)&pKeyDesc->KeyInfo, sizeof(KEY_INFO));

	*((USHORT *)&peerKeyInfo) = cpu2le16(*((USHORT *)&peerKeyInfo));


	// 1. Check EAPOL frame version and type
	EapolVr	= (UCHAR) Elem->Msg[LENGTH_802_11+LENGTH_802_1_H];

    if (((EapolVr != EAPOL_VER) && (EapolVr != EAPOL_VER2)) || ((pKeyDesc->Type != WPA1_KEY_DESC) && (pKeyDesc->Type != WPA2_KEY_DESC)))
	{
        DBGPRINT(RT_DEBUG_ERROR, ("Key descripter does not match with WPA rule\n"));
			return;
	}

	// First validate replay counter, only accept message with larger replay counter
	// Let equal pass, some AP start with all zero replay counter
	NdisZeroMemory(ZeroReplay, LEN_KEY_DESC_REPLAY);

	if((RTMPCompareMemory(pKeyDesc->ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY) != 1) &&
		(RTMPCompareMemory(pKeyDesc->ReplayCounter, ZeroReplay, LEN_KEY_DESC_REPLAY) != 0))
	{
		DBGPRINT(RT_DEBUG_ERROR, ("   ReplayCounter not match   \n"));
		return;
	}

	// Process WPA2PSK frame
	if(pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)
	{
		if((peerKeyInfo.KeyType == PAIRWISEKEY) &&
			(peerKeyInfo.EKD_DL == 0) &&
			(peerKeyInfo.KeyAck == 1) &&
			(peerKeyInfo.KeyMic == 0) &&
			(peerKeyInfo.Secure == 0) &&
			(peerKeyInfo.Error == 0) &&
			(peerKeyInfo.Request == 0))
		{
			MsgType = EAPOL_PAIR_MSG_1;
			DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL Key Pairwise Message 1\n"));
		} else if((peerKeyInfo.KeyType == PAIRWISEKEY) &&
			(peerKeyInfo.EKD_DL  == 1) &&
			(peerKeyInfo.KeyAck == 1) &&
			(peerKeyInfo.KeyMic == 1) &&
			(peerKeyInfo.Secure == 1) &&
			(peerKeyInfo.Error == 0) &&
			(peerKeyInfo.Request == 0))
		{
			MsgType = EAPOL_PAIR_MSG_3;
			DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL Key Pairwise Message 3\n"));
		} else if((peerKeyInfo.KeyType == GROUPKEY) &&
			(peerKeyInfo.EKD_DL == 1) &&
			(peerKeyInfo.KeyAck == 1) &&
			(peerKeyInfo.KeyMic == 1) &&
			(peerKeyInfo.Secure == 1) &&
			(peerKeyInfo.Error == 0) &&
			(peerKeyInfo.Request == 0))
		{
			MsgType = EAPOL_GROUP_MSG_1;
			DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL Key Group Message 1\n"));
		}

		// We will assume link is up (assoc suceess and port not secured).
		// All state has to be able to process message from previous state
		switch(pAd->StaCfg.WpaState)
		{
		case SS_START:
			if(MsgType == EAPOL_PAIR_MSG_1)
			{
				Wpa2PairMsg1Action(pAd, Elem);
				pAd->StaCfg.WpaState = SS_WAIT_MSG_3;
			}
			break;

		case SS_WAIT_MSG_3:
			if(MsgType == EAPOL_PAIR_MSG_1)
			{
				Wpa2PairMsg1Action(pAd, Elem);
				pAd->StaCfg.WpaState = SS_WAIT_MSG_3;
			}
			else if(MsgType == EAPOL_PAIR_MSG_3)
			{
				Wpa2PairMsg3Action(pAd, Elem);
				pAd->StaCfg.WpaState = SS_WAIT_GROUP;
			}
			break;

		case SS_WAIT_GROUP:		// When doing group key exchange
		case SS_FINISH:			// This happened when update group key
			if(MsgType == EAPOL_PAIR_MSG_1)
			{
			    // Reset port secured variable
				pAd->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
				Wpa2PairMsg1Action(pAd, Elem);
				pAd->StaCfg.WpaState = SS_WAIT_MSG_3;
			}
			else if(MsgType == EAPOL_PAIR_MSG_3)
			{
			    // Reset port secured variable
				pAd->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
				Wpa2PairMsg3Action(pAd, Elem);
				pAd->StaCfg.WpaState = SS_WAIT_GROUP;
			}
			else if(MsgType == EAPOL_GROUP_MSG_1)
			{
				WpaGroupMsg1Action(pAd, Elem);
				pAd->StaCfg.WpaState = SS_FINISH;
			}
			break;

		default:
			break;
		}
	}
	// Process WPAPSK Frame
	// Classify message Type, either pairwise message 1, 3, or group message 1 for supplicant
	else if(pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK)
	{
		if((peerKeyInfo.KeyType == PAIRWISEKEY) &&
			(peerKeyInfo.KeyIndex == 0) &&
			(peerKeyInfo.KeyAck == 1) &&
			(peerKeyInfo.KeyMic == 0) &&
			(peerKeyInfo.Secure == 0) &&
			(peerKeyInfo.Error == 0) &&
			(peerKeyInfo.Request == 0))
		{
			MsgType = EAPOL_PAIR_MSG_1;
			DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL Key Pairwise Message 1\n"));
		}
		else if((peerKeyInfo.KeyType == PAIRWISEKEY) &&
			(peerKeyInfo.KeyIndex == 0) &&
			(peerKeyInfo.KeyAck == 1) &&
			(peerKeyInfo.KeyMic == 1) &&
			(peerKeyInfo.Secure == 0) &&
			(peerKeyInfo.Error == 0) &&
			(peerKeyInfo.Request == 0))
		{
			MsgType = EAPOL_PAIR_MSG_3;
			DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL Key Pairwise Message 3\n"));
		}
		else if((peerKeyInfo.KeyType == GROUPKEY) &&
			(peerKeyInfo.KeyIndex != 0) &&
			(peerKeyInfo.KeyAck == 1) &&
			(peerKeyInfo.KeyMic == 1) &&
			(peerKeyInfo.Secure == 1) &&
			(peerKeyInfo.Error == 0) &&
			(peerKeyInfo.Request == 0))
		{
			MsgType = EAPOL_GROUP_MSG_1;
			DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL Key Group Message 1\n"));
		}

		// We will assume link is up (assoc suceess and port not secured).
		// All state has to be able to process message from previous state
		switch(pAd->StaCfg.WpaState)
		{
		case SS_START:
			if(MsgType == EAPOL_PAIR_MSG_1)
			{
				WpaPairMsg1Action(pAd, Elem);
				pAd->StaCfg.WpaState = SS_WAIT_MSG_3;
			}
			break;

		case SS_WAIT_MSG_3:
			if(MsgType == EAPOL_PAIR_MSG_1)
			{
				WpaPairMsg1Action(pAd, Elem);
				pAd->StaCfg.WpaState = SS_WAIT_MSG_3;
			}
			else if(MsgType == EAPOL_PAIR_MSG_3)
			{
				WpaPairMsg3Action(pAd, Elem);
				pAd->StaCfg.WpaState = SS_WAIT_GROUP;
			}
			break;

		case SS_WAIT_GROUP:		// When doing group key exchange
		case SS_FINISH:			// This happened when update group key
			if(MsgType == EAPOL_PAIR_MSG_1)
			{
				WpaPairMsg1Action(pAd, Elem);
				pAd->StaCfg.WpaState = SS_WAIT_MSG_3;
				// Reset port secured variable
				pAd->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
			}
			else if(MsgType == EAPOL_PAIR_MSG_3)
			{
				WpaPairMsg3Action(pAd, Elem);
				pAd->StaCfg.WpaState = SS_WAIT_GROUP;
				// Reset port secured variable
				pAd->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
			}
			else if(MsgType == EAPOL_GROUP_MSG_1)
			{
				WpaGroupMsg1Action(pAd, Elem);
				pAd->StaCfg.WpaState = SS_FINISH;
			}
			break;

		default:
			break;
		}
	}

	DBGPRINT(RT_DEBUG_TRACE, ("<----- WpaEAPOLKeyAction\n"));
}

/*
	========================================================================

	Routine Description:
		Process Pairwise key 4-way handshaking

	Arguments:
		pAd	Pointer	to our adapter
		Elem		Message body

	Return Value:
		None

	Note:

	========================================================================
*/
VOID	WpaPairMsg1Action(
	IN  PRTMP_ADAPTER   pAd,
	IN  MLME_QUEUE_ELEM *Elem)
{
	PHEADER_802_11      pHeader;
	UCHAR				*mpool, *PTK, *digest;
	PUCHAR              pOutBuffer = NULL;
	UCHAR               Header802_3[14];
	ULONG               FrameLen = 0;
	PEAPOL_PACKET       pMsg1;
	EAPOL_PACKET        Packet;
	UCHAR               Mic[16];

	DBGPRINT(RT_DEBUG_TRACE, ("WpaPairMsg1Action ----->\n"));

	// allocate memory pool
	os_alloc_mem(pAd, (PUCHAR *)&mpool, 256);

	if (mpool == NULL)
		return;

	// PTK Len = 80.
	PTK = (UCHAR *) ROUND_UP(mpool, 4);
	// digest Len = 80.
	digest = (UCHAR *) ROUND_UP(PTK + 80, 4);

	pHeader = (PHEADER_802_11) Elem->Msg;

	// Process message 1 from authenticator
	pMsg1 = (PEAPOL_PACKET) &Elem->Msg[LENGTH_802_11 + LENGTH_802_1_H];

	// 1. Save Replay counter, it will use to verify message 3 and construct message 2
	NdisMoveMemory(pAd->StaCfg.ReplayCounter, pMsg1->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);

	// 2. Save ANonce
	NdisMoveMemory(pAd->StaCfg.ANonce, pMsg1->KeyDesc.KeyNonce, LEN_KEY_DESC_NONCE);

	// Generate random SNonce
	GenRandom(pAd, pAd->CurrentAddress, pAd->StaCfg.SNonce);

	// Calc PTK(ANonce, SNonce)
	WpaCountPTK(pAd,
		pAd->StaCfg.PMK,
		pAd->StaCfg.ANonce,
		pAd->CommonCfg.Bssid,
		pAd->StaCfg.SNonce,
		pAd->CurrentAddress,
		PTK,
		LEN_PTK);

	// Save key to PTK entry
	NdisMoveMemory(pAd->StaCfg.PTK, PTK, LEN_PTK);

	// init 802.3 header and Fill Packet
	MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);

	// Zero Message 2 body
	NdisZeroMemory(&Packet, sizeof(Packet));
	Packet.ProVer	= EAPOL_VER;
	Packet.ProType	= EAPOLKey;
	//
	// Message 2 as  EAPOL-Key(0,1,0,0,0,P,0,SNonce,MIC,RSN IE)
	//
	Packet.KeyDesc.Type = WPA1_KEY_DESC;
	// 1. Key descriptor version and appropriate RSN IE
	if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
	{
		Packet.KeyDesc.KeyInfo.KeyDescVer = 2;
	}
	else	  // TKIP
	{
		Packet.KeyDesc.KeyInfo.KeyDescVer = 1;
	}

	// fill in Data Material and its length
	Packet.KeyDesc.KeyData[0] = IE_WPA;
	Packet.KeyDesc.KeyData[1] = pAd->StaCfg.RSNIE_Len;
	Packet.KeyDesc.KeyDataLen[1] = pAd->StaCfg.RSNIE_Len + 2;
	NdisMoveMemory(&Packet.KeyDesc.KeyData[2], pAd->StaCfg.RSN_IE, pAd->StaCfg.RSNIE_Len);

	// Update packet length after decide Key data payload
	Packet.Body_Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE + Packet.KeyDesc.KeyDataLen[1];

	// Update Key length
	Packet.KeyDesc.KeyLength[0] = pMsg1->KeyDesc.KeyLength[0];
	Packet.KeyDesc.KeyLength[1] = pMsg1->KeyDesc.KeyLength[1];
	// 2. Key Type PeerKey
	Packet.KeyDesc.KeyInfo.KeyType = PAIRWISEKEY;

	// 3. KeyMic field presented
	Packet.KeyDesc.KeyInfo.KeyMic  = 1;

	//Convert to little-endian format.
	*((USHORT *)&Packet.KeyDesc.KeyInfo) = cpu2le16(*((USHORT *)&Packet.KeyDesc.KeyInfo));


	// 4. Fill SNonce
	NdisMoveMemory(Packet.KeyDesc.KeyNonce, pAd->StaCfg.SNonce, LEN_KEY_DESC_NONCE);

	// 5. Key Replay Count
	NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY);

	// Send EAPOL(0, 1, 0, 0, 0, P, 0, SNonce, MIC, RSN_IE)
	// Out buffer for transmitting message 2
	MlmeAllocateMemory(pAd, (PUCHAR *)&pOutBuffer);  // allocate memory
	if(pOutBuffer == NULL)
	{
		os_free_mem(pAd, mpool);
		return;
	}
	// Prepare EAPOL frame for MIC calculation
	// Be careful, only EAPOL frame is counted for MIC calculation
	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
		Packet.Body_Len[1] + 4,    &Packet,
		END_OF_ARGS);

	// 6. Prepare and Fill MIC value
	NdisZeroMemory(Mic, sizeof(Mic));
	if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
	{	// AES

		HMAC_SHA1(pOutBuffer, FrameLen, PTK, LEN_EAP_MICK, digest);
		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
	}
	else
	{	// TKIP
		hmac_md5(PTK,  LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
	}
	NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);

	//hex_dump("MIC", Mic, LEN_KEY_DESC_MIC);

		MakeOutgoingFrame(pOutBuffer,           	&FrameLen,
			  			LENGTH_802_3,     			&Header802_3,
						Packet.Body_Len[1] + 4,    &Packet,
						END_OF_ARGS);


	// 5. Copy frame to Tx ring and send Msg 2 to authenticator
	RTMPToWirelessSta(pAd, Header802_3, LENGTH_802_3, (PUCHAR)&Packet, Packet.Body_Len[1] + 4, TRUE);

	MlmeFreeMemory(pAd, (PUCHAR)pOutBuffer);
	os_free_mem(pAd, (PUCHAR)mpool);

	DBGPRINT(RT_DEBUG_TRACE, ("WpaPairMsg1Action <-----\n"));
}

VOID Wpa2PairMsg1Action(
	IN  PRTMP_ADAPTER   pAd,
	IN  MLME_QUEUE_ELEM *Elem)
{
	PHEADER_802_11      pHeader;
	UCHAR				*mpool, *PTK, *digest;
	PUCHAR              pOutBuffer = NULL;
	UCHAR               Header802_3[14];
	ULONG               FrameLen = 0;
	PEAPOL_PACKET       pMsg1;
	EAPOL_PACKET        Packet;
	UCHAR               Mic[16];

	DBGPRINT(RT_DEBUG_TRACE, ("Wpa2PairMsg1Action ----->\n"));

	// allocate memory pool
	os_alloc_mem(pAd, (PUCHAR *)&mpool, 256);

	if (mpool == NULL)
		return;

	// PTK Len = 80.
	PTK = (UCHAR *) ROUND_UP(mpool, 4);
	// digest Len = 80.
	digest = (UCHAR *) ROUND_UP(PTK + 80, 4);

	pHeader = (PHEADER_802_11) Elem->Msg;

	// Process message 1 from authenticator
		pMsg1 = (PEAPOL_PACKET)	&Elem->Msg[LENGTH_802_11 + LENGTH_802_1_H];

	// 1. Save Replay counter, it will use to verify message 3 and construct message 2
	NdisMoveMemory(pAd->StaCfg.ReplayCounter, pMsg1->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);

	// 2. Save ANonce
	NdisMoveMemory(pAd->StaCfg.ANonce, pMsg1->KeyDesc.KeyNonce, LEN_KEY_DESC_NONCE);

	// Generate random SNonce
	GenRandom(pAd, pAd->CurrentAddress, pAd->StaCfg.SNonce);

	if(pMsg1->KeyDesc.KeyDataLen[1] > 0 )
	{
		// cached PMKID
	}

	// Calc PTK(ANonce, SNonce)
	WpaCountPTK(pAd,
		pAd->StaCfg.PMK,
		pAd->StaCfg.ANonce,
		pAd->CommonCfg.Bssid,
		pAd->StaCfg.SNonce,
		pAd->CurrentAddress,
		PTK,
		LEN_PTK);

	// Save key to PTK entry
	NdisMoveMemory(pAd->StaCfg.PTK, PTK, LEN_PTK);

	// init 802.3 header and Fill Packet
	MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);

	// Zero message 2 body
	NdisZeroMemory(&Packet, sizeof(Packet));
	Packet.ProVer	= EAPOL_VER;
	Packet.ProType	= EAPOLKey;
	//
	// Message 2 as  EAPOL-Key(0,1,0,0,0,P,0,SNonce,MIC,RSN IE)
	//
	Packet.KeyDesc.Type = WPA2_KEY_DESC;

	// 1. Key descriptor version and appropriate RSN IE
	if(pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
	{
		Packet.KeyDesc.KeyInfo.KeyDescVer = 2;
	}
	else	  // TKIP
	{
		Packet.KeyDesc.KeyInfo.KeyDescVer = 1;
	}

	// fill in Data Material and its length
	Packet.KeyDesc.KeyData[0] = IE_WPA2;
	Packet.KeyDesc.KeyData[1] = pAd->StaCfg.RSNIE_Len;
	Packet.KeyDesc.KeyDataLen[1] = pAd->StaCfg.RSNIE_Len + 2;
	NdisMoveMemory(&Packet.KeyDesc.KeyData[2], pAd->StaCfg.RSN_IE, pAd->StaCfg.RSNIE_Len);

	// Update packet length after decide Key data payload
	Packet.Body_Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE + Packet.KeyDesc.KeyDataLen[1];

	// 2. Key Type PeerKey
	Packet.KeyDesc.KeyInfo.KeyType = PAIRWISEKEY;

	// 3. KeyMic field presented
	Packet.KeyDesc.KeyInfo.KeyMic  = 1;

	// Update Key Length
	Packet.KeyDesc.KeyLength[0] = 0;
	Packet.KeyDesc.KeyLength[1] = pMsg1->KeyDesc.KeyLength[1];

	// 4. Fill SNonce
	NdisMoveMemory(Packet.KeyDesc.KeyNonce, pAd->StaCfg.SNonce, LEN_KEY_DESC_NONCE);

	// 5. Key Replay Count
	NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY);

	// Convert to little-endian format.
	*((USHORT *)&Packet.KeyDesc.KeyInfo) = cpu2le16(*((USHORT *)&Packet.KeyDesc.KeyInfo));

	// Send EAPOL-Key(0,1,0,0,0,P,0,SNonce,MIC,RSN IE)
	// Out buffer for transmitting message 2
	MlmeAllocateMemory(pAd,  (PUCHAR *)&pOutBuffer);  // allocate memory
	if(pOutBuffer == NULL)
	{
		os_free_mem(pAd, mpool);
		return;
	}

	// Prepare EAPOL frame for MIC calculation
	// Be careful, only EAPOL frame is counted for MIC calculation
	MakeOutgoingFrame(pOutBuffer,        &FrameLen,
		Packet.Body_Len[1] + 4, &Packet,
		END_OF_ARGS);

	// 6. Prepare and Fill MIC value
	NdisZeroMemory(Mic, sizeof(Mic));
	if(pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
	{
		// AES
		HMAC_SHA1(pOutBuffer, FrameLen, PTK, LEN_EAP_MICK, digest);
		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
	}
	else
	{
		hmac_md5(PTK,  LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
	}
	NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);


	// Make  Transmitting frame
	MakeOutgoingFrame(pOutBuffer,           	&FrameLen,
			  			LENGTH_802_3,     		&Header802_3,
						Packet.Body_Len[1] + 4, &Packet,
						END_OF_ARGS);


	// 5. Copy frame to Tx ring
	RTMPToWirelessSta(pAd, Header802_3, LENGTH_802_3, (PUCHAR)&Packet, Packet.Body_Len[1] + 4, TRUE);

	MlmeFreeMemory(pAd, pOutBuffer);
	os_free_mem(pAd, mpool);

	DBGPRINT(RT_DEBUG_TRACE, ("Wpa2PairMsg1Action <-----\n"));

}

/*
	========================================================================

	Routine Description:
		Process Pairwise key 4-way handshaking

	Arguments:
		pAd	Pointer	to our adapter
		Elem		Message body

	Return Value:
		None

	Note:

	========================================================================
*/
VOID	WpaPairMsg3Action(
	IN	PRTMP_ADAPTER	pAd,
	IN	MLME_QUEUE_ELEM	*Elem)

{
	PHEADER_802_11      pHeader;
	PUCHAR          	pOutBuffer = NULL;
	UCHAR               Header802_3[14];
	ULONG           	FrameLen = 0;
	EAPOL_PACKET        Packet;
	PEAPOL_PACKET       pMsg3;
	UCHAR           	Mic[16], OldMic[16];
	MAC_TABLE_ENTRY 	*pEntry = NULL;
	UCHAR				skip_offset;
	KEY_INFO			peerKeyInfo;

	DBGPRINT(RT_DEBUG_TRACE, ("WpaPairMsg3Action ----->\n"));

	// Record 802.11 header & the received EAPOL packet Msg3
	pHeader = (PHEADER_802_11) Elem->Msg;
	pMsg3 = (PEAPOL_PACKET)	&Elem->Msg[LENGTH_802_11 + LENGTH_802_1_H];

	NdisZeroMemory((PUCHAR)&peerKeyInfo, sizeof(peerKeyInfo));
	NdisMoveMemory((PUCHAR)&peerKeyInfo, (PUCHAR)&pMsg3->KeyDesc.KeyInfo, sizeof(KEY_INFO));

	*((USHORT*)&peerKeyInfo) = cpu2le16(*((USHORT*)&peerKeyInfo));


	// 1. Verify cipher type match
	if (pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled && (peerKeyInfo.KeyDescVer != 2))
	{
		return;
	}
	else if(pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled && (peerKeyInfo.KeyDescVer != 1))
	{
		return;
	}

	// Verify RSN IE
	//if (!RTMPEqualMemory(pMsg3->KeyDesc.KeyData, pAd->MacTab.Content[BSSID_WCID].RSN_IE, pAd->MacTab.Content[BSSID_WCID].RSNIE_Len))
	if (!CheckRSNIE(pAd, pMsg3->KeyDesc.KeyData, pMsg3->KeyDesc.KeyDataLen[1], &skip_offset))
	{
		DBGPRINT(RT_DEBUG_ERROR, ("RSN_IE Different in Msg 3 of WPA1 4-way handshake!! \n"));
		hex_dump("The original RSN_IE", pAd->MacTab.Content[BSSID_WCID].RSN_IE, pAd->MacTab.Content[BSSID_WCID].RSNIE_Len);
		hex_dump("The received RSN_IE", pMsg3->KeyDesc.KeyData, pMsg3->KeyDesc.KeyDataLen[1]);
		return;
	}
	else
		DBGPRINT(RT_DEBUG_TRACE, ("RSN_IE VALID in Msg 3 of WPA1 4-way handshake!! \n"));


	// 2. Check MIC value
	// Save the MIC and replace with zero
	NdisMoveMemory(OldMic, pMsg3->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
	NdisZeroMemory(pMsg3->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
	if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
	{
		// AES
		UCHAR digest[80];

		HMAC_SHA1((PUCHAR) pMsg3, pMsg3->Body_Len[1] + 4, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
	}
	else	// TKIP
	{
		hmac_md5(pAd->StaCfg.PTK, LEN_EAP_MICK, (PUCHAR) pMsg3, pMsg3->Body_Len[1] + 4, Mic);
	}

	if(!NdisEqualMemory(OldMic, Mic, LEN_KEY_DESC_MIC))
	{
		DBGPRINT(RT_DEBUG_ERROR, (" MIC Different in msg 3 of 4-way handshake!!!!!!!!!! \n"));
		return;
	}
	else
		DBGPRINT(RT_DEBUG_TRACE, (" MIC VALID in msg 3 of 4-way handshake!!!!!!!!!! \n"));

	// 3. Check Replay Counter, it has to be larger than last one. No need to be exact one larger
	if(RTMPCompareMemory(pMsg3->KeyDesc.ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY) != 1)
		return;

	// Update new replay counter
	NdisMoveMemory(pAd->StaCfg.ReplayCounter, pMsg3->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);

	// 4. Double check ANonce
	if(!NdisEqualMemory(pAd->StaCfg.ANonce, pMsg3->KeyDesc.KeyNonce, LEN_KEY_DESC_NONCE))
		return;

	// init 802.3 header and Fill Packet
	MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);

	// Zero Message 4 body
	NdisZeroMemory(&Packet, sizeof(Packet));
	Packet.ProVer	= EAPOL_VER;
	Packet.ProType	= EAPOLKey;
	Packet.Body_Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE;		// No data field

	//
	// Message 4 as  EAPOL-Key(0,1,0,0,0,P,0,0,MIC,0)
	//
	Packet.KeyDesc.Type = WPA1_KEY_DESC;

	// Key descriptor version and appropriate RSN IE
	Packet.KeyDesc.KeyInfo.KeyDescVer = peerKeyInfo.KeyDescVer;

	// Update Key Length
	Packet.KeyDesc.KeyLength[0] = pMsg3->KeyDesc.KeyLength[0];
	Packet.KeyDesc.KeyLength[1] = pMsg3->KeyDesc.KeyLength[1];

	// Key Type PeerKey
	Packet.KeyDesc.KeyInfo.KeyType = PAIRWISEKEY;

	// KeyMic field presented
	Packet.KeyDesc.KeyInfo.KeyMic  = 1;

	// In Msg3,  KeyInfo.secure =0 if Group Key HS to come. 1 if no group key HS
	// Station sends Msg4  KeyInfo.secure should be the same as that in Msg.3
	Packet.KeyDesc.KeyInfo.Secure= peerKeyInfo.Secure;

	// Convert to little-endian format.
	*((USHORT *)&Packet.KeyDesc.KeyInfo) = cpu2le16(*((USHORT *)&Packet.KeyDesc.KeyInfo));

	// Key Replay count
	NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pMsg3->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);

	// Out buffer for transmitting message 4
	MlmeAllocateMemory(pAd, (PUCHAR *)&pOutBuffer);  // allocate memory
	if(pOutBuffer == NULL)
		return;

	// Prepare EAPOL frame for MIC calculation
	// Be careful, only EAPOL frame is counted for MIC calculation
	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
		Packet.Body_Len[1] + 4,    &Packet,
		END_OF_ARGS);

	// Prepare and Fill MIC value
	NdisZeroMemory(Mic, sizeof(Mic));
	if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
	{
		// AES
		UCHAR digest[80];

		HMAC_SHA1(pOutBuffer, FrameLen, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
	}
	else
	{
		hmac_md5(pAd->StaCfg.PTK, LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
	}
	NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);

	// Update PTK
	// Prepare pair-wise key information into shared key table
	NdisZeroMemory(&pAd->SharedKey[BSS0][0], sizeof(CIPHER_KEY));
	pAd->SharedKey[BSS0][0].KeyLen = LEN_TKIP_EK;
    NdisMoveMemory(pAd->SharedKey[BSS0][0].Key, &pAd->StaCfg.PTK[32], LEN_TKIP_EK);
	NdisMoveMemory(pAd->SharedKey[BSS0][0].RxMic, &pAd->StaCfg.PTK[48], LEN_TKIP_RXMICK);
	NdisMoveMemory(pAd->SharedKey[BSS0][0].TxMic, &pAd->StaCfg.PTK[48+LEN_TKIP_RXMICK], LEN_TKIP_TXMICK);

	// Decide its ChiperAlg
	if (pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled)
		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_TKIP;
	else if (pAd->StaCfg.PairCipher == Ndis802_11Encryption3Enabled)
		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_AES;
	else
		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_NONE;

	// Update these related information to MAC_TABLE_ENTRY
	pEntry = &pAd->MacTab.Content[BSSID_WCID];
	NdisMoveMemory(pEntry->PairwiseKey.Key, &pAd->StaCfg.PTK[32], LEN_TKIP_EK);
	NdisMoveMemory(pEntry->PairwiseKey.RxMic, &pAd->StaCfg.PTK[48], LEN_TKIP_RXMICK);
	NdisMoveMemory(pEntry->PairwiseKey.TxMic, &pAd->StaCfg.PTK[48+LEN_TKIP_RXMICK], LEN_TKIP_TXMICK);
	pEntry->PairwiseKey.CipherAlg = pAd->SharedKey[BSS0][0].CipherAlg;

	// Update pairwise key information to ASIC Shared Key Table
	AsicAddSharedKeyEntry(pAd,
						  BSS0,
						  0,
						  pAd->SharedKey[BSS0][0].CipherAlg,
						  pAd->SharedKey[BSS0][0].Key,
						  pAd->SharedKey[BSS0][0].TxMic,
						  pAd->SharedKey[BSS0][0].RxMic);

	// Update ASIC WCID attribute table and IVEIV table
	RTMPAddWcidAttributeEntry(pAd,
							  BSS0,
							  0,
							  pAd->SharedKey[BSS0][0].CipherAlg,
							  pEntry);

	// Make transmitting frame
	MakeOutgoingFrame(pOutBuffer,           	&FrameLen,
			  			LENGTH_802_3,     		&Header802_3,
						Packet.Body_Len[1] + 4, &Packet,
						END_OF_ARGS);


	// Copy frame to Tx ring and Send Message 4 to authenticator
	RTMPToWirelessSta(pAd, Header802_3, LENGTH_802_3, (PUCHAR)&Packet, Packet.Body_Len[1] + 4, TRUE);

	MlmeFreeMemory(pAd, (PUCHAR)pOutBuffer);

	DBGPRINT(RT_DEBUG_TRACE, ("WpaPairMsg3Action <-----\n"));
}

VOID    Wpa2PairMsg3Action(
	IN  PRTMP_ADAPTER   pAd,
	IN  MLME_QUEUE_ELEM *Elem)

{
	PHEADER_802_11      pHeader;
	PUCHAR              pOutBuffer = NULL;
	UCHAR               Header802_3[14];
	ULONG               FrameLen = 0;
	EAPOL_PACKET        Packet;
	PEAPOL_PACKET       pMsg3;
	UCHAR               Mic[16], OldMic[16];
	UCHAR               *mpool, *KEYDATA, *digest;
	UCHAR               Key[32];
	MAC_TABLE_ENTRY 	*pEntry = NULL;
	KEY_INFO			peerKeyInfo;

	// allocate memory
	os_alloc_mem(pAd, (PUCHAR *)&mpool, 1024);

	if(mpool == NULL)
		return;

	// KEYDATA Len = 512.
	KEYDATA = (UCHAR *) ROUND_UP(mpool, 4);
	// digest Len = 80.
	digest = (UCHAR *) ROUND_UP(KEYDATA + 512, 4);

	DBGPRINT(RT_DEBUG_TRACE, ("Wpa2PairMsg3Action ----->\n"));

	pHeader = (PHEADER_802_11) Elem->Msg;

	// Process message 3 frame.
	pMsg3 = (PEAPOL_PACKET)	&Elem->Msg[LENGTH_802_11 + LENGTH_802_1_H];

	NdisZeroMemory((PUCHAR)&peerKeyInfo, sizeof(peerKeyInfo));
	NdisMoveMemory((PUCHAR)&peerKeyInfo, (PUCHAR)&pMsg3->KeyDesc.KeyInfo, sizeof(KEY_INFO));

	*((USHORT*)&peerKeyInfo) = cpu2le16(*((USHORT*)&peerKeyInfo));

	// 1. Verify cipher type match
	if (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled && (peerKeyInfo.KeyDescVer!= 2))
	{
		os_free_mem(pAd, (PUCHAR)mpool);
		return;
	}
	else if(pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled && (peerKeyInfo.KeyDescVer != 1))
	{
		os_free_mem(pAd, (PUCHAR)mpool);
		return;
	}

	// 2. Check MIC value
	// Save the MIC and replace with zero
	NdisMoveMemory(OldMic, pMsg3->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
	NdisZeroMemory(pMsg3->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
	if (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
	{
		// AES
		HMAC_SHA1((PUCHAR) pMsg3, pMsg3->Body_Len[1] + 4, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
	}
	else
	{
		hmac_md5(pAd->StaCfg.PTK, LEN_EAP_MICK, (PUCHAR) pMsg3, pMsg3->Body_Len[1] + 4, Mic);
	}

	if(!NdisEqualMemory(OldMic, Mic, LEN_KEY_DESC_MIC))
	{
		DBGPRINT(RT_DEBUG_ERROR, (" MIC Different in msg 3 of 4-way handshake!!!!!!!!!! \n"));
		os_free_mem(pAd, (PUCHAR)mpool);
		return;
	}
	else
		DBGPRINT(RT_DEBUG_TRACE, (" MIC VALID in msg 3 of 4-way handshake!!!!!!!!!! \n"));

	// 3. Check Replay Counter, it has to be larger than last one. No need to be exact one larger
	if(RTMPCompareMemory(pMsg3->KeyDesc.ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY) != 1)
	{
		os_free_mem(pAd, (PUCHAR)mpool);
		return;
	}

	// Update new replay counter
	NdisMoveMemory(pAd->StaCfg.ReplayCounter, pMsg3->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);

	// 4. Double check ANonce
	if(!NdisEqualMemory(pAd->StaCfg.ANonce, pMsg3->KeyDesc.KeyNonce, LEN_KEY_DESC_NONCE))
	{
		os_free_mem(pAd, (PUCHAR)mpool);
		return;
	}

	// Obtain GTK
	// 5. Decrypt GTK from Key Data
	DBGPRINT_RAW(RT_DEBUG_TRACE, ("EKD = %d\n", peerKeyInfo.EKD_DL));
	if(pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
	{
		// Decrypt AES GTK
		AES_GTK_KEY_UNWRAP(&pAd->StaCfg.PTK[16], KEYDATA, pMsg3->KeyDesc.KeyDataLen[1],pMsg3->KeyDesc.KeyData);
	}
	else	  // TKIP
	{
		INT i;
		// Decrypt TKIP GTK
		// Construct 32 bytes RC4 Key
		NdisMoveMemory(Key, pMsg3->KeyDesc.KeyIv, 16);
		NdisMoveMemory(&Key[16], &pAd->StaCfg.PTK[16], 16);
		ARCFOUR_INIT(&pAd->PrivateInfo.WEPCONTEXT, Key, 32);
		//discard first 256 bytes
		for(i = 0; i < 256; i++)
			ARCFOUR_BYTE(&pAd->PrivateInfo.WEPCONTEXT);
		// Decrypt GTK. Becareful, there is no ICV to check the result is correct or not
		ARCFOUR_DECRYPT(&pAd->PrivateInfo.WEPCONTEXT, KEYDATA, pMsg3->KeyDesc.KeyData, pMsg3->KeyDesc.KeyDataLen[1]);
	}

	if (!ParseKeyData(pAd, KEYDATA, pMsg3->KeyDesc.KeyDataLen[1], 1))
	{
		os_free_mem(pAd, (PUCHAR)mpool);
		return;
	}

	// Update GTK to ASIC
	// Update group key information to ASIC Shared Key Table
	AsicAddSharedKeyEntry(pAd,
						  BSS0,
						  pAd->StaCfg.DefaultKeyId,
						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key,
						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic,
						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic);

	// Update ASIC WCID attribute table and IVEIV table
	RTMPAddWcidAttributeEntry(pAd,
							  BSS0,
							  pAd->StaCfg.DefaultKeyId,
							  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
							  NULL);

	// init 802.3 header and Fill Packet
	MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);

	// Zero message 4 body
	NdisZeroMemory(&Packet, sizeof(Packet));
	Packet.ProVer	= EAPOL_VER;
	Packet.ProType	= EAPOLKey;
	Packet.Body_Len[1]  	= sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE;		// No data field

	//
	// Message 4 as  EAPOL-Key(0,1,0,0,0,P,0,0,MIC,0)
	//
	Packet.KeyDesc.Type = WPA2_KEY_DESC;

	// Key descriptor version and appropriate RSN IE
	Packet.KeyDesc.KeyInfo.KeyDescVer = peerKeyInfo.KeyDescVer;

	// Update Key Length
	Packet.KeyDesc.KeyLength[0] = pMsg3->KeyDesc.KeyLength[0];
	Packet.KeyDesc.KeyLength[1] = pMsg3->KeyDesc.KeyLength[1];

	// Key Type PeerKey
	Packet.KeyDesc.KeyInfo.KeyType = PAIRWISEKEY;

	// KeyMic field presented
	Packet.KeyDesc.KeyInfo.KeyMic  = 1;
	Packet.KeyDesc.KeyInfo.Secure = 1;

	// Convert to little-endian format.
	*((USHORT *)&Packet.KeyDesc.KeyInfo) = cpu2le16(*((USHORT *)&Packet.KeyDesc.KeyInfo));

	// Key Replay count
	NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pMsg3->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);

	// Out buffer for transmitting message 4
	MlmeAllocateMemory(pAd, (PUCHAR *)&pOutBuffer);  // allocate memory
	if(pOutBuffer == NULL)
	{
		os_free_mem(pAd, (PUCHAR)mpool);
		return;
	}

	// Prepare EAPOL frame for MIC calculation
	// Be careful, only EAPOL frame is counted for MIC calculation
	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
		Packet.Body_Len[1] + 4,    &Packet,
		END_OF_ARGS);

	// Prepare and Fill MIC value
	NdisZeroMemory(Mic, sizeof(Mic));
	if(pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
	{
		// AES
		HMAC_SHA1(pOutBuffer, FrameLen, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
	}
	else
	{
		hmac_md5(pAd->StaCfg.PTK, LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
	}
	NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);

	// Update PTK
	// Prepare pair-wise key information into shared key table
	NdisZeroMemory(&pAd->SharedKey[BSS0][0], sizeof(CIPHER_KEY));
	pAd->SharedKey[BSS0][0].KeyLen = LEN_TKIP_EK;
    NdisMoveMemory(pAd->SharedKey[BSS0][0].Key, &pAd->StaCfg.PTK[32], LEN_TKIP_EK);
	NdisMoveMemory(pAd->SharedKey[BSS0][0].RxMic, &pAd->StaCfg.PTK[48], LEN_TKIP_RXMICK);
	NdisMoveMemory(pAd->SharedKey[BSS0][0].TxMic, &pAd->StaCfg.PTK[48+LEN_TKIP_RXMICK], LEN_TKIP_TXMICK);

	// Decide its ChiperAlg
	if (pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled)
		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_TKIP;
	else if (pAd->StaCfg.PairCipher == Ndis802_11Encryption3Enabled)
		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_AES;
	else
		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_NONE;

	// Update these related information to MAC_TABLE_ENTRY
	pEntry = &pAd->MacTab.Content[BSSID_WCID];
	NdisMoveMemory(&pEntry->PairwiseKey.Key, &pAd->StaCfg.PTK[32], LEN_TKIP_EK);
	NdisMoveMemory(&pEntry->PairwiseKey.RxMic, &pAd->StaCfg.PTK[48], LEN_TKIP_RXMICK);
	NdisMoveMemory(&pEntry->PairwiseKey.TxMic, &pAd->StaCfg.PTK[48+LEN_TKIP_RXMICK], LEN_TKIP_TXMICK);
	pEntry->PairwiseKey.CipherAlg = pAd->SharedKey[BSS0][0].CipherAlg;

	// Update pairwise key information to ASIC Shared Key Table
	AsicAddSharedKeyEntry(pAd,
						  BSS0,
						  0,
						  pAd->SharedKey[BSS0][0].CipherAlg,
						  pAd->SharedKey[BSS0][0].Key,
						  pAd->SharedKey[BSS0][0].TxMic,
						  pAd->SharedKey[BSS0][0].RxMic);

	// Update ASIC WCID attribute table and IVEIV table
	RTMPAddWcidAttributeEntry(pAd,
							  BSS0,
							  0,
							  pAd->SharedKey[BSS0][0].CipherAlg,
							  pEntry);

	// Make  Transmitting frame
	MakeOutgoingFrame(pOutBuffer,           	&FrameLen,
			  			LENGTH_802_3,     		&Header802_3,
						Packet.Body_Len[1] + 4, &Packet,
						END_OF_ARGS);


	// Copy frame to Tx ring and Send Message 4 to authenticator
	RTMPToWirelessSta(pAd, Header802_3, LENGTH_802_3, (PUCHAR)&Packet, Packet.Body_Len[1] + 4, TRUE);

	// set 802.1x port control
	STA_PORT_SECURED(pAd);

    // Indicate Connected for GUI
    pAd->IndicateMediaState = NdisMediaStateConnected;

	MlmeFreeMemory(pAd, (PUCHAR)pOutBuffer);
	os_free_mem(pAd, (PUCHAR)mpool);


	// send wireless event - for set key done WPA2
	if (pAd->CommonCfg.bWirelessEvent)
		RTMPSendWirelessEvent(pAd, IW_SET_KEY_DONE_WPA2_EVENT_FLAG, pEntry->Addr, BSS0, 0);

	DBGPRINT(RT_DEBUG_ERROR, ("Wpa2PairMsg3Action <-----\n"));

}

/*
	========================================================================

	Routine Description:
		Process Group key 2-way handshaking

	Arguments:
		pAd	Pointer	to our adapter
		Elem		Message body

	Return Value:
		None

	Note:

	========================================================================
*/
VOID	WpaGroupMsg1Action(
	IN	PRTMP_ADAPTER	pAd,
	IN	MLME_QUEUE_ELEM	*Elem)

{
	PUCHAR              pOutBuffer = NULL;
	UCHAR               Header802_3[14];
	ULONG               FrameLen = 0;
	EAPOL_PACKET        Packet;
	PEAPOL_PACKET       pGroup;
	UCHAR               *mpool, *digest, *KEYDATA;
	UCHAR               Mic[16], OldMic[16];
	UCHAR               GTK[32], Key[32];
	KEY_INFO			peerKeyInfo;

	// allocate memory
	os_alloc_mem(pAd, (PUCHAR *)&mpool, 1024);

	if(mpool == NULL)
		return;

	// digest Len = 80.
	digest = (UCHAR *) ROUND_UP(mpool, 4);
	// KEYDATA Len = 512.
	KEYDATA = (UCHAR *) ROUND_UP(digest + 80, 4);

	DBGPRINT(RT_DEBUG_TRACE, ("WpaGroupMsg1Action ----->\n"));

	// Process Group Message 1 frame. skip 802.11 header(24) & LLC_SNAP header(8)
	pGroup = (PEAPOL_PACKET) &Elem->Msg[LENGTH_802_11 + LENGTH_802_1_H];

	NdisZeroMemory((PUCHAR)&peerKeyInfo, sizeof(peerKeyInfo));
	NdisMoveMemory((PUCHAR)&peerKeyInfo, (PUCHAR)&pGroup->KeyDesc.KeyInfo, sizeof(KEY_INFO));

	*((USHORT*)&peerKeyInfo) = cpu2le16(*((USHORT*)&peerKeyInfo));

	// 0. Check cipher type match
	if (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled && (peerKeyInfo.KeyDescVer != 2))
	{
		os_free_mem(pAd, (PUCHAR)mpool);
		return;
	}
	else if (pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled && (peerKeyInfo.KeyDescVer != 1))
	{
		os_free_mem(pAd, (PUCHAR)mpool);
		return;
	}

	// 1. Verify Replay counter
	//    Check Replay Counter, it has to be larger than last one. No need to be exact one larger
	if(RTMPCompareMemory(pGroup->KeyDesc.ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY) != 1)
	{
		os_free_mem(pAd, (PUCHAR)mpool);
		return;
	}

	// Update new replay counter
	NdisMoveMemory(pAd->StaCfg.ReplayCounter, pGroup->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);

	// 2. Verify MIC is valid
	// Save the MIC and replace with zero
	NdisMoveMemory(OldMic, pGroup->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
	NdisZeroMemory(pGroup->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);

	if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
	{	// AES
		HMAC_SHA1((PUCHAR) pGroup, pGroup->Body_Len[1] + 4, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
	}
	else
	{	// TKIP
		hmac_md5(pAd->StaCfg.PTK, LEN_EAP_MICK, (PUCHAR) pGroup, pGroup->Body_Len[1] + 4, Mic);
	}

	if(!NdisEqualMemory(OldMic, Mic, LEN_KEY_DESC_MIC))
	{
		DBGPRINT(RT_DEBUG_ERROR, (" MIC Different in group msg 1 of 2-way handshake!!!!!!!!!! \n"));
		MlmeFreeMemory(pAd, (PUCHAR)mpool);
		return;
	}
	else
		DBGPRINT(RT_DEBUG_TRACE, (" MIC VALID in group msg 1 of 2-way handshake!!!!!!!!!! \n"));


	// 3. Decrypt GTK from Key Data
	if (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
	{
		// Decrypt AES GTK
		AES_GTK_KEY_UNWRAP(&pAd->StaCfg.PTK[16], KEYDATA,  pGroup->KeyDesc.KeyDataLen[1], pGroup->KeyDesc.KeyData);
	}
	else	// TKIP
	{
		INT i;

		// Decrypt TKIP GTK
		// Construct 32 bytes RC4 Key
		NdisMoveMemory(Key, pGroup->KeyDesc.KeyIv, 16);
		NdisMoveMemory(&Key[16], &pAd->StaCfg.PTK[16], 16);
		ARCFOUR_INIT(&pAd->PrivateInfo.WEPCONTEXT, Key, 32);
		//discard first 256 bytes
		for(i = 0; i < 256; i++)
			ARCFOUR_BYTE(&pAd->PrivateInfo.WEPCONTEXT);
		// Decrypt GTK. Becareful, there is no ICV to check the result is correct or not
		ARCFOUR_DECRYPT(&pAd->PrivateInfo.WEPCONTEXT, KEYDATA, pGroup->KeyDesc.KeyData, pGroup->KeyDesc.KeyDataLen[1]);
	}

	// Process decrypted key data material
	// Parse keyData to handle KDE format for WPA2PSK
	if (peerKeyInfo.EKD_DL)
	{
		if (!ParseKeyData(pAd, KEYDATA, pGroup->KeyDesc.KeyDataLen[1], 0))
		{
			os_free_mem(pAd, (PUCHAR)mpool);
			return;
		}
	}
	else	// WPAPSK
	{
		// set key material, TxMic and RxMic for WPAPSK
		NdisMoveMemory(GTK, KEYDATA, 32);
		NdisMoveMemory(pAd->StaCfg.GTK, GTK, 32);
		pAd->StaCfg.DefaultKeyId = peerKeyInfo.KeyIndex;

		// Prepare pair-wise key information into shared key table
		NdisZeroMemory(&pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId], sizeof(CIPHER_KEY));
		pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].KeyLen = LEN_TKIP_EK;
		NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key, GTK, LEN_TKIP_EK);
		NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic, &GTK[16], LEN_TKIP_RXMICK);
		NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic, &GTK[24], LEN_TKIP_TXMICK);

		// Update Shared Key CipherAlg
		pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_NONE;
		if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption2Enabled)
			pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_TKIP;
		else if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption3Enabled)
			pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_AES;
		else if (pAd->StaCfg.GroupCipher == Ndis802_11GroupWEP40Enabled)
			pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_WEP64;
		else if (pAd->StaCfg.GroupCipher == Ndis802_11GroupWEP104Enabled)
			pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_WEP128;

    	//hex_dump("Group Key :", pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key, LEN_TKIP_EK);
	}

	// Update group key information to ASIC Shared Key Table
	AsicAddSharedKeyEntry(pAd,
						  BSS0,
						  pAd->StaCfg.DefaultKeyId,
						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key,
						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic,
						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic);

	// Update ASIC WCID attribute table and IVEIV table
	RTMPAddWcidAttributeEntry(pAd,
							  BSS0,
							  pAd->StaCfg.DefaultKeyId,
							  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
							  NULL);

	// set 802.1x port control
	STA_PORT_SECURED(pAd);

    // Indicate Connected for GUI
    pAd->IndicateMediaState = NdisMediaStateConnected;

	// init header and Fill Packet
	MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);

	// Zero Group message 1 body
	NdisZeroMemory(&Packet, sizeof(Packet));
	Packet.ProVer	= EAPOL_VER;
	Packet.ProType	= EAPOLKey;
	Packet.Body_Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE;		// No data field

	//
	// Group Message 2 as  EAPOL-Key(1,0,0,0,G,0,0,MIC,0)
	//
	if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)
	{
		Packet.KeyDesc.Type = WPA2_KEY_DESC;
	}
	else
	{
		Packet.KeyDesc.Type = WPA1_KEY_DESC;
	}

	// Key descriptor version and appropriate RSN IE
	Packet.KeyDesc.KeyInfo.KeyDescVer = peerKeyInfo.KeyDescVer;

	// Update Key Length
	Packet.KeyDesc.KeyLength[0] = pGroup->KeyDesc.KeyLength[0];
	Packet.KeyDesc.KeyLength[1] = pGroup->KeyDesc.KeyLength[1];

	// Key Index as G-Msg 1
	if(pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK)
		Packet.KeyDesc.KeyInfo.KeyIndex = peerKeyInfo.KeyIndex;

	// Key Type Group key
	Packet.KeyDesc.KeyInfo.KeyType = GROUPKEY;

	// KeyMic field presented
	Packet.KeyDesc.KeyInfo.KeyMic  = 1;

	// Secure bit
	Packet.KeyDesc.KeyInfo.Secure  = 1;

	// Convert to little-endian format.
	*((USHORT *)&Packet.KeyDesc.KeyInfo) = cpu2le16(*((USHORT *)&Packet.KeyDesc.KeyInfo));

	// Key Replay count
	NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pGroup->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);

	// Out buffer for transmitting group message 2
	MlmeAllocateMemory(pAd, (PUCHAR *)&pOutBuffer);  // allocate memory
	if(pOutBuffer == NULL)
	{
		MlmeFreeMemory(pAd, (PUCHAR)mpool);
		return;
	}

	// Prepare EAPOL frame for MIC calculation
	// Be careful, only EAPOL frame is counted for MIC calculation
	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
		Packet.Body_Len[1] + 4,    &Packet,
		END_OF_ARGS);

	// Prepare and Fill MIC value
	NdisZeroMemory(Mic, sizeof(Mic));
	if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
	{
		// AES
		HMAC_SHA1(pOutBuffer, FrameLen, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
	}
	else
	{
		hmac_md5(pAd->StaCfg.PTK, LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
	}
	NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);


	MakeOutgoingFrame(pOutBuffer,       		&FrameLen,
						LENGTH_802_3,     		&Header802_3,
						Packet.Body_Len[1] + 4, &Packet,
						END_OF_ARGS);


	// 5. Copy frame to Tx ring and prepare for encryption
	RTMPToWirelessSta(pAd, Header802_3, LENGTH_802_3, (PUCHAR)&Packet, Packet.Body_Len[1] + 4, FALSE);

	// 6 Free allocated memory
	MlmeFreeMemory(pAd, (PUCHAR)pOutBuffer);
	os_free_mem(pAd, (PUCHAR)mpool);

	// send wireless event - for set key done WPA2
	if (pAd->CommonCfg.bWirelessEvent)
		RTMPSendWirelessEvent(pAd, IW_SET_KEY_DONE_WPA2_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);

	DBGPRINT(RT_DEBUG_TRACE, ("WpaGroupMsg1Action <-----\n"));
}

/*
	========================================================================

	Routine Description:
		Init WPA MAC header

	Arguments:
		pAd	Pointer	to our adapter

	Return Value:
		None

	Note:

	========================================================================
*/
VOID	WpaMacHeaderInit(
	IN		PRTMP_ADAPTER	pAd,
	IN OUT	PHEADER_802_11	pHdr80211,
	IN		UCHAR			wep,
	IN		PUCHAR		    pAddr1)
{
	NdisZeroMemory(pHdr80211, sizeof(HEADER_802_11));
	pHdr80211->FC.Type	= BTYPE_DATA;
	pHdr80211->FC.ToDs	= 1;
	if (wep	== 1)
		pHdr80211->FC.Wep = 1;

	 //	Addr1: BSSID, Addr2: SA, Addr3:	DA
	COPY_MAC_ADDR(pHdr80211->Addr1, pAddr1);
	COPY_MAC_ADDR(pHdr80211->Addr2, pAd->CurrentAddress);
	COPY_MAC_ADDR(pHdr80211->Addr3, pAd->CommonCfg.Bssid);
	pHdr80211->Sequence =	pAd->Sequence;
}

/*
	========================================================================

	Routine	Description:
		Copy frame from waiting queue into relative ring buffer and set
	appropriate ASIC register to kick hardware encryption before really
	sent out to air.

	Arguments:
		pAd		Pointer	to our adapter
		PNDIS_PACKET	Pointer to outgoing Ndis frame
		NumberOfFrag	Number of fragment required

	Return Value:
		None

	Note:

	========================================================================
*/
VOID    RTMPToWirelessSta(
	IN	PRTMP_ADAPTER	pAd,
	IN  PUCHAR          pHeader802_3,
    IN  UINT            HdrLen,
	IN  PUCHAR          pData,
    IN  UINT            DataLen,
    IN	BOOLEAN			is4wayFrame)

{
	NDIS_STATUS     Status;
	PNDIS_PACKET    pPacket;
	UCHAR   Index;

	do
	{
		// 1. build a NDIS packet and call RTMPSendPacket();
		//    be careful about how/when to release this internal allocated NDIS PACKET buffer
		Status = RTMPAllocateNdisPacket(pAd, &pPacket, pHeader802_3, HdrLen, pData, DataLen);
		if (Status != NDIS_STATUS_SUCCESS)
			break;

		if (is4wayFrame)
			RTMP_SET_PACKET_CLEAR_EAP_FRAME(pPacket, 1);
		else
			RTMP_SET_PACKET_CLEAR_EAP_FRAME(pPacket, 0);

		// 2. send out the packet
		Status = STASendPacket(pAd, pPacket);
		if(Status == NDIS_STATUS_SUCCESS)
		{
			// Dequeue one frame from TxSwQueue0..3 queue and process it
			// There are three place calling dequeue for TX ring.
			// 1. Here, right after queueing the frame.
			// 2. At the end of TxRingTxDone service routine.
			// 3. Upon NDIS call RTMPSendPackets
			if((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS)) &&
				(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)))
			{
				for(Index = 0; Index < 5; Index ++)
					if(pAd->TxSwQueue[Index].Number > 0)
						RTMPDeQueuePacket(pAd, FALSE, Index, MAX_TX_PROCESS);
			}
		}
	} while(FALSE);

}

/*
    ========================================================================

    Routine Description:
    Check Sanity RSN IE form AP

    Arguments:

    Return Value:


    ========================================================================
*/
BOOLEAN CheckRSNIE(
	IN  PRTMP_ADAPTER   pAd,
	IN  PUCHAR          pData,
	IN  UCHAR           DataLen,
	OUT	UCHAR			*Offset)
{
	PUCHAR              pVIE;
	UCHAR               len;
	PEID_STRUCT         pEid;
	BOOLEAN				result = FALSE;

	pVIE = pData;
	len	 = DataLen;
	*Offset = 0;

	while (len > sizeof(RSNIE2))
	{
		pEid = (PEID_STRUCT) pVIE;
		// WPA RSN IE
		if ((pEid->Eid == IE_WPA) && (NdisEqualMemory(pEid->Octet, WPA_OUI, 4)))
		{
			if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA || pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) &&
				(NdisEqualMemory(pVIE, pAd->MacTab.Content[BSSID_WCID].RSN_IE, pAd->MacTab.Content[BSSID_WCID].RSNIE_Len)) &&
				(pAd->MacTab.Content[BSSID_WCID].RSNIE_Len == (pEid->Len + 2)))
			{
					DBGPRINT(RT_DEBUG_TRACE, ("CheckRSNIE ==> WPA/WPAPSK RSN IE matched in Msg 3, Length(%d) \n", (pEid->Len + 2)));
					result = TRUE;
			}

			*Offset += (pEid->Len + 2);
		}
		// WPA2 RSN IE
		else if ((pEid->Eid == IE_RSN) && (NdisEqualMemory(pEid->Octet + 2, RSN_OUI, 3)))
		{
			if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2 || pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK) &&
				(NdisEqualMemory(pVIE, pAd->MacTab.Content[BSSID_WCID].RSN_IE, pAd->MacTab.Content[BSSID_WCID].RSNIE_Len)) &&
				(pAd->MacTab.Content[BSSID_WCID].RSNIE_Len == (pEid->Len + 2)))
			{
					DBGPRINT(RT_DEBUG_TRACE, ("CheckRSNIE ==> WPA2/WPA2PSK RSN IE matched in Msg 3, Length(%d) \n", (pEid->Len + 2)));
					result = TRUE;
			}

			*Offset += (pEid->Len + 2);
		}
		else
		{
			break;
		}

		pVIE += (pEid->Len + 2);
		len  -= (pEid->Len + 2);
	}

	DBGPRINT(RT_DEBUG_TRACE, ("CheckRSNIE ==> skip_offset(%d) \n", *Offset));

	return result;

}


/*
    ========================================================================

    Routine Description:
    Parse KEYDATA field.  KEYDATA[] May contain 2 RSN IE and optionally GTK.
    GTK  is encaptulated in KDE format at  p.83 802.11i D10

    Arguments:

    Return Value:

    Note:
        802.11i D10

    ========================================================================
*/
BOOLEAN ParseKeyData(
	IN  PRTMP_ADAPTER   pAd,
	IN  PUCHAR          pKeyData,
	IN  UCHAR           KeyDataLen,
	IN	UCHAR			bPairewise)
{
    PKDE_ENCAP          pKDE = NULL;
    PUCHAR              pMyKeyData = pKeyData;
    UCHAR               KeyDataLength = KeyDataLen;
    UCHAR               GTKLEN;
	UCHAR				skip_offset;

	// Verify The RSN IE contained in Pairewise-Msg 3 and skip it
	if (bPairewise)
    {
		// Check RSN IE whether it is WPA2/WPA2PSK
		if (!CheckRSNIE(pAd, pKeyData, KeyDataLen, &skip_offset))
		{
			DBGPRINT(RT_DEBUG_ERROR, ("ParseKeyData ==> WPA2/WPA2PSK RSN IE mismatched \n"));
			hex_dump("Get KEYDATA :", pKeyData, KeyDataLen);
			return FALSE;
    	}
    	else
		{
			// skip RSN IE
			pMyKeyData += skip_offset;
			KeyDataLength -= skip_offset;

			//DBGPRINT(RT_DEBUG_TRACE, ("ParseKeyData ==> WPA2/WPA2PSK RSN IE matched in Msg 3, Length(%d) \n", skip_offset));
		}
	}

	DBGPRINT(RT_DEBUG_TRACE,("ParseKeyData ==> KeyDataLength %d without RSN_IE \n", KeyDataLength));

	// Parse EKD format
	if (KeyDataLength >= 8)
    {
        pKDE = (PKDE_ENCAP) pMyKeyData;
    }
	else
    {
		DBGPRINT(RT_DEBUG_ERROR, ("ERROR: KeyDataLength is too short \n"));
        return FALSE;
    }


	// Sanity check - shared key index should not be 0
	if (pKDE->GTKEncap.Kid == 0)
    {
        DBGPRINT(RT_DEBUG_ERROR, ("ERROR: GTK Key index zero \n"));
        return FALSE;
    }

	// Sanity check - KED length
	if (KeyDataLength < (pKDE->Len + 2))
    {
        DBGPRINT(RT_DEBUG_ERROR, ("ERROR: The len from KDE is too short \n"));
        return FALSE;
    }

	// Get GTK length - refer to IEEE 802.11i-2004 p.82
	GTKLEN = pKDE->Len -6;

	if (GTKLEN < LEN_AES_KEY)
	{
		DBGPRINT(RT_DEBUG_ERROR, ("ERROR: GTK Key length is too short (%d) \n", GTKLEN));
        return FALSE;
	}
	else
		DBGPRINT(RT_DEBUG_TRACE, ("GTK Key with KDE formet got index=%d, len=%d \n", pKDE->GTKEncap.Kid, GTKLEN));

	// Update GTK
	// set key material, TxMic and RxMic for WPAPSK
	NdisMoveMemory(pAd->StaCfg.GTK, pKDE->GTKEncap.GTK, 32);
	pAd->StaCfg.DefaultKeyId = pKDE->GTKEncap.Kid;

	// Update shared key table
	NdisZeroMemory(&pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId], sizeof(CIPHER_KEY));
	pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].KeyLen = LEN_TKIP_EK;
	NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key, pKDE->GTKEncap.GTK, LEN_TKIP_EK);
	NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic, &pKDE->GTKEncap.GTK[16], LEN_TKIP_RXMICK);
	NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic, &pKDE->GTKEncap.GTK[24], LEN_TKIP_TXMICK);

	// Update Shared Key CipherAlg
	pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_NONE;
	if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption2Enabled)
		pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_TKIP;
	else if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption3Enabled)
		pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_AES;
	else if (pAd->StaCfg.GroupCipher == Ndis802_11GroupWEP40Enabled)
		pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_WEP64;
	else if (pAd->StaCfg.GroupCipher == Ndis802_11GroupWEP104Enabled)
		pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_WEP128;

	return TRUE;

}

/*
	========================================================================

	Routine Description:
		Cisco CCKM PRF function

	Arguments:
		key				Cisco Base Transient Key (BTK)
		key_len			The key length of the BTK
		data			Ruquest Number(RN) + BSSID
		data_len		The length of the data
		output			Store for PTK(Pairwise transient keys)
		len				The length of the output
	Return Value:
		None

	Note:
		802.1i	Annex F.9

	========================================================================
*/
VOID CCKMPRF(
	IN	UCHAR	*key,
	IN	INT		key_len,
	IN	UCHAR	*data,
	IN	INT		data_len,
	OUT	UCHAR	*output,
	IN	INT		len)
{
	INT		i;
	UCHAR	input[1024];
	INT		currentindex = 0;
	INT		total_len;

	NdisMoveMemory(input, data, data_len);
	total_len = data_len;
	input[total_len] = 0;
	total_len++;
	for	(i = 0;	i <	(len + 19) / 20; i++)
	{
		HMAC_SHA1(input, total_len,	key, key_len, &output[currentindex]);
		currentindex +=	20;
		input[total_len - 1]++;
	}
}

/*
	========================================================================

	Routine Description:
		Process MIC error indication and record MIC error timer.

	Arguments:
		pAd 	Pointer to our adapter
		pWpaKey 		Pointer to the WPA key structure

	Return Value:
		None

	IRQL = DISPATCH_LEVEL

	Note:

	========================================================================
*/
VOID	RTMPReportMicError(
	IN	PRTMP_ADAPTER	pAd,
	IN	PCIPHER_KEY 	pWpaKey)
{
	ULONG	Now;
    UCHAR   unicastKey = (pWpaKey->Type == PAIRWISE_KEY ? 1:0);

	// Record Last MIC error time and count
	Now = jiffies;
	if (pAd->StaCfg.MicErrCnt == 0)
	{
		pAd->StaCfg.MicErrCnt++;
		pAd->StaCfg.LastMicErrorTime = Now;
        NdisZeroMemory(pAd->StaCfg.ReplayCounter, 8);
	}
	else if (pAd->StaCfg.MicErrCnt == 1)
	{
		if ((pAd->StaCfg.LastMicErrorTime + (60 * OS_HZ)) < Now)
		{
			// Update Last MIC error time, this did not violate two MIC errors within 60 seconds
			pAd->StaCfg.LastMicErrorTime = Now;
		}
		else
		{

			if (pAd->CommonCfg.bWirelessEvent)
				RTMPSendWirelessEvent(pAd, IW_COUNTER_MEASURES_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);

			pAd->StaCfg.LastMicErrorTime = Now;
			// Violate MIC error counts, MIC countermeasures kicks in
			pAd->StaCfg.MicErrCnt++;
		}
	}
	else
	{
		// MIC error count >= 2
		// This should not happen
		;
	}
    MlmeEnqueue(pAd,
				MLME_CNTL_STATE_MACHINE,
				OID_802_11_MIC_FAILURE_REPORT_FRAME,
				1,
				&unicastKey);

    if (pAd->StaCfg.MicErrCnt == 2)
    {
        RTMPSetTimer(&pAd->StaCfg.WpaDisassocAndBlockAssocTimer, 100);
    }
}

#define	LENGTH_EAP_H    4
// If the received frame is EAP-Packet ,find out its EAP-Code (Request(0x01), Response(0x02), Success(0x03), Failure(0x04)).
INT	    WpaCheckEapCode(
	IN  PRTMP_ADAPTER   		pAd,
	IN  PUCHAR				pFrame,
	IN  USHORT				FrameLen,
	IN  USHORT				OffSet)
{

	PUCHAR	pData;
	INT	result = 0;

	if( FrameLen < OffSet + LENGTH_EAPOL_H + LENGTH_EAP_H )
		return result;

	pData = pFrame + OffSet; // skip offset bytes

	if(*(pData+1) == EAPPacket) 	// 802.1x header - Packet Type
	{
		 result = *(pData+4);		// EAP header - Code
	}

	return result;
}

VOID    WpaSendMicFailureToWpaSupplicant(
    IN  PRTMP_ADAPTER    pAd,
    IN  BOOLEAN          bUnicast)
{
    union iwreq_data    wrqu;
    char custom[IW_CUSTOM_MAX] = {0};

    sprintf(custom, "MLME-MICHAELMICFAILURE.indication");
    if (bUnicast)
        sprintf(custom, "%s unicast", custom);
    wrqu.data.length = strlen(custom);
    wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, custom);

    return;
}

VOID	WpaMicFailureReportFrame(
	IN  PRTMP_ADAPTER   pAd,
	IN MLME_QUEUE_ELEM *Elem)
{
	PUCHAR              pOutBuffer = NULL;
	UCHAR               Header802_3[14];
	ULONG               FrameLen = 0;
	EAPOL_PACKET        Packet;
	UCHAR               Mic[16];
    BOOLEAN             bUnicast;

	DBGPRINT(RT_DEBUG_TRACE, ("WpaMicFailureReportFrame ----->\n"));

    bUnicast = (Elem->Msg[0] == 1 ? TRUE:FALSE);
	pAd->Sequence = ((pAd->Sequence) + 1) & (MAX_SEQ_NUMBER);

	// init 802.3 header and Fill Packet
	MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);

	NdisZeroMemory(&Packet, sizeof(Packet));
	Packet.ProVer	= EAPOL_VER;
	Packet.ProType	= EAPOLKey;

	Packet.KeyDesc.Type = WPA1_KEY_DESC;

    // Request field presented
    Packet.KeyDesc.KeyInfo.Request = 1;

	if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
	{
		Packet.KeyDesc.KeyInfo.KeyDescVer = 2;
	}
	else	  // TKIP
	{
		Packet.KeyDesc.KeyInfo.KeyDescVer = 1;
	}

    Packet.KeyDesc.KeyInfo.KeyType = (bUnicast ? PAIRWISEKEY : GROUPKEY);

	// KeyMic field presented
	Packet.KeyDesc.KeyInfo.KeyMic  = 1;

    // Error field presented
	Packet.KeyDesc.KeyInfo.Error  = 1;

	// Update packet length after decide Key data payload
	Packet.Body_Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE;

	// Key Replay Count
	NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY);
    inc_byte_array(pAd->StaCfg.ReplayCounter, 8);

	// Convert to little-endian format.
	*((USHORT *)&Packet.KeyDesc.KeyInfo) = cpu2le16(*((USHORT *)&Packet.KeyDesc.KeyInfo));


	MlmeAllocateMemory(pAd, (PUCHAR *)&pOutBuffer);  // allocate memory
	if(pOutBuffer == NULL)
	{
		return;
	}

	// Prepare EAPOL frame for MIC calculation
	// Be careful, only EAPOL frame is counted for MIC calculation
	MakeOutgoingFrame(pOutBuffer,               &FrameLen,
		              Packet.Body_Len[1] + 4,   &Packet,
		              END_OF_ARGS);

	// Prepare and Fill MIC value
	NdisZeroMemory(Mic, sizeof(Mic));
	if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
	{	// AES
        UCHAR digest[20] = {0};
		HMAC_SHA1(pOutBuffer, FrameLen, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
	}
	else
	{	// TKIP
		hmac_md5(pAd->StaCfg.PTK,  LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
	}
	NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);

    MakeOutgoingFrame(pOutBuffer,           	&FrameLen,
    	  			LENGTH_802_3,     			&Header802_3,
    				Packet.Body_Len[1] + 4,     &Packet,
    				END_OF_ARGS);

	// opy frame to Tx ring and send MIC failure report frame to authenticator
	RTMPToWirelessSta(pAd, Header802_3, LENGTH_802_3, (PUCHAR)&Packet, Packet.Body_Len[1] + 4, FALSE);

	MlmeFreeMemory(pAd, (PUCHAR)pOutBuffer);

	DBGPRINT(RT_DEBUG_TRACE, ("WpaMicFailureReportFrame <-----\n"));
}

/** from wpa_supplicant
 * inc_byte_array - Increment arbitrary length byte array by one
 * @counter: Pointer to byte array
 * @len: Length of the counter in bytes
 *
 * This function increments the last byte of the counter by one and continues
 * rolling over to more significant bytes if the byte was incremented from
 * 0xff to 0x00.
 */
void inc_byte_array(UCHAR *counter, int len)
{
	int pos = len - 1;
	while (pos >= 0) {
		counter[pos]++;
		if (counter[pos] != 0)
			break;
		pos--;
	}
}

VOID WpaDisassocApAndBlockAssoc(
    IN PVOID SystemSpecific1,
    IN PVOID FunctionContext,
    IN PVOID SystemSpecific2,
    IN PVOID SystemSpecific3)
{
    RTMP_ADAPTER                *pAd = (PRTMP_ADAPTER)FunctionContext;
    MLME_DISASSOC_REQ_STRUCT    DisassocReq;

	// disassoc from current AP first
	DBGPRINT(RT_DEBUG_TRACE, ("RTMPReportMicError - disassociate with current AP after sending second continuous EAPOL frame\n"));
	DisassocParmFill(pAd, &DisassocReq, pAd->CommonCfg.Bssid, REASON_MIC_FAILURE);
	MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_DISASSOC_REQ, sizeof(MLME_DISASSOC_REQ_STRUCT), &DisassocReq);

	pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_DISASSOC;
	pAd->StaCfg.bBlockAssoc = TRUE;
}