wpa.c

TP Link 321 Linux Driver

	UCHAR				*KeyDataBuf, *mpool;
	UCHAR               KeyDataLength = KeyDataLen;
	UCHAR               GTKLEN;
	INT                 i;
	ULONG               Idx;   
	PUCHAR              pVIE=NULL;
	UCHAR               Len;
	PEID_STRUCT         pEid;
	BOOLEAN				bKDE_Key = FALSE;	// Key Data Encapsulation format

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

	// KeyDataBuf len = 256.
	KeyDataBuf = (UCHAR *) ROUND_UP(mpool, 4);
	// Init the buffer
	NdisZeroMemory(KeyDataBuf, 256);
	
	if((Idx = BssTableSearch(&pAd->ScanTab, pAd->PortCfg.Bssid, pAd->PortCfg.Channel)) == BSS_NOT_FOUND)
	{
		DBGPRINT(RT_DEBUG_ERROR, "%s, Can't find BSS\n", __FUNCTION__);
		return;
	}
	
	pVIE = pAd->ScanTab.BssEntry[Idx].VarIEs;
	Len = (UCHAR)pAd->ScanTab.BssEntry[Idx].VarIELen;
	while(Len > 0)
	{
	    pEid = (PEID_STRUCT) pVIE;	
		if (pEid->Eid != IE_RSN)
		{
			pVIE += (pEid->Len + 2);
			Len  -= (pEid->Len + 2);
			continue;
		}

		if (!NdisEqualMemory(pKeyData, pEid->Octet, pEid->Len))
		{
//			DBGPRINT(RT_DEBUG_ERROR, " RSN IE mismatched !!!!!!!!!! \n");
		}
		else
		{
//			DBGPRINT(RT_DEBUG_TRACE, " RSN IE matched !!!!!!!!!! \n");
		}
        
		Len  -= (pEid->Len + 2);    
		break;
	}


