?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
added 2.6.29.6 aldebaran kernel
[nao-ulib.git] / kernel / 2.6.29.6-aldebaran-rt / drivers / staging / rt2870 / common / rtmp_tkip.c
blob2847409ff081624d1bfda62ef4c531c42fb7087b
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 */
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 */
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 */
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 */
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 */
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 */
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 */
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 // tkipIv.IV32 = *(PULONG)(pTSC + 2);
474 NdisMoveMemory(&tkipIv.IV32, (pTSC + 2), 4); // Copy IV
475
476 *pIV16 = tkipIv.IV16.word;
477 *pIV32 = tkipIv.IV32;
478 }
479
480 /*
481 ========================================================================
482
483 Routine Description:
484 Init MIC Value calculation function which include set MIC key &
485 calculate first 16 bytes (DA + SA + priority + 0)
486
487 Arguments:
488 pAd Pointer to our adapter
489 pTKey Pointer to the Temporal Key (TK), TK shall be 128bits.
490 pDA Pointer to DA address
491 pSA Pointer to SA address
492 pMICKey pointer to MIC Key
493
494 Return Value:
495 None
496
497 Note:
498
499 ========================================================================
500 */
501 VOID RTMPInitMICEngine(
502 IN PRTMP_ADAPTER pAd,
503 IN PUCHAR pKey,
504 IN PUCHAR pDA,
505 IN PUCHAR pSA,
506 IN UCHAR UserPriority,
507 IN PUCHAR pMICKey)
508 {
509 ULONG Priority = UserPriority;
510
511 // Init MIC value calculation
512 RTMPTkipSetMICKey(&pAd->PrivateInfo.Tx, pMICKey);
513 // DA
514 RTMPTkipAppend(&pAd->PrivateInfo.Tx, pDA, MAC_ADDR_LEN);
515 // SA
516 RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSA, MAC_ADDR_LEN);
517 // Priority + 3 bytes of 0
518 RTMPTkipAppend(&pAd->PrivateInfo.Tx, (PUCHAR)&Priority, 4);
519 }
520
521 /*
522 ========================================================================
523
524 Routine Description:
525 Compare MIC value of received MSDU
526
527 Arguments:
528 pAd Pointer to our adapter
529 pSrc Pointer to the received Plain text data
530 pDA Pointer to DA address
531 pSA Pointer to SA address
532 pMICKey pointer to MIC Key
533 Len the length of the received plain text data exclude MIC value
534
535 Return Value:
536 TRUE MIC value matched
537 FALSE MIC value mismatched
538
539 IRQL = DISPATCH_LEVEL
540
541 Note:
542
543 ========================================================================
544 */
545 BOOLEAN RTMPTkipCompareMICValue(
546 IN PRTMP_ADAPTER pAd,
547 IN PUCHAR pSrc,
548 IN PUCHAR pDA,
549 IN PUCHAR pSA,
550 IN PUCHAR pMICKey,
551 IN UCHAR UserPriority,
552 IN UINT Len)
553 {
554 UCHAR OldMic[8];
555 ULONG Priority = UserPriority;
556
557 // Init MIC value calculation
558 RTMPTkipSetMICKey(&pAd->PrivateInfo.Rx, pMICKey);
559 // DA
560 RTMPTkipAppend(&pAd->PrivateInfo.Rx, pDA, MAC_ADDR_LEN);
561 // SA
562 RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSA, MAC_ADDR_LEN);
563 // Priority + 3 bytes of 0
564 RTMPTkipAppend(&pAd->PrivateInfo.Rx, (PUCHAR)&Priority, 4);
565
566 // Calculate MIC value from plain text data
567 RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSrc, Len);
568
569 // Get MIC valude from received frame
570 NdisMoveMemory(OldMic, pSrc + Len, 8);
571
572 // Get MIC value from decrypted plain data
573 RTMPTkipGetMIC(&pAd->PrivateInfo.Rx);
574
575 // Move MIC value from MSDU, this steps should move to data path.
576 // Since the MIC value might cross MPDUs.
577 if(!NdisEqualMemory(pAd->PrivateInfo.Rx.MIC, OldMic, 8))
578 {
579 DBGPRINT_RAW(RT_DEBUG_ERROR, ("RTMPTkipCompareMICValue(): TKIP MIC Error !\n")); //MIC error.
580
581
582 return (FALSE);
583 }
584 return (TRUE);
585 }
586
587 /*
588 ========================================================================
589
590 Routine Description:
591 Compare MIC value of received MSDU
592
593 Arguments:
594 pAd Pointer to our adapter
595 pLLC LLC header
596 pSrc Pointer to the received Plain text data
597 pDA Pointer to DA address
598 pSA Pointer to SA address
599 pMICKey pointer to MIC Key
600 Len the length of the received plain text data exclude MIC value
601
602 Return Value:
603 TRUE MIC value matched
604 FALSE MIC value mismatched
605
606 IRQL = DISPATCH_LEVEL
607
608 Note:
609
610 ========================================================================
611 */
612 BOOLEAN RTMPTkipCompareMICValueWithLLC(
613 IN PRTMP_ADAPTER pAd,
614 IN PUCHAR pLLC,
615 IN PUCHAR pSrc,
616 IN PUCHAR pDA,
617 IN PUCHAR pSA,
618 IN PUCHAR pMICKey,
619 IN UINT Len)
620 {
621 UCHAR OldMic[8];
622 ULONG Priority = 0;
623
624 // Init MIC value calculation
625 RTMPTkipSetMICKey(&pAd->PrivateInfo.Rx, pMICKey);
626 // DA
627 RTMPTkipAppend(&pAd->PrivateInfo.Rx, pDA, MAC_ADDR_LEN);
628 // SA
629 RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSA, MAC_ADDR_LEN);
630 // Priority + 3 bytes of 0
631 RTMPTkipAppend(&pAd->PrivateInfo.Rx, (PUCHAR)&Priority, 4);
632
633 // Start with LLC header
634 RTMPTkipAppend(&pAd->PrivateInfo.Rx, pLLC, 8);
635
636 // Calculate MIC value from plain text data
637 RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSrc, Len);
638
639 // Get MIC valude from received frame
640 NdisMoveMemory(OldMic, pSrc + Len, 8);
641
642 // Get MIC value from decrypted plain data
643 RTMPTkipGetMIC(&pAd->PrivateInfo.Rx);
644
645 // Move MIC value from MSDU, this steps should move to data path.
646 // Since the MIC value might cross MPDUs.
647 if(!NdisEqualMemory(pAd->PrivateInfo.Rx.MIC, OldMic, 8))
648 {
649 DBGPRINT_RAW(RT_DEBUG_ERROR, ("RTMPTkipCompareMICValueWithLLC(): TKIP MIC Error !\n")); //MIC error.
650
651
652 return (FALSE);
653 }
654 return (TRUE);
655 }
656 /*
657 ========================================================================
658
659 Routine Description:
660 Copy frame from waiting queue into relative ring buffer and set
661 appropriate ASIC register to kick hardware transmit function
662
663 Arguments:
664 pAd Pointer to our adapter
665 PNDIS_PACKET Pointer to Ndis Packet for MIC calculation
666 pEncap Pointer to LLC encap data
667 LenEncap Total encap length, might be 0 which indicates no encap
668
669 Return Value:
670 None
671
672 IRQL = DISPATCH_LEVEL
673
674 Note:
675
676 ========================================================================
677 */
678 VOID RTMPCalculateMICValue(
679 IN PRTMP_ADAPTER pAd,
680 IN PNDIS_PACKET pPacket,
681 IN PUCHAR pEncap,
682 IN PCIPHER_KEY pKey,
683 IN UCHAR apidx)
684 {
685 PACKET_INFO PacketInfo;
686 PUCHAR pSrcBufVA;
687 UINT SrcBufLen;
688 PUCHAR pSrc;
689 UCHAR UserPriority;
690 UCHAR vlan_offset = 0;
691
692 RTMP_QueryPacketInfo(pPacket, &PacketInfo, &pSrcBufVA, &SrcBufLen);
693
694 UserPriority = RTMP_GET_PACKET_UP(pPacket);
695 pSrc = pSrcBufVA;
696
697 // determine if this is a vlan packet
698 if (((*(pSrc + 12) << 8) + *(pSrc + 13)) == 0x8100)
699 vlan_offset = 4;
700
701 #ifdef CONFIG_STA_SUPPORT
702 #endif // CONFIG_STA_SUPPORT //
703 {
704 RTMPInitMICEngine(
705 pAd,
706 pKey->Key,
707 pSrc,
708 pSrc + 6,
709 UserPriority,
710 pKey->TxMic);
711 }
712
713
714 if (pEncap != NULL)
715 {
716 // LLC encapsulation
717 RTMPTkipAppend(&pAd->PrivateInfo.Tx, pEncap, 6);
718 // Protocol Type
719 RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSrc + 12 + vlan_offset, 2);
720 }
721 SrcBufLen -= (14 + vlan_offset);
722 pSrc += (14 + vlan_offset);
723 do
724 {
725 if (SrcBufLen > 0)
726 {
727 RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSrc, SrcBufLen);
728 }
729
730 break; // No need handle next packet
731
732 } while (TRUE); // End of copying payload
733
734 // Compute the final MIC Value
735 RTMPTkipGetMIC(&pAd->PrivateInfo.Tx);
736 }
737
738
739 /************************************************************/
740 /* tkip_sbox() */
741 /* Returns a 16 bit value from a 64K entry table. The Table */
742 /* is synthesized from two 256 entry byte wide tables. */
743 /************************************************************/
744
745 UINT tkip_sbox(UINT index)
746 {
747 UINT index_low;
748 UINT index_high;
749 UINT left, right;
750
751 index_low = (index % 256);
752 index_high = ((index >> 8) % 256);
753
754 left = Tkip_Sbox_Lower[index_low] + (Tkip_Sbox_Upper[index_low] * 256);
755 right = Tkip_Sbox_Upper[index_high] + (Tkip_Sbox_Lower[index_high] * 256);
756
757 return (left ^ right);
758 }
759
760 UINT rotr1(UINT a)
761 {
762 unsigned int b;
763
764 if ((a & 0x01) == 0x01)
765 {
766 b = (a >> 1) | 0x8000;
767 }
768 else
769 {
770 b = (a >> 1) & 0x7fff;
771 }
772 b = b % 65536;
773 return b;
774 }
775
776 VOID RTMPTkipMixKey(
777 UCHAR *key,
778 UCHAR *ta,
779 ULONG pnl, /* Least significant 16 bits of PN */
780 ULONG pnh, /* Most significant 32 bits of PN */
781 UCHAR *rc4key,
782 UINT *p1k)
783 {
784
785 UINT tsc0;
786 UINT tsc1;
787 UINT tsc2;
788
789 UINT ppk0;
790 UINT ppk1;
791 UINT ppk2;
792 UINT ppk3;
793 UINT ppk4;
794 UINT ppk5;
795
796 INT i;
797 INT j;
798
799 tsc0 = (unsigned int)((pnh >> 16) % 65536); /* msb */
800 tsc1 = (unsigned int)(pnh % 65536);
801 tsc2 = (unsigned int)(pnl % 65536); /* lsb */
802
803 /* Phase 1, step 1 */
804 p1k[0] = tsc1;
805 p1k[1] = tsc0;
806 p1k[2] = (UINT)(ta[0] + (ta[1]*256));
807 p1k[3] = (UINT)(ta[2] + (ta[3]*256));
808 p1k[4] = (UINT)(ta[4] + (ta[5]*256));
809
810 /* Phase 1, step 2 */
811 for (i=0; i<8; i++)
812 {
813 j = 2*(i & 1);
814 p1k[0] = (p1k[0] + tkip_sbox( (p1k[4] ^ ((256*key[1+j]) + key[j])) % 65536 )) % 65536;
815 p1k[1] = (p1k[1] + tkip_sbox( (p1k[0] ^ ((256*key[5+j]) + key[4+j])) % 65536 )) % 65536;
816 p1k[2] = (p1k[2] + tkip_sbox( (p1k[1] ^ ((256*key[9+j]) + key[8+j])) % 65536 )) % 65536;
817 p1k[3] = (p1k[3] + tkip_sbox( (p1k[2] ^ ((256*key[13+j]) + key[12+j])) % 65536 )) % 65536;
818 p1k[4] = (p1k[4] + tkip_sbox( (p1k[3] ^ (((256*key[1+j]) + key[j]))) % 65536 )) % 65536;
819 p1k[4] = (p1k[4] + i) % 65536;
820 }
821
822 /* Phase 2, Step 1 */
823 ppk0 = p1k[0];
824 ppk1 = p1k[1];
825 ppk2 = p1k[2];
826 ppk3 = p1k[3];
827 ppk4 = p1k[4];
828 ppk5 = (p1k[4] + tsc2) % 65536;
829
830 /* Phase2, Step 2 */
831 ppk0 = ppk0 + tkip_sbox( (ppk5 ^ ((256*key[1]) + key[0])) % 65536);
832 ppk1 = ppk1 + tkip_sbox( (ppk0 ^ ((256*key[3]) + key[2])) % 65536);
833 ppk2 = ppk2 + tkip_sbox( (ppk1 ^ ((256*key[5]) + key[4])) % 65536);
834 ppk3 = ppk3 + tkip_sbox( (ppk2 ^ ((256*key[7]) + key[6])) % 65536);
835 ppk4 = ppk4 + tkip_sbox( (ppk3 ^ ((256*key[9]) + key[8])) % 65536);
836 ppk5 = ppk5 + tkip_sbox( (ppk4 ^ ((256*key[11]) + key[10])) % 65536);
837
838 ppk0 = ppk0 + rotr1(ppk5 ^ ((256*key[13]) + key[12]));
839 ppk1 = ppk1 + rotr1(ppk0 ^ ((256*key[15]) + key[14]));
840 ppk2 = ppk2 + rotr1(ppk1);
841 ppk3 = ppk3 + rotr1(ppk2);
842 ppk4 = ppk4 + rotr1(ppk3);
843 ppk5 = ppk5 + rotr1(ppk4);
844
845 /* Phase 2, Step 3 */
846 /* Phase 2, Step 3 */
847
848 tsc0 = (unsigned int)((pnh >> 16) % 65536); /* msb */
849 tsc1 = (unsigned int)(pnh % 65536);
850 tsc2 = (unsigned int)(pnl % 65536); /* lsb */
851
852 rc4key[0] = (tsc2 >> 8) % 256;
853 rc4key[1] = (((tsc2 >> 8) % 256) | 0x20) & 0x7f;
854 rc4key[2] = tsc2 % 256;
855 rc4key[3] = ((ppk5 ^ ((256*key[1]) + key[0])) >> 1) % 256;
856
857 rc4key[4] = ppk0 % 256;
858 rc4key[5] = (ppk0 >> 8) % 256;
859
860 rc4key[6] = ppk1 % 256;
861 rc4key[7] = (ppk1 >> 8) % 256;
862
863 rc4key[8] = ppk2 % 256;
864 rc4key[9] = (ppk2 >> 8) % 256;
865
866 rc4key[10] = ppk3 % 256;
867 rc4key[11] = (ppk3 >> 8) % 256;
868
869 rc4key[12] = ppk4 % 256;
870 rc4key[13] = (ppk4 >> 8) % 256;
871
872 rc4key[14] = ppk5 % 256;
873 rc4key[15] = (ppk5 >> 8) % 256;
874 }
875
876
877 /************************************************/
878 /* construct_mic_header1() */
879 /* Builds the first MIC header block from */
880 /* header fields. */
881 /************************************************/
882
883 void construct_mic_header1(
884 unsigned char *mic_header1,
885 int header_length,
886 unsigned char *mpdu)
887 {
888 mic_header1[0] = (unsigned char)((header_length - 2) / 256);
889 mic_header1[1] = (unsigned char)((header_length - 2) % 256);
890 mic_header1[2] = mpdu[0] & 0xcf; /* Mute CF poll & CF ack bits */
891 mic_header1[3] = mpdu[1] & 0xc7; /* Mute retry, more data and pwr mgt bits */
892 mic_header1[4] = mpdu[4]; /* A1 */
893 mic_header1[5] = mpdu[5];
894 mic_header1[6] = mpdu[6];
895 mic_header1[7] = mpdu[7];
896 mic_header1[8] = mpdu[8];
897 mic_header1[9] = mpdu[9];
898 mic_header1[10] = mpdu[10]; /* A2 */
899 mic_header1[11] = mpdu[11];
900 mic_header1[12] = mpdu[12];
901 mic_header1[13] = mpdu[13];
902 mic_header1[14] = mpdu[14];
903 mic_header1[15] = mpdu[15];
904 }
905
906 /************************************************/
907 /* construct_mic_header2() */
908 /* Builds the last MIC header block from */
909 /* header fields. */
910 /************************************************/
911
912 void construct_mic_header2(
913 unsigned char *mic_header2,
914 unsigned char *mpdu,
915 int a4_exists,
916 int qc_exists)
917 {
918 int i;
919
920 for (i = 0; i<16; i++) mic_header2[i]=0x00;
921
922 mic_header2[0] = mpdu[16]; /* A3 */
923 mic_header2[1] = mpdu[17];
924 mic_header2[2] = mpdu[18];
925 mic_header2[3] = mpdu[19];
926 mic_header2[4] = mpdu[20];
927 mic_header2[5] = mpdu[21];
928
929 // In Sequence Control field, mute sequence numer bits (12-bit)
930 mic_header2[6] = mpdu[22] & 0x0f; /* SC */
931 mic_header2[7] = 0x00; /* mpdu[23]; */
932
933 if ((!qc_exists) & a4_exists)
934 {
935 for (i=0;i<6;i++) mic_header2[8+i] = mpdu[24+i]; /* A4 */
936
937 }
938
939 if (qc_exists && (!a4_exists))
940 {
941 mic_header2[8] = mpdu[24] & 0x0f; /* mute bits 15 - 4 */
942 mic_header2[9] = mpdu[25] & 0x00;
943 }
944
945 if (qc_exists && a4_exists)
946 {
947 for (i=0;i<6;i++) mic_header2[8+i] = mpdu[24+i]; /* A4 */
948
949 mic_header2[14] = mpdu[30] & 0x0f;
950 mic_header2[15] = mpdu[31] & 0x00;
951 }
952 }
953
954
955 /************************************************/
956 /* construct_mic_iv() */
957 /* Builds the MIC IV from header fields and PN */
958 /************************************************/
959
960 void construct_mic_iv(
961 unsigned char *mic_iv,
962 int qc_exists,
963 int a4_exists,
964 unsigned char *mpdu,
965 unsigned int payload_length,
966 unsigned char *pn_vector)
967 {
968 int i;
969
970 mic_iv[0] = 0x59;
971 if (qc_exists && a4_exists)
972 mic_iv[1] = mpdu[30] & 0x0f; /* QoS_TC */
973 if (qc_exists && !a4_exists)
974 mic_iv[1] = mpdu[24] & 0x0f; /* mute bits 7-4 */
975 if (!qc_exists)
976 mic_iv[1] = 0x00;
977 for (i = 2; i < 8; i++)
978 mic_iv[i] = mpdu[i + 8]; /* mic_iv[2:7] = A2[0:5] = mpdu[10:15] */
979 #ifdef CONSISTENT_PN_ORDER
980 for (i = 8; i < 14; i++)
981 mic_iv[i] = pn_vector[i - 8]; /* mic_iv[8:13] = PN[0:5] */
982 #else
983 for (i = 8; i < 14; i++)
984 mic_iv[i] = pn_vector[13 - i]; /* mic_iv[8:13] = PN[5:0] */
985 #endif
986 i = (payload_length / 256);
987 i = (payload_length % 256);
988 mic_iv[14] = (unsigned char) (payload_length / 256);
989 mic_iv[15] = (unsigned char) (payload_length % 256);
990
991 }
992
993
994
995 /************************************/
996 /* bitwise_xor() */
997 /* A 128 bit, bitwise exclusive or */
998 /************************************/
999
1000 void bitwise_xor(unsigned char *ina, unsigned char *inb, unsigned char *out)
1001 {
1002 int i;
1003 for (i=0; i<16; i++)
1004 {
1005 out[i] = ina[i] ^ inb[i];
1006 }
1007 }
1008
1009
1010 void aes128k128d(unsigned char *key, unsigned char *data, unsigned char *ciphertext)
1011 {
1012 int round;
1013 int i;
1014 unsigned char intermediatea[16];
1015 unsigned char intermediateb[16];
1016 unsigned char round_key[16];
1017
1018 for(i=0; i<16; i++) round_key[i] = key[i];
1019
1020 for (round = 0; round < 11; round++)
1021 {
1022 if (round == 0)
1023 {
1024 xor_128(round_key, data, ciphertext);
1025 next_key(round_key, round);
1026 }
1027 else if (round == 10)
1028 {
1029 byte_sub(ciphertext, intermediatea);
1030 shift_row(intermediatea, intermediateb);
1031 xor_128(intermediateb, round_key, ciphertext);
1032 }
1033 else /* 1 - 9 */
1034 {
1035 byte_sub(ciphertext, intermediatea);
1036 shift_row(intermediatea, intermediateb);
1037 mix_column(&intermediateb[0], &intermediatea[0]);
1038 mix_column(&intermediateb[4], &intermediatea[4]);
1039 mix_column(&intermediateb[8], &intermediatea[8]);
1040 mix_column(&intermediateb[12], &intermediatea[12]);
1041 xor_128(intermediatea, round_key, ciphertext);
1042 next_key(round_key, round);
1043 }
1044 }
1045
1046 }
1047
1048 void construct_ctr_preload(
1049 unsigned char *ctr_preload,
1050 int a4_exists,
1051 int qc_exists,
1052 unsigned char *mpdu,
1053 unsigned char *pn_vector,
1054 int c)
1055 {
1056
1057 int i = 0;
1058 for (i=0; i<16; i++) ctr_preload[i] = 0x00;
1059 i = 0;
1060
1061 ctr_preload[0] = 0x01; /* flag */
1062 if (qc_exists && a4_exists) ctr_preload[1] = mpdu[30] & 0x0f; /* QoC_Control */
1063 if (qc_exists && !a4_exists) ctr_preload[1] = mpdu[24] & 0x0f;
1064
1065 for (i = 2; i < 8; i++)
1066 ctr_preload[i] = mpdu[i + 8]; /* ctr_preload[2:7] = A2[0:5] = mpdu[10:15] */
1067 #ifdef CONSISTENT_PN_ORDER
1068 for (i = 8; i < 14; i++)
1069 ctr_preload[i] = pn_vector[i - 8]; /* ctr_preload[8:13] = PN[0:5] */
1070 #else
1071 for (i = 8; i < 14; i++)
1072 ctr_preload[i] = pn_vector[13 - i]; /* ctr_preload[8:13] = PN[5:0] */
1073 #endif
1074 ctr_preload[14] = (unsigned char) (c / 256); // Ctr
1075 ctr_preload[15] = (unsigned char) (c % 256);
1076
1077 }
1078
1079
1080 //
1081 // TRUE: Success!
1082 // FALSE: Decrypt Error!
1083 //
1084 BOOLEAN RTMPSoftDecryptTKIP(
1085 IN PRTMP_ADAPTER pAd,
1086 IN PUCHAR pData,
1087 IN ULONG DataByteCnt,
1088 IN UCHAR UserPriority,
1089 IN PCIPHER_KEY pWpaKey)
1090 {
1091 UCHAR KeyID;
1092 UINT HeaderLen;
1093 UCHAR fc0;
1094 UCHAR fc1;
1095 USHORT fc;
1096 UINT frame_type;
1097 UINT frame_subtype;
1098 UINT from_ds;
1099 UINT to_ds;
1100 INT a4_exists;
1101 INT qc_exists;
1102 USHORT duration;
1103 USHORT seq_control;
1104 USHORT qos_control;
1105 UCHAR TA[MAC_ADDR_LEN];
1106 UCHAR DA[MAC_ADDR_LEN];
1107 UCHAR SA[MAC_ADDR_LEN];
1108 UCHAR RC4Key[16];
1109 UINT p1k[5]; //for mix_key;
1110 ULONG pnl;/* Least significant 16 bits of PN */
1111 ULONG pnh;/* Most significant 32 bits of PN */
1112 UINT num_blocks;
1113 UINT payload_remainder;
1114 ARCFOURCONTEXT ArcFourContext;
1115 UINT crc32 = 0;
1116 UINT trailfcs = 0;
1117 UCHAR MIC[8];
1118 UCHAR TrailMIC[8];
1119
1120 #ifdef RT_BIG_ENDIAN
1121 RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
1122 #endif
1123
1124 fc0 = *pData;
1125 fc1 = *(pData + 1);
1126
1127 fc = *((PUSHORT)pData);
1128
1129 frame_type = ((fc0 >> 2) & 0x03);
1130 frame_subtype = ((fc0 >> 4) & 0x0f);
1131
1132 from_ds = (fc1 & 0x2) >> 1;
1133 to_ds = (fc1 & 0x1);
1134
1135 a4_exists = (from_ds & to_ds);
1136 qc_exists = ((frame_subtype == 0x08) || /* Assumed QoS subtypes */
1137 (frame_subtype == 0x09) || /* Likely to change. */
1138 (frame_subtype == 0x0a) ||
1139 (frame_subtype == 0x0b)
1140 );
1141
1142 HeaderLen = 24;
1143 if (a4_exists)
1144 HeaderLen += 6;
1145
1146 KeyID = *((PUCHAR)(pData+ HeaderLen + 3));
1147 KeyID = KeyID >> 6;
1148
1149 if (pWpaKey[KeyID].KeyLen == 0)
1150 {
1151 DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptTKIP failed!(KeyID[%d] Length can not be 0)\n", KeyID));
1152 return FALSE;
1153 }
1154
1155 duration = *((PUSHORT)(pData+2));
1156
1157 seq_control = *((PUSHORT)(pData+22));
1158
1159 if (qc_exists)
1160 {
1161 if (a4_exists)
1162 {
1163 qos_control = *((PUSHORT)(pData+30));
1164 }
1165 else
1166 {
1167 qos_control = *((PUSHORT)(pData+24));
1168 }
1169 }
1170
1171 if (to_ds == 0 && from_ds == 1)
1172 {
1173 NdisMoveMemory(DA, pData+4, MAC_ADDR_LEN);
1174 NdisMoveMemory(SA, pData+16, MAC_ADDR_LEN);
1175 NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN); //BSSID
1176 }
1177 else if (to_ds == 0 && from_ds == 0 )
1178 {
1179 NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
1180 NdisMoveMemory(DA, pData+4, MAC_ADDR_LEN);
1181 NdisMoveMemory(SA, pData+10, MAC_ADDR_LEN);
1182 }
1183 else if (to_ds == 1 && from_ds == 0)
1184 {
1185 NdisMoveMemory(SA, pData+10, MAC_ADDR_LEN);
1186 NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
1187 NdisMoveMemory(DA, pData+16, MAC_ADDR_LEN);
1188 }
1189 else if (to_ds == 1 && from_ds == 1)
1190 {
1191 NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
1192 NdisMoveMemory(DA, pData+16, MAC_ADDR_LEN);
1193 NdisMoveMemory(SA, pData+22, MAC_ADDR_LEN);
1194 }
1195
1196 num_blocks = (DataByteCnt - 16) / 16;
1197 payload_remainder = (DataByteCnt - 16) % 16;
1198
1199 pnl = (*(pData + HeaderLen)) * 256 + *(pData + HeaderLen + 2);
1200 pnh = *((PULONG)(pData + HeaderLen + 4));
1201 pnh = cpu2le32(pnh);
1202 RTMPTkipMixKey(pWpaKey[KeyID].Key, TA, pnl, pnh, RC4Key, p1k);
1203
1204 ARCFOUR_INIT(&ArcFourContext, RC4Key, 16);
1205
1206 ARCFOUR_DECRYPT(&ArcFourContext, pData + HeaderLen, pData + HeaderLen + 8, DataByteCnt - HeaderLen - 8);
1207 NdisMoveMemory(&trailfcs, pData + DataByteCnt - 8 - 4, 4);
1208 crc32 = RTMP_CALC_FCS32(PPPINITFCS32, pData + HeaderLen, DataByteCnt - HeaderLen - 8 - 4); //Skip IV+EIV 8 bytes & Skip last 4 bytes(FCS).
1209 crc32 ^= 0xffffffff; /* complement */
1210
1211 if(crc32 != cpu2le32(trailfcs))
1212 {
1213 DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptTKIP, WEP Data ICV Error !\n")); //ICV error.
1214
1215 return (FALSE);
1216 }
1217
1218 NdisMoveMemory(TrailMIC, pData + DataByteCnt - 8 - 8 - 4, 8);
1219 RTMPInitMICEngine(pAd, pWpaKey[KeyID].Key, DA, SA, UserPriority, pWpaKey[KeyID].RxMic);
1220 RTMPTkipAppend(&pAd->PrivateInfo.Tx, pData + HeaderLen, DataByteCnt - HeaderLen - 8 - 12);
1221 RTMPTkipGetMIC(&pAd->PrivateInfo.Tx);
1222 NdisMoveMemory(MIC, pAd->PrivateInfo.Tx.MIC, 8);
1223
1224 if (!NdisEqualMemory(MIC, TrailMIC, 8))
1225 {
1226 DBGPRINT(RT_DEBUG_ERROR, ("RTMPSoftDecryptTKIP, WEP Data MIC Error !\n")); //MIC error.
1227 //RTMPReportMicError(pAd, &pWpaKey[KeyID]); // marked by AlbertY @ 20060630
1228 return (FALSE);
1229 }
1230
1231 #ifdef RT_BIG_ENDIAN
1232 RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
1233 #endif
1234 //DBGPRINT(RT_DEBUG_TRACE, "RTMPSoftDecryptTKIP Decript done!!\n");
1235 return TRUE;
1236 }
1237
1238
1239
1240
1241 BOOLEAN RTMPSoftDecryptAES(
1242 IN PRTMP_ADAPTER pAd,
1243 IN PUCHAR pData,
1244 IN ULONG DataByteCnt,
1245 IN PCIPHER_KEY pWpaKey)
1246 {
1247 UCHAR KeyID;
1248 UINT HeaderLen;
1249 UCHAR PN[6];
1250 UINT payload_len;
1251 UINT num_blocks;
1252 UINT payload_remainder;
1253 USHORT fc;
1254 UCHAR fc0;
1255 UCHAR fc1;
1256 UINT frame_type;
1257 UINT frame_subtype;
1258 UINT from_ds;
1259 UINT to_ds;
1260 INT a4_exists;
1261 INT qc_exists;
1262 UCHAR aes_out[16];
1263 int payload_index;
1264 UINT i;
1265 UCHAR ctr_preload[16];
1266 UCHAR chain_buffer[16];
1267 UCHAR padded_buffer[16];
1268 UCHAR mic_iv[16];
1269 UCHAR mic_header1[16];
1270 UCHAR mic_header2[16];
1271 UCHAR MIC[8];
1272 UCHAR TrailMIC[8];
1273
1274 #ifdef RT_BIG_ENDIAN
1275 RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
1276 #endif
1277
1278 fc0 = *pData;
1279 fc1 = *(pData + 1);
1280
1281 fc = *((PUSHORT)pData);
1282
1283 frame_type = ((fc0 >> 2) & 0x03);
1284 frame_subtype = ((fc0 >> 4) & 0x0f);
1285
1286 from_ds = (fc1 & 0x2) >> 1;
1287 to_ds = (fc1 & 0x1);
1288
1289 a4_exists = (from_ds & to_ds);
1290 qc_exists = ((frame_subtype == 0x08) || /* Assumed QoS subtypes */
1291 (frame_subtype == 0x09) || /* Likely to change. */
1292 (frame_subtype == 0x0a) ||
1293 (frame_subtype == 0x0b)
1294 );
1295
1296 HeaderLen = 24;
1297 if (a4_exists)
1298 HeaderLen += 6;
1299
1300 KeyID = *((PUCHAR)(pData+ HeaderLen + 3));
1301 KeyID = KeyID >> 6;
1302
1303 if (pWpaKey[KeyID].KeyLen == 0)
1304 {
1305 DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptAES failed!(KeyID[%d] Length can not be 0)\n", KeyID));
1306 return FALSE;
1307 }
1308
1309 PN[0] = *(pData+ HeaderLen);
1310 PN[1] = *(pData+ HeaderLen + 1);
1311 PN[2] = *(pData+ HeaderLen + 4);
1312 PN[3] = *(pData+ HeaderLen + 5);
1313 PN[4] = *(pData+ HeaderLen + 6);
1314 PN[5] = *(pData+ HeaderLen + 7);
1315
1316 payload_len = DataByteCnt - HeaderLen - 8 - 8; // 8 bytes for CCMP header , 8 bytes for MIC
1317 payload_remainder = (payload_len) % 16;
1318 num_blocks = (payload_len) / 16;
1319
1320
1321
1322 // Find start of payload
1323 payload_index = HeaderLen + 8; //IV+EIV
1324
1325 for (i=0; i< num_blocks; i++)
1326 {
1327 construct_ctr_preload(ctr_preload,
1328 a4_exists,
1329 qc_exists,
1330 pData,
1331 PN,
1332 i+1 );
1333
1334 aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);
1335
1336 bitwise_xor(aes_out, pData + payload_index, chain_buffer);
1337 NdisMoveMemory(pData + payload_index - 8, chain_buffer, 16);
1338 payload_index += 16;
1339 }
1340
1341 //
1342 // If there is a short final block, then pad it
1343 // encrypt it and copy the unpadded part back
1344 //
1345 if (payload_remainder > 0)
1346 {
1347 construct_ctr_preload(ctr_preload,
1348 a4_exists,
1349 qc_exists,
1350 pData,
1351 PN,
1352 num_blocks + 1);
1353
1354 NdisZeroMemory(padded_buffer, 16);
1355 NdisMoveMemory(padded_buffer, pData + payload_index, payload_remainder);
1356
1357 aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);
1358
1359 bitwise_xor(aes_out, padded_buffer, chain_buffer);
1360 NdisMoveMemory(pData + payload_index - 8, chain_buffer, payload_remainder);
1361 payload_index += payload_remainder;
1362 }
1363
1364 //
1365 // Descrypt the MIC
1366 //
1367 construct_ctr_preload(ctr_preload,
1368 a4_exists,
1369 qc_exists,
1370 pData,
1371 PN,
1372 0);
1373 NdisZeroMemory(padded_buffer, 16);
1374 NdisMoveMemory(padded_buffer, pData + payload_index, 8);
1375
1376 aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);
1377
1378 bitwise_xor(aes_out, padded_buffer, chain_buffer);
1379
1380 NdisMoveMemory(TrailMIC, chain_buffer, 8);
1381
1382 //
1383 // Calculate MIC
1384 //
1385
1386 //Force the protected frame bit on
1387 *(pData + 1) = *(pData + 1) | 0x40;
1388
1389 // Find start of payload
1390 // Because the CCMP header has been removed
1391 payload_index = HeaderLen;
1392
1393 construct_mic_iv(
1394 mic_iv,
1395 qc_exists,
1396 a4_exists,
1397 pData,
1398 payload_len,
1399 PN);
1400
1401 construct_mic_header1(
1402 mic_header1,
1403 HeaderLen,
1404 pData);
1405
1406 construct_mic_header2(
1407 mic_header2,
1408 pData,
1409 a4_exists,
1410 qc_exists);
1411
1412 aes128k128d(pWpaKey[KeyID].Key, mic_iv, aes_out);
1413 bitwise_xor(aes_out, mic_header1, chain_buffer);
1414 aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
1415 bitwise_xor(aes_out, mic_header2, chain_buffer);
1416 aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
1417
1418 // iterate through each 16 byte payload block
1419 for (i = 0; i < num_blocks; i++)
1420 {
1421 bitwise_xor(aes_out, pData + payload_index, chain_buffer);
1422 payload_index += 16;
1423 aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
1424 }
1425
1426 // Add on the final payload block if it needs padding
1427 if (payload_remainder > 0)
1428 {
1429 NdisZeroMemory(padded_buffer, 16);
1430 NdisMoveMemory(padded_buffer, pData + payload_index, payload_remainder);
1431
1432 bitwise_xor(aes_out, padded_buffer, chain_buffer);
1433 aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
1434 }
1435
1436 // aes_out contains padded mic, discard most significant
1437 // 8 bytes to generate 64 bit MIC
1438 for (i = 0 ; i < 8; i++) MIC[i] = aes_out[i];
1439
1440 if (!NdisEqualMemory(MIC, TrailMIC, 8))
1441 {
1442 DBGPRINT(RT_DEBUG_ERROR, ("RTMPSoftDecryptAES, MIC Error !\n")); //MIC error.
1443 return FALSE;
1444 }
1445
1446 #ifdef RT_BIG_ENDIAN
1447 RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
1448 #endif
1449
1450 return TRUE;
1451 }
1452
1453 /****************************************/
1454 /* aes128k128d() */
1455 /* Performs a 128 bit AES encrypt with */
1456 /* 128 bit data. */
1457 /****************************************/
1458 VOID xor_128(
1459 IN PUCHAR a,
1460 IN PUCHAR b,
1461 OUT PUCHAR out)
1462 {
1463 INT i;
1464
1465 for (i=0;i<16; i++)
1466 {
1467 out[i] = a[i] ^ b[i];
1468 }
1469 }
1470
1471 VOID next_key(
1472 IN PUCHAR key,
1473 IN INT round)
1474 {
1475 UCHAR rcon;
1476 UCHAR sbox_key[4];
1477 UCHAR rcon_table[12] =
1478 {
1479 0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80,
1480 0x1b, 0x36, 0x36, 0x36
1481 };
1482
1483 sbox_key[0] = RTMPCkipSbox(key[13]);
1484 sbox_key[1] = RTMPCkipSbox(key[14]);
1485 sbox_key[2] = RTMPCkipSbox(key[15]);
1486 sbox_key[3] = RTMPCkipSbox(key[12]);
1487
1488 rcon = rcon_table[round];
1489
1490 xor_32(&key[0], sbox_key, &key[0]);
1491 key[0] = key[0] ^ rcon;
1492
1493 xor_32(&key[4], &key[0], &key[4]);
1494 xor_32(&key[8], &key[4], &key[8]);
1495 xor_32(&key[12], &key[8], &key[12]);
1496 }
1497
1498 VOID xor_32(
1499 IN PUCHAR a,
1500 IN PUCHAR b,
1501 OUT PUCHAR out)
1502 {
1503 INT i;
1504
1505 for (i=0;i<4; i++)
1506 {
1507 out[i] = a[i] ^ b[i];
1508 }
1509 }
1510
1511 VOID byte_sub(
1512 IN PUCHAR in,
1513 OUT PUCHAR out)
1514 {
1515 INT i;
1516
1517 for (i=0; i< 16; i++)
1518 {
1519 out[i] = RTMPCkipSbox(in[i]);
1520 }
1521 }
1522
1523 UCHAR RTMPCkipSbox(
1524 IN UCHAR a)
1525 {
1526 return SboxTable[(int)a];
1527 }
1528
1529 VOID shift_row(
1530 IN PUCHAR in,
1531 OUT PUCHAR out)
1532 {
1533 out[0] = in[0];
1534 out[1] = in[5];
1535 out[2] = in[10];
1536 out[3] = in[15];
1537 out[4] = in[4];
1538 out[5] = in[9];
1539 out[6] = in[14];
1540 out[7] = in[3];
1541 out[8] = in[8];
1542 out[9] = in[13];
1543 out[10] = in[2];
1544 out[11] = in[7];
1545 out[12] = in[12];
1546 out[13] = in[1];
1547 out[14] = in[6];
1548 out[15] = in[11];
1549 }
1550
1551 VOID mix_column(
1552 IN PUCHAR in,
1553 OUT PUCHAR out)
1554 {
1555 INT i;
1556 UCHAR add1b[4];
1557 UCHAR add1bf7[4];
1558 UCHAR rotl[4];
1559 UCHAR swap_halfs[4];
1560 UCHAR andf7[4];
1561 UCHAR rotr[4];
1562 UCHAR temp[4];
1563 UCHAR tempb[4];
1564
1565 for (i=0 ; i<4; i++)
1566 {
1567 if ((in[i] & 0x80)== 0x80)
1568 add1b[i] = 0x1b;
1569 else
1570 add1b[i] = 0x00;
1571 }
1572
1573 swap_halfs[0] = in[2]; /* Swap halfs */
1574 swap_halfs[1] = in[3];
1575 swap_halfs[2] = in[0];
1576 swap_halfs[3] = in[1];
1577
1578 rotl[0] = in[3]; /* Rotate left 8 bits */
1579 rotl[1] = in[0];
1580 rotl[2] = in[1];
1581 rotl[3] = in[2];
1582
1583 andf7[0] = in[0] & 0x7f;
1584 andf7[1] = in[1] & 0x7f;
1585 andf7[2] = in[2] & 0x7f;
1586 andf7[3] = in[3] & 0x7f;
1587
1588 for (i = 3; i>0; i--) /* logical shift left 1 bit */
1589 {
1590 andf7[i] = andf7[i] << 1;
1591 if ((andf7[i-1] & 0x80) == 0x80)
1592 {
1593 andf7[i] = (andf7[i] | 0x01);
1594 }
1595 }
1596 andf7[0] = andf7[0] << 1;
1597 andf7[0] = andf7[0] & 0xfe;
1598
1599 xor_32(add1b, andf7, add1bf7);
1600
1601 xor_32(in, add1bf7, rotr);
1602
1603 temp[0] = rotr[0]; /* Rotate right 8 bits */
1604 rotr[0] = rotr[1];
1605 rotr[1] = rotr[2];
1606 rotr[2] = rotr[3];
1607 rotr[3] = temp[0];
1608
1609 xor_32(add1bf7, rotr, temp);
1610 xor_32(swap_halfs, rotl,tempb);
1611 xor_32(temp, tempb, out);
1612 }
1613
Nao low-level kernelspace/userspace library that runs on our robots, Nao-Team HTWK