rtmp_tkip.c

华硕无线网卡 167G linux 驱动

VOID	RTMPInitTkipEngine(
	IN	PRT2570ADAPTER	pAdapter,	
	IN	PUCHAR			pKey,
	IN	UCHAR			KeyId,
	IN	PUCHAR			pTA,
	IN	PUCHAR			pMICKey,
	IN	PUCHAR			pTSC,
	IN	INT				DoEncrypt,
	OUT	PULONG			pIV16,
	OUT	PULONG			pIV32,
	IN	PUCHAR			pDest)
{
    TKIP_IV	tkipIv;
    if (DoEncrypt == 0)
    {
        // Prepare 8 bytes TKIP encapsulation for MPDU
        memset(&tkipIv, 0, sizeof(TKIP_IV));
	tkipIv.IV16.word = 0;
        tkipIv.IV16.field.rc0 = *(pTSC + 1);	
        tkipIv.IV16.field.rc1 = (tkipIv.IV16.field.rc0 | 0x20) & 0x7f;	
        tkipIv.IV16.field.rc2 = *pTSC;	
        tkipIv.IV16.field.CONTROL.field.ExtIV = 1;  // 0: non-extended IV, 1: an extended IV
        tkipIv.IV16.field.CONTROL.field.KeyID = KeyId;
        tkipIv.IV32 = *(PULONG)(pTSC + 2);

        *pIV16 = tkipIv.IV16.word;
        *pIV32 = tkipIv.IV32;
    }
    else
    {
        pAdapter->PrivateInfo.FCSCRC32 = PPPINITFCS32;   //Init crc32.

        // Set IV16, IV32 from TSC value
        pAdapter->PrivateInfo.Tx.IV16 = (ULONG) *(pTSC) + (((ULONG) *(pTSC + 1)) << 8);	//TSC[0]:TSC[1]
        pAdapter->PrivateInfo.Tx.IV32 = *(PULONG)(pTSC + 2);

        // Init RC4 Key
        RTMPTkipMixTKey(&pAdapter->PrivateInfo.Tx, pKey, pTA);

        // Prepare 8 bytes TKIP encapsulation for MPDU
        memset(&tkipIv, 0, sizeof(TKIP_IV));
        tkipIv.IV16.field.rc2 = (UCHAR) (pAdapter->PrivateInfo.Tx.IV16 & 0xFF);	
        tkipIv.IV16.field.rc1 = (UCHAR) pAdapter->PrivateInfo.Tx.RC4KEY[1];	
        tkipIv.IV16.field.rc0 = (UCHAR) ((pAdapter->PrivateInfo.Tx.IV16 >> 8) & 0xFF);	
        tkipIv.IV32 = pAdapter->PrivateInfo.Tx.IV32;
        tkipIv.IV16.field.CONTROL.field.ExtIV = 1;  // 0: non-extended IV, 1: an extended IV
        tkipIv.IV16.field.CONTROL.field.KeyID = KeyId;

        *pIV16 = tkipIv.IV16.word;
        *pIV32 = tkipIv.IV32;


        // Copy 8 bytes encapsulation into Tx ring
        memcpy(pDest, &tkipIv, sizeof(TKIP_IV));

        // Init RC4 encyption engine
        ARCFOUR_INIT(&pAdapter->PrivateInfo.WEPCONTEXT, pAdapter->PrivateInfo.Tx.RC4KEY, 16);
    }


    // Prepare 8 bytes TKIP encapsulation for MPDU
    memset(&tkipIv, 0, sizeof(TKIP_IV));
    tkipIv.IV16.field.rc0 = *(pTSC + 1);	
    tkipIv.IV16.field.rc1 = (tkipIv.IV16.field.rc0 | 0x20) & 0x7f;	
    tkipIv.IV16.field.rc2 = *pTSC;	
    tkipIv.IV16.field.CONTROL.field.ExtIV = 1;  // 0: non-extended IV, 1: an extended IV
    tkipIv.IV16.field.CONTROL.field.KeyID = KeyId;
    tkipIv.IV32 = *(PULONG)(pTSC + 2);

    *pIV16 = tkipIv.IV16.word;
    *pIV32 = tkipIv.IV32;

}


