1 /*
2 *************************************************************************
3 * Ralink Tech Inc.
4 * 5F., No.36, Taiyuan St., Jhubei City,
5 * Hsinchu County 302,
6 * Taiwan, R.O.C.
7 *
8 * (c) Copyright 2002-2007, Ralink Technology, Inc.
9 *
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. *
14 * *
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. *
19 * *
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. *
24 * *
25 *************************************************************************
26
27 Module Name:
28 rtmp_tkip.c
29
30 Abstract:
31
32 Revision History:
33 Who When What
34 -------- ---------- ----------------------------------------------
35 Paul Wu 02-25-02 Initial
36 */
37
38 #include "../rt_config.h"
39
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) )
44
45 UINT Tkip_Sbox_Lower[256] =
46 {
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
79 };
80
81 UINT Tkip_Sbox_Upper[256] =
82 {
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
115 };
116
117 /*****************************/
118 /******** SBOX Table *********/
119 /*****************************/
120
121 UCHAR SboxTable[256] =
122 {
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
155 };
156
157 VOID xor_32(
158 IN PUCHAR a,
159 IN PUCHAR b,
160 OUT PUCHAR out);
161
162 VOID xor_128(
163 IN PUCHAR a,
164 IN PUCHAR b,
165 OUT PUCHAR out);
166
167 VOID next_key(
168 IN PUCHAR key,
169 IN INT round);
170
171 VOID byte_sub(
172 IN PUCHAR in,
173 OUT PUCHAR out);
174
175 VOID shift_row(
176 IN PUCHAR in,
177 OUT PUCHAR out);
178
179 VOID mix_column(
180 IN PUCHAR in,
181 OUT PUCHAR out);
182
183 UCHAR RTMPCkipSbox(
184 IN UCHAR a);
185 //
186 // Expanded IV for TKIP function.
187 //
188 typedef struct PACKED _IV_CONTROL_
189 {
190 union PACKED
191 {
192 struct PACKED
193 {
194 UCHAR rc0;
195 UCHAR rc1;
196 UCHAR rc2;
197
198 union PACKED
199 {
200 struct PACKED
201 {
202 #ifdef RT_BIG_ENDIAN
203 UCHAR KeyID:2;
204 UCHAR ExtIV:1;
205 UCHAR Rsvd:5;
206 #else
207 UCHAR Rsvd:5;
208 UCHAR ExtIV:1;
209 UCHAR KeyID:2;
210 #endif
211 } field;
212 UCHAR Byte;
213 } CONTROL;
214 } field;
215
216 ULONG word;
217 } IV16;
218
219 ULONG IV32;
220 } TKIP_IV, *PTKIP_IV;
221
222
223 /*
224 ========================================================================
225
226 Routine Description:
227 Convert from UCHAR[] to ULONG in a portable way
228
229 Arguments:
230 pMICKey pointer to MIC Key
231
232 Return Value:
233 None
234
235 Note:
236
237 ========================================================================
238 */
RTMPTkipGetUInt32(IN PUCHAR pMICKey)239 ULONG RTMPTkipGetUInt32(
240 IN PUCHAR pMICKey)
241 {
242 ULONG res = 0;
243 INT i;
244
245 for (i = 0; i < 4; i++)
246 {
247 res |= (*pMICKey++) << (8 * i);
248 }
249
250 return res;
251 }
252
253 /*
254 ========================================================================
255
256 Routine Description:
257 Convert from ULONG to UCHAR[] in a portable way
258
259 Arguments:
260 pDst pointer to destination for convert ULONG to UCHAR[]
261 val the value for convert
262
263 Return Value:
264 None
265
266 IRQL = DISPATCH_LEVEL
267
268 Note:
269
270 ========================================================================
271 */
RTMPTkipPutUInt32(IN OUT PUCHAR pDst,IN ULONG val)272 VOID RTMPTkipPutUInt32(
273 IN OUT PUCHAR pDst,
274 IN ULONG val)
275 {
276 INT i;
277
278 for(i = 0; i < 4; i++)
279 {
280 *pDst++ = (UCHAR) (val & 0xff);
281 val >>= 8;
282 }
283 }
284
285 /*
286 ========================================================================
287
288 Routine Description:
289 Set the MIC Key.
290
291 Arguments:
292 pAd Pointer to our adapter
293 pMICKey pointer to MIC Key
294
295 Return Value:
296 None
297
298 IRQL = DISPATCH_LEVEL
299
300 Note:
301
302 ========================================================================
303 */
RTMPTkipSetMICKey(IN PTKIP_KEY_INFO pTkip,IN PUCHAR pMICKey)304 VOID RTMPTkipSetMICKey(
305 IN PTKIP_KEY_INFO pTkip,
306 IN PUCHAR pMICKey)
307 {
308 // Set the key
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;
315 pTkip->M = 0;
316 }
317
318 /*
319 ========================================================================
320
321 Routine Description:
322 Calculate the MIC Value.
323
324 Arguments:
325 pAd Pointer to our adapter
326 uChar Append this uChar
327
328 Return Value:
329 None
330
331 IRQL = DISPATCH_LEVEL
332
333 Note:
334
335 ========================================================================
336 */
RTMPTkipAppendByte(IN PTKIP_KEY_INFO pTkip,IN UCHAR uChar)337 VOID RTMPTkipAppendByte(
338 IN PTKIP_KEY_INFO pTkip,
339 IN UCHAR uChar)
340 {
341 // Append the byte to our word-sized buffer
342 pTkip->M |= (uChar << (8* pTkip->nBytesInM));
343 pTkip->nBytesInM++;
344 // Process the word if it is full.
345 if( pTkip->nBytesInM >= 4 )
346 {
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;
356 // Clear the buffer
357 pTkip->M = 0;
358 pTkip->nBytesInM = 0;
359 }
360 }
361
362 /*
363 ========================================================================
364
365 Routine Description:
366 Calculate the MIC Value.
367
368 Arguments:
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
372
373 Return Value:
374 None
375
376 IRQL = DISPATCH_LEVEL
377
378 Note:
379
380 ========================================================================
381 */
RTMPTkipAppend(IN PTKIP_KEY_INFO pTkip,IN PUCHAR pSrc,IN UINT nBytes)382 VOID RTMPTkipAppend(
383 IN PTKIP_KEY_INFO pTkip,
384 IN PUCHAR pSrc,
385 IN UINT nBytes)
386 {
387 // This is simple
388 while(nBytes > 0)
389 {
390 RTMPTkipAppendByte(pTkip, *pSrc++);
391 nBytes--;
392 }
393 }
394
395 /*
396 ========================================================================
397
398 Routine Description:
399 Get the MIC Value.
400
401 Arguments:
402 pAd Pointer to our adapter
403
404 Return Value:
405 None
406
407 IRQL = DISPATCH_LEVEL
408
409 Note:
410 the MIC Value is store in pAd->PrivateInfo.MIC
411 ========================================================================
412 */
RTMPTkipGetMIC(IN PTKIP_KEY_INFO pTkip)413 VOID RTMPTkipGetMIC(
414 IN PTKIP_KEY_INFO pTkip)
415 {
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 )
424 {
425 RTMPTkipAppendByte(pTkip, 0 );
426 }
427 // The appendByte function has already computed the result.
428 RTMPTkipPutUInt32(pTkip->MIC, pTkip->L);
429 RTMPTkipPutUInt32(pTkip->MIC + 4, pTkip->R);
430 }
431
432 /*
433 ========================================================================
434
435 Routine Description:
436 Init Tkip function.
437
438 Arguments:
439 pAd Pointer to our adapter
440 pTKey Pointer to the Temporal Key (TK), TK shall be 128bits.
441 KeyId TK Key ID
442 pTA Pointer to transmitter address
443 pMICKey pointer to MIC Key
444
445 Return Value:
446 None
447
448 IRQL = DISPATCH_LEVEL
449
450 Note:
451
452 ========================================================================
453 */
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)454 VOID RTMPInitTkipEngine(
455 IN PRTMP_ADAPTER pAd,
456 IN PUCHAR pKey,
457 IN UCHAR KeyId,
458 IN PUCHAR pTA,
459 IN PUCHAR pMICKey,
460 IN PUCHAR pTSC,
461 OUT PULONG pIV16,
462 OUT PULONG pIV32)
463 {
464 TKIP_IV tkipIv;
465
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 NdisMoveMemory(&tkipIv.IV32, (pTSC + 2), 4); // Copy IV
474
475 *pIV16 = tkipIv.IV16.word;
476 *pIV32 = tkipIv.IV32;
477 }
478
479 /*
480 ========================================================================
481
482 Routine Description:
483 Init MIC Value calculation function which include set MIC key &
484 calculate first 16 bytes (DA + SA + priority + 0)
485
486 Arguments:
487 pAd Pointer to our adapter
488 pTKey Pointer to the Temporal Key (TK), TK shall be 128bits.
489 pDA Pointer to DA address
490 pSA Pointer to SA address
491 pMICKey pointer to MIC Key
492
493 Return Value:
494 None
495
496 Note:
497
498 ========================================================================
499 */
RTMPInitMICEngine(IN PRTMP_ADAPTER pAd,IN PUCHAR pKey,IN PUCHAR pDA,IN PUCHAR pSA,IN UCHAR UserPriority,IN PUCHAR pMICKey)500 VOID RTMPInitMICEngine(
501 IN PRTMP_ADAPTER pAd,
502 IN PUCHAR pKey,
503 IN PUCHAR pDA,
504 IN PUCHAR pSA,
505 IN UCHAR UserPriority,
506 IN PUCHAR pMICKey)
507 {
508 ULONG Priority = UserPriority;
509
510 // Init MIC value calculation
511 RTMPTkipSetMICKey(&pAd->PrivateInfo.Tx, pMICKey);
512 // DA
513 RTMPTkipAppend(&pAd->PrivateInfo.Tx, pDA, MAC_ADDR_LEN);
514 // SA
515 RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSA, MAC_ADDR_LEN);
516 // Priority + 3 bytes of 0
517 RTMPTkipAppend(&pAd->PrivateInfo.Tx, (PUCHAR)&Priority, 4);
518 }
519
520 /*
521 ========================================================================
522
523 Routine Description:
524 Compare MIC value of received MSDU
525
526 Arguments:
527 pAd Pointer to our adapter
528 pSrc Pointer to the received Plain text data
529 pDA Pointer to DA address
530 pSA Pointer to SA address
531 pMICKey pointer to MIC Key
532 Len the length of the received plain text data exclude MIC value
533
534 Return Value:
535 TRUE MIC value matched
536 FALSE MIC value mismatched
537
538 IRQL = DISPATCH_LEVEL
539
540 Note:
541
542 ========================================================================
543 */
RTMPTkipCompareMICValue(IN PRTMP_ADAPTER pAd,IN PUCHAR pSrc,IN PUCHAR pDA,IN PUCHAR pSA,IN PUCHAR pMICKey,IN UCHAR UserPriority,IN UINT Len)544 BOOLEAN RTMPTkipCompareMICValue(
545 IN PRTMP_ADAPTER pAd,
546 IN PUCHAR pSrc,
547 IN PUCHAR pDA,
548 IN PUCHAR pSA,
549 IN PUCHAR pMICKey,
550 IN UCHAR UserPriority,
551 IN UINT Len)
552 {
553 UCHAR OldMic[8];
554 ULONG Priority = UserPriority;
555
556 // Init MIC value calculation
557 RTMPTkipSetMICKey(&pAd->PrivateInfo.Rx, pMICKey);
558 // DA
559 RTMPTkipAppend(&pAd->PrivateInfo.Rx, pDA, MAC_ADDR_LEN);
560 // SA
561 RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSA, MAC_ADDR_LEN);
562 // Priority + 3 bytes of 0
563 RTMPTkipAppend(&pAd->PrivateInfo.Rx, (PUCHAR)&Priority, 4);
564
565 // Calculate MIC value from plain text data
566 RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSrc, Len);
567
568 // Get MIC valude from received frame
569 NdisMoveMemory(OldMic, pSrc + Len, 8);
570
571 // Get MIC value from decrypted plain data
572 RTMPTkipGetMIC(&pAd->PrivateInfo.Rx);
573
574 // Move MIC value from MSDU, this steps should move to data path.
575 // Since the MIC value might cross MPDUs.
576 if(!NdisEqualMemory(pAd->PrivateInfo.Rx.MIC, OldMic, 8))
577 {
578 DBGPRINT_RAW(RT_DEBUG_ERROR, ("RTMPTkipCompareMICValue(): TKIP MIC Error !\n")); //MIC error.
579
580
581 return (FALSE);
582 }
583 return (TRUE);
584 }
585
586 /*
587 ========================================================================
588
589 Routine Description:
590 Compare MIC value of received MSDU
591
592 Arguments:
593 pAd Pointer to our adapter
594 pLLC LLC header
595 pSrc Pointer to the received Plain text data
596 pDA Pointer to DA address
597 pSA Pointer to SA address
598 pMICKey pointer to MIC Key
599 Len the length of the received plain text data exclude MIC value
600
601 Return Value:
602 TRUE MIC value matched
603 FALSE MIC value mismatched
604
605 IRQL = DISPATCH_LEVEL
606
607 Note:
608
609 ========================================================================
610 */
RTMPTkipCompareMICValueWithLLC(IN PRTMP_ADAPTER pAd,IN PUCHAR pLLC,IN PUCHAR pSrc,IN PUCHAR pDA,IN PUCHAR pSA,IN PUCHAR pMICKey,IN UINT Len)611 BOOLEAN RTMPTkipCompareMICValueWithLLC(
612 IN PRTMP_ADAPTER pAd,
613 IN PUCHAR pLLC,
614 IN PUCHAR pSrc,
615 IN PUCHAR pDA,
616 IN PUCHAR pSA,
617 IN PUCHAR pMICKey,
618 IN UINT Len)
619 {
620 UCHAR OldMic[8];
621 ULONG Priority = 0;
622
623 // Init MIC value calculation
624 RTMPTkipSetMICKey(&pAd->PrivateInfo.Rx, pMICKey);
625 // DA
626 RTMPTkipAppend(&pAd->PrivateInfo.Rx, pDA, MAC_ADDR_LEN);
627 // SA
628 RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSA, MAC_ADDR_LEN);
629 // Priority + 3 bytes of 0
630 RTMPTkipAppend(&pAd->PrivateInfo.Rx, (PUCHAR)&Priority, 4);
631
632 // Start with LLC header
633 RTMPTkipAppend(&pAd->PrivateInfo.Rx, pLLC, 8);
634
635 // Calculate MIC value from plain text data
636 RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSrc, Len);
637
638 // Get MIC valude from received frame
639 NdisMoveMemory(OldMic, pSrc + Len, 8);
640
641 // Get MIC value from decrypted plain data
642 RTMPTkipGetMIC(&pAd->PrivateInfo.Rx);
643
644 // Move MIC value from MSDU, this steps should move to data path.
645 // Since the MIC value might cross MPDUs.
646 if(!NdisEqualMemory(pAd->PrivateInfo.Rx.MIC, OldMic, 8))
647 {
648 DBGPRINT_RAW(RT_DEBUG_ERROR, ("RTMPTkipCompareMICValueWithLLC(): TKIP MIC Error !\n")); //MIC error.
649
650
651 return (FALSE);
652 }
653 return (TRUE);
654 }
655 /*
656 ========================================================================
657
658 Routine Description:
659 Copy frame from waiting queue into relative ring buffer and set
660 appropriate ASIC register to kick hardware transmit function
661
662 Arguments:
663 pAd Pointer to our adapter
664 PNDIS_PACKET Pointer to Ndis Packet for MIC calculation
665 pEncap Pointer to LLC encap data
666 LenEncap Total encap length, might be 0 which indicates no encap
667
668 Return Value:
669 None
670
671 IRQL = DISPATCH_LEVEL
672
673 Note:
674
675 ========================================================================
676 */
RTMPCalculateMICValue(IN PRTMP_ADAPTER pAd,IN PNDIS_PACKET pPacket,IN PUCHAR pEncap,IN PCIPHER_KEY pKey,IN UCHAR apidx)677 VOID RTMPCalculateMICValue(
678 IN PRTMP_ADAPTER pAd,
679 IN PNDIS_PACKET pPacket,
680 IN PUCHAR pEncap,
681 IN PCIPHER_KEY pKey,
682 IN UCHAR apidx)
683 {
684 PACKET_INFO PacketInfo;
685 PUCHAR pSrcBufVA;
686 UINT SrcBufLen;
687 PUCHAR pSrc;
688 UCHAR UserPriority;
689 UCHAR vlan_offset = 0;
690
691 RTMP_QueryPacketInfo(pPacket, &PacketInfo, &pSrcBufVA, &SrcBufLen);
692
693 UserPriority = RTMP_GET_PACKET_UP(pPacket);
694 pSrc = pSrcBufVA;
695
696 // determine if this is a vlan packet
697 if (((*(pSrc + 12) << 8) + *(pSrc + 13)) == 0x8100)
698 vlan_offset = 4;
699
700 {
701 RTMPInitMICEngine(
702 pAd,
703 pKey->Key,
704 pSrc,
705 pSrc + 6,
706 UserPriority,
707 pKey->TxMic);
708 }
709
710
711 if (pEncap != NULL)
712 {
713 // LLC encapsulation
714 RTMPTkipAppend(&pAd->PrivateInfo.Tx, pEncap, 6);
715 // Protocol Type
716 RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSrc + 12 + vlan_offset, 2);
717 }
718 SrcBufLen -= (14 + vlan_offset);
719 pSrc += (14 + vlan_offset);
720 do
721 {
722 if (SrcBufLen > 0)
723 {
724 RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSrc, SrcBufLen);
725 }
726
727 break; // No need handle next packet
728
729 } while (TRUE); // End of copying payload
730
731 // Compute the final MIC Value
732 RTMPTkipGetMIC(&pAd->PrivateInfo.Tx);
733 }
734
735
736 /************************************************************/
737 /* tkip_sbox() */
738 /* Returns a 16 bit value from a 64K entry table. The Table */
739 /* is synthesized from two 256 entry byte wide tables. */
740 /************************************************************/
741
tkip_sbox(UINT index)742 UINT tkip_sbox(UINT index)
743 {
744 UINT index_low;
745 UINT index_high;
746 UINT left, right;
747
748 index_low = (index % 256);
749 index_high = ((index >> 8) % 256);
750
751 left = Tkip_Sbox_Lower[index_low] + (Tkip_Sbox_Upper[index_low] * 256);
752 right = Tkip_Sbox_Upper[index_high] + (Tkip_Sbox_Lower[index_high] * 256);
753
754 return (left ^ right);
755 }
756
rotr1(UINT a)757 UINT rotr1(UINT a)
758 {
759 unsigned int b;
760
761 if ((a & 0x01) == 0x01)
762 {
763 b = (a >> 1) | 0x8000;
764 }
765 else
766 {
767 b = (a >> 1) & 0x7fff;
768 }
769 b = b % 65536;
770 return b;
771 }
772
RTMPTkipMixKey(UCHAR * key,UCHAR * ta,ULONG pnl,ULONG pnh,UCHAR * rc4key,UINT * p1k)773 VOID RTMPTkipMixKey(
774 UCHAR *key,
775 UCHAR *ta,
776 ULONG pnl, /* Least significant 16 bits of PN */
777 ULONG pnh, /* Most significant 32 bits of PN */
778 UCHAR *rc4key,
779 UINT *p1k)
780 {
781
782 UINT tsc0;
783 UINT tsc1;
784 UINT tsc2;
785
786 UINT ppk0;
787 UINT ppk1;
788 UINT ppk2;
789 UINT ppk3;
790 UINT ppk4;
791 UINT ppk5;
792
793 INT i;
794 INT j;
795
796 tsc0 = (unsigned int)((pnh >> 16) % 65536); /* msb */
797 tsc1 = (unsigned int)(pnh % 65536);
798 tsc2 = (unsigned int)(pnl % 65536); /* lsb */
799
800 /* Phase 1, step 1 */
801 p1k[0] = tsc1;
802 p1k[1] = tsc0;
803 p1k[2] = (UINT)(ta[0] + (ta[1]*256));
804 p1k[3] = (UINT)(ta[2] + (ta[3]*256));
805 p1k[4] = (UINT)(ta[4] + (ta[5]*256));
806
807 /* Phase 1, step 2 */
808 for (i=0; i<8; i++)
809 {
810 j = 2*(i & 1);
811 p1k[0] = (p1k[0] + tkip_sbox( (p1k[4] ^ ((256*key[1+j]) + key[j])) % 65536 )) % 65536;
812 p1k[1] = (p1k[1] + tkip_sbox( (p1k[0] ^ ((256*key[5+j]) + key[4+j])) % 65536 )) % 65536;
813 p1k[2] = (p1k[2] + tkip_sbox( (p1k[1] ^ ((256*key[9+j]) + key[8+j])) % 65536 )) % 65536;
814 p1k[3] = (p1k[3] + tkip_sbox( (p1k[2] ^ ((256*key[13+j]) + key[12+j])) % 65536 )) % 65536;
815 p1k[4] = (p1k[4] + tkip_sbox( (p1k[3] ^ (((256*key[1+j]) + key[j]))) % 65536 )) % 65536;
816 p1k[4] = (p1k[4] + i) % 65536;
817 }
818
819 /* Phase 2, Step 1 */
820 ppk0 = p1k[0];
821 ppk1 = p1k[1];
822 ppk2 = p1k[2];
823 ppk3 = p1k[3];
824 ppk4 = p1k[4];
825 ppk5 = (p1k[4] + tsc2) % 65536;
826
827 /* Phase2, Step 2 */
828 ppk0 = ppk0 + tkip_sbox( (ppk5 ^ ((256*key[1]) + key[0])) % 65536);
829 ppk1 = ppk1 + tkip_sbox( (ppk0 ^ ((256*key[3]) + key[2])) % 65536);
830 ppk2 = ppk2 + tkip_sbox( (ppk1 ^ ((256*key[5]) + key[4])) % 65536);
831 ppk3 = ppk3 + tkip_sbox( (ppk2 ^ ((256*key[7]) + key[6])) % 65536);
832 ppk4 = ppk4 + tkip_sbox( (ppk3 ^ ((256*key[9]) + key[8])) % 65536);
833 ppk5 = ppk5 + tkip_sbox( (ppk4 ^ ((256*key[11]) + key[10])) % 65536);
834
835 ppk0 = ppk0 + rotr1(ppk5 ^ ((256*key[13]) + key[12]));
836 ppk1 = ppk1 + rotr1(ppk0 ^ ((256*key[15]) + key[14]));
837 ppk2 = ppk2 + rotr1(ppk1);
838 ppk3 = ppk3 + rotr1(ppk2);
839 ppk4 = ppk4 + rotr1(ppk3);
840 ppk5 = ppk5 + rotr1(ppk4);
841
842 /* Phase 2, Step 3 */
843 /* Phase 2, Step 3 */
844
845 tsc0 = (unsigned int)((pnh >> 16) % 65536); /* msb */
846 tsc1 = (unsigned int)(pnh % 65536);
847 tsc2 = (unsigned int)(pnl % 65536); /* lsb */
848
849 rc4key[0] = (tsc2 >> 8) % 256;
850 rc4key[1] = (((tsc2 >> 8) % 256) | 0x20) & 0x7f;
851 rc4key[2] = tsc2 % 256;
852 rc4key[3] = ((ppk5 ^ ((256*key[1]) + key[0])) >> 1) % 256;
853
854 rc4key[4] = ppk0 % 256;
855 rc4key[5] = (ppk0 >> 8) % 256;
856
857 rc4key[6] = ppk1 % 256;
858 rc4key[7] = (ppk1 >> 8) % 256;
859
860 rc4key[8] = ppk2 % 256;
861 rc4key[9] = (ppk2 >> 8) % 256;
862
863 rc4key[10] = ppk3 % 256;
864 rc4key[11] = (ppk3 >> 8) % 256;
865
866 rc4key[12] = ppk4 % 256;
867 rc4key[13] = (ppk4 >> 8) % 256;
868
869 rc4key[14] = ppk5 % 256;
870 rc4key[15] = (ppk5 >> 8) % 256;
871 }
872
873
874 /************************************************/
875 /* construct_mic_header1() */
876 /* Builds the first MIC header block from */
877 /* header fields. */
878 /************************************************/
879
construct_mic_header1(unsigned char * mic_header1,int header_length,unsigned char * mpdu)880 void construct_mic_header1(
881 unsigned char *mic_header1,
882 int header_length,
883 unsigned char *mpdu)
884 {
885 mic_header1[0] = (unsigned char)((header_length - 2) / 256);
886 mic_header1[1] = (unsigned char)((header_length - 2) % 256);
887 mic_header1[2] = mpdu[0] & 0xcf; /* Mute CF poll & CF ack bits */
888 mic_header1[3] = mpdu[1] & 0xc7; /* Mute retry, more data and pwr mgt bits */
889 mic_header1[4] = mpdu[4]; /* A1 */
890 mic_header1[5] = mpdu[5];
891 mic_header1[6] = mpdu[6];
892 mic_header1[7] = mpdu[7];
893 mic_header1[8] = mpdu[8];
894 mic_header1[9] = mpdu[9];
895 mic_header1[10] = mpdu[10]; /* A2 */
896 mic_header1[11] = mpdu[11];
897 mic_header1[12] = mpdu[12];
898 mic_header1[13] = mpdu[13];
899 mic_header1[14] = mpdu[14];
900 mic_header1[15] = mpdu[15];
901 }
902
903 /************************************************/
904 /* construct_mic_header2() */
905 /* Builds the last MIC header block from */
906 /* header fields. */
907 /************************************************/
908
construct_mic_header2(unsigned char * mic_header2,unsigned char * mpdu,int a4_exists,int qc_exists)909 void construct_mic_header2(
910 unsigned char *mic_header2,
911 unsigned char *mpdu,
912 int a4_exists,
913 int qc_exists)
914 {
915 int i;
916
917 for (i = 0; i<16; i++) mic_header2[i]=0x00;
918
919 mic_header2[0] = mpdu[16]; /* A3 */
920 mic_header2[1] = mpdu[17];
921 mic_header2[2] = mpdu[18];
922 mic_header2[3] = mpdu[19];
923 mic_header2[4] = mpdu[20];
924 mic_header2[5] = mpdu[21];
925
926 // In Sequence Control field, mute sequence numer bits (12-bit)
927 mic_header2[6] = mpdu[22] & 0x0f; /* SC */
928 mic_header2[7] = 0x00; /* mpdu[23]; */
929
930 if ((!qc_exists) & a4_exists)
931 {
932 for (i=0;i<6;i++) mic_header2[8+i] = mpdu[24+i]; /* A4 */
933
934 }
935
936 if (qc_exists && (!a4_exists))
937 {
938 mic_header2[8] = mpdu[24] & 0x0f; /* mute bits 15 - 4 */
939 mic_header2[9] = mpdu[25] & 0x00;
940 }
941
942 if (qc_exists && a4_exists)
943 {
944 for (i=0;i<6;i++) mic_header2[8+i] = mpdu[24+i]; /* A4 */
945
946 mic_header2[14] = mpdu[30] & 0x0f;
947 mic_header2[15] = mpdu[31] & 0x00;
948 }
949 }
950
951
952 /************************************************/
953 /* construct_mic_iv() */
954 /* Builds the MIC IV from header fields and PN */
955 /************************************************/
956
construct_mic_iv(unsigned char * mic_iv,int qc_exists,int a4_exists,unsigned char * mpdu,unsigned int payload_length,unsigned char * pn_vector)957 void construct_mic_iv(
958 unsigned char *mic_iv,
959 int qc_exists,
960 int a4_exists,
961 unsigned char *mpdu,
962 unsigned int payload_length,
963 unsigned char *pn_vector)
964 {
965 int i;
966
967 mic_iv[0] = 0x59;
968 if (qc_exists && a4_exists)
969 mic_iv[1] = mpdu[30] & 0x0f; /* QoS_TC */
970 if (qc_exists && !a4_exists)
971 mic_iv[1] = mpdu[24] & 0x0f; /* mute bits 7-4 */
972 if (!qc_exists)
973 mic_iv[1] = 0x00;
974 for (i = 2; i < 8; i++)
975 mic_iv[i] = mpdu[i + 8]; /* mic_iv[2:7] = A2[0:5] = mpdu[10:15] */
976 #ifdef CONSISTENT_PN_ORDER
977 for (i = 8; i < 14; i++)
978 mic_iv[i] = pn_vector[i - 8]; /* mic_iv[8:13] = PN[0:5] */
979 #else
980 for (i = 8; i < 14; i++)
981 mic_iv[i] = pn_vector[13 - i]; /* mic_iv[8:13] = PN[5:0] */
982 #endif
983 i = (payload_length / 256);
984 i = (payload_length % 256);
985 mic_iv[14] = (unsigned char) (payload_length / 256);
986 mic_iv[15] = (unsigned char) (payload_length % 256);
987
988 }
989
990
991
992 /************************************/
993 /* bitwise_xor() */
994 /* A 128 bit, bitwise exclusive or */
995 /************************************/
996
bitwise_xor(unsigned char * ina,unsigned char * inb,unsigned char * out)997 void bitwise_xor(unsigned char *ina, unsigned char *inb, unsigned char *out)
998 {
999 int i;
1000 for (i=0; i<16; i++)
1001 {
1002 out[i] = ina[i] ^ inb[i];
1003 }
1004 }
1005
1006
aes128k128d(unsigned char * key,unsigned char * data,unsigned char * ciphertext)1007 void aes128k128d(unsigned char *key, unsigned char *data, unsigned char *ciphertext)
1008 {
1009 int round;
1010 int i;
1011 unsigned char intermediatea[16];
1012 unsigned char intermediateb[16];
1013 unsigned char round_key[16];
1014
1015 for(i=0; i<16; i++) round_key[i] = key[i];
1016
1017 for (round = 0; round < 11; round++)
1018 {
1019 if (round == 0)
1020 {
1021 xor_128(round_key, data, ciphertext);
1022 next_key(round_key, round);
1023 }
1024 else if (round == 10)
1025 {
1026 byte_sub(ciphertext, intermediatea);
1027 shift_row(intermediatea, intermediateb);
1028 xor_128(intermediateb, round_key, ciphertext);
1029 }
1030 else /* 1 - 9 */
1031 {
1032 byte_sub(ciphertext, intermediatea);
1033 shift_row(intermediatea, intermediateb);
1034 mix_column(&intermediateb[0], &intermediatea[0]);
1035 mix_column(&intermediateb[4], &intermediatea[4]);
1036 mix_column(&intermediateb[8], &intermediatea[8]);
1037 mix_column(&intermediateb[12], &intermediatea[12]);
1038 xor_128(intermediatea, round_key, ciphertext);
1039 next_key(round_key, round);
1040 }
1041 }
1042
1043 }
1044
construct_ctr_preload(unsigned char * ctr_preload,int a4_exists,int qc_exists,unsigned char * mpdu,unsigned char * pn_vector,int c)1045 void construct_ctr_preload(
1046 unsigned char *ctr_preload,
1047 int a4_exists,
1048 int qc_exists,
1049 unsigned char *mpdu,
1050 unsigned char *pn_vector,
1051 int c)
1052 {
1053
1054 int i = 0;
1055 for (i=0; i<16; i++) ctr_preload[i] = 0x00;
1056 i = 0;
1057
1058 ctr_preload[0] = 0x01; /* flag */
1059 if (qc_exists && a4_exists) ctr_preload[1] = mpdu[30] & 0x0f; /* QoC_Control */
1060 if (qc_exists && !a4_exists) ctr_preload[1] = mpdu[24] & 0x0f;
1061
1062 for (i = 2; i < 8; i++)
1063 ctr_preload[i] = mpdu[i + 8]; /* ctr_preload[2:7] = A2[0:5] = mpdu[10:15] */
1064 #ifdef CONSISTENT_PN_ORDER
1065 for (i = 8; i < 14; i++)
1066 ctr_preload[i] = pn_vector[i - 8]; /* ctr_preload[8:13] = PN[0:5] */
1067 #else
1068 for (i = 8; i < 14; i++)
1069 ctr_preload[i] = pn_vector[13 - i]; /* ctr_preload[8:13] = PN[5:0] */
1070 #endif
1071 ctr_preload[14] = (unsigned char) (c / 256); // Ctr
1072 ctr_preload[15] = (unsigned char) (c % 256);
1073
1074 }
1075
1076
1077 //
1078 // TRUE: Success!
1079 // FALSE: Decrypt Error!
1080 //
RTMPSoftDecryptTKIP(IN PRTMP_ADAPTER pAd,IN PUCHAR pData,IN ULONG DataByteCnt,IN UCHAR UserPriority,IN PCIPHER_KEY pWpaKey)1081 BOOLEAN RTMPSoftDecryptTKIP(
1082 IN PRTMP_ADAPTER pAd,
1083 IN PUCHAR pData,
1084 IN ULONG DataByteCnt,
1085 IN UCHAR UserPriority,
1086 IN PCIPHER_KEY pWpaKey)
1087 {
1088 UCHAR KeyID;
1089 UINT HeaderLen;
1090 UCHAR fc0;
1091 UCHAR fc1;
1092 USHORT fc;
1093 UINT frame_type;
1094 UINT frame_subtype;
1095 UINT from_ds;
1096 UINT to_ds;
1097 INT a4_exists;
1098 INT qc_exists;
1099 USHORT duration;
1100 USHORT seq_control;
1101 USHORT qos_control;
1102 UCHAR TA[MAC_ADDR_LEN];
1103 UCHAR DA[MAC_ADDR_LEN];
1104 UCHAR SA[MAC_ADDR_LEN];
1105 UCHAR RC4Key[16];
1106 UINT p1k[5]; //for mix_key;
1107 ULONG pnl;/* Least significant 16 bits of PN */
1108 ULONG pnh;/* Most significant 32 bits of PN */
1109 UINT num_blocks;
1110 UINT payload_remainder;
1111 ARCFOURCONTEXT ArcFourContext;
1112 UINT crc32 = 0;
1113 UINT trailfcs = 0;
1114 UCHAR MIC[8];
1115 UCHAR TrailMIC[8];
1116
1117 #ifdef RT_BIG_ENDIAN
1118 RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
1119 #endif
1120
1121 fc0 = *pData;
1122 fc1 = *(pData + 1);
1123
1124 fc = *((PUSHORT)pData);
1125
1126 frame_type = ((fc0 >> 2) & 0x03);
1127 frame_subtype = ((fc0 >> 4) & 0x0f);
1128
1129 from_ds = (fc1 & 0x2) >> 1;
1130 to_ds = (fc1 & 0x1);
1131
1132 a4_exists = (from_ds & to_ds);
1133 qc_exists = ((frame_subtype == 0x08) || /* Assumed QoS subtypes */
1134 (frame_subtype == 0x09) || /* Likely to change. */
1135 (frame_subtype == 0x0a) ||
1136 (frame_subtype == 0x0b)
1137 );
1138
1139 HeaderLen = 24;
1140 if (a4_exists)
1141 HeaderLen += 6;
1142
1143 KeyID = *((PUCHAR)(pData+ HeaderLen + 3));
1144 KeyID = KeyID >> 6;
1145
1146 if (pWpaKey[KeyID].KeyLen == 0)
1147 {
1148 DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptTKIP failed!(KeyID[%d] Length can not be 0)\n", KeyID));
1149 return FALSE;
1150 }
1151
1152 duration = *((PUSHORT)(pData+2));
1153
1154 seq_control = *((PUSHORT)(pData+22));
1155
1156 if (qc_exists)
1157 {
1158 if (a4_exists)
1159 {
1160 qos_control = *((PUSHORT)(pData+30));
1161 }
1162 else
1163 {
1164 qos_control = *((PUSHORT)(pData+24));
1165 }
1166 }
1167
1168 if (to_ds == 0 && from_ds == 1)
1169 {
1170 NdisMoveMemory(DA, pData+4, MAC_ADDR_LEN);
1171 NdisMoveMemory(SA, pData+16, MAC_ADDR_LEN);
1172 NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN); //BSSID
1173 }
1174 else if (to_ds == 0 && from_ds == 0 )
1175 {
1176 NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
1177 NdisMoveMemory(DA, pData+4, MAC_ADDR_LEN);
1178 NdisMoveMemory(SA, pData+10, MAC_ADDR_LEN);
1179 }
1180 else if (to_ds == 1 && from_ds == 0)
1181 {
1182 NdisMoveMemory(SA, pData+10, MAC_ADDR_LEN);
1183 NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
1184 NdisMoveMemory(DA, pData+16, MAC_ADDR_LEN);
1185 }
1186 else if (to_ds == 1 && from_ds == 1)
1187 {
1188 NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
1189 NdisMoveMemory(DA, pData+16, MAC_ADDR_LEN);
1190 NdisMoveMemory(SA, pData+22, MAC_ADDR_LEN);
1191 }
1192
1193 num_blocks = (DataByteCnt - 16) / 16;
1194 payload_remainder = (DataByteCnt - 16) % 16;
1195
1196 pnl = (*(pData + HeaderLen)) * 256 + *(pData + HeaderLen + 2);
1197 pnh = *((PULONG)(pData + HeaderLen + 4));
1198 pnh = cpu2le32(pnh);
1199 RTMPTkipMixKey(pWpaKey[KeyID].Key, TA, pnl, pnh, RC4Key, p1k);
1200
1201 ARCFOUR_INIT(&ArcFourContext, RC4Key, 16);
1202
1203 ARCFOUR_DECRYPT(&ArcFourContext, pData + HeaderLen, pData + HeaderLen + 8, DataByteCnt - HeaderLen - 8);
1204 NdisMoveMemory(&trailfcs, pData + DataByteCnt - 8 - 4, 4);
1205 crc32 = RTMP_CALC_FCS32(PPPINITFCS32, pData + HeaderLen, DataByteCnt - HeaderLen - 8 - 4); //Skip IV+EIV 8 bytes & Skip last 4 bytes(FCS).
1206 crc32 ^= 0xffffffff; /* complement */
1207
1208 if(crc32 != cpu2le32(trailfcs))
1209 {
1210 DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptTKIP, WEP Data ICV Error !\n")); //ICV error.
1211
1212 return (FALSE);
1213 }
1214
1215 NdisMoveMemory(TrailMIC, pData + DataByteCnt - 8 - 8 - 4, 8);
1216 RTMPInitMICEngine(pAd, pWpaKey[KeyID].Key, DA, SA, UserPriority, pWpaKey[KeyID].RxMic);
1217 RTMPTkipAppend(&pAd->PrivateInfo.Tx, pData + HeaderLen, DataByteCnt - HeaderLen - 8 - 12);
1218 RTMPTkipGetMIC(&pAd->PrivateInfo.Tx);
1219 NdisMoveMemory(MIC, pAd->PrivateInfo.Tx.MIC, 8);
1220
1221 if (!NdisEqualMemory(MIC, TrailMIC, 8))
1222 {
1223 DBGPRINT(RT_DEBUG_ERROR, ("RTMPSoftDecryptTKIP, WEP Data MIC Error !\n")); //MIC error.
1224 return (FALSE);
1225 }
1226
1227 #ifdef RT_BIG_ENDIAN
1228 RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
1229 #endif
1230 return TRUE;
1231 }
1232
1233
1234
1235
RTMPSoftDecryptAES(IN PRTMP_ADAPTER pAd,IN PUCHAR pData,IN ULONG DataByteCnt,IN PCIPHER_KEY pWpaKey)1236 BOOLEAN RTMPSoftDecryptAES(
1237 IN PRTMP_ADAPTER pAd,
1238 IN PUCHAR pData,
1239 IN ULONG DataByteCnt,
1240 IN PCIPHER_KEY pWpaKey)
1241 {
1242 UCHAR KeyID;
1243 UINT HeaderLen;
1244 UCHAR PN[6];
1245 UINT payload_len;
1246 UINT num_blocks;
1247 UINT payload_remainder;
1248 USHORT fc;
1249 UCHAR fc0;
1250 UCHAR fc1;
1251 UINT frame_type;
1252 UINT frame_subtype;
1253 UINT from_ds;
1254 UINT to_ds;
1255 INT a4_exists;
1256 INT qc_exists;
1257 UCHAR aes_out[16];
1258 int payload_index;
1259 UINT i;
1260 UCHAR ctr_preload[16];
1261 UCHAR chain_buffer[16];
1262 UCHAR padded_buffer[16];
1263 UCHAR mic_iv[16];
1264 UCHAR mic_header1[16];
1265 UCHAR mic_header2[16];
1266 UCHAR MIC[8];
1267 UCHAR TrailMIC[8];
1268
1269 #ifdef RT_BIG_ENDIAN
1270 RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
1271 #endif
1272
1273 fc0 = *pData;
1274 fc1 = *(pData + 1);
1275
1276 fc = *((PUSHORT)pData);
1277
1278 frame_type = ((fc0 >> 2) & 0x03);
1279 frame_subtype = ((fc0 >> 4) & 0x0f);
1280
1281 from_ds = (fc1 & 0x2) >> 1;
1282 to_ds = (fc1 & 0x1);
1283
1284 a4_exists = (from_ds & to_ds);
1285 qc_exists = ((frame_subtype == 0x08) || /* Assumed QoS subtypes */
1286 (frame_subtype == 0x09) || /* Likely to change. */
1287 (frame_subtype == 0x0a) ||
1288 (frame_subtype == 0x0b)
1289 );
1290
1291 HeaderLen = 24;
1292 if (a4_exists)
1293 HeaderLen += 6;
1294
1295 KeyID = *((PUCHAR)(pData+ HeaderLen + 3));
1296 KeyID = KeyID >> 6;
1297
1298 if (pWpaKey[KeyID].KeyLen == 0)
1299 {
1300 DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptAES failed!(KeyID[%d] Length can not be 0)\n", KeyID));
1301 return FALSE;
1302 }
1303
1304 PN[0] = *(pData+ HeaderLen);
1305 PN[1] = *(pData+ HeaderLen + 1);
1306 PN[2] = *(pData+ HeaderLen + 4);
1307 PN[3] = *(pData+ HeaderLen + 5);
1308 PN[4] = *(pData+ HeaderLen + 6);
1309 PN[5] = *(pData+ HeaderLen + 7);
1310
1311 payload_len = DataByteCnt - HeaderLen - 8 - 8; // 8 bytes for CCMP header , 8 bytes for MIC
1312 payload_remainder = (payload_len) % 16;
1313 num_blocks = (payload_len) / 16;
1314
1315
1316
1317 // Find start of payload
1318 payload_index = HeaderLen + 8; //IV+EIV
1319
1320 for (i=0; i< num_blocks; i++)
1321 {
1322 construct_ctr_preload(ctr_preload,
1323 a4_exists,
1324 qc_exists,
1325 pData,
1326 PN,
1327 i+1 );
1328
1329 aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);
1330
1331 bitwise_xor(aes_out, pData + payload_index, chain_buffer);
1332 NdisMoveMemory(pData + payload_index - 8, chain_buffer, 16);
1333 payload_index += 16;
1334 }
1335
1336 //
1337 // If there is a short final block, then pad it
1338 // encrypt it and copy the unpadded part back
1339 //
1340 if (payload_remainder > 0)
1341 {
1342 construct_ctr_preload(ctr_preload,
1343 a4_exists,
1344 qc_exists,
1345 pData,
1346 PN,
1347 num_blocks + 1);
1348
1349 NdisZeroMemory(padded_buffer, 16);
1350 NdisMoveMemory(padded_buffer, pData + payload_index, payload_remainder);
1351
1352 aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);
1353
1354 bitwise_xor(aes_out, padded_buffer, chain_buffer);
1355 NdisMoveMemory(pData + payload_index - 8, chain_buffer, payload_remainder);
1356 payload_index += payload_remainder;
1357 }
1358
1359 //
1360 // Descrypt the MIC
1361 //
1362 construct_ctr_preload(ctr_preload,
1363 a4_exists,
1364 qc_exists,
1365 pData,
1366 PN,
1367 0);
1368 NdisZeroMemory(padded_buffer, 16);
1369 NdisMoveMemory(padded_buffer, pData + payload_index, 8);
1370
1371 aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);
1372
1373 bitwise_xor(aes_out, padded_buffer, chain_buffer);
1374
1375 NdisMoveMemory(TrailMIC, chain_buffer, 8);
1376
1377 //
1378 // Calculate MIC
1379 //
1380
1381 //Force the protected frame bit on
1382 *(pData + 1) = *(pData + 1) | 0x40;
1383
1384 // Find start of payload
1385 // Because the CCMP header has been removed
1386 payload_index = HeaderLen;
1387
1388 construct_mic_iv(
1389 mic_iv,
1390 qc_exists,
1391 a4_exists,
1392 pData,
1393 payload_len,
1394 PN);
1395
1396 construct_mic_header1(
1397 mic_header1,
1398 HeaderLen,
1399 pData);
1400
1401 construct_mic_header2(
1402 mic_header2,
1403 pData,
1404 a4_exists,
1405 qc_exists);
1406
1407 aes128k128d(pWpaKey[KeyID].Key, mic_iv, aes_out);
1408 bitwise_xor(aes_out, mic_header1, chain_buffer);
1409 aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
1410 bitwise_xor(aes_out, mic_header2, chain_buffer);
1411 aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
1412
1413 // iterate through each 16 byte payload block
1414 for (i = 0; i < num_blocks; i++)
1415 {
1416 bitwise_xor(aes_out, pData + payload_index, chain_buffer);
1417 payload_index += 16;
1418 aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
1419 }
1420
1421 // Add on the final payload block if it needs padding
1422 if (payload_remainder > 0)
1423 {
1424 NdisZeroMemory(padded_buffer, 16);
1425 NdisMoveMemory(padded_buffer, pData + payload_index, payload_remainder);
1426
1427 bitwise_xor(aes_out, padded_buffer, chain_buffer);
1428 aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
1429 }
1430 // aes_out contains padded mic, discard most significant
1431 // 8 bytes to generate 64 bit MIC
1432 for (i = 0 ; i < 8; i++) MIC[i] = aes_out[i];
1433
1434 if (!NdisEqualMemory(MIC, TrailMIC, 8))
1435 {
1436 DBGPRINT(RT_DEBUG_ERROR, ("RTMPSoftDecryptAES, MIC Error !\n")); //MIC error.
1437 return FALSE;
1438 }
1439
1440 #ifdef RT_BIG_ENDIAN
1441 RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
1442 #endif
1443
1444 return TRUE;
1445 }
1446
1447 /****************************************/
1448 /* aes128k128d() */
1449 /* Performs a 128 bit AES encrypt with */
1450 /* 128 bit data. */
1451 /****************************************/
xor_128(IN PUCHAR a,IN PUCHAR b,OUT PUCHAR out)1452 VOID xor_128(
1453 IN PUCHAR a,
1454 IN PUCHAR b,
1455 OUT PUCHAR out)
1456 {
1457 INT i;
1458
1459 for (i=0;i<16; i++)
1460 {
1461 out[i] = a[i] ^ b[i];
1462 }
1463 }
1464
next_key(IN PUCHAR key,IN INT round)1465 VOID next_key(
1466 IN PUCHAR key,
1467 IN INT round)
1468 {
1469 UCHAR rcon;
1470 UCHAR sbox_key[4];
1471 UCHAR rcon_table[12] =
1472 {
1473 0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80,
1474 0x1b, 0x36, 0x36, 0x36
1475 };
1476
1477 sbox_key[0] = RTMPCkipSbox(key[13]);
1478 sbox_key[1] = RTMPCkipSbox(key[14]);
1479 sbox_key[2] = RTMPCkipSbox(key[15]);
1480 sbox_key[3] = RTMPCkipSbox(key[12]);
1481
1482 rcon = rcon_table[round];
1483
1484 xor_32(&key[0], sbox_key, &key[0]);
1485 key[0] = key[0] ^ rcon;
1486
1487 xor_32(&key[4], &key[0], &key[4]);
1488 xor_32(&key[8], &key[4], &key[8]);
1489 xor_32(&key[12], &key[8], &key[12]);
1490 }
1491
xor_32(IN PUCHAR a,IN PUCHAR b,OUT PUCHAR out)1492 VOID xor_32(
1493 IN PUCHAR a,
1494 IN PUCHAR b,
1495 OUT PUCHAR out)
1496 {
1497 INT i;
1498
1499 for (i=0;i<4; i++)
1500 {
1501 out[i] = a[i] ^ b[i];
1502 }
1503 }
1504
byte_sub(IN PUCHAR in,OUT PUCHAR out)1505 VOID byte_sub(
1506 IN PUCHAR in,
1507 OUT PUCHAR out)
1508 {
1509 INT i;
1510
1511 for (i=0; i< 16; i++)
1512 {
1513 out[i] = RTMPCkipSbox(in[i]);
1514 }
1515 }
1516
RTMPCkipSbox(IN UCHAR a)1517 UCHAR RTMPCkipSbox(
1518 IN UCHAR a)
1519 {
1520 return SboxTable[(int)a];
1521 }
1522
shift_row(IN PUCHAR in,OUT PUCHAR out)1523 VOID shift_row(
1524 IN PUCHAR in,
1525 OUT PUCHAR out)
1526 {
1527 out[0] = in[0];
1528 out[1] = in[5];
1529 out[2] = in[10];
1530 out[3] = in[15];
1531 out[4] = in[4];
1532 out[5] = in[9];
1533 out[6] = in[14];
1534 out[7] = in[3];
1535 out[8] = in[8];
1536 out[9] = in[13];
1537 out[10] = in[2];
1538 out[11] = in[7];
1539 out[12] = in[12];
1540 out[13] = in[1];
1541 out[14] = in[6];
1542 out[15] = in[11];
1543 }
1544
mix_column(IN PUCHAR in,OUT PUCHAR out)1545 VOID mix_column(
1546 IN PUCHAR in,
1547 OUT PUCHAR out)
1548 {
1549 INT i;
1550 UCHAR add1b[4];
1551 UCHAR add1bf7[4];
1552 UCHAR rotl[4];
1553 UCHAR swap_halfs[4];
1554 UCHAR andf7[4];
1555 UCHAR rotr[4];
1556 UCHAR temp[4];
1557 UCHAR tempb[4];
1558
1559 for (i=0 ; i<4; i++)
1560 {
1561 if ((in[i] & 0x80)== 0x80)
1562 add1b[i] = 0x1b;
1563 else
1564 add1b[i] = 0x00;
1565 }
1566
1567 swap_halfs[0] = in[2]; /* Swap halfs */
1568 swap_halfs[1] = in[3];
1569 swap_halfs[2] = in[0];
1570 swap_halfs[3] = in[1];
1571
1572 rotl[0] = in[3]; /* Rotate left 8 bits */
1573 rotl[1] = in[0];
1574 rotl[2] = in[1];
1575 rotl[3] = in[2];
1576
1577 andf7[0] = in[0] & 0x7f;
1578 andf7[1] = in[1] & 0x7f;
1579 andf7[2] = in[2] & 0x7f;
1580 andf7[3] = in[3] & 0x7f;
1581
1582 for (i = 3; i>0; i--) /* logical shift left 1 bit */
1583 {
1584 andf7[i] = andf7[i] << 1;
1585 if ((andf7[i-1] & 0x80) == 0x80)
1586 {
1587 andf7[i] = (andf7[i] | 0x01);
1588 }
1589 }
1590 andf7[0] = andf7[0] << 1;
1591 andf7[0] = andf7[0] & 0xfe;
1592
1593 xor_32(add1b, andf7, add1bf7);
1594
1595 xor_32(in, add1bf7, rotr);
1596
1597 temp[0] = rotr[0]; /* Rotate right 8 bits */
1598 rotr[0] = rotr[1];
1599 rotr[1] = rotr[2];
1600 rotr[2] = rotr[3];
1601 rotr[3] = temp[0];
1602
1603 xor_32(add1bf7, rotr, temp);
1604 xor_32(swap_halfs, rotl,tempb);
1605 xor_32(temp, tempb, out);
1606 }
1607
1608