diff options
author | Greg Kroah-Hartman <gregkh@suse.de> | 2009-02-25 16:14:55 -0800 |
---|---|---|
committer | Greg Kroah-Hartman <gregkh@suse.de> | 2009-04-03 14:54:24 -0700 |
commit | e642f09951f7cbb69983781b07bb9cd881546ac4 (patch) | |
tree | c783967792f17f0abfab38c9a893fb9565b8bae2 /drivers/staging/rt3070/common/rtmp_tkip.c | |
parent | 21a6a6e9f81fb26fbcecbb1304018d34dcd1794f (diff) |
Staging: add rt3070 wireless driver
This is the Ralink RT3070 driver from the company that does horrible
things like reading a config file from /etc. However, the driver that
is currently under development from the wireless development community
is not working at all yet, so distros and users are using this version
instead (quite common hardware on a lot of netbook machines).
So here is this driver, for now, until the wireless developers get a
"clean" version into the main tree, or until this version is cleaned up
sufficiently to move out of the staging tree.
Ported to the Linux build system, fixed lots of build issues, forward
ported to the current kernel version, and other minor cleanups were all
done by me.
Cc: Linux wireless <linux-wireless@vger.kernel.org>
Signed-off-by: Greg Kroah-Hartman <gregkh@suse.de>
Diffstat (limited to 'drivers/staging/rt3070/common/rtmp_tkip.c')
-rw-r--r-- | drivers/staging/rt3070/common/rtmp_tkip.c | 1613 |
1 files changed, 1613 insertions, 0 deletions
diff --git a/drivers/staging/rt3070/common/rtmp_tkip.c b/drivers/staging/rt3070/common/rtmp_tkip.c new file mode 100644 index 00000000000..bf8986e8367 --- /dev/null +++ b/drivers/staging/rt3070/common/rtmp_tkip.c @@ -0,0 +1,1613 @@ +/* + ************************************************************************* + * 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: + rtmp_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 +}; + +/*****************************/ +/******** SBOX Table *********/ +/*****************************/ + +UCHAR SboxTable[256] = +{ + 0x63, 0x7c, 0x77, 0x7b, 0xf2, 0x6b, 0x6f, 0xc5, + 0x30, 0x01, 0x67, 0x2b, 0xfe, 0xd7, 0xab, 0x76, + 0xca, 0x82, 0xc9, 0x7d, 0xfa, 0x59, 0x47, 0xf0, + 0xad, 0xd4, 0xa2, 0xaf, 0x9c, 0xa4, 0x72, 0xc0, + 0xb7, 0xfd, 0x93, 0x26, 0x36, 0x3f, 0xf7, 0xcc, + 0x34, 0xa5, 0xe5, 0xf1, 0x71, 0xd8, 0x31, 0x15, + 0x04, 0xc7, 0x23, 0xc3, 0x18, 0x96, 0x05, 0x9a, + 0x07, 0x12, 0x80, 0xe2, 0xeb, 0x27, 0xb2, 0x75, + 0x09, 0x83, 0x2c, 0x1a, 0x1b, 0x6e, 0x5a, 0xa0, + 0x52, 0x3b, 0xd6, 0xb3, 0x29, 0xe3, 0x2f, 0x84, + 0x53, 0xd1, 0x00, 0xed, 0x20, 0xfc, 0xb1, 0x5b, + 0x6a, 0xcb, 0xbe, 0x39, 0x4a, 0x4c, 0x58, 0xcf, + 0xd0, 0xef, 0xaa, 0xfb, 0x43, 0x4d, 0x33, 0x85, + 0x45, 0xf9, 0x02, 0x7f, 0x50, 0x3c, 0x9f, 0xa8, + 0x51, 0xa3, 0x40, 0x8f, 0x92, 0x9d, 0x38, 0xf5, + 0xbc, 0xb6, 0xda, 0x21, 0x10, 0xff, 0xf3, 0xd2, + 0xcd, 0x0c, 0x13, 0xec, 0x5f, 0x97, 0x44, 0x17, + 0xc4, 0xa7, 0x7e, 0x3d, 0x64, 0x5d, 0x19, 0x73, + 0x60, 0x81, 0x4f, 0xdc, 0x22, 0x2a, 0x90, 0x88, + 0x46, 0xee, 0xb8, 0x14, 0xde, 0x5e, 0x0b, 0xdb, + 0xe0, 0x32, 0x3a, 0x0a, 0x49, 0x06, 0x24, 0x5c, + 0xc2, 0xd3, 0xac, 0x62, 0x91, 0x95, 0xe4, 0x79, + 0xe7, 0xc8, 0x37, 0x6d, 0x8d, 0xd5, 0x4e, 0xa9, + 0x6c, 0x56, 0xf4, 0xea, 0x65, 0x7a, 0xae, 0x08, + 0xba, 0x78, 0x25, 0x2e, 0x1c, 0xa6, 0xb4, 0xc6, + 0xe8, 0xdd, 0x74, 0x1f, 0x4b, 0xbd, 0x8b, 0x8a, + 0x70, 0x3e, 0xb5, 0x66, 0x48, 0x03, 0xf6, 0x0e, + 0x61, 0x35, 0x57, 0xb9, 0x86, 0xc1, 0x1d, 0x9e, + 0xe1, 0xf8, 0x98, 0x11, 0x69, 0xd9, 0x8e, 0x94, + 0x9b, 0x1e, 0x87, 0xe9, 0xce, 0x55, 0x28, 0xdf, + 0x8c, 0xa1, 0x89, 0x0d, 0xbf, 0xe6, 0x42, 0x68, + 0x41, 0x99, 0x2d, 0x0f, 0xb0, 0x54, 0xbb, 0x16 +}; + +VOID xor_32( + IN PUCHAR a, + IN PUCHAR b, + OUT PUCHAR out); + +VOID xor_128( + IN PUCHAR a, + IN PUCHAR b, + OUT PUCHAR out); + +VOID next_key( + IN PUCHAR key, + IN INT round); + +VOID byte_sub( + IN PUCHAR in, + OUT PUCHAR out); + +VOID shift_row( + IN PUCHAR in, + OUT PUCHAR out); + +VOID mix_column( + IN PUCHAR in, + OUT PUCHAR out); + +UCHAR RTMPCkipSbox( + IN UCHAR a); +// +// 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; +} + + +/************************************************/ +/* 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; + 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; +} + + + + +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 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, ("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; + + + + // 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); + 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); + 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); + + // + // 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); + } + + // 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 RT_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); +} + |