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