LXR linux/drivers/staging/rt3090/common/cmm

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