wpa.c

华硕无线网卡 167G linux 驱动

        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:
        pAdapter        Pointer to our adapter
        PNDIS_PACKET    Pointer to outgoing Ndis frame
        NumberOfFrag    Number of fragment required
        
    Return Value:
        None

    Note:
    
    ========================================================================
*/
VOID    WpaHardEncrypt(
    IN  PRT2570ADAPTER   pAdapter,
    IN  PUCHAR          pPacket,
    IN  ULONG           Len)
{
	CHAR                 *OutBuffer = NULL;
	UCHAR           FrameGap;
	PUCHAR          pDest;
	PUCHAR          pSrc;
	ULONG           Iv16;
	ULONG           Iv32;
	PWPA_KEY        pWpaKey;
	NDIS_STATUS           NStatus;

	NStatus = MlmeAllocateMemory(pAdapter, (PVOID)&OutBuffer);  //Get an unused nonpaged memory
	if (NStatus != NDIS_STATUS_SUCCESS) 
	{
		DBGPRINT(RT_DEBUG_TRACE, "WPA - WpaHardEncrypt allocate memory failed\n");
		pAdapter->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
		MlmeCntlConfirm(pAdapter, MT2_DISASSOC_CONF, MLME_FAIL_NO_RESOURCE);
		return;
	}
	
	FrameGap = IFS_BACKOFF;     // Default frame gap mode
	pDest = (PUCHAR)OutBuffer;
	pSrc = pPacket; // Point to start of MSDU
    
	// outgoing frame always wakeup PHY to prevent frame lost and 
	// turn off PSM bit to improve performance
	if (pAdapter->PortCfg.Psm == PWR_SAVE)
	{
		MlmeSetPsmBit(pAdapter, PWR_ACTIVE);
	}
	AsicForceWakeup(pAdapter);
    
	pWpaKey = (PWPA_KEY) &pAdapter->PortCfg.PairwiseKey[0];
	pWpaKey->Type = PAIRWISE_KEY;
	if (pWpaKey->KeyLen == 0)
	{
		// No pairwise key, this should not happen
		//NdisReleaseSpinLock(&pAdapter->TxRingLock);
		DBGPRINT(RT_DEBUG_ERROR,"no pairwise key\n");
		return;
	}

	if (pAdapter->PortCfg.WepStatus == Ndis802_11Encryption2Enabled)
	{
		INT     i = 0;
		// Start CALCULATE MIC Value
		RTMPInitMICEngine(
		                pAdapter,
		                pWpaKey->Key,
				pSrc + 4,
				pSrc + 10,
				pWpaKey->TxMic);
		
		memcpy(pDest, pSrc, LENGTH_802_11);
		pDest += LENGTH_802_11;
		pSrc += LENGTH_802_11;
		RTMPTkipAppend(&pAdapter->PrivateInfo.Tx, pSrc, (Len - LENGTH_802_11));
		RTMPTkipGetMIC(&pAdapter->PrivateInfo.Tx);
		//pAdapter->PrivateInfo.Tx.MIC[3]++;
		// Prepare IV, EIV, IV offset, Key for hardware encryption
		RTMPInitTkipEngine(
			pAdapter,
			pWpaKey->Key,
			0,
			pAdapter->CurrentAddress,
			pWpaKey->TxMic,
			pWpaKey->TxTsc,
			1,
			&Iv16,
			&Iv32,
			pDest);

		// Increase TxTsc value for next transmission
		while (++pWpaKey->TxTsc[i] == 0x0)
		{
			i++;
			if (i == 6)
			break;
		}

		if (i == 6)
		{
			// TODO: TSC has done one full cycle, do re-keying stuff follow specs
			// Should send a special event microsoft defined to request re-key
		}
            
		pDest += 8;
            
		RTMPTkipEncryptData(pAdapter, pSrc, pDest, (Len-LENGTH_802_11));
		pDest +=  (Len-LENGTH_802_11);
		//printk("-4\n");

		RTMPTkipEncryptData(pAdapter, pAdapter->PrivateInfo.Tx.MIC, pDest, 8);
		pDest +=  8;
		Len += 8;
		RTMPSetICV(pAdapter, pDest);
		Len += 12;
			
		MiniportMMRequest(pAdapter, OutBuffer, Len);
        
    }
    else if (pAdapter->PortCfg.WepStatus == Ndis802_11Encryption3Enabled)
    {
#if 0
        INT     i;
        PUCHAR  pTmp;

        i = 0;
        pTmp = (PUCHAR) &Iv16;
        *pTmp       = pWpaKey->TxTsc[0];
        *(pTmp + 1) = pWpaKey->TxTsc[1];
        *(pTmp + 2) = 0;
        *(pTmp + 3) = 0x20;
            
        Iv32 = *(PULONG)(&pWpaKey->TxTsc[2]);
            
        // Increase TxTsc value for next transmission
        while (++pWpaKey->TxTsc[i] == 0x0)
        {
            i++;
            if (i == 6)
                break;
        }
            
        // Copy IV
        memcpy(&pTxD->Iv, &Iv16, 4);
            
        // Copy EIV
        memcpy(&pTxD->Eiv, &Iv32, 4);
            
        // Set IV offset
        pTxD->IvOffset = LENGTH_802_11;

        // Copy TKey
        //red,memcpy(pTxD->Key, pWpaKey->Key, 16);

        // Set Cipher suite
        //red,CipherAlg = CIPHER_AES;

	//red
	pTempDest += LENGTH_802_11;
	memcpy(pTempDest, &Iv16, 4);
	pTempDest += 4;
	memcpy(pTempDest, &Iv32, 4);
	pTempDest += 4;
            
        // IV + EIV + HW MIC
        //red,Asic's suggestionLen += 16;
        PLCPLength = Len + 12;//12 is MIC + crc32
#endif
    }               
                
}

