drivers/staging/rtl8723au/core/rtw_security.c

   1 /******************************************************************************
   2  *
   3  * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
   4  *
   5  * This program is free software; you can redistribute it and/or modify it
   6  * under the terms of version 2 of the GNU General Public License as
   7  * published by the Free Software Foundation.
   8  *
   9  * This program is distributed in the hope that it will be useful, but WITHOUT
  10  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  11  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
  12  * more details.
  13  *
  14  ******************************************************************************/
  15 #define  _RTW_SECURITY_C_
  16
  17 #include <osdep_service.h>
  18 #include <drv_types.h>
  19 #include <wifi.h>
  20 #include <osdep_intf.h>
  21
  22 /* WEP related ===== */
  23
  24 #define CRC32_POLY 0x04c11db7
  25
  26 struct arc4context {
  27         u32 x;
  28         u32 y;
  29         u8 state[256];
  30 };
  31
  32 static void arcfour_init(struct arc4context *parc4ctx, u8 *key, u32 key_len)
  33 {
  34         u32     t, u;
  35         u32     keyindex;
  36         u32     stateindex;
  37         u8 *state;
  38         u32     counter;
  39
  40         state = parc4ctx->state;
  41         parc4ctx->x = 0;
  42         parc4ctx->y = 0;
  43         for (counter = 0; counter < 256; counter++)
  44                 state[counter] = (u8)counter;
  45         keyindex = 0;
  46         stateindex = 0;
  47         for (counter = 0; counter < 256; counter++) {
  48                 t = state[counter];
  49                 stateindex = (stateindex + key[keyindex] + t) & 0xff;
  50                 u = state[stateindex];
  51                 state[stateindex] = (u8)t;
  52                 state[counter] = (u8)u;
  53                 if (++keyindex >= key_len)
  54                         keyindex = 0;
  55         }
  56
  57 }
  58 static u32 arcfour_byte(        struct arc4context      *parc4ctx)
  59 {
  60         u32 x;
  61         u32 y;
  62         u32 sx, sy;
  63         u8 *state;
  64
  65         state = parc4ctx->state;
  66         x = (parc4ctx->x + 1) & 0xff;
  67         sx = state[x];
  68         y = (sx + parc4ctx->y) & 0xff;
  69         sy = state[y];
  70         parc4ctx->x = x;
  71         parc4ctx->y = y;
  72         state[y] = (u8)sx;
  73         state[x] = (u8)sy;
  74
  75         return state[(sx + sy) & 0xff];
  76 }
  77
  78 static void arcfour_encrypt(    struct arc4context      *parc4ctx,
  79         u8 *dest,
  80         u8 *src,
  81         u32 len)
  82 {
  83         u32     i;
  84
  85         for (i = 0; i < len; i++)
  86                 dest[i] = src[i] ^ (unsigned char)arcfour_byte(parc4ctx);
  87
  88 }
  89
  90 static int bcrc32initialized = 0;
  91 static u32 crc32_table[256];
  92
  93 static u8 crc32_reverseBit(u8 data)
  94 {
  95         u8 retval = ((data << 7) & 0x80) | ((data << 5) & 0x40) |
  96                 ((data << 3) & 0x20) | ((data << 1) & 0x10) |
  97                 ((data >> 1) & 0x08) | ((data >> 3) & 0x04) |
  98                 ((data >> 5) & 0x02) | ((data >> 7) & 0x01);
  99         return retval;
 100 }
 101
 102 static void crc32_init(void)
 103 {
 104
 105         if (bcrc32initialized == 1)
 106                 return;
 107         else{
 108                 int i, j;
 109                 u32 c;
 110                 u8 *p = (u8 *)&c, *p1;
 111                 u8 k;
 112
 113                 c = 0x12340000;
 114
 115                 for (i = 0; i < 256; ++i) {
 116                         k = crc32_reverseBit((u8)i);
 117                         for (c = ((u32)k) << 24, j = 8; j > 0; --j) {
 118                                 c = c & 0x80000000 ? (c << 1) ^ CRC32_POLY : (c << 1);
 119                         }
 120                         p1 = (u8 *)&crc32_table[i];
 121
 122                         p1[0] = crc32_reverseBit(p[3]);
 123                         p1[1] = crc32_reverseBit(p[2]);
 124                         p1[2] = crc32_reverseBit(p[1]);
 125                         p1[3] = crc32_reverseBit(p[0]);
 126                 }
 127                 bcrc32initialized = 1;
 128         }
 129 }
 130
 131 static u32 getcrc32(u8 *buf, int len)
 132 {
 133         u8 *p;
 134         u32  crc;
 135
 136         if (bcrc32initialized == 0) crc32_init();
 137
 138         crc = 0xffffffff;       /* preload shift register, per CRC-32 spec */
 139
 140         for (p = buf; len > 0; ++p, --len)
 141                 crc = crc32_table[ (crc ^ *p) & 0xff] ^ (crc >> 8);
 142
 143         return ~crc;    /* transmit complement, per CRC-32 spec */
 144 }
 145
 146 /* Need to consider the fragment  situation */
 147 void rtw_wep_encrypt23a(struct rtw_adapter *padapter,
 148                      struct xmit_frame *pxmitframe)
 149 {
 150         /*  exclude ICV */
 151         unsigned char crc[4];
 152         struct arc4context mycontext;
 153         int curfragnum, length, index;
 154         u32 keylength;
 155         u8 *pframe, *payload, *iv;    /* wepkey */
 156         u8 wepkey[16];
 157         u8 hw_hdr_offset = 0;
 158         struct pkt_attrib *pattrib = &pxmitframe->attrib;
 159         struct security_priv *psecuritypriv = &padapter->securitypriv;
 160         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 161
 162         if (!pxmitframe->buf_addr)
 163                 return;
 164
 165         hw_hdr_offset = TXDESC_OFFSET;
 166
 167         pframe = pxmitframe->buf_addr + hw_hdr_offset;
 168
 169         /* start to encrypt each fragment */
 170         if (pattrib->encrypt != WLAN_CIPHER_SUITE_WEP40 &&
 171             pattrib->encrypt != WLAN_CIPHER_SUITE_WEP104)
 172                 return;
 173
 174         index = psecuritypriv->dot11PrivacyKeyIndex;
 175         keylength = psecuritypriv->wep_key[index].keylen;
 176
 177         for (curfragnum = 0; curfragnum < pattrib->nr_frags ; curfragnum++) {
 178                 iv = pframe + pattrib->hdrlen;
 179                 memcpy(&wepkey[0], iv, 3);
 180                 memcpy(&wepkey[3], &psecuritypriv->wep_key[index].key,
 181                        keylength);
 182                 payload = pframe + pattrib->iv_len + pattrib->hdrlen;
 183
 184                 if ((curfragnum + 1) == pattrib->nr_frags) {
 185                         /* the last fragment */
 186                         length = pattrib->last_txcmdsz - pattrib->hdrlen -
 187                                 pattrib->iv_len- pattrib->icv_len;
 188
 189                         *((u32 *)crc) = cpu_to_le32(getcrc32(payload, length));
 190
 191                         arcfour_init(&mycontext, wepkey, 3 + keylength);
 192                         arcfour_encrypt(&mycontext, payload, payload, length);
 193                         arcfour_encrypt(&mycontext, payload + length, crc, 4);
 194                 } else {
 195                         length = pxmitpriv->frag_len - pattrib->hdrlen -
 196                                 pattrib->iv_len - pattrib->icv_len;
 197                         *((u32 *)crc) = cpu_to_le32(getcrc32(payload, length));
 198                         arcfour_init(&mycontext, wepkey, 3 + keylength);
 199                         arcfour_encrypt(&mycontext, payload, payload, length);
 200                         arcfour_encrypt(&mycontext, payload + length, crc, 4);
 201
 202                         pframe += pxmitpriv->frag_len;
 203                         pframe = PTR_ALIGN(pframe, 4);
 204                 }
 205         }
 206
 207 }
 208
 209 void rtw_wep_decrypt23a(struct rtw_adapter *padapter,
 210                      struct recv_frame *precvframe)
 211 {
 212         /*  exclude ICV */
 213         u8 crc[4];
 214         struct arc4context mycontext;
 215         int length;
 216         u32 keylength;
 217         u8 *pframe, *payload, *iv, wepkey[16];
 218         u8 keyindex;
 219         struct rx_pkt_attrib *prxattrib = &precvframe->attrib;
 220         struct security_priv *psecuritypriv = &padapter->securitypriv;
 221         struct sk_buff *skb = precvframe->pkt;
 222
 223         pframe = skb->data;
 224
 225         /* start to decrypt recvframe */
 226         if (prxattrib->encrypt != WLAN_CIPHER_SUITE_WEP40 &&
 227             prxattrib->encrypt != WLAN_CIPHER_SUITE_WEP104)
 228                 return;
 229
 230         iv = pframe + prxattrib->hdrlen;
 231         /* keyindex = (iv[3]&0x3); */
 232         keyindex = prxattrib->key_index;
 233         keylength = psecuritypriv->wep_key[keyindex].keylen;
 234         memcpy(&wepkey[0], iv, 3);
 235         /* memcpy(&wepkey[3], &psecuritypriv->dot11DefKey[psecuritypriv->dot11PrivacyKeyIndex].skey[0], keylength); */
 236         memcpy(&wepkey[3], &psecuritypriv->wep_key[keyindex].key, keylength);
 237         length = skb->len - prxattrib->hdrlen - prxattrib->iv_len;
 238
 239         payload = pframe + prxattrib->iv_len + prxattrib->hdrlen;
 240
 241         /* decrypt payload include icv */
 242         arcfour_init(&mycontext, wepkey, 3 + keylength);
 243         arcfour_encrypt(&mycontext, payload, payload, length);
 244
 245         /* calculate icv and compare the icv */
 246         *((u32 *)crc) = le32_to_cpu(getcrc32(payload, length - 4));
 247
 248         if (crc[3] != payload[length - 1] || crc[2] != payload[length - 2] ||
 249             crc[1] != payload[length - 3] || crc[0] != payload[length - 4]) {
 250                 RT_TRACE(_module_rtl871x_security_c_, _drv_err_,
 251                          ("rtw_wep_decrypt23a:icv error crc[3](%x)!= payload"
 252                           "[length-1](%x) || crc[2](%x)!= payload[length-2](%x)"
 253                           " || crc[1](%x)!= payload[length-3](%x) || crc[0](%x)"
 254                           "!= payload[length-4](%x)\n",
 255                           crc[3], payload[length - 1],
 256                           crc[2], payload[length - 2],
 257                           crc[1], payload[length - 3],
 258                           crc[0], payload[length - 4]));
 259         }
 260 }
 261
 262 /* 3            ===== TKIP related ===== */
 263
 264 static u32 secmicgetuint32(u8 *p)
 265 /*  Convert from Byte[] to u32 in a portable way */
 266 {
 267         s32 i;
 268         u32 res = 0;
 269
 270         for (i = 0; i<4; i++) {
 271                 res |= ((u32)(*p++)) << (8*i);
 272         }
 273
 274         return res;
 275 }
 276
 277 static void secmicputuint32(u8 *p, u32 val)
 278 /*  Convert from long to Byte[] in a portable way */
 279 {
 280         long i;
 281
 282         for (i = 0; i<4; i++) {
 283                 *p++ = (u8) (val & 0xff);
 284                 val >>= 8;
 285         }
 286
 287 }
 288
 289 static void secmicclear(struct mic_data *pmicdata)
 290 {
 291 /*  Reset the state to the empty message. */
 292
 293         pmicdata->L = pmicdata->K0;
 294         pmicdata->R = pmicdata->K1;
 295         pmicdata->nBytesInM = 0;
 296         pmicdata->M = 0;
 297
 298 }
 299
 300 void rtw_secmicsetkey23a(struct mic_data *pmicdata, u8 *key)
 301 {
 302         /*  Set the key */
 303
 304         pmicdata->K0 = secmicgetuint32(key);
 305         pmicdata->K1 = secmicgetuint32(key + 4);
 306         /*  and reset the message */
 307         secmicclear(pmicdata);
 308
 309 }
 310
 311 void rtw_secmicappend23abyte23a(struct mic_data *pmicdata, u8 b)
 312 {
 313
 314         /*  Append the byte to our word-sized buffer */
 315         pmicdata->M |= ((unsigned long)b) << (8*pmicdata->nBytesInM);
 316         pmicdata->nBytesInM++;
 317         /*  Process the word if it is full. */
 318         if (pmicdata->nBytesInM >= 4) {
 319                 pmicdata->L ^= pmicdata->M;
 320                 pmicdata->R ^= ROL32(pmicdata->L, 17);
 321                 pmicdata->L += pmicdata->R;
 322                 pmicdata->R ^= ((pmicdata->L & 0xff00ff00) >> 8) | ((pmicdata->L & 0x00ff00ff) << 8);
 323                 pmicdata->L += pmicdata->R;
 324                 pmicdata->R ^= ROL32(pmicdata->L, 3);
 325                 pmicdata->L += pmicdata->R;
 326                 pmicdata->R ^= ROR32(pmicdata->L, 2);
 327                 pmicdata->L += pmicdata->R;
 328                 /*  Clear the buffer */
 329                 pmicdata->M = 0;
 330                 pmicdata->nBytesInM = 0;
 331         }
 332
 333 }
 334
 335 void rtw_secmicappend23a(struct mic_data *pmicdata, u8 *src, u32 nbytes)
 336 {
 337
 338         /*  This is simple */
 339         while(nbytes > 0) {
 340                 rtw_secmicappend23abyte23a(pmicdata, *src++);
 341                 nbytes--;
 342         }
 343
 344 }
 345
 346 void rtw_secgetmic23a(struct mic_data *pmicdata, u8 *dst)
 347 {
 348
 349         /*  Append the minimum padding */
 350         rtw_secmicappend23abyte23a(pmicdata, 0x5a);
 351         rtw_secmicappend23abyte23a(pmicdata, 0);
 352         rtw_secmicappend23abyte23a(pmicdata, 0);
 353         rtw_secmicappend23abyte23a(pmicdata, 0);
 354         rtw_secmicappend23abyte23a(pmicdata, 0);
 355         /*  and then zeroes until the length is a multiple of 4 */
 356         while(pmicdata->nBytesInM != 0) {
 357                 rtw_secmicappend23abyte23a(pmicdata, 0);
 358         }
 359         /*  The appendByte function has already computed the result. */
 360         secmicputuint32(dst, pmicdata->L);
 361         secmicputuint32(dst+4, pmicdata->R);
 362         /*  Reset to the empty message. */
 363         secmicclear(pmicdata);
 364
 365 }
 366
 367 void rtw_seccalctkipmic23a(u8 *key, u8 *header, u8 *data, u32 data_len,
 368                            u8 *mic_code, u8 pri)
 369 {
 370
 371         struct mic_data micdata;
 372         u8 priority[4]={0x0, 0x0, 0x0, 0x0};
 373
 374         rtw_secmicsetkey23a(&micdata, key);
 375         priority[0]= pri;
 376
 377         /* Michael MIC pseudo header: DA, SA, 3 x 0, Priority */
 378         if (header[1]&1) {   /* ToDS == 1 */
 379                         rtw_secmicappend23a(&micdata, &header[16], 6);  /* DA */
 380                 if (header[1]&2)  /* From Ds == 1 */
 381                         rtw_secmicappend23a(&micdata, &header[24], 6);
 382                 else
 383                         rtw_secmicappend23a(&micdata, &header[10], 6);
 384         }
 385         else{   /* ToDS == 0 */
 386                 rtw_secmicappend23a(&micdata, &header[4], 6);   /* DA */
 387                 if (header[1]&2)  /* From Ds == 1 */
 388                         rtw_secmicappend23a(&micdata, &header[16], 6);
 389                 else
 390                         rtw_secmicappend23a(&micdata, &header[10], 6);
 391
 392         }
 393         rtw_secmicappend23a(&micdata, &priority[0], 4);
 394
 395         rtw_secmicappend23a(&micdata, data, data_len);
 396
 397         rtw_secgetmic23a(&micdata, mic_code);
 398
 399 }
 400
 401 /* macros for extraction/creation of unsigned char/unsigned short values  */
 402 #define RotR1(v16)   ((((v16) >> 1) & 0x7FFF) ^ (((v16) & 1) << 15))
 403 #define   Lo8(v16)   ((u8)((v16)       & 0x00FF))
 404 #define   Hi8(v16)   ((u8)(((v16) >> 8) & 0x00FF))
 405 #define  Lo16(v32)   ((u16)((v32)       & 0xFFFF))
 406 #define  Hi16(v32)   ((u16)(((v32) >>16) & 0xFFFF))
 407 #define  Mk16(hi, lo) ((lo) ^ (((u16)(hi)) << 8))
 408
 409 /* select the Nth 16-bit word of the temporal key unsigned char array TK[]   */
 410 #define  TK16(N)     Mk16(tk[2*(N)+1], tk[2*(N)])
 411
 412 /* S-box lookup: 16 bits --> 16 bits */
 413 #define _S_(v16)     (Sbox1[0][Lo8(v16)] ^ Sbox1[1][Hi8(v16)])
 414
 415 /* fixed algorithm "parameters" */
 416 #define PHASE1_LOOP_CNT   8    /* this needs to be "big enough"     */
 417 #define TA_SIZE           6    /*  48-bit transmitter address       */
 418 #define TK_SIZE          16    /* 128-bit temporal key              */
 419 #define P1K_SIZE         10    /*  80-bit Phase1 key                */
 420 #define RC4_KEY_SIZE     16    /* 128-bit RC4KEY (104 bits unknown) */
 421
 422 /* 2-unsigned char by 2-unsigned char subset of the full AES S-box table */
 423 static const unsigned short Sbox1[2][256]=       /* Sbox for hash (can be in ROM)     */
 424 { {
 425    0xC6A5, 0xF884, 0xEE99, 0xF68D, 0xFF0D, 0xD6BD, 0xDEB1, 0x9154,
 426    0x6050, 0x0203, 0xCEA9, 0x567D, 0xE719, 0xB562, 0x4DE6, 0xEC9A,
 427    0x8F45, 0x1F9D, 0x8940, 0xFA87, 0xEF15, 0xB2EB, 0x8EC9, 0xFB0B,
 428    0x41EC, 0xB367, 0x5FFD, 0x45EA, 0x23BF, 0x53F7, 0xE496, 0x9B5B,
 429    0x75C2, 0xE11C, 0x3DAE, 0x4C6A, 0x6C5A, 0x7E41, 0xF502, 0x834F,
 430    0x685C, 0x51F4, 0xD134, 0xF908, 0xE293, 0xAB73, 0x6253, 0x2A3F,
 431    0x080C, 0x9552, 0x4665, 0x9D5E, 0x3028, 0x37A1, 0x0A0F, 0x2FB5,
 432    0x0E09, 0x2436, 0x1B9B, 0xDF3D, 0xCD26, 0x4E69, 0x7FCD, 0xEA9F,
 433    0x121B, 0x1D9E, 0x5874, 0x342E, 0x362D, 0xDCB2, 0xB4EE, 0x5BFB,
 434    0xA4F6, 0x764D, 0xB761, 0x7DCE, 0x527B, 0xDD3E, 0x5E71, 0x1397,
 435    0xA6F5, 0xB968, 0x0000, 0xC12C, 0x4060, 0xE31F, 0x79C8, 0xB6ED,
 436    0xD4BE, 0x8D46, 0x67D9, 0x724B, 0x94DE, 0x98D4, 0xB0E8, 0x854A,
 437    0xBB6B, 0xC52A, 0x4FE5, 0xED16, 0x86C5, 0x9AD7, 0x6655, 0x1194,
 438    0x8ACF, 0xE910, 0x0406, 0xFE81, 0xA0F0, 0x7844, 0x25BA, 0x4BE3,
 439    0xA2F3, 0x5DFE, 0x80C0, 0x058A, 0x3FAD, 0x21BC, 0x7048, 0xF104,
 440    0x63DF, 0x77C1, 0xAF75, 0x4263, 0x2030, 0xE51A, 0xFD0E, 0xBF6D,
 441    0x814C, 0x1814, 0x2635, 0xC32F, 0xBEE1, 0x35A2, 0x88CC, 0x2E39,
 442    0x9357, 0x55F2, 0xFC82, 0x7A47, 0xC8AC, 0xBAE7, 0x322B, 0xE695,
 443    0xC0A0, 0x1998, 0x9ED1, 0xA37F, 0x4466, 0x547E, 0x3BAB, 0x0B83,
 444    0x8CCA, 0xC729, 0x6BD3, 0x283C, 0xA779, 0xBCE2, 0x161D, 0xAD76,
 445    0xDB3B, 0x6456, 0x744E, 0x141E, 0x92DB, 0x0C0A, 0x486C, 0xB8E4,
 446    0x9F5D, 0xBD6E, 0x43EF, 0xC4A6, 0x39A8, 0x31A4, 0xD337, 0xF28B,
 447    0xD532, 0x8B43, 0x6E59, 0xDAB7, 0x018C, 0xB164, 0x9CD2, 0x49E0,
 448    0xD8B4, 0xACFA, 0xF307, 0xCF25, 0xCAAF, 0xF48E, 0x47E9, 0x1018,
 449    0x6FD5, 0xF088, 0x4A6F, 0x5C72, 0x3824, 0x57F1, 0x73C7, 0x9751,
 450    0xCB23, 0xA17C, 0xE89C, 0x3E21, 0x96DD, 0x61DC, 0x0D86, 0x0F85,
 451    0xE090, 0x7C42, 0x71C4, 0xCCAA, 0x90D8, 0x0605, 0xF701, 0x1C12,
 452    0xC2A3, 0x6A5F, 0xAEF9, 0x69D0, 0x1791, 0x9958, 0x3A27, 0x27B9,
 453    0xD938, 0xEB13, 0x2BB3, 0x2233, 0xD2BB, 0xA970, 0x0789, 0x33A7,
 454    0x2DB6, 0x3C22, 0x1592, 0xC920, 0x8749, 0xAAFF, 0x5078, 0xA57A,
 455    0x038F, 0x59F8, 0x0980, 0x1A17, 0x65DA, 0xD731, 0x84C6, 0xD0B8,
 456    0x82C3, 0x29B0, 0x5A77, 0x1E11, 0x7BCB, 0xA8FC, 0x6DD6, 0x2C3A,
 457   },
 458
 459   {  /* second half of table is unsigned char-reversed version of first! */
 460    0xA5C6, 0x84F8, 0x99EE, 0x8DF6, 0x0DFF, 0xBDD6, 0xB1DE, 0x5491,
 461    0x5060, 0x0302, 0xA9CE, 0x7D56, 0x19E7, 0x62B5, 0xE64D, 0x9AEC,
 462    0x458F, 0x9D1F, 0x4089, 0x87FA, 0x15EF, 0xEBB2, 0xC98E, 0x0BFB,
 463    0xEC41, 0x67B3, 0xFD5F, 0xEA45, 0xBF23, 0xF753, 0x96E4, 0x5B9B,
 464    0xC275, 0x1CE1, 0xAE3D, 0x6A4C, 0x5A6C, 0x417E, 0x02F5, 0x4F83,
 465    0x5C68, 0xF451, 0x34D1, 0x08F9, 0x93E2, 0x73AB, 0x5362, 0x3F2A,
 466    0x0C08, 0x5295, 0x6546, 0x5E9D, 0x2830, 0xA137, 0x0F0A, 0xB52F,
 467    0x090E, 0x3624, 0x9B1B, 0x3DDF, 0x26CD, 0x694E, 0xCD7F, 0x9FEA,
 468    0x1B12, 0x9E1D, 0x7458, 0x2E34, 0x2D36, 0xB2DC, 0xEEB4, 0xFB5B,
 469    0xF6A4, 0x4D76, 0x61B7, 0xCE7D, 0x7B52, 0x3EDD, 0x715E, 0x9713,
 470    0xF5A6, 0x68B9, 0x0000, 0x2CC1, 0x6040, 0x1FE3, 0xC879, 0xEDB6,
 471    0xBED4, 0x468D, 0xD967, 0x4B72, 0xDE94, 0xD498, 0xE8B0, 0x4A85,
 472    0x6BBB, 0x2AC5, 0xE54F, 0x16ED, 0xC586, 0xD79A, 0x5566, 0x9411,
 473    0xCF8A, 0x10E9, 0x0604, 0x81FE, 0xF0A0, 0x4478, 0xBA25, 0xE34B,
 474    0xF3A2, 0xFE5D, 0xC080, 0x8A05, 0xAD3F, 0xBC21, 0x4870, 0x04F1,
 475    0xDF63, 0xC177, 0x75AF, 0x6342, 0x3020, 0x1AE5, 0x0EFD, 0x6DBF,
 476    0x4C81, 0x1418, 0x3526, 0x2FC3, 0xE1BE, 0xA235, 0xCC88, 0x392E,
 477    0x5793, 0xF255, 0x82FC, 0x477A, 0xACC8, 0xE7BA, 0x2B32, 0x95E6,
 478    0xA0C0, 0x9819, 0xD19E, 0x7FA3, 0x6644, 0x7E54, 0xAB3B, 0x830B,
 479    0xCA8C, 0x29C7, 0xD36B, 0x3C28, 0x79A7, 0xE2BC, 0x1D16, 0x76AD,
 480    0x3BDB, 0x5664, 0x4E74, 0x1E14, 0xDB92, 0x0A0C, 0x6C48, 0xE4B8,
 481    0x5D9F, 0x6EBD, 0xEF43, 0xA6C4, 0xA839, 0xA431, 0x37D3, 0x8BF2,
 482    0x32D5, 0x438B, 0x596E, 0xB7DA, 0x8C01, 0x64B1, 0xD29C, 0xE049,
 483    0xB4D8, 0xFAAC, 0x07F3, 0x25CF, 0xAFCA, 0x8EF4, 0xE947, 0x1810,
 484    0xD56F, 0x88F0, 0x6F4A, 0x725C, 0x2438, 0xF157, 0xC773, 0x5197,
 485    0x23CB, 0x7CA1, 0x9CE8, 0x213E, 0xDD96, 0xDC61, 0x860D, 0x850F,
 486    0x90E0, 0x427C, 0xC471, 0xAACC, 0xD890, 0x0506, 0x01F7, 0x121C,
 487    0xA3C2, 0x5F6A, 0xF9AE, 0xD069, 0x9117, 0x5899, 0x273A, 0xB927,
 488    0x38D9, 0x13EB, 0xB32B, 0x3322, 0xBBD2, 0x70A9, 0x8907, 0xA733,
 489    0xB62D, 0x223C, 0x9215, 0x20C9, 0x4987, 0xFFAA, 0x7850, 0x7AA5,
 490    0x8F03, 0xF859, 0x8009, 0x171A, 0xDA65, 0x31D7, 0xC684, 0xB8D0,
 491    0xC382, 0xB029, 0x775A, 0x111E, 0xCB7B, 0xFCA8, 0xD66D, 0x3A2C,
 492   }
 493 };
 494
 495  /*
 496 **********************************************************************
 497 * Routine: Phase 1 -- generate P1K, given TA, TK, IV32
 498 *
 499 * Inputs:
 500 *     tk[]      = temporal key                         [128 bits]
 501 *     ta[]      = transmitter's MAC address            [ 48 bits]
 502 *     iv32      = upper 32 bits of IV                  [ 32 bits]
 503 * Output:
 504 *     p1k[]     = Phase 1 key                          [ 80 bits]
 505 *
 506 * Note:
 507 *     This function only needs to be called every 2**16 packets,
 508 *     although in theory it could be called every packet.
 509 *
 510 **********************************************************************
 511 */
 512 static void phase1(u16 *p1k, const u8 *tk, const u8 *ta, u32 iv32)
 513 {
 514         int  i;
 515
 516         /* Initialize the 80 bits of P1K[] from IV32 and TA[0..5]     */
 517         p1k[0]      = Lo16(iv32);
 518         p1k[1]      = Hi16(iv32);
 519         p1k[2]      = Mk16(ta[1], ta[0]); /* use TA[] as little-endian */
 520         p1k[3]      = Mk16(ta[3], ta[2]);
 521         p1k[4]      = Mk16(ta[5], ta[4]);
 522
 523         /* Now compute an unbalanced Feistel cipher with 80-bit block */
 524         /* size on the 80-bit block P1K[], using the 128-bit key TK[] */
 525         for (i = 0; i < PHASE1_LOOP_CNT ;i++) {
 526                 /* Each add operation here is mod 2**16 */
 527                 p1k[0] += _S_(p1k[4] ^ TK16((i&1)+0));
 528                 p1k[1] += _S_(p1k[0] ^ TK16((i&1)+2));
 529                 p1k[2] += _S_(p1k[1] ^ TK16((i&1)+4));
 530                 p1k[3] += _S_(p1k[2] ^ TK16((i&1)+6));
 531                 p1k[4] += _S_(p1k[3] ^ TK16((i&1)+0));
 532                 p1k[4] +=  (unsigned short)i;                    /* avoid "slide attacks" */
 533                 }
 534
 535 }
 536
 537 /*
 538 **********************************************************************
 539 * Routine: Phase 2 -- generate RC4KEY, given TK, P1K, IV16
 540 *
 541 * Inputs:
 542 *     tk[]      = Temporal key                         [128 bits]
 543 *     p1k[]     = Phase 1 output key                   [ 80 bits]
 544 *     iv16      = low 16 bits of IV counter            [ 16 bits]
 545 * Output:
 546 *     rc4key[]  = the key used to encrypt the packet   [128 bits]
 547 *
 548 * Note:
 549 *     The value {TA, IV32, IV16} for Phase1/Phase2 must be unique
 550 *     across all packets using the same key TK value. Then, for a
 551 *     given value of TK[], this TKIP48 construction guarantees that
 552 *     the final RC4KEY value is unique across all packets.
 553 *
 554 * Suggested implementation optimization: if PPK[] is "overlaid"
 555 *     appropriately on RC4KEY[], there is no need for the final
 556 *     for loop below that copies the PPK[] result into RC4KEY[].
 557 *
 558 **********************************************************************
 559 */
 560 static void phase2(u8 *rc4key, const u8 *tk, const u16 *p1k, u16 iv16)
 561 {
 562         int  i;
 563         u16 PPK[6];                          /* temporary key for mixing    */
 564
 565         /* Note: all adds in the PPK[] equations below are mod 2**16         */
 566         for (i = 0;i<5;i++) PPK[i]= p1k[i];      /* first, copy P1K to PPK      */
 567                 PPK[5]  =  p1k[4] +iv16;             /* next,  add in IV16          */
 568
 569         /* Bijective non-linear mixing of the 96 bits of PPK[0..5]           */
 570         PPK[0] +=    _S_(PPK[5] ^ TK16(0));   /* Mix key in each "round"     */
 571         PPK[1] +=    _S_(PPK[0] ^ TK16(1));
 572         PPK[2] +=    _S_(PPK[1] ^ TK16(2));
 573         PPK[3] +=    _S_(PPK[2] ^ TK16(3));
 574         PPK[4] +=    _S_(PPK[3] ^ TK16(4));
 575         PPK[5] +=    _S_(PPK[4] ^ TK16(5));   /* Total # S-box lookups == 6  */
 576
 577         /* Final sweep: bijective, "linear". Rotates kill LSB correlations   */
 578         PPK[0] +=  RotR1(PPK[5] ^ TK16(6));
 579         PPK[1] +=  RotR1(PPK[0] ^ TK16(7));   /* Use all of TK[] in Phase2   */
 580         PPK[2] +=  RotR1(PPK[1]);
 581         PPK[3] +=  RotR1(PPK[2]);
 582         PPK[4] +=  RotR1(PPK[3]);
 583         PPK[5] +=  RotR1(PPK[4]);
 584         /* Note: At this point, for a given key TK[0..15], the 96-bit output */
 585         /*       value PPK[0..5] is guaranteed to be unique, as a function   */
 586         /*       of the 96-bit "input" value   {TA, IV32, IV16}. That is, P1K  */
 587         /*       is now a keyed permutation of {TA, IV32, IV16}.               */
 588
 589         /* Set RC4KEY[0..3], which includes "cleartext" portion of RC4 key   */
 590         rc4key[0] = Hi8(iv16);                /* RC4KEY[0..2] is the WEP IV  */
 591         rc4key[1] = (Hi8(iv16) | 0x20) & 0x7F; /* Help avoid weak (FMS) keys  */
 592         rc4key[2] = Lo8(iv16);
 593         rc4key[3] = Lo8((PPK[5] ^ TK16(0)) >> 1);
 594
 595         /* Copy 96 bits of PPK[0..5] to RC4KEY[4..15]  (little-endian)       */
 596         for (i = 0;i<6;i++) {
 597                 rc4key[4+2*i] = Lo8(PPK[i]);
 598                 rc4key[5+2*i] = Hi8(PPK[i]);
 599         }
 600
 601 }
 602
 603 /* The hlen isn't include the IV */
 604 int rtw_tkip_encrypt23a(struct rtw_adapter *padapter,
 605                         struct xmit_frame *pxmitframe)
 606 {
 607         u16     pnl;
 608         u32     pnh;
 609         u8      rc4key[16];
 610         u8   ttkey[16];
 611         u8      crc[4];
 612         u8   hw_hdr_offset = 0;
 613         struct arc4context mycontext;
 614         int                     curfragnum, length;
 615         u32     prwskeylen;
 616         u8      *pframe, *payload, *iv, *prwskey;
 617         union pn48 dot11txpn;
 618         struct  sta_info                *stainfo;
 619         struct  pkt_attrib       *pattrib = &pxmitframe->attrib;
 620         struct  security_priv   *psecuritypriv = &padapter->securitypriv;
 621         struct  xmit_priv               *pxmitpriv = &padapter->xmitpriv;
 622         int res = _SUCCESS;
 623
 624         if (!pxmitframe->buf_addr)
 625                 return _FAIL;
 626
 627         hw_hdr_offset = TXDESC_OFFSET;
 628
 629         pframe = pxmitframe->buf_addr + hw_hdr_offset;
 630         /* 4 start to encrypt each fragment */
 631         if (pattrib->encrypt == WLAN_CIPHER_SUITE_TKIP) {
 632                 if (pattrib->psta)
 633                         stainfo = pattrib->psta;
 634                 else {
 635                         DBG_8723A("%s, call rtw_get_stainfo()\n", __func__);
 636                         stainfo = rtw_get_stainfo23a(&padapter->stapriv,
 637                                                      &pattrib->ra[0]);
 638                 }
 639
 640                 if (stainfo!= NULL) {
 641
 642                         if (!(stainfo->state &_FW_LINKED)) {
 643                                 DBG_8723A("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, stainfo->state);
 644                                 return _FAIL;
 645                         }
 646
 647                         RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_tkip_encrypt23a: stainfo!= NULL!!!\n"));
 648
 649                         if (is_multicast_ether_addr(pattrib->ra))
 650                                 prwskey = psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey;
 651                         else
 652                                 prwskey = &stainfo->dot118021x_UncstKey.skey[0];
 653
 654                         prwskeylen = 16;
 655
 656                         for (curfragnum = 0;curfragnum<pattrib->nr_frags;curfragnum++) {
 657                                 iv = pframe+pattrib->hdrlen;
 658                                 payload = pframe+pattrib->iv_len+pattrib->hdrlen;
 659
 660                                 GET_TKIP_PN(iv, dot11txpn);
 661
 662                                 pnl = (u16)(dot11txpn.val);
 663                                 pnh = (u32)(dot11txpn.val>>16);
 664
 665                                 phase1((u16 *)&ttkey[0], prwskey,&pattrib->ta[0], pnh);
 666
 667                                 phase2(&rc4key[0], prwskey, (u16 *)&ttkey[0], pnl);
 668
 669                                 if ((curfragnum+1) == pattrib->nr_frags) {      /* 4 the last fragment */
 670                                         length = pattrib->last_txcmdsz-pattrib->hdrlen-pattrib->iv_len- pattrib->icv_len;
 671                                         RT_TRACE(_module_rtl871x_security_c_, _drv_info_, ("pattrib->iv_len =%x, pattrib->icv_len =%x\n", pattrib->iv_len, pattrib->icv_len));
 672                                         *((u32 *)crc) = cpu_to_le32(getcrc32(payload, length));/* modified by Amy*/
 673
 674                                         arcfour_init(&mycontext, rc4key, 16);
 675                                         arcfour_encrypt(&mycontext, payload, payload, length);
 676                                         arcfour_encrypt(&mycontext, payload+length, crc, 4);
 677
 678                                 }
 679                                 else{
 680                                         length = pxmitpriv->frag_len-pattrib->hdrlen-pattrib->iv_len-pattrib->icv_len ;
 681                                         *((u32 *)crc) = cpu_to_le32(getcrc32(payload, length));/* modified by Amy*/
 682                                         arcfour_init(&mycontext, rc4key, 16);
 683                                         arcfour_encrypt(&mycontext, payload, payload, length);
 684                                         arcfour_encrypt(&mycontext, payload+length, crc, 4);
 685
 686                                 pframe+= pxmitpriv->frag_len;
 687                                 pframe = PTR_ALIGN(pframe, 4);
 688                                 }
 689                         }
 690
 691                 }
 692                 else{
 693                         RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_tkip_encrypt23a: stainfo == NULL!!!\n"));
 694                         DBG_8723A("%s, psta == NUL\n", __func__);
 695                         res = _FAIL;
 696                 }
 697
 698         }
 699
 700         return res;
 701 }
 702
 703 /* The hlen isn't include the IV */
 704 int rtw_tkip_decrypt23a(struct rtw_adapter *padapter,
 705                         struct recv_frame *precvframe)
 706 {
 707         u16 pnl;
 708         u32 pnh;
 709         u8   rc4key[16];
 710         u8   ttkey[16];
 711         u8      crc[4];
 712         struct arc4context mycontext;
 713         int     length;
 714         u32     prwskeylen;
 715         u8      *pframe, *payload, *iv, *prwskey;
 716         union pn48 dot11txpn;
 717         struct  sta_info                *stainfo;
 718         struct  rx_pkt_attrib *prxattrib = &precvframe->attrib;
 719         struct  security_priv *psecuritypriv = &padapter->securitypriv;
 720         struct sk_buff *skb = precvframe->pkt;
 721         int res = _SUCCESS;
 722
 723         pframe = skb->data;
 724
 725         /* 4 start to decrypt recvframe */
 726         if (prxattrib->encrypt == WLAN_CIPHER_SUITE_TKIP) {
 727
 728                 stainfo = rtw_get_stainfo23a(&padapter->stapriv,
 729                                              &prxattrib->ta[0]);
 730                 if (stainfo!= NULL) {
 731
 732                         if (is_multicast_ether_addr(prxattrib->ra)) {
 733                                 if (psecuritypriv->binstallGrpkey == 0) {
 734                                         res = _FAIL;
 735                                         DBG_8723A("%s:rx bc/mc packets, but didn't install group key!!!!!!!!!!\n", __func__);
 736                                         goto exit;
 737                                 }
 738                                 prwskey = psecuritypriv->dot118021XGrpKey[prxattrib->key_index].skey;
 739                                 prwskeylen = 16;
 740                         } else {
 741                                 RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_tkip_decrypt23a: stainfo!= NULL!!!\n"));
 742                                 prwskey = &stainfo->dot118021x_UncstKey.skey[0];
 743                                 prwskeylen = 16;
 744                         }
 745
 746                         iv = pframe+prxattrib->hdrlen;
 747                         payload = pframe+prxattrib->iv_len+prxattrib->hdrlen;
 748                         length = skb->len - prxattrib->hdrlen-prxattrib->iv_len;
 749
 750                         GET_TKIP_PN(iv, dot11txpn);
 751
 752                         pnl = (u16)(dot11txpn.val);
 753                         pnh = (u32)(dot11txpn.val>>16);
 754
 755                         phase1((u16 *)&ttkey[0], prwskey,&prxattrib->ta[0], pnh);
 756                         phase2(&rc4key[0], prwskey, (unsigned short *)&ttkey[0], pnl);
 757
 758                         /* 4 decrypt payload include icv */
 759                         arcfour_init(&mycontext, rc4key, 16);
 760                         arcfour_encrypt(&mycontext, payload, payload, length);
 761
 762                         *((u32 *)crc) = le32_to_cpu(getcrc32(payload, length-4));
 763
 764                         if (crc[3]!= payload[length-1] || crc[2]!= payload[length-2] || crc[1]!= payload[length-3] || crc[0]!= payload[length-4])
 765                         {
 766                             RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_wep_decrypt23a:icv error crc[3](%x)!= payload[length-1](%x) || crc[2](%x)!= payload[length-2](%x) || crc[1](%x)!= payload[length-3](%x) || crc[0](%x)!= payload[length-4](%x)\n",
 767                                                 crc[3], payload[length-1], crc[2], payload[length-2], crc[1], payload[length-3], crc[0], payload[length-4]));
 768                                 res = _FAIL;
 769                         }
 770                 } else {
 771                         RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_tkip_decrypt23a: stainfo == NULL!!!\n"));
 772                         res = _FAIL;
 773                 }
 774         }
 775 exit:
 776         return res;
 777 }
 778
 779 /* 3                    ===== AES related ===== */
 780
 781 #define MAX_MSG_SIZE    2048
 782 /*****************************/
 783 /******** SBOX Table *********/
 784 /*****************************/
 785
 786 static  u8 sbox_table[256] = {
 787         0x63, 0x7c, 0x77, 0x7b, 0xf2, 0x6b, 0x6f, 0xc5,
 788         0x30, 0x01, 0x67, 0x2b, 0xfe, 0xd7, 0xab, 0x76,
 789         0xca, 0x82, 0xc9, 0x7d, 0xfa, 0x59, 0x47, 0xf0,
 790         0xad, 0xd4, 0xa2, 0xaf, 0x9c, 0xa4, 0x72, 0xc0,
 791         0xb7, 0xfd, 0x93, 0x26, 0x36, 0x3f, 0xf7, 0xcc,
 792         0x34, 0xa5, 0xe5, 0xf1, 0x71, 0xd8, 0x31, 0x15,
 793         0x04, 0xc7, 0x23, 0xc3, 0x18, 0x96, 0x05, 0x9a,
 794         0x07, 0x12, 0x80, 0xe2, 0xeb, 0x27, 0xb2, 0x75,
 795         0x09, 0x83, 0x2c, 0x1a, 0x1b, 0x6e, 0x5a, 0xa0,
 796         0x52, 0x3b, 0xd6, 0xb3, 0x29, 0xe3, 0x2f, 0x84,
 797         0x53, 0xd1, 0x00, 0xed, 0x20, 0xfc, 0xb1, 0x5b,
 798         0x6a, 0xcb, 0xbe, 0x39, 0x4a, 0x4c, 0x58, 0xcf,
 799         0xd0, 0xef, 0xaa, 0xfb, 0x43, 0x4d, 0x33, 0x85,
 800         0x45, 0xf9, 0x02, 0x7f, 0x50, 0x3c, 0x9f, 0xa8,
 801         0x51, 0xa3, 0x40, 0x8f, 0x92, 0x9d, 0x38, 0xf5,
 802         0xbc, 0xb6, 0xda, 0x21, 0x10, 0xff, 0xf3, 0xd2,
 803         0xcd, 0x0c, 0x13, 0xec, 0x5f, 0x97, 0x44, 0x17,
 804         0xc4, 0xa7, 0x7e, 0x3d, 0x64, 0x5d, 0x19, 0x73,
 805         0x60, 0x81, 0x4f, 0xdc, 0x22, 0x2a, 0x90, 0x88,
 806         0x46, 0xee, 0xb8, 0x14, 0xde, 0x5e, 0x0b, 0xdb,
 807         0xe0, 0x32, 0x3a, 0x0a, 0x49, 0x06, 0x24, 0x5c,
 808         0xc2, 0xd3, 0xac, 0x62, 0x91, 0x95, 0xe4, 0x79,
 809         0xe7, 0xc8, 0x37, 0x6d, 0x8d, 0xd5, 0x4e, 0xa9,
 810         0x6c, 0x56, 0xf4, 0xea, 0x65, 0x7a, 0xae, 0x08,
 811         0xba, 0x78, 0x25, 0x2e, 0x1c, 0xa6, 0xb4, 0xc6,
 812         0xe8, 0xdd, 0x74, 0x1f, 0x4b, 0xbd, 0x8b, 0x8a,
 813         0x70, 0x3e, 0xb5, 0x66, 0x48, 0x03, 0xf6, 0x0e,
 814         0x61, 0x35, 0x57, 0xb9, 0x86, 0xc1, 0x1d, 0x9e,
 815         0xe1, 0xf8, 0x98, 0x11, 0x69, 0xd9, 0x8e, 0x94,
 816         0x9b, 0x1e, 0x87, 0xe9, 0xce, 0x55, 0x28, 0xdf,
 817         0x8c, 0xa1, 0x89, 0x0d, 0xbf, 0xe6, 0x42, 0x68,
 818         0x41, 0x99, 0x2d, 0x0f, 0xb0, 0x54, 0xbb, 0x16
 819 };
 820
 821 /*****************************/
 822 /**** Function Prototypes ****/
 823 /*****************************/
 824
 825 static void construct_mic_header2(u8 *mic_header2, u8 *mpdu, int a4_exists,
 826                                   int qc_exists);
 827
 828 static void xor_128(u8 *a, u8 *b, u8 *out)
 829 {
 830         int i;
 831
 832         for (i = 0;i<16; i++)
 833                 out[i] = a[i] ^ b[i];
 834 }
 835
 836 static void xor_32(u8 *a, u8 *b, u8 *out)
 837 {
 838         int i;
 839
 840         for (i = 0; i < 4; i++)
 841                 out[i] = a[i] ^ b[i];
 842 }
 843
 844 static u8 sbox(u8 a)
 845 {
 846         return sbox_table[(int)a];
 847 }
 848
 849 static void next_key(u8 *key, int round)
 850 {
 851         u8 rcon;
 852         u8 sbox_key[4];
 853         u8 rcon_table[12] =
 854         {
 855                 0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80,
 856                 0x1b, 0x36, 0x36, 0x36
 857         };
 858
 859         sbox_key[0] = sbox(key[13]);
 860         sbox_key[1] = sbox(key[14]);
 861         sbox_key[2] = sbox(key[15]);
 862         sbox_key[3] = sbox(key[12]);
 863
 864         rcon = rcon_table[round];
 865
 866         xor_32(&key[0], sbox_key, &key[0]);
 867         key[0] = key[0] ^ rcon;
 868
 869         xor_32(&key[4], &key[0], &key[4]);
 870         xor_32(&key[8], &key[4], &key[8]);
 871         xor_32(&key[12], &key[8], &key[12]);
 872
 873 }
 874
 875 static void byte_sub(u8 *in, u8 *out)
 876 {
 877         int i;
 878
 879         for (i = 0; i< 16; i++) {
 880                 out[i] = sbox(in[i]);
 881         }
 882
 883 }
 884
 885 static void shift_row(u8 *in, u8 *out)
 886 {
 887
 888         out[0] =  in[0];
 889         out[1] =  in[5];
 890         out[2] =  in[10];
 891         out[3] =  in[15];
 892         out[4] =  in[4];
 893         out[5] =  in[9];
 894         out[6] =  in[14];
 895         out[7] =  in[3];
 896         out[8] =  in[8];
 897         out[9] =  in[13];
 898         out[10] = in[2];
 899         out[11] = in[7];
 900         out[12] = in[12];
 901         out[13] = in[1];
 902         out[14] = in[6];
 903         out[15] = in[11];
 904
 905 }
 906
 907 static void mix_column(u8 *in, u8 *out)
 908 {
 909         int i;
 910         u8 add1b[4];
 911         u8 add1bf7[4];
 912         u8 rotl[4];
 913         u8 swap_halfs[4];
 914         u8 andf7[4];
 915         u8 rotr[4];
 916         u8 temp[4];
 917         u8 tempb[4];
 918
 919         for (i = 0 ; i<4; i++) {
 920                 if ((in[i] & 0x80) == 0x80)
 921                     add1b[i] = 0x1b;
 922                 else
 923                     add1b[i] = 0x00;
 924         }
 925
 926         swap_halfs[0] = in[2];    /* Swap halfs */
 927         swap_halfs[1] = in[3];
 928         swap_halfs[2] = in[0];
 929         swap_halfs[3] = in[1];
 930
 931         rotl[0] = in[3];        /* Rotate left 8 bits */
 932         rotl[1] = in[0];
 933         rotl[2] = in[1];
 934         rotl[3] = in[2];
 935
 936         andf7[0] = in[0] & 0x7f;
 937         andf7[1] = in[1] & 0x7f;
 938         andf7[2] = in[2] & 0x7f;
 939         andf7[3] = in[3] & 0x7f;
 940
 941         for (i = 3; i>0; i--) { /* logical shift left 1 bit */
 942                 andf7[i] = andf7[i] << 1;
 943                 if ((andf7[i-1] & 0x80) == 0x80) {
 944                     andf7[i] = (andf7[i] | 0x01);
 945                 }
 946         }
 947         andf7[0] = andf7[0] << 1;
 948         andf7[0] = andf7[0] & 0xfe;
 949
 950         xor_32(add1b, andf7, add1bf7);
 951
 952         xor_32(in, add1bf7, rotr);
 953
 954         temp[0] = rotr[0];         /* Rotate right 8 bits */
 955         rotr[0] = rotr[1];
 956         rotr[1] = rotr[2];
 957         rotr[2] = rotr[3];
 958         rotr[3] = temp[0];
 959
 960         xor_32(add1bf7, rotr, temp);
 961         xor_32(swap_halfs, rotl, tempb);
 962         xor_32(temp, tempb, out);
 963
 964 }
 965
 966 static void aes128k128d(u8 *key, u8 *data, u8 *ciphertext)
 967 {
 968         int round;
 969         int i;
 970         u8 intermediatea[16];
 971         u8 intermediateb[16];
 972         u8 round_key[16];
 973
 974         for (i = 0; i<16; i++) round_key[i] = key[i];
 975
 976         for (round = 0; round < 11; round++) {
 977                 if (round == 0) {
 978                     xor_128(round_key, data, ciphertext);
 979                     next_key(round_key, round);
 980                 } else if (round == 10) {
 981                     byte_sub(ciphertext, intermediatea);
 982                     shift_row(intermediatea, intermediateb);
 983                     xor_128(intermediateb, round_key, ciphertext);
 984                 } else { /* 1 - 9 */
 985                     byte_sub(ciphertext, intermediatea);
 986                     shift_row(intermediatea, intermediateb);
 987                     mix_column(&intermediateb[0], &intermediatea[0]);
 988                     mix_column(&intermediateb[4], &intermediatea[4]);
 989                     mix_column(&intermediateb[8], &intermediatea[8]);
 990                     mix_column(&intermediateb[12], &intermediatea[12]);
 991                     xor_128(intermediatea, round_key, ciphertext);
 992                     next_key(round_key, round);
 993                 }
 994         }
 995
 996 }
 997
 998 /************************************************/
 999 /* construct_mic_iv()                           */
