drivers/staging/rt3090/sta/rtmp_ckipmic.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         rtmp_ckipmic.c
  29
  30         Abstract:
  31         Data path subroutines
  32
  33         Revision History:
  34         Who             When                    What
  35         --------        ----------              ----------------------------------------------
  36 */
  37
  38 #include "../rt_config.h"
  39 #include "../rtmp_ckipmic.h"
  40
  41
  42 #define MIC_ACCUM(v)            pContext->accum += (ULONGLONG)v * RTMPMicGetCoefficient(pContext)
  43 #define GB(p,i,s)               ( ((ULONG) *((UCHAR*)(p)+i) ) << (s) )
  44 #define GETBIG32(p)             GB(p,0,24)|GB(p,1,16)|GB(p,2,8)|GB(p,3,0)
  45
  46 /*****************************/
  47 /******** SBOX Table *********/
  48 /*****************************/
  49
  50 UCHAR SboxTable[256] =
  51 {
  52     0x63, 0x7c, 0x77, 0x7b, 0xf2, 0x6b, 0x6f, 0xc5,
  53     0x30, 0x01, 0x67, 0x2b, 0xfe, 0xd7, 0xab, 0x76,
  54     0xca, 0x82, 0xc9, 0x7d, 0xfa, 0x59, 0x47, 0xf0,
  55     0xad, 0xd4, 0xa2, 0xaf, 0x9c, 0xa4, 0x72, 0xc0,
  56     0xb7, 0xfd, 0x93, 0x26, 0x36, 0x3f, 0xf7, 0xcc,
  57     0x34, 0xa5, 0xe5, 0xf1, 0x71, 0xd8, 0x31, 0x15,
  58     0x04, 0xc7, 0x23, 0xc3, 0x18, 0x96, 0x05, 0x9a,
  59     0x07, 0x12, 0x80, 0xe2, 0xeb, 0x27, 0xb2, 0x75,
  60     0x09, 0x83, 0x2c, 0x1a, 0x1b, 0x6e, 0x5a, 0xa0,
  61     0x52, 0x3b, 0xd6, 0xb3, 0x29, 0xe3, 0x2f, 0x84,
  62     0x53, 0xd1, 0x00, 0xed, 0x20, 0xfc, 0xb1, 0x5b,
  63     0x6a, 0xcb, 0xbe, 0x39, 0x4a, 0x4c, 0x58, 0xcf,
  64     0xd0, 0xef, 0xaa, 0xfb, 0x43, 0x4d, 0x33, 0x85,
  65     0x45, 0xf9, 0x02, 0x7f, 0x50, 0x3c, 0x9f, 0xa8,
  66     0x51, 0xa3, 0x40, 0x8f, 0x92, 0x9d, 0x38, 0xf5,
  67     0xbc, 0xb6, 0xda, 0x21, 0x10, 0xff, 0xf3, 0xd2,
  68     0xcd, 0x0c, 0x13, 0xec, 0x5f, 0x97, 0x44, 0x17,
  69     0xc4, 0xa7, 0x7e, 0x3d, 0x64, 0x5d, 0x19, 0x73,
  70     0x60, 0x81, 0x4f, 0xdc, 0x22, 0x2a, 0x90, 0x88,
  71     0x46, 0xee, 0xb8, 0x14, 0xde, 0x5e, 0x0b, 0xdb,
  72     0xe0, 0x32, 0x3a, 0x0a, 0x49, 0x06, 0x24, 0x5c,
  73     0xc2, 0xd3, 0xac, 0x62, 0x91, 0x95, 0xe4, 0x79,
  74     0xe7, 0xc8, 0x37, 0x6d, 0x8d, 0xd5, 0x4e, 0xa9,
  75     0x6c, 0x56, 0xf4, 0xea, 0x65, 0x7a, 0xae, 0x08,
  76     0xba, 0x78, 0x25, 0x2e, 0x1c, 0xa6, 0xb4, 0xc6,
  77     0xe8, 0xdd, 0x74, 0x1f, 0x4b, 0xbd, 0x8b, 0x8a,
  78     0x70, 0x3e, 0xb5, 0x66, 0x48, 0x03, 0xf6, 0x0e,
  79     0x61, 0x35, 0x57, 0xb9, 0x86, 0xc1, 0x1d, 0x9e,
  80     0xe1, 0xf8, 0x98, 0x11, 0x69, 0xd9, 0x8e, 0x94,
  81     0x9b, 0x1e, 0x87, 0xe9, 0xce, 0x55, 0x28, 0xdf,
  82     0x8c, 0xa1, 0x89, 0x0d, 0xbf, 0xe6, 0x42, 0x68,
  83     0x41, 0x99, 0x2d, 0x0f, 0xb0, 0x54, 0xbb, 0x16
  84 };
  85
  86 /*===========================================================================*/
  87 /*=================== CKIP KEY PERMUTATION ==================================*/
  88 /*===========================================================================*/
  89
  90 /* 2-byte by 2-byte subset of the full AES table */
  91 static const USHORT Sbox[256] =
  92 {
  93     0xC6A5,0xF884,0xEE99,0xF68D,0xFF0D,0xD6BD,0xDEB1,0x9154,
  94     0x6050,0x0203,0xCEA9,0x567D,0xE719,0xB562,0x4DE6,0xEC9A,
  95     0x8F45,0x1F9D,0x8940,0xFA87,0xEF15,0xB2EB,0x8EC9,0xFB0B,
  96     0x41EC,0xB367,0x5FFD,0x45EA,0x23BF,0x53F7,0xE496,0x9B5B,
  97     0x75C2,0xE11C,0x3DAE,0x4C6A,0x6C5A,0x7E41,0xF502,0x834F,
  98     0x685C,0x51F4,0xD134,0xF908,0xE293,0xAB73,0x6253,0x2A3F,
  99     0x080C,0x9552,0x4665,0x9D5E,0x3028,0x37A1,0x0A0F,0x2FB5,
 100     0x0E09,0x2436,0x1B9B,0xDF3D,0xCD26,0x4E69,0x7FCD,0xEA9F,
 101     0x121B,0x1D9E,0x5874,0x342E,0x362D,0xDCB2,0xB4EE,0x5BFB,
 102     0xA4F6,0x764D,0xB761,0x7DCE,0x527B,0xDD3E,0x5E71,0x1397,
 103     0xA6F5,0xB968,0x0000,0xC12C,0x4060,0xE31F,0x79C8,0xB6ED,
 104     0xD4BE,0x8D46,0x67D9,0x724B,0x94DE,0x98D4,0xB0E8,0x854A,
 105     0xBB6B,0xC52A,0x4FE5,0xED16,0x86C5,0x9AD7,0x6655,0x1194,
 106     0x8ACF,0xE910,0x0406,0xFE81,0xA0F0,0x7844,0x25BA,0x4BE3,
 107     0xA2F3,0x5DFE,0x80C0,0x058A,0x3FAD,0x21BC,0x7048,0xF104,
 108     0x63DF,0x77C1,0xAF75,0x4263,0x2030,0xE51A,0xFD0E,0xBF6D,
 109     0x814C,0x1814,0x2635,0xC32F,0xBEE1,0x35A2,0x88CC,0x2E39,
 110     0x9357,0x55F2,0xFC82,0x7A47,0xC8AC,0xBAE7,0x322B,0xE695,
 111     0xC0A0,0x1998,0x9ED1,0xA37F,0x4466,0x547E,0x3BAB,0x0B83,
 112     0x8CCA,0xC729,0x6BD3,0x283C,0xA779,0xBCE2,0x161D,0xAD76,
 113     0xDB3B,0x6456,0x744E,0x141E,0x92DB,0x0C0A,0x486C,0xB8E4,
 114     0x9F5D,0xBD6E,0x43EF,0xC4A6,0x39A8,0x31A4,0xD337,0xF28B,
 115     0xD532,0x8B43,0x6E59,0xDAB7,0x018C,0xB164,0x9CD2,0x49E0,
 116     0xD8B4,0xACFA,0xF307,0xCF25,0xCAAF,0xF48E,0x47E9,0x1018,
 117     0x6FD5,0xF088,0x4A6F,0x5C72,0x3824,0x57F1,0x73C7,0x9751,
 118     0xCB23,0xA17C,0xE89C,0x3E21,0x96DD,0x61DC,0x0D86,0x0F85,
 119     0xE090,0x7C42,0x71C4,0xCCAA,0x90D8,0x0605,0xF701,0x1C12,
 120     0xC2A3,0x6A5F,0xAEF9,0x69D0,0x1791,0x9958,0x3A27,0x27B9,
 121     0xD938,0xEB13,0x2BB3,0x2233,0xD2BB,0xA970,0x0789,0x33A7,
 122     0x2DB6,0x3C22,0x1592,0xC920,0x8749,0xAAFF,0x5078,0xA57A,
 123     0x038F,0x59F8,0x0980,0x1A17,0x65DA,0xD731,0x84C6,0xD0B8,
 124     0x82C3,0x29B0,0x5A77,0x1E11,0x7BCB,0xA8FC,0x6DD6,0x2C3A
 125     };
 126
 127 #define Lo8(v16)     ((v16)       & 0xFF)
 128 #define Hi8(v16)    (((v16) >> 8) & 0xFF)
 129 #define u16Swap(i)  ( (((i) >> 8) & 0xFF) | (((i) << 8) & 0xFF00) )
 130 #define _S_(i)      (Sbox[Lo8(i)] ^ u16Swap(Sbox[Hi8(i)]))
 131
 132 #define rotLeft_1(x) ((((x) << 1) | ((x) >> 15)) & 0xFFFF)
 133 VOID CKIP_key_permute
 134     (
 135      OUT UCHAR  *PK,           /* output permuted key */
 136      IN UCHAR *CK,           /* input CKIP key */
 137      IN UCHAR  toDsFromDs,    /* input toDs/FromDs bits */
 138      IN UCHAR *piv           /* input pointer to IV */
 139      )
 140 {
 141     int i;
 142     USHORT H[2], tmp;          /* H=32-bits of per-packet hash value */
 143     USHORT L[8], R[8];         /* L=u16 array of CK, R=u16 array of PK */
 144
 145     /* build L from input key */
 146     memset(L, 0, sizeof(L));
 147     for (i=0; i<16; i++) {
 148         L[i>>1] |= ( ((USHORT)(CK[i])) << ( i & 1 ? 8 : 0) );
 149     }
 150
 151     H[0] = (((USHORT)piv[0]) << 8) + piv[1];
 152     H[1] = ( ((USHORT)toDsFromDs) << 8) | piv[2];
 153
 154     for (i=0; i<8; i++) {
 155         H[0] ^= L[i];           /* 16-bits of key material */
 156         tmp   = _S_(H[0]);      /* 16x16 permutation */
 157         H[0]  = tmp ^ H[1];     /* set up for next round */
 158         H[1]  = tmp;
 159         R[i]  = H[0];           /* store into key array  */
 160     }
 161
 162     /* sweep in the other direction */
 163     tmp=L[0];
 164     for (i=7; i>0; i--) {
 165         R[i] = tmp = rotLeft_1(tmp) + R[i];
 166     }
 167
 168     /* IV of the permuted key is unchanged */
 169     PK[0] = piv[0];
 170     PK[1] = piv[1];
 171     PK[2] = piv[2];
 172
 173     /* key portion of the permuted key is changed */
 174     for (i=3; i<16; i++) {
 175         PK[i] = (UCHAR) (R[i>>1] >> (i & 1 ? 8 : 0));
 176     }
 177 }
 178
 179 /* prepare for calculation of a new mic */
 180 VOID RTMPCkipMicInit(
 181     IN  PMIC_CONTEXT        pContext,
 182     IN  PUCHAR              CK)
 183 {
 184     /* prepare for new mic calculation */
 185     NdisMoveMemory(pContext->CK, CK, sizeof(pContext->CK));
 186     pContext->accum = 0;
 187     pContext->position = 0;
 188 }
 189
 190 /* add some bytes to the mic calculation */
 191 VOID RTMPMicUpdate(
 192     IN  PMIC_CONTEXT        pContext,
 193     IN  PUCHAR              pOctets,
 194     IN  INT                 len)
 195 {
 196     INT     byte_position;
 197     ULONG   val;
 198
 199     byte_position = (pContext->position & 3);
 200     while (len > 0) {
 201         /* build a 32-bit word for MIC multiply accumulate */
 202         do {
 203             if (len == 0) return;
 204             pContext->part[byte_position++] = *pOctets++;
 205             pContext->position++;
 206             len--;
 207         } while (byte_position < 4);
 208         /* have a full 32-bit word to process */
 209         val = GETBIG32(&pContext->part[0]);
 210         MIC_ACCUM(val);
 211         byte_position = 0;
 212     }
 213 }
 214
 215 ULONG RTMPMicGetCoefficient(
 216     IN  PMIC_CONTEXT         pContext)
 217 {
 218     UCHAR   aes_counter[16];
 219     INT     coeff_position;
 220     UCHAR   *p;
 221
 222     coeff_position = (pContext->position - 1) >> 2;
 223     if ( (coeff_position & 3) == 0) {
 224         /* fetching the first coefficient -- get new 16-byte aes counter output */
 225         u32 counter = (coeff_position >> 2);
 226
 227         /* new counter value */
 228         memset(&aes_counter[0], 0, sizeof(aes_counter));
 229         aes_counter[15] = (UINT8)(counter >> 0);
 230         aes_counter[14] = (UINT8)(counter >> 8);
 231         aes_counter[13] = (UINT8)(counter >> 16);
 232         aes_counter[12] = (UINT8)(counter >> 24);
 233
 234         RTMPAesEncrypt(&pContext->CK[0], &aes_counter[0], pContext->coefficient);
 235     }
 236     p = &(pContext->coefficient[ (coeff_position & 3) << 2 ]);
 237     return GETBIG32(p);
 238 }
 239
 240 /****************************************/
 241 /* aes128k128d()                        */
 242 /* Performs a 128 bit AES encrypt with  */
 243 /* 128 bit data.                        */
 244 /****************************************/
 245 VOID xor_128(
 246     IN  PUCHAR  a,
 247     IN  PUCHAR  b,
 248     OUT PUCHAR  out)
 249 {
 250     INT i;
 251
 252     for (i=0;i<16; i++)
 253     {
 254         out[i] = a[i] ^ b[i];
 255     }
 256 }
 257
 258 UCHAR RTMPCkipSbox(
 259     IN  UCHAR   a)
 260 {
 261     return SboxTable[(int)a];
 262 }
 263
 264 VOID xor_32(
 265     IN  PUCHAR  a,
 266     IN  PUCHAR  b,
 267     OUT PUCHAR  out)
 268 {
 269     INT i;
 270
 271     for (i=0;i<4; i++)
 272     {
 273         out[i] = a[i] ^ b[i];
 274     }
 275 }
 276
 277 VOID next_key(
 278     IN  PUCHAR  key,
 279     IN  INT     round)
 280 {
 281     UCHAR       rcon;
 282     UCHAR       sbox_key[4];
 283     UCHAR       rcon_table[12] =
 284     {
 285         0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80,
 286         0x1b, 0x36, 0x36, 0x36
 287     };
 288
 289     sbox_key[0] = RTMPCkipSbox(key[13]);
 290     sbox_key[1] = RTMPCkipSbox(key[14]);
 291     sbox_key[2] = RTMPCkipSbox(key[15]);
 292     sbox_key[3] = RTMPCkipSbox(key[12]);
 293
 294     rcon = rcon_table[round];
 295
 296     xor_32(&key[0], sbox_key, &key[0]);
 297     key[0] = key[0] ^ rcon;
 298
 299     xor_32(&key[4], &key[0], &key[4]);
 300     xor_32(&key[8], &key[4], &key[8]);
 301     xor_32(&key[12], &key[8], &key[12]);
 302 }
 303
 304 VOID byte_sub(
 305     IN  PUCHAR  in,
 306     OUT PUCHAR  out)
 307 {
 308     INT i;
 309
 310     for (i=0; i< 16; i++)
 311     {
 312         out[i] = RTMPCkipSbox(in[i]);
 313     }
 314 }
 315
 316 VOID shift_row(
 317     IN  PUCHAR  in,
 318     OUT PUCHAR  out)
 319 {
 320     out[0] =  in[0];
 321     out[1] =  in[5];
 322     out[2] =  in[10];
 323     out[3] =  in[15];
 324     out[4] =  in[4];
 325     out[5] =  in[9];
 326     out[6] =  in[14];
 327     out[7] =  in[3];
 328     out[8] =  in[8];
 329     out[9] =  in[13];
 330     out[10] = in[2];
 331     out[11] = in[7];
 332     out[12] = in[12];
 333     out[13] = in[1];
 334     out[14] = in[6];
 335     out[15] = in[11];
 336 }
 337
 338 VOID mix_column(
 339     IN  PUCHAR  in,
 340     OUT PUCHAR  out)
 341 {
 342     INT         i;
 343     UCHAR       add1b[4];
 344     UCHAR       add1bf7[4];
 345     UCHAR       rotl[4];
 346     UCHAR       swap_halfs[4];
 347     UCHAR       andf7[4];
 348     UCHAR       rotr[4];
 349     UCHAR       temp[4];
 350     UCHAR       tempb[4];
 351
 352     for (i=0 ; i<4; i++)
 353     {
 354         if ((in[i] & 0x80)== 0x80)
 355             add1b[i] = 0x1b;
 356         else
 357             add1b[i] = 0x00;
 358     }
 359
 360     swap_halfs[0] = in[2];    /* Swap halfs */
 361     swap_halfs[1] = in[3];
 362     swap_halfs[2] = in[0];
 363     swap_halfs[3] = in[1];
 364
 365     rotl[0] = in[3];        /* Rotate left 8 bits */
 366     rotl[1] = in[0];
 367     rotl[2] = in[1];
 368     rotl[3] = in[2];
 369
 370     andf7[0] = in[0] & 0x7f;
 371     andf7[1] = in[1] & 0x7f;
 372     andf7[2] = in[2] & 0x7f;
 373     andf7[3] = in[3] & 0x7f;
 374
 375     for (i = 3; i>0; i--)    /* logical shift left 1 bit */
 376     {
 377         andf7[i] = andf7[i] << 1;
 378         if ((andf7[i-1] & 0x80) == 0x80)
 379         {
 380             andf7[i] = (andf7[i] | 0x01);
 381         }
 382     }
 383     andf7[0] = andf7[0] << 1;
 384     andf7[0] = andf7[0] & 0xfe;
 385
 386     xor_32(add1b, andf7, add1bf7);
 387
 388     xor_32(in, add1bf7, rotr);
 389
 390     temp[0] = rotr[0];         /* Rotate right 8 bits */
 391     rotr[0] = rotr[1];
 392     rotr[1] = rotr[2];
 393     rotr[2] = rotr[3];
 394     rotr[3] = temp[0];
 395
 396     xor_32(add1bf7, rotr, temp);
 397     xor_32(swap_halfs, rotl,tempb);
 398     xor_32(temp, tempb, out);
 399 }
 400
 401 VOID RTMPAesEncrypt(
 402     IN  PUCHAR  key,
 403     IN  PUCHAR  data,
 404     IN  PUCHAR  ciphertext)
 405 {
 406     INT             round;
 407     INT             i;
 408     UCHAR           intermediatea[16];
 409     UCHAR           intermediateb[16];
 410     UCHAR           round_key[16];
 411
 412     for(i=0; i<16; i++) round_key[i] = key[i];
 413
 414     for (round = 0; round < 11; round++)
 415     {
 416         if (round == 0)
 417         {
 418             xor_128(round_key, data, ciphertext);
 419             next_key(round_key, round);
 420         }
 421         else if (round == 10)
 422         {
 423             byte_sub(ciphertext, intermediatea);
 424             shift_row(intermediatea, intermediateb);
 425             xor_128(intermediateb, round_key, ciphertext);
 426         }
 427         else    /* 1 - 9 */
 428         {
 429             byte_sub(ciphertext, intermediatea);
 430             shift_row(intermediatea, intermediateb);
 431             mix_column(&intermediateb[0], &intermediatea[0]);
 432             mix_column(&intermediateb[4], &intermediatea[4]);
 433             mix_column(&intermediateb[8], &intermediatea[8]);
 434             mix_column(&intermediateb[12], &intermediatea[12]);
 435             xor_128(intermediatea, round_key, ciphertext);
 436             next_key(round_key, round);
 437         }
 438     }
 439
 440 }
 441
 442 /* calculate the mic */
 443 VOID RTMPMicFinal(
 444     IN  PMIC_CONTEXT    pContext,
 445     OUT UCHAR           digest[4])
 446 {
 447     INT             byte_position;
 448     ULONG           val;
 449     ULONGLONG       sum, utmp;
 450     LONGLONG        stmp;
 451
 452     /* deal with partial 32-bit word left over from last update */
 453     if ( (byte_position = (pContext->position & 3)) != 0) {
 454         /* have a partial word in part to deal with -- zero unused bytes */
 455         do {
 456             pContext->part[byte_position++] = 0;
 457             pContext->position++;
 458         } while (byte_position < 4);
 459         val = GETBIG32(&pContext->part[0]);
 460         MIC_ACCUM(val);
 461     }
 462
 463     /* reduce the accumulated u64 to a 32-bit MIC */
 464     sum = pContext->accum;
 465     stmp = (sum  & 0xffffffffL) - ((sum >> 32)  * 15);
 466     utmp = (stmp & 0xffffffffL) - ((stmp >> 32) * 15);
 467     sum = utmp & 0xffffffffL;
 468     if (utmp > 0x10000000fL)
 469         sum -= 15;
 470
 471     val = (ULONG)sum;
 472     digest[0] = (UCHAR)((val>>24) & 0xFF);
 473     digest[1] = (UCHAR) ((val>>16) & 0xFF);
 474     digest[2] = (UCHAR) ((val>>8) & 0xFF);
 475     digest[3] = (UCHAR)((val>>0) & 0xFF);
 476 }
 477
 478 VOID RTMPCkipInsertCMIC(
 479     IN  PRTMP_ADAPTER   pAd,
 480     OUT PUCHAR          pMIC,
 481     IN  PUCHAR          p80211hdr,
 482     IN  PNDIS_PACKET    pPacket,
 483     IN  PCIPHER_KEY     pKey,
 484     IN  PUCHAR          mic_snap)
 485 {
 486         PACKET_INFO             PacketInfo;
 487         PUCHAR                  pSrcBufVA;
 488         ULONG                   SrcBufLen;
 489     PUCHAR          pDA, pSA, pProto;
 490     UCHAR           bigethlen[2];
 491         UCHAR                   ckip_ck[16];
 492     MIC_CONTEXT     mic_ctx;
 493     USHORT          payloadlen;
 494         UCHAR                   i;
 495
 496         if (pKey == NULL)
 497         {
 498                 DBGPRINT_ERR(("RTMPCkipInsertCMIC, Before to form the CKIP key (CK), pKey can't be NULL\n"));
 499                 return;
 500         }
 501
 502     switch (*(p80211hdr+1) & 3)
 503     {
 504         case 0: /* FromDs=0, ToDs=0 */
 505             pDA = p80211hdr+4;
 506             pSA = p80211hdr+10;
 507             break;
 508         case 1: /* FromDs=0, ToDs=1 */
 509             pDA = p80211hdr+16;
 510             pSA = p80211hdr+10;
 511             break;
 512         case 2: /* FromDs=1, ToDs=0 */
 513             pDA = p80211hdr+4;
 514             pSA = p80211hdr+16;
 515             break;
 516         case 3: /* FromDs=1, ToDs=1 */
 517             pDA = p80211hdr+16;
 518             pSA = p80211hdr+24;
 519             break;
 520     }
 521
 522         RTMP_QueryPacketInfo(pPacket, &PacketInfo, &pSrcBufVA, &SrcBufLen);
 523
 524     if (SrcBufLen < LENGTH_802_3)
 525         return;
 526
 527     pProto = pSrcBufVA + 12;
 528     payloadlen = PacketInfo.TotalPacketLength - LENGTH_802_3 + 18; // CKIP_LLC(8)+CMIC(4)+TxSEQ(4)+PROTO(2)=18
 529
 530     bigethlen[0] = (unsigned char)(payloadlen >> 8);
 531     bigethlen[1] = (unsigned char)payloadlen;
 532
 533         //
 534         // Encryption Key expansion to form the CKIP Key (CKIP_CK).
 535         //
 536         if (pKey->KeyLen < 16)
 537         {
 538                 for(i = 0; i < (16 / pKey->KeyLen); i++)
 539                 {
 540                         NdisMoveMemory(ckip_ck + i * pKey->KeyLen,
 541                                                         pKey->Key,
 542                                                         pKey->KeyLen);
 543                 }
 544                 NdisMoveMemory(ckip_ck + i * pKey->KeyLen,
 545                                                 pKey->Key,
 546                                                 16 - (i * pKey->KeyLen));
 547         }
 548         else
 549         {
 550                 NdisMoveMemory(ckip_ck, pKey->Key, pKey->KeyLen);
 551         }
 552     RTMPCkipMicInit(&mic_ctx, ckip_ck);
 553     RTMPMicUpdate(&mic_ctx, pDA, MAC_ADDR_LEN);            // MIC <-- DA
 554     RTMPMicUpdate(&mic_ctx, pSA, MAC_ADDR_LEN);            // MIC <-- SA
 555     RTMPMicUpdate(&mic_ctx, bigethlen, 2);                 // MIC <-- payload length starting from CKIP SNAP
 556     RTMPMicUpdate(&mic_ctx, mic_snap, 8);                  // MIC <-- snap header
 557     RTMPMicUpdate(&mic_ctx, pAd->StaCfg.TxSEQ, 4);   // MIC <-- TxSEQ
 558     RTMPMicUpdate(&mic_ctx, pProto, 2);                    // MIC <-- Protocol
 559
 560     pSrcBufVA += LENGTH_802_3;
 561     SrcBufLen -= LENGTH_802_3;
 562
 563     // Mic <-- original payload. loop until all payload processed
 564     do
 565     {
 566         if (SrcBufLen > 0)
 567             RTMPMicUpdate(&mic_ctx, pSrcBufVA, SrcBufLen);
 568
 569                 NdisGetNextBuffer(PacketInfo.pFirstBuffer, &PacketInfo.pFirstBuffer);
 570         if (PacketInfo.pFirstBuffer)
 571         {
 572             NDIS_QUERY_BUFFER(PacketInfo.pFirstBuffer, &pSrcBufVA, &SrcBufLen);
 573         }
 574         else
 575             break;
 576     } while (TRUE);
 577
 578     RTMPMicFinal(&mic_ctx, pMIC);                          // update MIC
 579 }