| /* |
| ************************************************************************* |
| * 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); |
| } |
| |