wpa.c

TP Link 321 Linux Driver

    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;
	ULONG				FrameLen = 0;
	UCHAR				EAPHEAD[8] = {0xaa,	0xaa, 0x03,	0x00, 0x00,	0x00,0x88,0x8e};
	EAPOL_PACKET		Packet;
	PEAPOL_PACKET		pGroup;
	UCHAR				Mic[16], OldMic[16];
	UCHAR				Key[32];
	PNDIS_802_11_KEY	pGroupKey = NULL;
	UCHAR				*mpool, *MSG, *KEYDATA;
	UCHAR               Header802_3[14];   
	USHORT              NStatus;
	UCHAR               *DescKeyData;

	mpool = kmalloc(3104, MEM_ALLOC_FLAG);  // allocate memory
	if (mpool == NULL)
		return;

	// MSG Len = 2048
	MSG = (UCHAR *) ROUND_UP(mpool, 4);
	// KEYDATA Len = 512
	KEYDATA = (UCHAR *) ROUND_UP(MSG+2048, 4);
	// DescKeyData Len = 512
	DescKeyData = (UCHAR *) ROUND_UP(KEYDATA+512, 4);

	if(NdisEqualMemory(&Elem->Msg[LENGTH_802_11 + 8], EAPHEAD, LENGTH_802_1_H))
	{
		DBGPRINT(RT_DEBUG_TRACE, "WpaGroupMsg1Action ----->MsgLen=%d\n",Elem->MsgLen);
		NdisMoveMemory(MSG, Elem->Msg, LENGTH_802_11);
		NdisMoveMemory(&MSG[LENGTH_802_11], &Elem->Msg[LENGTH_802_11+8], (Elem->MsgLen ));
	}
	else
	{
		DBGPRINT(RT_DEBUG_TRACE, "WpaGroupMsg1Action ----->\n");
		NdisMoveMemory(MSG, Elem->Msg, Elem->MsgLen);
	}
	
	// Process Group message 1 frame.
	pGroup = (PEAPOL_PACKET) &MSG[LENGTH_802_11 + LENGTH_802_1_H];

	// Store KeyData, protect received data more than MAX_LEN_OF_RSNIE
	NdisZeroMemory(DescKeyData, 512);
	NdisMoveMemory( DescKeyData, 
                    &MSG[LENGTH_802_11 + LENGTH_802_1_H + sizeof(EAPOL_PACKET) - MAX_LEN_OF_RSNIE], 
                    pGroup->KeyDesc.KeyDataLen[1]);

#ifdef BIG_ENDIAN
   // recovery original byte order, before forward Elem to another routine	   
{
    USHORT	tmpKeyinfo;

    NdisMoveMemory(&tmpKeyinfo, &pGroup->KeyDesc.KeyInfo, sizeof(USHORT));
    tmpKeyinfo = SWAP16(tmpKeyinfo);
    NdisMoveMemory(&pGroup->KeyDesc.KeyInfo, &tmpKeyinfo, sizeof(USHORT));
}
#endif

    // 0. Verify RSN IE & cipher type match
    if (pAd->PortCfg.WepStatus == Ndis802_11Encryption3Enabled && (pGroup->KeyDesc.KeyInfo.KeyDescVer != 2))
    {
		kfree(mpool);
        return;
    }
    else if (pAd->PortCfg.WepStatus == Ndis802_11Encryption2Enabled && (pGroup->KeyDesc.KeyInfo.KeyDescVer != 1))
    {
		kfree(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->PortCfg.ReplayCounter, LEN_KEY_DESC_REPLAY) != 1)
	{
        kfree(mpool);
		return;
	}
	