VOID	RTMPTkipEncryptData(
	IN	PRT2570ADAPTER	pAdapter,	
	IN	PUCHAR			pSrc,
	IN	PUCHAR			pDest,
	IN	UINT			Len)
{
	pAdapter->PrivateInfo.FCSCRC32 = RTMP_CALC_FCS32(pAdapter->PrivateInfo.FCSCRC32, pSrc, Len);
	ARCFOUR_ENCRYPT(&pAdapter->PrivateInfo.WEPCONTEXT, pDest, pSrc, Len);


}
/*
	========================================================================

	Routine	Description:
		Init MIC Value calculation function which include set MIC key & 
		calculate first 16 bytes (DA + SA + priority +  0)
		
	Arguments:
      pAdapter		Pointer to our adapter
		pTKey       Pointer to the Temporal Key (TK), TK shall be 128bits.
		pDA			Pointer to DA address
		pSA			Pointer to SA address
		pMICKey		pointer to MIC Key
		
	Return Value:
		None

	Note:
	
	========================================================================
*/
VOID	RTMPInitMICEngine(
	IN	PRT2570ADAPTER	pAdapter,	
	IN	PUCHAR			pKey,
	IN	PUCHAR			pDA,
	IN	PUCHAR			pSA,
	IN	PUCHAR			pMICKey)
{
	UCHAR	Priority[4];

	// Zero priority value. Can change to input parameter if required
	memset(Priority, 0, 4);
	// Init MIC value calculation
	RTMPTkipSetMICKey(&pAdapter->PrivateInfo.Tx, pMICKey);
	// DA
	RTMPTkipAppend(&pAdapter->PrivateInfo.Tx, pDA, 6);
	// SA
	RTMPTkipAppend(&pAdapter->PrivateInfo.Tx, pSA, 6);
	// Priority + 3 bytes of 0
	RTMPTkipAppend(&pAdapter->PrivateInfo.Tx, Priority, 4);
}

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

	Routine	Description:
		Compare MIC value of received MSDU
		
	Arguments:
		pAdapter	Pointer to our adapter
		pSrc        Pointer to the received Plain text data
		pDA			Pointer to DA address
		pSA			Pointer to SA address
		pMICKey		pointer to MIC Key
		Len         the length of the received plain text data exclude MIC value
		
	Return Value:
		TRUE        MIC value matched
		FALSE       MIC value mismatched
		
	IRQL = DISPATCH_LEVEL
	
	Note:
	
	========================================================================
