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