/*
====================================================================
======================================================================
*/
	NdisMoveMemory(KeyDataBuf, pKeyData, KeyDataLen);
	pMyKeyData =(PUCHAR) KeyDataBuf;
	if (!IsGroupMsg)
	{
		do{
			if ((*pMyKeyData == WPARSNIE) && (*(pMyKeyData+1) != 0) && (KeyDataLength >= (2 + *(pMyKeyData+1))))      
			{
				if ((*(pMyKeyData+2) == 0x00) && (*(pMyKeyData+3) == 0x0f) && (*(pMyKeyData+4) == 0xac))
					break;
					
				DBGPRINT_RAW(RT_DEBUG_TRACE,"WPA WPA_IE length %d contained in this message \n", (2 + *(pMyKeyData+1)));
				KeyDataLength -= (2 + *(pMyKeyData+1));
				pMyKeyData += *(pMyKeyData+1) + 2;
			}
			if ((*pMyKeyData == WPA2RSNIE) && (*(pMyKeyData+1) != 0) && (KeyDataLength >= (2 + *(pMyKeyData+1))))       
			{
				DBGPRINT_RAW(RT_DEBUG_TRACE,"WPA2 RSN_IE length %d contained in this message \n", (2 + *(pMyKeyData+1)));
				KeyDataLength -= (2 + *(pMyKeyData+1));
				pMyKeyData += (*(pMyKeyData+1) + 2);
			}
			
		} while(KeyDataLength > 0);
	}	
	DBGPRINT_RAW(RT_DEBUG_TRACE,"KeyDataLength %d   \n", KeyDataLength);

	// GTK key Data Encapsulation
	pKDE = (PKDE_ENCAP) pMyKeyData;
	if((KeyDataLength >= 8) && (pKDE->Type == WPARSNIE) && (pKDE->OUI[0] == 0x00) && (pKDE->OUI[1]== 0x0f) && (pKDE->OUI[2]== 0x0ac) && (pKDE->DataType== 0x01))
	{   
		DBGPRINT_RAW(RT_DEBUG_TRACE,"pKDE = \n");
		DBGPRINT_RAW(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");                      

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


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

	NdisZeroMemory(pGroupKey, sizeof(NDIS_802_11_KEY) + LEN_EAP_KEY);
	pGroupKey->Length = sizeof(NDIS_802_11_KEY) + LEN_EAP_KEY;
	COPY_MAC_ADDR(pGroupKey->BSSID, pAd->PortCfg.Bssid);

	if (bKDE_Key)	// GTK Key Data Encapsulation
	{
		GTKLEN = pKDE->Len -6;

		DBGPRINT_RAW(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");                       

		pGroupKey->KeyIndex  = 0x20000000 | pKDE->GTKEncap.Kid;
		pGroupKey->KeyLength = GTKLEN;

		NdisMoveMemory(pGroupKey->KeyMaterial, pKDE->GTKEncap.GTK, 32);

		// Call Add peer key function
		RTMPWPAAddKeyProc(pAd, pGroupKey);
	}		
	else if (KeyIdx != 0)	// non-KDE Key
	{
		pGroupKey->KeyIndex  = 0x20000000 | KeyIdx;
		pGroupKey->KeyLength = 32;	//KeyDataLen

		NdisMoveMemory(pGroupKey->KeyMaterial, pMyKeyData, LEN_EAP_KEY);

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

	kfree(pGroupKey);
	kfree(mpool);

}

VOID WPAMake8023Hdr(
    IN PRTMP_ADAPTER    pAd, 
    IN PCHAR            pDAddr, 
    IN OUT PCHAR        pHdr)
{    
     // Addr1: DA, Addr2: BSSID, Addr3: SA
    NdisMoveMemory(pHdr, pDAddr, MAC_ADDR_LEN);
    NdisMoveMemory(&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;
	INT		currentindex = 0;
	INT		total_len;
	BOOLEAN	bfree = TRUE;

	input = kmalloc(1024, MEM_ALLOC_FLAG);  // allocate memory
    if (input == NULL) 
    {
    	input = prf_input;
		bfree = FALSE;
    }
	
	NdisMoveMemory(input, prefix, prefix_len);
	input[prefix_len] =	0;
	NdisMoveMemory(&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]++;
	}	

	if (bfree==TRUE) 
	    kfree(input);
}

/*
	========================================================================
	
	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'};

	NdisZeroMemory(temp, sizeof(temp));

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

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

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

	// Get larger address
	if (RTMPCompareMemory(ANonce, SNonce, 32) == 1)
		NdisMoveMemory(&concatenation[CurrPos], ANonce, 32);
	else
		NdisMoveMemory(&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, *KeyCounter;
	UCHAR	        *result;
	unsigned long	CurrentTime;
	UCHAR	prefix[] = {'I', 'n', 'i', 't', ' ', 'C', 'o', 'u', 'n', 't', 'e', 'r'};
	UCHAR	        *mpool;

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

	// local Len = 80.
	local = (UCHAR *) ROUND_UP(mpool, 4);
	// KeyCounter Len = 32.
	KeyCounter = (UCHAR *) ROUND_UP(local + 80, 4);
	// result Len = 80.
	result = (UCHAR *) ROUND_UP(KeyCounter + 32, 4);

	NdisZeroMemory(result, 80);
	NdisZeroMemory(local, 80);
	NdisZeroMemory(KeyCounter, 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;
		NdisMoveMemory(&local[curr],  &CurrentTime,	sizeof(CurrentTime));
		curr +=	sizeof(CurrentTime);
		NdisMoveMemory(&local[curr],  result, 32);
		curr +=	32;
		NdisMoveMemory(&local[curr],  &i,  2);		
		curr +=	2;
		PRF(KeyCounter, 32, prefix,12, local,	curr, result, 32); 
	}
	NdisMoveMemory(random, result,	32);	
	kfree(mpool);
}

/*
    ========================================================================
    
    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, *BIN, *BOUT;
	UCHAR       xor;
	INT         i, j;
	aes_context aesctx;
	UCHAR        *R;
	INT         num_blocks = c_len/8;   // unit:64bits
	UCHAR       *mpool;

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

	// A len = 8.
	A = (UCHAR *) ROUND_UP(mpool, 4);
	// BIN len = 16.
	BIN = (UCHAR *) ROUND_UP(A + 8, 4);
	// BOUT len = 16.
	BOUT = (UCHAR *) ROUND_UP(BIN + 16, 4);
	// R len = 512.
	R = (UCHAR *) ROUND_UP(BOUT + 16, 4);

	// 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_TRACE, "plaintext = \n");
	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_RAW(RT_DEBUG_TRACE, "\n  \n");

	kfree(mpool);
}

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