drivers/staging/rt2860/common/cmm_tkip.c

   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 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 // Expanded IV for TKIP function.
 119 //
 120 typedef struct  PACKED _IV_CONTROL_
 121 {
 122         union PACKED
 123         {
 124                 struct PACKED
 125                 {
 126                         UCHAR           rc0;
 127                         UCHAR           rc1;
 128                         UCHAR           rc2;
 129
 130                         union PACKED
 131                         {
 132                                 struct PACKED
 133                                 {
 134                                         UCHAR   Rsvd:5;
 135                                         UCHAR   ExtIV:1;
 136                                         UCHAR   KeyID:2;
 137                                 }       field;
 138                                 UCHAR           Byte;
 139                         }       CONTROL;
 140                 }       field;
 141
 142                 ULONG   word;
 143         }       IV16;
 144
 145         ULONG   IV32;
 146 }       TKIP_IV, *PTKIP_IV;
 147
 148
 149 /*
 150         ========================================================================
 151
 152         Routine Description:
 153                 Convert from UCHAR[] to ULONG in a portable way
 154
 155         Arguments:
 156       pMICKey           pointer to MIC Key
 157
 158         Return Value:
 159                 None
 160
 161         Note:
 162
 163         ========================================================================
 164 */
 165 ULONG   RTMPTkipGetUInt32(
 166         IN      PUCHAR  pMICKey)
 167 {
 168         ULONG   res = 0;
 169         INT             i;
 170
 171         for (i = 0; i < 4; i++)
 172         {
 173                 res |= (*pMICKey++) << (8 * i);
 174         }
 175
 176         return res;
 177 }
 178
 179 /*
 180         ========================================================================
 181
 182         Routine Description:
 183                 Convert from ULONG to UCHAR[] in a portable way
 184
 185         Arguments:
 186       pDst                      pointer to destination for convert ULONG to UCHAR[]
 187       val                       the value for convert
 188
 189         Return Value:
 190                 None
 191
 192         IRQL = DISPATCH_LEVEL
 193
 194         Note:
 195
 196         ========================================================================
 197 */
 198 VOID    RTMPTkipPutUInt32(
 199         IN OUT  PUCHAR          pDst,
 200         IN              ULONG           val)
 201 {
 202         INT i;
 203
 204         for(i = 0; i < 4; i++)
 205         {
 206                 *pDst++ = (UCHAR) (val & 0xff);
 207                 val >>= 8;
 208         }
 209 }
 210
 211 /*
 212         ========================================================================
 213
 214         Routine Description:
 215                 Set the MIC Key.
 216
 217         Arguments:
 218       pAd               Pointer to our adapter
 219       pMICKey           pointer to MIC Key
 220
 221         Return Value:
 222                 None
 223
 224         IRQL = DISPATCH_LEVEL
 225
 226         Note:
 227
 228         ========================================================================
 229 */
 230 VOID RTMPTkipSetMICKey(
 231         IN      PTKIP_KEY_INFO  pTkip,
 232         IN      PUCHAR                  pMICKey)
 233 {
 234         // Set the key
 235         pTkip->K0 = RTMPTkipGetUInt32(pMICKey);
 236         pTkip->K1 = RTMPTkipGetUInt32(pMICKey + 4);
 237         // and reset the message
 238         pTkip->L = pTkip->K0;
 239         pTkip->R = pTkip->K1;
 240         pTkip->nBytesInM = 0;
 241         pTkip->M = 0;
 242 }
 243
 244 /*
 245         ========================================================================
 246
 247         Routine Description:
 248                 Calculate the MIC Value.
 249
 250         Arguments:
 251       pAd               Pointer to our adapter
 252       uChar                     Append this uChar
 253
 254         Return Value:
 255                 None
 256
 257         IRQL = DISPATCH_LEVEL
 258
 259         Note:
 260
 261         ========================================================================
 262 */
 263 VOID    RTMPTkipAppendByte(
 264         IN      PTKIP_KEY_INFO  pTkip,
 265         IN      UCHAR                   uChar)
 266 {
 267         // Append the byte to our word-sized buffer
 268         pTkip->M |= (uChar << (8* pTkip->nBytesInM));
 269         pTkip->nBytesInM++;
 270         // Process the word if it is full.
 271         if( pTkip->nBytesInM >= 4 )
 272         {
 273                 pTkip->L ^= pTkip->M;
 274                 pTkip->R ^= ROL32( pTkip->L, 17 );
 275                 pTkip->L += pTkip->R;
 276                 pTkip->R ^= ((pTkip->L & 0xff00ff00) >> 8) | ((pTkip->L & 0x00ff00ff) << 8);
 277                 pTkip->L += pTkip->R;
 278                 pTkip->R ^= ROL32( pTkip->L, 3 );
 279                 pTkip->L += pTkip->R;
 280                 pTkip->R ^= ROR32( pTkip->L, 2 );
 281                 pTkip->L += pTkip->R;
 282                 // Clear the buffer
 283                 pTkip->M = 0;
 284                 pTkip->nBytesInM = 0;
 285         }
 286 }
 287
 288 /*
 289         ========================================================================
 290
 291         Routine Description:
 292                 Calculate the MIC Value.
 293
 294         Arguments:
 295       pAd               Pointer to our adapter
 296       pSrc                      Pointer to source data for Calculate MIC Value
 297       Len                       Indicate the length of the source data
 298
 299         Return Value:
 300                 None
 301
 302         IRQL = DISPATCH_LEVEL
 303
 304         Note:
 305
 306         ========================================================================
 307 */
 308 VOID    RTMPTkipAppend(
 309         IN      PTKIP_KEY_INFO  pTkip,
 310         IN      PUCHAR                  pSrc,
 311         IN      UINT                    nBytes)
 312 {
 313         // This is simple
 314         while(nBytes > 0)
 315         {
 316                 RTMPTkipAppendByte(pTkip, *pSrc++);
 317                 nBytes--;
 318         }
 319 }
 320
 321 /*
 322         ========================================================================
 323
 324         Routine Description:
 325                 Get the MIC Value.
 326
 327         Arguments:
 328       pAd               Pointer to our adapter
 329
 330         Return Value:
 331                 None
 332
 333         IRQL = DISPATCH_LEVEL
 334
 335         Note:
 336                 the MIC Value is store in pAd->PrivateInfo.MIC
 337         ========================================================================
 338 */
 339 VOID    RTMPTkipGetMIC(
 340         IN      PTKIP_KEY_INFO  pTkip)
 341 {
 342         // Append the minimum padding
 343         RTMPTkipAppendByte(pTkip, 0x5a );
 344         RTMPTkipAppendByte(pTkip, 0 );
 345         RTMPTkipAppendByte(pTkip, 0 );
 346         RTMPTkipAppendByte(pTkip, 0 );
 347         RTMPTkipAppendByte(pTkip, 0 );
 348         // and then zeroes until the length is a multiple of 4
 349         while( pTkip->nBytesInM != 0 )
 350         {
 351                 RTMPTkipAppendByte(pTkip, 0 );
 352         }
 353         // The appendByte function has already computed the result.
 354         RTMPTkipPutUInt32(pTkip->MIC, pTkip->L);
 355         RTMPTkipPutUInt32(pTkip->MIC + 4, pTkip->R);
 356 }
 357
 358 /*
 359         ========================================================================
 360
 361         Routine Description:
 362                 Init Tkip function.
 363
 364         Arguments:
 365       pAd               Pointer to our adapter
 366                 pTKey       Pointer to the Temporal Key (TK), TK shall be 128bits.
 367                 KeyId           TK Key ID
 368                 pTA                     Pointer to transmitter address
 369                 pMICKey         pointer to MIC Key
 370
 371         Return Value:
 372                 None
 373
 374         IRQL = DISPATCH_LEVEL
 375
 376         Note:
 377
 378         ========================================================================
 379 */
 380 VOID    RTMPInitTkipEngine(
 381         IN      PRTMP_ADAPTER   pAd,
 382         IN      PUCHAR                  pKey,
 383         IN      UCHAR                   KeyId,
 384         IN      PUCHAR                  pTA,
 385         IN      PUCHAR                  pMICKey,
 386         IN      PUCHAR                  pTSC,
 387         OUT     PULONG                  pIV16,
 388         OUT     PULONG                  pIV32)
 389 {
 390         TKIP_IV tkipIv;
 391
 392         // Prepare 8 bytes TKIP encapsulation for MPDU
 393         NdisZeroMemory(&tkipIv, sizeof(TKIP_IV));
 394         tkipIv.IV16.field.rc0 = *(pTSC + 1);
 395         tkipIv.IV16.field.rc1 = (tkipIv.IV16.field.rc0 | 0x20) & 0x7f;
 396         tkipIv.IV16.field.rc2 = *pTSC;
 397         tkipIv.IV16.field.CONTROL.field.ExtIV = 1;  // 0: non-extended IV, 1: an extended IV
 398         tkipIv.IV16.field.CONTROL.field.KeyID = KeyId;
 399 //      tkipIv.IV32 = *(PULONG)(pTSC + 2);
 400         NdisMoveMemory(&tkipIv.IV32, (pTSC + 2), 4);   // Copy IV
 401
 402         *pIV16 = tkipIv.IV16.word;
 403         *pIV32 = tkipIv.IV32;
 404 }
 405
 406 /*
 407         ========================================================================
 408
 409         Routine Description:
 410                 Init MIC Value calculation function which include set MIC key &
 411                 calculate first 16 bytes (DA + SA + priority +  0)
 412
 413         Arguments:
 414       pAd               Pointer to our adapter
 415                 pTKey       Pointer to the Temporal Key (TK), TK shall be 128bits.
 416                 pDA                     Pointer to DA address
 417                 pSA                     Pointer to SA address
 418                 pMICKey         pointer to MIC Key
 419
 420         Return Value:
 421                 None
 422
 423         Note:
 424
 425         ========================================================================
 426 */
 427 VOID    RTMPInitMICEngine(
 428         IN      PRTMP_ADAPTER   pAd,
 429         IN      PUCHAR                  pKey,
 430         IN      PUCHAR                  pDA,
 431         IN      PUCHAR                  pSA,
 432         IN  UCHAR           UserPriority,
 433         IN      PUCHAR                  pMICKey)
 434 {
 435         ULONG Priority = UserPriority;
 436
 437         // Init MIC value calculation
 438         RTMPTkipSetMICKey(&pAd->PrivateInfo.Tx, pMICKey);
 439         // DA
 440         RTMPTkipAppend(&pAd->PrivateInfo.Tx, pDA, MAC_ADDR_LEN);
 441         // SA
 442         RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSA, MAC_ADDR_LEN);
 443         // Priority + 3 bytes of 0
 444         RTMPTkipAppend(&pAd->PrivateInfo.Tx, (PUCHAR)&Priority, 4);
 445 }
 446
 447 /*
 448         ========================================================================
 449
 450         Routine Description:
 451                 Compare MIC value of received MSDU
 452
 453         Arguments:
 454                 pAd     Pointer to our adapter
 455                 pSrc        Pointer to the received Plain text data
 456                 pDA                     Pointer to DA address
 457                 pSA                     Pointer to SA address
 458                 pMICKey         pointer to MIC Key
 459                 Len         the length of the received plain text data exclude MIC value
 460
 461         Return Value:
 462                 TRUE        MIC value matched
 463                 FALSE       MIC value mismatched
 464
 465         IRQL = DISPATCH_LEVEL
 466
 467         Note:
 468
 469         ========================================================================
 470 */
 471 BOOLEAN RTMPTkipCompareMICValue(
 472         IN      PRTMP_ADAPTER   pAd,
 473         IN      PUCHAR                  pSrc,
 474         IN      PUCHAR                  pDA,
 475         IN      PUCHAR                  pSA,
 476         IN      PUCHAR                  pMICKey,
 477         IN      UCHAR                   UserPriority,
 478         IN      UINT                    Len)
 479 {
 480         UCHAR   OldMic[8];
 481         ULONG   Priority = UserPriority;
 482
 483         // Init MIC value calculation
 484         RTMPTkipSetMICKey(&pAd->PrivateInfo.Rx, pMICKey);
 485         // DA
 486         RTMPTkipAppend(&pAd->PrivateInfo.Rx, pDA, MAC_ADDR_LEN);
 487         // SA
 488         RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSA, MAC_ADDR_LEN);
 489         // Priority + 3 bytes of 0
 490         RTMPTkipAppend(&pAd->PrivateInfo.Rx, (PUCHAR)&Priority, 4);
 491
 492         // Calculate MIC value from plain text data
 493         RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSrc, Len);
 494
 495         // Get MIC valude from received frame
 496         NdisMoveMemory(OldMic, pSrc + Len, 8);
 497
 498         // Get MIC value from decrypted plain data
 499         RTMPTkipGetMIC(&pAd->PrivateInfo.Rx);
 500
 501         // Move MIC value from MSDU, this steps should move to data path.
 502         // Since the MIC value might cross MPDUs.
 503         if(!NdisEqualMemory(pAd->PrivateInfo.Rx.MIC, OldMic, 8))
 504         {
 505                 DBGPRINT_RAW(RT_DEBUG_ERROR, ("RTMPTkipCompareMICValue(): TKIP MIC Error !\n"));  //MIC error.
 506
 507
 508                 return (FALSE);
 509         }
 510         return (TRUE);
 511 }
 512
 513 /*
 514         ========================================================================
 515
 516         Routine Description:
 517                 Copy frame from waiting queue into relative ring buffer and set
 518         appropriate ASIC register to kick hardware transmit function
 519
 520         Arguments:
 521                 pAd             Pointer to our adapter
 522                 PNDIS_PACKET    Pointer to Ndis Packet for MIC calculation
 523                 pEncap                  Pointer to LLC encap data
 524                 LenEncap                Total encap length, might be 0 which indicates no encap
 525
 526         Return Value:
 527                 None
 528
 529         IRQL = DISPATCH_LEVEL
 530
 531         Note:
 532
 533         ========================================================================
 534 */
 535 VOID    RTMPCalculateMICValue(
 536         IN      PRTMP_ADAPTER   pAd,
 537         IN      PNDIS_PACKET    pPacket,
 538         IN      PUCHAR                  pEncap,
 539         IN      PCIPHER_KEY             pKey,
 540         IN      UCHAR                   apidx)
 541 {
 542         PACKET_INFO             PacketInfo;
 543         PUCHAR                  pSrcBufVA;
 544         UINT                    SrcBufLen;
 545         PUCHAR                  pSrc;
 546     UCHAR           UserPriority;
 547         UCHAR                   vlan_offset = 0;
 548
 549         RTMP_QueryPacketInfo(pPacket, &PacketInfo, &pSrcBufVA, &SrcBufLen);
 550
 551         UserPriority = RTMP_GET_PACKET_UP(pPacket);
 552         pSrc = pSrcBufVA;
 553
 554         // determine if this is a vlan packet
 555         if (((*(pSrc + 12) << 8) + *(pSrc + 13)) == 0x8100)
 556                 vlan_offset = 4;
 557
 558         {
 559                 RTMPInitMICEngine(
 560                         pAd,
 561                         pKey->Key,
 562                         pSrc,
 563                         pSrc + 6,
 564                         UserPriority,
 565                         pKey->TxMic);
 566         }
 567
 568
 569         if (pEncap != NULL)
 570         {
 571                 // LLC encapsulation
 572                 RTMPTkipAppend(&pAd->PrivateInfo.Tx, pEncap, 6);
 573                 // Protocol Type
 574                 RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSrc + 12 + vlan_offset, 2);
 575         }
 576         SrcBufLen -= (14 + vlan_offset);
 577         pSrc += (14 + vlan_offset);
 578         do
 579         {
 580                 if (SrcBufLen > 0)
 581                 {
 582                         RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSrc, SrcBufLen);
 583                 }
 584
 585                 break;  // No need handle next packet
 586
 587         }       while (TRUE);           // End of copying payload
 588
 589         // Compute the final MIC Value
 590         RTMPTkipGetMIC(&pAd->PrivateInfo.Tx);
 591 }
 592
 593
 594 /************************************************************/
 595 /* tkip_sbox()                                                                                                                          */
 596 /* Returns a 16 bit value from a 64K entry table. The Table */
 597 /* is synthesized from two 256 entry byte wide tables.          */
 598 /************************************************************/
 599
 600 UINT tkip_sbox(UINT index)
 601 {
 602         UINT index_low;
 603         UINT index_high;
 604         UINT left, right;
 605
 606         index_low = (index % 256);
 607         index_high = ((index >> 8) % 256);
 608
 609         left = Tkip_Sbox_Lower[index_low] + (Tkip_Sbox_Upper[index_low] * 256);
 610         right = Tkip_Sbox_Upper[index_high] + (Tkip_Sbox_Lower[index_high] * 256);
 611
 612         return (left ^ right);
 613 }
 614
 615 UINT rotr1(UINT a)
 616 {
 617         unsigned int b;
 618
 619         if ((a & 0x01) == 0x01)
 620         {
 621                 b = (a >> 1) | 0x8000;
 622         }
 623         else
 624         {
 625                 b = (a >> 1) & 0x7fff;
 626         }
 627         b = b % 65536;
 628         return b;
 629 }
 630
 631 VOID RTMPTkipMixKey(
 632         UCHAR *key,
 633         UCHAR *ta,
 634         ULONG pnl, /* Least significant 16 bits of PN */
 635         ULONG pnh, /* Most significant 32 bits of PN */
 636         UCHAR *rc4key,
 637         UINT *p1k)
 638 {
 639
 640         UINT tsc0;
 641         UINT tsc1;
 642         UINT tsc2;
 643
 644         UINT ppk0;
 645         UINT ppk1;
 646         UINT ppk2;
 647         UINT ppk3;
 648         UINT ppk4;
 649         UINT ppk5;
 650
 651         INT i;
 652         INT j;
 653
 654         tsc0 = (unsigned int)((pnh >> 16) % 65536); /* msb */
 655         tsc1 = (unsigned int)(pnh % 65536);
 656         tsc2 = (unsigned int)(pnl % 65536); /* lsb */
 657
 658         /* Phase 1, step 1 */
 659         p1k[0] = tsc1;
 660         p1k[1] = tsc0;
 661         p1k[2] = (UINT)(ta[0] + (ta[1]*256));
 662         p1k[3] = (UINT)(ta[2] + (ta[3]*256));
 663         p1k[4] = (UINT)(ta[4] + (ta[5]*256));
 664
 665         /* Phase 1, step 2 */
 666         for (i=0; i<8; i++)
 667         {
 668                 j = 2*(i & 1);
 669                 p1k[0] = (p1k[0] + tkip_sbox( (p1k[4] ^ ((256*key[1+j]) + key[j])) % 65536 )) % 65536;
 670                 p1k[1] = (p1k[1] + tkip_sbox( (p1k[0] ^ ((256*key[5+j]) + key[4+j])) % 65536 )) % 65536;
 671                 p1k[2] = (p1k[2] + tkip_sbox( (p1k[1] ^ ((256*key[9+j]) + key[8+j])) % 65536 )) % 65536;
 672                 p1k[3] = (p1k[3] + tkip_sbox( (p1k[2] ^ ((256*key[13+j]) + key[12+j])) % 65536 )) % 65536;
 673                 p1k[4] = (p1k[4] + tkip_sbox( (p1k[3] ^ (((256*key[1+j]) + key[j]))) % 65536 )) % 65536;
 674                 p1k[4] = (p1k[4] + i) % 65536;
 675         }
 676
 677         /* Phase 2, Step 1 */
 678         ppk0 = p1k[0];
 679         ppk1 = p1k[1];
 680         ppk2 = p1k[2];
 681         ppk3 = p1k[3];
 682         ppk4 = p1k[4];
 683         ppk5 = (p1k[4] + tsc2) % 65536;
 684
 685         /* Phase2, Step 2 */
 686         ppk0 = ppk0 + tkip_sbox( (ppk5 ^ ((256*key[1]) + key[0])) % 65536);
 687         ppk1 = ppk1 + tkip_sbox( (ppk0 ^ ((256*key[3]) + key[2])) % 65536);
 688         ppk2 = ppk2 + tkip_sbox( (ppk1 ^ ((256*key[5]) + key[4])) % 65536);
 689         ppk3 = ppk3 + tkip_sbox( (ppk2 ^ ((256*key[7]) + key[6])) % 65536);
 690         ppk4 = ppk4 + tkip_sbox( (ppk3 ^ ((256*key[9]) + key[8])) % 65536);
 691         ppk5 = ppk5 + tkip_sbox( (ppk4 ^ ((256*key[11]) + key[10])) % 65536);
 692
 693         ppk0 = ppk0 + rotr1(ppk5 ^ ((256*key[13]) + key[12]));
 694         ppk1 = ppk1 + rotr1(ppk0 ^ ((256*key[15]) + key[14]));
 695         ppk2 = ppk2 + rotr1(ppk1);
 696         ppk3 = ppk3 + rotr1(ppk2);
 697         ppk4 = ppk4 + rotr1(ppk3);
 698         ppk5 = ppk5 + rotr1(ppk4);
 699
 700         /* Phase 2, Step 3 */
 701     /* Phase 2, Step 3 */
 702
 703         tsc0 = (unsigned int)((pnh >> 16) % 65536); /* msb */
 704         tsc1 = (unsigned int)(pnh % 65536);
 705         tsc2 = (unsigned int)(pnl % 65536); /* lsb */
 706
 707         rc4key[0] = (tsc2 >> 8) % 256;
 708         rc4key[1] = (((tsc2 >> 8) % 256) | 0x20) & 0x7f;
 709         rc4key[2] = tsc2 % 256;
 710         rc4key[3] = ((ppk5 ^ ((256*key[1]) + key[0])) >> 1) % 256;
 711
 712         rc4key[4] = ppk0 % 256;
 713         rc4key[5] = (ppk0 >> 8) % 256;
 714
 715         rc4key[6] = ppk1 % 256;
 716         rc4key[7] = (ppk1 >> 8) % 256;
 717
 718         rc4key[8] = ppk2 % 256;
 719         rc4key[9] = (ppk2 >> 8) % 256;
 720
 721         rc4key[10] = ppk3 % 256;
 722         rc4key[11] = (ppk3 >> 8) % 256;
 723
 724         rc4key[12] = ppk4 % 256;
 725         rc4key[13] = (ppk4 >> 8) % 256;
 726
 727         rc4key[14] = ppk5 % 256;
 728         rc4key[15] = (ppk5 >> 8) % 256;
 729 }
 730
 731
 732 //
 733 // TRUE: Success!
 734 // FALSE: Decrypt Error!
 735 //
 736 BOOLEAN RTMPSoftDecryptTKIP(
 737         IN PRTMP_ADAPTER pAd,
 738         IN PUCHAR       pData,
 739         IN ULONG        DataByteCnt,
 740         IN UCHAR    UserPriority,
 741         IN PCIPHER_KEY  pWpaKey)
 742 {
 743         UCHAR                   KeyID;
 744         UINT                    HeaderLen;
 745     UCHAR                       fc0;
 746         UCHAR                   fc1;
 747         USHORT                  fc;
 748         UINT                    frame_type;
 749         UINT                    frame_subtype;
 750     UINT                        from_ds;
 751     UINT                        to_ds;
 752         INT                             a4_exists;
 753         INT                             qc_exists;
 754         USHORT                  duration;
 755         USHORT                  seq_control;
 756         USHORT                  qos_control;
 757         UCHAR                   TA[MAC_ADDR_LEN];
 758         UCHAR                   DA[MAC_ADDR_LEN];
 759         UCHAR                   SA[MAC_ADDR_LEN];
 760         UCHAR                   RC4Key[16];
 761         UINT                    p1k[5]; //for mix_key;
 762         ULONG                   pnl;/* Least significant 16 bits of PN */
 763         ULONG                   pnh;/* Most significant 32 bits of PN */
 764         UINT                    num_blocks;
 765         UINT                    payload_remainder;
 766         ARCFOURCONTEXT  ArcFourContext;
 767         UINT                    crc32 = 0;
 768         UINT                    trailfcs = 0;
 769         UCHAR                   MIC[8];
 770         UCHAR                   TrailMIC[8];
 771
 772
 773         fc0 = *pData;
 774         fc1 = *(pData + 1);
 775
 776         fc = *((PUSHORT)pData);
 777
 778         frame_type = ((fc0 >> 2) & 0x03);
 779         frame_subtype = ((fc0 >> 4) & 0x0f);
 780
 781     from_ds = (fc1 & 0x2) >> 1;
 782     to_ds = (fc1 & 0x1);
 783
 784     a4_exists = (from_ds & to_ds);
 785     qc_exists = ((frame_subtype == 0x08) ||    /* Assumed QoS subtypes */
 786                   (frame_subtype == 0x09) ||   /* Likely to change.    */
 787                   (frame_subtype == 0x0a) ||
 788                   (frame_subtype == 0x0b)
 789                  );
 790
 791         HeaderLen = 24;
 792         if (a4_exists)
 793                 HeaderLen += 6;
 794
 795         KeyID = *((PUCHAR)(pData+ HeaderLen + 3));
 796         KeyID = KeyID >> 6;
 797
 798         if (pWpaKey[KeyID].KeyLen == 0)
 799         {
 800                 DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptTKIP failed!(KeyID[%d] Length can not be 0)\n", KeyID));
 801                 return FALSE;
 802         }
 803
 804         duration = *((PUSHORT)(pData+2));
 805
 806         seq_control = *((PUSHORT)(pData+22));
 807
 808         if (qc_exists)
 809         {
 810                 if (a4_exists)
 811                 {
 812                         qos_control = *((PUSHORT)(pData+30));
 813                 }
 814                 else
 815                 {
 816                         qos_control = *((PUSHORT)(pData+24));
 817                 }
 818         }
 819
 820         if (to_ds == 0 && from_ds == 1)
 821         {
 822                 NdisMoveMemory(DA, pData+4, MAC_ADDR_LEN);
 823                 NdisMoveMemory(SA, pData+16, MAC_ADDR_LEN);
 824                 NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);  //BSSID
 825         }
 826         else if (to_ds == 0 && from_ds == 0 )
 827         {
 828                 NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
 829                 NdisMoveMemory(DA, pData+4, MAC_ADDR_LEN);
 830                 NdisMoveMemory(SA, pData+10, MAC_ADDR_LEN);
 831         }
 832         else if (to_ds == 1 && from_ds == 0)
 833         {
 834                 NdisMoveMemory(SA, pData+10, MAC_ADDR_LEN);
 835                 NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
 836                 NdisMoveMemory(DA, pData+16, MAC_ADDR_LEN);
 837         }
 838         else if (to_ds == 1 && from_ds == 1)
 839         {
 840                 NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
 841                 NdisMoveMemory(DA, pData+16, MAC_ADDR_LEN);
 842                 NdisMoveMemory(SA, pData+22, MAC_ADDR_LEN);
 843         }
 844
 845         num_blocks = (DataByteCnt - 16) / 16;
 846         payload_remainder = (DataByteCnt - 16) % 16;
 847
 848         pnl = (*(pData + HeaderLen)) * 256 + *(pData + HeaderLen + 2);
 849         pnh = *((PULONG)(pData + HeaderLen + 4));
 850         pnh = cpu2le32(pnh);
 851         RTMPTkipMixKey(pWpaKey[KeyID].Key, TA, pnl, pnh, RC4Key, p1k);
 852
 853         ARCFOUR_INIT(&ArcFourContext, RC4Key, 16);
 854
 855         ARCFOUR_DECRYPT(&ArcFourContext, pData + HeaderLen, pData + HeaderLen + 8, DataByteCnt - HeaderLen - 8);
 856         NdisMoveMemory(&trailfcs, pData + DataByteCnt - 8 - 4, 4);
 857         crc32 = RTMP_CALC_FCS32(PPPINITFCS32, pData + HeaderLen, DataByteCnt - HeaderLen - 8 - 4);  //Skip IV+EIV 8 bytes & Skip last 4 bytes(FCS).
 858         crc32 ^= 0xffffffff;             /* complement */
 859
 860     if(crc32 != cpu2le32(trailfcs))
 861         {
 862                 DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptTKIP, WEP Data ICV Error !\n"));       //ICV error.
 863
 864                 return (FALSE);
 865         }
 866
 867         NdisMoveMemory(TrailMIC, pData + DataByteCnt - 8 - 8 - 4, 8);
 868         RTMPInitMICEngine(pAd, pWpaKey[KeyID].Key, DA, SA, UserPriority, pWpaKey[KeyID].RxMic);
 869         RTMPTkipAppend(&pAd->PrivateInfo.Tx, pData + HeaderLen, DataByteCnt - HeaderLen - 8 - 12);
 870         RTMPTkipGetMIC(&pAd->PrivateInfo.Tx);
 871         NdisMoveMemory(MIC, pAd->PrivateInfo.Tx.MIC, 8);
 872
 873         if (!NdisEqualMemory(MIC, TrailMIC, 8))
 874         {
 875                 DBGPRINT(RT_DEBUG_ERROR, ("RTMPSoftDecryptTKIP, WEP Data MIC Error !\n"));       //MIC error.
 876                 //RTMPReportMicError(pAd, &pWpaKey[KeyID]);     // marked by AlbertY @ 20060630
 877                 return (FALSE);
 878         }
 879
 880         //DBGPRINT(RT_DEBUG_TRACE, "RTMPSoftDecryptTKIP Decript done!!\n");
 881         return TRUE;
 882 }