rtmp_tkip.c

RT73无线网络芯片最新linux下驱动 支持到linux2.6.24 并向下兼容

#endif

	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)
	{
		NdisMoveMemory(DA, pData+4, MAC_ADDR_LEN);
		NdisMoveMemory(SA, pData+16, MAC_ADDR_LEN);
		NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);  //BSSID
	}	
	else if (to_ds == 0 && from_ds == 0 )
	{
		NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
		NdisMoveMemory(DA, pData+4, MAC_ADDR_LEN);
		NdisMoveMemory(SA, pData+10, MAC_ADDR_LEN);
	}
	else if (to_ds == 1 && from_ds == 0)
	{
		NdisMoveMemory(SA, pData+10, MAC_ADDR_LEN);
		NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
		NdisMoveMemory(DA, pData+16, MAC_ADDR_LEN);
	}
	else if (to_ds == 1 && from_ds == 1)
	{
		NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
		NdisMoveMemory(DA, pData+16, MAC_ADDR_LEN);
		NdisMoveMemory(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);
	NdisMoveMemory(&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 */

#ifndef BIG_ENDIAN
	if(crc32 != trailfcs)
#else
    if(crc32 != SWAP32(trailfcs))
#endif
	{
		DBGPRINT(RT_DEBUG_TRACE, "RTMPSoftDecryptTKIP, WEP Data ICV Error !\n");	 //ICV error.

		return (FALSE);
	}

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

	if (!NdisEqualMemory(MIC, TrailMIC, 8))
	{
		DBGPRINT(RT_DEBUG_ERROR, "RTMPSoftDecryptTKIP, WEP Data MIC Error !\n");	 //MIC error.
		//RTMPReportMicError(pAd, &pWpaKey[KeyID]);	// marked by AlbertY @ 20060630 
		return (FALSE);		
	}

#ifdef BIG_ENDIAN
	RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
#endif
	//DBGPRINT(RT_DEBUG_TRACE, "RTMPSoftDecryptTKIP Decript done!!\n");
	return TRUE;
}




BOOLEAN RTMPSoftDecryptAES(
	IN PRTMP_ADAPTER pAd,
	IN PUCHAR	pData,
	IN ULONG	DataByteCnt, 
	IN PCIPHER_KEY	pWpaKey)
{
	UCHAR			KeyID;
	UINT			HeaderLen;
	UCHAR			PN[6];
	UINT			payload_len;	
	UINT			num_blocks;
	UINT			payload_remainder;
	USHORT			fc;
	UCHAR			fc0;
	UCHAR			fc1;	
	UINT			frame_type;
	UINT			frame_subtype;
	UINT			from_ds;
	UINT			to_ds;
	INT				a4_exists;
	INT				qc_exists;
	UCHAR			aes_out[16];
	int 			payload_index;
	UINT 			i;
	UCHAR 			ctr_preload[16];
	UCHAR 			chain_buffer[16];
	UCHAR 			padded_buffer[16];
	UCHAR 			mic_iv[16];
	UCHAR 			mic_header1[16];
	UCHAR 			mic_header2[16];	
	UCHAR			MIC[8];
	UCHAR			TrailMIC[8];

#ifdef BIG_ENDIAN
	RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
#endif

	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, "RTMPSoftDecryptAES failed!(KeyID[%d] Length can not be 0)\n", KeyID);
		return FALSE;
	}

	PN[0] = *(pData+ HeaderLen);
	PN[1] = *(pData+ HeaderLen + 1);
	PN[2] = *(pData+ HeaderLen + 4);
	PN[3] = *(pData+ HeaderLen + 5);
	PN[4] = *(pData+ HeaderLen + 6);
	PN[5] = *(pData+ HeaderLen + 7);

	payload_len = DataByteCnt - HeaderLen - 8 - 8;	// 8 bytes for CCMP header , 8 bytes for MIC
	payload_remainder = (payload_len) % 16;
	num_blocks = (payload_len) / 16; 
	
	DBGPRINT(RT_DEBUG_INFO, "SoftDecryptAES: payload = %d, num_blocks = %d, payload_remainder = %d\n", payload_len, num_blocks, payload_remainder);
	
	// Find start of payload
	payload_index = HeaderLen + 8; //IV+EIV

	for (i=0; i< num_blocks; i++)	
	{
		construct_ctr_preload(ctr_preload,
								a4_exists,
								qc_exists,
								pData,
								PN,
								i+1 );

		aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);

		bitwise_xor(aes_out, pData + payload_index, chain_buffer);
