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