drivers/staging/rt2860/common/cmm_aes.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_aes.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 typedef struct
  42 {
  43     UINT32 erk[64];     /* encryption round keys */
  44     UINT32 drk[64];     /* decryption round keys */
  45     int nr;             /* number of rounds */
  46 }
  47 aes_context;
  48
  49 /*****************************/
  50 /******** SBOX Table *********/
  51 /*****************************/
  52
  53 UCHAR SboxTable[256] =
  54 {
  55         0x63, 0x7c, 0x77, 0x7b, 0xf2, 0x6b, 0x6f, 0xc5,
  56         0x30, 0x01, 0x67, 0x2b, 0xfe, 0xd7, 0xab, 0x76,
  57         0xca, 0x82, 0xc9, 0x7d, 0xfa, 0x59, 0x47, 0xf0,
  58         0xad, 0xd4, 0xa2, 0xaf, 0x9c, 0xa4, 0x72, 0xc0,
  59         0xb7, 0xfd, 0x93, 0x26, 0x36, 0x3f, 0xf7, 0xcc,
  60         0x34, 0xa5, 0xe5, 0xf1, 0x71, 0xd8, 0x31, 0x15,
  61         0x04, 0xc7, 0x23, 0xc3, 0x18, 0x96, 0x05, 0x9a,
  62         0x07, 0x12, 0x80, 0xe2, 0xeb, 0x27, 0xb2, 0x75,
  63         0x09, 0x83, 0x2c, 0x1a, 0x1b, 0x6e, 0x5a, 0xa0,
  64         0x52, 0x3b, 0xd6, 0xb3, 0x29, 0xe3, 0x2f, 0x84,
  65         0x53, 0xd1, 0x00, 0xed, 0x20, 0xfc, 0xb1, 0x5b,
  66         0x6a, 0xcb, 0xbe, 0x39, 0x4a, 0x4c, 0x58, 0xcf,
  67         0xd0, 0xef, 0xaa, 0xfb, 0x43, 0x4d, 0x33, 0x85,
  68         0x45, 0xf9, 0x02, 0x7f, 0x50, 0x3c, 0x9f, 0xa8,
  69         0x51, 0xa3, 0x40, 0x8f, 0x92, 0x9d, 0x38, 0xf5,
  70         0xbc, 0xb6, 0xda, 0x21, 0x10, 0xff, 0xf3, 0xd2,
  71         0xcd, 0x0c, 0x13, 0xec, 0x5f, 0x97, 0x44, 0x17,
  72         0xc4, 0xa7, 0x7e, 0x3d, 0x64, 0x5d, 0x19, 0x73,
  73         0x60, 0x81, 0x4f, 0xdc, 0x22, 0x2a, 0x90, 0x88,
  74         0x46, 0xee, 0xb8, 0x14, 0xde, 0x5e, 0x0b, 0xdb,
  75         0xe0, 0x32, 0x3a, 0x0a, 0x49, 0x06, 0x24, 0x5c,
  76         0xc2, 0xd3, 0xac, 0x62, 0x91, 0x95, 0xe4, 0x79,
  77         0xe7, 0xc8, 0x37, 0x6d, 0x8d, 0xd5, 0x4e, 0xa9,
  78         0x6c, 0x56, 0xf4, 0xea, 0x65, 0x7a, 0xae, 0x08,
  79         0xba, 0x78, 0x25, 0x2e, 0x1c, 0xa6, 0xb4, 0xc6,
  80         0xe8, 0xdd, 0x74, 0x1f, 0x4b, 0xbd, 0x8b, 0x8a,
  81         0x70, 0x3e, 0xb5, 0x66, 0x48, 0x03, 0xf6, 0x0e,
  82         0x61, 0x35, 0x57, 0xb9, 0x86, 0xc1, 0x1d, 0x9e,
  83         0xe1, 0xf8, 0x98, 0x11, 0x69, 0xd9, 0x8e, 0x94,
  84         0x9b, 0x1e, 0x87, 0xe9, 0xce, 0x55, 0x28, 0xdf,
  85         0x8c, 0xa1, 0x89, 0x0d, 0xbf, 0xe6, 0x42, 0x68,
  86         0x41, 0x99, 0x2d, 0x0f, 0xb0, 0x54, 0xbb, 0x16
  87 };
  88
  89 VOID xor_32(
  90         IN  PUCHAR  a,
  91         IN  PUCHAR  b,
  92         OUT PUCHAR  out)
  93 {
  94         INT i;
  95
  96         for (i=0;i<4; i++)
  97         {
  98                 out[i] = a[i] ^ b[i];
  99         }
 100 }
 101
 102 VOID xor_128(
 103         IN  PUCHAR  a,
 104         IN  PUCHAR  b,
 105         OUT PUCHAR  out)
 106 {
 107         INT i;
 108
 109         for (i=0;i<16; i++)
 110         {
 111                 out[i] = a[i] ^ b[i];
 112         }
 113 }
 114
 115 UCHAR RTMPCkipSbox(
 116         IN  UCHAR   a)
 117 {
 118         return SboxTable[(int)a];
 119 }
 120
 121 VOID next_key(
 122         IN  PUCHAR  key,
 123         IN  INT     round)
 124 {
 125         UCHAR       rcon;
 126         UCHAR       sbox_key[4];
 127         UCHAR       rcon_table[12] =
 128         {
 129                 0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80,
 130                 0x1b, 0x36, 0x36, 0x36
 131         };
 132
 133         sbox_key[0] = RTMPCkipSbox(key[13]);
 134         sbox_key[1] = RTMPCkipSbox(key[14]);
 135         sbox_key[2] = RTMPCkipSbox(key[15]);
 136         sbox_key[3] = RTMPCkipSbox(key[12]);
 137
 138         rcon = rcon_table[round];
 139
 140         xor_32(&key[0], sbox_key, &key[0]);
 141         key[0] = key[0] ^ rcon;
 142
 143         xor_32(&key[4], &key[0], &key[4]);
 144         xor_32(&key[8], &key[4], &key[8]);
 145         xor_32(&key[12], &key[8], &key[12]);
 146 }
 147
 148 VOID byte_sub(
 149         IN  PUCHAR  in,
 150         OUT PUCHAR  out)
 151 {
 152         INT i;
 153
 154         for (i=0; i< 16; i++)
 155         {
 156                 out[i] = RTMPCkipSbox(in[i]);
 157         }
 158 }
 159
 160 /************************************/
 161 /* bitwise_xor()                    */
 162 /* A 128 bit, bitwise exclusive or  */
 163 /************************************/
 164
 165 void bitwise_xor(unsigned char *ina, unsigned char *inb, unsigned char *out)
 166 {
 167         int i;
 168         for (i=0; i<16; i++)
 169         {
 170                 out[i] = ina[i] ^ inb[i];
 171         }
 172 }
 173
 174 VOID shift_row(
 175         IN  PUCHAR  in,
 176         OUT PUCHAR  out)
 177 {
 178         out[0] =  in[0];
 179         out[1] =  in[5];
 180         out[2] =  in[10];
 181         out[3] =  in[15];
 182         out[4] =  in[4];
 183         out[5] =  in[9];
 184         out[6] =  in[14];
 185         out[7] =  in[3];
 186         out[8] =  in[8];
 187         out[9] =  in[13];
 188         out[10] = in[2];
 189         out[11] = in[7];
 190         out[12] = in[12];
 191         out[13] = in[1];
 192         out[14] = in[6];
 193         out[15] = in[11];
 194 }
 195
 196 VOID mix_column(
 197         IN  PUCHAR  in,
 198         OUT PUCHAR  out)
 199 {
 200         INT         i;
 201         UCHAR       add1b[4];
 202         UCHAR       add1bf7[4];
 203         UCHAR       rotl[4];
 204         UCHAR       swap_halfs[4];
 205         UCHAR       andf7[4];
 206         UCHAR       rotr[4];
 207         UCHAR       temp[4];
 208         UCHAR       tempb[4];
 209
 210         for (i=0 ; i<4; i++)
 211         {
 212                 if ((in[i] & 0x80)== 0x80)
 213                         add1b[i] = 0x1b;
 214                 else
 215                         add1b[i] = 0x00;
 216         }
 217
 218         swap_halfs[0] = in[2];    /* Swap halfs */
 219         swap_halfs[1] = in[3];
 220         swap_halfs[2] = in[0];
 221         swap_halfs[3] = in[1];
 222
 223         rotl[0] = in[3];        /* Rotate left 8 bits */
 224         rotl[1] = in[0];
 225         rotl[2] = in[1];
 226         rotl[3] = in[2];
 227
 228         andf7[0] = in[0] & 0x7f;
 229         andf7[1] = in[1] & 0x7f;
 230         andf7[2] = in[2] & 0x7f;
 231         andf7[3] = in[3] & 0x7f;
 232
 233         for (i = 3; i>0; i--)    /* logical shift left 1 bit */
 234         {
 235                 andf7[i] = andf7[i] << 1;
 236                 if ((andf7[i-1] & 0x80) == 0x80)
 237                 {
 238                         andf7[i] = (andf7[i] | 0x01);
 239                 }
 240         }
 241         andf7[0] = andf7[0] << 1;
 242         andf7[0] = andf7[0] & 0xfe;
 243
 244         xor_32(add1b, andf7, add1bf7);
 245
 246         xor_32(in, add1bf7, rotr);
 247
 248         temp[0] = rotr[0];         /* Rotate right 8 bits */
 249         rotr[0] = rotr[1];
 250         rotr[1] = rotr[2];
 251         rotr[2] = rotr[3];
 252         rotr[3] = temp[0];
 253
 254         xor_32(add1bf7, rotr, temp);
 255         xor_32(swap_halfs, rotl,tempb);
 256         xor_32(temp, tempb, out);
 257 }
 258
 259
 260 /************************************************/
 261 /* construct_mic_header1()                      */
 262 /* Builds the first MIC header block from       */
 263 /* header fields.                               */
 264 /************************************************/
 265
 266 void construct_mic_header1(
 267         unsigned char *mic_header1,
 268         int header_length,
 269         unsigned char *mpdu)
 270 {
 271         mic_header1[0] = (unsigned char)((header_length - 2) / 256);
 272         mic_header1[1] = (unsigned char)((header_length - 2) % 256);
 273         mic_header1[2] = mpdu[0] & 0xcf;    /* Mute CF poll & CF ack bits */
 274         mic_header1[3] = mpdu[1] & 0xc7;    /* Mute retry, more data and pwr mgt bits */
 275         mic_header1[4] = mpdu[4];       /* A1 */
 276         mic_header1[5] = mpdu[5];
 277         mic_header1[6] = mpdu[6];
 278         mic_header1[7] = mpdu[7];
 279         mic_header1[8] = mpdu[8];
 280         mic_header1[9] = mpdu[9];
 281         mic_header1[10] = mpdu[10];     /* A2 */
 282         mic_header1[11] = mpdu[11];
 283         mic_header1[12] = mpdu[12];
 284         mic_header1[13] = mpdu[13];
 285         mic_header1[14] = mpdu[14];
 286         mic_header1[15] = mpdu[15];
 287 }
 288
 289 /************************************************/
 290 /* construct_mic_header2()                      */
 291 /* Builds the last MIC header block from        */
 292 /* header fields.                               */
 293 /************************************************/
 294
 295 void construct_mic_header2(
 296         unsigned char *mic_header2,
 297         unsigned char *mpdu,
 298         int a4_exists,
 299         int qc_exists)
 300 {
 301         int i;
 302
 303         for (i = 0; i<16; i++) mic_header2[i]=0x00;
 304
 305         mic_header2[0] = mpdu[16];    /* A3 */
 306         mic_header2[1] = mpdu[17];
 307         mic_header2[2] = mpdu[18];
 308         mic_header2[3] = mpdu[19];
 309         mic_header2[4] = mpdu[20];
 310         mic_header2[5] = mpdu[21];
 311
 312         // In Sequence Control field, mute sequence numer bits (12-bit)
 313         mic_header2[6] = mpdu[22] & 0x0f;   /* SC */
 314         mic_header2[7] = 0x00; /* mpdu[23]; */
 315
 316         if ((!qc_exists) & a4_exists)
 317         {
 318                 for (i=0;i<6;i++) mic_header2[8+i] = mpdu[24+i];   /* A4 */
 319
 320         }
 321
 322         if (qc_exists && (!a4_exists))
 323         {
 324                 mic_header2[8] = mpdu[24] & 0x0f; /* mute bits 15 - 4 */
 325                 mic_header2[9] = mpdu[25] & 0x00;
 326         }
 327
 328         if (qc_exists && a4_exists)
 329         {
 330                 for (i=0;i<6;i++) mic_header2[8+i] = mpdu[24+i];   /* A4 */
 331
 332                 mic_header2[14] = mpdu[30] & 0x0f;
 333                 mic_header2[15] = mpdu[31] & 0x00;
 334         }
 335 }
 336
 337
 338 /************************************************/
 339 /* construct_mic_iv()                           */
 340 /* Builds the MIC IV from header fields and PN  */
 341 /************************************************/
 342
 343 void construct_mic_iv(
 344         unsigned char *mic_iv,
 345         int qc_exists,
 346         int a4_exists,
 347         unsigned char *mpdu,
 348         unsigned int payload_length,
 349         unsigned char *pn_vector)
 350 {
 351         int i;
 352
 353         mic_iv[0] = 0x59;
 354         if (qc_exists && a4_exists)
 355                 mic_iv[1] = mpdu[30] & 0x0f;    /* QoS_TC           */
 356         if (qc_exists && !a4_exists)
 357                 mic_iv[1] = mpdu[24] & 0x0f;   /* mute bits 7-4    */
 358         if (!qc_exists)
 359                 mic_iv[1] = 0x00;
 360         for (i = 2; i < 8; i++)
 361                 mic_iv[i] = mpdu[i + 8];                    /* mic_iv[2:7] = A2[0:5] = mpdu[10:15] */
 362 #ifdef CONSISTENT_PN_ORDER
 363                 for (i = 8; i < 14; i++)
 364                         mic_iv[i] = pn_vector[i - 8];           /* mic_iv[8:13] = PN[0:5] */
 365 #else
 366                 for (i = 8; i < 14; i++)
 367                         mic_iv[i] = pn_vector[13 - i];          /* mic_iv[8:13] = PN[5:0] */
 368 #endif
 369         i = (payload_length / 256);
 370         i = (payload_length % 256);
 371         mic_iv[14] = (unsigned char) (payload_length / 256);
 372         mic_iv[15] = (unsigned char) (payload_length % 256);
 373
 374 }
 375
 376 /****************************************/
 377 /* aes128k128d()                        */
 378 /* Performs a 128 bit AES encrypt with  */
 379 /* 128 bit data.                        */
 380 /****************************************/
 381 void aes128k128d(unsigned char *key, unsigned char *data, unsigned char *ciphertext)
 382 {
 383         int round;
 384         int i;
 385         unsigned char intermediatea[16];
 386         unsigned char intermediateb[16];
 387         unsigned char round_key[16];
 388
 389         for(i=0; i<16; i++) round_key[i] = key[i];
 390
 391         for (round = 0; round < 11; round++)
 392         {
 393                 if (round == 0)
 394                 {
 395                         xor_128(round_key, data, ciphertext);
 396                         next_key(round_key, round);
 397                 }
 398                 else if (round == 10)
 399                 {
 400                         byte_sub(ciphertext, intermediatea);
 401                         shift_row(intermediatea, intermediateb);
 402                         xor_128(intermediateb, round_key, ciphertext);
 403                 }
 404                 else    /* 1 - 9 */
 405                 {
 406                         byte_sub(ciphertext, intermediatea);
 407                         shift_row(intermediatea, intermediateb);
 408                         mix_column(&intermediateb[0], &intermediatea[0]);
 409                         mix_column(&intermediateb[4], &intermediatea[4]);
 410                         mix_column(&intermediateb[8], &intermediatea[8]);
 411                         mix_column(&intermediateb[12], &intermediatea[12]);
 412                         xor_128(intermediatea, round_key, ciphertext);
 413                         next_key(round_key, round);
 414                 }
 415         }
 416
 417 }
 418
 419 void construct_ctr_preload(
 420         unsigned char *ctr_preload,
 421         int a4_exists,
 422         int qc_exists,
 423         unsigned char *mpdu,
 424         unsigned char *pn_vector,
 425         int c)
 426 {
 427
 428         int i = 0;
 429         for (i=0; i<16; i++) ctr_preload[i] = 0x00;
 430         i = 0;
 431
 432         ctr_preload[0] = 0x01;                                  /* flag */
 433         if (qc_exists && a4_exists) ctr_preload[1] = mpdu[30] & 0x0f;   /* QoC_Control  */
 434         if (qc_exists && !a4_exists) ctr_preload[1] = mpdu[24] & 0x0f;
 435
 436         for (i = 2; i < 8; i++)
 437                 ctr_preload[i] = mpdu[i + 8];                       /* ctr_preload[2:7] = A2[0:5] = mpdu[10:15] */
 438 #ifdef CONSISTENT_PN_ORDER
 439           for (i = 8; i < 14; i++)
 440                         ctr_preload[i] =    pn_vector[i - 8];           /* ctr_preload[8:13] = PN[0:5] */
 441 #else
 442           for (i = 8; i < 14; i++)
 443                         ctr_preload[i] =    pn_vector[13 - i];          /* ctr_preload[8:13] = PN[5:0] */
 444 #endif
 445         ctr_preload[14] =  (unsigned char) (c / 256); // Ctr
 446         ctr_preload[15] =  (unsigned char) (c % 256);
 447
 448 }
 449
 450 BOOLEAN RTMPSoftDecryptAES(
 451         IN PRTMP_ADAPTER pAd,
 452         IN PUCHAR       pData,
 453         IN ULONG        DataByteCnt,
 454         IN PCIPHER_KEY  pWpaKey)
 455 {
 456         UCHAR                   KeyID;
 457         UINT                    HeaderLen;
 458         UCHAR                   PN[6];
 459         UINT                    payload_len;
 460         UINT                    num_blocks;
 461         UINT                    payload_remainder;
 462         USHORT                  fc;
 463         UCHAR                   fc0;
 464         UCHAR                   fc1;
 465         UINT                    frame_type;
 466         UINT                    frame_subtype;
 467         UINT                    from_ds;
 468         UINT                    to_ds;
 469         INT                             a4_exists;
 470         INT                             qc_exists;
 471         UCHAR                   aes_out[16];
 472         int                     payload_index;
 473         UINT                    i;
 474         UCHAR                   ctr_preload[16];
 475         UCHAR                   chain_buffer[16];
 476         UCHAR                   padded_buffer[16];
 477         UCHAR                   mic_iv[16];
 478         UCHAR                   mic_header1[16];
 479         UCHAR                   mic_header2[16];
 480         UCHAR                   MIC[8];
 481         UCHAR                   TrailMIC[8];
 482
 483
 484         fc0 = *pData;
 485         fc1 = *(pData + 1);
 486
 487         fc = *((PUSHORT)pData);
 488
 489         frame_type = ((fc0 >> 2) & 0x03);
 490         frame_subtype = ((fc0 >> 4) & 0x0f);
 491
 492         from_ds = (fc1 & 0x2) >> 1;
 493         to_ds = (fc1 & 0x1);
 494
 495         a4_exists = (from_ds & to_ds);
 496         qc_exists = ((frame_subtype == 0x08) ||    /* Assumed QoS subtypes */
 497                                   (frame_subtype == 0x09) ||   /* Likely to change.    */
 498                                   (frame_subtype == 0x0a) ||
 499                                   (frame_subtype == 0x0b)
 500                                  );
 501
 502         HeaderLen = 24;
 503         if (a4_exists)
 504                 HeaderLen += 6;
 505
 506         KeyID = *((PUCHAR)(pData+ HeaderLen + 3));
 507         KeyID = KeyID >> 6;
 508
 509         if (pWpaKey[KeyID].KeyLen == 0)
 510         {
 511                 DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptAES failed!(KeyID[%d] Length can not be 0)\n", KeyID));
 512                 return FALSE;
 513         }
 514
 515         PN[0] = *(pData+ HeaderLen);
 516         PN[1] = *(pData+ HeaderLen + 1);
 517         PN[2] = *(pData+ HeaderLen + 4);
 518         PN[3] = *(pData+ HeaderLen + 5);
 519         PN[4] = *(pData+ HeaderLen + 6);
 520         PN[5] = *(pData+ HeaderLen + 7);
 521
 522         payload_len = DataByteCnt - HeaderLen - 8 - 8;  // 8 bytes for CCMP header , 8 bytes for MIC
 523         payload_remainder = (payload_len) % 16;
 524         num_blocks = (payload_len) / 16;
 525
 526
 527
 528         // Find start of payload
 529         payload_index = HeaderLen + 8; //IV+EIV
 530
 531         for (i=0; i< num_blocks; i++)
 532         {
 533                 construct_ctr_preload(ctr_preload,
 534                                                                 a4_exists,
 535                                                                 qc_exists,
 536                                                                 pData,
 537                                                                 PN,
 538                                                                 i+1 );
 539
 540                 aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);
 541
 542                 bitwise_xor(aes_out, pData + payload_index, chain_buffer);
 543                 NdisMoveMemory(pData + payload_index - 8, chain_buffer, 16);
 544                 payload_index += 16;
 545         }
 546
 547         //
 548         // If there is a short final block, then pad it
 549         // encrypt it and copy the unpadded part back
 550         //
 551         if (payload_remainder > 0)
 552         {
 553                 construct_ctr_preload(ctr_preload,
 554                                                                 a4_exists,
 555                                                                 qc_exists,
 556                                                                 pData,
 557                                                                 PN,
 558                                                                 num_blocks + 1);
 559
 560                 NdisZeroMemory(padded_buffer, 16);
 561                 NdisMoveMemory(padded_buffer, pData + payload_index, payload_remainder);
 562
 563                 aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);
 564
 565                 bitwise_xor(aes_out, padded_buffer, chain_buffer);
 566                 NdisMoveMemory(pData + payload_index - 8, chain_buffer, payload_remainder);
 567                 payload_index += payload_remainder;
 568         }
 569
 570         //
 571         // Descrypt the MIC
 572         //
 573         construct_ctr_preload(ctr_preload,
 574                                                         a4_exists,
 575                                                         qc_exists,
 576                                                         pData,
 577                                                         PN,
 578                                                         0);
 579         NdisZeroMemory(padded_buffer, 16);
 580         NdisMoveMemory(padded_buffer, pData + payload_index, 8);
 581
 582         aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);
 583
 584         bitwise_xor(aes_out, padded_buffer, chain_buffer);
 585
 586         NdisMoveMemory(TrailMIC, chain_buffer, 8);
 587
 588
 589         //
 590         // Calculate MIC
 591         //
 592
 593         //Force the protected frame bit on
 594         *(pData + 1) = *(pData + 1) | 0x40;
 595
 596         // Find start of payload
 597         // Because the CCMP header has been removed
 598         payload_index = HeaderLen;
 599
 600         construct_mic_iv(
 601                                         mic_iv,
 602                                         qc_exists,
 603                                         a4_exists,
 604                                         pData,
 605                                         payload_len,
 606                                         PN);
 607
 608         construct_mic_header1(
 609                                                 mic_header1,
 610                                                 HeaderLen,
 611                                                 pData);
 612
 613         construct_mic_header2(
 614                                                 mic_header2,
 615                                                 pData,
 616                                                 a4_exists,
 617                                                 qc_exists);
 618
 619         aes128k128d(pWpaKey[KeyID].Key, mic_iv, aes_out);
 620         bitwise_xor(aes_out, mic_header1, chain_buffer);
 621         aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
 622         bitwise_xor(aes_out, mic_header2, chain_buffer);
 623         aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
 624
 625         // iterate through each 16 byte payload block
 626         for (i = 0; i < num_blocks; i++)
 627         {
 628                 bitwise_xor(aes_out, pData + payload_index, chain_buffer);
 629                 payload_index += 16;
 630                 aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
 631         }
 632
 633         // Add on the final payload block if it needs padding
 634         if (payload_remainder > 0)
 635         {
 636                 NdisZeroMemory(padded_buffer, 16);
 637                 NdisMoveMemory(padded_buffer, pData + payload_index, payload_remainder);
 638
 639                 bitwise_xor(aes_out, padded_buffer, chain_buffer);
 640                 aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
 641         }
 642
 643         // aes_out contains padded mic, discard most significant
 644         // 8 bytes to generate 64 bit MIC
 645         for (i = 0 ; i < 8; i++) MIC[i] = aes_out[i];
 646
 647         if (!NdisEqualMemory(MIC, TrailMIC, 8))
 648         {
 649                 DBGPRINT(RT_DEBUG_ERROR, ("RTMPSoftDecryptAES, MIC Error !\n"));         //MIC error.
 650                 return FALSE;
 651         }
 652
 653
 654         return TRUE;
 655 }
 656
 657 /* =========================  AES En/Decryption ========================== */
 658 #ifndef uint8
 659 #define uint8  unsigned char
 660 #endif
 661
 662 #ifndef uint32
 663 #define uint32 unsigned int
 664 #endif
 665
 666 /* forward S-box */
 667 static uint32 FSb[256] =
 668 {
 669         0x63, 0x7C,     0x77, 0x7B,     0xF2, 0x6B,     0x6F, 0xC5,
 670         0x30, 0x01,     0x67, 0x2B,     0xFE, 0xD7,     0xAB, 0x76,
 671         0xCA, 0x82,     0xC9, 0x7D,     0xFA, 0x59,     0x47, 0xF0,
 672         0xAD, 0xD4,     0xA2, 0xAF,     0x9C, 0xA4,     0x72, 0xC0,
 673         0xB7, 0xFD,     0x93, 0x26,     0x36, 0x3F,     0xF7, 0xCC,
 674         0x34, 0xA5,     0xE5, 0xF1,     0x71, 0xD8,     0x31, 0x15,
 675         0x04, 0xC7,     0x23, 0xC3,     0x18, 0x96,     0x05, 0x9A,
 676         0x07, 0x12,     0x80, 0xE2,     0xEB, 0x27,     0xB2, 0x75,
 677         0x09, 0x83,     0x2C, 0x1A,     0x1B, 0x6E,     0x5A, 0xA0,
 678         0x52, 0x3B,     0xD6, 0xB3,     0x29, 0xE3,     0x2F, 0x84,
 679         0x53, 0xD1,     0x00, 0xED,     0x20, 0xFC,     0xB1, 0x5B,
 680         0x6A, 0xCB,     0xBE, 0x39,     0x4A, 0x4C,     0x58, 0xCF,
 681         0xD0, 0xEF,     0xAA, 0xFB,     0x43, 0x4D,     0x33, 0x85,
 682         0x45, 0xF9,     0x02, 0x7F,     0x50, 0x3C,     0x9F, 0xA8,
 683         0x51, 0xA3,     0x40, 0x8F,     0x92, 0x9D,     0x38, 0xF5,
 684         0xBC, 0xB6,     0xDA, 0x21,     0x10, 0xFF,     0xF3, 0xD2,
 685         0xCD, 0x0C,     0x13, 0xEC,     0x5F, 0x97,     0x44, 0x17,
 686         0xC4, 0xA7,     0x7E, 0x3D,     0x64, 0x5D,     0x19, 0x73,
 687         0x60, 0x81,     0x4F, 0xDC,     0x22, 0x2A,     0x90, 0x88,
 688         0x46, 0xEE,     0xB8, 0x14,     0xDE, 0x5E,     0x0B, 0xDB,
 689         0xE0, 0x32,     0x3A, 0x0A,     0x49, 0x06,     0x24, 0x5C,
 690         0xC2, 0xD3,     0xAC, 0x62,     0x91, 0x95,     0xE4, 0x79,
 691         0xE7, 0xC8,     0x37, 0x6D,     0x8D, 0xD5,     0x4E, 0xA9,
 692         0x6C, 0x56,     0xF4, 0xEA,     0x65, 0x7A,     0xAE, 0x08,
 693         0xBA, 0x78,     0x25, 0x2E,     0x1C, 0xA6,     0xB4, 0xC6,
 694         0xE8, 0xDD,     0x74, 0x1F,     0x4B, 0xBD,     0x8B, 0x8A,
 695         0x70, 0x3E,     0xB5, 0x66,     0x48, 0x03,     0xF6, 0x0E,
 696         0x61, 0x35,     0x57, 0xB9,     0x86, 0xC1,     0x1D, 0x9E,
 697         0xE1, 0xF8,     0x98, 0x11,     0x69, 0xD9,     0x8E, 0x94,
 698         0x9B, 0x1E,     0x87, 0xE9,     0xCE, 0x55,     0x28, 0xDF,
 699         0x8C, 0xA1,     0x89, 0x0D,     0xBF, 0xE6,     0x42, 0x68,
 700         0x41, 0x99,     0x2D, 0x0F,     0xB0, 0x54,     0xBB, 0x16
 701 };
 702
 703 /* forward table */
 704 #define FT \
 705 \
 706         V(C6,63,63,A5), V(F8,7C,7C,84), V(EE,77,77,99), V(F6,7B,7B,8D), \
 707         V(FF,F2,F2,0D), V(D6,6B,6B,BD), V(DE,6F,6F,B1), V(91,C5,C5,54), \
 708         V(60,30,30,50), V(02,01,01,03), V(CE,67,67,A9), V(56,2B,2B,7D), \
 709         V(E7,FE,FE,19), V(B5,D7,D7,62), V(4D,AB,AB,E6), V(EC,76,76,9A), \
 710         V(8F,CA,CA,45), V(1F,82,82,9D), V(89,C9,C9,40), V(FA,7D,7D,87), \
 711         V(EF,FA,FA,15), V(B2,59,59,EB), V(8E,47,47,C9), V(FB,F0,F0,0B), \
 712         V(41,AD,AD,EC), V(B3,D4,D4,67), V(5F,A2,A2,FD), V(45,AF,AF,EA), \
 713         V(23,9C,9C,BF), V(53,A4,A4,F7), V(E4,72,72,96), V(9B,C0,C0,5B), \
 714         V(75,B7,B7,C2), V(E1,FD,FD,1C), V(3D,93,93,AE), V(4C,26,26,6A), \
 715         V(6C,36,36,5A), V(7E,3F,3F,41), V(F5,F7,F7,02), V(83,CC,CC,4F), \
 716         V(68,34,34,5C), V(51,A5,A5,F4), V(D1,E5,E5,34), V(F9,F1,F1,08), \
 717         V(E2,71,71,93), V(AB,D8,D8,73), V(62,31,31,53), V(2A,15,15,3F), \
 718         V(08,04,04,0C), V(95,C7,C7,52), V(46,23,23,65), V(9D,C3,C3,5E), \
 719         V(30,18,18,28), V(37,96,96,A1), V(0A,05,05,0F), V(2F,9A,9A,B5), \
 720         V(0E,07,07,09), V(24,12,12,36), V(1B,80,80,9B), V(DF,E2,E2,3D), \
 721         V(CD,EB,EB,26), V(4E,27,27,69), V(7F,B2,B2,CD), V(EA,75,75,9F), \
 722         V(12,09,09,1B), V(1D,83,83,9E), V(58,2C,2C,74), V(34,1A,1A,2E), \
 723         V(36,1B,1B,2D), V(DC,6E,6E,B2), V(B4,5A,5A,EE), V(5B,A0,A0,FB), \
 724         V(A4,52,52,F6), V(76,3B,3B,4D), V(B7,D6,D6,61), V(7D,B3,B3,CE), \
 725         V(52,29,29,7B), V(DD,E3,E3,3E), V(5E,2F,2F,71), V(13,84,84,97), \
 726         V(A6,53,53,F5), V(B9,D1,D1,68), V(00,00,00,00), V(C1,ED,ED,2C), \
 727         V(40,20,20,60), V(E3,FC,FC,1F), V(79,B1,B1,C8), V(B6,5B,5B,ED), \
 728         V(D4,6A,6A,BE), V(8D,CB,CB,46), V(67,BE,BE,D9), V(72,39,39,4B), \
 729         V(94,4A,4A,DE), V(98,4C,4C,D4), V(B0,58,58,E8), V(85,CF,CF,4A), \
 730         V(BB,D0,D0,6B), V(C5,EF,EF,2A), V(4F,AA,AA,E5), V(ED,FB,FB,16), \
 731         V(86,43,43,C5), V(9A,4D,4D,D7), V(66,33,33,55), V(11,85,85,94), \
 732         V(8A,45,45,CF), V(E9,F9,F9,10), V(04,02,02,06), V(FE,7F,7F,81), \
 733         V(A0,50,50,F0), V(78,3C,3C,44), V(25,9F,9F,BA), V(4B,A8,A8,E3), \
 734         V(A2,51,51,F3), V(5D,A3,A3,FE), V(80,40,40,C0), V(05,8F,8F,8A), \
 735         V(3F,92,92,AD), V(21,9D,9D,BC), V(70,38,38,48), V(F1,F5,F5,04), \
 736         V(63,BC,BC,DF), V(77,B6,B6,C1), V(AF,DA,DA,75), V(42,21,21,63), \
 737         V(20,10,10,30), V(E5,FF,FF,1A), V(FD,F3,F3,0E), V(BF,D2,D2,6D), \
 738         V(81,CD,CD,4C), V(18,0C,0C,14), V(26,13,13,35), V(C3,EC,EC,2F), \
 739         V(BE,5F,5F,E1), V(35,97,97,A2), V(88,44,44,CC), V(2E,17,17,39), \
 740         V(93,C4,C4,57), V(55,A7,A7,F2), V(FC,7E,7E,82), V(7A,3D,3D,47), \
 741         V(C8,64,64,AC), V(BA,5D,5D,E7), V(32,19,19,2B), V(E6,73,73,95), \
 742         V(C0,60,60,A0), V(19,81,81,98), V(9E,4F,4F,D1), V(A3,DC,DC,7F), \
 743         V(44,22,22,66), V(54,2A,2A,7E), V(3B,90,90,AB), V(0B,88,88,83), \
 744         V(8C,46,46,CA), V(C7,EE,EE,29), V(6B,B8,B8,D3), V(28,14,14,3C), \
 745         V(A7,DE,DE,79), V(BC,5E,5E,E2), V(16,0B,0B,1D), V(AD,DB,DB,76), \
 746         V(DB,E0,E0,3B), V(64,32,32,56), V(74,3A,3A,4E), V(14,0A,0A,1E), \
 747         V(92,49,49,DB), V(0C,06,06,0A), V(48,24,24,6C), V(B8,5C,5C,E4), \
 748         V(9F,C2,C2,5D), V(BD,D3,D3,6E), V(43,AC,AC,EF), V(C4,62,62,A6), \
 749         V(39,91,91,A8), V(31,95,95,A4), V(D3,E4,E4,37), V(F2,79,79,8B), \
 750         V(D5,E7,E7,32), V(8B,C8,C8,43), V(6E,37,37,59), V(DA,6D,6D,B7), \
 751         V(01,8D,8D,8C), V(B1,D5,D5,64), V(9C,4E,4E,D2), V(49,A9,A9,E0), \
 752         V(D8,6C,6C,B4), V(AC,56,56,FA), V(F3,F4,F4,07), V(CF,EA,EA,25), \
 753         V(CA,65,65,AF), V(F4,7A,7A,8E), V(47,AE,AE,E9), V(10,08,08,18), \
 754         V(6F,BA,BA,D5), V(F0,78,78,88), V(4A,25,25,6F), V(5C,2E,2E,72), \
 755         V(38,1C,1C,24), V(57,A6,A6,F1), V(73,B4,B4,C7), V(97,C6,C6,51), \
 756         V(CB,E8,E8,23), V(A1,DD,DD,7C), V(E8,74,74,9C), V(3E,1F,1F,21), \
 757         V(96,4B,4B,DD), V(61,BD,BD,DC), V(0D,8B,8B,86), V(0F,8A,8A,85), \
 758         V(E0,70,70,90), V(7C,3E,3E,42), V(71,B5,B5,C4), V(CC,66,66,AA), \
 759         V(90,48,48,D8), V(06,03,03,05), V(F7,F6,F6,01), V(1C,0E,0E,12), \
 760         V(C2,61,61,A3), V(6A,35,35,5F), V(AE,57,57,F9), V(69,B9,B9,D0), \
 761         V(17,86,86,91), V(99,C1,C1,58), V(3A,1D,1D,27), V(27,9E,9E,B9), \
 762         V(D9,E1,E1,38), V(EB,F8,F8,13), V(2B,98,98,B3), V(22,11,11,33), \
 763         V(D2,69,69,BB), V(A9,D9,D9,70), V(07,8E,8E,89), V(33,94,94,A7), \
 764         V(2D,9B,9B,B6), V(3C,1E,1E,22), V(15,87,87,92), V(C9,E9,E9,20), \
 765         V(87,CE,CE,49), V(AA,55,55,FF), V(50,28,28,78), V(A5,DF,DF,7A), \
 766         V(03,8C,8C,8F), V(59,A1,A1,F8), V(09,89,89,80), V(1A,0D,0D,17), \
 767         V(65,BF,BF,DA), V(D7,E6,E6,31), V(84,42,42,C6), V(D0,68,68,B8), \
 768         V(82,41,41,C3), V(29,99,99,B0), V(5A,2D,2D,77), V(1E,0F,0F,11), \
 769         V(7B,B0,B0,CB), V(A8,54,54,FC), V(6D,BB,BB,D6), V(2C,16,16,3A)
 770
 771 #define V(a,b,c,d) 0x##a##b##c##d
 772 static uint32 FT0[256] = { FT };
 773 #undef V
 774
 775 #define V(a,b,c,d) 0x##d##a##b##c
 776 static uint32 FT1[256] = { FT };
 777 #undef V
 778
 779 #define V(a,b,c,d) 0x##c##d##a##b
 780 static uint32 FT2[256] = { FT };
 781 #undef V
 782
 783 #define V(a,b,c,d) 0x##b##c##d##a
 784 static uint32 FT3[256] = { FT };
 785 #undef V
 786
 787 #undef FT
 788
 789 /* reverse S-box */
 790
 791 static uint32 RSb[256] =
 792 {
 793         0x52, 0x09,     0x6A, 0xD5,     0x30, 0x36,     0xA5, 0x38,
 794         0xBF, 0x40,     0xA3, 0x9E,     0x81, 0xF3,     0xD7, 0xFB,
 795         0x7C, 0xE3,     0x39, 0x82,     0x9B, 0x2F,     0xFF, 0x87,
 796         0x34, 0x8E,     0x43, 0x44,     0xC4, 0xDE,     0xE9, 0xCB,
 797         0x54, 0x7B,     0x94, 0x32,     0xA6, 0xC2,     0x23, 0x3D,
 798         0xEE, 0x4C,     0x95, 0x0B,     0x42, 0xFA,     0xC3, 0x4E,
 799         0x08, 0x2E,     0xA1, 0x66,     0x28, 0xD9,     0x24, 0xB2,
 800         0x76, 0x5B,     0xA2, 0x49,     0x6D, 0x8B,     0xD1, 0x25,
 801         0x72, 0xF8,     0xF6, 0x64,     0x86, 0x68,     0x98, 0x16,
 802         0xD4, 0xA4,     0x5C, 0xCC,     0x5D, 0x65,     0xB6, 0x92,
 803         0x6C, 0x70,     0x48, 0x50,     0xFD, 0xED,     0xB9, 0xDA,
 804         0x5E, 0x15,     0x46, 0x57,     0xA7, 0x8D,     0x9D, 0x84,
 805         0x90, 0xD8,     0xAB, 0x00,     0x8C, 0xBC,     0xD3, 0x0A,
 806         0xF7, 0xE4,     0x58, 0x05,     0xB8, 0xB3,     0x45, 0x06,
 807         0xD0, 0x2C,     0x1E, 0x8F,     0xCA, 0x3F,     0x0F, 0x02,
 808         0xC1, 0xAF,     0xBD, 0x03,     0x01, 0x13,     0x8A, 0x6B,
 809         0x3A, 0x91,     0x11, 0x41,     0x4F, 0x67,     0xDC, 0xEA,
 810         0x97, 0xF2,     0xCF, 0xCE,     0xF0, 0xB4,     0xE6, 0x73,
 811         0x96, 0xAC,     0x74, 0x22,     0xE7, 0xAD,     0x35, 0x85,
 812         0xE2, 0xF9,     0x37, 0xE8,     0x1C, 0x75,     0xDF, 0x6E,
 813         0x47, 0xF1,     0x1A, 0x71,     0x1D, 0x29,     0xC5, 0x89,
 814         0x6F, 0xB7,     0x62, 0x0E,     0xAA, 0x18,     0xBE, 0x1B,
 815         0xFC, 0x56,     0x3E, 0x4B,     0xC6, 0xD2,     0x79, 0x20,
 816         0x9A, 0xDB,     0xC0, 0xFE,     0x78, 0xCD,     0x5A, 0xF4,
 817         0x1F, 0xDD,     0xA8, 0x33,     0x88, 0x07,     0xC7, 0x31,
 818         0xB1, 0x12,     0x10, 0x59,     0x27, 0x80,     0xEC, 0x5F,
 819         0x60, 0x51,     0x7F, 0xA9,     0x19, 0xB5,     0x4A, 0x0D,
 820         0x2D, 0xE5,     0x7A, 0x9F,     0x93, 0xC9,     0x9C, 0xEF,
 821         0xA0, 0xE0,     0x3B, 0x4D,     0xAE, 0x2A,     0xF5, 0xB0,
 822         0xC8, 0xEB,     0xBB, 0x3C,     0x83, 0x53,     0x99, 0x61,
 823         0x17, 0x2B,     0x04, 0x7E,     0xBA, 0x77,     0xD6, 0x26,
 824         0xE1, 0x69,     0x14, 0x63,     0x55, 0x21,     0x0C, 0x7D
 825 };
 826
 827 /* reverse table */
 828
 829 #define RT \
 830 \
 831         V(51,F4,A7,50), V(7E,41,65,53), V(1A,17,A4,C3), V(3A,27,5E,96), \
 832         V(3B,AB,6B,CB), V(1F,9D,45,F1), V(AC,FA,58,AB), V(4B,E3,03,93), \
 833         V(20,30,FA,55), V(AD,76,6D,F6), V(88,CC,76,91), V(F5,02,4C,25), \
 834         V(4F,E5,D7,FC), V(C5,2A,CB,D7), V(26,35,44,80), V(B5,62,A3,8F), \
 835         V(DE,B1,5A,49), V(25,BA,1B,67), V(45,EA,0E,98), V(5D,FE,C0,E1), \
 836         V(C3,2F,75,02), V(81,4C,F0,12), V(8D,46,97,A3), V(6B,D3,F9,C6), \
 837         V(03,8F,5F,E7), V(15,92,9C,95), V(BF,6D,7A,EB), V(95,52,59,DA), \
 838         V(D4,BE,83,2D), V(58,74,21,D3), V(49,E0,69,29), V(8E,C9,C8,44), \
 839         V(75,C2,89,6A), V(F4,8E,79,78), V(99,58,3E,6B), V(27,B9,71,DD), \
 840         V(BE,E1,4F,B6), V(F0,88,AD,17), V(C9,20,AC,66), V(7D,CE,3A,B4), \
 841         V(63,DF,4A,18), V(E5,1A,31,82), V(97,51,33,60), V(62,53,7F,45), \
 842         V(B1,64,77,E0), V(BB,6B,AE,84), V(FE,81,A0,1C), V(F9,08,2B,94), \
 843         V(70,48,68,58), V(8F,45,FD,19), V(94,DE,6C,87), V(52,7B,F8,B7), \
 844         V(AB,73,D3,23), V(72,4B,02,E2), V(E3,1F,8F,57), V(66,55,AB,2A), \
 845         V(B2,EB,28,07), V(2F,B5,C2,03), V(86,C5,7B,9A), V(D3,37,08,A5), \
 846         V(30,28,87,F2), V(23,BF,A5,B2), V(02,03,6A,BA), V(ED,16,82,5C), \
 847         V(8A,CF,1C,2B), V(A7,79,B4,92), V(F3,07,F2,F0), V(4E,69,E2,A1), \
 848         V(65,DA,F4,CD), V(06,05,BE,D5), V(D1,34,62,1F), V(C4,A6,FE,8A), \
 849         V(34,2E,53,9D), V(A2,F3,55,A0), V(05,8A,E1,32), V(A4,F6,EB,75), \
 850         V(0B,83,EC,39), V(40,60,EF,AA), V(5E,71,9F,06), V(BD,6E,10,51), \
 851         V(3E,21,8A,F9), V(96,DD,06,3D), V(DD,3E,05,AE), V(4D,E6,BD,46), \
 852         V(91,54,8D,B5), V(71,C4,5D,05), V(04,06,D4,6F), V(60,50,15,FF), \
 853         V(19,98,FB,24), V(D6,BD,E9,97), V(89,40,43,CC), V(67,D9,9E,77), \
 854         V(B0,E8,42,BD), V(07,89,8B,88), V(E7,19,5B,38), V(79,C8,EE,DB), \
 855         V(A1,7C,0A,47), V(7C,42,0F,E9), V(F8,84,1E,C9), V(00,00,00,00), \
 856         V(09,80,86,83), V(32,2B,ED,48), V(1E,11,70,AC), V(6C,5A,72,4E), \
 857         V(FD,0E,FF,FB), V(0F,85,38,56), V(3D,AE,D5,1E), V(36,2D,39,27), \
 858         V(0A,0F,D9,64), V(68,5C,A6,21), V(9B,5B,54,D1), V(24,36,2E,3A), \
 859         V(0C,0A,67,B1), V(93,57,E7,0F), V(B4,EE,96,D2), V(1B,9B,91,9E), \
 860         V(80,C0,C5,4F), V(61,DC,20,A2), V(5A,77,4B,69), V(1C,12,1A,16), \
 861         V(E2,93,BA,0A), V(C0,A0,2A,E5), V(3C,22,E0,43), V(12,1B,17,1D), \
 862         V(0E,09,0D,0B), V(F2,8B,C7,AD), V(2D,B6,A8,B9), V(14,1E,A9,C8), \
 863         V(57,F1,19,85), V(AF,75,07,4C), V(EE,99,DD,BB), V(A3,7F,60,FD), \
 864         V(F7,01,26,9F), V(5C,72,F5,BC), V(44,66,3B,C5), V(5B,FB,7E,34), \
 865         V(8B,43,29,76), V(CB,23,C6,DC), V(B6,ED,FC,68), V(B8,E4,F1,63), \
 866         V(D7,31,DC,CA), V(42,63,85,10), V(13,97,22,40), V(84,C6,11,20), \
 867         V(85,4A,24,7D), V(D2,BB,3D,F8), V(AE,F9,32,11), V(C7,29,A1,6D), \
 868         V(1D,9E,2F,4B), V(DC,B2,30,F3), V(0D,86,52,EC), V(77,C1,E3,D0), \
 869         V(2B,B3,16,6C), V(A9,70,B9,99), V(11,94,48,FA), V(47,E9,64,22), \
 870         V(A8,FC,8C,C4), V(A0,F0,3F,1A), V(56,7D,2C,D8), V(22,33,90,EF), \
 871         V(87,49,4E,C7), V(D9,38,D1,C1), V(8C,CA,A2,FE), V(98,D4,0B,36), \
 872         V(A6,F5,81,CF), V(A5,7A,DE,28), V(DA,B7,8E,26), V(3F,AD,BF,A4), \
 873         V(2C,3A,9D,E4), V(50,78,92,0D), V(6A,5F,CC,9B), V(54,7E,46,62), \
 874         V(F6,8D,13,C2), V(90,D8,B8,E8), V(2E,39,F7,5E), V(82,C3,AF,F5), \
 875         V(9F,5D,80,BE), V(69,D0,93,7C), V(6F,D5,2D,A9), V(CF,25,12,B3), \
 876         V(C8,AC,99,3B), V(10,18,7D,A7), V(E8,9C,63,6E), V(DB,3B,BB,7B), \
 877         V(CD,26,78,09), V(6E,59,18,F4), V(EC,9A,B7,01), V(83,4F,9A,A8), \
 878         V(E6,95,6E,65), V(AA,FF,E6,7E), V(21,BC,CF,08), V(EF,15,E8,E6), \
 879         V(BA,E7,9B,D9), V(4A,6F,36,CE), V(EA,9F,09,D4), V(29,B0,7C,D6), \
 880         V(31,A4,B2,AF), V(2A,3F,23,31), V(C6,A5,94,30), V(35,A2,66,C0), \
 881         V(74,4E,BC,37), V(FC,82,CA,A6), V(E0,90,D0,B0), V(33,A7,D8,15), \
 882         V(F1,04,98,4A), V(41,EC,DA,F7), V(7F,CD,50,0E), V(17,91,F6,2F), \
 883         V(76,4D,D6,8D), V(43,EF,B0,4D), V(CC,AA,4D,54), V(E4,96,04,DF), \
 884         V(9E,D1,B5,E3), V(4C,6A,88,1B), V(C1,2C,1F,B8), V(46,65,51,7F), \
 885         V(9D,5E,EA,04), V(01,8C,35,5D), V(FA,87,74,73), V(FB,0B,41,2E), \
 886         V(B3,67,1D,5A), V(92,DB,D2,52), V(E9,10,56,33), V(6D,D6,47,13), \
 887         V(9A,D7,61,8C), V(37,A1,0C,7A), V(59,F8,14,8E), V(EB,13,3C,89), \
 888         V(CE,A9,27,EE), V(B7,61,C9,35), V(E1,1C,E5,ED), V(7A,47,B1,3C), \
 889         V(9C,D2,DF,59), V(55,F2,73,3F), V(18,14,CE,79), V(73,C7,37,BF), \
 890         V(53,F7,CD,EA), V(5F,FD,AA,5B), V(DF,3D,6F,14), V(78,44,DB,86), \
 891         V(CA,AF,F3,81), V(B9,68,C4,3E), V(38,24,34,2C), V(C2,A3,40,5F), \
 892         V(16,1D,C3,72), V(BC,E2,25,0C), V(28,3C,49,8B), V(FF,0D,95,41), \
 893         V(39,A8,01,71), V(08,0C,B3,DE), V(D8,B4,E4,9C), V(64,56,C1,90), \
 894         V(7B,CB,84,61), V(D5,32,B6,70), V(48,6C,5C,74), V(D0,B8,57,42)
 895
 896 #define V(a,b,c,d) 0x##a##b##c##d
 897 static uint32 RT0[256] = { RT };
 898 #undef V
 899
 900 #define V(a,b,c,d) 0x##d##a##b##c
 901 static uint32 RT1[256] = { RT };
 902 #undef V
 903
 904 #define V(a,b,c,d) 0x##c##d##a##b
 905 static uint32 RT2[256] = { RT };
 906 #undef V
 907
 908 #define V(a,b,c,d) 0x##b##c##d##a
 909 static uint32 RT3[256] = { RT };
 910 #undef V
 911
 912 #undef RT
 913
 914 /* round constants */
 915
 916 static uint32 RCON[10] =
 917 {
 918         0x01000000,     0x02000000,     0x04000000,     0x08000000,
 919         0x10000000,     0x20000000,     0x40000000,     0x80000000,
 920         0x1B000000,     0x36000000
 921 };
 922
 923 /* key schedule tables */
 924
 925 static int KT_init = 1;
 926
 927 static uint32 KT0[256];
 928 static uint32 KT1[256];
 929 static uint32 KT2[256];
 930 static uint32 KT3[256];
 931
 932 /* platform-independant 32-bit integer manipulation     macros */
 933
 934 #define GET_UINT32(n,b,i)                                               \
 935 {                                                                                               \
 936         (n)     = (     (uint32) (b)[(i)        ] << 24 )               \
 937                 | (     (uint32) (b)[(i) + 1] << 16     )               \
 938                 | (     (uint32) (b)[(i) + 2] <<  8     )               \
 939                 | (     (uint32) (b)[(i) + 3]           );              \
 940 }
 941
 942 #define PUT_UINT32(n,b,i)                                               \
 943 {                                                                                               \
 944         (b)[(i)    ] = (uint8) ( (n) >> 24 );           \
 945         (b)[(i) + 1] = (uint8) ( (n) >> 16 );           \
 946         (b)[(i) + 2] = (uint8) ( (n) >>  8 );           \
 947         (b)[(i) + 3] = (uint8) ( (n)       );           \
 948 }
 949
 950
 951 int     rt_aes_set_key( aes_context *ctx, uint8 *key, int nbits )
 952 {
 953         int     i;
 954         uint32 *RK,     *SK;
 955
 956         switch( nbits )
 957         {
 958                 case 128: ctx->nr =     10;     break;
 959                 case 192: ctx->nr =     12;     break;
 960                 case 256: ctx->nr =     14;     break;
 961                 default : return( 1     );
 962         }
 963
 964         RK = (uint32 *) ctx->erk;
 965
 966         for( i = 0;     i <     (nbits >> 5); i++ )
 967         {
 968                 GET_UINT32(     RK[i], key,     i *     4 );
 969         }
 970
 971         /* setup encryption     round keys */
 972
 973         switch( nbits )
 974         {
 975         case 128:
 976
 977                 for( i = 0;     i <     10;     i++, RK += 4 )
 978                 {
 979                         RK[4]  = RK[0] ^ RCON[i] ^
 980                                                 ( FSb[ (uint8) ( RK[3] >> 16 ) ] <<     24 ) ^
 981                                                 ( FSb[ (uint8) ( RK[3] >>  8 ) ] <<     16 ) ^
 982                                                 ( FSb[ (uint8) ( RK[3]           ) ] <<  8 ) ^
 983                                                 ( FSb[ (uint8) ( RK[3] >> 24 ) ]           );
 984
 985                         RK[5]  = RK[1] ^ RK[4];
 986                         RK[6]  = RK[2] ^ RK[5];
 987                         RK[7]  = RK[3] ^ RK[6];
 988                 }
 989                 break;
 990
 991         case 192:
 992
 993                 for( i = 0;     i <     8; i++, RK += 6 )
 994                 {
 995                         RK[6]  = RK[0] ^ RCON[i] ^
 996                                                 ( FSb[ (uint8) ( RK[5] >> 16 ) ] <<     24 ) ^
 997                                                 ( FSb[ (uint8) ( RK[5] >>  8 ) ] <<     16 ) ^
 998                                                 ( FSb[ (uint8) ( RK[5]           ) ] <<  8 ) ^
 999                                                 ( FSb[ (uint8) ( RK[5] >> 24 ) ]           );
1000
1001                         RK[7]  = RK[1] ^ RK[6];
1002                         RK[8]  = RK[2] ^ RK[7];
1003                         RK[9]  = RK[3] ^ RK[8];
1004                         RK[10] = RK[4] ^ RK[9];
1005                         RK[11] = RK[5] ^ RK[10];
1006                 }
1007                 break;
1008
1009         case 256:
1010
1011                 for( i = 0;     i <     7; i++, RK += 8 )
1012                 {
1013                         RK[8]  = RK[0] ^ RCON[i] ^
1014                                                 ( FSb[ (uint8) ( RK[7] >> 16 ) ] <<     24 ) ^
1015                                                 ( FSb[ (uint8) ( RK[7] >>  8 ) ] <<     16 ) ^
1016                                                 ( FSb[ (uint8) ( RK[7]           ) ] <<  8 ) ^
1017                                                 ( FSb[ (uint8) ( RK[7] >> 24 ) ]           );
1018
1019                         RK[9]  = RK[1] ^ RK[8];
1020                         RK[10] = RK[2] ^ RK[9];
1021                         RK[11] = RK[3] ^ RK[10];
1022
1023                         RK[12] = RK[4] ^
1024                                                 ( FSb[ (uint8) ( RK[11] >> 24 ) ] << 24 ) ^
1025                                                 ( FSb[ (uint8) ( RK[11] >> 16 ) ] << 16 ) ^
1026                                                 ( FSb[ (uint8) ( RK[11] >>      8 )     ] <<  8 ) ^
1027                                                 ( FSb[ (uint8) ( RK[11]           )     ]               );
1028
1029                         RK[13] = RK[5] ^ RK[12];
1030                         RK[14] = RK[6] ^ RK[13];
1031                         RK[15] = RK[7] ^ RK[14];
1032                 }
1033                 break;
1034         }
1035
1036         /* setup decryption     round keys */
1037
1038         if(     KT_init )
1039         {
1040                 for( i = 0;     i <     256; i++ )
1041                 {
1042                         KT0[i] = RT0[ FSb[i] ];
1043                         KT1[i] = RT1[ FSb[i] ];
1044                         KT2[i] = RT2[ FSb[i] ];
1045                         KT3[i] = RT3[ FSb[i] ];
1046                 }
1047
1048                 KT_init = 0;
1049         }
1050
1051         SK = (uint32 *) ctx->drk;
1052
1053         *SK++ = *RK++;
1054         *SK++ = *RK++;
1055         *SK++ = *RK++;
1056         *SK++ = *RK++;
1057
1058         for( i = 1;     i <     ctx->nr; i++ )
1059         {
1060                 RK -= 8;
1061
1062                 *SK++ = KT0[ (uint8) ( *RK >> 24 ) ] ^
1063                                 KT1[ (uint8) ( *RK >> 16 ) ] ^
1064                                 KT2[ (uint8) ( *RK >>  8 ) ] ^
1065                                 KT3[ (uint8) ( *RK               ) ]; RK++;
1066
1067                 *SK++ = KT0[ (uint8) ( *RK >> 24 ) ] ^
1068                                 KT1[ (uint8) ( *RK >> 16 ) ] ^
1069                                 KT2[ (uint8) ( *RK >>  8 ) ] ^
1070                                 KT3[ (uint8) ( *RK               ) ]; RK++;
1071
1072                 *SK++ = KT0[ (uint8) ( *RK >> 24 ) ] ^
1073                                 KT1[ (uint8) ( *RK >> 16 ) ] ^
1074                                 KT2[ (uint8) ( *RK >>  8 ) ] ^
1075                                 KT3[ (uint8) ( *RK               ) ]; RK++;
1076
1077                 *SK++ = KT0[ (uint8) ( *RK >> 24 ) ] ^
1078                                 KT1[ (uint8) ( *RK >> 16 ) ] ^
1079                                 KT2[ (uint8) ( *RK >>  8 ) ] ^
1080                                 KT3[ (uint8) ( *RK               ) ]; RK++;
1081         }
1082
1083         RK -= 8;
1084
1085         *SK++ = *RK++;
1086         *SK++ = *RK++;
1087         *SK++ = *RK++;
1088         *SK++ = *RK++;
1089
1090         return( 0 );
1091 }
1092
1093 /* AES 128-bit block encryption routine */
1094
1095 void rt_aes_encrypt(aes_context *ctx, uint8 input[16],  uint8 output[16] )
1096 {
1097         uint32 *RK,     X0,     X1,     X2,     X3,     Y0,     Y1,     Y2,     Y3;
1098
1099         RK = (uint32 *) ctx->erk;
1100         GET_UINT32(     X0,     input,  0 ); X0 ^= RK[0];
1101         GET_UINT32(     X1,     input,  4 ); X1 ^= RK[1];
1102         GET_UINT32(     X2,     input,  8 ); X2 ^= RK[2];
1103         GET_UINT32(     X3,     input, 12 ); X3 ^= RK[3];
1104
1105 #define AES_FROUND(X0,X1,X2,X3,Y0,Y1,Y2,Y3)             \
1106 {                                                                                               \
1107         RK += 4;                                                                        \
1108                                                                                                 \
1109         X0 = RK[0] ^ FT0[ (uint8) (     Y0 >> 24 ) ] ^  \
1110                                  FT1[ (uint8) ( Y1 >> 16 ) ] ^  \
1111                                  FT2[ (uint8) ( Y2 >>  8 ) ] ^  \
1112                                  FT3[ (uint8) ( Y3               ) ];   \
1113                                                                                                 \
1114         X1 = RK[1] ^ FT0[ (uint8) (     Y1 >> 24 ) ] ^  \
1115                                  FT1[ (uint8) ( Y2 >> 16 ) ] ^  \
1116                                  FT2[ (uint8) ( Y3 >>  8 ) ] ^  \
1117                                  FT3[ (uint8) ( Y0               ) ];   \
1118                                                                                                 \
1119         X2 = RK[2] ^ FT0[ (uint8) (     Y2 >> 24 ) ] ^  \
1120                                  FT1[ (uint8) ( Y3 >> 16 ) ] ^  \
1121                                  FT2[ (uint8) ( Y0 >>  8 ) ] ^  \
1122                                  FT3[ (uint8) ( Y1               ) ];   \
1123                                                                                                 \
1124         X3 = RK[3] ^ FT0[ (uint8) (     Y3 >> 24 ) ] ^  \
1125                                  FT1[ (uint8) ( Y0 >> 16 ) ] ^  \
1126                                  FT2[ (uint8) ( Y1 >>  8 ) ] ^  \
1127                                  FT3[ (uint8) ( Y2               ) ];   \
1128 }
1129
1130         AES_FROUND(     Y0,     Y1,     Y2,     Y3,     X0,     X1,     X2,     X3 );           /* round 1 */
1131         AES_FROUND(     X0,     X1,     X2,     X3,     Y0,     Y1,     Y2,     Y3 );           /* round 2 */
1132         AES_FROUND(     Y0,     Y1,     Y2,     Y3,     X0,     X1,     X2,     X3 );           /* round 3 */
1133         AES_FROUND(     X0,     X1,     X2,     X3,     Y0,     Y1,     Y2,     Y3 );           /* round 4 */
1134         AES_FROUND(     Y0,     Y1,     Y2,     Y3,     X0,     X1,     X2,     X3 );           /* round 5 */
1135         AES_FROUND(     X0,     X1,     X2,     X3,     Y0,     Y1,     Y2,     Y3 );           /* round 6 */
1136         AES_FROUND(     Y0,     Y1,     Y2,     Y3,     X0,     X1,     X2,     X3 );           /* round 7 */
1137         AES_FROUND(     X0,     X1,     X2,     X3,     Y0,     Y1,     Y2,     Y3 );           /* round 8 */
1138         AES_FROUND(     Y0,     Y1,     Y2,     Y3,     X0,     X1,     X2,     X3 );           /* round 9 */
1139
1140         if(     ctx->nr > 10 )
1141         {
1142                 AES_FROUND(     X0,     X1,     X2,     X3,     Y0,     Y1,     Y2,     Y3 );   /* round 10     */
1143                 AES_FROUND(     Y0,     Y1,     Y2,     Y3,     X0,     X1,     X2,     X3 );   /* round 11     */
1144         }
1145
1146         if(     ctx->nr > 12 )
1147         {
1148                 AES_FROUND(     X0,     X1,     X2,     X3,     Y0,     Y1,     Y2,     Y3 );   /* round 12     */
1149                 AES_FROUND(     Y0,     Y1,     Y2,     Y3,     X0,     X1,     X2,     X3 );   /* round 13     */
1150         }
1151
1152         /* last round */
1153
1154         RK += 4;
1155
1156         X0 = RK[0] ^ ( FSb[     (uint8) ( Y0 >> 24 ) ] << 24 ) ^
1157                                  ( FSb[ (uint8) ( Y1 >> 16 ) ] << 16 ) ^
1158                                  ( FSb[ (uint8) ( Y2 >>  8 ) ] <<  8 ) ^
1159                                  ( FSb[ (uint8) ( Y3       ) ]           );
1160
1161         X1 = RK[1] ^ ( FSb[     (uint8) ( Y1 >> 24 ) ] << 24 ) ^
1162                                  ( FSb[ (uint8) ( Y2 >> 16 ) ] << 16 ) ^
1163                                  ( FSb[ (uint8) ( Y3 >>  8 ) ] <<  8 ) ^
1164                                  ( FSb[ (uint8) ( Y0       ) ]           );
1165
1166         X2 = RK[2] ^ ( FSb[     (uint8) ( Y2 >> 24 ) ] << 24 ) ^
1167                                  ( FSb[ (uint8) ( Y3 >> 16 ) ] << 16 ) ^
1168                                  ( FSb[ (uint8) ( Y0 >>  8 ) ] <<  8 ) ^
1169                                  ( FSb[ (uint8) ( Y1       ) ]           );
1170
1171         X3 = RK[3] ^ ( FSb[     (uint8) ( Y3 >> 24 ) ] << 24 ) ^
1172                                  ( FSb[ (uint8) ( Y0 >> 16 ) ] << 16 ) ^
1173                                  ( FSb[ (uint8) ( Y1 >>  8 ) ] <<  8 ) ^
1174                                  ( FSb[ (uint8) ( Y2       ) ]           );
1175
1176         PUT_UINT32(     X0,     output,  0 );
1177         PUT_UINT32(     X1,     output,  4 );
1178         PUT_UINT32(     X2,     output,  8 );
1179         PUT_UINT32(     X3,     output, 12 );
1180 }
1181
1182 /* AES 128-bit block decryption routine */
1183
1184 void rt_aes_decrypt( aes_context *ctx,  uint8 input[16], uint8 output[16] )
1185 {
1186         uint32 *RK,     X0,     X1,     X2,     X3,     Y0,     Y1,     Y2,     Y3;
1187
1188         RK = (uint32 *) ctx->drk;
1189
1190         GET_UINT32(     X0,     input,  0 ); X0 ^= RK[0];
1191         GET_UINT32(     X1,     input,  4 ); X1 ^= RK[1];
1192         GET_UINT32(     X2,     input,  8 ); X2 ^= RK[2];
1193         GET_UINT32(     X3,     input, 12 ); X3 ^= RK[3];
1194
1195 #define AES_RROUND(X0,X1,X2,X3,Y0,Y1,Y2,Y3)             \
1196 {                                                                                               \
1197         RK += 4;                                                                        \
1198                                                                                                 \
1199         X0 = RK[0] ^ RT0[ (uint8) (     Y0 >> 24 ) ] ^  \
1200                                  RT1[ (uint8) ( Y3 >> 16 ) ] ^  \
1201                                  RT2[ (uint8) ( Y2 >>  8 ) ] ^  \
1202                                  RT3[ (uint8) ( Y1               ) ];   \
1203                                                                                                 \
1204         X1 = RK[1] ^ RT0[ (uint8) (     Y1 >> 24 ) ] ^  \
1205                                  RT1[ (uint8) ( Y0 >> 16 ) ] ^  \
1206                                  RT2[ (uint8) ( Y3 >>  8 ) ] ^  \
1207                                  RT3[ (uint8) ( Y2               ) ];   \
1208                                                                                                 \
1209         X2 = RK[2] ^ RT0[ (uint8) (     Y2 >> 24 ) ] ^  \
1210                                  RT1[ (uint8) ( Y1 >> 16 ) ] ^  \
1211                                  RT2[ (uint8) ( Y0 >>  8 ) ] ^  \
1212                                  RT3[ (uint8) ( Y3               ) ];   \
1213                                                                                                 \
1214         X3 = RK[3] ^ RT0[ (uint8) (     Y3 >> 24 ) ] ^  \
1215                                  RT1[ (uint8) ( Y2 >> 16 ) ] ^  \
1216                                  RT2[ (uint8) ( Y1 >>  8 ) ] ^  \
1217                                  RT3[ (uint8) ( Y0               ) ];   \
1218 }
1219
1220         AES_RROUND(     Y0,     Y1,     Y2,     Y3,     X0,     X1,     X2,     X3 );           /* round 1 */
1221         AES_RROUND(     X0,     X1,     X2,     X3,     Y0,     Y1,     Y2,     Y3 );           /* round 2 */
1222         AES_RROUND(     Y0,     Y1,     Y2,     Y3,     X0,     X1,     X2,     X3 );           /* round 3 */
1223         AES_RROUND(     X0,     X1,     X2,     X3,     Y0,     Y1,     Y2,     Y3 );           /* round 4 */
1224         AES_RROUND(     Y0,     Y1,     Y2,     Y3,     X0,     X1,     X2,     X3 );           /* round 5 */
1225         AES_RROUND(     X0,     X1,     X2,     X3,     Y0,     Y1,     Y2,     Y3 );           /* round 6 */
1226         AES_RROUND(     Y0,     Y1,     Y2,     Y3,     X0,     X1,     X2,     X3 );           /* round 7 */
1227         AES_RROUND(     X0,     X1,     X2,     X3,     Y0,     Y1,     Y2,     Y3 );           /* round 8 */
1228         AES_RROUND(     Y0,     Y1,     Y2,     Y3,     X0,     X1,     X2,     X3 );           /* round 9 */
1229
1230         if(     ctx->nr > 10 )
1231         {
1232                 AES_RROUND(     X0,     X1,     X2,     X3,     Y0,     Y1,     Y2,     Y3 );   /* round 10     */
1233                 AES_RROUND(     Y0,     Y1,     Y2,     Y3,     X0,     X1,     X2,     X3 );   /* round 11     */
1234         }
1235
1236         if(     ctx->nr > 12 )
1237         {
1238                 AES_RROUND(     X0,     X1,     X2,     X3,     Y0,     Y1,     Y2,     Y3 );   /* round 12     */
1239                 AES_RROUND(     Y0,     Y1,     Y2,     Y3,     X0,     X1,     X2,     X3 );   /* round 13     */
1240         }
1241
1242         /* last round */
1243
1244         RK += 4;
1245
1246         X0 = RK[0] ^ ( RSb[     (uint8) ( Y0 >> 24 ) ] << 24 ) ^
1247                                  ( RSb[ (uint8) ( Y3 >> 16 ) ] << 16 ) ^
1248                                  ( RSb[ (uint8) ( Y2 >>  8 ) ] <<  8 ) ^
1249                                  ( RSb[ (uint8) ( Y1       ) ]           );
1250
1251         X1 = RK[1] ^ ( RSb[     (uint8) ( Y1 >> 24 ) ] << 24 ) ^
1252                                  ( RSb[ (uint8) ( Y0 >> 16 ) ] << 16 ) ^
1253                                  ( RSb[ (uint8) ( Y3 >>  8 ) ] <<  8 ) ^
1254                                  ( RSb[ (uint8) ( Y2       ) ]           );
1255
1256         X2 = RK[2] ^ ( RSb[     (uint8) ( Y2 >> 24 ) ] << 24 ) ^
1257                                  ( RSb[ (uint8) ( Y1 >> 16 ) ] << 16 ) ^
1258                                  ( RSb[ (uint8) ( Y0 >>  8 ) ] <<  8 ) ^
1259                                  ( RSb[ (uint8) ( Y3       ) ]           );
1260
1261         X3 = RK[3] ^ ( RSb[     (uint8) ( Y3 >> 24 ) ] << 24 ) ^
1262                                  ( RSb[ (uint8) ( Y2 >> 16 ) ] << 16 ) ^
1263                                  ( RSb[ (uint8) ( Y1 >>  8 ) ] <<  8 ) ^
1264                                  ( RSb[ (uint8) ( Y0       ) ]           );
1265
1266         PUT_UINT32(     X0,     output,  0 );
1267         PUT_UINT32(     X1,     output,  4 );
1268         PUT_UINT32(     X2,     output,  8 );
1269         PUT_UINT32(     X3,     output, 12 );
1270 }
1271
1272 /*
1273     ==========================================================================
1274     Description:
1275         ENCRYPT AES GTK before sending in EAPOL frame.
1276         AES GTK length = 128 bit,  so fix blocks for aes-key-wrap as 2 in this function.
1277         This function references to RFC 3394 for aes key wrap algorithm.
1278     Return:
1279     ==========================================================================
1280 */
1281 VOID AES_GTK_KEY_WRAP(
1282     IN UCHAR    *key,
1283     IN UCHAR    *plaintext,
1284     IN UINT32    p_len,
1285     OUT UCHAR   *ciphertext)
1286 {
1287     UCHAR       A[8], BIN[16], BOUT[16];
1288     UCHAR       R[512];
1289     INT         num_blocks = p_len/8;   // unit:64bits
1290     INT         i, j;
1291     aes_context aesctx;
1292     UCHAR       xor;
1293
1294     rt_aes_set_key(&aesctx, key, 128);
1295
1296     // Init IA
1297     for (i = 0; i < 8; i++)
1298         A[i] = 0xa6;
1299
1300     //Input plaintext
1301     for (i = 0; i < num_blocks; i++)
1302     {
1303         for (j = 0 ; j < 8; j++)
1304             R[8 * (i + 1) + j] = plaintext[8 * i + j];
1305     }
1306
1307     // Key Mix
1308     for (j = 0; j < 6; j++)
1309     {
1310         for(i = 1; i <= num_blocks; i++)
1311         {
1312             //phase 1
1313             NdisMoveMemory(BIN, A, 8);
1314             NdisMoveMemory(&BIN[8], &R[8 * i], 8);
1315             rt_aes_encrypt(&aesctx, BIN, BOUT);
1316
1317             NdisMoveMemory(A, &BOUT[0], 8);
1318             xor = num_blocks * j + i;
1319             A[7] = BOUT[7] ^ xor;
1320             NdisMoveMemory(&R[8 * i], &BOUT[8], 8);
1321         }
1322     }
1323
1324     // Output ciphertext
1325     NdisMoveMemory(ciphertext, A, 8);
1326
1327     for (i = 1; i <= num_blocks; i++)
1328     {
1329         for (j = 0 ; j < 8; j++)
1330             ciphertext[8 * i + j] = R[8 * i + j];
1331     }
1332 }
1333
1334 /*
1335         ========================================================================
1336
1337         Routine Description:
1338                 Misc function to decrypt AES body
1339
1340         Arguments:
1341
1342         Return Value:
1343
1344         Note:
1345                 This function references to     RFC     3394 for aes key unwrap algorithm.
1346
1347         ========================================================================
1348 */
1349 VOID    AES_GTK_KEY_UNWRAP(
1350         IN      UCHAR   *key,
1351         OUT     UCHAR   *plaintext,
1352         IN      UINT32   c_len,
1353         IN      UCHAR   *ciphertext)
1354
1355 {
1356         UCHAR       A[8], BIN[16], BOUT[16];
1357         UCHAR       xor;
1358         INT         i, j;
1359         aes_context aesctx;
1360         UCHAR       *R;
1361         INT         num_blocks = c_len/8;       // unit:64bits
1362
1363
1364         os_alloc_mem(NULL, (PUCHAR *)&R, 512);
1365
1366         if (R == NULL)
1367     {
1368         DBGPRINT(RT_DEBUG_ERROR, ("!!!AES_GTK_KEY_UNWRAP: no memory!!!\n"));
1369         return;
1370     } /* End of if */
1371
1372         // Initialize
1373         NdisMoveMemory(A, ciphertext, 8);
1374         //Input plaintext
1375         for(i = 0; i < (c_len-8); i++)
1376         {
1377                 R[ i] = ciphertext[i + 8];
1378         }
1379
1380         rt_aes_set_key(&aesctx, key, 128);
1381
1382         for(j = 5; j >= 0; j--)
1383         {
1384                 for(i = (num_blocks-1); i > 0; i--)
1385                 {
1386                         xor = (num_blocks -1 )* j + i;
1387                         NdisMoveMemory(BIN, A, 8);
1388                         BIN[7] = A[7] ^ xor;
1389                         NdisMoveMemory(&BIN[8], &R[(i-1)*8], 8);
1390                         rt_aes_decrypt(&aesctx, BIN, BOUT);
1391                         NdisMoveMemory(A, &BOUT[0], 8);
1392                         NdisMoveMemory(&R[(i-1)*8], &BOUT[8], 8);
1393                 }
1394         }
1395
1396         // OUTPUT
1397         for(i = 0; i < c_len; i++)
1398         {
1399                 plaintext[i] = R[i];
1400         }
1401
1402
1403         os_free_mem(NULL, R);
1404 }