diff options
Diffstat (limited to 'drivers/staging/rt2860/common/rtmp_tkip.c')
| -rw-r--r-- | drivers/staging/rt2860/common/rtmp_tkip.c | 1607 |
1 files changed, 1607 insertions, 0 deletions
diff --git a/drivers/staging/rt2860/common/rtmp_tkip.c b/drivers/staging/rt2860/common/rtmp_tkip.c new file mode 100644 index 000000000000..a87ea3a5d3ea --- /dev/null +++ b/drivers/staging/rt2860/common/rtmp_tkip.c @@ -0,0 +1,1607 @@ +/* + ************************************************************************* + * 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; + 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; + + { + 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. + return (FALSE); + } + +#ifdef RT_BIG_ENDIAN + RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE); +#endif + 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); +} + |