drivers/staging/rt3090/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
  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
  46 UINT 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
  82 UINT 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 //
 121 typedef 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 */
 172 ULONG   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 */
 205 VOID    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 */
 237 VOID 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 */
 270 VOID    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 */
 315 VOID    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 */
 346 VOID    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 */
 387 VOID    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 */
 434 VOID    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 */
 478 BOOLEAN 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 */
 545 BOOLEAN 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 */
 611 VOID    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
 678 UINT 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
 693 UINT 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
 709 VOID 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 //
 814 BOOLEAN 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 }