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