cmm_wpa.c

来自「Linux下的RT系列无线网卡驱动,可以直接在x86平台上编译」· C语言 代码 · 共 1,330 行 · 第 1/3 页

                break;

            case Ndis802_11Encryption3Enabled:
#ifdef CONFIG_STA_SUPPORT
				if (pAd->StaCfg.bMixCipher)
					NdisMoveMemory(pRsnie2->mcast, OUI_WPA2_TKIP, 4);
				else					
                	NdisMoveMemory(pRsnie2->mcast, OUI_WPA2_CCMP, 4);
#endif // CONFIG_STA_SUPPORT //
                pRsnie2->ucount = 1;
                NdisMoveMemory(pRsnie2->ucast[0].oui, OUI_WPA2_CCMP, 4);
                Rsnie_size = sizeof(RSNIE2);
                break;

        }

        pRsnie_auth2 = (RSNIE_AUTH*)((PUCHAR)pRsnie2 + Rsnie_size);

        switch (AuthMode)
        {
            case Ndis802_11AuthModeWPA2:
            case Ndis802_11AuthModeWPA1WPA2:
                pRsnie_auth2->acount = 1;
                NdisMoveMemory(pRsnie_auth2->auth[0].oui, OUI_WPA2_WEP40, 4);
                break;

            case Ndis802_11AuthModeWPA2PSK:
            case Ndis802_11AuthModeWPA1PSKWPA2PSK:
                pRsnie_auth2->acount = 1;
                NdisMoveMemory(pRsnie_auth2->auth[0].oui, OUI_WPA2_TKIP, 4);
                break;
        }

        pRSN_Cap = (RSN_CAPABILITIES*)((PUCHAR)pRsnie_auth2 + sizeof(RSNIE_AUTH));

#ifdef BIG_ENDIAN
	    pRsnie2->version = SWAP16(pRsnie2->version);
	    pRsnie2->ucount = SWAP16(pRsnie2->ucount);
	    pRsnie_auth2->acount = SWAP16(pRsnie_auth2->acount);
		pRSN_Cap->word = SWAP16(pRSN_Cap->word);
#endif


#ifdef CONFIG_STA_SUPPORT
		pAd->StaCfg.RSNIE_Len = Rsnie_size + sizeof(RSNIE_AUTH) + sizeof(RSN_CAPABILITIES);
#endif // CONFIG_STA_SUPPORT //		
    }


#ifdef CONFIG_STA_SUPPORT
    DBGPRINT(RT_DEBUG_TRACE,("RTMPMakeRSNIE in STA mode: RSNIE_Len = %d\n", pAd->StaCfg.RSNIE_Len));
#endif // CONFIG_STA_SUPPORT //

}


/*
    ==========================================================================
    Description:
       
    Return:
         TRUE if this is EAP frame
         FALSE otherwise
    ==========================================================================
*/
BOOLEAN RTMPCheckWPAframe(
    IN PRTMP_ADAPTER    pAd,
    IN PUCHAR           pData,
    IN ULONG            DataByteCount)
{
	ULONG	Body_len;

    if(DataByteCount < (LENGTH_802_1_H + LENGTH_EAPOL_H))
        return FALSE;

    DBGPRINT(RT_DEBUG_INFO, ("RTMPCheckWPAframe ===> \n"));
    
	// Skip LLC header	
    if (NdisEqualMemory(SNAP_802_1H, pData, 6)) 
    {
        pData += 6;
    }
    if (NdisEqualMemory(EAPOL, pData, 2)) 
    {
        pData += 2;         
    }
    else    
        return FALSE;

    switch (*(pData+1))     
    {   
        case EAPPacket:
			Body_len = (*(pData+2)<<8) | (*(pData+3));
            DBGPRINT(RT_DEBUG_TRACE, ("Receive EAP-Packet frame, TYPE = 0, Length = %ld\n", Body_len));
            break;
        case EAPOLStart:
            DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL-Start frame, TYPE = 1 \n"));
            break;
        case EAPOLLogoff:
            DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOLLogoff frame, TYPE = 2 \n"));
            break;
        case EAPOLKey:
			Body_len = (*(pData+2)<<8) | (*(pData+3));
            DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL-Key frame, TYPE = 3, Length = %ld\n", Body_len));
            break;
        case EAPOLASFAlert:
            DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOLASFAlert frame, TYPE = 4 \n"));
            break;
        default:
            return FALSE;
    
    }   
    return TRUE;
}