/*
    ========================================================================
    
    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;
    }
    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:
        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 (memcmp(SA, AA, 6) > 0)
        memcpy(concatenation, AA, 6);
    else
        memcpy(concatenation, SA, 6);
    CurrPos += 6;

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

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

    // Get larger address
    if (memcmp(ANonce, SNonce, 32) > 0)
        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  PRT2570ADAPTER   pAd, 
    OUT UCHAR           *random)
{   
    INT     i, curr ;
    UCHAR   local[80], KeyCounter[32];
    UCHAR   result[80];
     ULONG   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);
    memcpy(local, pAd->CurrentAddress, ETH_LENGTH_OF_ADDRESS);
    
    for (i = 0; i < 32; i++)
    {       
        curr =  ETH_LENGTH_OF_ADDRESS;
        CurrentTime = jiffies;
        memcpy(local,  pAd->CurrentAddress, ETH_LENGTH_OF_ADDRESS);
        curr += ETH_LENGTH_OF_ADDRESS;
        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   *ciphertext)
{
    UCHAR       A[8],   BIN[16], BOUT[16];
    UCHAR       R1[8],R2[8];
    UCHAR       xor;
    INT         num_blocks = 2;
    INT         j;
    aes_context aesctx;
    
    // Initialize
    // A = C[0]
    memcpy(A, ciphertext, 8);
    // R1 = C1
    memcpy(R1, &ciphertext[8], 8);
    // R2 = C2
    memcpy(R2, &ciphertext[16], 8);

    aes_set_key(&aesctx, key, 128);
    
    for (j = 5; j >= 0; j--)
    {
        xor = num_blocks * j + 2;
        memcpy(BIN, A, 8);
        BIN[7] = A[7] ^ xor;
        memcpy(&BIN[8], R2, 8);
        aes_decrypt(&aesctx, BIN, BOUT);
        memcpy(A, &BOUT[0], 8);
        memcpy(R2, &BOUT[8], 8);
        
        xor = num_blocks * j + 1;
        memcpy(BIN, A, 8);
        BIN[7] = A[7] ^ xor;
        memcpy(&BIN[8], R1, 8);
        aes_decrypt(&aesctx, BIN, BOUT);
        memcpy(A, &BOUT[0], 8);
        memcpy(R1, &BOUT[8], 8);
    }

    // OUTPUT
    memcpy(&plaintext[0], R1, 8);
    memcpy(&plaintext[8], R2, 8);
}