rtmp_tkip.c

Ralink RT61 SoftAP Driver source code. RT61:MiniPCI

}

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

	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
		
	Note:
	
	========================================================================
*/
BOOLEAN	RTMPTkipCompareMICValueWithLLC(
	IN	PRTMP_ADAPTER	pAdapter,
	IN	PUCHAR			pLLC,
	IN	PUCHAR			pSrc,
	IN	PUCHAR			pDA,
	IN	PUCHAR			pSA,
	IN	PUCHAR			pMICKey,
	IN	UINT			Len)
{
	UCHAR	OldMic[8];
	ULONG	Priority = 0;
	INT		i;

	// Init MIC value calculation
	RTMPTkipSetMICKey(&pAdapter->PrivateInfo.Rx, pMICKey);
	// DA
	RTMPTkipAppend(&pAdapter->PrivateInfo.Rx, pDA, MAC_ADDR_LEN);
	// SA
	RTMPTkipAppend(&pAdapter->PrivateInfo.Rx, pSA, MAC_ADDR_LEN);
	// Priority + 3 bytes of 0
	RTMPTkipAppend(&pAdapter->PrivateInfo.Rx, (PUCHAR)&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
	NdisMoveMemory(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(!NdisEqualMemory(pAdapter->PrivateInfo.Rx.MIC, OldMic, 8))
	{
		DBGPRINT(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(RT_DEBUG_INFO, "%02x:", OldMic[i]);  //MIC error.
		}
		DBGPRINT(RT_DEBUG_INFO, "\n");  //MIC error.
		DBGPRINT(RT_DEBUG_INFO, "Calculated MIC value =");  //MIC error.
		for (i = 0; i < 8; i++)
		{
			DBGPRINT(RT_DEBUG_INFO, "%02x:", pAdapter->PrivateInfo.Rx.MIC[i]);  //MIC error.
		}
		DBGPRINT(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

	Note:
	
	========================================================================
*/
VOID	RTMPCalculateMICValue(
	IN	PRTMP_ADAPTER	pAdapter,
	IN	struct sk_buff  *pSkb,
	IN	PUCHAR			pEncap,
	IN	PCIPHER_KEY		pKey)
{
	PVOID			pVirtualAddress;
	UINT			Length;
	PUCHAR			pSrc;
    UCHAR           UserPriority;

	pVirtualAddress = pSkb->data;
	Length = pSkb->len;

    UserPriority = RTMP_GET_PACKET_UP(pSkb);
	pSrc = (PUCHAR) pVirtualAddress;
    
	// Start Calculate MIC Value
	RTMPInitMICEngine(
		pAdapter,
		pKey->Key,
		pSrc,
		pSrc + 6,
		UserPriority,
		pKey->TxMic);

	if (pEncap != NULL)
	{
		// LLC encapsulation
		RTMPTkipAppend(&pAdapter->PrivateInfo.Tx, pEncap, 6);
		// Protocol Type
		RTMPTkipAppend(&pAdapter->PrivateInfo.Tx, pSrc + 12, 2);
	}
	Length -= 14;
	pSrc += 14;
	do
	{
		if (Length > 0)
		{
			RTMPTkipAppend(&pAdapter->PrivateInfo.Tx, pSrc, Length);
		}
	}	while (FALSE);		// End of copying payload

	// 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) 
	{ 
		b = (a >> 1) | 0x8000; 
	} 
	else 
	{ 
		b = (a >> 1) & 0x7fff; 
	} 
	b = b % 65536; 
	return b; 
} 

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; 
}


/************************************************/
/* construct_mic_header1()                      */
/* Builds the first MIC header block from       */
/* header fields.                               */
/************************************************/

void construct_mic_header1(
	unsigned char *mic_header1,
	int header_length,
	unsigned char *mpdu)
{
	mic_header1[0] = (unsigned char)((header_length - 2) / 256);
	mic_header1[1] = (unsigned char)((header_length - 2) % 256);
	mic_header1[2] = mpdu[0] & 0xcf;    /* Mute CF poll & CF ack bits */
	mic_header1[3] = mpdu[1] & 0xc7;    /* Mute retry, more data and pwr mgt bits */
	mic_header1[4] = mpdu[4];       /* A1 */
	mic_header1[5] = mpdu[5];
	mic_header1[6] = mpdu[6];
	mic_header1[7] = mpdu[7];
	mic_header1[8] = mpdu[8];
	mic_header1[9] = mpdu[9];
	mic_header1[10] = mpdu[10];     /* A2 */
	mic_header1[11] = mpdu[11];
	mic_header1[12] = mpdu[12];
	mic_header1[13] = mpdu[13];
	mic_header1[14] = mpdu[14];
	mic_header1[15] = mpdu[15];
}

/************************************************/
/* construct_mic_header2()                      */
/* Builds the last MIC header block from        */
/* header fields.                               */
/************************************************/

void construct_mic_header2(
	unsigned char *mic_header2,
	unsigned char *mpdu,
	int a4_exists,
	int qc_exists)
{
	int i;

	for (i = 0; i<16; i++) mic_header2[i]=0x00;

	mic_header2[0] = mpdu[16];    /* A3 */
	mic_header2[1] = mpdu[17];
	mic_header2[2] = mpdu[18];
	mic_header2[3] = mpdu[19];
	mic_header2[4] = mpdu[20];
	mic_header2[5] = mpdu[21];

	// In Sequence Control field, mute sequence numer bits (12-bit) 
	mic_header2[6] = mpdu[22] & 0x0f;   /* SC */
	mic_header2[7] = 0x00; /* mpdu[23]; */

	if ((!qc_exists) & a4_exists)
	{
		for (i=0;i<6;i++) mic_header2[8+i] = mpdu[24+i];   /* A4 */

	}

	if (qc_exists && (!a4_exists))
	{
		mic_header2[8] = mpdu[24] & 0x0f; /* mute bits 15 - 4 */
		mic_header2[9] = mpdu[25] & 0x00;
	}

	if (qc_exists && a4_exists)
	{
		for (i=0;i<6;i++) mic_header2[8+i] = mpdu[24+i];   /* A4 */

		mic_header2[14] = mpdu[30] & 0x0f;
		mic_header2[15] = mpdu[31] & 0x00;
	}
}


/************************************************/
/* construct_mic_iv()                           */
/* Builds the MIC IV from header fields and PN  */
/************************************************/

void construct_mic_iv(
	unsigned char *mic_iv,
	int qc_exists,
	int a4_exists,
	unsigned char *mpdu,
	unsigned int payload_length,
	unsigned char *pn_vector)
{
	int i;

	mic_iv[0] = 0x59;
	if (qc_exists && a4_exists) 
		mic_iv[1] = mpdu[30] & 0x0f;    /* QoS_TC           */
	if (qc_exists && !a4_exists) 
		mic_iv[1] = mpdu[24] & 0x0f;   /* mute bits 7-4    */
	if (!qc_exists) 
		mic_iv[1] = 0x00;
	for (i = 2; i < 8; i++)
		mic_iv[i] = mpdu[i + 8];                    /* mic_iv[2:7] = A2[0:5] = mpdu[10:15] */
#ifdef CONSISTENT_PN_ORDER
		for (i = 8; i < 14; i++)
			mic_iv[i] = pn_vector[i - 8];           /* mic_iv[8:13] = PN[0:5] */
#else
		for (i = 8; i < 14; i++)
			mic_iv[i] = pn_vector[13 - i];          /* mic_iv[8:13] = PN[5:0] */
#endif
	i = (payload_length / 256);
	i = (payload_length % 256);
	mic_iv[14] = (unsigned char) (payload_length / 256);
	mic_iv[15] = (unsigned char) (payload_length % 256);

}



/************************************/
/* bitwise_xor()                    */
/* A 128 bit, bitwise exclusive or  */
/************************************/

void bitwise_xor(unsigned char *ina, unsigned char *inb, unsigned char *out)
{
	int i;
	for (i=0; i<16; i++)
	{
		out[i] = ina[i] ^ inb[i];
	}
}


void aes128k128d(unsigned char *key, unsigned char *data, unsigned char *ciphertext)
{
	int round;
	int i;
	unsigned char intermediatea[16];
	unsigned char intermediateb[16];
	unsigned char round_key[16];

	for(i=0; i<16; i++) round_key[i] = key[i];

	for (round = 0; round < 11; round++)
	{
		if (round == 0)
		{
			xor_128(round_key, data, ciphertext);
			next_key(round_key, round);         
		}
		else if (round == 10)
		{
			byte_sub(ciphertext, intermediatea);
			shift_row(intermediatea, intermediateb);
			xor_128(intermediateb, round_key, ciphertext);
		}
		else    /* 1 - 9 */
		{
			byte_sub(ciphertext, intermediatea);
			shift_row(intermediatea, intermediateb);
			mix_column(&intermediateb[0], &intermediatea[0]);
			mix_column(&intermediateb[4], &intermediatea[4]);
			mix_column(&intermediateb[8], &intermediatea[8]);
			mix_column(&intermediateb[12], &intermediatea[12]);
			xor_128(intermediatea, round_key, ciphertext);
			next_key(round_key, round);
		}
	}

}

void construct_ctr_preload(
	unsigned char *ctr_preload,
	int a4_exists,
	int qc_exists,
	unsigned char *mpdu,
	unsigned char *pn_vector,
	int c)
{

	int i = 0;
	for (i=0; i<16; i++) ctr_preload[i] = 0x00;
	i = 0;

	ctr_preload[0] = 0x01;                                  /* flag */
	if (qc_exists && a4_exists) ctr_preload[1] = mpdu[30] & 0x0f;   /* QoC_Control  */
	if (qc_exists && !a4_exists) ctr_preload[1] = mpdu[24] & 0x0f;

	for (i = 2; i < 8; i++)
		ctr_preload[i] = mpdu[i + 8];                       /* ctr_preload[2:7] = A2[0:5] = mpdu[10:15] */
#ifdef CONSISTENT_PN_ORDER
	  for (i = 8; i < 14; i++)
			ctr_preload[i] =    pn_vector[i - 8];           /* ctr_preload[8:13] = PN[0:5] */
#else
	  for (i = 8; i < 14; i++)
			ctr_preload[i] =    pn_vector[13 - i];          /* ctr_preload[8:13] = PN[5:0] */
#endif
	ctr_preload[14] =  (unsigned char) (c / 256); // Ctr 
	ctr_preload[15] =  (unsigned char) (c % 256);

}


//
// TRUE: Success!
// FALSE: Decrypt Error!
//
BOOLEAN RTMPSoftDecryptTKIP(
	IN PRTMP_ADAPTER pAd,
	IN PUCHAR	pData,
	IN ULONG	DataByteCnt, 
	IN UCHAR    UserPriority,
	IN PCIPHER_KEY	pWpaKey)
{
	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];

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

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