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_
214 } TKIP_IV, *PTKIP_IV;
218 ========================================================================
221 Convert from UCHAR[] to ULONG in a portable way
224 pMICKey pointer to MIC Key
231 ========================================================================
233 ULONG RTMPTkipGetUInt32(
239 for (i = 0; i < 4; i++)
241 res |= (*pMICKey++) << (8 * i);
248 ========================================================================
251 Convert from ULONG to UCHAR[] in a portable way
254 pDst pointer to destination for convert ULONG to UCHAR[]
255 val the value for convert
260 IRQL = DISPATCH_LEVEL
264 ========================================================================
266 VOID RTMPTkipPutUInt32(
272 for(i = 0; i < 4; i++)
274 *pDst++ = (UCHAR) (val & 0xff);
280 ========================================================================
286 pAd Pointer to our adapter
287 pMICKey pointer to MIC Key
292 IRQL = DISPATCH_LEVEL
296 ========================================================================
298 VOID RTMPTkipSetMICKey(
299 IN PTKIP_KEY_INFO pTkip,
303 pTkip->K0 = RTMPTkipGetUInt32(pMICKey);
304 pTkip->K1 = RTMPTkipGetUInt32(pMICKey + 4);
305 // and reset the message
306 pTkip->L = pTkip->K0;
307 pTkip->R = pTkip->K1;
308 pTkip->nBytesInM = 0;
313 ========================================================================
316 Calculate the MIC Value.
319 pAd Pointer to our adapter
320 uChar Append this uChar
325 IRQL = DISPATCH_LEVEL
329 ========================================================================
331 VOID RTMPTkipAppendByte(
332 IN PTKIP_KEY_INFO pTkip,
335 // Append the byte to our word-sized buffer
336 pTkip->M |= (uChar << (8* pTkip->nBytesInM));
338 // Process the word if it is full.
339 if( pTkip->nBytesInM >= 4 )
341 pTkip->L ^= pTkip->M;
342 pTkip->R ^= ROL32( pTkip->L, 17 );
343 pTkip->L += pTkip->R;
344 pTkip->R ^= ((pTkip->L & 0xff00ff00) >> 8) | ((pTkip->L & 0x00ff00ff) << 8);
345 pTkip->L += pTkip->R;
346 pTkip->R ^= ROL32( pTkip->L, 3 );
347 pTkip->L += pTkip->R;
348 pTkip->R ^= ROR32( pTkip->L, 2 );
349 pTkip->L += pTkip->R;
352 pTkip->nBytesInM = 0;
357 ========================================================================
360 Calculate the MIC Value.
363 pAd Pointer to our adapter
364 pSrc Pointer to source data for Calculate MIC Value
365 Len Indicate the length of the source data
370 IRQL = DISPATCH_LEVEL
374 ========================================================================
377 IN PTKIP_KEY_INFO pTkip,
384 RTMPTkipAppendByte(pTkip, *pSrc++);
390 ========================================================================
396 pAd Pointer to our adapter
401 IRQL = DISPATCH_LEVEL
404 the MIC Value is store in pAd->PrivateInfo.MIC
405 ========================================================================
408 IN PTKIP_KEY_INFO pTkip)
410 // Append the minimum padding
411 RTMPTkipAppendByte(pTkip, 0x5a );
412 RTMPTkipAppendByte(pTkip, 0 );
413 RTMPTkipAppendByte(pTkip, 0 );
414 RTMPTkipAppendByte(pTkip, 0 );
415 RTMPTkipAppendByte(pTkip, 0 );
416 // and then zeroes until the length is a multiple of 4
417 while( pTkip->nBytesInM != 0 )
419 RTMPTkipAppendByte(pTkip, 0 );
421 // The appendByte function has already computed the result.
422 RTMPTkipPutUInt32(pTkip->MIC, pTkip->L);
423 RTMPTkipPutUInt32(pTkip->MIC + 4, pTkip->R);
427 ========================================================================
433 pAd Pointer to our adapter
434 pTKey Pointer to the Temporal Key (TK), TK shall be 128bits.
436 pTA Pointer to transmitter address
437 pMICKey pointer to MIC Key
442 IRQL = DISPATCH_LEVEL
446 ========================================================================
448 VOID RTMPInitTkipEngine(
449 IN PRTMP_ADAPTER pAd,
460 // Prepare 8 bytes TKIP encapsulation for MPDU
461 NdisZeroMemory(&tkipIv, sizeof(TKIP_IV));
462 tkipIv.IV16.field.rc0 = *(pTSC + 1);
463 tkipIv.IV16.field.rc1 = (tkipIv.IV16.field.rc0 | 0x20) & 0x7f;
464 tkipIv.IV16.field.rc2 = *pTSC;
465 tkipIv.IV16.field.CONTROL.field.ExtIV = 1; // 0: non-extended IV, 1: an extended IV
466 tkipIv.IV16.field.CONTROL.field.KeyID = KeyId;
467 NdisMoveMemory(&tkipIv.IV32, (pTSC + 2), 4); // Copy IV
469 *pIV16 = tkipIv.IV16.word;
470 *pIV32 = tkipIv.IV32;
474 ========================================================================
477 Init MIC Value calculation function which include set MIC key &
478 calculate first 16 bytes (DA + SA + priority + 0)
481 pAd Pointer to our adapter
482 pTKey Pointer to the Temporal Key (TK), TK shall be 128bits.
483 pDA Pointer to DA address
484 pSA Pointer to SA address
485 pMICKey pointer to MIC Key
492 ========================================================================
494 VOID RTMPInitMICEngine(
495 IN PRTMP_ADAPTER pAd,
499 IN UCHAR UserPriority,
502 ULONG Priority = UserPriority;
504 // Init MIC value calculation
505 RTMPTkipSetMICKey(&pAd->PrivateInfo.Tx, pMICKey);
507 RTMPTkipAppend(&pAd->PrivateInfo.Tx, pDA, MAC_ADDR_LEN);
509 RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSA, MAC_ADDR_LEN);
510 // Priority + 3 bytes of 0
511 RTMPTkipAppend(&pAd->PrivateInfo.Tx, (PUCHAR)&Priority, 4);
515 ========================================================================
518 Compare MIC value of received MSDU
521 pAd Pointer to our adapter
522 pSrc Pointer to the received Plain text data
523 pDA Pointer to DA address
524 pSA Pointer to SA address
525 pMICKey pointer to MIC Key
526 Len the length of the received plain text data exclude MIC value
529 TRUE MIC value matched
530 FALSE MIC value mismatched
532 IRQL = DISPATCH_LEVEL
536 ========================================================================
538 BOOLEAN RTMPTkipCompareMICValue(
539 IN PRTMP_ADAPTER pAd,
544 IN UCHAR UserPriority,
548 ULONG Priority = UserPriority;
550 // Init MIC value calculation
551 RTMPTkipSetMICKey(&pAd->PrivateInfo.Rx, pMICKey);
553 RTMPTkipAppend(&pAd->PrivateInfo.Rx, pDA, MAC_ADDR_LEN);
555 RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSA, MAC_ADDR_LEN);
556 // Priority + 3 bytes of 0
557 RTMPTkipAppend(&pAd->PrivateInfo.Rx, (PUCHAR)&Priority, 4);
559 // Calculate MIC value from plain text data
560 RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSrc, Len);
562 // Get MIC valude from received frame
563 NdisMoveMemory(OldMic, pSrc + Len, 8);
565 // Get MIC value from decrypted plain data
566 RTMPTkipGetMIC(&pAd->PrivateInfo.Rx);
568 // Move MIC value from MSDU, this steps should move to data path.
569 // Since the MIC value might cross MPDUs.
570 if(!NdisEqualMemory(pAd->PrivateInfo.Rx.MIC, OldMic, 8))
572 DBGPRINT_RAW(RT_DEBUG_ERROR, ("RTMPTkipCompareMICValue(): TKIP MIC Error !\n")); //MIC error.
581 ========================================================================
584 Compare MIC value of received MSDU
587 pAd Pointer to our adapter
589 pSrc Pointer to the received Plain text data
590 pDA Pointer to DA address
591 pSA Pointer to SA address
592 pMICKey pointer to MIC Key
593 Len the length of the received plain text data exclude MIC value
596 TRUE MIC value matched
597 FALSE MIC value mismatched
599 IRQL = DISPATCH_LEVEL
603 ========================================================================
605 BOOLEAN RTMPTkipCompareMICValueWithLLC(
606 IN PRTMP_ADAPTER pAd,
617 // Init MIC value calculation
618 RTMPTkipSetMICKey(&pAd->PrivateInfo.Rx, pMICKey);
620 RTMPTkipAppend(&pAd->PrivateInfo.Rx, pDA, MAC_ADDR_LEN);
622 RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSA, MAC_ADDR_LEN);
623 // Priority + 3 bytes of 0
624 RTMPTkipAppend(&pAd->PrivateInfo.Rx, (PUCHAR)&Priority, 4);
626 // Start with LLC header
627 RTMPTkipAppend(&pAd->PrivateInfo.Rx, pLLC, 8);
629 // Calculate MIC value from plain text data
630 RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSrc, Len);
632 // Get MIC valude from received frame
633 NdisMoveMemory(OldMic, pSrc + Len, 8);
635 // Get MIC value from decrypted plain data
636 RTMPTkipGetMIC(&pAd->PrivateInfo.Rx);
638 // Move MIC value from MSDU, this steps should move to data path.
639 // Since the MIC value might cross MPDUs.
640 if(!NdisEqualMemory(pAd->PrivateInfo.Rx.MIC, OldMic, 8))
642 DBGPRINT_RAW(RT_DEBUG_ERROR, ("RTMPTkipCompareMICValueWithLLC(): TKIP MIC Error !\n")); //MIC error.
650 ========================================================================
653 Copy frame from waiting queue into relative ring buffer and set
654 appropriate ASIC register to kick hardware transmit function
657 pAd Pointer to our adapter
658 PNDIS_PACKET Pointer to Ndis Packet for MIC calculation
659 pEncap Pointer to LLC encap data
660 LenEncap Total encap length, might be 0 which indicates no encap
665 IRQL = DISPATCH_LEVEL
669 ========================================================================
671 VOID RTMPCalculateMICValue(
672 IN PRTMP_ADAPTER pAd,
673 IN PNDIS_PACKET pPacket,
678 PACKET_INFO PacketInfo;
683 UCHAR vlan_offset = 0;
685 RTMP_QueryPacketInfo(pPacket, &PacketInfo, &pSrcBufVA, &SrcBufLen);
687 UserPriority = RTMP_GET_PACKET_UP(pPacket);
690 // determine if this is a vlan packet
691 if (((*(pSrc + 12) << 8) + *(pSrc + 13)) == 0x8100)
707 RTMPTkipAppend(&pAd->PrivateInfo.Tx, pEncap, 6);
709 RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSrc + 12 + vlan_offset, 2);
711 SrcBufLen -= (14 + vlan_offset);
712 pSrc += (14 + vlan_offset);
717 RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSrc, SrcBufLen);
720 break; // No need handle next packet
722 } while (TRUE); // End of copying payload
724 // Compute the final MIC Value
725 RTMPTkipGetMIC(&pAd->PrivateInfo.Tx);
729 /************************************************************/
731 /* Returns a 16 bit value from a 64K entry table. The Table */
732 /* is synthesized from two 256 entry byte wide tables. */
733 /************************************************************/
735 UINT tkip_sbox(UINT index)
741 index_low = (index % 256);
742 index_high = ((index >> 8) % 256);
744 left = Tkip_Sbox_Lower[index_low] + (Tkip_Sbox_Upper[index_low] * 256);
745 right = Tkip_Sbox_Upper[index_high] + (Tkip_Sbox_Lower[index_high] * 256);
747 return (left ^ right);
754 if ((a & 0x01) == 0x01)
756 b = (a >> 1) | 0x8000;
760 b = (a >> 1) & 0x7fff;
769 ULONG pnl, /* Least significant 16 bits of PN */
770 ULONG pnh, /* Most significant 32 bits of PN */
789 tsc0 = (unsigned int)((pnh >> 16) % 65536); /* msb */
790 tsc1 = (unsigned int)(pnh % 65536);
791 tsc2 = (unsigned int)(pnl % 65536); /* lsb */
793 /* Phase 1, step 1 */
796 p1k[2] = (UINT)(ta[0] + (ta[1]*256));
797 p1k[3] = (UINT)(ta[2] + (ta[3]*256));
798 p1k[4] = (UINT)(ta[4] + (ta[5]*256));
800 /* Phase 1, step 2 */
804 p1k[0] = (p1k[0] + tkip_sbox( (p1k[4] ^ ((256*key[1+j]) + key[j])) % 65536 )) % 65536;
805 p1k[1] = (p1k[1] + tkip_sbox( (p1k[0] ^ ((256*key[5+j]) + key[4+j])) % 65536 )) % 65536;
806 p1k[2] = (p1k[2] + tkip_sbox( (p1k[1] ^ ((256*key[9+j]) + key[8+j])) % 65536 )) % 65536;
807 p1k[3] = (p1k[3] + tkip_sbox( (p1k[2] ^ ((256*key[13+j]) + key[12+j])) % 65536 )) % 65536;
808 p1k[4] = (p1k[4] + tkip_sbox( (p1k[3] ^ (((256*key[1+j]) + key[j]))) % 65536 )) % 65536;
809 p1k[4] = (p1k[4] + i) % 65536;
812 /* Phase 2, Step 1 */
818 ppk5 = (p1k[4] + tsc2) % 65536;
821 ppk0 = ppk0 + tkip_sbox( (ppk5 ^ ((256*key[1]) + key[0])) % 65536);
822 ppk1 = ppk1 + tkip_sbox( (ppk0 ^ ((256*key[3]) + key[2])) % 65536);
823 ppk2 = ppk2 + tkip_sbox( (ppk1 ^ ((256*key[5]) + key[4])) % 65536);
824 ppk3 = ppk3 + tkip_sbox( (ppk2 ^ ((256*key[7]) + key[6])) % 65536);
825 ppk4 = ppk4 + tkip_sbox( (ppk3 ^ ((256*key[9]) + key[8])) % 65536);
826 ppk5 = ppk5 + tkip_sbox( (ppk4 ^ ((256*key[11]) + key[10])) % 65536);
828 ppk0 = ppk0 + rotr1(ppk5 ^ ((256*key[13]) + key[12]));
829 ppk1 = ppk1 + rotr1(ppk0 ^ ((256*key[15]) + key[14]));
830 ppk2 = ppk2 + rotr1(ppk1);
831 ppk3 = ppk3 + rotr1(ppk2);
832 ppk4 = ppk4 + rotr1(ppk3);
833 ppk5 = ppk5 + rotr1(ppk4);
835 /* Phase 2, Step 3 */
836 /* Phase 2, Step 3 */
838 tsc0 = (unsigned int)((pnh >> 16) % 65536); /* msb */
839 tsc1 = (unsigned int)(pnh % 65536);
840 tsc2 = (unsigned int)(pnl % 65536); /* lsb */
842 rc4key[0] = (tsc2 >> 8) % 256;
843 rc4key[1] = (((tsc2 >> 8) % 256) | 0x20) & 0x7f;
844 rc4key[2] = tsc2 % 256;
845 rc4key[3] = ((ppk5 ^ ((256*key[1]) + key[0])) >> 1) % 256;
847 rc4key[4] = ppk0 % 256;
848 rc4key[5] = (ppk0 >> 8) % 256;
850 rc4key[6] = ppk1 % 256;
851 rc4key[7] = (ppk1 >> 8) % 256;
853 rc4key[8] = ppk2 % 256;
854 rc4key[9] = (ppk2 >> 8) % 256;
856 rc4key[10] = ppk3 % 256;
857 rc4key[11] = (ppk3 >> 8) % 256;
859 rc4key[12] = ppk4 % 256;
860 rc4key[13] = (ppk4 >> 8) % 256;
862 rc4key[14] = ppk5 % 256;
863 rc4key[15] = (ppk5 >> 8) % 256;
867 /************************************************/
868 /* construct_mic_header1() */
869 /* Builds the first MIC header block from */
871 /************************************************/
873 void construct_mic_header1(
874 unsigned char *mic_header1,
878 mic_header1[0] = (unsigned char)((header_length - 2) / 256);
879 mic_header1[1] = (unsigned char)((header_length - 2) % 256);
880 mic_header1[2] = mpdu[0] & 0xcf; /* Mute CF poll & CF ack bits */
881 mic_header1[3] = mpdu[1] & 0xc7; /* Mute retry, more data and pwr mgt bits */
882 mic_header1[4] = mpdu[4]; /* A1 */
883 mic_header1[5] = mpdu[5];
884 mic_header1[6] = mpdu[6];
885 mic_header1[7] = mpdu[7];
886 mic_header1[8] = mpdu[8];
887 mic_header1[9] = mpdu[9];
888 mic_header1[10] = mpdu[10]; /* A2 */
889 mic_header1[11] = mpdu[11];
890 mic_header1[12] = mpdu[12];
891 mic_header1[13] = mpdu[13];
892 mic_header1[14] = mpdu[14];
893 mic_header1[15] = mpdu[15];
896 /************************************************/
897 /* construct_mic_header2() */
898 /* Builds the last MIC header block from */
900 /************************************************/
902 void construct_mic_header2(
903 unsigned char *mic_header2,
910 for (i = 0; i<16; i++) mic_header2[i]=0x00;
912 mic_header2[0] = mpdu[16]; /* A3 */
913 mic_header2[1] = mpdu[17];
914 mic_header2[2] = mpdu[18];
915 mic_header2[3] = mpdu[19];
916 mic_header2[4] = mpdu[20];
917 mic_header2[5] = mpdu[21];
919 // In Sequence Control field, mute sequence numer bits (12-bit)
920 mic_header2[6] = mpdu[22] & 0x0f; /* SC */
921 mic_header2[7] = 0x00; /* mpdu[23]; */
923 if ((!qc_exists) & a4_exists)
925 for (i=0;i<6;i++) mic_header2[8+i] = mpdu[24+i]; /* A4 */
929 if (qc_exists && (!a4_exists))
931 mic_header2[8] = mpdu[24] & 0x0f; /* mute bits 15 - 4 */
932 mic_header2[9] = mpdu[25] & 0x00;
935 if (qc_exists && a4_exists)
937 for (i=0;i<6;i++) mic_header2[8+i] = mpdu[24+i]; /* A4 */
939 mic_header2[14] = mpdu[30] & 0x0f;
940 mic_header2[15] = mpdu[31] & 0x00;
945 /************************************************/
946 /* construct_mic_iv() */
947 /* Builds the MIC IV from header fields and PN */
948 /************************************************/
950 void construct_mic_iv(
951 unsigned char *mic_iv,
955 unsigned int payload_length,
956 unsigned char *pn_vector)
961 if (qc_exists && a4_exists)
962 mic_iv[1] = mpdu[30] & 0x0f; /* QoS_TC */
963 if (qc_exists && !a4_exists)
964 mic_iv[1] = mpdu[24] & 0x0f; /* mute bits 7-4 */
967 for (i = 2; i < 8; i++)
968 mic_iv[i] = mpdu[i + 8]; /* mic_iv[2:7] = A2[0:5] = mpdu[10:15] */
969 #ifdef CONSISTENT_PN_ORDER
970 for (i = 8; i < 14; i++)
971 mic_iv[i] = pn_vector[i - 8]; /* mic_iv[8:13] = PN[0:5] */
973 for (i = 8; i < 14; i++)
974 mic_iv[i] = pn_vector[13 - i]; /* mic_iv[8:13] = PN[5:0] */
976 i = (payload_length / 256);
977 i = (payload_length % 256);
978 mic_iv[14] = (unsigned char) (payload_length / 256);
979 mic_iv[15] = (unsigned char) (payload_length % 256);
985 /************************************/
987 /* A 128 bit, bitwise exclusive or */
988 /************************************/
990 void bitwise_xor(unsigned char *ina, unsigned char *inb, unsigned char *out)
995 out[i] = ina[i] ^ inb[i];
1000 void aes128k128d(unsigned char *key, unsigned char *data, unsigned char *ciphertext)
1004 unsigned char intermediatea[16];
1005 unsigned char intermediateb[16];
1006 unsigned char round_key[16];
1008 for(i=0; i<16; i++) round_key[i] = key[i];
1010 for (round = 0; round < 11; round++)
1014 xor_128(round_key, data, ciphertext);
1015 next_key(round_key, round);
1017 else if (round == 10)
1019 byte_sub(ciphertext, intermediatea);
1020 shift_row(intermediatea, intermediateb);
1021 xor_128(intermediateb, round_key, ciphertext);
1025 byte_sub(ciphertext, intermediatea);
1026 shift_row(intermediatea, intermediateb);
1027 mix_column(&intermediateb[0], &intermediatea[0]);
1028 mix_column(&intermediateb[4], &intermediatea[4]);
1029 mix_column(&intermediateb[8], &intermediatea[8]);
1030 mix_column(&intermediateb[12], &intermediatea[12]);
1031 xor_128(intermediatea, round_key, ciphertext);
1032 next_key(round_key, round);
1038 void construct_ctr_preload(
1039 unsigned char *ctr_preload,
1042 unsigned char *mpdu,
1043 unsigned char *pn_vector,
1048 for (i=0; i<16; i++) ctr_preload[i] = 0x00;
1051 ctr_preload[0] = 0x01; /* flag */
1052 if (qc_exists && a4_exists) ctr_preload[1] = mpdu[30] & 0x0f; /* QoC_Control */
1053 if (qc_exists && !a4_exists) ctr_preload[1] = mpdu[24] & 0x0f;
1055 for (i = 2; i < 8; i++)
1056 ctr_preload[i] = mpdu[i + 8]; /* ctr_preload[2:7] = A2[0:5] = mpdu[10:15] */
1057 #ifdef CONSISTENT_PN_ORDER
1058 for (i = 8; i < 14; i++)
1059 ctr_preload[i] = pn_vector[i - 8]; /* ctr_preload[8:13] = PN[0:5] */
1061 for (i = 8; i < 14; i++)
1062 ctr_preload[i] = pn_vector[13 - i]; /* ctr_preload[8:13] = PN[5:0] */
1064 ctr_preload[14] = (unsigned char) (c / 256); // Ctr
1065 ctr_preload[15] = (unsigned char) (c % 256);
1072 // FALSE: Decrypt Error!
1074 BOOLEAN RTMPSoftDecryptTKIP(
1075 IN PRTMP_ADAPTER pAd,
1077 IN ULONG DataByteCnt,
1078 IN UCHAR UserPriority,
1079 IN PCIPHER_KEY pWpaKey)
1095 UCHAR TA[MAC_ADDR_LEN];
1096 UCHAR DA[MAC_ADDR_LEN];
1097 UCHAR SA[MAC_ADDR_LEN];
1099 UINT p1k[5]; //for mix_key;
1100 ULONG pnl;/* Least significant 16 bits of PN */
1101 ULONG pnh;/* Most significant 32 bits of PN */
1103 UINT payload_remainder;
1104 ARCFOURCONTEXT ArcFourContext;
1113 fc = *((PUSHORT)pData);
1115 frame_type = ((fc0 >> 2) & 0x03);
1116 frame_subtype = ((fc0 >> 4) & 0x0f);
1118 from_ds = (fc1 & 0x2) >> 1;
1119 to_ds = (fc1 & 0x1);
1121 a4_exists = (from_ds & to_ds);
1122 qc_exists = ((frame_subtype == 0x08) || /* Assumed QoS subtypes */
1123 (frame_subtype == 0x09) || /* Likely to change. */
1124 (frame_subtype == 0x0a) ||
1125 (frame_subtype == 0x0b)
1132 KeyID = *((PUCHAR)(pData+ HeaderLen + 3));
1135 if (pWpaKey[KeyID].KeyLen == 0)
1137 DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptTKIP failed!(KeyID[%d] Length can not be 0)\n", KeyID));
1141 duration = *((PUSHORT)(pData+2));
1143 seq_control = *((PUSHORT)(pData+22));
1149 qos_control = *((PUSHORT)(pData+30));
1153 qos_control = *((PUSHORT)(pData+24));
1157 if (to_ds == 0 && from_ds == 1)
1159 NdisMoveMemory(DA, pData+4, MAC_ADDR_LEN);
1160 NdisMoveMemory(SA, pData+16, MAC_ADDR_LEN);
1161 NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN); //BSSID
1163 else if (to_ds == 0 && from_ds == 0 )
1165 NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
1166 NdisMoveMemory(DA, pData+4, MAC_ADDR_LEN);
1167 NdisMoveMemory(SA, pData+10, MAC_ADDR_LEN);
1169 else if (to_ds == 1 && from_ds == 0)
1171 NdisMoveMemory(SA, pData+10, MAC_ADDR_LEN);
1172 NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
1173 NdisMoveMemory(DA, pData+16, MAC_ADDR_LEN);
1175 else if (to_ds == 1 && from_ds == 1)
1177 NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
1178 NdisMoveMemory(DA, pData+16, MAC_ADDR_LEN);
1179 NdisMoveMemory(SA, pData+22, MAC_ADDR_LEN);
1182 num_blocks = (DataByteCnt - 16) / 16;
1183 payload_remainder = (DataByteCnt - 16) % 16;
1185 pnl = (*(pData + HeaderLen)) * 256 + *(pData + HeaderLen + 2);
1186 pnh = *((PULONG)(pData + HeaderLen + 4));
1187 pnh = cpu2le32(pnh);
1188 RTMPTkipMixKey(pWpaKey[KeyID].Key, TA, pnl, pnh, RC4Key, p1k);
1190 ARCFOUR_INIT(&ArcFourContext, RC4Key, 16);
1192 ARCFOUR_DECRYPT(&ArcFourContext, pData + HeaderLen, pData + HeaderLen + 8, DataByteCnt - HeaderLen - 8);
1193 NdisMoveMemory(&trailfcs, pData + DataByteCnt - 8 - 4, 4);
1194 crc32 = RTMP_CALC_FCS32(PPPINITFCS32, pData + HeaderLen, DataByteCnt - HeaderLen - 8 - 4); //Skip IV+EIV 8 bytes & Skip last 4 bytes(FCS).
1195 crc32 ^= 0xffffffff; /* complement */
1197 if(crc32 != cpu2le32(trailfcs))
1199 DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptTKIP, WEP Data ICV Error !\n")); //ICV error.
1204 NdisMoveMemory(TrailMIC, pData + DataByteCnt - 8 - 8 - 4, 8);
1205 RTMPInitMICEngine(pAd, pWpaKey[KeyID].Key, DA, SA, UserPriority, pWpaKey[KeyID].RxMic);
1206 RTMPTkipAppend(&pAd->PrivateInfo.Tx, pData + HeaderLen, DataByteCnt - HeaderLen - 8 - 12);
1207 RTMPTkipGetMIC(&pAd->PrivateInfo.Tx);
1208 NdisMoveMemory(MIC, pAd->PrivateInfo.Tx.MIC, 8);
1210 if (!NdisEqualMemory(MIC, TrailMIC, 8))
1212 DBGPRINT(RT_DEBUG_ERROR, ("RTMPSoftDecryptTKIP, WEP Data MIC Error !\n")); //MIC error.
1222 BOOLEAN RTMPSoftDecryptAES(
1223 IN PRTMP_ADAPTER pAd,
1225 IN ULONG DataByteCnt,
1226 IN PCIPHER_KEY pWpaKey)
1233 UINT payload_remainder;
1246 UCHAR ctr_preload[16];
1247 UCHAR chain_buffer[16];
1248 UCHAR padded_buffer[16];
1250 UCHAR mic_header1[16];
1251 UCHAR mic_header2[16];
1258 fc = *((PUSHORT)pData);
1260 frame_type = ((fc0 >> 2) & 0x03);
1261 frame_subtype = ((fc0 >> 4) & 0x0f);
1263 from_ds = (fc1 & 0x2) >> 1;
1264 to_ds = (fc1 & 0x1);
1266 a4_exists = (from_ds & to_ds);
1267 qc_exists = ((frame_subtype == 0x08) || /* Assumed QoS subtypes */
1268 (frame_subtype == 0x09) || /* Likely to change. */
1269 (frame_subtype == 0x0a) ||
1270 (frame_subtype == 0x0b)
1277 KeyID = *((PUCHAR)(pData+ HeaderLen + 3));
1280 if (pWpaKey[KeyID].KeyLen == 0)
1282 DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptAES failed!(KeyID[%d] Length can not be 0)\n", KeyID));
1286 PN[0] = *(pData+ HeaderLen);
1287 PN[1] = *(pData+ HeaderLen + 1);
1288 PN[2] = *(pData+ HeaderLen + 4);
1289 PN[3] = *(pData+ HeaderLen + 5);
1290 PN[4] = *(pData+ HeaderLen + 6);
1291 PN[5] = *(pData+ HeaderLen + 7);
1293 payload_len = DataByteCnt - HeaderLen - 8 - 8; // 8 bytes for CCMP header , 8 bytes for MIC
1294 payload_remainder = (payload_len) % 16;
1295 num_blocks = (payload_len) / 16;
1299 // Find start of payload
1300 payload_index = HeaderLen + 8; //IV+EIV
1302 for (i=0; i< num_blocks; i++)
1304 construct_ctr_preload(ctr_preload,
1311 aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);
1313 bitwise_xor(aes_out, pData + payload_index, chain_buffer);
1314 NdisMoveMemory(pData + payload_index - 8, chain_buffer, 16);
1315 payload_index += 16;
1319 // If there is a short final block, then pad it
1320 // encrypt it and copy the unpadded part back
1322 if (payload_remainder > 0)
1324 construct_ctr_preload(ctr_preload,
1331 NdisZeroMemory(padded_buffer, 16);
1332 NdisMoveMemory(padded_buffer, pData + payload_index, payload_remainder);
1334 aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);
1336 bitwise_xor(aes_out, padded_buffer, chain_buffer);
1337 NdisMoveMemory(pData + payload_index - 8, chain_buffer, payload_remainder);
1338 payload_index += payload_remainder;
1344 construct_ctr_preload(ctr_preload,
1350 NdisZeroMemory(padded_buffer, 16);
1351 NdisMoveMemory(padded_buffer, pData + payload_index, 8);
1353 aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);
1355 bitwise_xor(aes_out, padded_buffer, chain_buffer);
1357 NdisMoveMemory(TrailMIC, chain_buffer, 8);
1363 //Force the protected frame bit on
1364 *(pData + 1) = *(pData + 1) | 0x40;
1366 // Find start of payload
1367 // Because the CCMP header has been removed
1368 payload_index = HeaderLen;
1378 construct_mic_header1(
1383 construct_mic_header2(
1389 aes128k128d(pWpaKey[KeyID].Key, mic_iv, aes_out);
1390 bitwise_xor(aes_out, mic_header1, chain_buffer);
1391 aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
1392 bitwise_xor(aes_out, mic_header2, chain_buffer);
1393 aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
1395 // iterate through each 16 byte payload block
1396 for (i = 0; i < num_blocks; i++)
1398 bitwise_xor(aes_out, pData + payload_index, chain_buffer);
1399 payload_index += 16;
1400 aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
1403 // Add on the final payload block if it needs padding
1404 if (payload_remainder > 0)
1406 NdisZeroMemory(padded_buffer, 16);
1407 NdisMoveMemory(padded_buffer, pData + payload_index, payload_remainder);
1409 bitwise_xor(aes_out, padded_buffer, chain_buffer);
1410 aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
1413 // aes_out contains padded mic, discard most significant
1414 // 8 bytes to generate 64 bit MIC
1415 for (i = 0 ; i < 8; i++) MIC[i] = aes_out[i];
1417 if (!NdisEqualMemory(MIC, TrailMIC, 8))
1419 DBGPRINT(RT_DEBUG_ERROR, ("RTMPSoftDecryptAES, MIC Error !\n")); //MIC error.
1426 /****************************************/
1428 /* Performs a 128 bit AES encrypt with */
1430 /****************************************/
1440 out[i] = a[i] ^ b[i];
1450 UCHAR rcon_table[12] =
1452 0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80,
1453 0x1b, 0x36, 0x36, 0x36
1456 sbox_key[0] = RTMPCkipSbox(key[13]);
1457 sbox_key[1] = RTMPCkipSbox(key[14]);
1458 sbox_key[2] = RTMPCkipSbox(key[15]);
1459 sbox_key[3] = RTMPCkipSbox(key[12]);
1461 rcon = rcon_table[round];
1463 xor_32(&key[0], sbox_key, &key[0]);
1464 key[0] = key[0] ^ rcon;
1466 xor_32(&key[4], &key[0], &key[4]);
1467 xor_32(&key[8], &key[4], &key[8]);
1468 xor_32(&key[12], &key[8], &key[12]);
1480 out[i] = a[i] ^ b[i];
1490 for (i=0; i< 16; i++)
1492 out[i] = RTMPCkipSbox(in[i]);
1499 return SboxTable[(int)a];
1532 UCHAR swap_halfs[4];
1538 for (i=0 ; i<4; i++)
1540 if ((in[i] & 0x80)== 0x80)
1546 swap_halfs[0] = in[2]; /* Swap halfs */
1547 swap_halfs[1] = in[3];
1548 swap_halfs[2] = in[0];
1549 swap_halfs[3] = in[1];
1551 rotl[0] = in[3]; /* Rotate left 8 bits */
1556 andf7[0] = in[0] & 0x7f;
1557 andf7[1] = in[1] & 0x7f;
1558 andf7[2] = in[2] & 0x7f;
1559 andf7[3] = in[3] & 0x7f;
1561 for (i = 3; i>0; i--) /* logical shift left 1 bit */
1563 andf7[i] = andf7[i] << 1;
1564 if ((andf7[i-1] & 0x80) == 0x80)
1566 andf7[i] = (andf7[i] | 0x01);
1569 andf7[0] = andf7[0] << 1;
1570 andf7[0] = andf7[0] & 0xfe;
1572 xor_32(add1b, andf7, add1bf7);
1574 xor_32(in, add1bf7, rotr);
1576 temp[0] = rotr[0]; /* Rotate right 8 bits */
1582 xor_32(add1bf7, rotr, temp);
1583 xor_32(swap_halfs, rotl,tempb);
1584 xor_32(temp, tempb, out);