rtmp_tkip.c

华硕无线网卡 167G linux 驱动

	{ 
		b = (a >> 1) | 0x8000; 
	} 
	else 
	{ 
		b = (a >> 1) & 0x7fff; 
	} 
	b = b % 65536; 
	return b; 
} 
/*
	========================================================================

	Routine	Description:
		Key Mixing function.
		
	Arguments:
      pAdapter		Pointer to our adapter
		pTKey       Pointer to the Temporal Key (TK), TK shall be 128bits.		
		pTA			Pointer to transmitter address
		nl			Least significant 16 bits of IV16
		nh			Most significant 32 bits of IV32 
		
	Return Value:
		None

	Note:
		Calculates an RC4 key.
	========================================================================
*/
VOID	RTMPTkipMixTKey( 
	IN	PTKIP_KEY_INFO	pTkip,
	IN	PUCHAR			pTKey,
	IN	PUCHAR			pTA)
{ 
	UINT	tsc0; 
	UINT	tsc1;
	UINT	tsc2; 

	UINT	ppk0; 
	UINT	ppk1; 
	UINT	ppk2; 
	UINT	ppk3; 
	UINT	ppk4; 
	UINT	ppk5; 
	UINT	p1k[5];

	int i; 
	int j; 

	tsc0 = (UINT)((pTkip->IV32 >> 16) % 65536); /* msb */ 
	tsc1 = (UINT)(pTkip->IV32 % 65536); 
	tsc2 = (UINT)(pTkip->IV16 % 65536); /* lsb */ 

	/* Phase 1, step 1 */ 
	p1k[0] = tsc1; 
	p1k[1] = tsc0; 
	p1k[2] = (unsigned int)(pTA[0] + (pTA[1]*256)); 
	p1k[3] = (unsigned int)(pTA[2] + (pTA[3]*256)); 
	p1k[4] = (unsigned int)(pTA[4] + (pTA[5]*256)); 

	/* Phase 1, step 2 */ 
	for (i=0; i<8; i++) 
	{ 
		j = 2*(i & 1); 
		p1k[0] = (p1k[0] + tkip_sbox( (p1k[4] ^ ((256*pTKey[1+j]) + pTKey[j])) % 65536 )) % 65536; 
		p1k[1] = (p1k[1] + tkip_sbox( (p1k[0] ^ ((256*pTKey[5+j]) + pTKey[4+j])) % 65536 )) % 65536; 
		p1k[2] = (p1k[2] + tkip_sbox( (p1k[1] ^ ((256*pTKey[9+j]) + pTKey[8+j])) % 65536 )) % 65536; 
		p1k[3] = (p1k[3] + tkip_sbox( (p1k[2] ^ ((256*pTKey[13+j]) + pTKey[12+j])) % 65536 )) % 65536; 
		p1k[4] = (p1k[4] + tkip_sbox( (p1k[3] ^ (((256*pTKey[1+j]) + pTKey[j]))) % 65536 )) % 65536; 
		p1k[4] = (p1k[4] + i) % 65536; 
	} 

	/* Phase 2, Step 1 */ 
	ppk0 = p1k[0]; 
	ppk1 = p1k[1]; 
	ppk2 = p1k[2]; 
	ppk3 = p1k[3]; 
	ppk4 = p1k[4]; 
	ppk5 = (p1k[4] + tsc2) % 65536; 

	/* Phase2, Step 2 */ 
	ppk0 = ppk0 + tkip_sbox( (ppk5 ^ ((256*pTKey[1]) + pTKey[0])) % 65536); 
	ppk1 = ppk1 + tkip_sbox( (ppk0 ^ ((256*pTKey[3]) + pTKey[2])) % 65536); 
	ppk2 = ppk2 + tkip_sbox( (ppk1 ^ ((256*pTKey[5]) + pTKey[4])) % 65536); 
	ppk3 = ppk3 + tkip_sbox( (ppk2 ^ ((256*pTKey[7]) + pTKey[6])) % 65536); 
	ppk4 = ppk4 + tkip_sbox( (ppk3 ^ ((256*pTKey[9]) + pTKey[8])) % 65536); 
	ppk5 = ppk5 + tkip_sbox( (ppk4 ^ ((256*pTKey[11]) + pTKey[10])) % 65536); 

	ppk0 = ppk0 + rotr1(ppk5 ^ ((256*pTKey[13]) + pTKey[12])); 
	ppk1 = ppk1 + rotr1(ppk0 ^ ((256*pTKey[15]) + pTKey[14])); 
	ppk2 = ppk2 + rotr1(ppk1); 
	ppk3 = ppk3 + rotr1(ppk2); 
	ppk4 = ppk4 + rotr1(ppk3); 
	ppk5 = ppk5 + rotr1(ppk4); 

	/* Phase 2, Step 3 */ 
	pTkip->RC4KEY[0] = (tsc2 >> 8) % 256; 
	pTkip->RC4KEY[1] = (((tsc2 >> 8) % 256) | 0x20) & 0x7f; 
	pTkip->RC4KEY[2] = tsc2 % 256; 
	pTkip->RC4KEY[3] = ((ppk5 ^ ((256*pTKey[1]) + pTKey[0])) >> 1) % 256; 

	pTkip->RC4KEY[4] = ppk0 % 256; 
	pTkip->RC4KEY[5] = (ppk0 >> 8) % 256; 

	pTkip->RC4KEY[6] = ppk1 % 256; 
	pTkip->RC4KEY[7] = (ppk1 >> 8) % 256; 

	pTkip->RC4KEY[8] = ppk2 % 256; 
	pTkip->RC4KEY[9] = (ppk2 >> 8) % 256; 

	pTkip->RC4KEY[10] = ppk3 % 256; 
	pTkip->RC4KEY[11] = (ppk3 >> 8) % 256; 

	pTkip->RC4KEY[12] = ppk4 % 256; 
	pTkip->RC4KEY[13] = (ppk4 >> 8) % 256; 

	pTkip->RC4KEY[14] = ppk5 % 256; 
	pTkip->RC4KEY[15] = (ppk5 >> 8) % 256; 	
} 