*/
BOOLEAN	RTMPTkipCompareMICValue(
	IN	PRT2570ADAPTER	pAdapter,
	IN	PUCHAR			pSrc,
	IN	PUCHAR			pDA,
	IN	PUCHAR			pSA,
	IN	PUCHAR			pMICKey,
	IN	UINT			Len)
{
	UCHAR	OldMic[8];
	UCHAR	Priority[4];
	INT		i;

	// Zero priority value. Can change to input parameter if required
	memset(Priority, 0, 4);
	// Init MIC value calculation
	RTMPTkipSetMICKey(&pAdapter->PrivateInfo.Rx, pMICKey);
	// DA
	RTMPTkipAppend(&pAdapter->PrivateInfo.Rx, pDA, 6);
	// SA
	RTMPTkipAppend(&pAdapter->PrivateInfo.Rx, pSA, 6);
	// Priority + 3 bytes of 0
	RTMPTkipAppend(&pAdapter->PrivateInfo.Rx, Priority, 4);
	
	// Calculate MIC value from plain text data
	RTMPTkipAppend(&pAdapter->PrivateInfo.Rx, pSrc, Len);

	// Get MIC valude from received frame
	memcpy(OldMic, pSrc + Len, 8);
	
	// Get MIC value from decrypted plain data
	RTMPTkipGetMIC(&pAdapter->PrivateInfo.Rx);
		
	// Move MIC value from MSDU, this steps should move to data path.
	// Since the MIC value might cross MPDUs.
	if(memcmp(pAdapter->PrivateInfo.Rx.MIC, OldMic, 8) != 0)
	{
		DBGPRINT(RT_DEBUG_ERROR, "! TKIP MIC Error Len=%d!\n", Len);  //MIC error.
		DBGPRINT(RT_DEBUG_INFO, "Orig MIC value = ");  //MIC error.
		for (i = 0; i < 8; i++)
		{
			DBGPRINT_RAW(RT_DEBUG_INFO, "%02x:", OldMic[i]);  //MIC error.
		}
		DBGPRINT_RAW(RT_DEBUG_INFO, "\n");  //MIC error.
		DBGPRINT(RT_DEBUG_INFO, "Calculated MIC value = ");  //MIC error.
		for (i = 0; i < 8; i++)
		{
			DBGPRINT_RAW(RT_DEBUG_INFO, "%02x:", pAdapter->PrivateInfo.Rx.MIC[i]);  //MIC error.
		}
		DBGPRINT_RAW(RT_DEBUG_INFO, "\n");  //MIC error.
		return (FALSE);
	}
	return (TRUE);
}


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

	Routine	Description:
		Compare MIC value of received MSDU
		
	Arguments:
		pAdapter	Pointer to our adapter
		pLLC		LLC header
		pSrc        Pointer to the received Plain text data
		pDA			Pointer to DA address
		pSA			Pointer to SA address
		pMICKey		pointer to MIC Key
		Len         the length of the received plain text data exclude MIC value
		
	Return Value:
		TRUE        MIC value matched
		FALSE       MIC value mismatched
		
	IRQL = DISPATCH_LEVEL
	
	Note:
	
	========================================================================