#if 0
		{
			int j;
			printk("chain_buffer[%d]: ", i+1);
			for (j = 0; j < 16; j++)
			{
				printk("%02x ", chain_buffer[j]);
			}
			printk("\n");	
		}
#endif
		NdisMoveMemory(pData + payload_index - 8, chain_buffer, 16);
		payload_index += 16;
	}

	//
	// If there is a short final block, then pad it
	// encrypt it and copy the unpadded part back 
	//
	if (payload_remainder > 0)
	{
		construct_ctr_preload(ctr_preload,
								a4_exists,
								qc_exists,
								pData,
								PN,
								num_blocks + 1);

		NdisZeroMemory(padded_buffer, 16);
		NdisMoveMemory(padded_buffer, pData + payload_index, payload_remainder);

		aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);

		bitwise_xor(aes_out, padded_buffer, chain_buffer);
#if 0
		{
			int j;
			printk("chain_buffer[%d]: ", num_blocks + 1);
			for (j = 0; j < payload_remainder; j++)
			{
				printk("%02x ", chain_buffer[j]);
			}
			printk("\n");	
		}
#endif
		NdisMoveMemory(pData + payload_index - 8, chain_buffer, payload_remainder);
		payload_index += payload_remainder;
	}

	//
	// Descrypt the MIC
	// 
	construct_ctr_preload(ctr_preload,
							a4_exists,
							qc_exists,
							pData,
							PN,
							0);
	NdisZeroMemory(padded_buffer, 16);
	NdisMoveMemory(padded_buffer, pData + payload_index, 8); 
	
	aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);

	bitwise_xor(aes_out, padded_buffer, chain_buffer);	

	NdisMoveMemory(TrailMIC, chain_buffer, 8);
	
#if 0
	{
		printk("chain_buffer: ");
		for (i = 0; i < 16; i++)
		{
			printk("%02x ", chain_buffer[i]);
		}
		printk("\n");
	}	
#endif
	
	//
	// Calculate MIC
	//

	//Force the protected frame bit on
	*(pData + 1) = *(pData + 1) | 0x40;

	// Find start of payload
	// Because the CCMP header has been removed
	payload_index = HeaderLen;

	construct_mic_iv(
					mic_iv,
					qc_exists,
					a4_exists,
					pData,
					payload_len,
					PN);

	construct_mic_header1(
						mic_header1,
						HeaderLen,
						pData);

	construct_mic_header2(
						mic_header2,
						pData,
						a4_exists,
						qc_exists);

	aes128k128d(pWpaKey[KeyID].Key, mic_iv, aes_out);
	bitwise_xor(aes_out, mic_header1, chain_buffer);
	aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
	bitwise_xor(aes_out, mic_header2, chain_buffer);
	aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);

	// iterate through each 16 byte payload block
	for (i = 0; i < num_blocks; i++)     
	{
		bitwise_xor(aes_out, pData + payload_index, chain_buffer);
		payload_index += 16;
		aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
	}

	// Add on the final payload block if it needs padding
	if (payload_remainder > 0)
	{
		NdisZeroMemory(padded_buffer, 16);
		NdisMoveMemory(padded_buffer, pData + payload_index, payload_remainder);

		bitwise_xor(aes_out, padded_buffer, chain_buffer);
		aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);		
	}

#if 0
	{
		printk("aes_out:      ");
		for (i = 0; i < 16; i++)
		{
			printk("%02x ", aes_out[i]);
		}
		printk("\n");
	}	
#endif
	// aes_out contains padded mic, discard most significant
	// 8 bytes to generate 64 bit MIC
	for (i = 0 ; i < 8; i++) MIC[i] = aes_out[i];

	if (!NdisEqualMemory(MIC, TrailMIC, 8))
	{
		DBGPRINT(RT_DEBUG_ERROR, "RTMPSoftDecryptAES, MIC Error !\n");	 //MIC error.	
		return FALSE;
	}

#ifdef BIG_ENDIAN
	RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
#endif

	return TRUE;
}

