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 45u32 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 80u32 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/* */ 118struct 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*/ 157unsigned 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*/ 188void 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*/ 217void 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*/ 248void 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*/ 292void 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*/ 319void 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*/ 358void 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*/ 402void 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*/ 443BOOLEAN 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*/ 503void 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 558u32 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 574u32 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 587void 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/* */ 701BOOLEAN 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