#ifdef BIG_ENDIAN
   // recovery original byte order, before forward Elem to another routine	   
{
    USHORT	tmpKeyinfo;

    NdisMoveMemory(&tmpKeyinfo, &pGroup->KeyDesc.KeyInfo, sizeof(USHORT));
    tmpKeyinfo = SWAP16(tmpKeyinfo);
    NdisMoveMemory(&pGroup->KeyDesc.KeyInfo, &tmpKeyinfo, sizeof(USHORT));
}
#endif	

	// Update new replay counter
	NdisMoveMemory(pAd->PortCfg.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->PortCfg.WepStatus	== Ndis802_11Encryption3Enabled)
	{
		// AES
		UCHAR digest[80];
		
		HMAC_SHA1((PUCHAR) pGroup, pGroup->Len[1] + 4, pAd->PortCfg.PTK, LEN_EAP_MICK, digest);
		NdisMoveMemory(Mic,	digest,	LEN_KEY_DESC_MIC);
	}
	else
	{
		hmac_md5(pAd->PortCfg.PTK, LEN_EAP_MICK, (PUCHAR) pGroup, pGroup->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");
		kfree(mpool);
		return;
	}
	else
		DBGPRINT(RT_DEBUG_TRACE, " MIC VALID in group msg 1 of 2-way handshake!!!!!!!!!! \n");

#ifdef BIG_ENDIAN
   // recovery original byte order, before forward Elem to another routine	   
{
    USHORT	tmpKeyinfo;

    NdisMoveMemory(&tmpKeyinfo, &pGroup->KeyDesc.KeyInfo, sizeof(USHORT));
    tmpKeyinfo = SWAP16(tmpKeyinfo);
    NdisMoveMemory(&pGroup->KeyDesc.KeyInfo, &tmpKeyinfo, sizeof(USHORT));
}
#endif

	// 3. Decrypt GTK from Key Data
	DBGPRINT(RT_DEBUG_TRACE, " Install = %d!!!!EKD_DL = %d!!!!!KeyIndex = %d! \n", pGroup->KeyDesc.KeyInfo.Install, pGroup->KeyDesc.KeyInfo.EKD_DL,
        pGroup->KeyDesc.KeyInfo.KeyIndex);

  
	pGroupKey = kmalloc(MAX_LEN_OF_MLME_BUFFER, MEM_ALLOC_FLAG);  // allocate memory    
	if (pGroupKey == NULL)
	{
		kfree(mpool);
		return;	
	}
			    
	if (pAd->PortCfg.WepStatus	== Ndis802_11Encryption3Enabled)
	{      
		// Decrypt AES GTK
		NdisZeroMemory(KEYDATA, 128);
	
		//if (pGroup->KeyDesc.KeyInfo.EKD_DL == 1)
			AES_GTK_KEY_UNWRAP(&pAd->PortCfg.PTK[16], KEYDATA,  pGroup->KeyDesc.KeyDataLen[1], DescKeyData);       

		// Update GTK
		ParseKeyData(pAd, KEYDATA, pGroup->KeyDesc.KeyDataLen[1], pGroup->KeyDesc.KeyInfo.KeyIndex, TRUE);
	
	}
	else	// TKIP
	{
		INT	i;

		// Decrypt TKIP GTK
		// Construct 32 bytes RC4 Key
		NdisMoveMemory(Key, pGroup->KeyDesc.KeyIv, 16);
		NdisMoveMemory(&Key[16], &pAd->PortCfg.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, DescKeyData, pGroup->KeyDesc.KeyDataLen[1]);

		DBGPRINT_RAW(RT_DEBUG_TRACE, "TKIP GTK = \n");
		for (i = 0; i < 32; i++)
		{
			DBGPRINT_RAW(RT_DEBUG_TRACE, "%2x ", KEYDATA[i]);
			if (i%16 == 15)
			DBGPRINT_RAW(RT_DEBUG_TRACE, "\n ");
		}
		DBGPRINT_RAW(RT_DEBUG_TRACE, "\n  \n");

		// Update GTK
		ParseKeyData(pAd, KEYDATA, pGroup->KeyDesc.KeyDataLen[1], pGroup->KeyDesc.KeyInfo.KeyIndex, TRUE);
	}
	
	// 4. Construct Group Message 2
	pAd->Sequence = ((pAd->Sequence) + 1) & (MAX_SEQ_NUMBER);
	WPAMake8023Hdr(pAd, pAd->PortCfg.Bssid, Header802_3);

	// Zero Group message 1 body
	NdisZeroMemory(&Packet, sizeof(Packet));
	Packet.Version = EAPOL_VER;
	Packet.Type    = EAPOLKey;
	Packet.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)
	//
	Packet.KeyDesc.Type = RSN_KEY_DESC;
	
	// Key descriptor version and appropriate RSN IE
	Packet.KeyDesc.KeyInfo.KeyDescVer = pGroup->KeyDesc.KeyInfo.KeyDescVer;

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

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

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

	// Secure bit
	if (pAd->PortCfg.AuthMode == Ndis802_11AuthModeWPAPSK)
		Packet.KeyDesc.KeyInfo.Secure  = 1;
	
	// Key Replay count	
	NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pGroup->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);		