*/
BOOLEAN	RTMPTkipCompareMICValueWithLLC(
	IN	PRT2570ADAPTER	pAdapter,
	IN	PUCHAR			pLLC,
	IN	PUCHAR			pSrc,
	IN	PUCHAR			pDA,
	IN	PUCHAR			pSA,
	IN	PUCHAR			pMICKey,
	IN	UINT			Len)
{
	UCHAR	OldMic[8];
	UCHAR	Priority[4];
	INT		i;

	// Zero priority value. Can change to input parameter if required
	memset(Priority, 0, 4);
	// Init MIC value calculation
	RTMPTkipSetMICKey(&pAdapter->PrivateInfo.Rx, pMICKey);
	// DA
	RTMPTkipAppend(&pAdapter->PrivateInfo.Rx, pDA, 6);
	// SA
	RTMPTkipAppend(&pAdapter->PrivateInfo.Rx, pSA, 6);
	// Priority + 3 bytes of 0
	RTMPTkipAppend(&pAdapter->PrivateInfo.Rx, Priority, 4);
	
	// Start with LLC header
	RTMPTkipAppend(&pAdapter->PrivateInfo.Rx, pLLC, 8);

	// Calculate MIC value from plain text data
	RTMPTkipAppend(&pAdapter->PrivateInfo.Rx, pSrc, Len);

	// Get MIC valude from received frame
	memcpy(OldMic, pSrc + Len, 8);
	
	// Get MIC value from decrypted plain data
	RTMPTkipGetMIC(&pAdapter->PrivateInfo.Rx);
		
	// Move MIC value from MSDU, this steps should move to data path.
	// Since the MIC value might cross MPDUs.
	if(memcmp(pAdapter->PrivateInfo.Rx.MIC, OldMic, 8) != 0)
	{
		DBGPRINT_RAW(RT_DEBUG_ERROR, "! TKIP MIC Error !\n");  //MIC error.
		DBGPRINT(RT_DEBUG_INFO, "Orig MIC value = ");  //MIC error.
		for (i = 0; i < 8; i++)
		{
			DBGPRINT_RAW(RT_DEBUG_INFO, "%02x:", OldMic[i]);  //MIC error.
		}
		DBGPRINT_RAW(RT_DEBUG_INFO, "\n");  //MIC error.
		DBGPRINT(RT_DEBUG_INFO, "Calculated MIC value = ");  //MIC error.
		for (i = 0; i < 8; i++)
		{
			DBGPRINT_RAW(RT_DEBUG_INFO, "%02x:", pAdapter->PrivateInfo.Rx.MIC[i]);  //MIC error.
		}
		DBGPRINT_RAW(RT_DEBUG_INFO, "\n");  //MIC error.
		return (FALSE);
	}
	return (TRUE);
}
/*
	========================================================================

	Routine	Description:
		Copy frame from waiting queue into relative ring buffer and set 
	appropriate ASIC register to kick hardware transmit function
		
	Arguments:
		pAdapter		Pointer	to our adapter
		PNDIS_PACKET	Pointer to Ndis Packet for MIC calculation
		pEncap			Pointer to LLC encap data
		LenEncap		Total encap length, might be 0 which indicates no encap
		
	Return Value:
		None

	IRQL = DISPATCH_LEVEL
	
	Note:
	
	========================================================================
*/
VOID	RTMPCalculateMICValue(
	IN	PRT2570ADAPTER	pAdapter,
	IN  struct sk_buff  *skb,
	IN	PUCHAR			pEncap,
	IN	INT				LenEncap,
	IN	PWPA_KEY		pWpaKey)
{
    PUCHAR          pSrc;
    static UCHAR    Priority[4] = {"\x00\x00\x00\x00"};
   
    pSrc = (PUCHAR) skb->data;
    
    // Init MIC value calculation and reset the message
    pAdapter->PrivateInfo.Tx.L = RTMPTkipGetUInt32(pWpaKey->TxMic);
    pAdapter->PrivateInfo.Tx.R = RTMPTkipGetUInt32(pWpaKey->TxMic + 4);
    pAdapter->PrivateInfo.Tx.nBytesInM = 0;
    pAdapter->PrivateInfo.Tx.M = 0;
	
    // DA & SA field
    RTMPTkipAppend(&pAdapter->PrivateInfo.Tx, pSrc, 12);
    
    // Priority + 3 bytes of 0
    RTMPTkipAppend(&pAdapter->PrivateInfo.Tx, Priority, 4);
    
    if (LenEncap > 0)
    {
        // LLC encapsulation
        RTMPTkipAppend(&pAdapter->PrivateInfo.Tx, pEncap, LenEncap);
        // Protocol Type
        RTMPTkipAppend(&pAdapter->PrivateInfo.Tx, pSrc + 12, skb->len - 12);
    }
    else
        RTMPTkipAppend(&pAdapter->PrivateInfo.Tx, pSrc + 14, skb->len - 14);
    
    // Compute the final MIC Value
    RTMPTkipGetMIC(&pAdapter->PrivateInfo.Tx);

}

/************************************************************/ 
/* tkip_sbox()																*/ 
/* Returns a 16 bit value from a 64K entry table. The Table */ 
/* is synthesized from two 256 entry byte wide tables.		*/ 
/************************************************************/ 

UINT tkip_sbox(UINT index) 
{ 
	UINT index_low; 
	UINT index_high; 
	UINT left, right; 

	index_low = (index % 256); 
	index_high = ((index >> 8) % 256); 

	left = Tkip_Sbox_Lower[index_low] + (Tkip_Sbox_Upper[index_low] * 256); 
	right = Tkip_Sbox_Upper[index_high] + (Tkip_Sbox_Lower[index_high] * 256); 

	return (left ^ right); 
}

UINT rotr1(UINT a) 
{ 
	unsigned int b; 

	if ((a & 0x01) == 0x01)