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