VOID RTMPTkipMixKey(
	UCHAR *key, 
	UCHAR *ta, 
	ULONG pnl, /* Least significant 16 bits of PN */
	ULONG pnh, /* Most significant 32 bits of PN */ 
	UCHAR *rc4key, 
	UINT *p1k)
{

	UINT tsc0; 
	UINT tsc1;
	UINT tsc2; 

	UINT ppk0; 
	UINT ppk1; 
	UINT ppk2; 
	UINT ppk3; 
	UINT ppk4; 
	UINT ppk5; 

	INT i; 
	INT j; 

	tsc0 = (unsigned int)((pnh >> 16) % 65536); /* msb */ 
	tsc1 = (unsigned int)(pnh % 65536); 
	tsc2 = (unsigned int)(pnl % 65536); /* lsb */ 

	/* Phase 1, step 1 */ 
	p1k[0] = tsc1; 
	p1k[1] = tsc0; 
	p1k[2] = (UINT)(ta[0] + (ta[1]*256)); 
	p1k[3] = (UINT)(ta[2] + (ta[3]*256)); 
	p1k[4] = (UINT)(ta[4] + (ta[5]*256)); 

	/* Phase 1, step 2 */ 
	for (i=0; i<8; i++) 
	{ 
		j = 2*(i & 1); 
		p1k[0] = (p1k[0] + tkip_sbox( (p1k[4] ^ ((256*key[1+j]) + key[j])) % 65536 )) % 65536; 
		p1k[1] = (p1k[1] + tkip_sbox( (p1k[0] ^ ((256*key[5+j]) + key[4+j])) % 65536 )) % 65536; 
		p1k[2] = (p1k[2] + tkip_sbox( (p1k[1] ^ ((256*key[9+j]) + key[8+j])) % 65536 )) % 65536; 
		p1k[3] = (p1k[3] + tkip_sbox( (p1k[2] ^ ((256*key[13+j]) + key[12+j])) % 65536 )) % 65536; 
		p1k[4] = (p1k[4] + tkip_sbox( (p1k[3] ^ (((256*key[1+j]) + key[j]))) % 65536 )) % 65536; 
		p1k[4] = (p1k[4] + i) % 65536; 
	} 

	/* Phase 2, Step 1 */ 
	ppk0 = p1k[0]; 
	ppk1 = p1k[1]; 
	ppk2 = p1k[2]; 
	ppk3 = p1k[3]; 
	ppk4 = p1k[4]; 
	ppk5 = (p1k[4] + tsc2) % 65536; 

	/* Phase2, Step 2 */ 
	ppk0 = ppk0 + tkip_sbox( (ppk5 ^ ((256*key[1]) + key[0])) % 65536); 
	ppk1 = ppk1 + tkip_sbox( (ppk0 ^ ((256*key[3]) + key[2])) % 65536); 
	ppk2 = ppk2 + tkip_sbox( (ppk1 ^ ((256*key[5]) + key[4])) % 65536); 
	ppk3 = ppk3 + tkip_sbox( (ppk2 ^ ((256*key[7]) + key[6])) % 65536); 
	ppk4 = ppk4 + tkip_sbox( (ppk3 ^ ((256*key[9]) + key[8])) % 65536); 
	ppk5 = ppk5 + tkip_sbox( (ppk4 ^ ((256*key[11]) + key[10])) % 65536); 

	ppk0 = ppk0 + rotr1(ppk5 ^ ((256*key[13]) + key[12])); 
	ppk1 = ppk1 + rotr1(ppk0 ^ ((256*key[15]) + key[14])); 
	ppk2 = ppk2 + rotr1(ppk1); 
	ppk3 = ppk3 + rotr1(ppk2); 
	ppk4 = ppk4 + rotr1(ppk3); 
	ppk5 = ppk5 + rotr1(ppk4); 

	/* Phase 2, Step 3 */ 
    /* Phase 2, Step 3 */

	tsc0 = (unsigned int)((pnh >> 16) % 65536); /* msb */ 
	tsc1 = (unsigned int)(pnh % 65536); 
	tsc2 = (unsigned int)(pnl % 65536); /* lsb */ 

	rc4key[0] = (tsc2 >> 8) % 256; 
	rc4key[1] = (((tsc2 >> 8) % 256) | 0x20) & 0x7f; 
	rc4key[2] = tsc2 % 256; 
	rc4key[3] = ((ppk5 ^ ((256*key[1]) + key[0])) >> 1) % 256; 

	rc4key[4] = ppk0 % 256; 
	rc4key[5] = (ppk0 >> 8) % 256; 

	rc4key[6] = ppk1 % 256; 
	rc4key[7] = (ppk1 >> 8) % 256; 

	rc4key[8] = ppk2 % 256; 
	rc4key[9] = (ppk2 >> 8) % 256; 

	rc4key[10] = ppk3 % 256; 
	rc4key[11] = (ppk3 >> 8) % 256; 

	rc4key[12] = ppk4 % 256; 
	rc4key[13] = (ppk4 >> 8) % 256; 

	rc4key[14] = ppk5 % 256; 
	rc4key[15] = (ppk5 >> 8) % 256; 

}

	
//
// TRUE: Success!
// FALSE: Decrypt Error!
//
BOOLEAN RTMPSoftDecryptTKIP(
	IN PRT2570ADAPTER pAdapter,
	IN PUCHAR	pData,
	IN ULONG	DataByteCnt, 
	IN PWPA_KEY	pWpaKey)
{
	PHEADER_802_11	pHeader;
	UCHAR			KeyID;
	UINT			HeaderLen;
    UCHAR			fc0;
	UCHAR			fc1;
	USHORT			fc;
	UINT			frame_type;
	UINT			frame_subtype;
    UINT			from_ds;
    UINT			to_ds;
	INT				a4_exists;
	INT				qc_exists;
	USHORT			duration;
	USHORT			seq_control;
	USHORT			qos_control;
	UCHAR			TA[MAC_ADDR_LEN];
	UCHAR			DA[MAC_ADDR_LEN];
	UCHAR			SA[MAC_ADDR_LEN];
	UCHAR			RC4Key[16];
	UINT			p1k[5]; //for mix_key;
	ULONG			pnl;/* Least significant 16 bits of PN */
	ULONG			pnh;/* Most significant 32 bits of PN */ 
	UINT			num_blocks;
	UINT			payload_remainder;
	ARCFOURCONTEXT 	ArcFourContext;
	ULONG			crc32 = 0;
	ULONG			trailfcs = 0;
	UCHAR			MIC[8];
	UCHAR			TrailMIC[8];

	fc0 = *pData;
	fc1 = *(pData + 1);

	fc = *((PUSHORT)pData);	
	
	frame_type = ((fc0 >> 2) & 0x03);
	frame_subtype = ((fc0 >> 4) & 0x0f);	

    from_ds = (fc1 & 0x2) >> 1;
    to_ds = (fc1 & 0x1);

    a4_exists = (from_ds & to_ds);
    qc_exists = ((frame_subtype == 0x08) ||    /* Assumed QoS subtypes */
                  (frame_subtype == 0x09) ||   /* Likely to change.    */
                  (frame_subtype == 0x0a) || 
                  (frame_subtype == 0x0b)
                 );

	HeaderLen = 24;
	if (a4_exists)
		HeaderLen += 6;

	KeyID = *((PUCHAR)(pData+ HeaderLen + 3));	
	KeyID = KeyID >> 6;

	if (pWpaKey[KeyID].KeyLen == 0)
	{
		DBGPRINT(RT_DEBUG_TRACE, "RTMPSoftDecryptTKIP failed!(KeyID[%d] Length can not be 0)\n", KeyID);
		return FALSE;
	}
	duration = *((PUSHORT)(pData+2));	

	seq_control = *((PUSHORT)(pData+22));
	
	if (qc_exists)
	{
		if (a4_exists)
		{
			qos_control = *((PUSHORT)(pData+30));
		}
		else
		{
			qos_control = *((PUSHORT)(pData+24));
		}
	}
	
	if (to_ds == 0 && from_ds == 1)
	{
		memcpy(&DA, pData+4, MAC_ADDR_LEN);
		memcpy(&SA, pData+16, MAC_ADDR_LEN);
		memcpy(&TA, pData+10, MAC_ADDR_LEN);  //BSSID
	}	
	else if (to_ds == 0 && from_ds == 0 )
	{
		memcpy(&TA, pData+10, MAC_ADDR_LEN);
		memcpy(&DA, pData+4, MAC_ADDR_LEN);
		memcpy(&SA, pData+10, MAC_ADDR_LEN);
	}
	else if (to_ds == 1 && from_ds == 0)
	{
		memcpy(&SA, pData+10, MAC_ADDR_LEN);
		memcpy(&TA, pData+10, MAC_ADDR_LEN);
		memcpy(&DA, pData+16, MAC_ADDR_LEN);
	}
	else if (to_ds == 1 && from_ds == 1)
	{
		memcpy(&TA, pData+10, MAC_ADDR_LEN);
		memcpy(&DA, pData+16, MAC_ADDR_LEN);
		memcpy(&SA, pData+22, MAC_ADDR_LEN);
	}

	num_blocks = (DataByteCnt - 16) / 16;
	payload_remainder = (DataByteCnt - 16) % 16;

	pnl = (*(pData + HeaderLen)) * 256 + *(pData + HeaderLen + 2);	
	pnh = *((PULONG)(pData + HeaderLen + 4));
	RTMPTkipMixKey(pWpaKey[KeyID].Key, TA, pnl, pnh, RC4Key, p1k);

	ARCFOUR_INIT(&ArcFourContext, RC4Key, 16); 
	ARCFOUR_DECRYPT(&ArcFourContext, pData + HeaderLen, pData + HeaderLen + 8, DataByteCnt - HeaderLen - 8);
	memcpy(&trailfcs, pData + DataByteCnt - 8 - 4, 4);
	crc32 = RTMP_CALC_FCS32(PPPINITFCS32, pData + HeaderLen, DataByteCnt - HeaderLen - 8 - 4);  //Skip IV+EIV 8 bytes & Skip last 4 bytes(FCS).
	crc32 ^= 0xffffffff;             /* complement */
	if(crc32 != trailfcs)
	{
		DBGPRINT_RAW(RT_DEBUG_TRACE, "RTMPSoftDecryptTKIP, WEP Data ICV Error !\n");	 //ICV error.

		return (FALSE);
	}

	memcpy(TrailMIC, pData + DataByteCnt - 8 - 8 - 4, 8);
	RTMPInitMICEngine(pAdapter, pWpaKey[KeyID].Key, DA, SA, pWpaKey[KeyID].RxMic);
	RTMPTkipAppend(&pAdapter->PrivateInfo.Tx, pData + HeaderLen, DataByteCnt - HeaderLen - 8 - 12);
	RTMPTkipGetMIC(&pAdapter->PrivateInfo.Tx);
	memcpy(MIC, pAdapter->PrivateInfo.Tx.MIC, 8);

	if (memcmp(MIC, TrailMIC, 8) != 0)
	{
		DBGPRINT_RAW(RT_DEBUG_TRACE, "RTMPSoftDecryptTKIP, WEP Data MIC Error !\n");	 //MIC error.
		RTMPReportMicError(pAdapter, &pWpaKey[KeyID]);
		return (FALSE);		
	}

	pHeader	= (PHEADER_802_11)pData;
	pHeader->Controlhead.Frame.Wep = 0; //None WEP
	
	DBGPRINT(RT_DEBUG_TRACE, "RTMPSoftDecryptTKIP Decript done!!\n");
	return TRUE;
}