]> bbs.cooldavid.org Git - net-next-2.6.git/blame - drivers/staging/rt3090/common/cmm_tkip.c
git://bbs.cooldavid.org / net-next-2.6.git / blame
? search:
summary | shortlog | log | commit | commitdiff | tree
blob | blame (incremental) | history | HEAD
Staging: rt2860: add RT3090 chipset support
[net-next-2.6.git] / drivers / staging / rt3090 / common / cmm_tkip.c
CommitLineData
36c7928c
BZ		 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 cmm_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
41// Rotation functions on 32 bit values
42#define ROL32( A, n ) \
43 ( ((A) << (n)) | ( ((A)>>(32-(n))) & ( (1UL << (n)) - 1 ) ) )
44#define ROR32( A, n ) ROL32( (A), 32-(n) )
45
46UINT Tkip_Sbox_Lower[256] =
47{
48 0xA5,0x84,0x99,0x8D,0x0D,0xBD,0xB1,0x54,
49 0x50,0x03,0xA9,0x7D,0x19,0x62,0xE6,0x9A,
50 0x45,0x9D,0x40,0x87,0x15,0xEB,0xC9,0x0B,
51 0xEC,0x67,0xFD,0xEA,0xBF,0xF7,0x96,0x5B,
52 0xC2,0x1C,0xAE,0x6A,0x5A,0x41,0x02,0x4F,
53 0x5C,0xF4,0x34,0x08,0x93,0x73,0x53,0x3F,
54 0x0C,0x52,0x65,0x5E,0x28,0xA1,0x0F,0xB5,
55 0x09,0x36,0x9B,0x3D,0x26,0x69,0xCD,0x9F,
56 0x1B,0x9E,0x74,0x2E,0x2D,0xB2,0xEE,0xFB,
57 0xF6,0x4D,0x61,0xCE,0x7B,0x3E,0x71,0x97,
58 0xF5,0x68,0x00,0x2C,0x60,0x1F,0xC8,0xED,
59 0xBE,0x46,0xD9,0x4B,0xDE,0xD4,0xE8,0x4A,
60 0x6B,0x2A,0xE5,0x16,0xC5,0xD7,0x55,0x94,
61 0xCF,0x10,0x06,0x81,0xF0,0x44,0xBA,0xE3,
62 0xF3,0xFE,0xC0,0x8A,0xAD,0xBC,0x48,0x04,
63 0xDF,0xC1,0x75,0x63,0x30,0x1A,0x0E,0x6D,
64 0x4C,0x14,0x35,0x2F,0xE1,0xA2,0xCC,0x39,
65 0x57,0xF2,0x82,0x47,0xAC,0xE7,0x2B,0x95,
66 0xA0,0x98,0xD1,0x7F,0x66,0x7E,0xAB,0x83,
67 0xCA,0x29,0xD3,0x3C,0x79,0xE2,0x1D,0x76,
68 0x3B,0x56,0x4E,0x1E,0xDB,0x0A,0x6C,0xE4,
69 0x5D,0x6E,0xEF,0xA6,0xA8,0xA4,0x37,0x8B,
70 0x32,0x43,0x59,0xB7,0x8C,0x64,0xD2,0xE0,
71 0xB4,0xFA,0x07,0x25,0xAF,0x8E,0xE9,0x18,
72 0xD5,0x88,0x6F,0x72,0x24,0xF1,0xC7,0x51,
73 0x23,0x7C,0x9C,0x21,0xDD,0xDC,0x86,0x85,
74 0x90,0x42,0xC4,0xAA,0xD8,0x05,0x01,0x12,
75 0xA3,0x5F,0xF9,0xD0,0x91,0x58,0x27,0xB9,
76 0x38,0x13,0xB3,0x33,0xBB,0x70,0x89,0xA7,
77 0xB6,0x22,0x92,0x20,0x49,0xFF,0x78,0x7A,
78 0x8F,0xF8,0x80,0x17,0xDA,0x31,0xC6,0xB8,
79 0xC3,0xB0,0x77,0x11,0xCB,0xFC,0xD6,0x3A
80};
81
82UINT Tkip_Sbox_Upper[256] =
83{
84 0xC6,0xF8,0xEE,0xF6,0xFF,0xD6,0xDE,0x91,
85 0x60,0x02,0xCE,0x56,0xE7,0xB5,0x4D,0xEC,
86 0x8F,0x1F,0x89,0xFA,0xEF,0xB2,0x8E,0xFB,
87 0x41,0xB3,0x5F,0x45,0x23,0x53,0xE4,0x9B,
88 0x75,0xE1,0x3D,0x4C,0x6C,0x7E,0xF5,0x83,
89 0x68,0x51,0xD1,0xF9,0xE2,0xAB,0x62,0x2A,
90 0x08,0x95,0x46,0x9D,0x30,0x37,0x0A,0x2F,
91 0x0E,0x24,0x1B,0xDF,0xCD,0x4E,0x7F,0xEA,
92 0x12,0x1D,0x58,0x34,0x36,0xDC,0xB4,0x5B,
93 0xA4,0x76,0xB7,0x7D,0x52,0xDD,0x5E,0x13,
94 0xA6,0xB9,0x00,0xC1,0x40,0xE3,0x79,0xB6,
95 0xD4,0x8D,0x67,0x72,0x94,0x98,0xB0,0x85,
96 0xBB,0xC5,0x4F,0xED,0x86,0x9A,0x66,0x11,
97 0x8A,0xE9,0x04,0xFE,0xA0,0x78,0x25,0x4B,
98 0xA2,0x5D,0x80,0x05,0x3F,0x21,0x70,0xF1,
99 0x63,0x77,0xAF,0x42,0x20,0xE5,0xFD,0xBF,
100 0x81,0x18,0x26,0xC3,0xBE,0x35,0x88,0x2E,
101 0x93,0x55,0xFC,0x7A,0xC8,0xBA,0x32,0xE6,
102 0xC0,0x19,0x9E,0xA3,0x44,0x54,0x3B,0x0B,
103 0x8C,0xC7,0x6B,0x28,0xA7,0xBC,0x16,0xAD,
104 0xDB,0x64,0x74,0x14,0x92,0x0C,0x48,0xB8,
105 0x9F,0xBD,0x43,0xC4,0x39,0x31,0xD3,0xF2,
106 0xD5,0x8B,0x6E,0xDA,0x01,0xB1,0x9C,0x49,
107 0xD8,0xAC,0xF3,0xCF,0xCA,0xF4,0x47,0x10,
108 0x6F,0xF0,0x4A,0x5C,0x38,0x57,0x73,0x97,
109 0xCB,0xA1,0xE8,0x3E,0x96,0x61,0x0D,0x0F,
110 0xE0,0x7C,0x71,0xCC,0x90,0x06,0xF7,0x1C,
111 0xC2,0x6A,0xAE,0x69,0x17,0x99,0x3A,0x27,
112 0xD9,0xEB,0x2B,0x22,0xD2,0xA9,0x07,0x33,
113 0x2D,0x3C,0x15,0xC9,0x87,0xAA,0x50,0xA5,
114 0x03,0x59,0x09,0x1A,0x65,0xD7,0x84,0xD0,
115 0x82,0x29,0x5A,0x1E,0x7B,0xA8,0x6D,0x2C
116};
117
118//
119// Expanded IV for TKIP function.
120//
121typedef struct PACKED _IV_CONTROL_
122{
123 union PACKED
124 {
125 struct PACKED
126 {
127 UCHAR rc0;
128 UCHAR rc1;
129 UCHAR rc2;
130
131 union PACKED
132 {
133 struct PACKED
134 {
135#ifdef RT_BIG_ENDIAN
136 UCHAR KeyID:2;
137 UCHAR ExtIV:1;
138 UCHAR Rsvd:5;
139#else
140 UCHAR Rsvd:5;
141 UCHAR ExtIV:1;
142 UCHAR KeyID:2;
143#endif
144 } field;
145 UCHAR Byte;
146 } CONTROL;
147 } field;
148
149 ULONG word;
150 } IV16;
151
152 ULONG IV32;
153} TKIP_IV, *PTKIP_IV;
154
155
156/*
157 ========================================================================
158
159 Routine Description:
160 Convert from UCHAR[] to ULONG in a portable way
161
162 Arguments:
163 pMICKey pointer to MIC Key
164
165 Return Value:
166 None
167
168 Note:
169
170 ========================================================================
171*/
172ULONG RTMPTkipGetUInt32(
173 IN PUCHAR pMICKey)
174{
175 ULONG res = 0;
176 INT i;
177
178 for (i = 0; i < 4; i++)
179 {
180 res |= (*pMICKey++) << (8 * i);
181 }
182
183 return res;
184}
185
186/*
187 ========================================================================
188
189 Routine Description:
190 Convert from ULONG to UCHAR[] in a portable way
191
192 Arguments:
193 pDst pointer to destination for convert ULONG to UCHAR[]
194 val the value for convert
195
196 Return Value:
197 None
198
199 IRQL = DISPATCH_LEVEL
200
201 Note:
202
203 ========================================================================
204*/
205VOID RTMPTkipPutUInt32(
206 IN OUT PUCHAR pDst,
207 IN ULONG val)
208{
209 INT i;
210
211 for(i = 0; i < 4; i++)
212 {
213 *pDst++ = (UCHAR) (val & 0xff);
214 val >>= 8;
215 }
216}
217
218/*
219 ========================================================================
220
221 Routine Description:
222 Set the MIC Key.
223
224 Arguments:
225 pAd Pointer to our adapter
226 pMICKey pointer to MIC Key
227
228 Return Value:
229 None
230
231 IRQL = DISPATCH_LEVEL
232
233 Note:
234
235 ========================================================================
236*/
237VOID RTMPTkipSetMICKey(
238 IN PTKIP_KEY_INFO pTkip,
239 IN PUCHAR pMICKey)
240{
241 // Set the key
242 pTkip->K0 = RTMPTkipGetUInt32(pMICKey);
243 pTkip->K1 = RTMPTkipGetUInt32(pMICKey + 4);
244 // and reset the message
245 pTkip->L = pTkip->K0;
246 pTkip->R = pTkip->K1;
247 pTkip->nBytesInM = 0;
248 pTkip->M = 0;
249}
250
251/*
252 ========================================================================
253
254 Routine Description:
255 Calculate the MIC Value.
256
257 Arguments:
258 pAd Pointer to our adapter
259 uChar Append this uChar
260
261 Return Value:
262 None
263
264 IRQL = DISPATCH_LEVEL
265
266 Note:
267
268 ========================================================================
269*/
270VOID RTMPTkipAppendByte(
271 IN PTKIP_KEY_INFO pTkip,
272 IN UCHAR uChar)
273{
274 // Append the byte to our word-sized buffer
275 pTkip->M |= (uChar << (8* pTkip->nBytesInM));
276 pTkip->nBytesInM++;
277 // Process the word if it is full.
278 if( pTkip->nBytesInM >= 4 )
279 {
280 pTkip->L ^= pTkip->M;
281 pTkip->R ^= ROL32( pTkip->L, 17 );
282 pTkip->L += pTkip->R;
283 pTkip->R ^= ((pTkip->L & 0xff00ff00) >> 8) | ((pTkip->L & 0x00ff00ff) << 8);
284 pTkip->L += pTkip->R;
285 pTkip->R ^= ROL32( pTkip->L, 3 );
286 pTkip->L += pTkip->R;
287 pTkip->R ^= ROR32( pTkip->L, 2 );
288 pTkip->L += pTkip->R;
289 // Clear the buffer
290 pTkip->M = 0;
291 pTkip->nBytesInM = 0;
292 }
293}
294
295/*
296 ========================================================================
297
298 Routine Description:
299 Calculate the MIC Value.
300
301 Arguments:
302 pAd Pointer to our adapter
303 pSrc Pointer to source data for Calculate MIC Value
304 Len Indicate the length of the source data
305
306 Return Value:
307 None
308
309 IRQL = DISPATCH_LEVEL
310
311 Note:
312
313 ========================================================================
314*/
315VOID RTMPTkipAppend(
316 IN PTKIP_KEY_INFO pTkip,
317 IN PUCHAR pSrc,
318 IN UINT nBytes)
319{
320 // This is simple
321 while(nBytes > 0)
322 {
323 RTMPTkipAppendByte(pTkip, *pSrc++);
324 nBytes--;
325 }
326}
327
328/*
329 ========================================================================
330
331 Routine Description:
332 Get the MIC Value.
333
334 Arguments:
335 pAd Pointer to our adapter
336
337 Return Value:
338 None
339
340 IRQL = DISPATCH_LEVEL
341
342 Note:
343 the MIC Value is store in pAd->PrivateInfo.MIC
344 ========================================================================
345*/
346VOID RTMPTkipGetMIC(
347 IN PTKIP_KEY_INFO pTkip)
348{
349 // Append the minimum padding
350 RTMPTkipAppendByte(pTkip, 0x5a );
351 RTMPTkipAppendByte(pTkip, 0 );
352 RTMPTkipAppendByte(pTkip, 0 );
353 RTMPTkipAppendByte(pTkip, 0 );
354 RTMPTkipAppendByte(pTkip, 0 );
355 // and then zeroes until the length is a multiple of 4
356 while( pTkip->nBytesInM != 0 )
357 {
358 RTMPTkipAppendByte(pTkip, 0 );
359 }
360 // The appendByte function has already computed the result.
361 RTMPTkipPutUInt32(pTkip->MIC, pTkip->L);
362 RTMPTkipPutUInt32(pTkip->MIC + 4, pTkip->R);
363}
364
365/*
366 ========================================================================
367
368 Routine Description:
369 Init Tkip function.
370
371 Arguments:
372 pAd Pointer to our adapter
373 pTKey Pointer to the Temporal Key (TK), TK shall be 128bits.
374 KeyId TK Key ID
375 pTA Pointer to transmitter address
376 pMICKey pointer to MIC Key
377
378 Return Value:
379 None
380
381 IRQL = DISPATCH_LEVEL
382
383 Note:
384
385 ========================================================================
386*/
387VOID RTMPInitTkipEngine(
388 IN PRTMP_ADAPTER pAd,
389 IN PUCHAR pKey,
390 IN UCHAR KeyId,
391 IN PUCHAR pTA,
392 IN PUCHAR pMICKey,
393 IN PUCHAR pTSC,
394 OUT PULONG pIV16,
395 OUT PULONG pIV32)
396{
397 TKIP_IV tkipIv;
398
399 // Prepare 8 bytes TKIP encapsulation for MPDU
400 NdisZeroMemory(&tkipIv, sizeof(TKIP_IV));
401 tkipIv.IV16.field.rc0 = *(pTSC + 1);
402 tkipIv.IV16.field.rc1 = (tkipIv.IV16.field.rc0 | 0x20) & 0x7f;
403 tkipIv.IV16.field.rc2 = *pTSC;
404 tkipIv.IV16.field.CONTROL.field.ExtIV = 1; // 0: non-extended IV, 1: an extended IV
405 tkipIv.IV16.field.CONTROL.field.KeyID = KeyId;
406// tkipIv.IV32 = *(PULONG)(pTSC + 2);
407 NdisMoveMemory(&tkipIv.IV32, (pTSC + 2), 4); // Copy IV
408
409 *pIV16 = tkipIv.IV16.word;
410 *pIV32 = tkipIv.IV32;
411}
412
413/*
414 ========================================================================
415
416 Routine Description:
417 Init MIC Value calculation function which include set MIC key &
418 calculate first 16 bytes (DA + SA + priority + 0)
419
420 Arguments:
421 pAd Pointer to our adapter
422 pTKey Pointer to the Temporal Key (TK), TK shall be 128bits.
423 pDA Pointer to DA address
424 pSA Pointer to SA address
425 pMICKey pointer to MIC Key
426
427 Return Value:
428 None
429
430 Note:
431
432 ========================================================================
433*/
434VOID RTMPInitMICEngine(
435 IN PRTMP_ADAPTER pAd,
436 IN PUCHAR pKey,
437 IN PUCHAR pDA,
438 IN PUCHAR pSA,
439 IN UCHAR UserPriority,
440 IN PUCHAR pMICKey)
441{
442 ULONG Priority = UserPriority;
443
444 // Init MIC value calculation
445 RTMPTkipSetMICKey(&pAd->PrivateInfo.Tx, pMICKey);
446 // DA
447 RTMPTkipAppend(&pAd->PrivateInfo.Tx, pDA, MAC_ADDR_LEN);
448 // SA
449 RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSA, MAC_ADDR_LEN);
450 // Priority + 3 bytes of 0
451 RTMPTkipAppend(&pAd->PrivateInfo.Tx, (PUCHAR)&Priority, 4);
452}
453
454/*
455 ========================================================================
456
457 Routine Description:
458 Compare MIC value of received MSDU
459
460 Arguments:
461 pAd Pointer to our adapter
462 pSrc Pointer to the received Plain text data
463 pDA Pointer to DA address
464 pSA Pointer to SA address
465 pMICKey pointer to MIC Key
466 Len the length of the received plain text data exclude MIC value
467
468 Return Value:
469 TRUE MIC value matched
470 FALSE MIC value mismatched
471
472 IRQL = DISPATCH_LEVEL
473
474 Note:
475
476 ========================================================================
477*/
478BOOLEAN RTMPTkipCompareMICValue(
479 IN PRTMP_ADAPTER pAd,
480 IN PUCHAR pSrc,
481 IN PUCHAR pDA,
482 IN PUCHAR pSA,
483 IN PUCHAR pMICKey,
484 IN UCHAR UserPriority,
485 IN UINT Len)
486{
487 UCHAR OldMic[8];
488 ULONG Priority = UserPriority;
489
490 // Init MIC value calculation
491 RTMPTkipSetMICKey(&pAd->PrivateInfo.Rx, pMICKey);
492 // DA
493 RTMPTkipAppend(&pAd->PrivateInfo.Rx, pDA, MAC_ADDR_LEN);
494 // SA
495 RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSA, MAC_ADDR_LEN);
496 // Priority + 3 bytes of 0
497 RTMPTkipAppend(&pAd->PrivateInfo.Rx, (PUCHAR)&Priority, 4);
498
499 // Calculate MIC value from plain text data
500 RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSrc, Len);
501
502 // Get MIC valude from received frame
503 NdisMoveMemory(OldMic, pSrc + Len, 8);
504
505 // Get MIC value from decrypted plain data
506 RTMPTkipGetMIC(&pAd->PrivateInfo.Rx);
507
508 // Move MIC value from MSDU, this steps should move to data path.
509 // Since the MIC value might cross MPDUs.
510 if(!NdisEqualMemory(pAd->PrivateInfo.Rx.MIC, OldMic, 8))
511 {
512 DBGPRINT_RAW(RT_DEBUG_ERROR, ("RTMPTkipCompareMICValue(): TKIP MIC Error !\n")); //MIC error.
513
514
515 return (FALSE);
516 }
517 return (TRUE);
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 pLLC LLC header
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*/
545BOOLEAN RTMPTkipCompareMICValueWithLLC(
546 IN PRTMP_ADAPTER pAd,
547 IN PUCHAR pLLC,
548 IN PUCHAR pSrc,
549 IN PUCHAR pDA,
550 IN PUCHAR pSA,
551 IN PUCHAR pMICKey,
552 IN UINT Len)
553{
554 UCHAR OldMic[8];
555 ULONG Priority = 0;
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 // Start with LLC header
567 RTMPTkipAppend(&pAd->PrivateInfo.Rx, pLLC, 8);
568
569 // Calculate MIC value from plain text data
570 RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSrc, Len);
571
572 // Get MIC valude from received frame
573 NdisMoveMemory(OldMic, pSrc + Len, 8);
574
575 // Get MIC value from decrypted plain data
576 RTMPTkipGetMIC(&pAd->PrivateInfo.Rx);
577
578 // Move MIC value from MSDU, this steps should move to data path.
579 // Since the MIC value might cross MPDUs.
580 if(!NdisEqualMemory(pAd->PrivateInfo.Rx.MIC, OldMic, 8))
581 {
582 DBGPRINT_RAW(RT_DEBUG_ERROR, ("RTMPTkipCompareMICValueWithLLC(): TKIP MIC Error !\n")); //MIC error.
583
584
585 return (FALSE);
586 }
587 return (TRUE);
588}
589/*
590 ========================================================================
591
592 Routine Description:
593 Copy frame from waiting queue into relative ring buffer and set
594 appropriate ASIC register to kick hardware transmit function
595
596 Arguments:
597 pAd Pointer to our adapter
598 PNDIS_PACKET Pointer to Ndis Packet for MIC calculation
599 pEncap Pointer to LLC encap data
600 LenEncap Total encap length, might be 0 which indicates no encap
601
602 Return Value:
603 None
604
605 IRQL = DISPATCH_LEVEL
606
607 Note:
608
609 ========================================================================
610*/
611VOID RTMPCalculateMICValue(
612 IN PRTMP_ADAPTER pAd,
613 IN PNDIS_PACKET pPacket,
614 IN PUCHAR pEncap,
615 IN PCIPHER_KEY pKey,
616 IN UCHAR apidx)
617{
618 PACKET_INFO PacketInfo;
619 PUCHAR pSrcBufVA;
620 UINT SrcBufLen;
621 PUCHAR pSrc;
622 UCHAR UserPriority;
623 UCHAR vlan_offset = 0;
624
625 RTMP_QueryPacketInfo(pPacket, &PacketInfo, &pSrcBufVA, &SrcBufLen);
626
627 UserPriority = RTMP_GET_PACKET_UP(pPacket);
628 pSrc = pSrcBufVA;
629
630 // determine if this is a vlan packet
631 if (((*(pSrc + 12) << 8) + *(pSrc + 13)) == 0x8100)
632 vlan_offset = 4;
633
634#ifdef CONFIG_STA_SUPPORT
635#endif // CONFIG_STA_SUPPORT //
636 {
637 RTMPInitMICEngine(
638 pAd,
639 pKey->Key,
640 pSrc,
641 pSrc + 6,
642 UserPriority,
643 pKey->TxMic);
644 }
645
646
647 if (pEncap != NULL)
648 {
649 // LLC encapsulation
650 RTMPTkipAppend(&pAd->PrivateInfo.Tx, pEncap, 6);
651 // Protocol Type
652 RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSrc + 12 + vlan_offset, 2);
653 }
654 SrcBufLen -= (14 + vlan_offset);
655 pSrc += (14 + vlan_offset);
656 do
657 {
658 if (SrcBufLen > 0)
659 {
660 RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSrc, SrcBufLen);
661 }
662
663 break; // No need handle next packet
664
665 } while (TRUE); // End of copying payload
666
667 // Compute the final MIC Value
668 RTMPTkipGetMIC(&pAd->PrivateInfo.Tx);
669}
670
671
672/************************************************************/
673/* tkip_sbox() */
674/* Returns a 16 bit value from a 64K entry table. The Table */
675/* is synthesized from two 256 entry byte wide tables. */
676/************************************************************/
677
678UINT tkip_sbox(UINT index)
679{
680 UINT index_low;
681 UINT index_high;
682 UINT left, right;
683
684 index_low = (index % 256);
685 index_high = ((index >> 8) % 256);
686
687 left = Tkip_Sbox_Lower[index_low] + (Tkip_Sbox_Upper[index_low] * 256);
688 right = Tkip_Sbox_Upper[index_high] + (Tkip_Sbox_Lower[index_high] * 256);
689
690 return (left ^ right);
691}
692
693UINT rotr1(UINT a)
694{
695 unsigned int b;
696
697 if ((a & 0x01) == 0x01)
698 {
699 b = (a >> 1) | 0x8000;
700 }
701 else
702 {
703 b = (a >> 1) & 0x7fff;
704 }
705 b = b % 65536;
706 return b;
707}
708
709VOID RTMPTkipMixKey(
710 UCHAR *key,
711 UCHAR *ta,
712 ULONG pnl, /* Least significant 16 bits of PN */
713 ULONG pnh, /* Most significant 32 bits of PN */
714 UCHAR *rc4key,
715 UINT *p1k)
716{
717
718 UINT tsc0;
719 UINT tsc1;
720 UINT tsc2;
721
722 UINT ppk0;
723 UINT ppk1;
724 UINT ppk2;
725 UINT ppk3;
726 UINT ppk4;
727 UINT ppk5;
728
729 INT i;
730 INT j;
731
732 tsc0 = (unsigned int)((pnh >> 16) % 65536); /* msb */
733 tsc1 = (unsigned int)(pnh % 65536);
734 tsc2 = (unsigned int)(pnl % 65536); /* lsb */
735
736 /* Phase 1, step 1 */
737 p1k[0] = tsc1;
738 p1k[1] = tsc0;
739 p1k[2] = (UINT)(ta[0] + (ta[1]*256));
740 p1k[3] = (UINT)(ta[2] + (ta[3]*256));
741 p1k[4] = (UINT)(ta[4] + (ta[5]*256));
742
743 /* Phase 1, step 2 */
744 for (i=0; i<8; i++)
745 {
746 j = 2*(i & 1);
747 p1k[0] = (p1k[0] + tkip_sbox( (p1k[4] ^ ((256*key[1+j]) + key[j])) % 65536 )) % 65536;
748 p1k[1] = (p1k[1] + tkip_sbox( (p1k[0] ^ ((256*key[5+j]) + key[4+j])) % 65536 )) % 65536;
749 p1k[2] = (p1k[2] + tkip_sbox( (p1k[1] ^ ((256*key[9+j]) + key[8+j])) % 65536 )) % 65536;
750 p1k[3] = (p1k[3] + tkip_sbox( (p1k[2] ^ ((256*key[13+j]) + key[12+j])) % 65536 )) % 65536;
751 p1k[4] = (p1k[4] + tkip_sbox( (p1k[3] ^ (((256*key[1+j]) + key[j]))) % 65536 )) % 65536;
752 p1k[4] = (p1k[4] + i) % 65536;
753 }
754
755 /* Phase 2, Step 1 */
756 ppk0 = p1k[0];
757 ppk1 = p1k[1];
758 ppk2 = p1k[2];
759 ppk3 = p1k[3];
760 ppk4 = p1k[4];
761 ppk5 = (p1k[4] + tsc2) % 65536;
762
763 /* Phase2, Step 2 */
764 ppk0 = ppk0 + tkip_sbox( (ppk5 ^ ((256*key[1]) + key[0])) % 65536);
765 ppk1 = ppk1 + tkip_sbox( (ppk0 ^ ((256*key[3]) + key[2])) % 65536);
766 ppk2 = ppk2 + tkip_sbox( (ppk1 ^ ((256*key[5]) + key[4])) % 65536);
767 ppk3 = ppk3 + tkip_sbox( (ppk2 ^ ((256*key[7]) + key[6])) % 65536);
768 ppk4 = ppk4 + tkip_sbox( (ppk3 ^ ((256*key[9]) + key[8])) % 65536);
769 ppk5 = ppk5 + tkip_sbox( (ppk4 ^ ((256*key[11]) + key[10])) % 65536);
770
771 ppk0 = ppk0 + rotr1(ppk5 ^ ((256*key[13]) + key[12]));
772 ppk1 = ppk1 + rotr1(ppk0 ^ ((256*key[15]) + key[14]));
773 ppk2 = ppk2 + rotr1(ppk1);
774 ppk3 = ppk3 + rotr1(ppk2);
775 ppk4 = ppk4 + rotr1(ppk3);
776 ppk5 = ppk5 + rotr1(ppk4);
777
778 /* Phase 2, Step 3 */
779 /* Phase 2, Step 3 */
780
781 tsc0 = (unsigned int)((pnh >> 16) % 65536); /* msb */
782 tsc1 = (unsigned int)(pnh % 65536);
783 tsc2 = (unsigned int)(pnl % 65536); /* lsb */
784
785 rc4key[0] = (tsc2 >> 8) % 256;
786 rc4key[1] = (((tsc2 >> 8) % 256) | 0x20) & 0x7f;
787 rc4key[2] = tsc2 % 256;
788 rc4key[3] = ((ppk5 ^ ((256*key[1]) + key[0])) >> 1) % 256;
789
790 rc4key[4] = ppk0 % 256;
791 rc4key[5] = (ppk0 >> 8) % 256;
792
793 rc4key[6] = ppk1 % 256;
794 rc4key[7] = (ppk1 >> 8) % 256;
795
796 rc4key[8] = ppk2 % 256;
797 rc4key[9] = (ppk2 >> 8) % 256;
798
799 rc4key[10] = ppk3 % 256;
800 rc4key[11] = (ppk3 >> 8) % 256;
801
802 rc4key[12] = ppk4 % 256;
803 rc4key[13] = (ppk4 >> 8) % 256;
804
805 rc4key[14] = ppk5 % 256;
806 rc4key[15] = (ppk5 >> 8) % 256;
807}
808
809
810//
811// TRUE: Success!
812// FALSE: Decrypt Error!
813//
814BOOLEAN RTMPSoftDecryptTKIP(
815 IN PRTMP_ADAPTER pAd,
816 IN PUCHAR pData,
817 IN ULONG DataByteCnt,
818 IN UCHAR UserPriority,
819 IN PCIPHER_KEY pWpaKey)
820{
821 UCHAR KeyID;
822 UINT HeaderLen;
823 UCHAR fc0;
824 UCHAR fc1;
825 USHORT fc;
826 UINT frame_type;
827 UINT frame_subtype;
828 UINT from_ds;
829 UINT to_ds;
830 INT a4_exists;
831 INT qc_exists;
832 USHORT duration;
833 USHORT seq_control;
834 USHORT qos_control;
835 UCHAR TA[MAC_ADDR_LEN];
836 UCHAR DA[MAC_ADDR_LEN];
837 UCHAR SA[MAC_ADDR_LEN];
838 UCHAR RC4Key[16];
839 UINT p1k[5]; //for mix_key;
840 ULONG pnl;/* Least significant 16 bits of PN */
841 ULONG pnh;/* Most significant 32 bits of PN */
842 UINT num_blocks;
843 UINT payload_remainder;
844 ARCFOURCONTEXT ArcFourContext;
845 UINT crc32 = 0;
846 UINT trailfcs = 0;
847 UCHAR MIC[8];
848 UCHAR TrailMIC[8];
849
850#ifdef RT_BIG_ENDIAN
851 RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
852#endif
853
854 fc0 = *pData;
855 fc1 = *(pData + 1);
856
857 fc = *((PUSHORT)pData);
858
859 frame_type = ((fc0 >> 2) & 0x03);
860 frame_subtype = ((fc0 >> 4) & 0x0f);
861
862 from_ds = (fc1 & 0x2) >> 1;
863 to_ds = (fc1 & 0x1);
864
865 a4_exists = (from_ds & to_ds);
866 qc_exists = ((frame_subtype == 0x08) || /* Assumed QoS subtypes */
867 (frame_subtype == 0x09) || /* Likely to change. */
868 (frame_subtype == 0x0a) ||
869 (frame_subtype == 0x0b)
870 );
871
872 HeaderLen = 24;
873 if (a4_exists)
874 HeaderLen += 6;
875
876 KeyID = *((PUCHAR)(pData+ HeaderLen + 3));
877 KeyID = KeyID >> 6;
878
879 if (pWpaKey[KeyID].KeyLen == 0)
880 {
881 DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptTKIP failed!(KeyID[%d] Length can not be 0)\n", KeyID));
882 return FALSE;
883 }
884
885 duration = *((PUSHORT)(pData+2));
886
887 seq_control = *((PUSHORT)(pData+22));
888
889 if (qc_exists)
890 {
891 if (a4_exists)
892 {
893 qos_control = *((PUSHORT)(pData+30));
894 }
895 else
896 {
897 qos_control = *((PUSHORT)(pData+24));
898 }
899 }
900
901 if (to_ds == 0 && from_ds == 1)
902 {
903 NdisMoveMemory(DA, pData+4, MAC_ADDR_LEN);
904 NdisMoveMemory(SA, pData+16, MAC_ADDR_LEN);
905 NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN); //BSSID
906 }
907 else if (to_ds == 0 && from_ds == 0 )
908 {
909 NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
910 NdisMoveMemory(DA, pData+4, MAC_ADDR_LEN);
911 NdisMoveMemory(SA, pData+10, MAC_ADDR_LEN);
912 }
913 else if (to_ds == 1 && from_ds == 0)
914 {
915 NdisMoveMemory(SA, pData+10, MAC_ADDR_LEN);
916 NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
917 NdisMoveMemory(DA, pData+16, MAC_ADDR_LEN);
918 }
919 else if (to_ds == 1 && from_ds == 1)
920 {
921 NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
922 NdisMoveMemory(DA, pData+16, MAC_ADDR_LEN);
923 NdisMoveMemory(SA, pData+22, MAC_ADDR_LEN);
924 }
925
926 num_blocks = (DataByteCnt - 16) / 16;
927 payload_remainder = (DataByteCnt - 16) % 16;
928
929 pnl = (*(pData + HeaderLen)) * 256 + *(pData + HeaderLen + 2);
930 pnh = *((PULONG)(pData + HeaderLen + 4));
931 pnh = cpu2le32(pnh);
932 RTMPTkipMixKey(pWpaKey[KeyID].Key, TA, pnl, pnh, RC4Key, p1k);
933
934 ARCFOUR_INIT(&ArcFourContext, RC4Key, 16);
935
936 ARCFOUR_DECRYPT(&ArcFourContext, pData + HeaderLen, pData + HeaderLen + 8, DataByteCnt - HeaderLen - 8);
937 NdisMoveMemory(&trailfcs, pData + DataByteCnt - 8 - 4, 4);
938 crc32 = RTMP_CALC_FCS32(PPPINITFCS32, pData + HeaderLen, DataByteCnt - HeaderLen - 8 - 4); //Skip IV+EIV 8 bytes & Skip last 4 bytes(FCS).
939 crc32 ^= 0xffffffff; /* complement */
940
941 if(crc32 != cpu2le32(trailfcs))
942 {
943 DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptTKIP, WEP Data ICV Error !\n")); //ICV error.
944
945 return (FALSE);
946 }
947
948 NdisMoveMemory(TrailMIC, pData + DataByteCnt - 8 - 8 - 4, 8);
949 RTMPInitMICEngine(pAd, pWpaKey[KeyID].Key, DA, SA, UserPriority, pWpaKey[KeyID].RxMic);
950 RTMPTkipAppend(&pAd->PrivateInfo.Tx, pData + HeaderLen, DataByteCnt - HeaderLen - 8 - 12);
951 RTMPTkipGetMIC(&pAd->PrivateInfo.Tx);
952 NdisMoveMemory(MIC, pAd->PrivateInfo.Tx.MIC, 8);
953
954 if (!NdisEqualMemory(MIC, TrailMIC, 8))
955 {
956 DBGPRINT(RT_DEBUG_ERROR, ("RTMPSoftDecryptTKIP, WEP Data MIC Error !\n")); //MIC error.
957 //RTMPReportMicError(pAd, &pWpaKey[KeyID]); // marked by AlbertY @ 20060630
958 return (FALSE);
959 }
960
961#ifdef RT_BIG_ENDIAN
962 RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
963#endif
964 //DBGPRINT(RT_DEBUG_TRACE, "RTMPSoftDecryptTKIP Decript done!!\n");
965 return TRUE;
966}
Clone of davem's net-next-2.6 tree