967 lines
24 KiB
C
967 lines
24 KiB
C
|
/*
|
||
|
*************************************************************************
|
||
|
* Ralink Tech Inc.
|
||
|
* 5F., No.36, Taiyuan St., Jhubei City,
|
||
|
* Hsinchu County 302,
|
||
|
* Taiwan, R.O.C.
|
||
|
*
|
||
|
* (c) Copyright 2002-2007, Ralink Technology, Inc.
|
||
|
*
|
||
|
* This program is free software; you can redistribute it and/or modify *
|
||
|
* it under the terms of the GNU General Public License as published by *
|
||
|
* the Free Software Foundation; either version 2 of the License, or *
|
||
|
* (at your option) any later version. *
|
||
|
* *
|
||
|
* This program is distributed in the hope that it will be useful, *
|
||
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
|
||
|
* GNU General Public License for more details. *
|
||
|
* *
|
||
|
* You should have received a copy of the GNU General Public License *
|
||
|
* along with this program; if not, write to the *
|
||
|
* Free Software Foundation, Inc., *
|
||
|
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
|
||
|
* *
|
||
|
*************************************************************************
|
||
|
|
||
|
Module Name:
|
||
|
cmm_tkip.c
|
||
|
|
||
|
Abstract:
|
||
|
|
||
|
Revision History:
|
||
|
Who When What
|
||
|
-------- ---------- ----------------------------------------------
|
||
|
Paul Wu 02-25-02 Initial
|
||
|
*/
|
||
|
|
||
|
#include "../rt_config.h"
|
||
|
|
||
|
|
||
|
// Rotation functions on 32 bit values
|
||
|
#define ROL32( A, n ) \
|
||
|
( ((A) << (n)) | ( ((A)>>(32-(n))) & ( (1UL << (n)) - 1 ) ) )
|
||
|
#define ROR32( A, n ) ROL32( (A), 32-(n) )
|
||
|
|
||
|
UINT Tkip_Sbox_Lower[256] =
|
||
|
{
|
||
|
0xA5,0x84,0x99,0x8D,0x0D,0xBD,0xB1,0x54,
|
||
|
0x50,0x03,0xA9,0x7D,0x19,0x62,0xE6,0x9A,
|
||
|
0x45,0x9D,0x40,0x87,0x15,0xEB,0xC9,0x0B,
|
||
|
0xEC,0x67,0xFD,0xEA,0xBF,0xF7,0x96,0x5B,
|
||
|
0xC2,0x1C,0xAE,0x6A,0x5A,0x41,0x02,0x4F,
|
||
|
0x5C,0xF4,0x34,0x08,0x93,0x73,0x53,0x3F,
|
||
|
0x0C,0x52,0x65,0x5E,0x28,0xA1,0x0F,0xB5,
|
||
|
0x09,0x36,0x9B,0x3D,0x26,0x69,0xCD,0x9F,
|
||
|
0x1B,0x9E,0x74,0x2E,0x2D,0xB2,0xEE,0xFB,
|
||
|
0xF6,0x4D,0x61,0xCE,0x7B,0x3E,0x71,0x97,
|
||
|
0xF5,0x68,0x00,0x2C,0x60,0x1F,0xC8,0xED,
|
||
|
0xBE,0x46,0xD9,0x4B,0xDE,0xD4,0xE8,0x4A,
|
||
|
0x6B,0x2A,0xE5,0x16,0xC5,0xD7,0x55,0x94,
|
||
|
0xCF,0x10,0x06,0x81,0xF0,0x44,0xBA,0xE3,
|
||
|
0xF3,0xFE,0xC0,0x8A,0xAD,0xBC,0x48,0x04,
|
||
|
0xDF,0xC1,0x75,0x63,0x30,0x1A,0x0E,0x6D,
|
||
|
0x4C,0x14,0x35,0x2F,0xE1,0xA2,0xCC,0x39,
|
||
|
0x57,0xF2,0x82,0x47,0xAC,0xE7,0x2B,0x95,
|
||
|
0xA0,0x98,0xD1,0x7F,0x66,0x7E,0xAB,0x83,
|
||
|
0xCA,0x29,0xD3,0x3C,0x79,0xE2,0x1D,0x76,
|
||
|
0x3B,0x56,0x4E,0x1E,0xDB,0x0A,0x6C,0xE4,
|
||
|
0x5D,0x6E,0xEF,0xA6,0xA8,0xA4,0x37,0x8B,
|
||
|
0x32,0x43,0x59,0xB7,0x8C,0x64,0xD2,0xE0,
|
||
|
0xB4,0xFA,0x07,0x25,0xAF,0x8E,0xE9,0x18,
|
||
|
0xD5,0x88,0x6F,0x72,0x24,0xF1,0xC7,0x51,
|
||
|
0x23,0x7C,0x9C,0x21,0xDD,0xDC,0x86,0x85,
|
||
|
0x90,0x42,0xC4,0xAA,0xD8,0x05,0x01,0x12,
|
||
|
0xA3,0x5F,0xF9,0xD0,0x91,0x58,0x27,0xB9,
|
||
|
0x38,0x13,0xB3,0x33,0xBB,0x70,0x89,0xA7,
|
||
|
0xB6,0x22,0x92,0x20,0x49,0xFF,0x78,0x7A,
|
||
|
0x8F,0xF8,0x80,0x17,0xDA,0x31,0xC6,0xB8,
|
||
|
0xC3,0xB0,0x77,0x11,0xCB,0xFC,0xD6,0x3A
|
||
|
};
|
||
|
|
||
|
UINT Tkip_Sbox_Upper[256] =
|
||
|
{
|
||
|
0xC6,0xF8,0xEE,0xF6,0xFF,0xD6,0xDE,0x91,
|
||
|
0x60,0x02,0xCE,0x56,0xE7,0xB5,0x4D,0xEC,
|
||
|
0x8F,0x1F,0x89,0xFA,0xEF,0xB2,0x8E,0xFB,
|
||
|
0x41,0xB3,0x5F,0x45,0x23,0x53,0xE4,0x9B,
|
||
|
0x75,0xE1,0x3D,0x4C,0x6C,0x7E,0xF5,0x83,
|
||
|
0x68,0x51,0xD1,0xF9,0xE2,0xAB,0x62,0x2A,
|
||
|
0x08,0x95,0x46,0x9D,0x30,0x37,0x0A,0x2F,
|
||
|
0x0E,0x24,0x1B,0xDF,0xCD,0x4E,0x7F,0xEA,
|
||
|
0x12,0x1D,0x58,0x34,0x36,0xDC,0xB4,0x5B,
|
||
|
0xA4,0x76,0xB7,0x7D,0x52,0xDD,0x5E,0x13,
|
||
|
0xA6,0xB9,0x00,0xC1,0x40,0xE3,0x79,0xB6,
|
||
|
0xD4,0x8D,0x67,0x72,0x94,0x98,0xB0,0x85,
|
||
|
0xBB,0xC5,0x4F,0xED,0x86,0x9A,0x66,0x11,
|
||
|
0x8A,0xE9,0x04,0xFE,0xA0,0x78,0x25,0x4B,
|
||
|
0xA2,0x5D,0x80,0x05,0x3F,0x21,0x70,0xF1,
|
||
|
0x63,0x77,0xAF,0x42,0x20,0xE5,0xFD,0xBF,
|
||
|
0x81,0x18,0x26,0xC3,0xBE,0x35,0x88,0x2E,
|
||
|
0x93,0x55,0xFC,0x7A,0xC8,0xBA,0x32,0xE6,
|
||
|
0xC0,0x19,0x9E,0xA3,0x44,0x54,0x3B,0x0B,
|
||
|
0x8C,0xC7,0x6B,0x28,0xA7,0xBC,0x16,0xAD,
|
||
|
0xDB,0x64,0x74,0x14,0x92,0x0C,0x48,0xB8,
|
||
|
0x9F,0xBD,0x43,0xC4,0x39,0x31,0xD3,0xF2,
|
||
|
0xD5,0x8B,0x6E,0xDA,0x01,0xB1,0x9C,0x49,
|
||
|
0xD8,0xAC,0xF3,0xCF,0xCA,0xF4,0x47,0x10,
|
||
|
0x6F,0xF0,0x4A,0x5C,0x38,0x57,0x73,0x97,
|
||
|
0xCB,0xA1,0xE8,0x3E,0x96,0x61,0x0D,0x0F,
|
||
|
0xE0,0x7C,0x71,0xCC,0x90,0x06,0xF7,0x1C,
|
||
|
0xC2,0x6A,0xAE,0x69,0x17,0x99,0x3A,0x27,
|
||
|
0xD9,0xEB,0x2B,0x22,0xD2,0xA9,0x07,0x33,
|
||
|
0x2D,0x3C,0x15,0xC9,0x87,0xAA,0x50,0xA5,
|
||
|
0x03,0x59,0x09,0x1A,0x65,0xD7,0x84,0xD0,
|
||
|
0x82,0x29,0x5A,0x1E,0x7B,0xA8,0x6D,0x2C
|
||
|
};
|
||
|
|
||
|
//
|
||
|
// Expanded IV for TKIP function.
|
||
|
//
|
||
|
typedef struct PACKED _IV_CONTROL_
|
||
|
{
|
||
|
union PACKED
|
||
|
{
|
||
|
struct PACKED
|
||
|
{
|
||
|
UCHAR rc0;
|
||
|
UCHAR rc1;
|
||
|
UCHAR rc2;
|
||
|
|
||
|
union PACKED
|
||
|
{
|
||
|
struct PACKED
|
||
|
{
|
||
|
#ifdef RT_BIG_ENDIAN
|
||
|
UCHAR KeyID:2;
|
||
|
UCHAR ExtIV:1;
|
||
|
UCHAR Rsvd:5;
|
||
|
#else
|
||
|
UCHAR Rsvd:5;
|
||
|
UCHAR ExtIV:1;
|
||
|
UCHAR KeyID:2;
|
||
|
#endif
|
||
|
} field;
|
||
|
UCHAR Byte;
|
||
|
} CONTROL;
|
||
|
} field;
|
||
|
|
||
|
ULONG word;
|
||
|
} IV16;
|
||
|
|
||
|
ULONG IV32;
|
||
|
} TKIP_IV, *PTKIP_IV;
|
||
|
|
||
|
|
||
|
/*
|
||
|
========================================================================
|
||
|
|
||
|
Routine Description:
|
||
|
Convert from UCHAR[] to ULONG in a portable way
|
||
|
|
||
|
Arguments:
|
||
|
pMICKey pointer to MIC Key
|
||
|
|
||
|
Return Value:
|
||
|
None
|
||
|
|
||
|
Note:
|
||
|
|
||
|
========================================================================
|
||
|
*/
|
||
|
ULONG RTMPTkipGetUInt32(
|
||
|
IN PUCHAR pMICKey)
|
||
|
{
|
||
|
ULONG res = 0;
|
||
|
INT i;
|
||
|
|
||
|
for (i = 0; i < 4; i++)
|
||
|
{
|
||
|
res |= (*pMICKey++) << (8 * i);
|
||
|
}
|
||
|
|
||
|
return res;
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
========================================================================
|
||
|
|
||
|
Routine Description:
|
||
|
Convert from ULONG to UCHAR[] in a portable way
|
||
|
|
||
|
Arguments:
|
||
|
pDst pointer to destination for convert ULONG to UCHAR[]
|
||
|
val the value for convert
|
||
|
|
||
|
Return Value:
|
||
|
None
|
||
|
|
||
|
IRQL = DISPATCH_LEVEL
|
||
|
|
||
|
Note:
|
||
|
|
||
|
========================================================================
|
||
|
*/
|
||
|
VOID RTMPTkipPutUInt32(
|
||
|
IN OUT PUCHAR pDst,
|
||
|
IN ULONG val)
|
||
|
{
|
||
|
INT i;
|
||
|
|
||
|
for(i = 0; i < 4; i++)
|
||
|
{
|
||
|
*pDst++ = (UCHAR) (val & 0xff);
|
||
|
val >>= 8;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
========================================================================
|
||
|
|
||
|
Routine Description:
|
||
|
Set the MIC Key.
|
||
|
|
||
|
Arguments:
|
||
|
pAd Pointer to our adapter
|
||
|
pMICKey pointer to MIC Key
|
||
|
|
||
|
Return Value:
|
||
|
None
|
||
|
|
||
|
IRQL = DISPATCH_LEVEL
|
||
|
|
||
|
Note:
|
||
|
|
||
|
========================================================================
|
||
|
*/
|
||
|
VOID RTMPTkipSetMICKey(
|
||
|
IN PTKIP_KEY_INFO pTkip,
|
||
|
IN PUCHAR pMICKey)
|
||
|
{
|
||
|
// Set the key
|
||
|
pTkip->K0 = RTMPTkipGetUInt32(pMICKey);
|
||
|
pTkip->K1 = RTMPTkipGetUInt32(pMICKey + 4);
|
||
|
// and reset the message
|
||
|
pTkip->L = pTkip->K0;
|
||
|
pTkip->R = pTkip->K1;
|
||
|
pTkip->nBytesInM = 0;
|
||
|
pTkip->M = 0;
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
========================================================================
|
||
|
|
||
|
Routine Description:
|
||
|
Calculate the MIC Value.
|
||
|
|
||
|
Arguments:
|
||
|
pAd Pointer to our adapter
|
||
|
uChar Append this uChar
|
||
|
|
||
|
Return Value:
|
||
|
None
|
||
|
|
||
|
IRQL = DISPATCH_LEVEL
|
||
|
|
||
|
Note:
|
||
|
|
||
|
========================================================================
|
||
|
*/
|
||
|
VOID RTMPTkipAppendByte(
|
||
|
IN PTKIP_KEY_INFO pTkip,
|
||
|
IN UCHAR uChar)
|
||
|
{
|
||
|
// Append the byte to our word-sized buffer
|
||
|
pTkip->M |= (uChar << (8* pTkip->nBytesInM));
|
||
|
pTkip->nBytesInM++;
|
||
|
// Process the word if it is full.
|
||
|
if( pTkip->nBytesInM >= 4 )
|
||
|
{
|
||
|
pTkip->L ^= pTkip->M;
|
||
|
pTkip->R ^= ROL32( pTkip->L, 17 );
|
||
|
pTkip->L += pTkip->R;
|
||
|
pTkip->R ^= ((pTkip->L & 0xff00ff00) >> 8) | ((pTkip->L & 0x00ff00ff) << 8);
|
||
|
pTkip->L += pTkip->R;
|
||
|
pTkip->R ^= ROL32( pTkip->L, 3 );
|
||
|
pTkip->L += pTkip->R;
|
||
|
pTkip->R ^= ROR32( pTkip->L, 2 );
|
||
|
pTkip->L += pTkip->R;
|
||
|
// Clear the buffer
|
||
|
pTkip->M = 0;
|
||
|
pTkip->nBytesInM = 0;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
========================================================================
|
||
|
|
||
|
Routine Description:
|
||
|
Calculate the MIC Value.
|
||
|
|
||
|
Arguments:
|
||
|
pAd Pointer to our adapter
|
||
|
pSrc Pointer to source data for Calculate MIC Value
|
||
|
Len Indicate the length of the source data
|
||
|
|
||
|
Return Value:
|
||
|
None
|
||
|
|
||
|
IRQL = DISPATCH_LEVEL
|
||
|
|
||
|
Note:
|
||
|
|
||
|
========================================================================
|
||
|
*/
|
||
|
VOID RTMPTkipAppend(
|
||
|
IN PTKIP_KEY_INFO pTkip,
|
||
|
IN PUCHAR pSrc,
|
||
|
IN UINT nBytes)
|
||
|
{
|
||
|
// This is simple
|
||
|
while(nBytes > 0)
|
||
|
{
|
||
|
RTMPTkipAppendByte(pTkip, *pSrc++);
|
||
|
nBytes--;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
========================================================================
|
||
|
|
||
|
Routine Description:
|
||
|
Get the MIC Value.
|
||
|
|
||
|
Arguments:
|
||
|
pAd Pointer to our adapter
|
||
|
|
||
|
Return Value:
|
||
|
None
|
||
|
|
||
|
IRQL = DISPATCH_LEVEL
|
||
|
|
||
|
Note:
|
||
|
the MIC Value is store in pAd->PrivateInfo.MIC
|
||
|
========================================================================
|
||
|
*/
|
||
|
VOID RTMPTkipGetMIC(
|
||
|
IN PTKIP_KEY_INFO pTkip)
|
||
|
{
|
||
|
// Append the minimum padding
|
||
|
RTMPTkipAppendByte(pTkip, 0x5a );
|
||
|
RTMPTkipAppendByte(pTkip, 0 );
|
||
|
RTMPTkipAppendByte(pTkip, 0 );
|
||
|
RTMPTkipAppendByte(pTkip, 0 );
|
||
|
RTMPTkipAppendByte(pTkip, 0 );
|
||
|
// and then zeroes until the length is a multiple of 4
|
||
|
while( pTkip->nBytesInM != 0 )
|
||
|
{
|
||
|
RTMPTkipAppendByte(pTkip, 0 );
|
||
|
}
|
||
|
// The appendByte function has already computed the result.
|
||
|
RTMPTkipPutUInt32(pTkip->MIC, pTkip->L);
|
||
|
RTMPTkipPutUInt32(pTkip->MIC + 4, pTkip->R);
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
========================================================================
|
||
|
|
||
|
Routine Description:
|
||
|
Init Tkip function.
|
||
|
|
||
|
Arguments:
|
||
|
pAd Pointer to our adapter
|
||
|
pTKey Pointer to the Temporal Key (TK), TK shall be 128bits.
|
||
|
KeyId TK Key ID
|
||
|
pTA Pointer to transmitter address
|
||
|
pMICKey pointer to MIC Key
|
||
|
|
||
|
Return Value:
|
||
|
None
|
||
|
|
||
|
IRQL = DISPATCH_LEVEL
|
||
|
|
||
|
Note:
|
||
|
|
||
|
========================================================================
|
||
|
*/
|
||
|
VOID RTMPInitTkipEngine(
|
||
|
IN PRTMP_ADAPTER pAd,
|
||
|
IN PUCHAR pKey,
|
||
|
IN UCHAR KeyId,
|
||
|
IN PUCHAR pTA,
|
||
|
IN PUCHAR pMICKey,
|
||
|
IN PUCHAR pTSC,
|
||
|
OUT PULONG pIV16,
|
||
|
OUT PULONG pIV32)
|
||
|
{
|
||
|
TKIP_IV tkipIv;
|
||
|
|
||
|
// Prepare 8 bytes TKIP encapsulation for MPDU
|
||
|
NdisZeroMemory(&tkipIv, 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);
|
||
|
NdisMoveMemory(&tkipIv.IV32, (pTSC + 2), 4); // Copy IV
|
||
|
|
||
|
*pIV16 = tkipIv.IV16.word;
|
||
|
*pIV32 = tkipIv.IV32;
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
========================================================================
|
||
|
|
||
|
Routine Description:
|
||
|
Init MIC Value calculation function which include set MIC key &
|
||
|
calculate first 16 bytes (DA + SA + priority + 0)
|
||
|
|
||
|
Arguments:
|
||
|
pAd 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 PRTMP_ADAPTER pAd,
|
||
|
IN PUCHAR pKey,
|
||
|
IN PUCHAR pDA,
|
||
|
IN PUCHAR pSA,
|
||
|
IN UCHAR UserPriority,
|
||
|
IN PUCHAR pMICKey)
|
||
|
{
|
||
|
ULONG Priority = UserPriority;
|
||
|
|
||
|
// Init MIC value calculation
|
||
|
RTMPTkipSetMICKey(&pAd->PrivateInfo.Tx, pMICKey);
|
||
|
// DA
|
||
|
RTMPTkipAppend(&pAd->PrivateInfo.Tx, pDA, MAC_ADDR_LEN);
|
||
|
// SA
|
||
|
RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSA, MAC_ADDR_LEN);
|
||
|
// Priority + 3 bytes of 0
|
||
|
RTMPTkipAppend(&pAd->PrivateInfo.Tx, (PUCHAR)&Priority, 4);
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
========================================================================
|
||
|
|
||
|
Routine Description:
|
||
|
Compare MIC value of received MSDU
|
||
|
|
||
|
Arguments:
|
||
|
pAd 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 PRTMP_ADAPTER pAd,
|
||
|
IN PUCHAR pSrc,
|
||
|
IN PUCHAR pDA,
|
||
|
IN PUCHAR pSA,
|
||
|
IN PUCHAR pMICKey,
|
||
|
IN UCHAR UserPriority,
|
||
|
IN UINT Len)
|
||
|
{
|
||
|
UCHAR OldMic[8];
|
||
|
ULONG Priority = UserPriority;
|
||
|
|
||
|
// Init MIC value calculation
|
||
|
RTMPTkipSetMICKey(&pAd->PrivateInfo.Rx, pMICKey);
|
||
|
// DA
|
||
|
RTMPTkipAppend(&pAd->PrivateInfo.Rx, pDA, MAC_ADDR_LEN);
|
||
|
// SA
|
||
|
RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSA, MAC_ADDR_LEN);
|
||
|
// Priority + 3 bytes of 0
|
||
|
RTMPTkipAppend(&pAd->PrivateInfo.Rx, (PUCHAR)&Priority, 4);
|
||
|
|
||
|
// Calculate MIC value from plain text data
|
||
|
RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSrc, Len);
|
||
|
|
||
|
// Get MIC valude from received frame
|
||
|
NdisMoveMemory(OldMic, pSrc + Len, 8);
|
||
|
|
||
|
// Get MIC value from decrypted plain data
|
||
|
RTMPTkipGetMIC(&pAd->PrivateInfo.Rx);
|
||
|
|
||
|
// Move MIC value from MSDU, this steps should move to data path.
|
||
|
// Since the MIC value might cross MPDUs.
|
||
|
if(!NdisEqualMemory(pAd->PrivateInfo.Rx.MIC, OldMic, 8))
|
||
|
{
|
||
|
DBGPRINT_RAW(RT_DEBUG_ERROR, ("RTMPTkipCompareMICValue(): TKIP MIC Error !\n")); //MIC error.
|
||
|
|
||
|
|
||
|
return (FALSE);
|
||
|
}
|
||
|
return (TRUE);
|
||
|
}
|
||
|
|
||
|
/*
|
||
|
========================================================================
|
||
|
|
||
|
Routine Description:
|
||
|
Compare MIC value of received MSDU
|
||
|
|
||
|
Arguments:
|
||
|
pAd 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 PRTMP_ADAPTER pAd,
|
||
|
IN PUCHAR pLLC,
|
||
|
IN PUCHAR pSrc,
|
||
|
IN PUCHAR pDA,
|
||
|
IN PUCHAR pSA,
|
||
|
IN PUCHAR pMICKey,
|
||
|
IN UINT Len)
|
||
|
{
|
||
|
UCHAR OldMic[8];
|
||
|
ULONG Priority = 0;
|
||
|
|
||
|
// Init MIC value calculation
|
||
|
RTMPTkipSetMICKey(&pAd->PrivateInfo.Rx, pMICKey);
|
||
|
// DA
|
||
|
RTMPTkipAppend(&pAd->PrivateInfo.Rx, pDA, MAC_ADDR_LEN);
|
||
|
// SA
|
||
|
RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSA, MAC_ADDR_LEN);
|
||
|
// Priority + 3 bytes of 0
|
||
|
RTMPTkipAppend(&pAd->PrivateInfo.Rx, (PUCHAR)&Priority, 4);
|
||
|
|
||
|
// Start with LLC header
|
||
|
RTMPTkipAppend(&pAd->PrivateInfo.Rx, pLLC, 8);
|
||
|
|
||
|
// Calculate MIC value from plain text data
|
||
|
RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSrc, Len);
|
||
|
|
||
|
// Get MIC valude from received frame
|
||
|
NdisMoveMemory(OldMic, pSrc + Len, 8);
|
||
|
|
||
|
// Get MIC value from decrypted plain data
|
||
|
RTMPTkipGetMIC(&pAd->PrivateInfo.Rx);
|
||
|
|
||
|
// Move MIC value from MSDU, this steps should move to data path.
|
||
|
// Since the MIC value might cross MPDUs.
|
||
|
if(!NdisEqualMemory(pAd->PrivateInfo.Rx.MIC, OldMic, 8))
|
||
|
{
|
||
|
DBGPRINT_RAW(RT_DEBUG_ERROR, ("RTMPTkipCompareMICValueWithLLC(): TKIP MIC Error !\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:
|
||
|
pAd 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 PRTMP_ADAPTER pAd,
|
||
|
IN PNDIS_PACKET pPacket,
|
||
|
IN PUCHAR pEncap,
|
||
|
IN PCIPHER_KEY pKey,
|
||
|
IN UCHAR apidx)
|
||
|
{
|
||
|
PACKET_INFO PacketInfo;
|
||
|
PUCHAR pSrcBufVA;
|
||
|
UINT SrcBufLen;
|
||
|
PUCHAR pSrc;
|
||
|
UCHAR UserPriority;
|
||
|
UCHAR vlan_offset = 0;
|
||
|
|
||
|
RTMP_QueryPacketInfo(pPacket, &PacketInfo, &pSrcBufVA, &SrcBufLen);
|
||
|
|
||
|
UserPriority = RTMP_GET_PACKET_UP(pPacket);
|
||
|
pSrc = pSrcBufVA;
|
||
|
|
||
|
// determine if this is a vlan packet
|
||
|
if (((*(pSrc + 12) << 8) + *(pSrc + 13)) == 0x8100)
|
||
|
vlan_offset = 4;
|
||
|
|
||
|
#ifdef CONFIG_STA_SUPPORT
|
||
|
#endif // CONFIG_STA_SUPPORT //
|
||
|
{
|
||
|
RTMPInitMICEngine(
|
||
|
pAd,
|
||
|
pKey->Key,
|
||
|
pSrc,
|
||
|
pSrc + 6,
|
||
|
UserPriority,
|
||
|
pKey->TxMic);
|
||
|
}
|
||
|
|
||
|
|
||
|
if (pEncap != NULL)
|
||
|
{
|
||
|
// LLC encapsulation
|
||
|
RTMPTkipAppend(&pAd->PrivateInfo.Tx, pEncap, 6);
|
||
|
// Protocol Type
|
||
|
RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSrc + 12 + vlan_offset, 2);
|
||
|
}
|
||
|
SrcBufLen -= (14 + vlan_offset);
|
||
|
pSrc += (14 + vlan_offset);
|
||
|
do
|
||
|
{
|
||
|
if (SrcBufLen > 0)
|
||
|
{
|
||
|
RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSrc, SrcBufLen);
|
||
|
}
|
||
|
|
||
|
break; // No need handle next packet
|
||
|
|
||
|
} while (TRUE); // End of copying payload
|
||
|
|
||
|
// Compute the final MIC Value
|
||
|
RTMPTkipGetMIC(&pAd->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;
|
||
|
}
|
||
|
|
||
|
|
||
|
//
|
||
|
// 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;
|
||
|
UINT crc32 = 0;
|
||
|
UINT trailfcs = 0;
|
||
|
UCHAR MIC[8];
|
||
|
UCHAR TrailMIC[8];
|
||
|
|
||
|
#ifdef RT_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, ("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));
|
||
|
pnh = cpu2le32(pnh);
|
||
|
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 */
|
||
|
|
||
|
if(crc32 != cpu2le32(trailfcs))
|
||
|
{
|
||
|
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 RT_BIG_ENDIAN
|
||
|
RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
|
||
|
#endif
|
||
|
//DBGPRINT(RT_DEBUG_TRACE, "RTMPSoftDecryptTKIP Decript done!!\n");
|
||
|
return TRUE;
|
||
|
}
|