wpa.c

r73模块的无线网卡在Linux下的驱动程序

	// 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;
	}
	memset(k_ipad, 0, sizeof(k_ipad));
	memset(k_opad, 0, sizeof(k_opad));
	memcpy(k_ipad, key,	key_len);
	memcpy(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)
{
    PKDE_ENCAP          pKDE = NULL;
    PNDIS_802_11_KEY    pGroupKey = NULL;
    PUCHAR              pMyKeyData = pKeyData;
    UCHAR               KeyDataLength = KeyDataLen;
    UCHAR               GTKLEN;
    INT                 i;

    if (!NdisEqualMemory(pKeyData, pAd->PortCfg.RSN_IE, pAd->PortCfg.RSN_IELen))
    {
        DBGPRINT(RT_DEBUG_ERROR, " RSN IE mismatched !!!!!!!!!! \n");
    }
    else
        DBGPRINT(RT_DEBUG_TRACE, " RSN IE matched !!!!!!!!!! \n");

    DBGPRINT(RT_DEBUG_ERROR, "KeyDataLen = %d  \n", KeyDataLen);

/*
====================================================================
======================================================================
*/
    if ((*pKeyData == WPARSNIE) && (*(pKeyData+1) != 0) && (KeyDataLength >= (2 + *(pKeyData+1))))
    {
        pMyKeyData = pKeyData + *(pKeyData+1) + 2;
        KeyDataLength -= (2 + *(pKeyData+1));
        DBGPRINT(RT_DEBUG_TRACE,"WPA RSN IE length %d contained in Msg3 = \n", (2 + *(pKeyData+1)));
    }
    if ((*pMyKeyData == WPA2RSNIE) && (*(pMyKeyData+1) != 0) && (KeyDataLength >= (2 + *(pMyKeyData+1))))
    {
        pMyKeyData += (*(pMyKeyData+1) + 2);
        KeyDataLength -= (2 + *(pMyKeyData+1));
        DBGPRINT(RT_DEBUG_TRACE,"WPA2 RSN IE length %d contained in Msg3 = \n", (2 + *(pMyKeyData+1)));
    }
        DBGPRINT(RT_DEBUG_TRACE,"KeyDataLength %d   \n", KeyDataLength);
    pKDE = (PKDE_ENCAP) pMyKeyData;//Modified by Thomas:for WPA2 crashed  error.

    if ((KeyDataLength >= 8) && (KeyDataLength <= sizeof(KDE_ENCAP)))
    {
	//pKDE = (PKDE_ENCAP) pMyKeyData;
        DBGPRINT(RT_DEBUG_TRACE,"pKDE = \n");
        DBGPRINT(RT_DEBUG_TRACE,"pKDE->Type %x:", pKDE->Type);
        DBGPRINT_RAW(RT_DEBUG_TRACE,"pKDE->Len 0x%x:", pKDE->Len);
        DBGPRINT_RAW(RT_DEBUG_TRACE,"pKDE->OUI %x %x %x :", pKDE->OUI[0],pKDE->OUI[1],pKDE->OUI[2] );
        DBGPRINT_RAW(RT_DEBUG_TRACE,"\n");
    }

    if (pKDE->GTKEncap.Kid == 0)
    {
        DBGPRINT(RT_DEBUG_ERROR,"GTK Key index zero , error\n");
        return;
    }

        GTKLEN = pKDE->Len -6;

        DBGPRINT(RT_DEBUG_TRACE,"GTK Key[%d] len=%d ", pKDE->GTKEncap.Kid, GTKLEN);
        for (i = 0; i < GTKLEN; i++)
        {
            DBGPRINT_RAW(RT_DEBUG_TRACE,"%02x:", pKDE->GTKEncap.GTK[i]);
        }
        DBGPRINT_RAW(RT_DEBUG_TRACE,"\n");

        // Update GTK
        pGroupKey = kmalloc(MAX_LEN_OF_MLME_BUFFER, MEM_ALLOC_FLAG);  // allocate memory
        if (pGroupKey == NULL)
		    return;

        memset(pGroupKey, 0, sizeof(NDIS_802_11_KEY) + LEN_EAP_KEY);
	    pGroupKey->Length    = sizeof(NDIS_802_11_KEY) + LEN_EAP_KEY;
	    pGroupKey->KeyIndex  = 0x20000000 | pKDE->GTKEncap.Kid;
	    pGroupKey->KeyLength = GTKLEN;

	    COPY_MAC_ADDR(pGroupKey->BSSID, pAd->PortCfg.Bssid);
	    memcpy(pGroupKey->KeyMaterial, pKDE->GTKEncap.GTK, 32);

        // Call Add peer key function
        RTMPWPAAddKeyProc(pAd, pGroupKey);

	if(pGroupKey != NULL){
		kfree(pGroupKey);
	}

}

VOID WPAMake8023Hdr(
    IN PRTMP_ADAPTER    pAd,
    IN PCHAR            pDAddr,
    IN OUT PCHAR        pHdr)
{
     // Addr1: DA, Addr2: BSSID, Addr3: SA
    memcpy(pHdr, pDAddr, MAC_ADDR_LEN);
    memcpy(&pHdr[MAC_ADDR_LEN], pAd->CurrentAddress, MAC_ADDR_LEN);
    pHdr[2*MAC_ADDR_LEN] = 0x88;
    pHdr[2*MAC_ADDR_LEN+1] = 0x8e;

}

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

	Routine Description:
		PRF function

	Arguments:

	Return Value:

	Note:
		802.1i	Annex F.9

	========================================================================