/*
    ==========================================================================
    Description:
        ENCRYPT AES GTK before sending in EAPOL frame.
        AES GTK length = 128 bit,  so fix blocks for aes-key-wrap as 2 in this function.
        This function references to RFC 3394 for aes key wrap algorithm.
    Return:
    ==========================================================================
*/  
VOID AES_GTK_KEY_WRAP( 
    IN UCHAR    *key,
    IN UCHAR    *plaintext,
    IN UCHAR    p_len,
    OUT UCHAR   *ciphertext)
{
    UCHAR       A[8], BIN[16], BOUT[16];
    UCHAR       R[512];
    INT         num_blocks = p_len/8;   // unit:64bits
    INT         i, j;
    aes_context aesctx;
    UCHAR       xor;

    aes_set_key(&aesctx, key, 128);

    // Init IA
    for (i = 0; i < 8; i++)
        A[i] = 0xa6;

    //Input plaintext
    for (i = 0; i < num_blocks; i++)
    {
        for (j = 0 ; j < 8; j++)
            R[8 * (i + 1) + j] = plaintext[8 * i + j];
    }

    // Key Mix
    for (j = 0; j < 6; j++)
    {
        for(i = 1; i <= num_blocks; i++)
        {
            //phase 1
            NdisMoveMemory(BIN, A, 8);
            NdisMoveMemory(&BIN[8], &R[8 * i], 8);
            aes_encrypt(&aesctx, BIN, BOUT);

            NdisMoveMemory(A, &BOUT[0], 8);
            xor = num_blocks * j + i;
            A[7] = BOUT[7] ^ xor;
            NdisMoveMemory(&R[8 * i], &BOUT[8], 8);
        }
    }

    // Output ciphertext
    NdisMoveMemory(ciphertext, A, 8);

    for (i = 1; i <= num_blocks; i++)
    {
        for (j = 0 ; j < 8; j++)
            ciphertext[8 * i + j] = R[8 * i + j];
    }
}


/*
	========================================================================
	
	Routine Description:
		Misc function to decrypt AES body
	
	Arguments:
			
	Return Value:
	
	Note:
		This function references to	RFC	3394 for aes key unwrap algorithm.
			
	========================================================================
*/
VOID	AES_GTK_KEY_UNWRAP( 
	IN	UCHAR	*key,
	OUT	UCHAR	*plaintext,
	IN	UCHAR    c_len,
	IN	UCHAR	*ciphertext)
	
{
	UCHAR       A[8], BIN[16], BOUT[16];
	UCHAR       xor;
	INT         i, j;
	aes_context aesctx;
	UCHAR       *R;
	INT         num_blocks = c_len/8;	// unit:64bits

	
	os_alloc_mem(NULL, (PUCHAR *)&R, 512);

	if (R == NULL)
    {
        DBGPRINT(RT_DEBUG_ERROR, ("!!!AES_GTK_KEY_UNWRAP: no memory!!!\n"));
        return;
    } /* End of if */

	// Initialize
	NdisMoveMemory(A, ciphertext, 8);
	//Input plaintext
	for(i = 0; i < (c_len-8); i++)
	{
		R[ i] = ciphertext[i + 8];
	}

	aes_set_key(&aesctx, key, 128);

	for(j = 5; j >= 0; j--)
	{
		for(i = (num_blocks-1); i > 0; i--)
		{
			xor = (num_blocks -1 )* j + i;
			NdisMoveMemory(BIN, A, 8);
			BIN[7] = A[7] ^ xor;
			NdisMoveMemory(&BIN[8], &R[(i-1)*8], 8);
			aes_decrypt(&aesctx, BIN, BOUT);
			NdisMoveMemory(A, &BOUT[0], 8);
			NdisMoveMemory(&R[(i-1)*8], &BOUT[8], 8);
		}
	}

	// OUTPUT
	for(i = 0; i < c_len; i++)
	{
		plaintext[i] = R[i];
	}

	DBGPRINT_RAW(RT_DEBUG_INFO, ("plaintext = \n"));
	for(i = 0; i < (num_blocks *8); i++)
	{
		DBGPRINT_RAW(RT_DEBUG_INFO, ("%2x ", plaintext[i]));
		if(i%16 == 15)
			DBGPRINT_RAW(RT_DEBUG_INFO, ("\n "));
	}
	DBGPRINT_RAW(RT_DEBUG_INFO, ("\n  \n"));	

	os_free_mem(NULL, R);
}


CHAR *GetEapolMsgType(CHAR msg)
{
    if(msg == EAPOL_PAIR_MSG_1)
        return "Pairwise Message 1";
    else if(msg == EAPOL_PAIR_MSG_2)
        return "Pairwise Message 2";
	else if(msg == EAPOL_PAIR_MSG_3)
        return "Pairwise Message 3";
	else if(msg == EAPOL_PAIR_MSG_4)
        return "Pairwise Message 4";
	else if(msg == EAPOL_GROUP_MSG_1)
        return "Group Message 1";
	else if(msg == EAPOL_GROUP_MSG_2)
        return "Group Message 2";
    else
    	return "Invalid Message";
}