#ifdef BIG_ENDIAN
   // recovery original byte order, before forward Elem to another routine	   
{
    USHORT	tmpKeyinfo;

    NdisMoveMemory(&tmpKeyinfo, &Packet.KeyDesc.KeyInfo, sizeof(USHORT));
    tmpKeyinfo = SWAP16(tmpKeyinfo);
    NdisMoveMemory(&Packet.KeyDesc.KeyInfo, &tmpKeyinfo, sizeof(USHORT));
}
#endif

	// Out buffer for transmitting group message 2
	NStatus = MlmeAllocateMemory(pAd, (PVOID *)&pOutBuffer);  //Get an unused nonpaged memory
	if (NStatus != NDIS_STATUS_SUCCESS) 
	{
		kfree(pGroupKey);
		kfree(mpool);  
		return;
	}		

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

	// Prepare and Fill MIC value
	NdisZeroMemory(Mic,	sizeof(Mic));
	if (pAd->PortCfg.WepStatus	== Ndis802_11Encryption3Enabled)
	{
		// AES
		UCHAR digest[80];
			
		HMAC_SHA1(pOutBuffer, FrameLen, pAd->PortCfg.PTK, LEN_EAP_MICK, digest);
		NdisMoveMemory(Mic,	digest,	LEN_KEY_DESC_MIC);
	}
	else
	{
		INT i;
		
		DBGPRINT_RAW(RT_DEBUG_INFO, "PTK = ");
		for (i = 0; i < 64; i++)
			DBGPRINT_RAW(RT_DEBUG_INFO, "%2x-", pAd->PortCfg.PTK[i]);
		DBGPRINT_RAW(RT_DEBUG_INFO, "\n FrameLen = %d\n", FrameLen);
			
		hmac_md5(pAd->PortCfg.PTK, LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
	}
	NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);


    FrameLen = 0;   
    MakeOutgoingFrame(pOutBuffer,   &FrameLen, 
                      LENGTH_802_3, &Header802_3,
//                    sizeof(EAPHEAD), EAPHEAD, 
		              Packet.Len[1] + 4,    &Packet,
		              END_OF_ARGS);

	// 5. Copy frame to Tx ring and prepare for encryption
	RTMPToWirelessSta(pAd, pOutBuffer, FrameLen);
	
	// Free allocated memory
	MlmeFreeMemory(pAd, pOutBuffer);
	
	kfree(pGroupKey);
	kfree(mpool);
    
	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: DA, Addr2: BSSID, Addr3:	SA
	COPY_MAC_ADDR(pHdr80211->Addr1, pAddr1);
	COPY_MAC_ADDR(pHdr80211->Addr2, pAd->CurrentAddress);
	COPY_MAC_ADDR(pHdr80211->Addr3, pAd->PortCfg.Bssid);
	pHdr80211->Sequence =	pAd->Sequence;
}

/*
	========================================================================
	
	Routine Description:
		SHA1 function 

	Arguments:
		
	Return Value:

	Note:
		
	========================================================================
*/
VOID	HMAC_SHA1(
	IN	UCHAR	*text,
	IN	UINT	text_len,
	IN	UCHAR	*key,
	IN	UINT	key_len,
	IN	UCHAR	*digest)
{
	SHA_CTX	context;
	UCHAR	k_ipad[65]; /* inner padding - key XORd with ipad	*/
	UCHAR	k_opad[65]; /* outer padding - key XORd with opad	*/
	INT		i;

	// if key is longer	than 64	bytes reset	it to key=SHA1(key)	
	if (key_len	> 64) 
	{
		SHA_CTX		 tctx;
		SHAInit(&tctx);
		SHAUpdate(&tctx, key, key_len);
		SHAFinal(&tctx,	key);
		key_len	= 20;
	}
	NdisZeroMemory(k_ipad, sizeof(k_ipad));
	NdisZeroMemory(k_opad, sizeof(k_opad));
	NdisMoveMemory(k_ipad, key,	key_len);
	NdisMoveMemory(k_opad, key,	key_len);

	// XOR key with	ipad and opad values  
	for	(i = 0;	i <	64;	i++) 
	{	
		k_ipad[i] ^= 0x36;
		k_opad[i] ^= 0x5c;
	}

	// perform inner SHA1 
	SHAInit(&context); 						/* init context for 1st pass */
	SHAUpdate(&context,	k_ipad,	64);		/*	start with inner pad */
	SHAUpdate(&context,	text, text_len);	/*	then text of datagram */
	SHAFinal(&context, digest);				/* finish up 1st pass */

	//perform outer	SHA1  
	SHAInit(&context);					/* init context for 2nd pass */
	SHAUpdate(&context,	k_opad,	64);	/*	start with outer pad */
	SHAUpdate(&context,	digest,	20);	/*	then results of	1st	hash */
	SHAFinal(&context, digest);			/* finish up 2nd pass */
}

/*
    ========================================================================
    
    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  
        
    ========================================================================
*/
VOID ParseKeyData(
	IN  PRTMP_ADAPTER   pAd,
	IN  PUCHAR          pKeyData,
	IN  UCHAR           KeyDataLen,
	IN	UCHAR			KeyIdx,
	IN	BOOLEAN			IsGroupMsg)
{
	PKDE_ENCAP          pKDE = NULL;
	PNDIS_802_11_KEY    pGroupKey = NULL;
	PUCHAR              pMyKeyData=NULL;// = pKeyData;