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