/*
    ========================================================================
    
    Routine Description:
    Check Sanity RSN IE of EAPoL message

    Arguments:
        
    Return Value:

		
    ========================================================================
*/
BOOLEAN RTMPCheckRSNIE(
	IN  PRTMP_ADAPTER   pAd,
	IN  PUCHAR          pData,
	IN  UCHAR           DataLen,
	IN  MAC_TABLE_ENTRY *pEntry,
	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 ((pEntry->AuthMode == Ndis802_11AuthModeWPA || pEntry->AuthMode == Ndis802_11AuthModeWPAPSK) &&
				(NdisEqualMemory(pVIE, pEntry->RSN_IE, pEntry->RSNIE_Len)) &&
				(pEntry->RSNIE_Len == (pEid->Len + 2)))
			{				
					DBGPRINT(RT_DEBUG_INFO, ("RTMPCheckRSNIE ==> WPA/WPAPSK RSN IE matched, 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 ((pEntry->AuthMode == Ndis802_11AuthModeWPA2 || pEntry->AuthMode == Ndis802_11AuthModeWPA2PSK) &&
				(NdisEqualMemory(pVIE, pEntry->RSN_IE, pEntry->RSNIE_Len)) &&
				(pEntry->RSNIE_Len == (pEid->Len + 2)))
			{				
					DBGPRINT(RT_DEBUG_INFO, ("RTMPCheckRSNIE ==> WPA2/WPA2PSK RSN IE matched, 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_INFO, ("RTMPCheckRSNIE ==> 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 RTMPParseEapolKeyData(
	IN  PRTMP_ADAPTER   pAd,
	IN  PUCHAR          pKeyData,
	IN  UCHAR           KeyDataLen,
	IN	UCHAR			GroupKeyIndex,
	IN	UCHAR			MsgType,
	IN	BOOLEAN			bWPA2,
	IN  MAC_TABLE_ENTRY *pEntry)
{
    PKDE_ENCAP          pKDE = NULL;
    PUCHAR              pMyKeyData = pKeyData;
    UCHAR               KeyDataLength = KeyDataLen;
    UCHAR               GTKLEN = 0;
	UCHAR				DefaultIdx = 0;
	UCHAR				skip_offset;		
	    
	// Verify The RSN IE contained in pairewise_msg_2 && pairewise_msg_3 and skip it
	if (MsgType == EAPOL_PAIR_MSG_2 || MsgType == EAPOL_PAIR_MSG_3)
    {
		// Check RSN IE whether it is WPA2/WPA2PSK   		
		if (!RTMPCheckRSNIE(pAd, pKeyData, KeyDataLen, pEntry, &skip_offset))
		{
			// send wireless event - for RSN IE different
			if (pAd->CommonCfg.bWirelessEvent)
				RTMPSendWirelessEvent(pAd, IW_RSNIE_DIFF_EVENT_FLAG, pEntry); 

        	DBGPRINT(RT_DEBUG_ERROR, ("RSN_IE Different in msg %d of 4-way handshake!\n", MsgType));			
			hex_dump("Receive RSN_IE ", pKeyData, KeyDataLen);
			hex_dump("Desired RSN_IE ", pEntry->RSN_IE, pEntry->RSNIE_Len);	
					
			return FALSE;			
    	}
    	else
		{
			if (bWPA2 && MsgType == EAPOL_PAIR_MSG_3)
			{
				// skip RSN IE
				pMyKeyData += skip_offset;
				KeyDataLength -= skip_offset;
				DBGPRINT(RT_DEBUG_TRACE, ("RTMPParseEapolKeyData ==> WPA2/WPA2PSK RSN IE matched in Msg 3, Length(%d) \n", skip_offset));
			}
			else
				return TRUE;			
		}
	}

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

	// Parse EKD format in pairwise_msg_3_WPA2 && group_msg_1_WPA2
	if (bWPA2 && (MsgType == EAPOL_PAIR_MSG_3 || MsgType == EAPOL_GROUP_MSG_1))
	{				
		if (KeyDataLength >= 8)	// KDE format exclude GTK length
    	{
        	pKDE = (PKDE_ENCAP) pMyKeyData;			
	        DBGPRINT(RT_DEBUG_INFO, ("pKDE->Type %x \n", pKDE->Type));
    	    DBGPRINT(RT_DEBUG_INFO, ("pKDE->Len 0x%x \n", pKDE->Len));
        	DBGPRINT(RT_DEBUG_INFO, ("pKDE->OUI %x %x %x \n", pKDE->OUI[0],pKDE->OUI[1],pKDE->OUI[2]));
	    	DBGPRINT(RT_DEBUG_INFO, ("pKDE->DataType %x \n", pKDE->DataType));
			
			DefaultIdx = pKDE->GTKEncap.Kid;

			// 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;
			}