drivers/staging/rtl8188eu/core/rtw_security.c

   1 /******************************************************************************
   2  *
   3  * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
   4  *
   5  * This program is free software; you can redistribute it and/or modify it
   6  * under the terms of version 2 of the GNU General Public License as
   7  * published by the Free Software Foundation.
   8  *
   9  * This program is distributed in the hope that it will be useful, but WITHOUT
  10  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  11  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
  12  * more details.
  13  *
  14  ******************************************************************************/
  15 #define  _RTW_SECURITY_C_
  16
  17 #include <osdep_service.h>
  18 #include <drv_types.h>
  19 #include <wifi.h>
  20 #include <osdep_intf.h>
  21
  22 /* WEP related ===== */
  23
  24 #define CRC32_POLY 0x04c11db7
  25
  26 struct arc4context {
  27         u32 x;
  28         u32 y;
  29         u8 state[256];
  30 };
  31
  32 static void arcfour_init(struct arc4context *parc4ctx, u8 *key, u32     key_len)
  33 {
  34         u32     t, u;
  35         u32     keyindex;
  36         u32     stateindex;
  37         u8 *state;
  38         u32     counter;
  39
  40         state = parc4ctx->state;
  41         parc4ctx->x = 0;
  42         parc4ctx->y = 0;
  43         for (counter = 0; counter < 256; counter++)
  44                 state[counter] = (u8)counter;
  45         keyindex = 0;
  46         stateindex = 0;
  47         for (counter = 0; counter < 256; counter++) {
  48                 t = state[counter];
  49                 stateindex = (stateindex + key[keyindex] + t) & 0xff;
  50                 u = state[stateindex];
  51                 state[stateindex] = (u8)t;
  52                 state[counter] = (u8)u;
  53                 if (++keyindex >= key_len)
  54                         keyindex = 0;
  55         }
  56 }
  57
  58 static u32 arcfour_byte(struct arc4context *parc4ctx)
  59 {
  60         u32 x;
  61         u32 y;
  62         u32 sx, sy;
  63         u8 *state;
  64
  65         state = parc4ctx->state;
  66         x = (parc4ctx->x + 1) & 0xff;
  67         sx = state[x];
  68         y = (sx + parc4ctx->y) & 0xff;
  69         sy = state[y];
  70         parc4ctx->x = x;
  71         parc4ctx->y = y;
  72         state[y] = (u8)sx;
  73         state[x] = (u8)sy;
  74         return state[(sx + sy) & 0xff];
  75 }
  76
  77 static void arcfour_encrypt(struct arc4context *parc4ctx, u8 *dest, u8 *src, u32 len)
  78 {
  79         u32     i;
  80
  81         for (i = 0; i < len; i++)
  82                 dest[i] = src[i] ^ (unsigned char)arcfour_byte(parc4ctx);
  83 }
  84
  85 static int bcrc32initialized;
  86 static u32 crc32_table[256];
  87
  88 static u8 crc32_reverseBit(u8 data)
  89 {
  90         return (u8)((data<<7)&0x80) | ((data<<5)&0x40) | ((data<<3)&0x20) |
  91                    ((data<<1)&0x10) | ((data>>1)&0x08) | ((data>>3)&0x04) |
  92                    ((data>>5)&0x02) | ((data>>7)&0x01);
  93 }
  94
  95 static void crc32_init(void)
  96 {
  97         if (bcrc32initialized == 1) {
  98                 return;
  99         } else {
 100                 int i, j;
 101                 u32 c;
 102                 u8 *p = (u8 *)&c, *p1;
 103                 u8 k;
 104
 105                 c = 0x12340000;
 106
 107                 for (i = 0; i < 256; ++i) {
 108                         k = crc32_reverseBit((u8)i);
 109                         for (c = ((u32)k) << 24, j = 8; j > 0; --j)
 110                                 c = c & 0x80000000 ? (c << 1) ^ CRC32_POLY : (c << 1);
 111                         p1 = (u8 *)&crc32_table[i];
 112
 113                         p1[0] = crc32_reverseBit(p[3]);
 114                         p1[1] = crc32_reverseBit(p[2]);
 115                         p1[2] = crc32_reverseBit(p[1]);
 116                         p1[3] = crc32_reverseBit(p[0]);
 117                 }
 118                 bcrc32initialized = 1;
 119         }
 120 }
 121
 122 static __le32 getcrc32(u8 *buf, int len)
 123 {
 124         u8 *p;
 125         u32  crc;
 126
 127         if (bcrc32initialized == 0)
 128                 crc32_init();
 129
 130         crc = 0xffffffff;       /* preload shift register, per CRC-32 spec */
 131
 132         for (p = buf; len > 0; ++p, --len)
 133                 crc = crc32_table[(crc ^ *p) & 0xff] ^ (crc >> 8);
 134         return cpu_to_le32(~crc);    /* transmit complement, per CRC-32 spec */
 135 }
 136
 137 /*
 138         Need to consider the fragment  situation
 139 */
 140 void rtw_wep_encrypt(struct adapter *padapter, u8 *pxmitframe)
 141 {       /*  exclude ICV */
 142
 143         unsigned char   crc[4];
 144         struct arc4context       mycontext;
 145
 146         int     curfragnum, length;
 147         u32     keylength;
 148
 149         u8      *pframe, *payload, *iv;    /* wepkey */
 150         u8      wepkey[16];
 151         u8   hw_hdr_offset = 0;
 152         struct  pkt_attrib       *pattrib = &((struct xmit_frame *)pxmitframe)->attrib;
 153         struct  security_priv   *psecuritypriv = &padapter->securitypriv;
 154         struct  xmit_priv               *pxmitpriv = &padapter->xmitpriv;
 155
 156
 157         if (((struct xmit_frame *)pxmitframe)->buf_addr == NULL)
 158                 return;
 159
 160         hw_hdr_offset = TXDESC_SIZE +
 161                  (((struct xmit_frame *)pxmitframe)->pkt_offset * PACKET_OFFSET_SZ);
 162
 163         pframe = ((struct xmit_frame *)pxmitframe)->buf_addr + hw_hdr_offset;
 164
 165         /* start to encrypt each fragment */
 166         if ((pattrib->encrypt == _WEP40_) || (pattrib->encrypt == _WEP104_)) {
 167                 keylength = psecuritypriv->dot11DefKeylen[psecuritypriv->dot11PrivacyKeyIndex];
 168
 169                 for (curfragnum = 0; curfragnum < pattrib->nr_frags; curfragnum++) {
 170                         iv = pframe+pattrib->hdrlen;
 171                         memcpy(&wepkey[0], iv, 3);
 172                         memcpy(&wepkey[3], &psecuritypriv->dot11DefKey[psecuritypriv->dot11PrivacyKeyIndex].skey[0], keylength);
 173                         payload = pframe+pattrib->iv_len+pattrib->hdrlen;
 174
 175                         if ((curfragnum+1) == pattrib->nr_frags) {      /* the last fragment */
 176                                 length = pattrib->last_txcmdsz-pattrib->hdrlen-pattrib->iv_len-pattrib->icv_len;
 177
 178                                 *((__le32 *)crc) = getcrc32(payload, length);
 179
 180                                 arcfour_init(&mycontext, wepkey, 3+keylength);
 181                                 arcfour_encrypt(&mycontext, payload, payload, length);
 182                                 arcfour_encrypt(&mycontext, payload+length, crc, 4);
 183                         } else {
 184                                 length = pxmitpriv->frag_len-pattrib->hdrlen-pattrib->iv_len-pattrib->icv_len;
 185                                 *((__le32 *)crc) = getcrc32(payload, length);
 186                                 arcfour_init(&mycontext, wepkey, 3+keylength);
 187                                 arcfour_encrypt(&mycontext, payload, payload, length);
 188                                 arcfour_encrypt(&mycontext, payload+length, crc, 4);
 189
 190                                 pframe += pxmitpriv->frag_len;
 191                                 pframe = (u8 *)round_up((size_t)(pframe), 4);
 192                         }
 193                 }
 194         }
 195
 196 }
 197
 198 void rtw_wep_decrypt(struct adapter  *padapter, u8 *precvframe)
 199 {
 200         /*  exclude ICV */
 201         u8      crc[4];
 202         struct arc4context       mycontext;
 203         int     length;
 204         u32     keylength;
 205         u8      *pframe, *payload, *iv, wepkey[16];
 206         u8       keyindex;
 207         struct  rx_pkt_attrib    *prxattrib = &(((struct recv_frame *)precvframe)->attrib);
 208         struct  security_priv   *psecuritypriv = &padapter->securitypriv;
 209
 210
 211         pframe = (unsigned char *)((struct recv_frame *)precvframe)->pkt->data;
 212
 213         /* start to decrypt recvframe */
 214         if ((prxattrib->encrypt == _WEP40_) || (prxattrib->encrypt == _WEP104_)) {
 215                 iv = pframe+prxattrib->hdrlen;
 216                 keyindex = prxattrib->key_index;
 217                 keylength = psecuritypriv->dot11DefKeylen[keyindex];
 218                 memcpy(&wepkey[0], iv, 3);
 219                 memcpy(&wepkey[3], &psecuritypriv->dot11DefKey[keyindex].skey[0], keylength);
 220                 length = ((struct recv_frame *)precvframe)->pkt->len-prxattrib->hdrlen-prxattrib->iv_len;
 221
 222                 payload = pframe+prxattrib->iv_len+prxattrib->hdrlen;
 223
 224                 /* decrypt payload include icv */
 225                 arcfour_init(&mycontext, wepkey, 3+keylength);
 226                 arcfour_encrypt(&mycontext, payload, payload,  length);
 227
 228                 /* calculate icv and compare the icv */
 229                 *((__le32 *)crc) = getcrc32(payload, length - 4);
 230
 231                 if (crc[3] != payload[length-1] ||
 232                     crc[2] != payload[length-2] ||
 233                     crc[1] != payload[length-3] ||
 234                     crc[0] != payload[length-4]) {
 235                         RT_TRACE(_module_rtl871x_security_c_, _drv_err_,
 236                                  ("rtw_wep_decrypt:icv error crc (%4ph)!=payload (%4ph)\n",
 237                                  &crc, &payload[length-4]));
 238                 }
 239         }
 240 }
 241
 242 /* 3            ===== TKIP related ===== */
 243
 244 static u32 secmicgetuint32(u8 *p)
 245 /*  Convert from Byte[] to Us3232 in a portable way */
 246 {
 247         s32 i;
 248         u32 res = 0;
 249
 250         for (i = 0; i < 4; i++)
 251                 res |= ((u32)(*p++)) << (8*i);
 252         return res;
 253 }
 254
 255 static void secmicputuint32(u8 *p, u32 val)
 256 /*  Convert from Us3232 to Byte[] in a portable way */
 257 {
 258         long i;
 259
 260         for (i = 0; i < 4; i++) {
 261                 *p++ = (u8)(val & 0xff);
 262                 val >>= 8;
 263         }
 264 }
 265
 266 static void secmicclear(struct mic_data *pmicdata)
 267 {
 268 /*  Reset the state to the empty message. */
 269         pmicdata->L = pmicdata->K0;
 270         pmicdata->R = pmicdata->K1;
 271         pmicdata->nBytesInM = 0;
 272         pmicdata->M = 0;
 273 }
 274
 275 void rtw_secmicsetkey(struct mic_data *pmicdata, u8 *key)
 276 {
 277         /*  Set the key */
 278         pmicdata->K0 = secmicgetuint32(key);
 279         pmicdata->K1 = secmicgetuint32(key + 4);
 280         /*  and reset the message */
 281         secmicclear(pmicdata);
 282 }
 283
 284 void rtw_secmicappendbyte(struct mic_data *pmicdata, u8 b)
 285 {
 286         /*  Append the byte to our word-sized buffer */
 287         pmicdata->M |= ((unsigned long)b) << (8*pmicdata->nBytesInM);
 288         pmicdata->nBytesInM++;
 289         /*  Process the word if it is full. */
 290         if (pmicdata->nBytesInM >= 4) {
 291                 pmicdata->L ^= pmicdata->M;
 292                 pmicdata->R ^= ROL32(pmicdata->L, 17);
 293                 pmicdata->L += pmicdata->R;
 294                 pmicdata->R ^= ((pmicdata->L & 0xff00ff00) >> 8) | ((pmicdata->L & 0x00ff00ff) << 8);
 295                 pmicdata->L += pmicdata->R;
 296                 pmicdata->R ^= ROL32(pmicdata->L, 3);
 297                 pmicdata->L += pmicdata->R;
 298                 pmicdata->R ^= ROR32(pmicdata->L, 2);
 299                 pmicdata->L += pmicdata->R;
 300                 /*  Clear the buffer */
 301                 pmicdata->M = 0;
 302                 pmicdata->nBytesInM = 0;
 303         }
 304 }
 305
 306 void rtw_secmicappend(struct mic_data *pmicdata, u8 *src, u32 nbytes)
 307 {
 308         /*  This is simple */
 309         while (nbytes > 0) {
 310                 rtw_secmicappendbyte(pmicdata, *src++);
 311                 nbytes--;
 312         }
 313 }
 314
 315 void rtw_secgetmic(struct mic_data *pmicdata, u8 *dst)
 316 {
 317         /*  Append the minimum padding */
 318         rtw_secmicappendbyte(pmicdata, 0x5a);
 319         rtw_secmicappendbyte(pmicdata, 0);
 320         rtw_secmicappendbyte(pmicdata, 0);
 321         rtw_secmicappendbyte(pmicdata, 0);
 322         rtw_secmicappendbyte(pmicdata, 0);
 323         /*  and then zeroes until the length is a multiple of 4 */
 324         while (pmicdata->nBytesInM != 0)
 325                 rtw_secmicappendbyte(pmicdata, 0);
 326         /*  The appendByte function has already computed the result. */
 327         secmicputuint32(dst, pmicdata->L);
 328         secmicputuint32(dst+4, pmicdata->R);
 329         /*  Reset to the empty message. */
 330         secmicclear(pmicdata);
 331 }
 332
 333 void rtw_seccalctkipmic(u8 *key, u8 *header, u8 *data, u32 data_len, u8 *mic_code, u8 pri)
 334 {
 335         struct mic_data micdata;
 336         u8 priority[4] = {0x0, 0x0, 0x0, 0x0};
 337
 338         rtw_secmicsetkey(&micdata, key);
 339         priority[0] = pri;
 340
 341         /* Michael MIC pseudo header: DA, SA, 3 x 0, Priority */
 342         if (header[1]&1) {   /* ToDS == 1 */
 343                         rtw_secmicappend(&micdata, &header[16], 6);  /* DA */
 344                 if (header[1]&2)  /* From Ds == 1 */
 345                         rtw_secmicappend(&micdata, &header[24], 6);
 346                 else
 347                         rtw_secmicappend(&micdata, &header[10], 6);
 348         } else {        /* ToDS == 0 */
 349                 rtw_secmicappend(&micdata, &header[4], 6);   /* DA */
 350                 if (header[1]&2)  /* From Ds == 1 */
 351                         rtw_secmicappend(&micdata, &header[16], 6);
 352                 else
 353                         rtw_secmicappend(&micdata, &header[10], 6);
 354         }
 355         rtw_secmicappend(&micdata, &priority[0], 4);
 356
 357         rtw_secmicappend(&micdata, data, data_len);
 358
 359         rtw_secgetmic(&micdata, mic_code);
 360 }
 361
 362
 363
 364 /* macros for extraction/creation of unsigned char/unsigned short values  */
 365 #define RotR1(v16)   ((((v16) >> 1) & 0x7FFF) ^ (((v16) & 1) << 15))
 366 #define   Lo8(v16)   ((u8)((v16)       & 0x00FF))
 367 #define   Hi8(v16)   ((u8)(((v16) >> 8) & 0x00FF))
 368 #define  Lo16(v32)   ((u16)((v32)       & 0xFFFF))
 369 #define  Hi16(v32)   ((u16)(((v32) >> 16) & 0xFFFF))
 370 #define  Mk16(hi, lo) ((lo) ^ (((u16)(hi)) << 8))
 371
 372 /* select the Nth 16-bit word of the temporal key unsigned char array TK[]   */
 373 #define  TK16(N)     Mk16(tk[2*(N)+1], tk[2*(N)])
 374
 375 /* S-box lookup: 16 bits --> 16 bits */
 376 #define _S_(v16)     (Sbox1[0][Lo8(v16)] ^ Sbox1[1][Hi8(v16)])
 377
 378 /* fixed algorithm "parameters" */
 379 #define PHASE1_LOOP_CNT   8    /* this needs to be "big enough"     */
 380 #define TA_SIZE    6    /*  48-bit transmitter address       */
 381 #define TK_SIZE   16    /* 128-bit temporal key       */
 382 #define P1K_SIZE         10    /*  80-bit Phase1 key            */
 383 #define RC4_KEY_SIZE     16    /* 128-bit RC4KEY (104 bits unknown) */
 384
 385 /* 2-unsigned char by 2-unsigned char subset of the full AES S-box table */
 386 static const unsigned short Sbox1[2][256] = {  /* Sbox for hash (can be in ROM)     */
 387 {
 388         0xC6A5, 0xF884, 0xEE99, 0xF68D, 0xFF0D, 0xD6BD, 0xDEB1, 0x9154,
 389         0x6050, 0x0203, 0xCEA9, 0x567D, 0xE719, 0xB562, 0x4DE6, 0xEC9A,
 390         0x8F45, 0x1F9D, 0x8940, 0xFA87, 0xEF15, 0xB2EB, 0x8EC9, 0xFB0B,
 391         0x41EC, 0xB367, 0x5FFD, 0x45EA, 0x23BF, 0x53F7, 0xE496, 0x9B5B,
 392         0x75C2, 0xE11C, 0x3DAE, 0x4C6A, 0x6C5A, 0x7E41, 0xF502, 0x834F,
 393         0x685C, 0x51F4, 0xD134, 0xF908, 0xE293, 0xAB73, 0x6253, 0x2A3F,
 394         0x080C, 0x9552, 0x4665, 0x9D5E, 0x3028, 0x37A1, 0x0A0F, 0x2FB5,
 395         0x0E09, 0x2436, 0x1B9B, 0xDF3D, 0xCD26, 0x4E69, 0x7FCD, 0xEA9F,
 396         0x121B, 0x1D9E, 0x5874, 0x342E, 0x362D, 0xDCB2, 0xB4EE, 0x5BFB,
 397         0xA4F6, 0x764D, 0xB761, 0x7DCE, 0x527B, 0xDD3E, 0x5E71, 0x1397,
 398         0xA6F5, 0xB968, 0x0000, 0xC12C, 0x4060, 0xE31F, 0x79C8, 0xB6ED,
 399         0xD4BE, 0x8D46, 0x67D9, 0x724B, 0x94DE, 0x98D4, 0xB0E8, 0x854A,
 400         0xBB6B, 0xC52A, 0x4FE5, 0xED16, 0x86C5, 0x9AD7, 0x6655, 0x1194,
 401         0x8ACF, 0xE910, 0x0406, 0xFE81, 0xA0F0, 0x7844, 0x25BA, 0x4BE3,
 402         0xA2F3, 0x5DFE, 0x80C0, 0x058A, 0x3FAD, 0x21BC, 0x7048, 0xF104,
 403         0x63DF, 0x77C1, 0xAF75, 0x4263, 0x2030, 0xE51A, 0xFD0E, 0xBF6D,
 404         0x814C, 0x1814, 0x2635, 0xC32F, 0xBEE1, 0x35A2, 0x88CC, 0x2E39,
 405         0x9357, 0x55F2, 0xFC82, 0x7A47, 0xC8AC, 0xBAE7, 0x322B, 0xE695,
 406         0xC0A0, 0x1998, 0x9ED1, 0xA37F, 0x4466, 0x547E, 0x3BAB, 0x0B83,
 407         0x8CCA, 0xC729, 0x6BD3, 0x283C, 0xA779, 0xBCE2, 0x161D, 0xAD76,
 408         0xDB3B, 0x6456, 0x744E, 0x141E, 0x92DB, 0x0C0A, 0x486C, 0xB8E4,
 409         0x9F5D, 0xBD6E, 0x43EF, 0xC4A6, 0x39A8, 0x31A4, 0xD337, 0xF28B,
 410         0xD532, 0x8B43, 0x6E59, 0xDAB7, 0x018C, 0xB164, 0x9CD2, 0x49E0,
 411         0xD8B4, 0xACFA, 0xF307, 0xCF25, 0xCAAF, 0xF48E, 0x47E9, 0x1018,
 412         0x6FD5, 0xF088, 0x4A6F, 0x5C72, 0x3824, 0x57F1, 0x73C7, 0x9751,
 413         0xCB23, 0xA17C, 0xE89C, 0x3E21, 0x96DD, 0x61DC, 0x0D86, 0x0F85,
 414         0xE090, 0x7C42, 0x71C4, 0xCCAA, 0x90D8, 0x0605, 0xF701, 0x1C12,
 415         0xC2A3, 0x6A5F, 0xAEF9, 0x69D0, 0x1791, 0x9958, 0x3A27, 0x27B9,
 416         0xD938, 0xEB13, 0x2BB3, 0x2233, 0xD2BB, 0xA970, 0x0789, 0x33A7,
 417         0x2DB6, 0x3C22, 0x1592, 0xC920, 0x8749, 0xAAFF, 0x5078, 0xA57A,
 418         0x038F, 0x59F8, 0x0980, 0x1A17, 0x65DA, 0xD731, 0x84C6, 0xD0B8,
 419         0x82C3, 0x29B0, 0x5A77, 0x1E11, 0x7BCB, 0xA8FC, 0x6DD6, 0x2C3A,
 420   },
 421
 422   {  /* second half of table is unsigned char-reversed version of first! */
 423         0xA5C6, 0x84F8, 0x99EE, 0x8DF6, 0x0DFF, 0xBDD6, 0xB1DE, 0x5491,
 424         0x5060, 0x0302, 0xA9CE, 0x7D56, 0x19E7, 0x62B5, 0xE64D, 0x9AEC,
 425         0x458F, 0x9D1F, 0x4089, 0x87FA, 0x15EF, 0xEBB2, 0xC98E, 0x0BFB,
 426         0xEC41, 0x67B3, 0xFD5F, 0xEA45, 0xBF23, 0xF753, 0x96E4, 0x5B9B,
 427         0xC275, 0x1CE1, 0xAE3D, 0x6A4C, 0x5A6C, 0x417E, 0x02F5, 0x4F83,
 428         0x5C68, 0xF451, 0x34D1, 0x08F9, 0x93E2, 0x73AB, 0x5362, 0x3F2A,
 429         0x0C08, 0x5295, 0x6546, 0x5E9D, 0x2830, 0xA137, 0x0F0A, 0xB52F,
 430         0x090E, 0x3624, 0x9B1B, 0x3DDF, 0x26CD, 0x694E, 0xCD7F, 0x9FEA,
 431         0x1B12, 0x9E1D, 0x7458, 0x2E34, 0x2D36, 0xB2DC, 0xEEB4, 0xFB5B,
 432         0xF6A4, 0x4D76, 0x61B7, 0xCE7D, 0x7B52, 0x3EDD, 0x715E, 0x9713,
 433         0xF5A6, 0x68B9, 0x0000, 0x2CC1, 0x6040, 0x1FE3, 0xC879, 0xEDB6,
 434         0xBED4, 0x468D, 0xD967, 0x4B72, 0xDE94, 0xD498, 0xE8B0, 0x4A85,
 435         0x6BBB, 0x2AC5, 0xE54F, 0x16ED, 0xC586, 0xD79A, 0x5566, 0x9411,
 436         0xCF8A, 0x10E9, 0x0604, 0x81FE, 0xF0A0, 0x4478, 0xBA25, 0xE34B,
 437         0xF3A2, 0xFE5D, 0xC080, 0x8A05, 0xAD3F, 0xBC21, 0x4870, 0x04F1,
 438         0xDF63, 0xC177, 0x75AF, 0x6342, 0x3020, 0x1AE5, 0x0EFD, 0x6DBF,
 439         0x4C81, 0x1418, 0x3526, 0x2FC3, 0xE1BE, 0xA235, 0xCC88, 0x392E,
 440         0x5793, 0xF255, 0x82FC, 0x477A, 0xACC8, 0xE7BA, 0x2B32, 0x95E6,
 441         0xA0C0, 0x9819, 0xD19E, 0x7FA3, 0x6644, 0x7E54, 0xAB3B, 0x830B,
 442         0xCA8C, 0x29C7, 0xD36B, 0x3C28, 0x79A7, 0xE2BC, 0x1D16, 0x76AD,
 443         0x3BDB, 0x5664, 0x4E74, 0x1E14, 0xDB92, 0x0A0C, 0x6C48, 0xE4B8,
 444         0x5D9F, 0x6EBD, 0xEF43, 0xA6C4, 0xA839, 0xA431, 0x37D3, 0x8BF2,
 445         0x32D5, 0x438B, 0x596E, 0xB7DA, 0x8C01, 0x64B1, 0xD29C, 0xE049,
 446         0xB4D8, 0xFAAC, 0x07F3, 0x25CF, 0xAFCA, 0x8EF4, 0xE947, 0x1810,
 447         0xD56F, 0x88F0, 0x6F4A, 0x725C, 0x2438, 0xF157, 0xC773, 0x5197,
 448         0x23CB, 0x7CA1, 0x9CE8, 0x213E, 0xDD96, 0xDC61, 0x860D, 0x850F,
 449         0x90E0, 0x427C, 0xC471, 0xAACC, 0xD890, 0x0506, 0x01F7, 0x121C,
 450         0xA3C2, 0x5F6A, 0xF9AE, 0xD069, 0x9117, 0x5899, 0x273A, 0xB927,
 451         0x38D9, 0x13EB, 0xB32B, 0x3322, 0xBBD2, 0x70A9, 0x8907, 0xA733,
 452         0xB62D, 0x223C, 0x9215, 0x20C9, 0x4987, 0xFFAA, 0x7850, 0x7AA5,
 453         0x8F03, 0xF859, 0x8009, 0x171A, 0xDA65, 0x31D7, 0xC684, 0xB8D0,
 454         0xC382, 0xB029, 0x775A, 0x111E, 0xCB7B, 0xFCA8, 0xD66D, 0x3A2C,
 455   }
 456 };
 457
 458  /*
 459 **********************************************************************
 460 * Routine: Phase 1 -- generate P1K, given TA, TK, IV32
 461 *
 462 * Inputs:
 463 *     tk[]      = temporal key                   [128 bits]
 464 *     ta[]      = transmitter's MAC address         [ 48 bits]
 465 *     iv32      = upper 32 bits of IV             [ 32 bits]
 466 * Output:
 467 *     p1k[]     = Phase 1 key                     [ 80 bits]
 468 *
 469 * Note:
 470 *     This function only needs to be called every 2**16 packets,
 471 *     although in theory it could be called every packet.
 472 *
 473 **********************************************************************
 474 */
 475 static void phase1(u16 *p1k, const u8 *tk, const u8 *ta, u32 iv32)
 476 {
 477         int  i;
 478         /* Initialize the 80 bits of P1K[] from IV32 and TA[0..5]     */
 479         p1k[0]      = Lo16(iv32);
 480         p1k[1]      = Hi16(iv32);
 481         p1k[2]      = Mk16(ta[1], ta[0]); /* use TA[] as little-endian */
 482         p1k[3]      = Mk16(ta[3], ta[2]);
 483         p1k[4]      = Mk16(ta[5], ta[4]);
 484
 485         /* Now compute an unbalanced Feistel cipher with 80-bit block */
 486         /* size on the 80-bit block P1K[], using the 128-bit key TK[] */
 487         for (i = 0; i < PHASE1_LOOP_CNT; i++) { /* Each add operation here is mod 2**16 */
 488                 p1k[0] += _S_(p1k[4] ^ TK16((i&1)+0));
 489                 p1k[1] += _S_(p1k[0] ^ TK16((i&1)+2));
 490                 p1k[2] += _S_(p1k[1] ^ TK16((i&1)+4));
 491                 p1k[3] += _S_(p1k[2] ^ TK16((i&1)+6));
 492                 p1k[4] += _S_(p1k[3] ^ TK16((i&1)+0));
 493                 p1k[4] +=  (unsigned short)i;   /* avoid "slide attacks" */
 494         }
 495 }
 496
 497 /*
 498 **********************************************************************
 499 * Routine: Phase 2 -- generate RC4KEY, given TK, P1K, IV16
 500 *
 501 * Inputs:
 502 *     tk[]      = Temporal key                   [128 bits]
 503 *     p1k[]     = Phase 1 output key               [ 80 bits]
 504 *     iv16      = low 16 bits of IV counter         [ 16 bits]
 505 * Output:
 506 *     rc4key[]  = the key used to encrypt the packet   [128 bits]
 507 *
 508 * Note:
 509 *     The value {TA, IV32, IV16} for Phase1/Phase2 must be unique
 510 *     across all packets using the same key TK value. Then, for a
 511 *     given value of TK[], this TKIP48 construction guarantees that
 512 *     the final RC4KEY value is unique across all packets.
 513 *
 514 * Suggested implementation optimization: if PPK[] is "overlaid"
 515 *     appropriately on RC4KEY[], there is no need for the final
 516 *     for loop below that copies the PPK[] result into RC4KEY[].
 517 *
 518 **********************************************************************
 519 */
 520 static void phase2(u8 *rc4key, const u8 *tk, const u16 *p1k, u16 iv16)
 521 {
 522         int  i;
 523         u16 PPK[6];                     /* temporary key for mixing    */
 524         /* Note: all adds in the PPK[] equations below are mod 2**16     */
 525         for (i = 0; i < 5; i++)
 526                 PPK[i] = p1k[i];        /* first, copy P1K to PPK      */
 527         PPK[5]  =  p1k[4] + iv16;       /* next,  add in IV16     */
 528
 529         /* Bijective non-linear mixing of the 96 bits of PPK[0..5]         */
 530         PPK[0] +=    _S_(PPK[5] ^ TK16(0));   /* Mix key in each "round"     */
 531         PPK[1] +=    _S_(PPK[0] ^ TK16(1));
 532         PPK[2] +=    _S_(PPK[1] ^ TK16(2));
 533         PPK[3] +=    _S_(PPK[2] ^ TK16(3));
 534         PPK[4] +=    _S_(PPK[3] ^ TK16(4));
 535         PPK[5] +=    _S_(PPK[4] ^ TK16(5));   /* Total # S-box lookups == 6  */
 536
 537         /* Final sweep: bijective, "linear". Rotates kill LSB correlations   */
 538         PPK[0] +=  RotR1(PPK[5] ^ TK16(6));
 539         PPK[1] +=  RotR1(PPK[0] ^ TK16(7));   /* Use all of TK[] in Phase2   */
 540         PPK[2] +=  RotR1(PPK[1]);
 541         PPK[3] +=  RotR1(PPK[2]);
 542         PPK[4] +=  RotR1(PPK[3]);
 543         PPK[5] +=  RotR1(PPK[4]);
 544         /* Note: At this point, for a given key TK[0..15], the 96-bit output */
 545         /*       value PPK[0..5] is guaranteed to be unique, as a function   */
 546         /*       of the 96-bit "input" value   {TA, IV32, IV16}. That is, P1K  */
 547         /*       is now a keyed permutation of {TA, IV32, IV16}.               */
 548
 549         /* Set RC4KEY[0..3], which includes "cleartext" portion of RC4 key   */
 550         rc4key[0] = Hi8(iv16);          /* RC4KEY[0..2] is the WEP IV  */
 551         rc4key[1] = (Hi8(iv16) | 0x20) & 0x7F; /* Help avoid weak (FMS) keys  */
 552         rc4key[2] = Lo8(iv16);
 553         rc4key[3] = Lo8((PPK[5] ^ TK16(0)) >> 1);
 554
 555         /* Copy 96 bits of PPK[0..5] to RC4KEY[4..15]  (little-endian)       */
 556         for (i = 0; i < 6; i++) {
 557                 rc4key[4+2*i] = Lo8(PPK[i]);
 558                 rc4key[5+2*i] = Hi8(PPK[i]);
 559         }
 560 }
 561
 562 /* The hlen isn't include the IV */
 563 u32     rtw_tkip_encrypt(struct adapter *padapter, u8 *pxmitframe)
 564 {                                                                                                                                       /*  exclude ICV */
 565         u16     pnl;
 566         u32     pnh;
 567         u8      rc4key[16];
 568         u8   ttkey[16];
 569         u8      crc[4];
 570         u8   hw_hdr_offset = 0;
 571         struct arc4context mycontext;
 572         int                     curfragnum, length;
 573
 574         u8      *pframe, *payload, *iv, *prwskey;
 575         union pn48 dot11txpn;
 576         struct  sta_info                *stainfo;
 577         struct  pkt_attrib       *pattrib = &((struct xmit_frame *)pxmitframe)->attrib;
 578         struct  security_priv   *psecuritypriv = &padapter->securitypriv;
 579         struct  xmit_priv               *pxmitpriv = &padapter->xmitpriv;
 580         u32     res = _SUCCESS;
 581
 582         if (((struct xmit_frame *)pxmitframe)->buf_addr == NULL)
 583                 return _FAIL;
 584
 585         hw_hdr_offset = TXDESC_SIZE +
 586                  (((struct xmit_frame *)pxmitframe)->pkt_offset * PACKET_OFFSET_SZ);
 587         pframe = ((struct xmit_frame *)pxmitframe)->buf_addr + hw_hdr_offset;
 588         /* 4 start to encrypt each fragment */
 589         if (pattrib->encrypt == _TKIP_) {
 590                 if (pattrib->psta)
 591                         stainfo = pattrib->psta;
 592                 else
 593                         stainfo = rtw_get_stainfo(&padapter->stapriv, &pattrib->ra[0]);
 594
 595                 if (stainfo != NULL) {
 596                         RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_tkip_encrypt: stainfo!= NULL!!!\n"));
 597
 598                         if (IS_MCAST(pattrib->ra))
 599                                 prwskey = psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey;
 600                         else
 601                                 prwskey = &stainfo->dot118021x_UncstKey.skey[0];
 602
 603                         for (curfragnum = 0; curfragnum < pattrib->nr_frags; curfragnum++) {
 604                                 iv = pframe+pattrib->hdrlen;
 605                                 payload = pframe+pattrib->iv_len+pattrib->hdrlen;
 606
 607                                 GET_TKIP_PN(iv, dot11txpn);
 608
 609                                 pnl = (u16)(dot11txpn.val);
 610                                 pnh = (u32)(dot11txpn.val>>16);
 611                                 phase1((u16 *)&ttkey[0], prwskey, &pattrib->ta[0], pnh);
 612                                 phase2(&rc4key[0], prwskey, (u16 *)&ttkey[0], pnl);
 613
 614                                 if ((curfragnum+1) == pattrib->nr_frags) {      /* 4 the last fragment */
 615                                         length = pattrib->last_txcmdsz-pattrib->hdrlen-pattrib->iv_len-pattrib->icv_len;
 616                                         RT_TRACE(_module_rtl871x_security_c_, _drv_info_,
 617                                                  ("pattrib->iv_len=%x, pattrib->icv_len=%x\n",
 618                                                  pattrib->iv_len, pattrib->icv_len));
 619                                         *((__le32 *)crc) = getcrc32(payload, length);/* modified by Amy*/
 620
 621                                         arcfour_init(&mycontext, rc4key, 16);
 622                                         arcfour_encrypt(&mycontext, payload, payload, length);
 623                                         arcfour_encrypt(&mycontext, payload+length, crc, 4);
 624                                 } else {
 625                                         length = pxmitpriv->frag_len-pattrib->hdrlen-pattrib->iv_len-pattrib->icv_len;
 626                                         *((__le32 *)crc) = getcrc32(payload, length);/* modified by Amy*/
 627                                         arcfour_init(&mycontext, rc4key, 16);
 628                                         arcfour_encrypt(&mycontext, payload, payload, length);
 629                                         arcfour_encrypt(&mycontext, payload+length, crc, 4);
 630
 631                                         pframe += pxmitpriv->frag_len;
 632                                         pframe = (u8 *)round_up((size_t)(pframe), 4);
 633                                 }
 634                         }
 635                 } else {
 636                         RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_tkip_encrypt: stainfo==NULL!!!\n"));
 637                         res = _FAIL;
 638                 }
 639         }
 640         return res;
 641 }
 642
 643 /* The hlen isn't include the IV */
 644 u32 rtw_tkip_decrypt(struct adapter *padapter, u8 *precvframe)
 645 {                                                                                                                                       /*  exclude ICV */
 646         u16 pnl;
 647         u32 pnh;
 648         u8   rc4key[16];
 649         u8   ttkey[16];
 650         u8      crc[4];
 651         struct arc4context mycontext;
 652         int                     length;
 653
 654         u8      *pframe, *payload, *iv, *prwskey;
 655         union pn48 dot11txpn;
 656         struct  sta_info                *stainfo;
 657         struct  rx_pkt_attrib    *prxattrib = &((struct recv_frame *)precvframe)->attrib;
 658         struct  security_priv   *psecuritypriv = &padapter->securitypriv;
 659         u32             res = _SUCCESS;
 660
 661
 662         pframe = (unsigned char *)((struct recv_frame *)precvframe)->pkt->data;
 663
 664         /* 4 start to decrypt recvframe */
 665         if (prxattrib->encrypt == _TKIP_) {
 666                 stainfo = rtw_get_stainfo(&padapter->stapriv, &prxattrib->ta[0]);
 667                 if (stainfo != NULL) {
 668                         if (IS_MCAST(prxattrib->ra)) {
 669                                 if (!psecuritypriv->binstallGrpkey) {
 670                                         res = _FAIL;
 671                                         DBG_88E("%s:rx bc/mc packets, but didn't install group key!!!!!!!!!!\n", __func__);
 672                                         goto exit;
 673                                 }
 674                                 prwskey = psecuritypriv->dot118021XGrpKey[prxattrib->key_index].skey;
 675                         } else {
 676                                 RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_tkip_decrypt: stainfo!= NULL!!!\n"));
 677                                 prwskey = &stainfo->dot118021x_UncstKey.skey[0];
 678                         }
 679
 680                         iv = pframe+prxattrib->hdrlen;
 681                         payload = pframe+prxattrib->iv_len+prxattrib->hdrlen;
 682                         length = ((struct recv_frame *)precvframe)->pkt->len-prxattrib->hdrlen-prxattrib->iv_len;
 683
 684                         GET_TKIP_PN(iv, dot11txpn);
 685
 686                         pnl = (u16)(dot11txpn.val);
 687                         pnh = (u32)(dot11txpn.val>>16);
 688
 689                         phase1((u16 *)&ttkey[0], prwskey, &prxattrib->ta[0], pnh);
 690                         phase2(&rc4key[0], prwskey, (unsigned short *)&ttkey[0], pnl);
 691
 692                         /* 4 decrypt payload include icv */
 693
 694                         arcfour_init(&mycontext, rc4key, 16);
 695                         arcfour_encrypt(&mycontext, payload, payload, length);
 696
 697                         *((__le32 *)crc) = getcrc32(payload, length-4);
 698
 699                         if (crc[3] != payload[length-1] ||
 700                             crc[2] != payload[length-2] ||
 701                             crc[1] != payload[length-3] ||
 702                             crc[0] != payload[length-4]) {
 703                                 RT_TRACE(_module_rtl871x_security_c_, _drv_err_,
 704                                          ("rtw_wep_decrypt:icv error crc (%4ph)!=payload (%4ph)\n",
 705                                          &crc, &payload[length-4]));
 706                                 res = _FAIL;
 707                         }
 708                 } else {
 709                         RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_tkip_decrypt: stainfo==NULL!!!\n"));
 710                         res = _FAIL;
 711                 }
 712         }
 713 exit:
 714         return res;
 715 }
 716
 717 /* 3                    ===== AES related ===== */
 718
 719
 720 #define MAX_MSG_SIZE    2048
 721 /*****************************/
 722 /******** SBOX Table *********/
 723 /*****************************/
 724
 725 static  u8 sbox_table[256] = {
 726         0x63, 0x7c, 0x77, 0x7b, 0xf2, 0x6b, 0x6f, 0xc5,
 727         0x30, 0x01, 0x67, 0x2b, 0xfe, 0xd7, 0xab, 0x76,
 728         0xca, 0x82, 0xc9, 0x7d, 0xfa, 0x59, 0x47, 0xf0,
 729         0xad, 0xd4, 0xa2, 0xaf, 0x9c, 0xa4, 0x72, 0xc0,
 730         0xb7, 0xfd, 0x93, 0x26, 0x36, 0x3f, 0xf7, 0xcc,
 731         0x34, 0xa5, 0xe5, 0xf1, 0x71, 0xd8, 0x31, 0x15,
 732         0x04, 0xc7, 0x23, 0xc3, 0x18, 0x96, 0x05, 0x9a,
 733         0x07, 0x12, 0x80, 0xe2, 0xeb, 0x27, 0xb2, 0x75,
 734         0x09, 0x83, 0x2c, 0x1a, 0x1b, 0x6e, 0x5a, 0xa0,
 735         0x52, 0x3b, 0xd6, 0xb3, 0x29, 0xe3, 0x2f, 0x84,
 736         0x53, 0xd1, 0x00, 0xed, 0x20, 0xfc, 0xb1, 0x5b,
 737         0x6a, 0xcb, 0xbe, 0x39, 0x4a, 0x4c, 0x58, 0xcf,
 738         0xd0, 0xef, 0xaa, 0xfb, 0x43, 0x4d, 0x33, 0x85,
 739         0x45, 0xf9, 0x02, 0x7f, 0x50, 0x3c, 0x9f, 0xa8,
 740         0x51, 0xa3, 0x40, 0x8f, 0x92, 0x9d, 0x38, 0xf5,
 741         0xbc, 0xb6, 0xda, 0x21, 0x10, 0xff, 0xf3, 0xd2,
 742         0xcd, 0x0c, 0x13, 0xec, 0x5f, 0x97, 0x44, 0x17,
 743         0xc4, 0xa7, 0x7e, 0x3d, 0x64, 0x5d, 0x19, 0x73,
 744         0x60, 0x81, 0x4f, 0xdc, 0x22, 0x2a, 0x90, 0x88,
 745         0x46, 0xee, 0xb8, 0x14, 0xde, 0x5e, 0x0b, 0xdb,
 746         0xe0, 0x32, 0x3a, 0x0a, 0x49, 0x06, 0x24, 0x5c,
 747         0xc2, 0xd3, 0xac, 0x62, 0x91, 0x95, 0xe4, 0x79,
 748         0xe7, 0xc8, 0x37, 0x6d, 0x8d, 0xd5, 0x4e, 0xa9,
 749         0x6c, 0x56, 0xf4, 0xea, 0x65, 0x7a, 0xae, 0x08,
 750         0xba, 0x78, 0x25, 0x2e, 0x1c, 0xa6, 0xb4, 0xc6,
 751         0xe8, 0xdd, 0x74, 0x1f, 0x4b, 0xbd, 0x8b, 0x8a,
 752         0x70, 0x3e, 0xb5, 0x66, 0x48, 0x03, 0xf6, 0x0e,
 753         0x61, 0x35, 0x57, 0xb9, 0x86, 0xc1, 0x1d, 0x9e,
 754         0xe1, 0xf8, 0x98, 0x11, 0x69, 0xd9, 0x8e, 0x94,
 755         0x9b, 0x1e, 0x87, 0xe9, 0xce, 0x55, 0x28, 0xdf,
 756         0x8c, 0xa1, 0x89, 0x0d, 0xbf, 0xe6, 0x42, 0x68,
 757         0x41, 0x99, 0x2d, 0x0f, 0xb0, 0x54, 0xbb, 0x16
 758 };
 759
 760 /*****************************/
 761 /**** Function Prototypes ****/
 762 /*****************************/
 763
 764 static void bitwise_xor(u8 *ina, u8 *inb, u8 *out);
 765 static void construct_mic_iv(u8 *mic_header1, int qc_exists, int a4_exists, u8 *mpdu, uint payload_length, u8 *pn_vector);
 766 static void construct_mic_header1(u8 *mic_header1, int header_length, u8 *mpdu);
 767 static void construct_mic_header2(u8 *mic_header2, u8 *mpdu, int a4_exists, int qc_exists);
 768 static void construct_ctr_preload(u8 *ctr_preload, int a4_exists, int qc_exists, u8 *mpdu, u8 *pn_vector, int c);
 769 static void xor_128(u8 *a, u8 *b, u8 *out);
 770 static void xor_32(u8 *a, u8 *b, u8 *out);
 771 static u8 sbox(u8 a);
 772 static void next_key(u8 *key, int round);
 773 static void byte_sub(u8 *in, u8 *out);
 774 static void shift_row(u8 *in, u8 *out);
 775 static void mix_column(u8 *in, u8 *out);
 776 static void aes128k128d(u8 *key, u8 *data, u8 *ciphertext);
 777
 778 /****************************************/
 779 /* aes128k128d()                        */
 780 /* Performs a 128 bit AES encrypt with  */
 781 /* 128 bit data.                        */
 782 /****************************************/
 783 static void xor_128(u8 *a, u8 *b, u8 *out)
 784 {
 785         int i;
 786
 787         for (i = 0; i < 16; i++)
 788                 out[i] = a[i] ^ b[i];
 789 }
 790
 791 static void xor_32(u8 *a, u8 *b, u8 *out)
 792 {
 793         int i;
 794
 795         for (i = 0; i < 4; i++)
 796                 out[i] = a[i] ^ b[i];
 797 }
 798
 799 static u8 sbox(u8 a)
 800 {
 801         return sbox_table[(int)a];
 802 }
 803
 804 static void next_key(u8 *key, int round)
 805 {
 806         u8 rcon;
 807         u8 sbox_key[4];
 808         u8 rcon_table[12] = {
 809                 0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80,
 810                 0x1b, 0x36, 0x36, 0x36
 811         };
 812
 813         sbox_key[0] = sbox(key[13]);
 814         sbox_key[1] = sbox(key[14]);
 815         sbox_key[2] = sbox(key[15]);
 816         sbox_key[3] = sbox(key[12]);
 817
 818         rcon = rcon_table[round];
 819
 820         xor_32(&key[0], sbox_key, &key[0]);
 821         key[0] = key[0] ^ rcon;
 822
 823         xor_32(&key[4], &key[0], &key[4]);
 824         xor_32(&key[8], &key[4], &key[8]);
 825         xor_32(&key[12], &key[8], &key[12]);
 826 }
 827
 828 static void byte_sub(u8 *in, u8 *out)
 829 {
 830         int i;
 831         for (i = 0; i < 16; i++)
 832                 out[i] = sbox(in[i]);
 833 }
 834
 835 static void shift_row(u8 *in, u8 *out)
 836 {
 837         out[0] =  in[0];
 838         out[1] =  in[5];
 839         out[2] =  in[10];
 840         out[3] =  in[15];
 841         out[4] =  in[4];
 842         out[5] =  in[9];
 843         out[6] =  in[14];
 844         out[7] =  in[3];
 845         out[8] =  in[8];
 846         out[9] =  in[13];
 847         out[10] = in[2];
 848         out[11] = in[7];
 849         out[12] = in[12];
 850         out[13] = in[1];
 851         out[14] = in[6];
 852         out[15] = in[11];
 853 }
 854
 855 static void mix_column(u8 *in, u8 *out)
 856 {
 857         int i;
 858         u8 add1b[4];
 859         u8 add1bf7[4];
 860         u8 rotl[4];
 861         u8 swap_halves[4];
 862         u8 andf7[4];
 863         u8 rotr[4];
 864         u8 temp[4];
 865         u8 tempb[4];
 866
 867         for (i = 0 ; i < 4; i++) {
 868                 if ((in[i] & 0x80) == 0x80)
 869                         add1b[i] = 0x1b;
 870                 else
 871                         add1b[i] = 0x00;
 872         }
 873
 874         swap_halves[0] = in[2];    /* Swap halves */
 875         swap_halves[1] = in[3];
 876         swap_halves[2] = in[0];
 877         swap_halves[3] = in[1];
 878
 879         rotl[0] = in[3];        /* Rotate left 8 bits */
 880         rotl[1] = in[0];
 881         rotl[2] = in[1];
 882         rotl[3] = in[2];
 883
 884         andf7[0] = in[0] & 0x7f;
 885         andf7[1] = in[1] & 0x7f;
 886         andf7[2] = in[2] & 0x7f;
 887         andf7[3] = in[3] & 0x7f;
 888
 889         for (i = 3; i > 0; i--) {    /* logical shift left 1 bit */
 890                 andf7[i] = andf7[i] << 1;
 891                 if ((andf7[i-1] & 0x80) == 0x80)
 892                         andf7[i] = (andf7[i] | 0x01);
 893         }
 894         andf7[0] = andf7[0] << 1;
 895         andf7[0] = andf7[0] & 0xfe;
 896
 897         xor_32(add1b, andf7, add1bf7);
 898
 899         xor_32(in, add1bf7, rotr);
 900
 901         temp[0] = rotr[0];       /* Rotate right 8 bits */
 902         rotr[0] = rotr[1];
 903         rotr[1] = rotr[2];
 904         rotr[2] = rotr[3];
 905         rotr[3] = temp[0];
 906
 907         xor_32(add1bf7, rotr, temp);
 908         xor_32(swap_halves, rotl, tempb);
 909         xor_32(temp, tempb, out);
 910 }
 911
 912 static void aes128k128d(u8 *key, u8 *data, u8 *ciphertext)
 913 {
 914         int round;
 915         int i;
 916         u8 intermediatea[16];
 917         u8 intermediateb[16];
 918         u8 round_key[16];
 919
 920         for (i = 0; i < 16; i++)
 921                 round_key[i] = key[i];
 922         for (round = 0; round < 11; round++) {
 923                 if (round == 0) {
 924                         xor_128(round_key, data, ciphertext);
 925                         next_key(round_key, round);
 926                 } else if (round == 10) {
 927                         byte_sub(ciphertext, intermediatea);
 928                         shift_row(intermediatea, intermediateb);
 929                         xor_128(intermediateb, round_key, ciphertext);
 930                 } else {    /* 1 - 9 */
 931                         byte_sub(ciphertext, intermediatea);
 932                         shift_row(intermediatea, intermediateb);
 933                         mix_column(&intermediateb[0], &intermediatea[0]);
 934                         mix_column(&intermediateb[4], &intermediatea[4]);
 935                         mix_column(&intermediateb[8], &intermediatea[8]);
 936                         mix_column(&intermediateb[12], &intermediatea[12]);
 937                         xor_128(intermediatea, round_key, ciphertext);
 938                         next_key(round_key, round);
 939                 }
 940         }
 941 }
 942
 943 /************************************************/
 944 /* construct_mic_iv()                      */
 945 /* Builds the MIC IV from header fields and PN  */
 946 /************************************************/
 947 static void construct_mic_iv(u8 *mic_iv, int qc_exists, int a4_exists, u8 *mpdu,
 948                              uint payload_length, u8 *pn_vector)
 949 {
 950         int i;
 951
 952         mic_iv[0] = 0x59;
 953         if (qc_exists && a4_exists)
 954                 mic_iv[1] = mpdu[30] & 0x0f;    /* QoS_TC          */
 955         if (qc_exists && !a4_exists)
 956                 mic_iv[1] = mpdu[24] & 0x0f;    /* mute bits 7-4    */
 957         if (!qc_exists)
 958                 mic_iv[1] = 0x00;
 959         for (i = 2; i < 8; i++)
 960                 mic_iv[i] = mpdu[i + 8];        /* mic_iv[2:7] = A2[0:5] = mpdu[10:15] */
 961         for (i = 8; i < 14; i++)
 962                 mic_iv[i] = pn_vector[13 - i];  /* mic_iv[8:13] = PN[5:0] */
 963         mic_iv[14] = (unsigned char)(payload_length / 256);
 964         mic_iv[15] = (unsigned char)(payload_length % 256);
 965 }
 966
 967 /************************************************/
 968 /* construct_mic_header1()                    */
 969 /* Builds the first MIC header block from       */
 970 /* header fields.                              */
 971 /************************************************/
 972 static void construct_mic_header1(u8 *mic_header1, int header_length, u8 *mpdu)
 973 {
 974         mic_header1[0] = (u8)((header_length - 2) / 256);
 975         mic_header1[1] = (u8)((header_length - 2) % 256);
 976         mic_header1[2] = mpdu[0] & 0xcf;    /* Mute CF poll & CF ack bits */
 977         mic_header1[3] = mpdu[1] & 0xc7;    /* Mute retry, more data and pwr mgt bits */
 978         mic_header1[4] = mpdu[4];       /* A1 */
 979         mic_header1[5] = mpdu[5];
 980         mic_header1[6] = mpdu[6];
 981         mic_header1[7] = mpdu[7];
 982         mic_header1[8] = mpdu[8];
 983         mic_header1[9] = mpdu[9];
 984         mic_header1[10] = mpdu[10];     /* A2 */
 985         mic_header1[11] = mpdu[11];
 986         mic_header1[12] = mpdu[12];
 987         mic_header1[13] = mpdu[13];
 988         mic_header1[14] = mpdu[14];
 989         mic_header1[15] = mpdu[15];
 990 }
 991
 992 /************************************************/
 993 /* construct_mic_header2()                    */
 994 /* Builds the last MIC header block from        */
 995 /* header fields.                              */
 996 /************************************************/
 997 static void construct_mic_header2(u8 *mic_header2, u8 *mpdu, int a4_exists, int qc_exists)
 998 {
 999         int i;
1000
1001         for (i = 0; i < 16; i++)
1002                 mic_header2[i] = 0x00;
1003
1004         mic_header2[0] = mpdu[16];    /* A3 */
1005         mic_header2[1] = mpdu[17];
1006         mic_header2[2] = mpdu[18];
1007         mic_header2[3] = mpdu[19];
1008         mic_header2[4] = mpdu[20];
1009         mic_header2[5] = mpdu[21];
1010
1011         mic_header2[6] = 0x00;
1012         mic_header2[7] = 0x00; /* mpdu[23]; */
1013
1014         if (!qc_exists && a4_exists) {
1015                 for (i = 0; i < 6; i++)
1016                         mic_header2[8+i] = mpdu[24+i];   /* A4 */
1017         }
1018
1019         if (qc_exists && !a4_exists) {
1020                 mic_header2[8] = mpdu[24] & 0x0f; /* mute bits 15 - 4 */
1021                 mic_header2[9] = mpdu[25] & 0x00;
1022         }
1023
1024         if (qc_exists && a4_exists) {
1025                 for (i = 0; i < 6; i++)
1026                         mic_header2[8+i] = mpdu[24+i];   /* A4 */
1027
1028                 mic_header2[14] = mpdu[30] & 0x0f;
1029                 mic_header2[15] = mpdu[31] & 0x00;
1030         }
1031
1032 }
1033
1034 /************************************************/
1035 /* construct_mic_header2()                    */
1036 /* Builds the last MIC header block from        */
1037 /* header fields.                              */
1038 /************************************************/
1039 static void construct_ctr_preload(u8 *ctr_preload, int a4_exists, int qc_exists, u8 *mpdu, u8 *pn_vector, int c)
1040 {
1041         int i;
1042
1043         for (i = 0; i < 16; i++)
1044                 ctr_preload[i] = 0x00;
1045         i = 0;
1046
1047         ctr_preload[0] = 0x01;                            /* flag */
1048         if (qc_exists && a4_exists)
1049                 ctr_preload[1] = mpdu[30] & 0x0f;   /* QoC_Control */
1050         if (qc_exists && !a4_exists)
1051                 ctr_preload[1] = mpdu[24] & 0x0f;
1052
1053         for (i = 2; i < 8; i++)
1054                 ctr_preload[i] = mpdu[i + 8];                  /* ctr_preload[2:7] = A2[0:5] = mpdu[10:15] */
1055         for (i = 8; i < 14; i++)
1056                 ctr_preload[i] =    pn_vector[13 - i];    /* ctr_preload[8:13] = PN[5:0] */
1057         ctr_preload[14] =  (unsigned char)(c / 256); /* Ctr */
1058         ctr_preload[15] =  (unsigned char)(c % 256);
1059 }
1060
1061 /************************************/
1062 /* bitwise_xor()                    */
1063 /* A 128 bit, bitwise exclusive or  */
1064 /************************************/
1065 static void bitwise_xor(u8 *ina, u8 *inb, u8 *out)
1066 {
1067         int i;
1068
1069         for (i = 0; i < 16; i++)
1070                 out[i] = ina[i] ^ inb[i];
1071 }
1072
1073 static int aes_cipher(u8 *key, uint hdrlen, u8 *pframe, uint plen)
1074 {
1075         uint    qc_exists, a4_exists, i, j, payload_remainder,
1076                 num_blocks, payload_index;
1077
1078         u8 pn_vector[6];
1079         u8 mic_iv[16];
1080         u8 mic_header1[16];
1081         u8 mic_header2[16];
1082         u8 ctr_preload[16];
1083
1084         /* Intermediate Buffers */
1085         u8 chain_buffer[16];
1086         u8 aes_out[16];
1087         u8 padded_buffer[16];
1088         u8 mic[8];
1089         uint    frtype  = GetFrameType(pframe);
1090         uint    frsubtype  = GetFrameSubType(pframe);
1091
1092         frsubtype >>= 4;
1093
1094         memset(mic_iv, 0, 16);
1095         memset(mic_header1, 0, 16);
1096         memset(mic_header2, 0, 16);
1097         memset(ctr_preload, 0, 16);
1098         memset(chain_buffer, 0, 16);
1099         memset(aes_out, 0, 16);
1100         memset(padded_buffer, 0, 16);
1101
1102         if ((hdrlen == WLAN_HDR_A3_LEN) || (hdrlen ==  WLAN_HDR_A3_QOS_LEN))
1103                 a4_exists = 0;
1104         else
1105                 a4_exists = 1;
1106
1107         if ((frtype == WIFI_DATA_CFACK) || (frtype == WIFI_DATA_CFPOLL) || (frtype == WIFI_DATA_CFACKPOLL)) {
1108                 qc_exists = 1;
1109                 if (hdrlen !=  WLAN_HDR_A3_QOS_LEN)
1110                         hdrlen += 2;
1111         } else if ((frsubtype == 0x08) || (frsubtype == 0x09) || (frsubtype == 0x0a) || (frsubtype == 0x0b)) {
1112                 if (hdrlen !=  WLAN_HDR_A3_QOS_LEN)
1113                         hdrlen += 2;
1114                 qc_exists = 1;
1115         } else {
1116                 qc_exists = 0;
1117         }
1118
1119         pn_vector[0] = pframe[hdrlen];
1120         pn_vector[1] = pframe[hdrlen+1];
1121         pn_vector[2] = pframe[hdrlen+4];
1122         pn_vector[3] = pframe[hdrlen+5];
1123         pn_vector[4] = pframe[hdrlen+6];
1124         pn_vector[5] = pframe[hdrlen+7];
1125
1126         construct_mic_iv(mic_iv, qc_exists, a4_exists, pframe, plen, pn_vector);
1127
1128         construct_mic_header1(mic_header1, hdrlen, pframe);
1129         construct_mic_header2(mic_header2, pframe, a4_exists, qc_exists);
1130
1131         payload_remainder = plen % 16;
1132         num_blocks = plen / 16;
1133
1134         /* Find start of payload */
1135         payload_index = hdrlen + 8;
1136
1137         /* Calculate MIC */
1138         aes128k128d(key, mic_iv, aes_out);
1139         bitwise_xor(aes_out, mic_header1, chain_buffer);
1140         aes128k128d(key, chain_buffer, aes_out);
1141         bitwise_xor(aes_out, mic_header2, chain_buffer);
1142         aes128k128d(key, chain_buffer, aes_out);
1143
1144         for (i = 0; i < num_blocks; i++) {
1145                 bitwise_xor(aes_out, &pframe[payload_index], chain_buffer);/* bitwise_xor(aes_out, &message[payload_index], chain_buffer); */
1146
1147                 payload_index += 16;
1148                 aes128k128d(key, chain_buffer, aes_out);
1149         }
1150
1151         /* Add on the final payload block if it needs padding */
1152         if (payload_remainder > 0) {
1153                 for (j = 0; j < 16; j++)
1154                         padded_buffer[j] = 0x00;
1155                 for (j = 0; j < payload_remainder; j++)
1156                         padded_buffer[j] = pframe[payload_index++];/* padded_buffer[j] = message[payload_index++]; */
1157                 bitwise_xor(aes_out, padded_buffer, chain_buffer);
1158                 aes128k128d(key, chain_buffer, aes_out);
1159         }
1160
1161         for (j = 0; j < 8; j++)
1162                 mic[j] = aes_out[j];
1163
1164         /* Insert MIC into payload */
1165         for (j = 0; j < 8; j++)
1166                 pframe[payload_index+j] = mic[j];
1167
1168         payload_index = hdrlen + 8;
1169         for (i = 0; i < num_blocks; i++) {
1170                 construct_ctr_preload(ctr_preload, a4_exists, qc_exists, pframe, pn_vector, i+1);
1171                 aes128k128d(key, ctr_preload, aes_out);
1172                 bitwise_xor(aes_out, &pframe[payload_index], chain_buffer);
1173                 for (j = 0; j < 16; j++)
1174                         pframe[payload_index++] = chain_buffer[j];
1175         }
1176
1177         if (payload_remainder > 0) {    /* If there is a short final block, then pad it,*/
1178                                         /* encrypt it and copy the unpadded part back   */
1179                 construct_ctr_preload(ctr_preload, a4_exists, qc_exists, pframe, pn_vector, num_blocks+1);
1180
1181                 for (j = 0; j < 16; j++)
1182                         padded_buffer[j] = 0x00;
1183                 for (j = 0; j < payload_remainder; j++)
1184                         padded_buffer[j] = pframe[payload_index+j];
1185                 aes128k128d(key, ctr_preload, aes_out);
1186                 bitwise_xor(aes_out, padded_buffer, chain_buffer);
1187                 for (j = 0; j < payload_remainder; j++)
1188                         pframe[payload_index++] = chain_buffer[j];
1189         }
1190         /* Encrypt the MIC */
1191         construct_ctr_preload(ctr_preload, a4_exists, qc_exists, pframe, pn_vector, 0);
1192
1193         for (j = 0; j < 16; j++)
1194                 padded_buffer[j] = 0x00;
1195         for (j = 0; j < 8; j++)
1196                 padded_buffer[j] = pframe[j+hdrlen+8+plen];
1197
1198         aes128k128d(key, ctr_preload, aes_out);
1199         bitwise_xor(aes_out, padded_buffer, chain_buffer);
1200         for (j = 0; j < 8; j++)
1201                 pframe[payload_index++] = chain_buffer[j];
1202         return _SUCCESS;
1203 }
1204
1205 u32     rtw_aes_encrypt(struct adapter *padapter, u8 *pxmitframe)
1206 {       /*  exclude ICV */
1207
1208         /*static*/
1209 /*      unsigned char   message[MAX_MSG_SIZE]; */
1210
1211         /* Intermediate Buffers */
1212         int     curfragnum, length;
1213         u8      *pframe, *prwskey;      /*  *payload,*iv */
1214         u8   hw_hdr_offset = 0;
1215         struct  sta_info                *stainfo;
1216         struct  pkt_attrib       *pattrib = &((struct xmit_frame *)pxmitframe)->attrib;
1217         struct  security_priv   *psecuritypriv = &padapter->securitypriv;
1218         struct  xmit_priv               *pxmitpriv = &padapter->xmitpriv;
1219
1220 /*      uint    offset = 0; */
1221         u32 res = _SUCCESS;
1222
1223         if (((struct xmit_frame *)pxmitframe)->buf_addr == NULL)
1224                 return _FAIL;
1225
1226         hw_hdr_offset = TXDESC_SIZE +
1227                  (((struct xmit_frame *)pxmitframe)->pkt_offset * PACKET_OFFSET_SZ);
1228
1229         pframe = ((struct xmit_frame *)pxmitframe)->buf_addr + hw_hdr_offset;
1230
1231         /* 4 start to encrypt each fragment */
1232         if (pattrib->encrypt == _AES_) {
1233                 if (pattrib->psta)
1234                         stainfo = pattrib->psta;
1235                 else
1236                         stainfo = rtw_get_stainfo(&padapter->stapriv, &pattrib->ra[0]);
1237
1238                 if (stainfo != NULL) {
1239                         RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_aes_encrypt: stainfo!= NULL!!!\n"));
1240
1241                         if (IS_MCAST(pattrib->ra))
1242                                 prwskey = psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey;
1243                         else
1244                                 prwskey = &stainfo->dot118021x_UncstKey.skey[0];
1245                         for (curfragnum = 0; curfragnum < pattrib->nr_frags; curfragnum++) {
1246                                 if ((curfragnum+1) == pattrib->nr_frags) {      /* 4 the last fragment */
1247                                         length = pattrib->last_txcmdsz-pattrib->hdrlen-pattrib->iv_len-pattrib->icv_len;
1248
1249                                         aes_cipher(prwskey, pattrib->hdrlen, pframe, length);
1250                                 } else{
1251                                         length = pxmitpriv->frag_len-pattrib->hdrlen-pattrib->iv_len-pattrib->icv_len;
1252
1253                                         aes_cipher(prwskey, pattrib->hdrlen, pframe, length);
1254                                         pframe += pxmitpriv->frag_len;
1255                                         pframe = (u8 *)round_up((size_t)(pframe), 8);
1256                                 }
1257                         }
1258                 } else{
1259                         RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_aes_encrypt: stainfo==NULL!!!\n"));
1260                         res = _FAIL;
1261                 }
1262         }
1263
1264
1265                 return res;
1266 }
1267
1268 static int aes_decipher(u8 *key, uint   hdrlen,
1269                         u8 *pframe, uint plen)
1270 {
1271         static u8       message[MAX_MSG_SIZE];
1272         uint    qc_exists, a4_exists, i, j, payload_remainder,
1273                         num_blocks, payload_index;
1274         int res = _SUCCESS;
1275
1276         u8 pn_vector[6];
1277         u8 mic_iv[16];
1278         u8 mic_header1[16];
1279         u8 mic_header2[16];
1280         u8 ctr_preload[16];
1281
1282         /* Intermediate Buffers */
1283         u8 chain_buffer[16];
1284         u8 aes_out[16];
1285         u8 padded_buffer[16];
1286         u8 mic[8];
1287
1288 /*      uint    offset = 0; */
1289         uint    frtype  = GetFrameType(pframe);
1290         uint    frsubtype  = GetFrameSubType(pframe);
1291         frsubtype >>= 4;
1292
1293         memset(mic_iv, 0, 16);
1294         memset(mic_header1, 0, 16);
1295         memset(mic_header2, 0, 16);
1296         memset(ctr_preload, 0, 16);
1297         memset(chain_buffer, 0, 16);
1298         memset(aes_out, 0, 16);
1299         memset(padded_buffer, 0, 16);
1300
1301         /* start to decrypt the payload */
1302
1303         num_blocks = (plen-8) / 16; /* plen including llc, payload_length and mic) */
1304
1305         payload_remainder = (plen-8) % 16;
1306
1307         pn_vector[0]  = pframe[hdrlen];
1308         pn_vector[1]  = pframe[hdrlen+1];
1309         pn_vector[2]  = pframe[hdrlen+4];
1310         pn_vector[3]  = pframe[hdrlen+5];
1311         pn_vector[4]  = pframe[hdrlen+6];
1312         pn_vector[5]  = pframe[hdrlen+7];
1313
1314         if ((hdrlen == WLAN_HDR_A3_LEN) || (hdrlen ==  WLAN_HDR_A3_QOS_LEN))
1315                 a4_exists = 0;
1316         else
1317                 a4_exists = 1;
1318
1319         if ((frtype == WIFI_DATA_CFACK) || (frtype == WIFI_DATA_CFPOLL) ||
1320             (frtype == WIFI_DATA_CFACKPOLL)) {
1321                         qc_exists = 1;
1322                         if (hdrlen !=  WLAN_HDR_A3_QOS_LEN)
1323                                 hdrlen += 2;
1324         } else if ((frsubtype == 0x08) || (frsubtype == 0x09) ||
1325                    (frsubtype == 0x0a) || (frsubtype == 0x0b)) {
1326                 if (hdrlen !=  WLAN_HDR_A3_QOS_LEN)
1327                         hdrlen += 2;
1328                 qc_exists = 1;
1329         } else {
1330                 qc_exists = 0;
1331         }
1332
1333         /*  now, decrypt pframe with hdrlen offset and plen long */
1334
1335         payload_index = hdrlen + 8; /*  8 is for extiv */
1336
1337         for (i = 0; i < num_blocks; i++) {
1338                 construct_ctr_preload(ctr_preload, a4_exists, qc_exists, pframe, pn_vector, i+1);
1339
1340                 aes128k128d(key, ctr_preload, aes_out);
1341                 bitwise_xor(aes_out, &pframe[payload_index], chain_buffer);
1342
1343                 for (j = 0; j < 16; j++)
1344                         pframe[payload_index++] = chain_buffer[j];
1345         }
1346
1347         if (payload_remainder > 0) {    /* If there is a short final block, then pad it,*/
1348                                         /* encrypt it and copy the unpadded part back   */
1349                 construct_ctr_preload(ctr_preload, a4_exists, qc_exists, pframe, pn_vector, num_blocks+1);
1350
1351                 for (j = 0; j < 16; j++)
1352                         padded_buffer[j] = 0x00;
1353                 for (j = 0; j < payload_remainder; j++)
1354                         padded_buffer[j] = pframe[payload_index+j];
1355                 aes128k128d(key, ctr_preload, aes_out);
1356                 bitwise_xor(aes_out, padded_buffer, chain_buffer);
1357                 for (j = 0; j < payload_remainder; j++)
1358                         pframe[payload_index++] = chain_buffer[j];
1359         }
1360
1361         /* start to calculate the mic */
1362         if ((hdrlen+plen+8) <= MAX_MSG_SIZE)
1363                 memcpy(message, pframe, (hdrlen + plen+8)); /* 8 is for ext iv len */
1364
1365         pn_vector[0] = pframe[hdrlen];
1366         pn_vector[1] = pframe[hdrlen+1];
1367         pn_vector[2] = pframe[hdrlen+4];
1368         pn_vector[3] = pframe[hdrlen+5];
1369         pn_vector[4] = pframe[hdrlen+6];
1370         pn_vector[5] = pframe[hdrlen+7];
1371         construct_mic_iv(mic_iv, qc_exists, a4_exists, message, plen-8, pn_vector);
1372
1373         construct_mic_header1(mic_header1, hdrlen, message);
1374         construct_mic_header2(mic_header2, message, a4_exists, qc_exists);
1375
1376         payload_remainder = (plen-8) % 16;
1377         num_blocks = (plen-8) / 16;
1378
1379         /* Find start of payload */
1380         payload_index = hdrlen + 8;
1381
1382         /* Calculate MIC */
1383         aes128k128d(key, mic_iv, aes_out);
1384         bitwise_xor(aes_out, mic_header1, chain_buffer);
1385         aes128k128d(key, chain_buffer, aes_out);
1386         bitwise_xor(aes_out, mic_header2, chain_buffer);
1387         aes128k128d(key, chain_buffer, aes_out);
1388
1389         for (i = 0; i < num_blocks; i++) {
1390                 bitwise_xor(aes_out, &message[payload_index], chain_buffer);
1391
1392                 payload_index += 16;
1393                 aes128k128d(key, chain_buffer, aes_out);
1394         }
1395
1396         /* Add on the final payload block if it needs padding */
1397         if (payload_remainder > 0) {
1398                 for (j = 0; j < 16; j++)
1399                         padded_buffer[j] = 0x00;
1400                 for (j = 0; j < payload_remainder; j++)
1401                         padded_buffer[j] = message[payload_index++];
1402                 bitwise_xor(aes_out, padded_buffer, chain_buffer);
1403                 aes128k128d(key, chain_buffer, aes_out);
1404         }
1405
1406         for (j = 0 ; j < 8; j++)
1407                 mic[j] = aes_out[j];
1408
1409         /* Insert MIC into payload */
1410         for (j = 0; j < 8; j++)
1411                 message[payload_index+j] = mic[j];
1412
1413         payload_index = hdrlen + 8;
1414         for (i = 0; i < num_blocks; i++) {
1415                 construct_ctr_preload(ctr_preload, a4_exists, qc_exists, message, pn_vector, i+1);
1416                 aes128k128d(key, ctr_preload, aes_out);
1417                 bitwise_xor(aes_out, &message[payload_index], chain_buffer);
1418                 for (j = 0; j < 16; j++)
1419                         message[payload_index++] = chain_buffer[j];
1420         }
1421
1422         if (payload_remainder > 0) { /* If there is a short final block, then pad it,*/
1423                 /* encrypt it and copy the unpadded part back   */
1424                 construct_ctr_preload(ctr_preload, a4_exists, qc_exists, message, pn_vector, num_blocks+1);
1425
1426                 for (j = 0; j < 16; j++)
1427                         padded_buffer[j] = 0x00;
1428                 for (j = 0; j < payload_remainder; j++)
1429                         padded_buffer[j] = message[payload_index+j];
1430                 aes128k128d(key, ctr_preload, aes_out);
1431                 bitwise_xor(aes_out, padded_buffer, chain_buffer);
1432                 for (j = 0; j < payload_remainder; j++)
1433                         message[payload_index++] = chain_buffer[j];
1434         }
1435
1436         /* Encrypt the MIC */
1437         construct_ctr_preload(ctr_preload, a4_exists, qc_exists, message, pn_vector, 0);
1438
1439         for (j = 0; j < 16; j++)
1440                 padded_buffer[j] = 0x00;
1441         for (j = 0; j < 8; j++)
1442                 padded_buffer[j] = message[j+hdrlen+8+plen-8];
1443
1444         aes128k128d(key, ctr_preload, aes_out);
1445         bitwise_xor(aes_out, padded_buffer, chain_buffer);
1446         for (j = 0; j < 8; j++)
1447                 message[payload_index++] = chain_buffer[j];
1448
1449         /* compare the mic */
1450         for (i = 0; i < 8; i++) {
1451                 if (pframe[hdrlen+8+plen-8+i] != message[hdrlen+8+plen-8+i]) {
1452                         RT_TRACE(_module_rtl871x_security_c_, _drv_err_,
1453                                  ("aes_decipher:mic check error mic[%d]: pframe(%x)!=message(%x)\n",
1454                                  i, pframe[hdrlen+8+plen-8+i], message[hdrlen+8+plen-8+i]));
1455                         DBG_88E("aes_decipher:mic check error mic[%d]: pframe(%x)!=message(%x)\n",
1456                                 i, pframe[hdrlen+8+plen-8+i], message[hdrlen+8+plen-8+i]);
1457                         res = _FAIL;
1458                 }
1459         }
1460         return res;
1461 }
1462
1463 u32     rtw_aes_decrypt(struct adapter *padapter, u8 *precvframe)
1464 {       /*  exclude ICV */
1465         /* Intermediate Buffers */
1466         int             length;
1467         u8      *pframe, *prwskey;      /*  *payload,*iv */
1468         struct  sta_info                *stainfo;
1469         struct  rx_pkt_attrib    *prxattrib = &((struct recv_frame *)precvframe)->attrib;
1470         struct  security_priv   *psecuritypriv = &padapter->securitypriv;
1471         u32     res = _SUCCESS;
1472
1473         pframe = (unsigned char *)((struct recv_frame *)precvframe)->pkt->data;
1474         /* 4 start to encrypt each fragment */
1475         if (prxattrib->encrypt == _AES_) {
1476                 stainfo = rtw_get_stainfo(&padapter->stapriv, &prxattrib->ta[0]);
1477                 if (stainfo != NULL) {
1478                         RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_aes_decrypt: stainfo!= NULL!!!\n"));
1479
1480                         if (IS_MCAST(prxattrib->ra)) {
1481                                 /* in concurrent we should use sw descrypt in group key, so we remove this message */
1482                                 if (!psecuritypriv->binstallGrpkey) {
1483                                         res = _FAIL;
1484                                         DBG_88E("%s:rx bc/mc packets, but didn't install group key!!!!!!!!!!\n", __func__);
1485                                         goto exit;
1486                                 }
1487                                 prwskey = psecuritypriv->dot118021XGrpKey[prxattrib->key_index].skey;
1488                                 if (psecuritypriv->dot118021XGrpKeyid != prxattrib->key_index) {
1489                                         DBG_88E("not match packet_index=%d, install_index=%d\n",
1490                                                 prxattrib->key_index, psecuritypriv->dot118021XGrpKeyid);
1491                                         res = _FAIL;
1492                                         goto exit;
1493                                 }
1494                         } else {
1495                                 prwskey = &stainfo->dot118021x_UncstKey.skey[0];
1496                         }
1497                         length = ((struct recv_frame *)precvframe)->pkt->len-prxattrib->hdrlen-prxattrib->iv_len;
1498                         res = aes_decipher(prwskey, prxattrib->hdrlen, pframe, length);
1499                 } else {
1500                         RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_aes_encrypt: stainfo==NULL!!!\n"));
1501                         res = _FAIL;
1502                 }
1503         }
1504 exit:
1505         return res;
1506 }
1507
1508 /* AES tables*/
1509 const u32 Te0[256] = {
1510         0xc66363a5U, 0xf87c7c84U, 0xee777799U, 0xf67b7b8dU,
1511         0xfff2f20dU, 0xd66b6bbdU, 0xde6f6fb1U, 0x91c5c554U,
1512         0x60303050U, 0x02010103U, 0xce6767a9U, 0x562b2b7dU,
1513         0xe7fefe19U, 0xb5d7d762U, 0x4dababe6U, 0xec76769aU,
1514         0x8fcaca45U, 0x1f82829dU, 0x89c9c940U, 0xfa7d7d87U,
1515         0xeffafa15U, 0xb25959ebU, 0x8e4747c9U, 0xfbf0f00bU,
1516         0x41adadecU, 0xb3d4d467U, 0x5fa2a2fdU, 0x45afafeaU,
1517         0x239c9cbfU, 0x53a4a4f7U, 0xe4727296U, 0x9bc0c05bU,
1518         0x75b7b7c2U, 0xe1fdfd1cU, 0x3d9393aeU, 0x4c26266aU,
1519         0x6c36365aU, 0x7e3f3f41U, 0xf5f7f702U, 0x83cccc4fU,
1520         0x6834345cU, 0x51a5a5f4U, 0xd1e5e534U, 0xf9f1f108U,
1521         0xe2717193U, 0xabd8d873U, 0x62313153U, 0x2a15153fU,
1522         0x0804040cU, 0x95c7c752U, 0x46232365U, 0x9dc3c35eU,
1523         0x30181828U, 0x379696a1U, 0x0a05050fU, 0x2f9a9ab5U,
1524         0x0e070709U, 0x24121236U, 0x1b80809bU, 0xdfe2e23dU,
1525         0xcdebeb26U, 0x4e272769U, 0x7fb2b2cdU, 0xea75759fU,
1526         0x1209091bU, 0x1d83839eU, 0x582c2c74U, 0x341a1a2eU,
1527         0x361b1b2dU, 0xdc6e6eb2U, 0xb45a5aeeU, 0x5ba0a0fbU,
1528         0xa45252f6U, 0x763b3b4dU, 0xb7d6d661U, 0x7db3b3ceU,
1529         0x5229297bU, 0xdde3e33eU, 0x5e2f2f71U, 0x13848497U,
1530         0xa65353f5U, 0xb9d1d168U, 0x00000000U, 0xc1eded2cU,
1531         0x40202060U, 0xe3fcfc1fU, 0x79b1b1c8U, 0xb65b5bedU,
1532         0xd46a6abeU, 0x8dcbcb46U, 0x67bebed9U, 0x7239394bU,
1533         0x944a4adeU, 0x984c4cd4U, 0xb05858e8U, 0x85cfcf4aU,
1534         0xbbd0d06bU, 0xc5efef2aU, 0x4faaaae5U, 0xedfbfb16U,
1535         0x864343c5U, 0x9a4d4dd7U, 0x66333355U, 0x11858594U,
1536         0x8a4545cfU, 0xe9f9f910U, 0x04020206U, 0xfe7f7f81U,
1537         0xa05050f0U, 0x783c3c44U, 0x259f9fbaU, 0x4ba8a8e3U,
1538         0xa25151f3U, 0x5da3a3feU, 0x804040c0U, 0x058f8f8aU,
1539         0x3f9292adU, 0x219d9dbcU, 0x70383848U, 0xf1f5f504U,
1540         0x63bcbcdfU, 0x77b6b6c1U, 0xafdada75U, 0x42212163U,
1541         0x20101030U, 0xe5ffff1aU, 0xfdf3f30eU, 0xbfd2d26dU,
1542         0x81cdcd4cU, 0x180c0c14U, 0x26131335U, 0xc3ecec2fU,
1543         0xbe5f5fe1U, 0x359797a2U, 0x884444ccU, 0x2e171739U,
1544         0x93c4c457U, 0x55a7a7f2U, 0xfc7e7e82U, 0x7a3d3d47U,
1545         0xc86464acU, 0xba5d5de7U, 0x3219192bU, 0xe6737395U,
1546         0xc06060a0U, 0x19818198U, 0x9e4f4fd1U, 0xa3dcdc7fU,
1547         0x44222266U, 0x542a2a7eU, 0x3b9090abU, 0x0b888883U,
1548         0x8c4646caU, 0xc7eeee29U, 0x6bb8b8d3U, 0x2814143cU,
1549         0xa7dede79U, 0xbc5e5ee2U, 0x160b0b1dU, 0xaddbdb76U,
1550         0xdbe0e03bU, 0x64323256U, 0x743a3a4eU, 0x140a0a1eU,
1551         0x924949dbU, 0x0c06060aU, 0x4824246cU, 0xb85c5ce4U,
1552         0x9fc2c25dU, 0xbdd3d36eU, 0x43acacefU, 0xc46262a6U,
1553         0x399191a8U, 0x319595a4U, 0xd3e4e437U, 0xf279798bU,
1554         0xd5e7e732U, 0x8bc8c843U, 0x6e373759U, 0xda6d6db7U,
1555         0x018d8d8cU, 0xb1d5d564U, 0x9c4e4ed2U, 0x49a9a9e0U,
1556         0xd86c6cb4U, 0xac5656faU, 0xf3f4f407U, 0xcfeaea25U,
1557         0xca6565afU, 0xf47a7a8eU, 0x47aeaee9U, 0x10080818U,
1558         0x6fbabad5U, 0xf0787888U, 0x4a25256fU, 0x5c2e2e72U,
1559         0x381c1c24U, 0x57a6a6f1U, 0x73b4b4c7U, 0x97c6c651U,
1560         0xcbe8e823U, 0xa1dddd7cU, 0xe874749cU, 0x3e1f1f21U,
1561         0x964b4bddU, 0x61bdbddcU, 0x0d8b8b86U, 0x0f8a8a85U,
1562         0xe0707090U, 0x7c3e3e42U, 0x71b5b5c4U, 0xcc6666aaU,
1563         0x904848d8U, 0x06030305U, 0xf7f6f601U, 0x1c0e0e12U,
1564         0xc26161a3U, 0x6a35355fU, 0xae5757f9U, 0x69b9b9d0U,
1565         0x17868691U, 0x99c1c158U, 0x3a1d1d27U, 0x279e9eb9U,
1566         0xd9e1e138U, 0xebf8f813U, 0x2b9898b3U, 0x22111133U,
1567         0xd26969bbU, 0xa9d9d970U, 0x078e8e89U, 0x339494a7U,
1568         0x2d9b9bb6U, 0x3c1e1e22U, 0x15878792U, 0xc9e9e920U,
1569         0x87cece49U, 0xaa5555ffU, 0x50282878U, 0xa5dfdf7aU,
1570         0x038c8c8fU, 0x59a1a1f8U, 0x09898980U, 0x1a0d0d17U,
1571         0x65bfbfdaU, 0xd7e6e631U, 0x844242c6U, 0xd06868b8U,
1572         0x824141c3U, 0x299999b0U, 0x5a2d2d77U, 0x1e0f0f11U,
1573         0x7bb0b0cbU, 0xa85454fcU, 0x6dbbbbd6U, 0x2c16163aU,
1574 };
1575
1576 const u32 Td0[256] = {
1577         0x51f4a750U, 0x7e416553U, 0x1a17a4c3U, 0x3a275e96U,
1578         0x3bab6bcbU, 0x1f9d45f1U, 0xacfa58abU, 0x4be30393U,
1579         0x2030fa55U, 0xad766df6U, 0x88cc7691U, 0xf5024c25U,
1580         0x4fe5d7fcU, 0xc52acbd7U, 0x26354480U, 0xb562a38fU,
1581         0xdeb15a49U, 0x25ba1b67U, 0x45ea0e98U, 0x5dfec0e1U,
1582         0xc32f7502U, 0x814cf012U, 0x8d4697a3U, 0x6bd3f9c6U,
1583         0x038f5fe7U, 0x15929c95U, 0xbf6d7aebU, 0x955259daU,
1584         0xd4be832dU, 0x587421d3U, 0x49e06929U, 0x8ec9c844U,
1585         0x75c2896aU, 0xf48e7978U, 0x99583e6bU, 0x27b971ddU,
1586         0xbee14fb6U, 0xf088ad17U, 0xc920ac66U, 0x7dce3ab4U,
1587         0x63df4a18U, 0xe51a3182U, 0x97513360U, 0x62537f45U,
1588         0xb16477e0U, 0xbb6bae84U, 0xfe81a01cU, 0xf9082b94U,
1589         0x70486858U, 0x8f45fd19U, 0x94de6c87U, 0x527bf8b7U,
1590         0xab73d323U, 0x724b02e2U, 0xe31f8f57U, 0x6655ab2aU,
1591         0xb2eb2807U, 0x2fb5c203U, 0x86c57b9aU, 0xd33708a5U,
1592         0x302887f2U, 0x23bfa5b2U, 0x02036abaU, 0xed16825cU,
1593         0x8acf1c2bU, 0xa779b492U, 0xf307f2f0U, 0x4e69e2a1U,
1594         0x65daf4cdU, 0x0605bed5U, 0xd134621fU, 0xc4a6fe8aU,
1595         0x342e539dU, 0xa2f355a0U, 0x058ae132U, 0xa4f6eb75U,
1596         0x0b83ec39U, 0x4060efaaU, 0x5e719f06U, 0xbd6e1051U,
1597         0x3e218af9U, 0x96dd063dU, 0xdd3e05aeU, 0x4de6bd46U,
1598         0x91548db5U, 0x71c45d05U, 0x0406d46fU, 0x605015ffU,
1599         0x1998fb24U, 0xd6bde997U, 0x894043ccU, 0x67d99e77U,
1600         0xb0e842bdU, 0x07898b88U, 0xe7195b38U, 0x79c8eedbU,
1601         0xa17c0a47U, 0x7c420fe9U, 0xf8841ec9U, 0x00000000U,
1602         0x09808683U, 0x322bed48U, 0x1e1170acU, 0x6c5a724eU,
1603         0xfd0efffbU, 0x0f853856U, 0x3daed51eU, 0x362d3927U,
1604         0x0a0fd964U, 0x685ca621U, 0x9b5b54d1U, 0x24362e3aU,
1605         0x0c0a67b1U, 0x9357e70fU, 0xb4ee96d2U, 0x1b9b919eU,
1606         0x80c0c54fU, 0x61dc20a2U, 0x5a774b69U, 0x1c121a16U,
1607         0xe293ba0aU, 0xc0a02ae5U, 0x3c22e043U, 0x121b171dU,
1608         0x0e090d0bU, 0xf28bc7adU, 0x2db6a8b9U, 0x141ea9c8U,
1609         0x57f11985U, 0xaf75074cU, 0xee99ddbbU, 0xa37f60fdU,
1610         0xf701269fU, 0x5c72f5bcU, 0x44663bc5U, 0x5bfb7e34U,
1611         0x8b432976U, 0xcb23c6dcU, 0xb6edfc68U, 0xb8e4f163U,
1612         0xd731dccaU, 0x42638510U, 0x13972240U, 0x84c61120U,
1613         0x854a247dU, 0xd2bb3df8U, 0xaef93211U, 0xc729a16dU,
1614         0x1d9e2f4bU, 0xdcb230f3U, 0x0d8652ecU, 0x77c1e3d0U,
1615         0x2bb3166cU, 0xa970b999U, 0x119448faU, 0x47e96422U,
1616         0xa8fc8cc4U, 0xa0f03f1aU, 0x567d2cd8U, 0x223390efU,
1617         0x87494ec7U, 0xd938d1c1U, 0x8ccaa2feU, 0x98d40b36U,
1618         0xa6f581cfU, 0xa57ade28U, 0xdab78e26U, 0x3fadbfa4U,
1619         0x2c3a9de4U, 0x5078920dU, 0x6a5fcc9bU, 0x547e4662U,
1620         0xf68d13c2U, 0x90d8b8e8U, 0x2e39f75eU, 0x82c3aff5U,
1621         0x9f5d80beU, 0x69d0937cU, 0x6fd52da9U, 0xcf2512b3U,
1622         0xc8ac993bU, 0x10187da7U, 0xe89c636eU, 0xdb3bbb7bU,
1623         0xcd267809U, 0x6e5918f4U, 0xec9ab701U, 0x834f9aa8U,
1624         0xe6956e65U, 0xaaffe67eU, 0x21bccf08U, 0xef15e8e6U,
1625         0xbae79bd9U, 0x4a6f36ceU, 0xea9f09d4U, 0x29b07cd6U,
1626         0x31a4b2afU, 0x2a3f2331U, 0xc6a59430U, 0x35a266c0U,
1627         0x744ebc37U, 0xfc82caa6U, 0xe090d0b0U, 0x33a7d815U,
1628         0xf104984aU, 0x41ecdaf7U, 0x7fcd500eU, 0x1791f62fU,
1629         0x764dd68dU, 0x43efb04dU, 0xccaa4d54U, 0xe49604dfU,
1630         0x9ed1b5e3U, 0x4c6a881bU, 0xc12c1fb8U, 0x4665517fU,
1631         0x9d5eea04U, 0x018c355dU, 0xfa877473U, 0xfb0b412eU,
1632         0xb3671d5aU, 0x92dbd252U, 0xe9105633U, 0x6dd64713U,
1633         0x9ad7618cU, 0x37a10c7aU, 0x59f8148eU, 0xeb133c89U,
1634         0xcea927eeU, 0xb761c935U, 0xe11ce5edU, 0x7a47b13cU,
1635         0x9cd2df59U, 0x55f2733fU, 0x1814ce79U, 0x73c737bfU,
1636         0x53f7cdeaU, 0x5ffdaa5bU, 0xdf3d6f14U, 0x7844db86U,
1637         0xcaaff381U, 0xb968c43eU, 0x3824342cU, 0xc2a3405fU,
1638         0x161dc372U, 0xbce2250cU, 0x283c498bU, 0xff0d9541U,
1639         0x39a80171U, 0x080cb3deU, 0xd8b4e49cU, 0x6456c190U,
1640         0x7bcb8461U, 0xd532b670U, 0x486c5c74U, 0xd0b85742U,
1641 };
1642
1643 const u8 Td4s[256] = {
1644         0x52U, 0x09U, 0x6aU, 0xd5U, 0x30U, 0x36U, 0xa5U, 0x38U,
1645         0xbfU, 0x40U, 0xa3U, 0x9eU, 0x81U, 0xf3U, 0xd7U, 0xfbU,
1646         0x7cU, 0xe3U, 0x39U, 0x82U, 0x9bU, 0x2fU, 0xffU, 0x87U,
1647         0x34U, 0x8eU, 0x43U, 0x44U, 0xc4U, 0xdeU, 0xe9U, 0xcbU,
1648         0x54U, 0x7bU, 0x94U, 0x32U, 0xa6U, 0xc2U, 0x23U, 0x3dU,
1649         0xeeU, 0x4cU, 0x95U, 0x0bU, 0x42U, 0xfaU, 0xc3U, 0x4eU,
1650         0x08U, 0x2eU, 0xa1U, 0x66U, 0x28U, 0xd9U, 0x24U, 0xb2U,
1651         0x76U, 0x5bU, 0xa2U, 0x49U, 0x6dU, 0x8bU, 0xd1U, 0x25U,
1652         0x72U, 0xf8U, 0xf6U, 0x64U, 0x86U, 0x68U, 0x98U, 0x16U,
1653         0xd4U, 0xa4U, 0x5cU, 0xccU, 0x5dU, 0x65U, 0xb6U, 0x92U,
1654         0x6cU, 0x70U, 0x48U, 0x50U, 0xfdU, 0xedU, 0xb9U, 0xdaU,
1655         0x5eU, 0x15U, 0x46U, 0x57U, 0xa7U, 0x8dU, 0x9dU, 0x84U,
1656         0x90U, 0xd8U, 0xabU, 0x00U, 0x8cU, 0xbcU, 0xd3U, 0x0aU,
1657         0xf7U, 0xe4U, 0x58U, 0x05U, 0xb8U, 0xb3U, 0x45U, 0x06U,
1658         0xd0U, 0x2cU, 0x1eU, 0x8fU, 0xcaU, 0x3fU, 0x0fU, 0x02U,
1659         0xc1U, 0xafU, 0xbdU, 0x03U, 0x01U, 0x13U, 0x8aU, 0x6bU,
1660         0x3aU, 0x91U, 0x11U, 0x41U, 0x4fU, 0x67U, 0xdcU, 0xeaU,
1661         0x97U, 0xf2U, 0xcfU, 0xceU, 0xf0U, 0xb4U, 0xe6U, 0x73U,
1662         0x96U, 0xacU, 0x74U, 0x22U, 0xe7U, 0xadU, 0x35U, 0x85U,
1663         0xe2U, 0xf9U, 0x37U, 0xe8U, 0x1cU, 0x75U, 0xdfU, 0x6eU,
1664         0x47U, 0xf1U, 0x1aU, 0x71U, 0x1dU, 0x29U, 0xc5U, 0x89U,
1665         0x6fU, 0xb7U, 0x62U, 0x0eU, 0xaaU, 0x18U, 0xbeU, 0x1bU,
1666         0xfcU, 0x56U, 0x3eU, 0x4bU, 0xc6U, 0xd2U, 0x79U, 0x20U,
1667         0x9aU, 0xdbU, 0xc0U, 0xfeU, 0x78U, 0xcdU, 0x5aU, 0xf4U,
1668         0x1fU, 0xddU, 0xa8U, 0x33U, 0x88U, 0x07U, 0xc7U, 0x31U,
1669         0xb1U, 0x12U, 0x10U, 0x59U, 0x27U, 0x80U, 0xecU, 0x5fU,
1670         0x60U, 0x51U, 0x7fU, 0xa9U, 0x19U, 0xb5U, 0x4aU, 0x0dU,
1671         0x2dU, 0xe5U, 0x7aU, 0x9fU, 0x93U, 0xc9U, 0x9cU, 0xefU,
1672         0xa0U, 0xe0U, 0x3bU, 0x4dU, 0xaeU, 0x2aU, 0xf5U, 0xb0U,
1673         0xc8U, 0xebU, 0xbbU, 0x3cU, 0x83U, 0x53U, 0x99U, 0x61U,
1674         0x17U, 0x2bU, 0x04U, 0x7eU, 0xbaU, 0x77U, 0xd6U, 0x26U,
1675         0xe1U, 0x69U, 0x14U, 0x63U, 0x55U, 0x21U, 0x0cU, 0x7dU,
1676 };
1677 const u8 rcons[] = {
1678         0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80, 0x1B, 0x36
1679         /* for 128-bit blocks, Rijndael never uses more than 10 rcon values */
1680 };
1681
1682 /**
1683  * Expand the cipher key into the encryption key schedule.
1684  *
1685  * @return      the number of rounds for the given cipher key size.
1686  */
1687 #define ROUND(i, d, s) \
1688 do {                                                                    \
1689         d##0 = TE0(s##0) ^ TE1(s##1) ^ TE2(s##2) ^ TE3(s##3) ^ rk[4 * i]; \
1690         d##1 = TE0(s##1) ^ TE1(s##2) ^ TE2(s##3) ^ TE3(s##0) ^ rk[4 * i + 1]; \
1691         d##2 = TE0(s##2) ^ TE1(s##3) ^ TE2(s##0) ^ TE3(s##1) ^ rk[4 * i + 2]; \
1692         d##3 = TE0(s##3) ^ TE1(s##0) ^ TE2(s##1) ^ TE3(s##2) ^ rk[4 * i + 3]; \
1693 } while (0);