wpa.c

ralink最新rt3070 usb wifi 无线网卡驱动程序

	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;