1000 /* Builds the MIC IV from header fields and PN  */
1001 /************************************************/
1002 static void construct_mic_iv(u8 *mic_iv, int qc_exists, int a4_exists, u8 *mpdu,
1003                              uint payload_length, u8 *pn_vector)
1004 {
1005         int i;
1006
1007         mic_iv[0] = 0x59;
1008         if (qc_exists && a4_exists)
1009                 mic_iv[1] = mpdu[30] & 0x0f;    /* QoS_TC           */
1010         if (qc_exists && !a4_exists)
1011                 mic_iv[1] = mpdu[24] & 0x0f;   /* mute bits 7-4    */
1012         if (!qc_exists)
1013                 mic_iv[1] = 0x00;
1014         for (i = 2; i < 8; i++)
1015                 mic_iv[i] = mpdu[i + 8]; /* mic_iv[2:7] = A2[0:5] = mpdu[10:15] */
1016         for (i = 8; i < 14; i++)
1017                 mic_iv[i] = pn_vector[13 - i]; /* mic_iv[8:13] = PN[5:0] */
1018         mic_iv[14] = (unsigned char)(payload_length / 256);
1019         mic_iv[15] = (unsigned char)(payload_length % 256);
1020 }
1021
1022 /************************************************/
1023 /* construct_mic_header1()                      */
1024 /* Builds the first MIC header block from       */
1025 /* header fields.                               */
1026 /************************************************/
1027 static void construct_mic_header1(u8 *mic_header1, int header_length, u8 *mpdu)
1028 {
1029         mic_header1[0] = (u8)((header_length - 2) / 256);
1030         mic_header1[1] = (u8)((header_length - 2) % 256);
1031         mic_header1[2] = mpdu[0] & 0xcf;    /* Mute CF poll & CF ack bits */
1032         mic_header1[3] = mpdu[1] & 0xc7;    /* Mute retry, more data and pwr mgt bits */
1033         mic_header1[4] = mpdu[4];       /* A1 */
1034         mic_header1[5] = mpdu[5];
1035         mic_header1[6] = mpdu[6];
1036         mic_header1[7] = mpdu[7];
1037         mic_header1[8] = mpdu[8];
1038         mic_header1[9] = mpdu[9];
1039         mic_header1[10] = mpdu[10];     /* A2 */
1040         mic_header1[11] = mpdu[11];
1041         mic_header1[12] = mpdu[12];
1042         mic_header1[13] = mpdu[13];
1043         mic_header1[14] = mpdu[14];
1044         mic_header1[15] = mpdu[15];
1045
1046 }
1047
1048 /************************************************/
1049         /* construct_mic_header2()                      */
1050 /* Builds the last MIC header block from        */
1051 /* header fields.                               */
1052 /************************************************/
1053 static void construct_mic_header2(u8 *mic_header2, u8 *mpdu, int a4_exists,
1054                                   int qc_exists)
1055 {
1056         int i;
1057
1058         for (i = 0; i<16; i++) mic_header2[i]= 0x00;
1059
1060         mic_header2[0] = mpdu[16];    /* A3 */
1061         mic_header2[1] = mpdu[17];
1062         mic_header2[2] = mpdu[18];
1063         mic_header2[3] = mpdu[19];
1064         mic_header2[4] = mpdu[20];
1065         mic_header2[5] = mpdu[21];
1066
1067         mic_header2[6] = 0x00;
1068         mic_header2[7] = 0x00; /* mpdu[23]; */
1069
1070         if (!qc_exists && a4_exists) {
1071                 for (i = 0;i<6;i++) mic_header2[8+i] = mpdu[24+i];   /* A4 */
1072
1073         }
1074
1075         if (qc_exists && !a4_exists) {
1076                 mic_header2[8] = mpdu[24] & 0x0f; /* mute bits 15 - 4 */
1077                 mic_header2[9] = mpdu[25] & 0x00;
1078         }
1079
1080         if (qc_exists && a4_exists) {
1081                 for (i = 0;i<6;i++) mic_header2[8+i] = mpdu[24+i];   /* A4 */
1082
1083                 mic_header2[14] = mpdu[30] & 0x0f;
1084                 mic_header2[15] = mpdu[31] & 0x00;
1085         }
1086
1087 }
1088
1089 /************************************************/
1090 /* construct_mic_header2()                      */
1091 /* Builds the last MIC header block from        */
1092 /* header fields.                               */
1093 /************************************************/
1094 static void construct_ctr_preload(u8 *ctr_preload, int a4_exists, int qc_exists,
1095                                   u8 *mpdu, u8 *pn_vector, int c)
1096 {
1097         int i = 0;
1098
1099         for (i = 0; i<16; i++) ctr_preload[i] = 0x00;
1100         i = 0;
1101
1102         ctr_preload[0] = 0x01;                                  /* flag */
1103         if (qc_exists && a4_exists)
1104                 ctr_preload[1] = mpdu[30] & 0x0f;   /* QoC_Control */
1105         if (qc_exists && !a4_exists)
1106                 ctr_preload[1] = mpdu[24] & 0x0f;
1107
1108         for (i = 2; i < 8; i++)
1109                 ctr_preload[i] = mpdu[i + 8];                       /* ctr_preload[2:7] = A2[0:5] = mpdu[10:15] */
1110         for (i = 8; i < 14; i++)
1111                 ctr_preload[i] =    pn_vector[13 - i];          /* ctr_preload[8:13] = PN[5:0] */
1112         ctr_preload[14] =  (unsigned char) (c / 256); /* Ctr */
1113         ctr_preload[15] =  (unsigned char) (c % 256);
1114
1115 }
1116
1117 /************************************/
1118 /* bitwise_xor()                    */
1119 /* A 128 bit, bitwise exclusive or  */
1120 /************************************/
1121 static void bitwise_xor(u8 *ina, u8 *inb, u8 *out)
1122 {
1123         int i;
1124
1125         for (i = 0; i < 16; i++)
1126                 out[i] = ina[i] ^ inb[i];
1127 }
1128
1129 static int aes_cipher(u8 *key, uint hdrlen, u8 *pframe, uint plen)
1130 {
1131         uint    qc_exists, a4_exists, i, j, payload_remainder,
1132                 num_blocks, payload_index;
1133         u8 pn_vector[6];
1134         u8 mic_iv[16];
1135         u8 mic_header1[16];
1136         u8 mic_header2[16];
1137         u8 ctr_preload[16];
1138         /* Intermediate Buffers */
1139         u8 chain_buffer[16];
1140         u8 aes_out[16];
1141         u8 padded_buffer[16];
1142         u8 mic[8];
1143         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)pframe;
1144         u16 frsubtype = le16_to_cpu(hdr->frame_control) & IEEE80211_FCTL_STYPE;
1145
1146         memset((void *)mic_iv, 0, 16);
1147         memset((void *)mic_header1, 0, 16);
1148         memset((void *)mic_header2, 0, 16);
1149         memset((void *)ctr_preload, 0, 16);
1150         memset((void *)chain_buffer, 0, 16);
1151         memset((void *)aes_out, 0, 16);
1152         memset((void *)padded_buffer, 0, 16);
1153
1154         if ((hdrlen == sizeof(struct ieee80211_hdr_3addr) ||
1155             (hdrlen == sizeof(struct ieee80211_qos_hdr))))
1156                 a4_exists = 0;
1157         else
1158                 a4_exists = 1;
1159
1160         if (ieee80211_is_data(hdr->frame_control)) {
1161                 if ((frsubtype == IEEE80211_STYPE_DATA_CFACK) ||
1162                     (frsubtype == IEEE80211_STYPE_DATA_CFPOLL) ||
1163                     (frsubtype == IEEE80211_STYPE_DATA_CFACKPOLL)) {
1164                         qc_exists = 1;
1165                         if (hdrlen != sizeof(struct ieee80211_qos_hdr))
1166                                 hdrlen += 2;
1167                 } else if ((frsubtype == IEEE80211_STYPE_QOS_DATA) ||
1168                            (frsubtype == IEEE80211_STYPE_QOS_DATA_CFACK) ||
1169                            (frsubtype == IEEE80211_STYPE_QOS_DATA_CFPOLL) ||
1170                            (frsubtype == IEEE80211_STYPE_QOS_DATA_CFACKPOLL)) {
1171                         if (hdrlen != sizeof(struct ieee80211_qos_hdr))
1172                                 hdrlen += 2;
1173                         qc_exists = 1;
1174                 } else {
1175                         qc_exists = 0;
1176                 }
1177         } else {
1178                 qc_exists = 0;
1179         }
1180         pn_vector[0]= pframe[hdrlen];
1181         pn_vector[1]= pframe[hdrlen+1];
1182         pn_vector[2]= pframe[hdrlen+4];
1183         pn_vector[3]= pframe[hdrlen+5];
1184         pn_vector[4]= pframe[hdrlen+6];
1185         pn_vector[5]= pframe[hdrlen+7];
1186
1187         construct_mic_iv(mic_iv, qc_exists, a4_exists, pframe, plen, pn_vector);
1188
1189         construct_mic_header1(mic_header1, hdrlen, pframe);
1190         construct_mic_header2(mic_header2, pframe, a4_exists, qc_exists);
1191
1192         payload_remainder = plen % 16;
1193         num_blocks = plen / 16;
1194
1195         /* Find start of payload */
1196         payload_index = (hdrlen + 8);
1197
1198         /* Calculate MIC */
1199         aes128k128d(key, mic_iv, aes_out);
1200         bitwise_xor(aes_out, mic_header1, chain_buffer);
1201         aes128k128d(key, chain_buffer, aes_out);
1202         bitwise_xor(aes_out, mic_header2, chain_buffer);
1203         aes128k128d(key, chain_buffer, aes_out);
1204
1205         for (i = 0; i < num_blocks; i++) {
1206                 bitwise_xor(aes_out, &pframe[payload_index], chain_buffer);
1207
1208                 payload_index += 16;
1209                 aes128k128d(key, chain_buffer, aes_out);
1210         }
1211
1212         /* Add on the final payload block if it needs padding */
1213         if (payload_remainder > 0) {
1214                 for (j = 0; j < 16; j++)
1215                         padded_buffer[j] = 0x00;
1216                 for (j = 0; j < payload_remainder; j++)
1217                         padded_buffer[j] = pframe[payload_index++];
1218                 bitwise_xor(aes_out, padded_buffer, chain_buffer);
1219                 aes128k128d(key, chain_buffer, aes_out);
1220         }
1221
1222         for (j = 0; j < 8; j++)
1223                 mic[j] = aes_out[j];
1224
1225         /* Insert MIC into payload */
1226         for (j = 0; j < 8; j++)
1227                 pframe[payload_index+j] = mic[j];
1228
1229         payload_index = hdrlen + 8;
1230         for (i = 0; i < num_blocks; i++) {
1231                 construct_ctr_preload(ctr_preload, a4_exists, qc_exists,
1232                                       pframe, pn_vector, i+1);
1233                 aes128k128d(key, ctr_preload, aes_out);
1234                 bitwise_xor(aes_out, &pframe[payload_index], chain_buffer);
1235                 for (j = 0; j < 16; j++)
1236                         pframe[payload_index++] = chain_buffer[j];
1237         }
1238
1239         if (payload_remainder > 0) {
1240                 /* If there is a short final block, then pad it,
1241                  * encrypt it and copy the unpadded part back
1242                  */
1243                 construct_ctr_preload(ctr_preload, a4_exists, qc_exists, pframe,
1244                                       pn_vector, num_blocks+1);
1245
1246                 for (j = 0; j < 16; j++)
1247                         padded_buffer[j] = 0x00;
1248                 for (j = 0; j < payload_remainder; j++)
1249                         padded_buffer[j] = pframe[payload_index+j];
1250                 aes128k128d(key, ctr_preload, aes_out);
1251                 bitwise_xor(aes_out, padded_buffer, chain_buffer);
1252                 for (j = 0; j < payload_remainder;j++)
1253                         pframe[payload_index++] = chain_buffer[j];
1254         }
1255
1256         /* Encrypt the MIC */
1257         construct_ctr_preload(ctr_preload, a4_exists, qc_exists, pframe,
1258                               pn_vector, 0);
1259
1260         for (j = 0; j < 16; j++)
1261                 padded_buffer[j] = 0x00;
1262         for (j = 0; j < 8; j++)
1263                 padded_buffer[j] = pframe[j+hdrlen+8+plen];
1264
1265         aes128k128d(key, ctr_preload, aes_out);
1266         bitwise_xor(aes_out, padded_buffer, chain_buffer);
1267         for (j = 0; j < 8;j++)
1268                 pframe[payload_index++] = chain_buffer[j];
1269
1270         return _SUCCESS;
1271 }
1272
1273 int rtw_aes_encrypt23a(struct rtw_adapter *padapter,
1274                        struct xmit_frame *pxmitframe)
1275 {       /*  exclude ICV */
1276         /* Intermediate Buffers */
1277         int curfragnum, length;
1278         u32 prwskeylen;
1279         u8 *pframe, *prwskey;   /*  *payload,*iv */
1280         u8 hw_hdr_offset = 0;
1281         struct sta_info *stainfo;
1282         struct pkt_attrib *pattrib = &pxmitframe->attrib;
1283         struct security_priv *psecuritypriv = &padapter->securitypriv;
1284         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
1285         int res = _SUCCESS;
1286
1287         if (!pxmitframe->buf_addr)
1288                 return _FAIL;
1289
1290         hw_hdr_offset = TXDESC_OFFSET;
1291
1292         pframe = pxmitframe->buf_addr + hw_hdr_offset;
1293
1294         /* 4 start to encrypt each fragment */
1295         if (pattrib->encrypt != WLAN_CIPHER_SUITE_CCMP)
1296                 return _FAIL;
1297
1298         if (pattrib->psta) {
1299                 stainfo = pattrib->psta;
1300         } else {
1301                 DBG_8723A("%s, call rtw_get_stainfo23a()\n", __func__);
1302                 stainfo = rtw_get_stainfo23a(&padapter->stapriv, &pattrib->ra[0]);
1303         }
1304
1305         if (!stainfo) {
1306                 RT_TRACE(_module_rtl871x_security_c_, _drv_err_,
1307                          ("rtw_aes_encrypt23a: stainfo == NULL!!!\n"));
1308                 DBG_8723A("%s, psta == NUL\n", __func__);
1309                 res = _FAIL;
1310                 goto out;
1311         }
1312         if (!(stainfo->state &_FW_LINKED)) {
1313                 DBG_8723A("%s, psta->state(0x%x) != _FW_LINKED\n",
1314                           __func__, stainfo->state);
1315                 return _FAIL;
1316         }
1317         RT_TRACE(_module_rtl871x_security_c_, _drv_err_,
1318                  ("rtw_aes_encrypt23a: stainfo!= NULL!!!\n"));
1319
1320         if (is_multicast_ether_addr(pattrib->ra))
1321                 prwskey = psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey;
1322         else
1323                 prwskey = &stainfo->dot118021x_UncstKey.skey[0];
1324
1325         prwskeylen = 16;
1326
1327         for (curfragnum = 0; curfragnum < pattrib->nr_frags; curfragnum++) {
1328                 /* 4 the last fragment */
1329                 if ((curfragnum + 1) == pattrib->nr_frags) {
1330                         length = pattrib->last_txcmdsz -
1331                                 pattrib->hdrlen-pattrib->iv_len -
1332                                 pattrib->icv_len;
1333
1334                         aes_cipher(prwskey, pattrib->hdrlen, pframe, length);
1335                 } else {
1336                         length = pxmitpriv->frag_len-pattrib->hdrlen -
1337                                 pattrib->iv_len - pattrib->icv_len;
1338
1339                         aes_cipher(prwskey, pattrib->hdrlen, pframe, length);
1340                         pframe += pxmitpriv->frag_len;
1341                         pframe = PTR_ALIGN(pframe, 4);
1342                 }
1343         }
1344 out:
1345         return res;
1346 }
1347
1348 static int aes_decipher(u8 *key, uint   hdrlen,
1349                         u8 *pframe, uint plen)
1350 {
1351         static u8       message[MAX_MSG_SIZE];
1352         uint    qc_exists, a4_exists, i, j, payload_remainder,
1353                         num_blocks, payload_index;
1354         int res = _SUCCESS;
1355         u8 pn_vector[6];
1356         u8 mic_iv[16];
1357         u8 mic_header1[16];
1358         u8 mic_header2[16];
1359         u8 ctr_preload[16];
1360         /* Intermediate Buffers */
1361         u8 chain_buffer[16];
1362         u8 aes_out[16];
1363         u8 padded_buffer[16];
1364         u8 mic[8];
1365         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)pframe;
1366         u16 frsubtype = le16_to_cpu(hdr->frame_control) & IEEE80211_FCTL_STYPE;
1367
1368         memset((void *)mic_iv, 0, 16);
1369         memset((void *)mic_header1, 0, 16);
1370         memset((void *)mic_header2, 0, 16);
1371         memset((void *)ctr_preload, 0, 16);
1372         memset((void *)chain_buffer, 0, 16);
1373         memset((void *)aes_out, 0, 16);
1374         memset((void *)padded_buffer, 0, 16);
1375
1376         /* start to decrypt the payload */
1377
1378         num_blocks = (plen-8) / 16; /* plen including llc, payload_length and mic) */
1379
1380         payload_remainder = (plen-8) % 16;
1381
1382         pn_vector[0]  = pframe[hdrlen];
1383         pn_vector[1]  = pframe[hdrlen+1];
1384         pn_vector[2]  = pframe[hdrlen+4];
1385         pn_vector[3]  = pframe[hdrlen+5];
1386         pn_vector[4]  = pframe[hdrlen+6];
1387         pn_vector[5]  = pframe[hdrlen+7];
1388
1389         if ((hdrlen == sizeof(struct ieee80211_hdr_3addr) ||
1390             (hdrlen == sizeof(struct ieee80211_qos_hdr))))
1391                 a4_exists = 0;
1392         else
1393                 a4_exists = 1;
1394
1395         if (ieee80211_is_data(hdr->frame_control)) {
1396                 if ((frsubtype == IEEE80211_STYPE_DATA_CFACK) ||
1397                     (frsubtype == IEEE80211_STYPE_DATA_CFPOLL) ||
1398                     (frsubtype == IEEE80211_STYPE_DATA_CFACKPOLL)) {
1399                         qc_exists = 1;
1400                         if (hdrlen != sizeof(struct ieee80211_hdr_3addr))
1401                                 hdrlen += 2;
1402                 } else if ((frsubtype == IEEE80211_STYPE_QOS_DATA) ||
1403                            (frsubtype == IEEE80211_STYPE_QOS_DATA_CFACK) ||
1404                            (frsubtype == IEEE80211_STYPE_QOS_DATA_CFPOLL) ||
1405                            (frsubtype == IEEE80211_STYPE_QOS_DATA_CFACKPOLL)) {
1406                         if (hdrlen != sizeof(struct ieee80211_hdr_3addr))
1407                                 hdrlen += 2;
1408                         qc_exists = 1;
1409                 } else {
1410                         qc_exists = 0;
1411                 }
1412         } else {
1413                 qc_exists = 0;
1414         }
1415
1416         /*  now, decrypt pframe with hdrlen offset and plen long */
1417
1418         payload_index = hdrlen + 8; /*  8 is for extiv */
1419
1420         for (i = 0; i < num_blocks; i++) {
1421                 construct_ctr_preload(ctr_preload, a4_exists, qc_exists,
1422                                       pframe, pn_vector, i+1);
1423
1424                 aes128k128d(key, ctr_preload, aes_out);
1425                 bitwise_xor(aes_out, &pframe[payload_index], chain_buffer);
1426
1427                 for (j = 0; j < 16; j++)
1428                         pframe[payload_index++] = chain_buffer[j];
1429         }
1430
1431         if (payload_remainder > 0) {
1432                 /* If there is a short final block, then pad it,
1433                  * encrypt it and copy the unpadded part back
1434                  */
1435                 construct_ctr_preload(ctr_preload, a4_exists, qc_exists, pframe,
1436                                       pn_vector, num_blocks+1);
1437
1438                 for (j = 0; j < 16; j++)
1439                         padded_buffer[j] = 0x00;
1440                 for (j = 0; j < payload_remainder; j++)
1441                         padded_buffer[j] = pframe[payload_index+j];
1442                 aes128k128d(key, ctr_preload, aes_out);
1443                 bitwise_xor(aes_out, padded_buffer, chain_buffer);
1444                 for (j = 0; j < payload_remainder; j++)
1445                         pframe[payload_index++] = chain_buffer[j];
1446         }
1447
1448         /* start to calculate the mic */
1449         if ((hdrlen +plen+8) <= MAX_MSG_SIZE)
1450                 memcpy(message, pframe, (hdrlen+plen+8)); /* 8 is for ext iv len */
1451
1452         pn_vector[0] = pframe[hdrlen];
1453         pn_vector[1] = pframe[hdrlen+1];
1454         pn_vector[2] = pframe[hdrlen+4];
1455         pn_vector[3] = pframe[hdrlen+5];
1456         pn_vector[4] = pframe[hdrlen+6];
1457         pn_vector[5] = pframe[hdrlen+7];
1458
1459         construct_mic_iv(mic_iv, qc_exists, a4_exists, message,
1460                          plen-8, pn_vector);
1461
1462         construct_mic_header1(mic_header1, hdrlen, message);
1463         construct_mic_header2(mic_header2, message, a4_exists, qc_exists);
1464
1465         payload_remainder = (plen-8) % 16;
1466         num_blocks = (plen-8) / 16;
1467
1468         /* Find start of payload */
1469         payload_index = (hdrlen + 8);
1470
1471         /* Calculate MIC */
1472         aes128k128d(key, mic_iv, aes_out);
1473         bitwise_xor(aes_out, mic_header1, chain_buffer);
1474         aes128k128d(key, chain_buffer, aes_out);
1475         bitwise_xor(aes_out, mic_header2, chain_buffer);
1476         aes128k128d(key, chain_buffer, aes_out);
1477
1478         for (i = 0; i < num_blocks; i++) {
1479                 bitwise_xor(aes_out, &message[payload_index], chain_buffer);
1480
1481                 payload_index += 16;
1482                 aes128k128d(key, chain_buffer, aes_out);
1483         }
1484
1485         /* Add on the final payload block if it needs padding */
1486         if (payload_remainder > 0) {
1487                 for (j = 0; j < 16; j++)
1488                         padded_buffer[j] = 0x00;
1489                 for (j = 0; j < payload_remainder; j++)
1490                     padded_buffer[j] = message[payload_index++];
1491                 bitwise_xor(aes_out, padded_buffer, chain_buffer);
1492                 aes128k128d(key, chain_buffer, aes_out);
1493         }
1494
1495         for (j = 0 ; j < 8; j++)
1496                 mic[j] = aes_out[j];
1497
1498         /* Insert MIC into payload */
1499         for (j = 0; j < 8; j++)
1500                 message[payload_index+j] = mic[j];
1501
1502         payload_index = hdrlen + 8;
1503         for (i = 0; i< num_blocks; i++) {
1504                 construct_ctr_preload(ctr_preload, a4_exists, qc_exists,
1505                                       message, pn_vector, i+1);
1506                 aes128k128d(key, ctr_preload, aes_out);
1507                 bitwise_xor(aes_out, &message[payload_index], chain_buffer);
1508                 for (j = 0; j < 16; j++)
1509                         message[payload_index++] = chain_buffer[j];
1510         }
1511
1512         if (payload_remainder > 0) {
1513                 /* If there is a short final block, then pad it,
1514                  * encrypt it and copy the unpadded part back
1515                  */
1516                 construct_ctr_preload(ctr_preload, a4_exists, qc_exists,
1517                                       message, pn_vector, num_blocks+1);
1518
1519                 for (j = 0; j < 16; j++)
1520                          padded_buffer[j] = 0x00;
1521                 for (j = 0; j < payload_remainder; j++)
1522                         padded_buffer[j] = message[payload_index+j];
1523                 aes128k128d(key, ctr_preload, aes_out);
1524                 bitwise_xor(aes_out, padded_buffer, chain_buffer);
1525                 for (j = 0; j < payload_remainder; j++)
1526                         message[payload_index++] = chain_buffer[j];
1527         }
1528
1529         /* Encrypt the MIC */
1530         construct_ctr_preload(ctr_preload, a4_exists, qc_exists, message,
1531                               pn_vector, 0);
1532
1533         for (j = 0; j < 16; j++)
1534                 padded_buffer[j] = 0x00;
1535         for (j = 0; j < 8; j++)
1536                 padded_buffer[j] = message[j+hdrlen+8+plen-8];
1537
1538         aes128k128d(key, ctr_preload, aes_out);
1539         bitwise_xor(aes_out, padded_buffer, chain_buffer);
1540         for (j = 0; j < 8; j++)
1541                 message[payload_index++] = chain_buffer[j];
1542
1543         /* compare the mic */
1544         for (i = 0; i < 8; i++) {
1545                 if (pframe[hdrlen+8+plen-8+i] != message[hdrlen+8+plen-8+i]) {
1546                         RT_TRACE(_module_rtl871x_security_c_, _drv_err_,
1547                                  ("aes_decipher:mic check error mic[%d]: pframe(%x) != message(%x)\n",
1548                                  i, pframe[hdrlen+8+plen-8+i], message[hdrlen+8+plen-8+i]));
1549                         DBG_8723A("aes_decipher:mic check error mic[%d]: pframe(%x) != message(%x)\n",
1550                                   i, pframe[hdrlen+8+plen-8+i], message[hdrlen+8+plen-8+i]);
1551                         res = _FAIL;
1552                 }
1553         }
1554         return res;
1555 }
1556
1557 int rtw_aes_decrypt23a(struct rtw_adapter *padapter,
1558                        struct recv_frame *precvframe)
1559 {       /*  exclude ICV */
1560         struct sta_info *stainfo;
1561         struct rx_pkt_attrib *prxattrib = &precvframe->attrib;
1562         struct security_priv *psecuritypriv = &padapter->securitypriv;
1563         struct sk_buff *skb = precvframe->pkt;
1564         int length;
1565         u8 *pframe, *prwskey;   /*  *payload,*iv */
1566         int res = _SUCCESS;
1567
1568         pframe = skb->data;
1569         /* 4 start to encrypt each fragment */
1570         if (prxattrib->encrypt != WLAN_CIPHER_SUITE_CCMP)
1571                 return _FAIL;
1572
1573         stainfo = rtw_get_stainfo23a(&padapter->stapriv, &prxattrib->ta[0]);
1574         if (!stainfo) {
1575                 RT_TRACE(_module_rtl871x_security_c_, _drv_err_,
1576                          ("rtw_aes_encrypt23a: stainfo == NULL!!!\n"));
1577                 res = _FAIL;
1578                 goto exit;
1579         }
1580
1581         RT_TRACE(_module_rtl871x_security_c_, _drv_err_,
1582                  ("rtw_aes_decrypt23a: stainfo!= NULL!!!\n"));
1583
1584         if (is_multicast_ether_addr(prxattrib->ra)) {
1585                 /* in concurrent we should use sw decrypt in group key,
1586                    so we remove this message */
1587                 if (!psecuritypriv->binstallGrpkey) {
1588                         res = _FAIL;
1589                         DBG_8723A("%s:rx bc/mc packets, but didn't install "
1590                                   "group key!!!!!!!!!!\n", __func__);
1591                         goto exit;
1592                 }
1593                 prwskey = psecuritypriv->dot118021XGrpKey[prxattrib->key_index].skey;
1594                 if (psecuritypriv->dot118021XGrpKeyid != prxattrib->key_index) {
1595                         DBG_8723A("not match packet_index =%d, install_index ="
1596                                   "%d\n", prxattrib->key_index,
1597                                   psecuritypriv->dot118021XGrpKeyid);
1598                         res = _FAIL;
1599                         goto exit;
1600                 }
1601         } else {
1602                 prwskey = &stainfo->dot118021x_UncstKey.skey[0];
1603         }
1604
1605         length = skb->len - prxattrib->hdrlen - prxattrib->iv_len;
1606
1607         res = aes_decipher(prwskey, prxattrib->hdrlen, pframe, length);
1608 exit:
1609         return res;
1610 }
1611
1612 void rtw_use_tkipkey_handler23a(void *FunctionContext)
1613 {
1614         struct rtw_adapter *padapter = (struct rtw_adapter *)FunctionContext;
1615
1616         RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("^^^rtw_use_tkipkey_handler23a ^^^\n"));
1617         padapter->securitypriv.busetkipkey = 1;
1618         RT_TRACE(_module_rtl871x_security_c_, _drv_err_,
1619                  ("^^^rtw_use_tkipkey_handler23a padapter->securitypriv.busetkipkey =%d^^^\n",
1620                  padapter->securitypriv.busetkipkey));
1621 }