*/
VOID	PRF(
	IN	UCHAR	*key,
	IN	INT		key_len,
	IN	UCHAR	*prefix,
	IN	INT		prefix_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;

	memcpy(input, prefix, prefix_len);
	input[prefix_len] =	0;
	memcpy(&input[prefix_len + 1], data, data_len);
	total_len =	prefix_len + 1 + 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:
		Count TPTK from PMK

	Arguments:

	Return Value:
		Output		Store the TPTK

	Note:

	========================================================================
*/
VOID    WpaCountPTK(
	IN	UCHAR	*PMK,
	IN	UCHAR	*ANonce,
	IN	UCHAR	*AA,
	IN	UCHAR	*SNonce,
	IN	UCHAR	*SA,
	OUT	UCHAR	*output,
	IN	UINT	len)
{
	UCHAR	concatenation[76];
	UINT	CurrPos = 0;
	UCHAR	temp[32];
	UCHAR	Prefix[] = {'P', 'a', 'i', 'r', 'w', 'i', 's', 'e', ' ', 'k', 'e', 'y', ' ',
						'e', 'x', 'p', 'a', 'n', 's', 'i', 'o', 'n'};

	memset(temp, 0, sizeof(temp));

    // Get smaller address
    if (RTMPCompareMemory(SA, AA, 6) == 1)
	    memcpy(concatenation, AA, 6);
    else
        memcpy(concatenation, SA, 6);
    CurrPos += 6;

    // Get larger address
    if (RTMPCompareMemory(SA, AA, 6) == 1)
	    memcpy(&concatenation[CurrPos], SA, 6);
    else
	    memcpy(&concatenation[CurrPos], AA, 6);
    CurrPos += 6;

    // Get smaller address
    if (RTMPCompareMemory(ANonce, SNonce, 32) == 1)
	    memcpy(&concatenation[CurrPos], SNonce, 32);
    else
	    memcpy(&concatenation[CurrPos], ANonce, 32);
    CurrPos += 32;

    // Get larger address
    if (RTMPCompareMemory(ANonce, SNonce, 32) == 1)
	    memcpy(&concatenation[CurrPos], ANonce, 32);
    else
	    memcpy(&concatenation[CurrPos], SNonce, 32);
    CurrPos += 32;

    PRF(PMK, LEN_MASTER_KEY, Prefix, 22, concatenation, 76, output, len);

}

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

	Routine Description:
		Misc function to Generate random number

	Arguments:

	Return Value:

	Note:
		802.1i  Annex F.9

	========================================================================
*/
VOID	GenRandom(
	IN	PRTMP_ADAPTER	pAd,
	OUT	UCHAR			*random)
{
	INT		i, curr;
	UCHAR	local[80], KeyCounter[32];
	UCHAR	result[80];
	unsigned long CurrentTime;
	UCHAR	prefix[] = {'I', 'n', 'i', 't', ' ', 'C', 'o', 'u', 'n', 't', 'e', 'r'};

	memset(result, 0, 80);
	memset(local, 0, 80);
	memset(KeyCounter, 0, 32);
	COPY_MAC_ADDR(local, pAd->CurrentAddress);

	for	(i = 0;	i <	32;	i++)
	{
		curr =	MAC_ADDR_LEN;
		CurrentTime = jiffies;
		COPY_MAC_ADDR(local,  pAd->CurrentAddress);
		curr +=	MAC_ADDR_LEN;
		memcpy(&local[curr],  &CurrentTime,	sizeof(CurrentTime));
		curr +=	sizeof(CurrentTime);
		memcpy(&local[curr],  result, 32);
		curr +=	32;
		memcpy(&local[curr],  &i,  2);
		curr +=	2;
		PRF(KeyCounter, 32, prefix,12, local,	curr, result, 32);
	}
	memcpy(random, result,	32);
}

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

    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[512];
    INT         num_blocks = c_len/8;   // unit:64bits

    // Initialize
    memcpy(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;
            memcpy(BIN, A, 8);
            BIN[7] = A[7] ^ xor;
            memcpy(&BIN[8], &R[(i-1)*8], 8);
            aes_decrypt(&aesctx, BIN, BOUT);
            memcpy(A, &BOUT[0], 8);
            memcpy(&R[(i-1)*8], &BOUT[8], 8);
        }
    }

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

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


}

/*
    ========================================================================
    Routine Description:
       Send all EAP frames to wireless station.
       These frames don't come from normal SendPackets routine, but are EAPPacket, EAPOL,

    Arguments:
        pRxD        Pointer to the Rx descriptor

    Return Value:
    None
    ========================================================================
*/
VOID RTMPToWirelessSta(
    IN  PRTMP_ADAPTER   pAd,
    IN  PUCHAR          pFrame,
    IN  UINT            FrameLen)
{
    struct sk_buff  *skb;
    NDIS_STATUS     Status;

    do {
    	// 1. build a NDIS packet and call RTMPSendPacket();
     	//    be careful about how/when to release this internal allocated NDIS PACKET buffer
#ifdef RTMP_EMBEDDED
   		if ((skb = __dev_alloc_skb(FrameLen + 2, GFP_DMA|GFP_KERNEL)) != NULL)
#else
    	if ((skb = dev_alloc_skb(FrameLen + 2)) != NULL)
#endif
        {
            skb->len = FrameLen;
            skb->data_len = FrameLen;
            memcpy((skb->data), pFrame, FrameLen);
        }
        else
        {
            break;
        }


        // 2. send out the packet
        Status = RTMPSendPacket(pAd, skb);
        if (Status == N