diff options
Diffstat (limited to 'drivers/staging/rt3070/common/rtmp_tkip.c')
| -rw-r--r-- | drivers/staging/rt3070/common/rtmp_tkip.c | 1614 |
1 files changed, 1 insertions, 1613 deletions
diff --git a/drivers/staging/rt3070/common/rtmp_tkip.c b/drivers/staging/rt3070/common/rtmp_tkip.c index bf8986e8367..57a5ee96f31 100644 --- a/drivers/staging/rt3070/common/rtmp_tkip.c +++ b/drivers/staging/rt3070/common/rtmp_tkip.c @@ -1,1613 +1 @@ -/* - ************************************************************************* - * 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); -} - +#include "../../rt2870/common/rtmp_tkip.c" |