2 *************************************************************************
4 * 5F., No.36, Taiyuan St., Jhubei City,
8 * (c) Copyright 2002-2007, Ralink Technology, Inc.
10 * This program is free software; you can redistribute it and/or modify *
11 * it under the terms of the GNU General Public License as published by *
12 * the Free Software Foundation; either version 2 of the License, or *
13 * (at your option) any later version. *
15 * This program is distributed in the hope that it will be useful, *
16 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
18 * GNU General Public License for more details. *
20 * You should have received a copy of the GNU General Public License *
21 * along with this program; if not, write to the *
22 * Free Software Foundation, Inc., *
23 * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
25 *************************************************************************
34 -------- ---------- ----------------------------------------------
35 Paul Wu 02-25-02 Initial
38 #include "../rt_config.h"
40 // Rotation functions on 32 bit values
41 #define ROL32( A, n ) \
42 ( ((A) << (n)) | ( ((A)>>(32-(n))) & ( (1UL << (n)) - 1 ) ) )
43 #define ROR32( A, n ) ROL32( (A), 32-(n) )
45 UINT Tkip_Sbox_Lower[256] =
47 0xA5,0x84,0x99,0x8D,0x0D,0xBD,0xB1,0x54,
48 0x50,0x03,0xA9,0x7D,0x19,0x62,0xE6,0x9A,
49 0x45,0x9D,0x40,0x87,0x15,0xEB,0xC9,0x0B,
50 0xEC,0x67,0xFD,0xEA,0xBF,0xF7,0x96,0x5B,
51 0xC2,0x1C,0xAE,0x6A,0x5A,0x41,0x02,0x4F,
52 0x5C,0xF4,0x34,0x08,0x93,0x73,0x53,0x3F,
53 0x0C,0x52,0x65,0x5E,0x28,0xA1,0x0F,0xB5,
54 0x09,0x36,0x9B,0x3D,0x26,0x69,0xCD,0x9F,
55 0x1B,0x9E,0x74,0x2E,0x2D,0xB2,0xEE,0xFB,
56 0xF6,0x4D,0x61,0xCE,0x7B,0x3E,0x71,0x97,
57 0xF5,0x68,0x00,0x2C,0x60,0x1F,0xC8,0xED,
58 0xBE,0x46,0xD9,0x4B,0xDE,0xD4,0xE8,0x4A,
59 0x6B,0x2A,0xE5,0x16,0xC5,0xD7,0x55,0x94,
60 0xCF,0x10,0x06,0x81,0xF0,0x44,0xBA,0xE3,
61 0xF3,0xFE,0xC0,0x8A,0xAD,0xBC,0x48,0x04,
62 0xDF,0xC1,0x75,0x63,0x30,0x1A,0x0E,0x6D,
63 0x4C,0x14,0x35,0x2F,0xE1,0xA2,0xCC,0x39,
64 0x57,0xF2,0x82,0x47,0xAC,0xE7,0x2B,0x95,
65 0xA0,0x98,0xD1,0x7F,0x66,0x7E,0xAB,0x83,
66 0xCA,0x29,0xD3,0x3C,0x79,0xE2,0x1D,0x76,
67 0x3B,0x56,0x4E,0x1E,0xDB,0x0A,0x6C,0xE4,
68 0x5D,0x6E,0xEF,0xA6,0xA8,0xA4,0x37,0x8B,
69 0x32,0x43,0x59,0xB7,0x8C,0x64,0xD2,0xE0,
70 0xB4,0xFA,0x07,0x25,0xAF,0x8E,0xE9,0x18,
71 0xD5,0x88,0x6F,0x72,0x24,0xF1,0xC7,0x51,
72 0x23,0x7C,0x9C,0x21,0xDD,0xDC,0x86,0x85,
73 0x90,0x42,0xC4,0xAA,0xD8,0x05,0x01,0x12,
74 0xA3,0x5F,0xF9,0xD0,0x91,0x58,0x27,0xB9,
75 0x38,0x13,0xB3,0x33,0xBB,0x70,0x89,0xA7,
76 0xB6,0x22,0x92,0x20,0x49,0xFF,0x78,0x7A,
77 0x8F,0xF8,0x80,0x17,0xDA,0x31,0xC6,0xB8,
78 0xC3,0xB0,0x77,0x11,0xCB,0xFC,0xD6,0x3A
81 UINT Tkip_Sbox_Upper[256] =
83 0xC6,0xF8,0xEE,0xF6,0xFF,0xD6,0xDE,0x91,
84 0x60,0x02,0xCE,0x56,0xE7,0xB5,0x4D,0xEC,
85 0x8F,0x1F,0x89,0xFA,0xEF,0xB2,0x8E,0xFB,
86 0x41,0xB3,0x5F,0x45,0x23,0x53,0xE4,0x9B,
87 0x75,0xE1,0x3D,0x4C,0x6C,0x7E,0xF5,0x83,
88 0x68,0x51,0xD1,0xF9,0xE2,0xAB,0x62,0x2A,
89 0x08,0x95,0x46,0x9D,0x30,0x37,0x0A,0x2F,
90 0x0E,0x24,0x1B,0xDF,0xCD,0x4E,0x7F,0xEA,
91 0x12,0x1D,0x58,0x34,0x36,0xDC,0xB4,0x5B,
92 0xA4,0x76,0xB7,0x7D,0x52,0xDD,0x5E,0x13,
93 0xA6,0xB9,0x00,0xC1,0x40,0xE3,0x79,0xB6,
94 0xD4,0x8D,0x67,0x72,0x94,0x98,0xB0,0x85,
95 0xBB,0xC5,0x4F,0xED,0x86,0x9A,0x66,0x11,
96 0x8A,0xE9,0x04,0xFE,0xA0,0x78,0x25,0x4B,
97 0xA2,0x5D,0x80,0x05,0x3F,0x21,0x70,0xF1,
98 0x63,0x77,0xAF,0x42,0x20,0xE5,0xFD,0xBF,
99 0x81,0x18,0x26,0xC3,0xBE,0x35,0x88,0x2E,
100 0x93,0x55,0xFC,0x7A,0xC8,0xBA,0x32,0xE6,
101 0xC0,0x19,0x9E,0xA3,0x44,0x54,0x3B,0x0B,
102 0x8C,0xC7,0x6B,0x28,0xA7,0xBC,0x16,0xAD,
103 0xDB,0x64,0x74,0x14,0x92,0x0C,0x48,0xB8,
104 0x9F,0xBD,0x43,0xC4,0x39,0x31,0xD3,0xF2,
105 0xD5,0x8B,0x6E,0xDA,0x01,0xB1,0x9C,0x49,
106 0xD8,0xAC,0xF3,0xCF,0xCA,0xF4,0x47,0x10,
107 0x6F,0xF0,0x4A,0x5C,0x38,0x57,0x73,0x97,
108 0xCB,0xA1,0xE8,0x3E,0x96,0x61,0x0D,0x0F,
109 0xE0,0x7C,0x71,0xCC,0x90,0x06,0xF7,0x1C,
110 0xC2,0x6A,0xAE,0x69,0x17,0x99,0x3A,0x27,
111 0xD9,0xEB,0x2B,0x22,0xD2,0xA9,0x07,0x33,
112 0x2D,0x3C,0x15,0xC9,0x87,0xAA,0x50,0xA5,
113 0x03,0x59,0x09,0x1A,0x65,0xD7,0x84,0xD0,
114 0x82,0x29,0x5A,0x1E,0x7B,0xA8,0x6D,0x2C
117 /*****************************/
118 /******** SBOX Table *********/
119 /*****************************/
121 UCHAR SboxTable[256] =
123 0x63, 0x7c, 0x77, 0x7b, 0xf2, 0x6b, 0x6f, 0xc5,
124 0x30, 0x01, 0x67, 0x2b, 0xfe, 0xd7, 0xab, 0x76,
125 0xca, 0x82, 0xc9, 0x7d, 0xfa, 0x59, 0x47, 0xf0,
126 0xad, 0xd4, 0xa2, 0xaf, 0x9c, 0xa4, 0x72, 0xc0,
127 0xb7, 0xfd, 0x93, 0x26, 0x36, 0x3f, 0xf7, 0xcc,
128 0x34, 0xa5, 0xe5, 0xf1, 0x71, 0xd8, 0x31, 0x15,
129 0x04, 0xc7, 0x23, 0xc3, 0x18, 0x96, 0x05, 0x9a,
130 0x07, 0x12, 0x80, 0xe2, 0xeb, 0x27, 0xb2, 0x75,
131 0x09, 0x83, 0x2c, 0x1a, 0x1b, 0x6e, 0x5a, 0xa0,
132 0x52, 0x3b, 0xd6, 0xb3, 0x29, 0xe3, 0x2f, 0x84,
133 0x53, 0xd1, 0x00, 0xed, 0x20, 0xfc, 0xb1, 0x5b,
134 0x6a, 0xcb, 0xbe, 0x39, 0x4a, 0x4c, 0x58, 0xcf,
135 0xd0, 0xef, 0xaa, 0xfb, 0x43, 0x4d, 0x33, 0x85,
136 0x45, 0xf9, 0x02, 0x7f, 0x50, 0x3c, 0x9f, 0xa8,
137 0x51, 0xa3, 0x40, 0x8f, 0x92, 0x9d, 0x38, 0xf5,
138 0xbc, 0xb6, 0xda, 0x21, 0x10, 0xff, 0xf3, 0xd2,
139 0xcd, 0x0c, 0x13, 0xec, 0x5f, 0x97, 0x44, 0x17,
140 0xc4, 0xa7, 0x7e, 0x3d, 0x64, 0x5d, 0x19, 0x73,
141 0x60, 0x81, 0x4f, 0xdc, 0x22, 0x2a, 0x90, 0x88,
142 0x46, 0xee, 0xb8, 0x14, 0xde, 0x5e, 0x0b, 0xdb,
143 0xe0, 0x32, 0x3a, 0x0a, 0x49, 0x06, 0x24, 0x5c,
144 0xc2, 0xd3, 0xac, 0x62, 0x91, 0x95, 0xe4, 0x79,
145 0xe7, 0xc8, 0x37, 0x6d, 0x8d, 0xd5, 0x4e, 0xa9,
146 0x6c, 0x56, 0xf4, 0xea, 0x65, 0x7a, 0xae, 0x08,
147 0xba, 0x78, 0x25, 0x2e, 0x1c, 0xa6, 0xb4, 0xc6,
148 0xe8, 0xdd, 0x74, 0x1f, 0x4b, 0xbd, 0x8b, 0x8a,
149 0x70, 0x3e, 0xb5, 0x66, 0x48, 0x03, 0xf6, 0x0e,
150 0x61, 0x35, 0x57, 0xb9, 0x86, 0xc1, 0x1d, 0x9e,
151 0xe1, 0xf8, 0x98, 0x11, 0x69, 0xd9, 0x8e, 0x94,
152 0x9b, 0x1e, 0x87, 0xe9, 0xce, 0x55, 0x28, 0xdf,
153 0x8c, 0xa1, 0x89, 0x0d, 0xbf, 0xe6, 0x42, 0x68,
154 0x41, 0x99, 0x2d, 0x0f, 0xb0, 0x54, 0xbb, 0x16
186 // Expanded IV for TKIP function.
188 typedef struct PACKED _IV_CONTROL_
220 } TKIP_IV, *PTKIP_IV;
224 ========================================================================
227 Convert from UCHAR[] to ULONG in a portable way
230 pMICKey pointer to MIC Key
237 ========================================================================
239 ULONG RTMPTkipGetUInt32(
245 for (i = 0; i < 4; i++)
247 res |= (*pMICKey++) << (8 * i);
254 ========================================================================
257 Convert from ULONG to UCHAR[] in a portable way
260 pDst pointer to destination for convert ULONG to UCHAR[]
261 val the value for convert
266 IRQL = DISPATCH_LEVEL
270 ========================================================================
272 VOID RTMPTkipPutUInt32(
278 for(i = 0; i < 4; i++)
280 *pDst++ = (UCHAR) (val & 0xff);
286 ========================================================================
292 pAd Pointer to our adapter
293 pMICKey pointer to MIC Key
298 IRQL = DISPATCH_LEVEL
302 ========================================================================
304 VOID RTMPTkipSetMICKey(
305 IN PTKIP_KEY_INFO pTkip,
309 pTkip->K0 = RTMPTkipGetUInt32(pMICKey);
310 pTkip->K1 = RTMPTkipGetUInt32(pMICKey + 4);
311 // and reset the message
312 pTkip->L = pTkip->K0;
313 pTkip->R = pTkip->K1;
314 pTkip->nBytesInM = 0;
319 ========================================================================
322 Calculate the MIC Value.
325 pAd Pointer to our adapter
326 uChar Append this uChar
331 IRQL = DISPATCH_LEVEL
335 ========================================================================
337 VOID RTMPTkipAppendByte(
338 IN PTKIP_KEY_INFO pTkip,
341 // Append the byte to our word-sized buffer
342 pTkip->M |= (uChar << (8* pTkip->nBytesInM));
344 // Process the word if it is full.
345 if( pTkip->nBytesInM >= 4 )
347 pTkip->L ^= pTkip->M;
348 pTkip->R ^= ROL32( pTkip->L, 17 );
349 pTkip->L += pTkip->R;
350 pTkip->R ^= ((pTkip->L & 0xff00ff00) >> 8) | ((pTkip->L & 0x00ff00ff) << 8);
351 pTkip->L += pTkip->R;
352 pTkip->R ^= ROL32( pTkip->L, 3 );
353 pTkip->L += pTkip->R;
354 pTkip->R ^= ROR32( pTkip->L, 2 );
355 pTkip->L += pTkip->R;
358 pTkip->nBytesInM = 0;
363 ========================================================================
366 Calculate the MIC Value.
369 pAd Pointer to our adapter
370 pSrc Pointer to source data for Calculate MIC Value
371 Len Indicate the length of the source data
376 IRQL = DISPATCH_LEVEL
380 ========================================================================
383 IN PTKIP_KEY_INFO pTkip,
390 RTMPTkipAppendByte(pTkip, *pSrc++);
396 ========================================================================
402 pAd Pointer to our adapter
407 IRQL = DISPATCH_LEVEL
410 the MIC Value is store in pAd->PrivateInfo.MIC
411 ========================================================================
414 IN PTKIP_KEY_INFO pTkip)
416 // Append the minimum padding
417 RTMPTkipAppendByte(pTkip, 0x5a );
418 RTMPTkipAppendByte(pTkip, 0 );
419 RTMPTkipAppendByte(pTkip, 0 );
420 RTMPTkipAppendByte(pTkip, 0 );
421 RTMPTkipAppendByte(pTkip, 0 );
422 // and then zeroes until the length is a multiple of 4
423 while( pTkip->nBytesInM != 0 )
425 RTMPTkipAppendByte(pTkip, 0 );
427 // The appendByte function has already computed the result.
428 RTMPTkipPutUInt32(pTkip->MIC, pTkip->L);
429 RTMPTkipPutUInt32(pTkip->MIC + 4, pTkip->R);
433 ========================================================================
439 pAd Pointer to our adapter
440 pTKey Pointer to the Temporal Key (TK), TK shall be 128bits.
442 pTA Pointer to transmitter address
443 pMICKey pointer to MIC Key
448 IRQL = DISPATCH_LEVEL
452 ========================================================================
454 VOID RTMPInitTkipEngine(
455 IN PRTMP_ADAPTER pAd,
466 // Prepare 8 bytes TKIP encapsulation for MPDU
467 NdisZeroMemory(&tkipIv, sizeof(TKIP_IV));
468 tkipIv.IV16.field.rc0 = *(pTSC + 1);
469 tkipIv.IV16.field.rc1 = (tkipIv.IV16.field.rc0 | 0x20) & 0x7f;
470 tkipIv.IV16.field.rc2 = *pTSC;
471 tkipIv.IV16.field.CONTROL.field.ExtIV = 1; // 0: non-extended IV, 1: an extended IV
472 tkipIv.IV16.field.CONTROL.field.KeyID = KeyId;
473 // tkipIv.IV32 = *(PULONG)(pTSC + 2);
474 NdisMoveMemory(&tkipIv.IV32, (pTSC + 2), 4); // Copy IV
476 *pIV16 = tkipIv.IV16.word;
477 *pIV32 = tkipIv.IV32;
481 ========================================================================
484 Init MIC Value calculation function which include set MIC key &
485 calculate first 16 bytes (DA + SA + priority + 0)
488 pAd Pointer to our adapter
489 pTKey Pointer to the Temporal Key (TK), TK shall be 128bits.
490 pDA Pointer to DA address
491 pSA Pointer to SA address
492 pMICKey pointer to MIC Key
499 ========================================================================
501 VOID RTMPInitMICEngine(
502 IN PRTMP_ADAPTER pAd,
506 IN UCHAR UserPriority,
509 ULONG Priority = UserPriority;
511 // Init MIC value calculation
512 RTMPTkipSetMICKey(&pAd->PrivateInfo.Tx, pMICKey);
514 RTMPTkipAppend(&pAd->PrivateInfo.Tx, pDA, MAC_ADDR_LEN);
516 RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSA, MAC_ADDR_LEN);
517 // Priority + 3 bytes of 0
518 RTMPTkipAppend(&pAd->PrivateInfo.Tx, (PUCHAR)&Priority, 4);
522 ========================================================================
525 Compare MIC value of received MSDU
528 pAd Pointer to our adapter
529 pSrc Pointer to the received Plain text data
530 pDA Pointer to DA address
531 pSA Pointer to SA address
532 pMICKey pointer to MIC Key
533 Len the length of the received plain text data exclude MIC value
536 TRUE MIC value matched
537 FALSE MIC value mismatched
539 IRQL = DISPATCH_LEVEL
543 ========================================================================
545 BOOLEAN RTMPTkipCompareMICValue(
546 IN PRTMP_ADAPTER pAd,
551 IN UCHAR UserPriority,
555 ULONG Priority = UserPriority;
557 // Init MIC value calculation
558 RTMPTkipSetMICKey(&pAd->PrivateInfo.Rx, pMICKey);
560 RTMPTkipAppend(&pAd->PrivateInfo.Rx, pDA, MAC_ADDR_LEN);
562 RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSA, MAC_ADDR_LEN);
563 // Priority + 3 bytes of 0
564 RTMPTkipAppend(&pAd->PrivateInfo.Rx, (PUCHAR)&Priority, 4);
566 // Calculate MIC value from plain text data
567 RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSrc, Len);
569 // Get MIC valude from received frame
570 NdisMoveMemory(OldMic, pSrc + Len, 8);
572 // Get MIC value from decrypted plain data
573 RTMPTkipGetMIC(&pAd->PrivateInfo.Rx);
575 // Move MIC value from MSDU, this steps should move to data path.
576 // Since the MIC value might cross MPDUs.
577 if(!NdisEqualMemory(pAd->PrivateInfo.Rx.MIC, OldMic, 8))
579 DBGPRINT_RAW(RT_DEBUG_ERROR, ("RTMPTkipCompareMICValue(): TKIP MIC Error !\n")); //MIC error.
588 ========================================================================
591 Compare MIC value of received MSDU
594 pAd Pointer to our adapter
596 pSrc Pointer to the received Plain text data
597 pDA Pointer to DA address
598 pSA Pointer to SA address
599 pMICKey pointer to MIC Key
600 Len the length of the received plain text data exclude MIC value
603 TRUE MIC value matched
604 FALSE MIC value mismatched
606 IRQL = DISPATCH_LEVEL
610 ========================================================================
612 BOOLEAN RTMPTkipCompareMICValueWithLLC(
613 IN PRTMP_ADAPTER pAd,
624 // Init MIC value calculation
625 RTMPTkipSetMICKey(&pAd->PrivateInfo.Rx, pMICKey);
627 RTMPTkipAppend(&pAd->PrivateInfo.Rx, pDA, MAC_ADDR_LEN);
629 RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSA, MAC_ADDR_LEN);
630 // Priority + 3 bytes of 0
631 RTMPTkipAppend(&pAd->PrivateInfo.Rx, (PUCHAR)&Priority, 4);
633 // Start with LLC header
634 RTMPTkipAppend(&pAd->PrivateInfo.Rx, pLLC, 8);
636 // Calculate MIC value from plain text data
637 RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSrc, Len);
639 // Get MIC valude from received frame
640 NdisMoveMemory(OldMic, pSrc + Len, 8);
642 // Get MIC value from decrypted plain data
643 RTMPTkipGetMIC(&pAd->PrivateInfo.Rx);
645 // Move MIC value from MSDU, this steps should move to data path.
646 // Since the MIC value might cross MPDUs.
647 if(!NdisEqualMemory(pAd->PrivateInfo.Rx.MIC, OldMic, 8))
649 DBGPRINT_RAW(RT_DEBUG_ERROR, ("RTMPTkipCompareMICValueWithLLC(): TKIP MIC Error !\n")); //MIC error.
657 ========================================================================
660 Copy frame from waiting queue into relative ring buffer and set
661 appropriate ASIC register to kick hardware transmit function
664 pAd Pointer to our adapter
665 PNDIS_PACKET Pointer to Ndis Packet for MIC calculation
666 pEncap Pointer to LLC encap data
667 LenEncap Total encap length, might be 0 which indicates no encap
672 IRQL = DISPATCH_LEVEL
676 ========================================================================
678 VOID RTMPCalculateMICValue(
679 IN PRTMP_ADAPTER pAd,
680 IN PNDIS_PACKET pPacket,
685 PACKET_INFO PacketInfo;
690 UCHAR vlan_offset = 0;
692 RTMP_QueryPacketInfo(pPacket, &PacketInfo, &pSrcBufVA, &SrcBufLen);
694 UserPriority = RTMP_GET_PACKET_UP(pPacket);
697 // determine if this is a vlan packet
698 if (((*(pSrc + 12) << 8) + *(pSrc + 13)) == 0x8100)
701 #ifdef CONFIG_STA_SUPPORT
702 #endif // CONFIG_STA_SUPPORT //
717 RTMPTkipAppend(&pAd->PrivateInfo.Tx, pEncap, 6);
719 RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSrc + 12 + vlan_offset, 2);
721 SrcBufLen -= (14 + vlan_offset);
722 pSrc += (14 + vlan_offset);
727 RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSrc, SrcBufLen);
730 break; // No need handle next packet
732 } while (TRUE); // End of copying payload
734 // Compute the final MIC Value
735 RTMPTkipGetMIC(&pAd->PrivateInfo.Tx);
739 /************************************************************/
741 /* Returns a 16 bit value from a 64K entry table. The Table */
742 /* is synthesized from two 256 entry byte wide tables. */
743 /************************************************************/
745 UINT tkip_sbox(UINT index)
751 index_low = (index % 256);
752 index_high = ((index >> 8) % 256);
754 left = Tkip_Sbox_Lower[index_low] + (Tkip_Sbox_Upper[index_low] * 256);
755 right = Tkip_Sbox_Upper[index_high] + (Tkip_Sbox_Lower[index_high] * 256);
757 return (left ^ right);
764 if ((a & 0x01) == 0x01)
766 b = (a >> 1) | 0x8000;
770 b = (a >> 1) & 0x7fff;
779 ULONG pnl, /* Least significant 16 bits of PN */
780 ULONG pnh, /* Most significant 32 bits of PN */
799 tsc0 = (unsigned int)((pnh >> 16) % 65536); /* msb */
800 tsc1 = (unsigned int)(pnh % 65536);
801 tsc2 = (unsigned int)(pnl % 65536); /* lsb */
803 /* Phase 1, step 1 */
806 p1k[2] = (UINT)(ta[0] + (ta[1]*256));
807 p1k[3] = (UINT)(ta[2] + (ta[3]*256));
808 p1k[4] = (UINT)(ta[4] + (ta[5]*256));
810 /* Phase 1, step 2 */
814 p1k[0] = (p1k[0] + tkip_sbox( (p1k[4] ^ ((256*key[1+j]) + key[j])) % 65536 )) % 65536;
815 p1k[1] = (p1k[1] + tkip_sbox( (p1k[0] ^ ((256*key[5+j]) + key[4+j])) % 65536 )) % 65536;
816 p1k[2] = (p1k[2] + tkip_sbox( (p1k[1] ^ ((256*key[9+j]) + key[8+j])) % 65536 )) % 65536;
817 p1k[3] = (p1k[3] + tkip_sbox( (p1k[2] ^ ((256*key[13+j]) + key[12+j])) % 65536 )) % 65536;
818 p1k[4] = (p1k[4] + tkip_sbox( (p1k[3] ^ (((256*key[1+j]) + key[j]))) % 65536 )) % 65536;
819 p1k[4] = (p1k[4] + i) % 65536;
822 /* Phase 2, Step 1 */
828 ppk5 = (p1k[4] + tsc2) % 65536;
831 ppk0 = ppk0 + tkip_sbox( (ppk5 ^ ((256*key[1]) + key[0])) % 65536);
832 ppk1 = ppk1 + tkip_sbox( (ppk0 ^ ((256*key[3]) + key[2])) % 65536);
833 ppk2 = ppk2 + tkip_sbox( (ppk1 ^ ((256*key[5]) + key[4])) % 65536);
834 ppk3 = ppk3 + tkip_sbox( (ppk2 ^ ((256*key[7]) + key[6])) % 65536);
835 ppk4 = ppk4 + tkip_sbox( (ppk3 ^ ((256*key[9]) + key[8])) % 65536);
836 ppk5 = ppk5 + tkip_sbox( (ppk4 ^ ((256*key[11]) + key[10])) % 65536);
838 ppk0 = ppk0 + rotr1(ppk5 ^ ((256*key[13]) + key[12]));
839 ppk1 = ppk1 + rotr1(ppk0 ^ ((256*key[15]) + key[14]));
840 ppk2 = ppk2 + rotr1(ppk1);
841 ppk3 = ppk3 + rotr1(ppk2);
842 ppk4 = ppk4 + rotr1(ppk3);
843 ppk5 = ppk5 + rotr1(ppk4);
845 /* Phase 2, Step 3 */
846 /* Phase 2, Step 3 */
848 tsc0 = (unsigned int)((pnh >> 16) % 65536); /* msb */
849 tsc1 = (unsigned int)(pnh % 65536);
850 tsc2 = (unsigned int)(pnl % 65536); /* lsb */
852 rc4key[0] = (tsc2 >> 8) % 256;
853 rc4key[1] = (((tsc2 >> 8) % 256) | 0x20) & 0x7f;
854 rc4key[2] = tsc2 % 256;
855 rc4key[3] = ((ppk5 ^ ((256*key[1]) + key[0])) >> 1) % 256;
857 rc4key[4] = ppk0 % 256;
858 rc4key[5] = (ppk0 >> 8) % 256;
860 rc4key[6] = ppk1 % 256;
861 rc4key[7] = (ppk1 >> 8) % 256;
863 rc4key[8] = ppk2 % 256;
864 rc4key[9] = (ppk2 >> 8) % 256;
866 rc4key[10] = ppk3 % 256;
867 rc4key[11] = (ppk3 >> 8) % 256;
869 rc4key[12] = ppk4 % 256;
870 rc4key[13] = (ppk4 >> 8) % 256;
872 rc4key[14] = ppk5 % 256;
873 rc4key[15] = (ppk5 >> 8) % 256;
877 /************************************************/
878 /* construct_mic_header1() */
879 /* Builds the first MIC header block from */
881 /************************************************/
883 void construct_mic_header1(
884 unsigned char *mic_header1,
888 mic_header1[0] = (unsigned char)((header_length - 2) / 256);
889 mic_header1[1] = (unsigned char)((header_length - 2) % 256);
890 mic_header1[2] = mpdu[0] & 0xcf; /* Mute CF poll & CF ack bits */
891 mic_header1[3] = mpdu[1] & 0xc7; /* Mute retry, more data and pwr mgt bits */
892 mic_header1[4] = mpdu[4]; /* A1 */
893 mic_header1[5] = mpdu[5];
894 mic_header1[6] = mpdu[6];
895 mic_header1[7] = mpdu[7];
896 mic_header1[8] = mpdu[8];
897 mic_header1[9] = mpdu[9];
898 mic_header1[10] = mpdu[10]; /* A2 */
899 mic_header1[11] = mpdu[11];
900 mic_header1[12] = mpdu[12];
901 mic_header1[13] = mpdu[13];
902 mic_header1[14] = mpdu[14];
903 mic_header1[15] = mpdu[15];
906 /************************************************/
907 /* construct_mic_header2() */
908 /* Builds the last MIC header block from */
910 /************************************************/
912 void construct_mic_header2(
913 unsigned char *mic_header2,
920 for (i = 0; i<16; i++) mic_header2[i]=0x00;
922 mic_header2[0] = mpdu[16]; /* A3 */
923 mic_header2[1] = mpdu[17];
924 mic_header2[2] = mpdu[18];
925 mic_header2[3] = mpdu[19];
926 mic_header2[4] = mpdu[20];
927 mic_header2[5] = mpdu[21];
929 // In Sequence Control field, mute sequence numer bits (12-bit)
930 mic_header2[6] = mpdu[22] & 0x0f; /* SC */
931 mic_header2[7] = 0x00; /* mpdu[23]; */
933 if ((!qc_exists) & a4_exists)
935 for (i=0;i<6;i++) mic_header2[8+i] = mpdu[24+i]; /* A4 */
939 if (qc_exists && (!a4_exists))
941 mic_header2[8] = mpdu[24] & 0x0f; /* mute bits 15 - 4 */
942 mic_header2[9] = mpdu[25] & 0x00;
945 if (qc_exists && a4_exists)
947 for (i=0;i<6;i++) mic_header2[8+i] = mpdu[24+i]; /* A4 */
949 mic_header2[14] = mpdu[30] & 0x0f;
950 mic_header2[15] = mpdu[31] & 0x00;
955 /************************************************/
956 /* construct_mic_iv() */
957 /* Builds the MIC IV from header fields and PN */
958 /************************************************/
960 void construct_mic_iv(
961 unsigned char *mic_iv,
965 unsigned int payload_length,
966 unsigned char *pn_vector)
971 if (qc_exists && a4_exists)
972 mic_iv[1] = mpdu[30] & 0x0f; /* QoS_TC */
973 if (qc_exists && !a4_exists)
974 mic_iv[1] = mpdu[24] & 0x0f; /* mute bits 7-4 */
977 for (i = 2; i < 8; i++)
978 mic_iv[i] = mpdu[i + 8]; /* mic_iv[2:7] = A2[0:5] = mpdu[10:15] */
979 #ifdef CONSISTENT_PN_ORDER
980 for (i = 8; i < 14; i++)
981 mic_iv[i] = pn_vector[i - 8]; /* mic_iv[8:13] = PN[0:5] */
983 for (i = 8; i < 14; i++)
984 mic_iv[i] = pn_vector[13 - i]; /* mic_iv[8:13] = PN[5:0] */
986 i = (payload_length / 256);
987 i = (payload_length % 256);
988 mic_iv[14] = (unsigned char) (payload_length / 256);
989 mic_iv[15] = (unsigned char) (payload_length % 256);
995 /************************************/
997 /* A 128 bit, bitwise exclusive or */
998 /************************************/
1000 void bitwise_xor(unsigned char *ina, unsigned char *inb, unsigned char *out)
1003 for (i=0; i<16; i++)
1005 out[i] = ina[i] ^ inb[i];
1010 void aes128k128d(unsigned char *key, unsigned char *data, unsigned char *ciphertext)
1014 unsigned char intermediatea[16];
1015 unsigned char intermediateb[16];
1016 unsigned char round_key[16];
1018 for(i=0; i<16; i++) round_key[i] = key[i];
1020 for (round = 0; round < 11; round++)
1024 xor_128(round_key, data, ciphertext);
1025 next_key(round_key, round);
1027 else if (round == 10)
1029 byte_sub(ciphertext, intermediatea);
1030 shift_row(intermediatea, intermediateb);
1031 xor_128(intermediateb, round_key, ciphertext);
1035 byte_sub(ciphertext, intermediatea);
1036 shift_row(intermediatea, intermediateb);
1037 mix_column(&intermediateb[0], &intermediatea[0]);
1038 mix_column(&intermediateb[4], &intermediatea[4]);
1039 mix_column(&intermediateb[8], &intermediatea[8]);
1040 mix_column(&intermediateb[12], &intermediatea[12]);
1041 xor_128(intermediatea, round_key, ciphertext);
1042 next_key(round_key, round);
1048 void construct_ctr_preload(
1049 unsigned char *ctr_preload,
1052 unsigned char *mpdu,
1053 unsigned char *pn_vector,
1058 for (i=0; i<16; i++) ctr_preload[i] = 0x00;
1061 ctr_preload[0] = 0x01; /* flag */
1062 if (qc_exists && a4_exists) ctr_preload[1] = mpdu[30] & 0x0f; /* QoC_Control */
1063 if (qc_exists && !a4_exists) ctr_preload[1] = mpdu[24] & 0x0f;
1065 for (i = 2; i < 8; i++)
1066 ctr_preload[i] = mpdu[i + 8]; /* ctr_preload[2:7] = A2[0:5] = mpdu[10:15] */
1067 #ifdef CONSISTENT_PN_ORDER
1068 for (i = 8; i < 14; i++)
1069 ctr_preload[i] = pn_vector[i - 8]; /* ctr_preload[8:13] = PN[0:5] */
1071 for (i = 8; i < 14; i++)
1072 ctr_preload[i] = pn_vector[13 - i]; /* ctr_preload[8:13] = PN[5:0] */
1074 ctr_preload[14] = (unsigned char) (c / 256); // Ctr
1075 ctr_preload[15] = (unsigned char) (c % 256);
1082 // FALSE: Decrypt Error!
1084 BOOLEAN RTMPSoftDecryptTKIP(
1085 IN PRTMP_ADAPTER pAd,
1087 IN ULONG DataByteCnt,
1088 IN UCHAR UserPriority,
1089 IN PCIPHER_KEY pWpaKey)
1105 UCHAR TA[MAC_ADDR_LEN];
1106 UCHAR DA[MAC_ADDR_LEN];
1107 UCHAR SA[MAC_ADDR_LEN];
1109 UINT p1k[5]; //for mix_key;
1110 ULONG pnl;/* Least significant 16 bits of PN */
1111 ULONG pnh;/* Most significant 32 bits of PN */
1113 UINT payload_remainder;
1114 ARCFOURCONTEXT ArcFourContext;
1120 #ifdef RT_BIG_ENDIAN
1121 RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
1127 fc = *((PUSHORT)pData);
1129 frame_type = ((fc0 >> 2) & 0x03);
1130 frame_subtype = ((fc0 >> 4) & 0x0f);
1132 from_ds = (fc1 & 0x2) >> 1;
1133 to_ds = (fc1 & 0x1);
1135 a4_exists = (from_ds & to_ds);
1136 qc_exists = ((frame_subtype == 0x08) || /* Assumed QoS subtypes */
1137 (frame_subtype == 0x09) || /* Likely to change. */
1138 (frame_subtype == 0x0a) ||
1139 (frame_subtype == 0x0b)
1146 KeyID = *((PUCHAR)(pData+ HeaderLen + 3));
1149 if (pWpaKey[KeyID].KeyLen == 0)
1151 DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptTKIP failed!(KeyID[%d] Length can not be 0)\n", KeyID));
1155 duration = *((PUSHORT)(pData+2));
1157 seq_control = *((PUSHORT)(pData+22));
1163 qos_control = *((PUSHORT)(pData+30));
1167 qos_control = *((PUSHORT)(pData+24));
1171 if (to_ds == 0 && from_ds == 1)
1173 NdisMoveMemory(DA, pData+4, MAC_ADDR_LEN);
1174 NdisMoveMemory(SA, pData+16, MAC_ADDR_LEN);
1175 NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN); //BSSID
1177 else if (to_ds == 0 && from_ds == 0 )
1179 NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
1180 NdisMoveMemory(DA, pData+4, MAC_ADDR_LEN);
1181 NdisMoveMemory(SA, pData+10, MAC_ADDR_LEN);
1183 else if (to_ds == 1 && from_ds == 0)
1185 NdisMoveMemory(SA, pData+10, MAC_ADDR_LEN);
1186 NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
1187 NdisMoveMemory(DA, pData+16, MAC_ADDR_LEN);
1189 else if (to_ds == 1 && from_ds == 1)
1191 NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
1192 NdisMoveMemory(DA, pData+16, MAC_ADDR_LEN);
1193 NdisMoveMemory(SA, pData+22, MAC_ADDR_LEN);
1196 num_blocks = (DataByteCnt - 16) / 16;
1197 payload_remainder = (DataByteCnt - 16) % 16;
1199 pnl = (*(pData + HeaderLen)) * 256 + *(pData + HeaderLen + 2);
1200 pnh = *((PULONG)(pData + HeaderLen + 4));
1201 pnh = cpu2le32(pnh);
1202 RTMPTkipMixKey(pWpaKey[KeyID].Key, TA, pnl, pnh, RC4Key, p1k);
1204 ARCFOUR_INIT(&ArcFourContext, RC4Key, 16);
1206 ARCFOUR_DECRYPT(&ArcFourContext, pData + HeaderLen, pData + HeaderLen + 8, DataByteCnt - HeaderLen - 8);
1207 NdisMoveMemory(&trailfcs, pData + DataByteCnt - 8 - 4, 4);
1208 crc32 = RTMP_CALC_FCS32(PPPINITFCS32, pData + HeaderLen, DataByteCnt - HeaderLen - 8 - 4); //Skip IV+EIV 8 bytes & Skip last 4 bytes(FCS).
1209 crc32 ^= 0xffffffff; /* complement */
1211 if(crc32 != cpu2le32(trailfcs))
1213 DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptTKIP, WEP Data ICV Error !\n")); //ICV error.
1218 NdisMoveMemory(TrailMIC, pData + DataByteCnt - 8 - 8 - 4, 8);
1219 RTMPInitMICEngine(pAd, pWpaKey[KeyID].Key, DA, SA, UserPriority, pWpaKey[KeyID].RxMic);
1220 RTMPTkipAppend(&pAd->PrivateInfo.Tx, pData + HeaderLen, DataByteCnt - HeaderLen - 8 - 12);
1221 RTMPTkipGetMIC(&pAd->PrivateInfo.Tx);
1222 NdisMoveMemory(MIC, pAd->PrivateInfo.Tx.MIC, 8);
1224 if (!NdisEqualMemory(MIC, TrailMIC, 8))
1226 DBGPRINT(RT_DEBUG_ERROR, ("RTMPSoftDecryptTKIP, WEP Data MIC Error !\n")); //MIC error.
1227 //RTMPReportMicError(pAd, &pWpaKey[KeyID]); // marked by AlbertY @ 20060630
1231 #ifdef RT_BIG_ENDIAN
1232 RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
1234 //DBGPRINT(RT_DEBUG_TRACE, "RTMPSoftDecryptTKIP Decript done!!\n");
1241 BOOLEAN RTMPSoftDecryptAES(
1242 IN PRTMP_ADAPTER pAd,
1244 IN ULONG DataByteCnt,
1245 IN PCIPHER_KEY pWpaKey)
1252 UINT payload_remainder;
1265 UCHAR ctr_preload[16];
1266 UCHAR chain_buffer[16];
1267 UCHAR padded_buffer[16];
1269 UCHAR mic_header1[16];
1270 UCHAR mic_header2[16];
1274 #ifdef RT_BIG_ENDIAN
1275 RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
1281 fc = *((PUSHORT)pData);
1283 frame_type = ((fc0 >> 2) & 0x03);
1284 frame_subtype = ((fc0 >> 4) & 0x0f);
1286 from_ds = (fc1 & 0x2) >> 1;
1287 to_ds = (fc1 & 0x1);
1289 a4_exists = (from_ds & to_ds);
1290 qc_exists = ((frame_subtype == 0x08) || /* Assumed QoS subtypes */
1291 (frame_subtype == 0x09) || /* Likely to change. */
1292 (frame_subtype == 0x0a) ||
1293 (frame_subtype == 0x0b)
1300 KeyID = *((PUCHAR)(pData+ HeaderLen + 3));
1303 if (pWpaKey[KeyID].KeyLen == 0)
1305 DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptAES failed!(KeyID[%d] Length can not be 0)\n", KeyID));
1309 PN[0] = *(pData+ HeaderLen);
1310 PN[1] = *(pData+ HeaderLen + 1);
1311 PN[2] = *(pData+ HeaderLen + 4);
1312 PN[3] = *(pData+ HeaderLen + 5);
1313 PN[4] = *(pData+ HeaderLen + 6);
1314 PN[5] = *(pData+ HeaderLen + 7);
1316 payload_len = DataByteCnt - HeaderLen - 8 - 8; // 8 bytes for CCMP header , 8 bytes for MIC
1317 payload_remainder = (payload_len) % 16;
1318 num_blocks = (payload_len) / 16;
1322 // Find start of payload
1323 payload_index = HeaderLen + 8; //IV+EIV
1325 for (i=0; i< num_blocks; i++)
1327 construct_ctr_preload(ctr_preload,
1334 aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);
1336 bitwise_xor(aes_out, pData + payload_index, chain_buffer);
1337 NdisMoveMemory(pData + payload_index - 8, chain_buffer, 16);
1338 payload_index += 16;
1342 // If there is a short final block, then pad it
1343 // encrypt it and copy the unpadded part back
1345 if (payload_remainder > 0)
1347 construct_ctr_preload(ctr_preload,
1354 NdisZeroMemory(padded_buffer, 16);
1355 NdisMoveMemory(padded_buffer, pData + payload_index, payload_remainder);
1357 aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);
1359 bitwise_xor(aes_out, padded_buffer, chain_buffer);
1360 NdisMoveMemory(pData + payload_index - 8, chain_buffer, payload_remainder);
1361 payload_index += payload_remainder;
1367 construct_ctr_preload(ctr_preload,
1373 NdisZeroMemory(padded_buffer, 16);
1374 NdisMoveMemory(padded_buffer, pData + payload_index, 8);
1376 aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);
1378 bitwise_xor(aes_out, padded_buffer, chain_buffer);
1380 NdisMoveMemory(TrailMIC, chain_buffer, 8);
1386 //Force the protected frame bit on
1387 *(pData + 1) = *(pData + 1) | 0x40;
1389 // Find start of payload
1390 // Because the CCMP header has been removed
1391 payload_index = HeaderLen;
1401 construct_mic_header1(
1406 construct_mic_header2(
1412 aes128k128d(pWpaKey[KeyID].Key, mic_iv, aes_out);
1413 bitwise_xor(aes_out, mic_header1, chain_buffer);
1414 aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
1415 bitwise_xor(aes_out, mic_header2, chain_buffer);
1416 aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
1418 // iterate through each 16 byte payload block
1419 for (i = 0; i < num_blocks; i++)
1421 bitwise_xor(aes_out, pData + payload_index, chain_buffer);
1422 payload_index += 16;
1423 aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
1426 // Add on the final payload block if it needs padding
1427 if (payload_remainder > 0)
1429 NdisZeroMemory(padded_buffer, 16);
1430 NdisMoveMemory(padded_buffer, pData + payload_index, payload_remainder);
1432 bitwise_xor(aes_out, padded_buffer, chain_buffer);
1433 aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
1436 // aes_out contains padded mic, discard most significant
1437 // 8 bytes to generate 64 bit MIC
1438 for (i = 0 ; i < 8; i++) MIC[i] = aes_out[i];
1440 if (!NdisEqualMemory(MIC, TrailMIC, 8))
1442 DBGPRINT(RT_DEBUG_ERROR, ("RTMPSoftDecryptAES, MIC Error !\n")); //MIC error.
1446 #ifdef RT_BIG_ENDIAN
1447 RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
1453 /****************************************/
1455 /* Performs a 128 bit AES encrypt with */
1457 /****************************************/
1467 out[i] = a[i] ^ b[i];
1477 UCHAR rcon_table[12] =
1479 0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80,
1480 0x1b, 0x36, 0x36, 0x36
1483 sbox_key[0] = RTMPCkipSbox(key[13]);
1484 sbox_key[1] = RTMPCkipSbox(key[14]);
1485 sbox_key[2] = RTMPCkipSbox(key[15]);
1486 sbox_key[3] = RTMPCkipSbox(key[12]);
1488 rcon = rcon_table[round];
1490 xor_32(&key[0], sbox_key, &key[0]);
1491 key[0] = key[0] ^ rcon;
1493 xor_32(&key[4], &key[0], &key[4]);
1494 xor_32(&key[8], &key[4], &key[8]);
1495 xor_32(&key[12], &key[8], &key[12]);
1507 out[i] = a[i] ^ b[i];
1517 for (i=0; i< 16; i++)
1519 out[i] = RTMPCkipSbox(in[i]);
1526 return SboxTable[(int)a];
1559 UCHAR swap_halfs[4];
1565 for (i=0 ; i<4; i++)
1567 if ((in[i] & 0x80)== 0x80)
1573 swap_halfs[0] = in[2]; /* Swap halfs */
1574 swap_halfs[1] = in[3];
1575 swap_halfs[2] = in[0];
1576 swap_halfs[3] = in[1];
1578 rotl[0] = in[3]; /* Rotate left 8 bits */
1583 andf7[0] = in[0] & 0x7f;
1584 andf7[1] = in[1] & 0x7f;
1585 andf7[2] = in[2] & 0x7f;
1586 andf7[3] = in[3] & 0x7f;
1588 for (i = 3; i>0; i--) /* logical shift left 1 bit */
1590 andf7[i] = andf7[i] << 1;
1591 if ((andf7[i-1] & 0x80) == 0x80)
1593 andf7[i] = (andf7[i] | 0x01);
1596 andf7[0] = andf7[0] << 1;
1597 andf7[0] = andf7[0] & 0xfe;
1599 xor_32(add1b, andf7, add1bf7);
1601 xor_32(in, add1bf7, rotr);
1603 temp[0] = rotr[0]; /* Rotate right 8 bits */
1609 xor_32(add1bf7, rotr, temp);
1610 xor_32(swap_halfs, rotl,tempb);
1611 xor_32(temp, tempb, out);