wpa.c

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

	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;

    	//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
	//pAd->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
	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));