/****************************************/
/* aes128k128d()                        */
/* Performs a 128 bit AES encrypt with  */
/* 128 bit data.                        */
/****************************************/
VOID xor_128(
	IN  PUCHAR  a,
	IN  PUCHAR  b,
	OUT PUCHAR  out)
{
	INT i;

	for (i=0;i<16; i++)
	{
		out[i] = a[i] ^ b[i];
	}
}

VOID next_key(
	IN  PUCHAR  key,
	IN  INT     round)
{
	UCHAR       rcon;
	UCHAR       sbox_key[4];
	UCHAR       rcon_table[12] =
	{
		0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80,
		0x1b, 0x36, 0x36, 0x36
	};

	sbox_key[0] = RTMPCkipSbox(key[13]);
	sbox_key[1] = RTMPCkipSbox(key[14]);
	sbox_key[2] = RTMPCkipSbox(key[15]);
	sbox_key[3] = RTMPCkipSbox(key[12]);

	rcon = rcon_table[round];

	xor_32(&key[0], sbox_key, &key[0]);
	key[0] = key[0] ^ rcon;

	xor_32(&key[4], &key[0], &key[4]);
	xor_32(&key[8], &key[4], &key[8]);
	xor_32(&key[12], &key[8], &key[12]);
}

VOID xor_32(
	IN  PUCHAR  a,
	IN  PUCHAR  b,
	OUT PUCHAR  out)
{
	INT i;

	for (i=0;i<4; i++)
	{
		out[i] = a[i] ^ b[i];
	}
}

VOID byte_sub(
	IN  PUCHAR  in,
	OUT PUCHAR  out)
{
	INT i;

	for (i=0; i< 16; i++)
	{
		out[i] = RTMPCkipSbox(in[i]);
	}
}

UCHAR RTMPCkipSbox(
	IN  UCHAR   a)
{
	return SboxTable[(int)a];
}

VOID shift_row(
	IN  PUCHAR  in,
	OUT PUCHAR  out)
{
	out[0] =  in[0];
	out[1] =  in[5];
	out[2] =  in[10];
	out[3] =  in[15];
	out[4] =  in[4];
	out[5] =  in[9];
	out[6] =  in[14];
	out[7] =  in[3];
	out[8] =  in[8];
	out[9] =  in[13];
	out[10] = in[2];
	out[11] = in[7];
	out[12] = in[12];
	out[13] = in[1];
	out[14] = in[6];
	out[15] = in[11];
}

VOID mix_column(
	IN  PUCHAR  in,
	OUT PUCHAR  out)
{
	INT         i;
	UCHAR       add1b[4];
	UCHAR       add1bf7[4];
	UCHAR       rotl[4];
	UCHAR       swap_halfs[4];
	UCHAR       andf7[4];
	UCHAR       rotr[4];
	UCHAR       temp[4];
	UCHAR       tempb[4];

	for (i=0 ; i<4; i++)
	{
		if ((in[i] & 0x80)== 0x80)
			add1b[i] = 0x1b;
		else
			add1b[i] = 0x00;
	}

	swap_halfs[0] = in[2];    /* Swap halfs */
	swap_halfs[1] = in[3];
	swap_halfs[2] = in[0];
	swap_halfs[3] = in[1];

	rotl[0] = in[3];        /* Rotate left 8 bits */
	rotl[1] = in[0];
	rotl[2] = in[1];
	rotl[3] = in[2];

	andf7[0] = in[0] & 0x7f;
	andf7[1] = in[1] & 0x7f;
	andf7[2] = in[2] & 0x7f;
	andf7[3] = in[3] & 0x7f;

	for (i = 3; i>0; i--)    /* logical shift left 1 bit */
	{
		andf7[i] = andf7[i] << 1;
		if ((andf7[i-1] & 0x80) == 0x80)
		{
			andf7[i] = (andf7[i] | 0x01);
		}
	}
	andf7[0] = andf7[0] << 1;
	andf7[0] = andf7[0] & 0xfe;

	xor_32(add1b, andf7, add1bf7);

	xor_32(in, add1bf7, rotr);

	temp[0] = rotr[0];         /* Rotate right 8 bits */
	rotr[0] = rotr[1];
	rotr[1] = rotr[2];
	rotr[2] = rotr[3];
	rotr[3] = temp[0];

	xor_32(add1bf7, rotr, temp);
	xor_32(swap_halfs, rotl,tempb);
	xor_32(temp, tempb, out);
}