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