drivers/net/wireless/rockchip_wlan/rtl8723bu/core/rtw_security.c

   1 /******************************************************************************
   2  *
   3  * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
   4  *
   5  * This program is free software; you can redistribute it and/or modify it
   6  * under the terms of version 2 of the GNU General Public License as
   7  * published by the Free Software Foundation.
   8  *
   9  * This program is distributed in the hope that it will be useful, but WITHOUT
  10  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  11  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
  12  * more details.
  13  *
  14  * You should have received a copy of the GNU General Public License along with
  15  * this program; if not, write to the Free Software Foundation, Inc.,
  16  * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
  17  *
  18  *
  19  ******************************************************************************/
  20 #define  _RTW_SECURITY_C_
  21
  22 #include <drv_types.h>
  23
  24 static const char *_security_type_str[] = {
  25         "N/A",
  26         "WEP40",
  27         "TKIP",
  28         "TKIP_WM",
  29         "AES",
  30         "WEP104",
  31         "SMS4",
  32         "WEP_WPA",
  33         "BIP",
  34 };
  35
  36 const char *security_type_str(u8 value)
  37 {
  38 #ifdef CONFIG_IEEE80211W
  39         if (value <= _BIP_)
  40 #else
  41         if (value <= _WEP_WPA_MIXED_)
  42 #endif
  43                 return _security_type_str[value];
  44         return NULL;
  45 }
  46
  47 #ifdef DBG_SW_SEC_CNT
  48 #define WEP_SW_ENC_CNT_INC(sec, ra) \
  49         if (is_broadcast_mac_addr(ra)) \
  50                 sec->wep_sw_enc_cnt_bc++; \
  51         else if (is_multicast_mac_addr(ra)) \
  52                 sec->wep_sw_enc_cnt_mc++; \
  53         else \
  54                 sec->wep_sw_enc_cnt_uc++;
  55
  56 #define WEP_SW_DEC_CNT_INC(sec, ra) \
  57         if (is_broadcast_mac_addr(ra)) \
  58                 sec->wep_sw_dec_cnt_bc++; \
  59         else if (is_multicast_mac_addr(ra)) \
  60                 sec->wep_sw_dec_cnt_mc++; \
  61         else \
  62                 sec->wep_sw_dec_cnt_uc++;
  63
  64 #define TKIP_SW_ENC_CNT_INC(sec, ra) \
  65         if (is_broadcast_mac_addr(ra)) \
  66                 sec->tkip_sw_enc_cnt_bc++; \
  67         else if (is_multicast_mac_addr(ra)) \
  68                 sec->tkip_sw_enc_cnt_mc++; \
  69         else \
  70                 sec->tkip_sw_enc_cnt_uc++;
  71
  72 #define TKIP_SW_DEC_CNT_INC(sec, ra) \
  73         if (is_broadcast_mac_addr(ra)) \
  74                 sec->tkip_sw_dec_cnt_bc++; \
  75         else if (is_multicast_mac_addr(ra)) \
  76                 sec->tkip_sw_dec_cnt_mc++; \
  77         else \
  78                 sec->tkip_sw_dec_cnt_uc++;
  79
  80 #define AES_SW_ENC_CNT_INC(sec, ra) \
  81         if (is_broadcast_mac_addr(ra)) \
  82                 sec->aes_sw_enc_cnt_bc++; \
  83         else if (is_multicast_mac_addr(ra)) \
  84                 sec->aes_sw_enc_cnt_mc++; \
  85         else \
  86                 sec->aes_sw_enc_cnt_uc++;
  87
  88 #define AES_SW_DEC_CNT_INC(sec, ra) \
  89         if (is_broadcast_mac_addr(ra)) \
  90                 sec->aes_sw_dec_cnt_bc++; \
  91         else if (is_multicast_mac_addr(ra)) \
  92                 sec->aes_sw_dec_cnt_mc++; \
  93         else \
  94                 sec->aes_sw_dec_cnt_uc++;
  95 #else
  96 #define WEP_SW_ENC_CNT_INC(sec, ra)
  97 #define WEP_SW_DEC_CNT_INC(sec, ra)
  98 #define TKIP_SW_ENC_CNT_INC(sec, ra)
  99 #define TKIP_SW_DEC_CNT_INC(sec, ra)
 100 #define AES_SW_ENC_CNT_INC(sec, ra)
 101 #define AES_SW_DEC_CNT_INC(sec, ra)
 102 #endif /* DBG_SW_SEC_CNT */
 103
 104 //=====WEP related=====
 105
 106 #define CRC32_POLY 0x04c11db7
 107
 108 struct arc4context
 109 {
 110         u32 x;
 111         u32 y;
 112         u8 state[256];
 113 };
 114
 115
 116 static void arcfour_init(struct arc4context     *parc4ctx, u8 * key,u32 key_len)
 117 {
 118         u32     t, u;
 119         u32     keyindex;
 120         u32     stateindex;
 121         u8 * state;
 122         u32     counter;
 123 _func_enter_;
 124         state = parc4ctx->state;
 125         parc4ctx->x = 0;
 126         parc4ctx->y = 0;
 127         for (counter = 0; counter < 256; counter++)
 128                 state[counter] = (u8)counter;
 129         keyindex = 0;
 130         stateindex = 0;
 131         for (counter = 0; counter < 256; counter++)
 132         {
 133                 t = state[counter];
 134                 stateindex = (stateindex + key[keyindex] + t) & 0xff;
 135                 u = state[stateindex];
 136                 state[stateindex] = (u8)t;
 137                 state[counter] = (u8)u;
 138                 if (++keyindex >= key_len)
 139                         keyindex = 0;
 140         }
 141 _func_exit_;
 142 }
 143 static u32 arcfour_byte(        struct arc4context      *parc4ctx)
 144 {
 145         u32 x;
 146         u32 y;
 147         u32 sx, sy;
 148         u8 * state;
 149 _func_enter_;
 150         state = parc4ctx->state;
 151         x = (parc4ctx->x + 1) & 0xff;
 152         sx = state[x];
 153         y = (sx + parc4ctx->y) & 0xff;
 154         sy = state[y];
 155         parc4ctx->x = x;
 156         parc4ctx->y = y;
 157         state[y] = (u8)sx;
 158         state[x] = (u8)sy;
 159 _func_exit_;
 160         return state[(sx + sy) & 0xff];
 161 }
 162
 163
 164 static void arcfour_encrypt(    struct arc4context      *parc4ctx,
 165         u8 * dest,
 166         u8 * src,
 167         u32 len)
 168 {
 169         u32     i;
 170 _func_enter_;
 171         for (i = 0; i < len; i++)
 172                 dest[i] = src[i] ^ (unsigned char)arcfour_byte(parc4ctx);
 173 _func_exit_;
 174 }
 175
 176 static sint bcrc32initialized = 0;
 177 static u32 crc32_table[256];
 178
 179
 180 static u8 crc32_reverseBit( u8 data)
 181 {
 182         return( (u8)((data<<7)&0x80) | ((data<<5)&0x40) | ((data<<3)&0x20) | ((data<<1)&0x10) | ((data>>1)&0x08) | ((data>>3)&0x04) | ((data>>5)&0x02) | ((data>>7)&0x01) );
 183 }
 184
 185 static void crc32_init(void)
 186 {
 187 _func_enter_;
 188         if (bcrc32initialized == 1)
 189                 goto exit;
 190         else{
 191                 sint i, j;
 192                 u32 c;
 193                 u8 *p=(u8 *)&c, *p1;
 194                 u8 k;
 195
 196                 c = 0x12340000;
 197
 198                 for (i = 0; i < 256; ++i)
 199                 {
 200                         k = crc32_reverseBit((u8)i);
 201                         for (c = ((u32)k) << 24, j = 8; j > 0; --j){
 202                                 c = c & 0x80000000 ? (c << 1) ^ CRC32_POLY : (c << 1);
 203                         }
 204                         p1 = (u8 *)&crc32_table[i];
 205
 206                         p1[0] = crc32_reverseBit(p[3]);
 207                         p1[1] = crc32_reverseBit(p[2]);
 208                         p1[2] = crc32_reverseBit(p[1]);
 209                         p1[3] = crc32_reverseBit(p[0]);
 210                 }
 211                 bcrc32initialized= 1;
 212         }
 213 exit:
 214 _func_exit_;
 215 }
 216
 217 static u32 getcrc32(u8 *buf, sint len)
 218 {
 219         u8 *p;
 220         u32  crc;
 221 _func_enter_;
 222         if (bcrc32initialized == 0) crc32_init();
 223
 224         crc = 0xffffffff;       /* preload shift register, per CRC-32 spec */
 225
 226         for (p = buf; len > 0; ++p, --len)
 227         {
 228                 crc = crc32_table[ (crc ^ *p) & 0xff] ^ (crc >> 8);
 229         }
 230 _func_exit_;
 231         return ~crc;    /* transmit complement, per CRC-32 spec */
 232 }
 233
 234
 235 /*
 236         Need to consider the fragment  situation
 237 */
 238 void rtw_wep_encrypt(_adapter *padapter, u8 *pxmitframe)
 239 {                                                                                                                                       // exclude ICV
 240
 241         unsigned char   crc[4];
 242         struct arc4context       mycontext;
 243
 244         sint    curfragnum,length;
 245         u32     keylength;
 246
 247         u8      *pframe, *payload,*iv;    //,*wepkey
 248         u8      wepkey[16];
 249         u8   hw_hdr_offset=0;
 250         struct  pkt_attrib       *pattrib = &((struct xmit_frame*)pxmitframe)->attrib;
 251         struct  security_priv   *psecuritypriv=&padapter->securitypriv;
 252         struct  xmit_priv               *pxmitpriv=&padapter->xmitpriv;
 253
 254 _func_enter_;
 255
 256
 257         if(((struct xmit_frame*)pxmitframe)->buf_addr==NULL)
 258                 return;
 259
 260 #ifdef CONFIG_USB_TX_AGGREGATION
 261         hw_hdr_offset = TXDESC_SIZE +
 262                  (((struct xmit_frame*)pxmitframe)->pkt_offset * PACKET_OFFSET_SZ);
 263 #else
 264         #ifdef CONFIG_TX_EARLY_MODE
 265         hw_hdr_offset = TXDESC_OFFSET+EARLY_MODE_INFO_SIZE;
 266         #else
 267         hw_hdr_offset = TXDESC_OFFSET;
 268         #endif
 269 #endif
 270
 271         pframe = ((struct xmit_frame*)pxmitframe)->buf_addr + hw_hdr_offset;
 272
 273         //start to encrypt each fragment
 274         if((pattrib->encrypt==_WEP40_)||(pattrib->encrypt==_WEP104_))
 275         {
 276                 keylength=psecuritypriv->dot11DefKeylen[psecuritypriv->dot11PrivacyKeyIndex];
 277
 278                 for(curfragnum=0;curfragnum<pattrib->nr_frags;curfragnum++)
 279                 {
 280                         iv=pframe+pattrib->hdrlen;
 281                         _rtw_memcpy(&wepkey[0], iv, 3);
 282                         _rtw_memcpy(&wepkey[3], &psecuritypriv->dot11DefKey[psecuritypriv->dot11PrivacyKeyIndex].skey[0],keylength);
 283                         payload=pframe+pattrib->iv_len+pattrib->hdrlen;
 284
 285                         if((curfragnum+1)==pattrib->nr_frags)
 286                         {       //the last fragment
 287
 288                                 length=pattrib->last_txcmdsz-pattrib->hdrlen-pattrib->iv_len- pattrib->icv_len;
 289
 290                                 *((u32 *)crc)=cpu_to_le32(getcrc32(payload,length));
 291
 292                                 arcfour_init(&mycontext, wepkey,3+keylength);
 293                                 arcfour_encrypt(&mycontext, payload, payload, length);
 294                                 arcfour_encrypt(&mycontext, payload+length, crc, 4);
 295
 296                         }
 297                         else
 298                         {
 299                         length=pxmitpriv->frag_len-pattrib->hdrlen-pattrib->iv_len-pattrib->icv_len ;
 300                                 *((u32 *)crc)=cpu_to_le32(getcrc32(payload,length));
 301                                 arcfour_init(&mycontext, wepkey,3+keylength);
 302                                 arcfour_encrypt(&mycontext, payload, payload, length);
 303                                 arcfour_encrypt(&mycontext, payload+length, crc, 4);
 304
 305                         pframe+=pxmitpriv->frag_len;
 306                         pframe=(u8 *)RND4((SIZE_PTR)(pframe));
 307
 308                         }
 309
 310                 }
 311
 312                 WEP_SW_ENC_CNT_INC(psecuritypriv, pattrib->ra);
 313         }
 314
 315 _func_exit_;
 316
 317 }
 318
 319 void rtw_wep_decrypt(_adapter  *padapter, u8 *precvframe)
 320 {
 321         // exclude ICV
 322         u8      crc[4];
 323         struct arc4context       mycontext;
 324         sint    length;
 325         u32     keylength;
 326         u8      *pframe, *payload,*iv,wepkey[16];
 327         u8       keyindex;
 328         struct  rx_pkt_attrib    *prxattrib = &(((union recv_frame*)precvframe)->u.hdr.attrib);
 329         struct  security_priv   *psecuritypriv=&padapter->securitypriv;
 330
 331 _func_enter_;
 332
 333         pframe=(unsigned char *)((union recv_frame*)precvframe)->u.hdr.rx_data;
 334
 335         //start to decrypt recvframe
 336         if((prxattrib->encrypt==_WEP40_)||(prxattrib->encrypt==_WEP104_))
 337         {
 338                 iv=pframe+prxattrib->hdrlen;
 339                 //keyindex=(iv[3]&0x3);
 340                 keyindex = prxattrib->key_index;
 341                 keylength=psecuritypriv->dot11DefKeylen[keyindex];
 342                 _rtw_memcpy(&wepkey[0], iv, 3);
 343                 //_rtw_memcpy(&wepkey[3], &psecuritypriv->dot11DefKey[psecuritypriv->dot11PrivacyKeyIndex].skey[0],keylength);
 344                 _rtw_memcpy(&wepkey[3], &psecuritypriv->dot11DefKey[keyindex].skey[0],keylength);
 345                 length= ((union recv_frame *)precvframe)->u.hdr.len-prxattrib->hdrlen-prxattrib->iv_len;
 346
 347                 payload=pframe+prxattrib->iv_len+prxattrib->hdrlen;
 348
 349                 //decrypt payload include icv
 350                 arcfour_init(&mycontext, wepkey,3+keylength);
 351                 arcfour_encrypt(&mycontext, payload, payload,  length);
 352
 353                 //calculate icv and compare the icv
 354                 *((u32 *)crc)=le32_to_cpu(getcrc32(payload,length-4));
 355
 356                 if(crc[3]!=payload[length-1] || crc[2]!=payload[length-2] || crc[1]!=payload[length-3] || crc[0]!=payload[length-4])
 357                 {
 358                         RT_TRACE(_module_rtl871x_security_c_,_drv_err_,("rtw_wep_decrypt:icv error crc[3](%x)!=payload[length-1](%x) || crc[2](%x)!=payload[length-2](%x) || crc[1](%x)!=payload[length-3](%x) || crc[0](%x)!=payload[length-4](%x)\n",
 359                                                 crc[3],payload[length-1],crc[2],payload[length-2],crc[1],payload[length-3],crc[0],payload[length-4]));
 360                 }
 361
 362                 WEP_SW_DEC_CNT_INC(psecuritypriv, prxattrib->ra);
 363         }
 364
 365 _func_exit_;
 366
 367         return;
 368
 369 }
 370
 371 //3             =====TKIP related=====
 372
 373 static u32 secmicgetuint32( u8 * p )
 374 // Convert from Byte[] to Us4Byte32 in a portable way
 375 {
 376         s32 i;
 377         u32 res = 0;
 378 _func_enter_;
 379         for( i=0; i<4; i++ )
 380         {
 381                 res |= ((u32)(*p++)) << (8*i);
 382         }
 383 _func_exit_;
 384         return res;
 385 }
 386
 387 static void secmicputuint32( u8 * p, u32 val )
 388 // Convert from Us4Byte32 to Byte[] in a portable way
 389 {
 390         long i;
 391 _func_enter_;
 392         for( i=0; i<4; i++ )
 393         {
 394                 *p++ = (u8) (val & 0xff);
 395                 val >>= 8;
 396         }
 397 _func_exit_;
 398 }
 399
 400 static void secmicclear(struct mic_data *pmicdata)
 401 {
 402 // Reset the state to the empty message.
 403 _func_enter_;
 404         pmicdata->L = pmicdata->K0;
 405         pmicdata->R = pmicdata->K1;
 406         pmicdata->nBytesInM = 0;
 407         pmicdata->M = 0;
 408 _func_exit_;
 409 }
 410
 411 void rtw_secmicsetkey(struct mic_data *pmicdata, u8 * key )
 412 {
 413         // Set the key
 414 _func_enter_;
 415         pmicdata->K0 = secmicgetuint32( key );
 416         pmicdata->K1 = secmicgetuint32( key + 4 );
 417         // and reset the message
 418         secmicclear(pmicdata);
 419 _func_exit_;
 420 }
 421
 422 void rtw_secmicappendbyte(struct mic_data *pmicdata, u8 b )
 423 {
 424 _func_enter_;
 425         // Append the byte to our word-sized buffer
 426         pmicdata->M |= ((unsigned long)b) << (8*pmicdata->nBytesInM);
 427         pmicdata->nBytesInM++;
 428         // Process the word if it is full.
 429         if( pmicdata->nBytesInM >= 4 )
 430         {
 431                 pmicdata->L ^= pmicdata->M;
 432                 pmicdata->R ^= ROL32( pmicdata->L, 17 );
 433                 pmicdata->L += pmicdata->R;
 434                 pmicdata->R ^= ((pmicdata->L & 0xff00ff00) >> 8) | ((pmicdata->L & 0x00ff00ff) << 8);
 435                 pmicdata->L += pmicdata->R;
 436                 pmicdata->R ^= ROL32( pmicdata->L, 3 );
 437                 pmicdata->L += pmicdata->R;
 438                 pmicdata->R ^= ROR32( pmicdata->L, 2 );
 439                 pmicdata->L += pmicdata->R;
 440                 // Clear the buffer
 441                 pmicdata->M = 0;
 442                 pmicdata->nBytesInM = 0;
 443         }
 444 _func_exit_;
 445 }
 446
 447 void rtw_secmicappend(struct mic_data *pmicdata, u8 * src, u32 nbytes )
 448 {
 449 _func_enter_;
 450         // This is simple
 451         while( nbytes > 0 )
 452         {
 453                 rtw_secmicappendbyte(pmicdata, *src++ );
 454                 nbytes--;
 455         }
 456 _func_exit_;
 457 }
 458
 459 void rtw_secgetmic(struct mic_data *pmicdata, u8 * dst )
 460 {
 461 _func_enter_;
 462         // Append the minimum padding
 463         rtw_secmicappendbyte(pmicdata, 0x5a );
 464         rtw_secmicappendbyte(pmicdata, 0 );
 465         rtw_secmicappendbyte(pmicdata, 0 );
 466         rtw_secmicappendbyte(pmicdata, 0 );
 467         rtw_secmicappendbyte(pmicdata, 0 );
 468         // and then zeroes until the length is a multiple of 4
 469         while( pmicdata->nBytesInM != 0 )
 470         {
 471                 rtw_secmicappendbyte(pmicdata, 0 );
 472         }
 473         // The appendByte function has already computed the result.
 474         secmicputuint32( dst, pmicdata->L );
 475         secmicputuint32( dst+4, pmicdata->R );
 476         // Reset to the empty message.
 477         secmicclear(pmicdata);
 478 _func_exit_;
 479 }
 480
 481
 482 void rtw_seccalctkipmic(u8 * key,u8 *header,u8 *data,u32 data_len,u8 *mic_code, u8 pri)
 483 {
 484
 485         struct mic_data micdata;
 486         u8 priority[4]={0x0,0x0,0x0,0x0};
 487 _func_enter_;
 488         rtw_secmicsetkey(&micdata, key);
 489         priority[0]=pri;
 490
 491         /* Michael MIC pseudo header: DA, SA, 3 x 0, Priority */
 492         if(header[1]&1){   //ToDS==1
 493                         rtw_secmicappend(&micdata, &header[16], 6);  //DA
 494                 if(header[1]&2)  //From Ds==1
 495                         rtw_secmicappend(&micdata, &header[24], 6);
 496                 else
 497                         rtw_secmicappend(&micdata, &header[10], 6);
 498         }
 499         else{   //ToDS==0
 500                 rtw_secmicappend(&micdata, &header[4], 6);   //DA
 501                 if(header[1]&2)  //From Ds==1
 502                         rtw_secmicappend(&micdata, &header[16], 6);
 503                 else
 504                         rtw_secmicappend(&micdata, &header[10], 6);
 505
 506         }
 507         rtw_secmicappend(&micdata, &priority[0], 4);
 508
 509
 510         rtw_secmicappend(&micdata, data, data_len);
 511
 512         rtw_secgetmic(&micdata,mic_code);
 513 _func_exit_;
 514 }
 515
 516
 517
 518
 519 /* macros for extraction/creation of unsigned char/unsigned short values  */
 520 #define RotR1(v16)   ((((v16) >> 1) & 0x7FFF) ^ (((v16) & 1) << 15))
 521 #define   Lo8(v16)   ((u8)( (v16)       & 0x00FF))
 522 #define   Hi8(v16)   ((u8)(((v16) >> 8) & 0x00FF))
 523 #define  Lo16(v32)   ((u16)( (v32)       & 0xFFFF))
 524 #define  Hi16(v32)   ((u16)(((v32) >>16) & 0xFFFF))
 525 #define  Mk16(hi,lo) ((lo) ^ (((u16)(hi)) << 8))
 526
 527 /* select the Nth 16-bit word of the temporal key unsigned char array TK[]   */
 528 #define  TK16(N)     Mk16(tk[2*(N)+1],tk[2*(N)])
 529
 530 /* S-box lookup: 16 bits --> 16 bits */
 531 #define _S_(v16)     (Sbox1[0][Lo8(v16)] ^ Sbox1[1][Hi8(v16)])
 532
 533 /* fixed algorithm "parameters" */
 534 #define PHASE1_LOOP_CNT   8    /* this needs to be "big enough"     */
 535 #define TA_SIZE           6    /*  48-bit transmitter address       */
 536 #define TK_SIZE          16    /* 128-bit temporal key              */
 537 #define P1K_SIZE         10    /*  80-bit Phase1 key                */
 538 #define RC4_KEY_SIZE     16    /* 128-bit RC4KEY (104 bits unknown) */
 539
 540
 541 /* 2-unsigned char by 2-unsigned char subset of the full AES S-box table */
 542 static const unsigned short Sbox1[2][256]=       /* Sbox for hash (can be in ROM)     */
 543 { {
 544    0xC6A5,0xF884,0xEE99,0xF68D,0xFF0D,0xD6BD,0xDEB1,0x9154,
 545    0x6050,0x0203,0xCEA9,0x567D,0xE719,0xB562,0x4DE6,0xEC9A,
 546    0x8F45,0x1F9D,0x8940,0xFA87,0xEF15,0xB2EB,0x8EC9,0xFB0B,
 547    0x41EC,0xB367,0x5FFD,0x45EA,0x23BF,0x53F7,0xE496,0x9B5B,
 548    0x75C2,0xE11C,0x3DAE,0x4C6A,0x6C5A,0x7E41,0xF502,0x834F,
 549    0x685C,0x51F4,0xD134,0xF908,0xE293,0xAB73,0x6253,0x2A3F,
 550    0x080C,0x9552,0x4665,0x9D5E,0x3028,0x37A1,0x0A0F,0x2FB5,
 551    0x0E09,0x2436,0x1B9B,0xDF3D,0xCD26,0x4E69,0x7FCD,0xEA9F,
 552    0x121B,0x1D9E,0x5874,0x342E,0x362D,0xDCB2,0xB4EE,0x5BFB,
 553    0xA4F6,0x764D,0xB761,0x7DCE,0x527B,0xDD3E,0x5E71,0x1397,
 554    0xA6F5,0xB968,0x0000,0xC12C,0x4060,0xE31F,0x79C8,0xB6ED,
 555    0xD4BE,0x8D46,0x67D9,0x724B,0x94DE,0x98D4,0xB0E8,0x854A,
 556    0xBB6B,0xC52A,0x4FE5,0xED16,0x86C5,0x9AD7,0x6655,0x1194,
 557    0x8ACF,0xE910,0x0406,0xFE81,0xA0F0,0x7844,0x25BA,0x4BE3,
 558    0xA2F3,0x5DFE,0x80C0,0x058A,0x3FAD,0x21BC,0x7048,0xF104,
 559    0x63DF,0x77C1,0xAF75,0x4263,0x2030,0xE51A,0xFD0E,0xBF6D,
 560    0x814C,0x1814,0x2635,0xC32F,0xBEE1,0x35A2,0x88CC,0x2E39,
 561    0x9357,0x55F2,0xFC82,0x7A47,0xC8AC,0xBAE7,0x322B,0xE695,
 562    0xC0A0,0x1998,0x9ED1,0xA37F,0x4466,0x547E,0x3BAB,0x0B83,
 563    0x8CCA,0xC729,0x6BD3,0x283C,0xA779,0xBCE2,0x161D,0xAD76,
 564    0xDB3B,0x6456,0x744E,0x141E,0x92DB,0x0C0A,0x486C,0xB8E4,
 565    0x9F5D,0xBD6E,0x43EF,0xC4A6,0x39A8,0x31A4,0xD337,0xF28B,
 566    0xD532,0x8B43,0x6E59,0xDAB7,0x018C,0xB164,0x9CD2,0x49E0,
 567    0xD8B4,0xACFA,0xF307,0xCF25,0xCAAF,0xF48E,0x47E9,0x1018,
 568    0x6FD5,0xF088,0x4A6F,0x5C72,0x3824,0x57F1,0x73C7,0x9751,
 569    0xCB23,0xA17C,0xE89C,0x3E21,0x96DD,0x61DC,0x0D86,0x0F85,
 570    0xE090,0x7C42,0x71C4,0xCCAA,0x90D8,0x0605,0xF701,0x1C12,
 571    0xC2A3,0x6A5F,0xAEF9,0x69D0,0x1791,0x9958,0x3A27,0x27B9,
 572    0xD938,0xEB13,0x2BB3,0x2233,0xD2BB,0xA970,0x0789,0x33A7,
 573    0x2DB6,0x3C22,0x1592,0xC920,0x8749,0xAAFF,0x5078,0xA57A,
 574    0x038F,0x59F8,0x0980,0x1A17,0x65DA,0xD731,0x84C6,0xD0B8,
 575    0x82C3,0x29B0,0x5A77,0x1E11,0x7BCB,0xA8FC,0x6DD6,0x2C3A,
 576   },
 577
 578
 579   {  /* second half of table is unsigned char-reversed version of first! */
 580    0xA5C6,0x84F8,0x99EE,0x8DF6,0x0DFF,0xBDD6,0xB1DE,0x5491,
 581    0x5060,0x0302,0xA9CE,0x7D56,0x19E7,0x62B5,0xE64D,0x9AEC,
 582    0x458F,0x9D1F,0x4089,0x87FA,0x15EF,0xEBB2,0xC98E,0x0BFB,
 583    0xEC41,0x67B3,0xFD5F,0xEA45,0xBF23,0xF753,0x96E4,0x5B9B,
 584    0xC275,0x1CE1,0xAE3D,0x6A4C,0x5A6C,0x417E,0x02F5,0x4F83,
 585    0x5C68,0xF451,0x34D1,0x08F9,0x93E2,0x73AB,0x5362,0x3F2A,
 586    0x0C08,0x5295,0x6546,0x5E9D,0x2830,0xA137,0x0F0A,0xB52F,
 587    0x090E,0x3624,0x9B1B,0x3DDF,0x26CD,0x694E,0xCD7F,0x9FEA,
 588    0x1B12,0x9E1D,0x7458,0x2E34,0x2D36,0xB2DC,0xEEB4,0xFB5B,
 589    0xF6A4,0x4D76,0x61B7,0xCE7D,0x7B52,0x3EDD,0x715E,0x9713,
 590    0xF5A6,0x68B9,0x0000,0x2CC1,0x6040,0x1FE3,0xC879,0xEDB6,
 591    0xBED4,0x468D,0xD967,0x4B72,0xDE94,0xD498,0xE8B0,0x4A85,
 592    0x6BBB,0x2AC5,0xE54F,0x16ED,0xC586,0xD79A,0x5566,0x9411,
 593    0xCF8A,0x10E9,0x0604,0x81FE,0xF0A0,0x4478,0xBA25,0xE34B,
 594    0xF3A2,0xFE5D,0xC080,0x8A05,0xAD3F,0xBC21,0x4870,0x04F1,
 595    0xDF63,0xC177,0x75AF,0x6342,0x3020,0x1AE5,0x0EFD,0x6DBF,
 596    0x4C81,0x1418,0x3526,0x2FC3,0xE1BE,0xA235,0xCC88,0x392E,
 597    0x5793,0xF255,0x82FC,0x477A,0xACC8,0xE7BA,0x2B32,0x95E6,
 598    0xA0C0,0x9819,0xD19E,0x7FA3,0x6644,0x7E54,0xAB3B,0x830B,
 599    0xCA8C,0x29C7,0xD36B,0x3C28,0x79A7,0xE2BC,0x1D16,0x76AD,
 600    0x3BDB,0x5664,0x4E74,0x1E14,0xDB92,0x0A0C,0x6C48,0xE4B8,
 601    0x5D9F,0x6EBD,0xEF43,0xA6C4,0xA839,0xA431,0x37D3,0x8BF2,
 602    0x32D5,0x438B,0x596E,0xB7DA,0x8C01,0x64B1,0xD29C,0xE049,
 603    0xB4D8,0xFAAC,0x07F3,0x25CF,0xAFCA,0x8EF4,0xE947,0x1810,
 604    0xD56F,0x88F0,0x6F4A,0x725C,0x2438,0xF157,0xC773,0x5197,
 605    0x23CB,0x7CA1,0x9CE8,0x213E,0xDD96,0xDC61,0x860D,0x850F,
 606    0x90E0,0x427C,0xC471,0xAACC,0xD890,0x0506,0x01F7,0x121C,
 607    0xA3C2,0x5F6A,0xF9AE,0xD069,0x9117,0x5899,0x273A,0xB927,
 608    0x38D9,0x13EB,0xB32B,0x3322,0xBBD2,0x70A9,0x8907,0xA733,
 609    0xB62D,0x223C,0x9215,0x20C9,0x4987,0xFFAA,0x7850,0x7AA5,
 610    0x8F03,0xF859,0x8009,0x171A,0xDA65,0x31D7,0xC684,0xB8D0,
 611    0xC382,0xB029,0x775A,0x111E,0xCB7B,0xFCA8,0xD66D,0x3A2C,
 612   }
 613 };
 614
 615  /*
 616 **********************************************************************
 617 * Routine: Phase 1 -- generate P1K, given TA, TK, IV32
 618 *
 619 * Inputs:
 620 *     tk[]      = temporal key                         [128 bits]
 621 *     ta[]      = transmitter's MAC address            [ 48 bits]
 622 *     iv32      = upper 32 bits of IV                  [ 32 bits]
 623 * Output:
 624 *     p1k[]     = Phase 1 key                          [ 80 bits]
 625 *
 626 * Note:
 627 *     This function only needs to be called every 2**16 packets,
 628 *     although in theory it could be called every packet.
 629 *
 630 **********************************************************************
 631 */
 632 static void phase1(u16 *p1k,const u8 *tk,const u8 *ta,u32 iv32)
 633 {
 634         sint  i;
 635 _func_enter_;
 636         /* Initialize the 80 bits of P1K[] from IV32 and TA[0..5]     */
 637         p1k[0]      = Lo16(iv32);
 638         p1k[1]      = Hi16(iv32);
 639         p1k[2]      = Mk16(ta[1],ta[0]); /* use TA[] as little-endian */
 640         p1k[3]      = Mk16(ta[3],ta[2]);
 641         p1k[4]      = Mk16(ta[5],ta[4]);
 642
 643         /* Now compute an unbalanced Feistel cipher with 80-bit block */
 644         /* size on the 80-bit block P1K[], using the 128-bit key TK[] */
 645         for (i=0; i < PHASE1_LOOP_CNT ;i++)
 646         {                 /* Each add operation here is mod 2**16 */
 647                 p1k[0] += _S_(p1k[4] ^ TK16((i&1)+0));
 648                 p1k[1] += _S_(p1k[0] ^ TK16((i&1)+2));
 649                 p1k[2] += _S_(p1k[1] ^ TK16((i&1)+4));
 650                 p1k[3] += _S_(p1k[2] ^ TK16((i&1)+6));
 651                 p1k[4] += _S_(p1k[3] ^ TK16((i&1)+0));
 652                 p1k[4] +=  (unsigned short)i;                    /* avoid "slide attacks" */
 653         }
 654 _func_exit_;
 655 }
 656
 657
 658 /*
 659 **********************************************************************
 660 * Routine: Phase 2 -- generate RC4KEY, given TK, P1K, IV16
 661 *
 662 * Inputs:
 663 *     tk[]      = Temporal key                         [128 bits]
 664 *     p1k[]     = Phase 1 output key                   [ 80 bits]
 665 *     iv16      = low 16 bits of IV counter            [ 16 bits]
 666 * Output:
 667 *     rc4key[]  = the key used to encrypt the packet   [128 bits]
 668 *
 669 * Note:
 670 *     The value {TA,IV32,IV16} for Phase1/Phase2 must be unique
 671 *     across all packets using the same key TK value. Then, for a
 672 *     given value of TK[], this TKIP48 construction guarantees that
 673 *     the final RC4KEY value is unique across all packets.
 674 *
 675 * Suggested implementation optimization: if PPK[] is "overlaid"
 676 *     appropriately on RC4KEY[], there is no need for the final
 677 *     for loop below that copies the PPK[] result into RC4KEY[].
 678 *
 679 **********************************************************************
 680 */
 681 static void phase2(u8 *rc4key,const u8 *tk,const u16 *p1k,u16 iv16)
 682 {
 683         sint  i;
 684         u16 PPK[6];                          /* temporary key for mixing    */
 685 _func_enter_;
 686         /* Note: all adds in the PPK[] equations below are mod 2**16         */
 687         for (i=0;i<5;i++) PPK[i]=p1k[i];      /* first, copy P1K to PPK      */
 688                 PPK[5]  =  p1k[4] +iv16;             /* next,  add in IV16          */
 689
 690         /* Bijective non-linear mixing of the 96 bits of PPK[0..5]           */
 691         PPK[0] +=    _S_(PPK[5] ^ TK16(0));   /* Mix key in each "round"     */
 692         PPK[1] +=    _S_(PPK[0] ^ TK16(1));
 693         PPK[2] +=    _S_(PPK[1] ^ TK16(2));
 694         PPK[3] +=    _S_(PPK[2] ^ TK16(3));
 695         PPK[4] +=    _S_(PPK[3] ^ TK16(4));
 696         PPK[5] +=    _S_(PPK[4] ^ TK16(5));   /* Total # S-box lookups == 6  */
 697
 698         /* Final sweep: bijective, "linear". Rotates kill LSB correlations   */
 699         PPK[0] +=  RotR1(PPK[5] ^ TK16(6));
 700         PPK[1] +=  RotR1(PPK[0] ^ TK16(7));   /* Use all of TK[] in Phase2   */
 701         PPK[2] +=  RotR1(PPK[1]);
 702         PPK[3] +=  RotR1(PPK[2]);
 703         PPK[4] +=  RotR1(PPK[3]);
 704         PPK[5] +=  RotR1(PPK[4]);
 705         /* Note: At this point, for a given key TK[0..15], the 96-bit output */
 706         /*       value PPK[0..5] is guaranteed to be unique, as a function   */
 707         /*       of the 96-bit "input" value   {TA,IV32,IV16}. That is, P1K  */
 708         /*       is now a keyed permutation of {TA,IV32,IV16}.               */
 709
 710         /* Set RC4KEY[0..3], which includes "cleartext" portion of RC4 key   */
 711         rc4key[0] = Hi8(iv16);                /* RC4KEY[0..2] is the WEP IV  */
 712         rc4key[1] =(Hi8(iv16) | 0x20) & 0x7F; /* Help avoid weak (FMS) keys  */
 713         rc4key[2] = Lo8(iv16);
 714         rc4key[3] = Lo8((PPK[5] ^ TK16(0)) >> 1);
 715
 716
 717         /* Copy 96 bits of PPK[0..5] to RC4KEY[4..15]  (little-endian)       */
 718         for (i=0;i<6;i++)
 719         {
 720                 rc4key[4+2*i] = Lo8(PPK[i]);
 721                 rc4key[5+2*i] = Hi8(PPK[i]);
 722         }
 723 _func_exit_;
 724 }
 725
 726
 727 //The hlen isn't include the IV
 728 u32     rtw_tkip_encrypt(_adapter *padapter, u8 *pxmitframe)
 729 {                                                                                                                                       // exclude ICV
 730         u16     pnl;
 731         u32     pnh;
 732         u8      rc4key[16];
 733         u8   ttkey[16];
 734         u8      crc[4];
 735         u8   hw_hdr_offset = 0;
 736         struct arc4context mycontext;
 737         sint                    curfragnum,length;
 738         u32     prwskeylen;
 739
 740         u8      *pframe, *payload,*iv,*prwskey;
 741         union pn48 dot11txpn;
 742         //struct        sta_info                *stainfo;
 743         struct  pkt_attrib       *pattrib = &((struct xmit_frame *)pxmitframe)->attrib;
 744         struct  security_priv   *psecuritypriv=&padapter->securitypriv;
 745         struct  xmit_priv               *pxmitpriv=&padapter->xmitpriv;
 746         u32     res=_SUCCESS;
 747 _func_enter_;
 748
 749         if(((struct xmit_frame*)pxmitframe)->buf_addr==NULL)
 750                 return _FAIL;
 751
 752 #ifdef CONFIG_USB_TX_AGGREGATION
 753         hw_hdr_offset = TXDESC_SIZE +
 754                  (((struct xmit_frame*)pxmitframe)->pkt_offset * PACKET_OFFSET_SZ);
 755 #else
 756         #ifdef CONFIG_TX_EARLY_MODE
 757         hw_hdr_offset = TXDESC_OFFSET+EARLY_MODE_INFO_SIZE;
 758         #else
 759         hw_hdr_offset = TXDESC_OFFSET;
 760         #endif
 761 #endif
 762
 763         pframe = ((struct xmit_frame*)pxmitframe)->buf_addr + hw_hdr_offset;
 764         //4 start to encrypt each fragment
 765         if(pattrib->encrypt==_TKIP_){
 766
 767 /*
 768                 if(pattrib->psta)
 769                 {
 770                         stainfo = pattrib->psta;
 771                 }
 772                 else
 773                 {
 774                         DBG_871X("%s, call rtw_get_stainfo()\n", __func__);
 775                         stainfo=rtw_get_stainfo(&padapter->stapriv ,&pattrib->ra[0] );
 776                 }
 777 */
 778                 //if (stainfo!=NULL)
 779                 {
 780 /*
 781                         if(!(stainfo->state &_FW_LINKED))
 782                         {
 783                                 DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, stainfo->state);
 784                                 return _FAIL;
 785                         }
 786 */
 787                         RT_TRACE(_module_rtl871x_security_c_,_drv_err_,("rtw_tkip_encrypt: stainfo!=NULL!!!\n"));
 788
 789                         if(IS_MCAST(pattrib->ra))
 790                         {
 791                                 prwskey=psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey;
 792                         }
 793                         else
 794                         {
 795                                 //prwskey=&stainfo->dot118021x_UncstKey.skey[0];
 796                                 prwskey=pattrib->dot118021x_UncstKey.skey;
 797                         }
 798
 799                         prwskeylen=16;
 800
 801                         for(curfragnum=0;curfragnum<pattrib->nr_frags;curfragnum++){
 802                                 iv=pframe+pattrib->hdrlen;
 803                                 payload=pframe+pattrib->iv_len+pattrib->hdrlen;
 804
 805                                 GET_TKIP_PN(iv, dot11txpn);
 806
 807                                 pnl=(u16)(dot11txpn.val);
 808                                 pnh=(u32)(dot11txpn.val>>16);
 809
 810                                 phase1((u16 *)&ttkey[0],prwskey,&pattrib->ta[0],pnh);
 811
 812                                 phase2(&rc4key[0],prwskey,(u16 *)&ttkey[0],pnl);
 813
 814                                 if((curfragnum+1)==pattrib->nr_frags){  //4 the last fragment
 815                                         length=pattrib->last_txcmdsz-pattrib->hdrlen-pattrib->iv_len- pattrib->icv_len;
 816                                         RT_TRACE(_module_rtl871x_security_c_,_drv_info_,("pattrib->iv_len =%x, pattrib->icv_len =%x\n", pattrib->iv_len,pattrib->icv_len));
 817                                         *((u32 *)crc)=cpu_to_le32(getcrc32(payload,length));/* modified by Amy*/
 818
 819                                         arcfour_init(&mycontext, rc4key,16);
 820                                         arcfour_encrypt(&mycontext, payload, payload, length);
 821                                         arcfour_encrypt(&mycontext, payload+length, crc, 4);
 822
 823                                 }
 824                                 else{
 825                                         length=pxmitpriv->frag_len-pattrib->hdrlen-pattrib->iv_len-pattrib->icv_len ;
 826                                         *((u32 *)crc)=cpu_to_le32(getcrc32(payload,length));/* modified by Amy*/
 827                                         arcfour_init(&mycontext,rc4key,16);
 828                                         arcfour_encrypt(&mycontext, payload, payload, length);
 829                                         arcfour_encrypt(&mycontext, payload+length, crc, 4);
 830
 831                                 pframe+=pxmitpriv->frag_len;
 832                                 pframe=(u8 *)RND4((SIZE_PTR)(pframe));
 833
 834                                 }
 835                         }
 836
 837                         TKIP_SW_ENC_CNT_INC(psecuritypriv,pattrib->ra);
 838                 }
 839 /*
 840                 else{
 841                         RT_TRACE(_module_rtl871x_security_c_,_drv_err_,("rtw_tkip_encrypt: stainfo==NULL!!!\n"));
 842                         DBG_871X("%s, psta==NUL\n", __func__);
 843                         res=_FAIL;
 844                 }
 845 */
 846
 847         }
 848 _func_exit_;
 849         return res;
 850
 851 }
 852
 853
 854 //The hlen isn't include the IV
 855 u32 rtw_tkip_decrypt(_adapter *padapter, u8 *precvframe)
 856 {                                                                                                                                       // exclude ICV
 857         u16 pnl;
 858         u32 pnh;
 859         u8   rc4key[16];
 860         u8   ttkey[16];
 861         u8      crc[4];
 862         struct arc4context mycontext;
 863         sint                    length;
 864         u32     prwskeylen;
 865
 866         u8      *pframe, *payload,*iv,*prwskey;
 867         union pn48 dot11txpn;
 868         struct  sta_info                *stainfo;
 869         struct  rx_pkt_attrib    *prxattrib = &((union recv_frame *)precvframe)->u.hdr.attrib;
 870         struct  security_priv   *psecuritypriv=&padapter->securitypriv;
 871 //      struct  recv_priv               *precvpriv=&padapter->recvpriv;
 872         u32             res=_SUCCESS;
 873
 874 _func_enter_;
 875
 876         pframe=(unsigned char *)((union recv_frame*)precvframe)->u.hdr.rx_data;
 877
 878         //4 start to decrypt recvframe
 879         if(prxattrib->encrypt==_TKIP_){
 880
 881                 stainfo=rtw_get_stainfo(&padapter->stapriv ,&prxattrib->ta[0] );
 882                 if (stainfo!=NULL){
 883
 884                         if(IS_MCAST(prxattrib->ra))
 885                         {
 886                                 static u32 start = 0;
 887                                 static u32 no_gkey_bc_cnt = 0;
 888                                 static u32 no_gkey_mc_cnt = 0;
 889
 890                                 if(psecuritypriv->binstallGrpkey==_FALSE)
 891                                 {
 892                                         res=_FAIL;
 893
 894                                         if (start == 0)
 895                                                 start = rtw_get_current_time();
 896
 897                                         if (is_broadcast_mac_addr(prxattrib->ra))
 898                                                 no_gkey_bc_cnt++;
 899                                         else
 900                                                 no_gkey_mc_cnt++;
 901
 902                                         if (rtw_get_passing_time_ms(start) > 1000) {
 903                                                 if (no_gkey_bc_cnt || no_gkey_mc_cnt) {
 904                                                         DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" no_gkey_bc_cnt:%u, no_gkey_mc_cnt:%u\n",
 905                                                                 FUNC_ADPT_ARG(padapter), no_gkey_bc_cnt, no_gkey_mc_cnt);
 906                                                 }
 907                                                 start = rtw_get_current_time();
 908                                                 no_gkey_bc_cnt = 0;
 909                                                 no_gkey_mc_cnt = 0;
 910                                         }
 911                                         goto exit;
 912                                 }
 913
 914                                 if (no_gkey_bc_cnt || no_gkey_mc_cnt) {
 915                                         DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" gkey installed. no_gkey_bc_cnt:%u, no_gkey_mc_cnt:%u\n",
 916                                                 FUNC_ADPT_ARG(padapter), no_gkey_bc_cnt, no_gkey_mc_cnt);
 917                                 }
 918                                 start = 0;
 919                                 no_gkey_bc_cnt = 0;
 920                                 no_gkey_mc_cnt = 0;
 921
 922                                 //DBG_871X("rx bc/mc packets, to perform sw rtw_tkip_decrypt\n");
 923                                 //prwskey = psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey;
 924                                 prwskey = psecuritypriv->dot118021XGrpKey[prxattrib->key_index].skey;
 925                                 prwskeylen=16;
 926                         }
 927                         else
 928                         {
 929                                 prwskey=&stainfo->dot118021x_UncstKey.skey[0];
 930                                 prwskeylen=16;
 931                         }
 932
 933                         iv=pframe+prxattrib->hdrlen;
 934                         payload=pframe+prxattrib->iv_len+prxattrib->hdrlen;
 935                         length= ((union recv_frame *)precvframe)->u.hdr.len-prxattrib->hdrlen-prxattrib->iv_len;
 936
 937                         GET_TKIP_PN(iv, dot11txpn);
 938
 939                         pnl=(u16)(dot11txpn.val);
 940                         pnh=(u32)(dot11txpn.val>>16);
 941
 942                         phase1((u16 *)&ttkey[0],prwskey,&prxattrib->ta[0],pnh);
 943                         phase2(&rc4key[0],prwskey,(unsigned short *)&ttkey[0],pnl);
 944
 945                         //4 decrypt payload include icv
 946
 947                         arcfour_init(&mycontext, rc4key,16);
 948                         arcfour_encrypt(&mycontext, payload, payload, length);
 949
 950                         *((u32 *)crc)=le32_to_cpu(getcrc32(payload,length-4));
 951
 952                         if(crc[3]!=payload[length-1] || crc[2]!=payload[length-2] || crc[1]!=payload[length-3] || crc[0]!=payload[length-4])
 953                         {
 954                             RT_TRACE(_module_rtl871x_security_c_,_drv_err_,("rtw_wep_decrypt:icv error crc[3](%x)!=payload[length-1](%x) || crc[2](%x)!=payload[length-2](%x) || crc[1](%x)!=payload[length-3](%x) || crc[0](%x)!=payload[length-4](%x)\n",
 955                                                 crc[3],payload[length-1],crc[2],payload[length-2],crc[1],payload[length-3],crc[0],payload[length-4]));
 956                                 res=_FAIL;
 957                         }
 958
 959                         TKIP_SW_DEC_CNT_INC(psecuritypriv, prxattrib->ra);
 960                 }
 961                 else{
 962                         RT_TRACE(_module_rtl871x_security_c_,_drv_err_,("rtw_tkip_decrypt: stainfo==NULL!!!\n"));
 963                         res=_FAIL;
 964                 }
 965
 966         }
 967 _func_exit_;
 968 exit:
 969         return res;
 970
 971 }
 972
 973
 974 //3                     =====AES related=====
 975
 976
 977
 978 #define MAX_MSG_SIZE    2048
 979 /*****************************/
 980 /******** SBOX Table *********/
 981 /*****************************/
 982
 983   static  u8 sbox_table[256] =
 984     {
 985         0x63, 0x7c, 0x77, 0x7b, 0xf2, 0x6b, 0x6f, 0xc5,
 986         0x30, 0x01, 0x67, 0x2b, 0xfe, 0xd7, 0xab, 0x76,
 987         0xca, 0x82, 0xc9, 0x7d, 0xfa, 0x59, 0x47, 0xf0,
 988         0xad, 0xd4, 0xa2, 0xaf, 0x9c, 0xa4, 0x72, 0xc0,
 989         0xb7, 0xfd, 0x93, 0x26, 0x36, 0x3f, 0xf7, 0xcc,
 990         0x34, 0xa5, 0xe5, 0xf1, 0x71, 0xd8, 0x31, 0x15,
 991         0x04, 0xc7, 0x23, 0xc3, 0x18, 0x96, 0x05, 0x9a,
 992         0x07, 0x12, 0x80, 0xe2, 0xeb, 0x27, 0xb2, 0x75,
 993         0x09, 0x83, 0x2c, 0x1a, 0x1b, 0x6e, 0x5a, 0xa0,
 994         0x52, 0x3b, 0xd6, 0xb3, 0x29, 0xe3, 0x2f, 0x84,
 995         0x53, 0xd1, 0x00, 0xed, 0x20, 0xfc, 0xb1, 0x5b,
 996         0x6a, 0xcb, 0xbe, 0x39, 0x4a, 0x4c, 0x58, 0xcf,
 997         0xd0, 0xef, 0xaa, 0xfb, 0x43, 0x4d, 0x33, 0x85,
 998         0x45, 0xf9, 0x02, 0x7f, 0x50, 0x3c, 0x9f, 0xa8,
 999         0x51, 0xa3, 0x40, 0x8f, 0x92, 0x9d, 0x38, 0xf5,
1000         0xbc, 0xb6, 0xda, 0x21, 0x10, 0xff, 0xf3, 0xd2,
1001         0xcd, 0x0c, 0x13, 0xec, 0x5f, 0x97, 0x44, 0x17,
1002         0xc4, 0xa7, 0x7e, 0x3d, 0x64, 0x5d, 0x19, 0x73,
1003         0x60, 0x81, 0x4f, 0xdc, 0x22, 0x2a, 0x90, 0x88,
1004         0x46, 0xee, 0xb8, 0x14, 0xde, 0x5e, 0x0b, 0xdb,
1005         0xe0, 0x32, 0x3a, 0x0a, 0x49, 0x06, 0x24, 0x5c,
1006         0xc2, 0xd3, 0xac, 0x62, 0x91, 0x95, 0xe4, 0x79,
1007         0xe7, 0xc8, 0x37, 0x6d, 0x8d, 0xd5, 0x4e, 0xa9,
1008         0x6c, 0x56, 0xf4, 0xea, 0x65, 0x7a, 0xae, 0x08,
1009         0xba, 0x78, 0x25, 0x2e, 0x1c, 0xa6, 0xb4, 0xc6,
1010         0xe8, 0xdd, 0x74, 0x1f, 0x4b, 0xbd, 0x8b, 0x8a,
1011         0x70, 0x3e, 0xb5, 0x66, 0x48, 0x03, 0xf6, 0x0e,
1012         0x61, 0x35, 0x57, 0xb9, 0x86, 0xc1, 0x1d, 0x9e,
1013         0xe1, 0xf8, 0x98, 0x11, 0x69, 0xd9, 0x8e, 0x94,
1014         0x9b, 0x1e, 0x87, 0xe9, 0xce, 0x55, 0x28, 0xdf,
1015         0x8c, 0xa1, 0x89, 0x0d, 0xbf, 0xe6, 0x42, 0x68,
1016         0x41, 0x99, 0x2d, 0x0f, 0xb0, 0x54, 0xbb, 0x16
1017     };
1018
1019 /*****************************/
1020 /**** Function Prototypes ****/
1021 /*****************************/
1022
1023 static void bitwise_xor(u8 *ina, u8 *inb, u8 *out);
1024 static void construct_mic_iv(
1025                         u8 *mic_header1,
1026                         sint qc_exists,
1027                         sint a4_exists,
1028                         u8 *mpdu,
1029                         uint payload_length,
1030                         u8 * pn_vector,
1031                         uint frtype);// add for CONFIG_IEEE80211W, none 11w also can use
1032 static void construct_mic_header1(
1033                         u8 *mic_header1,
1034                         sint header_length,
1035                         u8 *mpdu,
1036                         uint frtype);// add for CONFIG_IEEE80211W, none 11w also can use
1037 static void construct_mic_header2(
1038                     u8 *mic_header2,
1039                     u8 *mpdu,
1040                     sint a4_exists,
1041                     sint qc_exists);
1042 static void construct_ctr_preload(
1043                         u8 *ctr_preload,
1044                         sint a4_exists,
1045                         sint qc_exists,
1046                         u8 *mpdu,
1047                         u8 *pn_vector,
1048                         sint c,
1049                         uint frtype);// add for CONFIG_IEEE80211W, none 11w also can use
1050 static void xor_128(u8 *a, u8 *b, u8 *out);
1051 static void xor_32(u8 *a, u8 *b, u8 *out);
1052 static u8 sbox(u8 a);
1053 static void next_key(u8 *key, sint round);
1054 static void byte_sub(u8 *in, u8 *out);
1055 static void shift_row(u8 *in, u8 *out);
1056 static void mix_column(u8 *in, u8 *out);
1057 #ifndef PLATFORM_FREEBSD
1058 static void add_round_key( u8 *shiftrow_in,
1059                     u8 *mcol_in,
1060                     u8 *block_in,
1061                     sint round,
1062                     u8 *out);
1063 #endif //PLATFORM_FREEBSD
1064 static void aes128k128d(u8 *key, u8 *data, u8 *ciphertext);
1065
1066
1067 /****************************************/
1068 /* aes128k128d()                        */
1069 /* Performs a 128 bit AES encrypt with  */
1070 /* 128 bit data.                        */
1071 /****************************************/
1072 static void xor_128(u8 *a, u8 *b, u8 *out)
1073 {
1074     sint i;
1075 _func_enter_;
1076     for (i=0;i<16; i++)
1077     {
1078         out[i] = a[i] ^ b[i];
1079     }
1080 _func_exit_;
1081 }
1082
1083
1084 static void xor_32(u8 *a, u8 *b, u8 *out)
1085 {
1086     sint i;
1087 _func_enter_;
1088     for (i=0;i<4; i++)
1089     {
1090         out[i] = a[i] ^ b[i];
1091     }
1092 _func_exit_;
1093 }
1094
1095
1096 static u8 sbox(u8 a)
1097 {
1098     return sbox_table[(sint)a];
1099 }
1100
1101
1102 static void next_key(u8 *key, sint round)
1103 {
1104     u8 rcon;
1105     u8 sbox_key[4];
1106     u8 rcon_table[12] =
1107     {
1108         0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80,
1109         0x1b, 0x36, 0x36, 0x36
1110     };
1111 _func_enter_;
1112     sbox_key[0] = sbox(key[13]);
1113     sbox_key[1] = sbox(key[14]);
1114     sbox_key[2] = sbox(key[15]);
1115     sbox_key[3] = sbox(key[12]);
1116
1117     rcon = rcon_table[round];
1118
1119     xor_32(&key[0], sbox_key, &key[0]);
1120     key[0] = key[0] ^ rcon;
1121
1122     xor_32(&key[4], &key[0], &key[4]);
1123     xor_32(&key[8], &key[4], &key[8]);
1124     xor_32(&key[12], &key[8], &key[12]);
1125 _func_exit_;
1126 }
1127
1128
1129 static void byte_sub(u8 *in, u8 *out)
1130 {
1131     sint i;
1132 _func_enter_;
1133     for (i=0; i< 16; i++)
1134     {
1135         out[i] = sbox(in[i]);
1136     }
1137 _func_exit_;
1138 }
1139
1140
1141 static void shift_row(u8 *in, u8 *out)
1142 {
1143 _func_enter_;
1144     out[0] =  in[0];
1145     out[1] =  in[5];
1146     out[2] =  in[10];
1147     out[3] =  in[15];
1148     out[4] =  in[4];
1149     out[5] =  in[9];
1150     out[6] =  in[14];
1151     out[7] =  in[3];
1152     out[8] =  in[8];
1153     out[9] =  in[13];
1154     out[10] = in[2];
1155     out[11] = in[7];
1156     out[12] = in[12];
1157     out[13] = in[1];
1158     out[14] = in[6];
1159     out[15] = in[11];
1160 _func_exit_;
1161 }
1162
1163
1164 static void mix_column(u8 *in, u8 *out)
1165 {
1166     sint i;
1167     u8 add1b[4];
1168     u8 add1bf7[4];
1169     u8 rotl[4];
1170     u8 swap_halfs[4];
1171     u8 andf7[4];
1172     u8 rotr[4];
1173     u8 temp[4];
1174     u8 tempb[4];
1175 _func_enter_;
1176     for (i=0 ; i<4; i++)
1177     {
1178         if ((in[i] & 0x80)== 0x80)
1179             add1b[i] = 0x1b;
1180         else
1181             add1b[i] = 0x00;
1182     }
1183
1184     swap_halfs[0] = in[2];    /* Swap halfs */
1185     swap_halfs[1] = in[3];
1186     swap_halfs[2] = in[0];
1187     swap_halfs[3] = in[1];
1188
1189     rotl[0] = in[3];        /* Rotate left 8 bits */
1190     rotl[1] = in[0];
1191     rotl[2] = in[1];
1192     rotl[3] = in[2];
1193
1194     andf7[0] = in[0] & 0x7f;
1195     andf7[1] = in[1] & 0x7f;
1196     andf7[2] = in[2] & 0x7f;
1197     andf7[3] = in[3] & 0x7f;
1198
1199     for (i = 3; i>0; i--)    /* logical shift left 1 bit */
1200     {
1201         andf7[i] = andf7[i] << 1;
1202         if ((andf7[i-1] & 0x80) == 0x80)
1203         {
1204             andf7[i] = (andf7[i] | 0x01);
1205         }
1206     }
1207     andf7[0] = andf7[0] << 1;
1208     andf7[0] = andf7[0] & 0xfe;
1209
1210     xor_32(add1b, andf7, add1bf7);
1211
1212     xor_32(in, add1bf7, rotr);
1213
1214     temp[0] = rotr[0];         /* Rotate right 8 bits */
1215     rotr[0] = rotr[1];
1216     rotr[1] = rotr[2];
1217     rotr[2] = rotr[3];
1218     rotr[3] = temp[0];
1219
1220     xor_32(add1bf7, rotr, temp);
1221     xor_32(swap_halfs, rotl,tempb);
1222     xor_32(temp, tempb, out);
1223 _func_exit_;
1224 }
1225
1226
1227 static void aes128k128d(u8 *key, u8 *data, u8 *ciphertext)
1228 {
1229     sint round;
1230     sint i;
1231     u8 intermediatea[16];
1232     u8 intermediateb[16];
1233     u8 round_key[16];
1234 _func_enter_;
1235     for(i=0; i<16; i++) round_key[i] = key[i];
1236
1237     for (round = 0; round < 11; round++)
1238     {
1239         if (round == 0)
1240         {
1241             xor_128(round_key, data, ciphertext);
1242             next_key(round_key, round);
1243         }
1244         else if (round == 10)
1245         {
1246             byte_sub(ciphertext, intermediatea);
1247             shift_row(intermediatea, intermediateb);
1248             xor_128(intermediateb, round_key, ciphertext);
1249         }
1250         else    /* 1 - 9 */
1251         {
1252             byte_sub(ciphertext, intermediatea);
1253             shift_row(intermediatea, intermediateb);
1254             mix_column(&intermediateb[0], &intermediatea[0]);
1255             mix_column(&intermediateb[4], &intermediatea[4]);
1256             mix_column(&intermediateb[8], &intermediatea[8]);
1257             mix_column(&intermediateb[12], &intermediatea[12]);
1258             xor_128(intermediatea, round_key, ciphertext);
1259             next_key(round_key, round);
1260         }
1261     }
1262 _func_exit_;
1263 }
1264
1265
1266 /************************************************/
1267 /* construct_mic_iv()                           */
1268 /* Builds the MIC IV from header fields and PN  */
1269 /* Baron think the function is construct CCM    */
1270 /* nonce                                        */
1271 /************************************************/
1272 static void construct_mic_iv(
1273                         u8 *mic_iv,
1274                         sint qc_exists,
1275                         sint a4_exists,
1276                         u8 *mpdu,
1277                         uint payload_length,
1278                         u8 *pn_vector,
1279                         uint frtype// add for CONFIG_IEEE80211W, none 11w also can use
1280                         )
1281 {
1282     sint i;
1283 _func_enter_;
1284     mic_iv[0] = 0x59;
1285     if (qc_exists && a4_exists) mic_iv[1] = mpdu[30] & 0x0f;    /* QoS_TC           */
1286     if (qc_exists && !a4_exists) mic_iv[1] = mpdu[24] & 0x0f;   /* mute bits 7-4    */
1287     if (!qc_exists) mic_iv[1] = 0x00;
1288 #ifdef CONFIG_IEEE80211W
1289         //802.11w management frame should set management bit(4)
1290     if(frtype == WIFI_MGT_TYPE)
1291                 mic_iv[1] |= BIT(4);
1292 #endif //CONFIG_IEEE80211W
1293     for (i = 2; i < 8; i++)
1294         mic_iv[i] = mpdu[i + 8];                    /* mic_iv[2:7] = A2[0:5] = mpdu[10:15] */
1295     #ifdef CONSISTENT_PN_ORDER
1296         for (i = 8; i < 14; i++)
1297             mic_iv[i] = pn_vector[i - 8];           /* mic_iv[8:13] = PN[0:5] */
1298     #else
1299         for (i = 8; i < 14; i++)
1300             mic_iv[i] = pn_vector[13 - i];          /* mic_iv[8:13] = PN[5:0] */
1301     #endif
1302     mic_iv[14] = (unsigned char) (payload_length / 256);
1303     mic_iv[15] = (unsigned char) (payload_length % 256);
1304 _func_exit_;
1305 }
1306
1307
1308 /************************************************/
1309 /* construct_mic_header1()                      */
1310 /* Builds the first MIC header block from       */
1311 /* header fields.                               */
1312 /* Build AAD SC,A1,A2                           */
1313 /************************************************/
1314 static void construct_mic_header1(
1315                         u8 *mic_header1,
1316                         sint header_length,
1317                         u8 *mpdu,
1318                         uint frtype// add for CONFIG_IEEE80211W, none 11w also can use
1319                         )
1320 {
1321 _func_enter_;
1322     mic_header1[0] = (u8)((header_length - 2) / 256);
1323     mic_header1[1] = (u8)((header_length - 2) % 256);
1324 #ifdef CONFIG_IEEE80211W
1325     //802.11w management frame don't AND subtype bits 4,5,6 of frame control field
1326     if(frtype == WIFI_MGT_TYPE)
1327                 mic_header1[2] = mpdu[0];
1328         else
1329 #endif //CONFIG_IEEE80211W
1330                 mic_header1[2] = mpdu[0] & 0xcf;    /* Mute CF poll & CF ack bits */
1331
1332     mic_header1[3] = mpdu[1] & 0xc7;    /* Mute retry, more data and pwr mgt bits */
1333     mic_header1[4] = mpdu[4];       /* A1 */
1334     mic_header1[5] = mpdu[5];
1335     mic_header1[6] = mpdu[6];
1336     mic_header1[7] = mpdu[7];
1337     mic_header1[8] = mpdu[8];
1338     mic_header1[9] = mpdu[9];
1339     mic_header1[10] = mpdu[10];     /* A2 */
1340     mic_header1[11] = mpdu[11];
1341     mic_header1[12] = mpdu[12];
1342     mic_header1[13] = mpdu[13];
1343     mic_header1[14] = mpdu[14];
1344     mic_header1[15] = mpdu[15];
1345 _func_exit_;
1346 }
1347
1348
1349 /************************************************/
1350 /* construct_mic_header2()                      */
1351 /* Builds the last MIC header block from        */
1352 /* header fields.                               */
1353 /************************************************/
1354 static void construct_mic_header2(
1355                 u8 *mic_header2,
1356                 u8 *mpdu,
1357                 sint a4_exists,
1358                 sint qc_exists
1359                 )
1360 {
1361     sint i;
1362 _func_enter_;
1363     for (i = 0; i<16; i++) mic_header2[i]=0x00;
1364
1365     mic_header2[0] = mpdu[16];    /* A3 */
1366     mic_header2[1] = mpdu[17];
1367     mic_header2[2] = mpdu[18];
1368     mic_header2[3] = mpdu[19];
1369     mic_header2[4] = mpdu[20];
1370     mic_header2[5] = mpdu[21];
1371
1372     //mic_header2[6] = mpdu[22] & 0xf0;   /* SC */
1373     mic_header2[6] = 0x00;
1374     mic_header2[7] = 0x00; /* mpdu[23]; */
1375
1376
1377     if (!qc_exists && a4_exists)
1378     {
1379         for (i=0;i<6;i++) mic_header2[8+i] = mpdu[24+i];   /* A4 */
1380
1381     }
1382
1383     if (qc_exists && !a4_exists)
1384     {
1385         mic_header2[8] = mpdu[24] & 0x0f; /* mute bits 15 - 4 */
1386         mic_header2[9] = mpdu[25] & 0x00;
1387     }
1388
1389     if (qc_exists && a4_exists)
1390     {
1391         for (i=0;i<6;i++) mic_header2[8+i] = mpdu[24+i];   /* A4 */
1392
1393         mic_header2[14] = mpdu[30] & 0x0f;
1394         mic_header2[15] = mpdu[31] & 0x00;
1395     }
1396
1397 _func_exit_;
1398 }
1399
1400
1401 /************************************************/
1402 /* construct_mic_header2()                      */
1403 /* Builds the last MIC header block from        */
1404 /* header fields.                               */
1405 /* Baron think the function is construct CCM    */
1406 /* nonce                                        */
1407 /************************************************/
1408 static void construct_ctr_preload(
1409                         u8 *ctr_preload,
1410                         sint a4_exists,
1411                         sint qc_exists,
1412                         u8 *mpdu,
1413                         u8 *pn_vector,
1414                         sint c,
1415                         uint frtype // add for CONFIG_IEEE80211W, none 11w also can use
1416                         )
1417 {
1418     sint i = 0;
1419 _func_enter_;
1420     for (i=0; i<16; i++) ctr_preload[i] = 0x00;
1421     i = 0;
1422
1423     ctr_preload[0] = 0x01;                                  /* flag */
1424     if (qc_exists && a4_exists)
1425                 ctr_preload[1] = mpdu[30] & 0x0f;   /* QoC_Control */
1426     if (qc_exists && !a4_exists)
1427                 ctr_preload[1] = mpdu[24] & 0x0f;
1428 #ifdef CONFIG_IEEE80211W
1429         //802.11w management frame should set management bit(4)
1430         if(frtype == WIFI_MGT_TYPE)
1431                 ctr_preload[1] |= BIT(4);
1432 #endif //CONFIG_IEEE80211W
1433     for (i = 2; i < 8; i++)
1434         ctr_preload[i] = mpdu[i + 8];                       /* ctr_preload[2:7] = A2[0:5] = mpdu[10:15] */
1435     #ifdef CONSISTENT_PN_ORDER
1436       for (i = 8; i < 14; i++)
1437             ctr_preload[i] =    pn_vector[i - 8];           /* ctr_preload[8:13] = PN[0:5] */
1438     #else
1439       for (i = 8; i < 14; i++)
1440             ctr_preload[i] =    pn_vector[13 - i];          /* ctr_preload[8:13] = PN[5:0] */
1441     #endif
1442     ctr_preload[14] =  (unsigned char) (c / 256); /* Ctr */
1443     ctr_preload[15] =  (unsigned char) (c % 256);
1444 _func_exit_;
1445 }
1446
1447
1448 /************************************/
1449 /* bitwise_xor()                    */
1450 /* A 128 bit, bitwise exclusive or  */
1451 /************************************/
1452 static void bitwise_xor(u8 *ina, u8 *inb, u8 *out)
1453 {
1454     sint i;
1455 _func_enter_;
1456     for (i=0; i<16; i++)
1457     {
1458         out[i] = ina[i] ^ inb[i];
1459     }
1460 _func_exit_;
1461 }
1462
1463
1464 static sint aes_cipher(u8 *key, uint    hdrlen,
1465                         u8 *pframe, uint plen)
1466 {
1467 //      /*static*/ unsigned char        message[MAX_MSG_SIZE];
1468         uint    qc_exists, a4_exists, i, j, payload_remainder,
1469                 num_blocks, payload_index;
1470
1471         u8 pn_vector[6];
1472         u8 mic_iv[16];
1473         u8 mic_header1[16];
1474         u8 mic_header2[16];
1475         u8 ctr_preload[16];
1476
1477         /* Intermediate Buffers */
1478         u8 chain_buffer[16];
1479         u8 aes_out[16];
1480         u8 padded_buffer[16];
1481         u8 mic[8];
1482 //      uint    offset = 0;
1483         uint    frtype  = GetFrameType(pframe);
1484         uint    frsubtype  = GetFrameSubType(pframe);
1485
1486 _func_enter_;
1487         frsubtype=frsubtype>>4;
1488
1489
1490         _rtw_memset((void *)mic_iv, 0, 16);
1491         _rtw_memset((void *)mic_header1, 0, 16);
1492         _rtw_memset((void *)mic_header2, 0, 16);
1493         _rtw_memset((void *)ctr_preload, 0, 16);
1494         _rtw_memset((void *)chain_buffer, 0, 16);
1495         _rtw_memset((void *)aes_out, 0, 16);
1496         _rtw_memset((void *)padded_buffer, 0, 16);
1497
1498         if ((hdrlen == WLAN_HDR_A3_LEN )||(hdrlen ==  WLAN_HDR_A3_QOS_LEN))
1499                 a4_exists = 0;
1500         else
1501                 a4_exists = 1;
1502
1503         if (
1504                 ((frtype|frsubtype) == WIFI_DATA_CFACK) ||
1505                 ((frtype|frsubtype) == WIFI_DATA_CFPOLL)||
1506                 ((frtype|frsubtype) == WIFI_DATA_CFACKPOLL))
1507                 {
1508                         qc_exists = 1;
1509                                         if(hdrlen !=  WLAN_HDR_A3_QOS_LEN){
1510
1511                                         hdrlen += 2;
1512                         }
1513                 }
1514         // add for CONFIG_IEEE80211W, none 11w also can use
1515         else if ((frtype == WIFI_DATA) &&
1516                 ((frsubtype == 0x08) ||
1517                 (frsubtype == 0x09)||
1518                 (frsubtype == 0x0a)||
1519                 (frsubtype == 0x0b)))
1520                 {
1521                         if(hdrlen !=  WLAN_HDR_A3_QOS_LEN){
1522
1523                                         hdrlen += 2;
1524                         }
1525                         qc_exists = 1;
1526                 }
1527         else
1528                 qc_exists = 0;
1529
1530         pn_vector[0]=pframe[hdrlen];
1531         pn_vector[1]=pframe[hdrlen+1];
1532         pn_vector[2]=pframe[hdrlen+4];
1533         pn_vector[3]=pframe[hdrlen+5];
1534         pn_vector[4]=pframe[hdrlen+6];
1535         pn_vector[5]=pframe[hdrlen+7];
1536
1537         construct_mic_iv(
1538                         mic_iv,
1539                         qc_exists,
1540                         a4_exists,
1541                         pframe,  //message,
1542                         plen,
1543                         pn_vector,
1544                         frtype // add for CONFIG_IEEE80211W, none 11w also can use
1545                         );
1546
1547     construct_mic_header1(
1548                             mic_header1,
1549                             hdrlen,
1550                             pframe,     //message
1551                             frtype // add for CONFIG_IEEE80211W, none 11w also can use
1552                             );
1553     construct_mic_header2(
1554                             mic_header2,
1555                             pframe,     //message,
1556                             a4_exists,
1557                             qc_exists
1558                             );
1559
1560
1561         payload_remainder = plen % 16;
1562     num_blocks = plen / 16;
1563
1564     /* Find start of payload */
1565     payload_index = (hdrlen + 8);
1566
1567     /* Calculate MIC */
1568     aes128k128d(key, mic_iv, aes_out);
1569     bitwise_xor(aes_out, mic_header1, chain_buffer);
1570     aes128k128d(key, chain_buffer, aes_out);
1571     bitwise_xor(aes_out, mic_header2, chain_buffer);
1572     aes128k128d(key, chain_buffer, aes_out);
1573
1574         for (i = 0; i < num_blocks; i++)
1575     {
1576         bitwise_xor(aes_out, &pframe[payload_index], chain_buffer);//bitwise_xor(aes_out, &message[payload_index], chain_buffer);
1577
1578         payload_index += 16;
1579         aes128k128d(key, chain_buffer, aes_out);
1580     }
1581
1582     /* Add on the final payload block if it needs padding */
1583     if (payload_remainder > 0)
1584     {
1585         for (j = 0; j < 16; j++) padded_buffer[j] = 0x00;
1586         for (j = 0; j < payload_remainder; j++)
1587         {
1588             padded_buffer[j] = pframe[payload_index++];//padded_buffer[j] = message[payload_index++];
1589         }
1590         bitwise_xor(aes_out, padded_buffer, chain_buffer);
1591         aes128k128d(key, chain_buffer, aes_out);
1592
1593     }
1594
1595     for (j = 0 ; j < 8; j++) mic[j] = aes_out[j];
1596
1597     /* Insert MIC into payload */
1598     for (j = 0; j < 8; j++)
1599         pframe[payload_index+j] = mic[j];       //message[payload_index+j] = mic[j];
1600
1601         payload_index = hdrlen + 8;
1602         for (i=0; i< num_blocks; i++)
1603     {
1604         construct_ctr_preload(
1605                                 ctr_preload,
1606                                 a4_exists,
1607                                 qc_exists,
1608                                 pframe, //message,
1609                                 pn_vector,
1610                                 i+1,
1611                                 frtype); // add for CONFIG_IEEE80211W, none 11w also can use
1612         aes128k128d(key, ctr_preload, aes_out);
1613         bitwise_xor(aes_out, &pframe[payload_index], chain_buffer);//bitwise_xor(aes_out, &message[payload_index], chain_buffer);
1614         for (j=0; j<16;j++) pframe[payload_index++] = chain_buffer[j];//for (j=0; j<16;j++) message[payload_index++] = chain_buffer[j];
1615     }
1616
1617     if (payload_remainder > 0)          /* If there is a short final block, then pad it,*/
1618     {                                   /* encrypt it and copy the unpadded part back   */
1619         construct_ctr_preload(
1620                                 ctr_preload,
1621                                 a4_exists,
1622                                 qc_exists,
1623                                 pframe, //message,
1624                                 pn_vector,
1625                                 num_blocks+1,
1626                                 frtype); // add for CONFIG_IEEE80211W, none 11w also can use
1627
1628         for (j = 0; j < 16; j++) padded_buffer[j] = 0x00;
1629         for (j = 0; j < payload_remainder; j++)
1630         {
1631             padded_buffer[j] = pframe[payload_index+j];//padded_buffer[j] = message[payload_index+j];
1632         }
1633         aes128k128d(key, ctr_preload, aes_out);
1634         bitwise_xor(aes_out, padded_buffer, chain_buffer);
1635         for (j=0; j<payload_remainder;j++) pframe[payload_index++] = chain_buffer[j];//for (j=0; j<payload_remainder;j++) message[payload_index++] = chain_buffer[j];
1636     }
1637
1638     /* Encrypt the MIC */
1639     construct_ctr_preload(
1640                         ctr_preload,
1641                         a4_exists,
1642                         qc_exists,
1643                         pframe, //message,
1644                         pn_vector,
1645                         0,
1646                         frtype); // add for CONFIG_IEEE80211W, none 11w also can use
1647
1648     for (j = 0; j < 16; j++) padded_buffer[j] = 0x00;
1649     for (j = 0; j < 8; j++)
1650     {
1651         padded_buffer[j] = pframe[j+hdrlen+8+plen];//padded_buffer[j] = message[j+hdrlen+8+plen];
1652     }
1653
1654     aes128k128d(key, ctr_preload, aes_out);
1655     bitwise_xor(aes_out, padded_buffer, chain_buffer);
1656     for (j=0; j<8;j++) pframe[payload_index++] = chain_buffer[j];//for (j=0; j<8;j++) message[payload_index++] = chain_buffer[j];
1657 _func_exit_;
1658         return _SUCCESS;
1659 }
1660
1661
1662
1663
1664
1665 u32     rtw_aes_encrypt(_adapter *padapter, u8 *pxmitframe)
1666 {       // exclude ICV
1667
1668
1669         /*static*/
1670 //      unsigned char   message[MAX_MSG_SIZE];
1671
1672         /* Intermediate Buffers */
1673         sint    curfragnum,length;
1674         u32     prwskeylen;
1675         u8      *pframe,*prwskey;       //, *payload,*iv
1676         u8   hw_hdr_offset = 0;
1677         //struct        sta_info                *stainfo=NULL;
1678         struct  pkt_attrib       *pattrib = &((struct xmit_frame *)pxmitframe)->attrib;
1679         struct  security_priv   *psecuritypriv=&padapter->securitypriv;
1680         struct  xmit_priv               *pxmitpriv=&padapter->xmitpriv;
1681
1682 //      uint    offset = 0;
1683         u32 res=_SUCCESS;
1684 _func_enter_;
1685
1686         if(((struct xmit_frame*)pxmitframe)->buf_addr==NULL)
1687                 return _FAIL;
1688
1689 #ifdef CONFIG_USB_TX_AGGREGATION
1690         hw_hdr_offset = TXDESC_SIZE +
1691                  (((struct xmit_frame*)pxmitframe)->pkt_offset * PACKET_OFFSET_SZ);
1692 #else
1693         #ifdef CONFIG_TX_EARLY_MODE
1694         hw_hdr_offset = TXDESC_OFFSET+EARLY_MODE_INFO_SIZE;
1695         #else
1696         hw_hdr_offset = TXDESC_OFFSET;
1697         #endif
1698 #endif
1699
1700         pframe = ((struct xmit_frame*)pxmitframe)->buf_addr + hw_hdr_offset;
1701
1702         //4 start to encrypt each fragment
1703         if((pattrib->encrypt==_AES_)){
1704 /*
1705                 if(pattrib->psta)
1706                 {
1707                         stainfo = pattrib->psta;
1708                 }
1709                 else
1710                 {
1711                         DBG_871X("%s, call rtw_get_stainfo()\n", __func__);
1712                         stainfo=rtw_get_stainfo(&padapter->stapriv ,&pattrib->ra[0] );
1713                 }
1714 */
1715                 //if (stainfo!=NULL)
1716                 {
1717 /*
1718                         if(!(stainfo->state &_FW_LINKED))
1719                         {
1720                                 DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, stainfo->state);
1721                                 return _FAIL;
1722                         }
1723 */
1724                         RT_TRACE(_module_rtl871x_security_c_,_drv_err_,("rtw_aes_encrypt: stainfo!=NULL!!!\n"));
1725
1726                         if(IS_MCAST(pattrib->ra))
1727                         {
1728                                 prwskey=psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey;
1729                         }
1730                         else
1731                         {
1732                                 //prwskey=&stainfo->dot118021x_UncstKey.skey[0];
1733                                 prwskey=pattrib->dot118021x_UncstKey.skey;
1734                         }
1735
1736 #ifdef CONFIG_TDLS      //swencryption
1737                         {
1738                                 struct  sta_info                *ptdls_sta;
1739                                 ptdls_sta=rtw_get_stainfo(&padapter->stapriv ,&pattrib->dst[0] );
1740                                 if((ptdls_sta != NULL) && (ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE) )
1741                                 {
1742                                         DBG_871X("[%s] for tdls link\n", __FUNCTION__);
1743                                         prwskey=&ptdls_sta->tpk.tk[0];
1744                                 }
1745                         }
1746 #endif //CONFIG_TDLS
1747
1748                         prwskeylen=16;
1749
1750                         for(curfragnum=0;curfragnum<pattrib->nr_frags;curfragnum++){
1751
1752                                 if((curfragnum+1)==pattrib->nr_frags){  //4 the last fragment
1753                                         length=pattrib->last_txcmdsz-pattrib->hdrlen-pattrib->iv_len- pattrib->icv_len;
1754
1755                                         aes_cipher(prwskey,pattrib->hdrlen,pframe, length);
1756                                 }
1757                                 else{
1758                                         length=pxmitpriv->frag_len-pattrib->hdrlen-pattrib->iv_len-pattrib->icv_len ;
1759
1760                                         aes_cipher(prwskey,pattrib->hdrlen,pframe, length);
1761                                 pframe+=pxmitpriv->frag_len;
1762                                 pframe=(u8*)RND4((SIZE_PTR)(pframe));
1763
1764                                 }
1765                         }
1766
1767                         AES_SW_ENC_CNT_INC(psecuritypriv, pattrib->ra);
1768                 }
1769 /*
1770                 else{
1771                         RT_TRACE(_module_rtl871x_security_c_,_drv_err_,("rtw_aes_encrypt: stainfo==NULL!!!\n"));
1772                         DBG_871X("%s, psta==NUL\n", __func__);
1773                         res=_FAIL;
1774                 }
1775 */
1776         }
1777
1778
1779
1780 _func_exit_;
1781                 return res;
1782 }
1783
1784 static sint aes_decipher(u8 *key, uint  hdrlen,
1785                         u8 *pframe, uint plen)
1786 {
1787         static u8       message[MAX_MSG_SIZE];
1788         uint    qc_exists, a4_exists, i, j, payload_remainder,
1789                         num_blocks, payload_index;
1790         sint res = _SUCCESS;
1791         u8 pn_vector[6];
1792         u8 mic_iv[16];
1793         u8 mic_header1[16];
1794         u8 mic_header2[16];
1795         u8 ctr_preload[16];
1796
1797     /* Intermediate Buffers */
1798         u8 chain_buffer[16];
1799         u8 aes_out[16];
1800         u8 padded_buffer[16];
1801         u8 mic[8];
1802
1803
1804 //      uint    offset = 0;
1805         uint    frtype  = GetFrameType(pframe);
1806         uint    frsubtype  = GetFrameSubType(pframe);
1807 _func_enter_;
1808         frsubtype=frsubtype>>4;
1809
1810
1811         _rtw_memset((void *)mic_iv, 0, 16);
1812         _rtw_memset((void *)mic_header1, 0, 16);
1813         _rtw_memset((void *)mic_header2, 0, 16);
1814         _rtw_memset((void *)ctr_preload, 0, 16);
1815         _rtw_memset((void *)chain_buffer, 0, 16);
1816         _rtw_memset((void *)aes_out, 0, 16);
1817         _rtw_memset((void *)padded_buffer, 0, 16);
1818
1819         //start to decrypt the payload
1820
1821         num_blocks = (plen-8) / 16; //(plen including LLC, payload_length and mic )
1822
1823         payload_remainder = (plen-8) % 16;
1824
1825         pn_vector[0]  = pframe[hdrlen];
1826         pn_vector[1]  = pframe[hdrlen+1];
1827         pn_vector[2]  = pframe[hdrlen+4];
1828         pn_vector[3]  = pframe[hdrlen+5];
1829         pn_vector[4]  = pframe[hdrlen+6];
1830         pn_vector[5]  = pframe[hdrlen+7];
1831
1832         if ((hdrlen == WLAN_HDR_A3_LEN )||(hdrlen ==  WLAN_HDR_A3_QOS_LEN))
1833                 a4_exists = 0;
1834         else
1835                 a4_exists = 1;
1836
1837         if (
1838                 ((frtype|frsubtype) == WIFI_DATA_CFACK) ||
1839                 ((frtype|frsubtype) == WIFI_DATA_CFPOLL)||
1840                 ((frtype|frsubtype) == WIFI_DATA_CFACKPOLL))
1841                 {
1842                         qc_exists = 1;
1843                                         if(hdrlen !=  WLAN_HDR_A3_QOS_LEN){
1844
1845                                         hdrlen += 2;
1846                         }
1847                 }//only for data packet . add for CONFIG_IEEE80211W, none 11w also can use
1848         else if ((frtype == WIFI_DATA) &&
1849                 ((frsubtype == 0x08) ||
1850                 (frsubtype == 0x09)||
1851                 (frsubtype == 0x0a)||
1852                 (frsubtype == 0x0b)))
1853                 {
1854                         if(hdrlen !=  WLAN_HDR_A3_QOS_LEN){
1855
1856                                         hdrlen += 2;
1857                         }
1858                         qc_exists = 1;
1859                 }
1860         else
1861                 qc_exists = 0;
1862
1863
1864         // now, decrypt pframe with hdrlen offset and plen long
1865
1866         payload_index = hdrlen + 8; // 8 is for extiv
1867
1868         for (i=0; i< num_blocks; i++)
1869     {
1870         construct_ctr_preload(
1871                                 ctr_preload,
1872                                 a4_exists,
1873                                 qc_exists,
1874                                 pframe,
1875                                 pn_vector,
1876                                 i+1,
1877                                 frtype // add for CONFIG_IEEE80211W, none 11w also can use
1878                             );
1879
1880         aes128k128d(key, ctr_preload, aes_out);
1881         bitwise_xor(aes_out, &pframe[payload_index], chain_buffer);
1882
1883         for (j=0; j<16;j++) pframe[payload_index++] = chain_buffer[j];
1884     }
1885
1886     if (payload_remainder > 0)          /* If there is a short final block, then pad it,*/
1887     {                                   /* encrypt it and copy the unpadded part back   */
1888         construct_ctr_preload(
1889                                 ctr_preload,
1890                                 a4_exists,
1891                                 qc_exists,
1892                                 pframe,
1893                                 pn_vector,
1894                                 num_blocks+1,
1895                                 frtype // add for CONFIG_IEEE80211W, none 11w also can use
1896                             );
1897
1898         for (j = 0; j < 16; j++) padded_buffer[j] = 0x00;
1899         for (j = 0; j < payload_remainder; j++)
1900         {
1901             padded_buffer[j] = pframe[payload_index+j];
1902         }
1903         aes128k128d(key, ctr_preload, aes_out);
1904         bitwise_xor(aes_out, padded_buffer, chain_buffer);
1905         for (j=0; j<payload_remainder;j++) pframe[payload_index++] = chain_buffer[j];
1906     }
1907
1908         //start to calculate the mic
1909         if((hdrlen +plen+8) <= MAX_MSG_SIZE)
1910                 _rtw_memcpy((void *)message, pframe, (hdrlen +plen+8)); //8 is for ext iv len
1911
1912
1913         pn_vector[0]=pframe[hdrlen];
1914         pn_vector[1]=pframe[hdrlen+1];
1915         pn_vector[2]=pframe[hdrlen+4];
1916         pn_vector[3]=pframe[hdrlen+5];
1917         pn_vector[4]=pframe[hdrlen+6];
1918         pn_vector[5]=pframe[hdrlen+7];
1919
1920
1921
1922         construct_mic_iv(
1923                         mic_iv,
1924                         qc_exists,
1925                         a4_exists,
1926                         message,
1927                         plen-8,
1928                         pn_vector,
1929                         frtype // add for CONFIG_IEEE80211W, none 11w also can use
1930                         );
1931
1932     construct_mic_header1(
1933                             mic_header1,
1934                             hdrlen,
1935                             message,
1936                             frtype // add for CONFIG_IEEE80211W, none 11w also can use
1937                             );
1938     construct_mic_header2(
1939                             mic_header2,
1940                             message,
1941                             a4_exists,
1942                             qc_exists
1943                             );
1944
1945
1946         payload_remainder = (plen-8) % 16;
1947     num_blocks = (plen-8) / 16;
1948
1949     /* Find start of payload */
1950     payload_index = (hdrlen + 8);
1951
1952     /* Calculate MIC */
1953     aes128k128d(key, mic_iv, aes_out);
1954     bitwise_xor(aes_out, mic_header1, chain_buffer);
1955     aes128k128d(key, chain_buffer, aes_out);
1956     bitwise_xor(aes_out, mic_header2, chain_buffer);
1957     aes128k128d(key, chain_buffer, aes_out);
1958
1959         for (i = 0; i < num_blocks; i++)
1960     {
1961         bitwise_xor(aes_out, &message[payload_index], chain_buffer);
1962
1963         payload_index += 16;
1964         aes128k128d(key, chain_buffer, aes_out);
1965     }
1966
1967     /* Add on the final payload block if it needs padding */
1968     if (payload_remainder > 0)
1969     {
1970         for (j = 0; j < 16; j++) padded_buffer[j] = 0x00;
1971         for (j = 0; j < payload_remainder; j++)
1972         {
1973             padded_buffer[j] = message[payload_index++];
1974         }
1975         bitwise_xor(aes_out, padded_buffer, chain_buffer);
1976         aes128k128d(key, chain_buffer, aes_out);
1977
1978     }
1979
1980     for (j = 0 ; j < 8; j++) mic[j] = aes_out[j];
1981
1982     /* Insert MIC into payload */
1983     for (j = 0; j < 8; j++)
1984         message[payload_index+j] = mic[j];
1985
1986         payload_index = hdrlen + 8;
1987         for (i=0; i< num_blocks; i++)
1988     {
1989         construct_ctr_preload(
1990                                 ctr_preload,
1991                                 a4_exists,
1992                                 qc_exists,
1993                                 message,
1994                                 pn_vector,
1995                                 i+1,
1996                                 frtype); // add for CONFIG_IEEE80211W, none 11w also can use
1997         aes128k128d(key, ctr_preload, aes_out);
1998         bitwise_xor(aes_out, &message[payload_index], chain_buffer);
1999         for (j=0; j<16;j++) message[payload_index++] = chain_buffer[j];
2000     }
2001
2002     if (payload_remainder > 0)          /* If there is a short final block, then pad it,*/
2003     {                                   /* encrypt it and copy the unpadded part back   */
2004         construct_ctr_preload(
2005                                 ctr_preload,
2006                                 a4_exists,
2007                                 qc_exists,
2008                                 message,
2009                                 pn_vector,
2010                                 num_blocks+1,
2011                                 frtype); // add for CONFIG_IEEE80211W, none 11w also can use
2012
2013         for (j = 0; j < 16; j++) padded_buffer[j] = 0x00;
2014         for (j = 0; j < payload_remainder; j++)
2015         {
2016             padded_buffer[j] = message[payload_index+j];
2017         }
2018         aes128k128d(key, ctr_preload, aes_out);
2019         bitwise_xor(aes_out, padded_buffer, chain_buffer);
2020         for (j=0; j<payload_remainder;j++) message[payload_index++] = chain_buffer[j];
2021     }
2022
2023     /* Encrypt the MIC */
2024     construct_ctr_preload(
2025                         ctr_preload,
2026                         a4_exists,
2027                         qc_exists,
2028                         message,
2029                         pn_vector,
2030                         0,
2031                         frtype); // add for CONFIG_IEEE80211W, none 11w also can use
2032
2033     for (j = 0; j < 16; j++) padded_buffer[j] = 0x00;
2034     for (j = 0; j < 8; j++)
2035     {
2036         padded_buffer[j] = message[j+hdrlen+8+plen-8];
2037     }
2038
2039     aes128k128d(key, ctr_preload, aes_out);
2040     bitwise_xor(aes_out, padded_buffer, chain_buffer);
2041     for (j=0; j<8;j++) message[payload_index++] = chain_buffer[j];
2042
2043         //compare the mic
2044         for(i=0;i<8;i++){
2045                 if(pframe[hdrlen+8+plen-8+i] != message[hdrlen+8+plen-8+i])
2046                 {
2047                         RT_TRACE(_module_rtl871x_security_c_,_drv_err_,("aes_decipher:mic check error mic[%d]: pframe(%x) != message(%x) \n",
2048                                                 i,pframe[hdrlen+8+plen-8+i],message[hdrlen+8+plen-8+i]));
2049                         DBG_871X("aes_decipher:mic check error mic[%d]: pframe(%x) != message(%x) \n",
2050                                                 i,pframe[hdrlen+8+plen-8+i],message[hdrlen+8+plen-8+i]);
2051                         res = _FAIL;
2052                 }
2053         }
2054 _func_exit_;
2055         return res;
2056 }
2057
2058 u32     rtw_aes_decrypt(_adapter *padapter, u8 *precvframe)
2059 {       // exclude ICV
2060
2061
2062         /*static*/
2063 //      unsigned char   message[MAX_MSG_SIZE];
2064
2065
2066         /* Intermediate Buffers */
2067
2068
2069         sint            length;
2070         u32     prwskeylen;
2071         u8      *pframe,*prwskey;       //, *payload,*iv
2072         struct  sta_info                *stainfo;
2073         struct  rx_pkt_attrib    *prxattrib = &((union recv_frame *)precvframe)->u.hdr.attrib;
2074         struct  security_priv   *psecuritypriv=&padapter->securitypriv;
2075 //      struct  recv_priv               *precvpriv=&padapter->recvpriv;
2076         u32     res=_SUCCESS;
2077 _func_enter_;
2078         pframe=(unsigned char *)((union recv_frame*)precvframe)->u.hdr.rx_data;
2079         //4 start to encrypt each fragment
2080         if((prxattrib->encrypt==_AES_)){
2081
2082                 stainfo=rtw_get_stainfo(&padapter->stapriv ,&prxattrib->ta[0] );
2083                 if (stainfo!=NULL){
2084                         RT_TRACE(_module_rtl871x_security_c_,_drv_err_,("rtw_aes_decrypt: stainfo!=NULL!!!\n"));
2085
2086                         if(IS_MCAST(prxattrib->ra))
2087                         {
2088                                 static u32 start = 0;
2089                                 static u32 no_gkey_bc_cnt = 0;
2090                                 static u32 no_gkey_mc_cnt = 0;
2091
2092                                 //DBG_871X("rx bc/mc packets, to perform sw rtw_aes_decrypt\n");
2093                                 //prwskey = psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey;
2094                                 if(psecuritypriv->binstallGrpkey==_FALSE)
2095                                 {
2096                                         res=_FAIL;
2097
2098                                         if (start == 0)
2099                                                 start = rtw_get_current_time();
2100
2101                                         if (is_broadcast_mac_addr(prxattrib->ra))
2102                                                 no_gkey_bc_cnt++;
2103                                         else
2104                                                 no_gkey_mc_cnt++;
2105
2106                                         if (rtw_get_passing_time_ms(start) > 1000) {
2107                                                 if (no_gkey_bc_cnt || no_gkey_mc_cnt) {
2108                                                         DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" no_gkey_bc_cnt:%u, no_gkey_mc_cnt:%u\n",
2109                                                                 FUNC_ADPT_ARG(padapter), no_gkey_bc_cnt, no_gkey_mc_cnt);
2110                                                 }
2111                                                 start = rtw_get_current_time();
2112                                                 no_gkey_bc_cnt = 0;
2113                                                 no_gkey_mc_cnt = 0;
2114                                         }
2115
2116                                         goto exit;
2117                                 }
2118
2119                                 if (no_gkey_bc_cnt || no_gkey_mc_cnt) {
2120                                         DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" gkey installed. no_gkey_bc_cnt:%u, no_gkey_mc_cnt:%u\n",
2121                                                 FUNC_ADPT_ARG(padapter), no_gkey_bc_cnt, no_gkey_mc_cnt);
2122                                 }
2123                                 start = 0;
2124                                 no_gkey_bc_cnt = 0;
2125                                 no_gkey_mc_cnt = 0;
2126
2127                                 prwskey = psecuritypriv->dot118021XGrpKey[prxattrib->key_index].skey;
2128                                 if(psecuritypriv->dot118021XGrpKeyid != prxattrib->key_index)
2129                                 {
2130                                         DBG_871X("not match packet_index=%d, install_index=%d \n"
2131                                         , prxattrib->key_index, psecuritypriv->dot118021XGrpKeyid);
2132                                         res=_FAIL;
2133                                         goto exit;
2134                                 }
2135                         }
2136                         else
2137                         {
2138                                 prwskey=&stainfo->dot118021x_UncstKey.skey[0];
2139                         }
2140
2141                         length= ((union recv_frame *)precvframe)->u.hdr.len-prxattrib->hdrlen-prxattrib->iv_len;
2142                         /*// add for CONFIG_IEEE80211W, debug
2143                         if(0)
2144                         printk("@@@@@@@@@@@@@@@@@@ length=%d, prxattrib->hdrlen=%d, prxattrib->pkt_len=%d \n"
2145                         , length, prxattrib->hdrlen, prxattrib->pkt_len);
2146                         if(0)
2147                         {
2148                                 int no;
2149                                 //test print PSK
2150                                 printk("PSK key below:\n");
2151                                 for(no=0;no<16;no++)
2152                                         printk(" %02x ", prwskey[no]);
2153                                 printk("\n");
2154                         }
2155                         if(0)
2156                         {
2157                                 int no;
2158                                 //test print PSK
2159                                 printk("frame:\n");
2160                                 for(no=0;no<prxattrib->pkt_len;no++)
2161                                         printk(" %02x ", pframe[no]);
2162                                 printk("\n");
2163                         }*/
2164
2165                         res= aes_decipher(prwskey,prxattrib->hdrlen,pframe, length);
2166
2167                         AES_SW_DEC_CNT_INC(psecuritypriv, prxattrib->ra);
2168                 }
2169                 else{
2170                         RT_TRACE(_module_rtl871x_security_c_,_drv_err_,("rtw_aes_decrypt: stainfo==NULL!!!\n"));
2171                         res=_FAIL;
2172                 }
2173
2174         }
2175 _func_exit_;
2176 exit:
2177         return res;
2178 }
2179
2180 #ifdef CONFIG_IEEE80211W
2181 u32     rtw_BIP_verify(_adapter *padapter, u8 *precvframe)
2182 {
2183         struct rx_pkt_attrib *pattrib = &((union recv_frame *)precvframe)->u.hdr.attrib;
2184         u8 *pframe;
2185         u8 *BIP_AAD, *p;
2186         u32     res=_FAIL;
2187         uint len, ori_len;
2188         struct rtw_ieee80211_hdr *pwlanhdr;
2189         u8 mic[16];
2190         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
2191         ori_len = pattrib->pkt_len-WLAN_HDR_A3_LEN+BIP_AAD_SIZE;
2192         BIP_AAD = rtw_zmalloc(ori_len);
2193
2194         if(BIP_AAD == NULL)
2195         {
2196                 DBG_871X("BIP AAD allocate fail\n");
2197                 return _FAIL;
2198         }
2199         //PKT start
2200         pframe=(unsigned char *)((union recv_frame*)precvframe)->u.hdr.rx_data;
2201         //mapping to wlan header
2202         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
2203         //save the frame body + MME
2204         _rtw_memcpy(BIP_AAD+BIP_AAD_SIZE, pframe+WLAN_HDR_A3_LEN, pattrib->pkt_len-WLAN_HDR_A3_LEN);
2205         //find MME IE pointer
2206         p = rtw_get_ie(BIP_AAD+BIP_AAD_SIZE, _MME_IE_, &len, pattrib->pkt_len-WLAN_HDR_A3_LEN);
2207         //Baron
2208         if(p)
2209         {
2210                 u16 keyid=0;
2211                 u64 temp_ipn=0;
2212                 //save packet number
2213                 _rtw_memcpy(&temp_ipn, p+4, 6);
2214                 temp_ipn = le64_to_cpu(temp_ipn);
2215                 //BIP packet number should bigger than previous BIP packet
2216                 if(temp_ipn <= pmlmeext->mgnt_80211w_IPN_rx)
2217                 {
2218                         DBG_871X("replay BIP packet\n");
2219                         goto BIP_exit;
2220                 }
2221                 //copy key index
2222                 _rtw_memcpy(&keyid, p+2, 2);
2223                 keyid = le16_to_cpu(keyid);
2224                 if(keyid != padapter->securitypriv.dot11wBIPKeyid)
2225                 {
2226                         DBG_871X("BIP key index error!\n");
2227                         goto BIP_exit;
2228                 }
2229                 //clear the MIC field of MME to zero
2230                 _rtw_memset(p+2+len-8, 0, 8);
2231
2232                 //conscruct AAD, copy frame control field
2233                 _rtw_memcpy(BIP_AAD, &pwlanhdr->frame_ctl, 2);
2234                 ClearRetry(BIP_AAD);
2235                 ClearPwrMgt(BIP_AAD);
2236                 ClearMData(BIP_AAD);
2237                 //conscruct AAD, copy address 1 to address 3
2238                 _rtw_memcpy(BIP_AAD+2, pwlanhdr->addr1, 18);
2239
2240                 if(omac1_aes_128(padapter->securitypriv.dot11wBIPKey[padapter->securitypriv.dot11wBIPKeyid].skey
2241                         , BIP_AAD, ori_len, mic))
2242                         goto BIP_exit;
2243
2244                 /*//management packet content
2245                 {
2246                         int pp;
2247                         DBG_871X("pkt: ");
2248                         for(pp=0;pp< pattrib->pkt_len; pp++)
2249                                 printk(" %02x ", pframe[pp]);
2250                         DBG_871X("\n");
2251                         //BIP AAD + management frame body + MME(MIC is zero)
2252                         DBG_871X("AAD+PKT: ");
2253                         for(pp=0;pp< ori_len; pp++)
2254                                 DBG_871X(" %02x ", BIP_AAD[pp]);
2255                         DBG_871X("\n");
2256                         //show the MIC result
2257                         DBG_871X("mic: ");
2258                         for(pp=0;pp<16; pp++)
2259                                 DBG_871X(" %02x ", mic[pp]);
2260                         DBG_871X("\n");
2261                 }
2262                 */
2263                 //MIC field should be last 8 bytes of packet (packet without FCS)
2264                 if(_rtw_memcmp(mic, pframe+pattrib->pkt_len-8, 8))
2265                 {
2266                         pmlmeext->mgnt_80211w_IPN_rx = temp_ipn;
2267                         res=_SUCCESS;
2268                 }
2269                 else
2270                         DBG_871X("BIP MIC error!\n");
2271
2272         }
2273         else
2274                 res = RTW_RX_HANDLED;
2275 BIP_exit:
2276
2277         rtw_mfree(BIP_AAD, ori_len);
2278         return res;
2279 }
2280 #endif //CONFIG_IEEE80211W
2281
2282 #ifndef PLATFORM_FREEBSD
2283 /* compress 512-bits */
2284 static int sha256_compress(struct sha256_state *md, unsigned char *buf)
2285 {
2286         u32 S[8], W[64], t0, t1;
2287         u32 t;
2288         int i;
2289
2290         /* copy state into S */
2291         for (i = 0; i < 8; i++) {
2292                 S[i] = md->state[i];
2293         }
2294
2295         /* copy the state into 512-bits into W[0..15] */
2296         for (i = 0; i < 16; i++)
2297                 W[i] = WPA_GET_BE32(buf + (4 * i));
2298
2299         /* fill W[16..63] */
2300         for (i = 16; i < 64; i++) {
2301                 W[i] = Gamma1(W[i - 2]) + W[i - 7] + Gamma0(W[i - 15]) +
2302                         W[i - 16];
2303         }
2304
2305         /* Compress */
2306 #define RND(a,b,c,d,e,f,g,h,i)                          \
2307         t0 = h + Sigma1(e) + Ch(e, f, g) + K[i] + W[i]; \
2308         t1 = Sigma0(a) + Maj(a, b, c);                  \
2309         d += t0;                                        \
2310         h  = t0 + t1;
2311
2312         for (i = 0; i < 64; ++i) {
2313                 RND(S[0], S[1], S[2], S[3], S[4], S[5], S[6], S[7], i);
2314                 t = S[7]; S[7] = S[6]; S[6] = S[5]; S[5] = S[4];
2315                 S[4] = S[3]; S[3] = S[2]; S[2] = S[1]; S[1] = S[0]; S[0] = t;
2316         }
2317
2318         /* feedback */
2319         for (i = 0; i < 8; i++) {
2320                 md->state[i] = md->state[i] + S[i];
2321         }
2322         return 0;
2323 }
2324
2325 /* Initialize the hash state */
2326 static void sha256_init(struct sha256_state *md)
2327 {
2328         md->curlen = 0;
2329         md->length = 0;
2330         md->state[0] = 0x6A09E667UL;
2331         md->state[1] = 0xBB67AE85UL;
2332         md->state[2] = 0x3C6EF372UL;
2333         md->state[3] = 0xA54FF53AUL;
2334         md->state[4] = 0x510E527FUL;
2335         md->state[5] = 0x9B05688CUL;
2336         md->state[6] = 0x1F83D9ABUL;
2337         md->state[7] = 0x5BE0CD19UL;
2338 }
2339
2340 /**
2341    Process a block of memory though the hash
2342    @param md     The hash state
2343    @param in     The data to hash
2344    @param inlen  The length of the data (octets)
2345    @return CRYPT_OK if successful
2346 */
2347 static int sha256_process(struct sha256_state *md, unsigned char *in,
2348                           unsigned long inlen)
2349 {
2350         unsigned long n;
2351 #define block_size 64
2352
2353         if (md->curlen > sizeof(md->buf))
2354                 return -1;
2355
2356         while (inlen > 0) {
2357                 if (md->curlen == 0 && inlen >= block_size) {
2358                         if (sha256_compress(md, (unsigned char *) in) < 0)
2359                                 return -1;
2360                         md->length += block_size * 8;
2361                         in += block_size;
2362                         inlen -= block_size;
2363                 } else {
2364                         n = MIN(inlen, (block_size - md->curlen));
2365                         _rtw_memcpy(md->buf + md->curlen, in, n);
2366                         md->curlen += n;
2367                         in += n;
2368                         inlen -= n;
2369                         if (md->curlen == block_size) {
2370                                 if (sha256_compress(md, md->buf) < 0)
2371                                         return -1;
2372                                 md->length += 8 * block_size;
2373                                 md->curlen = 0;
2374                         }
2375                 }
2376         }
2377
2378         return 0;
2379 }
2380
2381
2382 /**
2383    Terminate the hash to get the digest
2384    @param md  The hash state
2385    @param out [out] The destination of the hash (32 bytes)
2386    @return CRYPT_OK if successful
2387 */
2388 static int sha256_done(struct sha256_state *md, unsigned char *out)
2389 {
2390         int i;
2391
2392         if (md->curlen >= sizeof(md->buf))
2393                 return -1;
2394
2395         /* increase the length of the message */
2396         md->length += md->curlen * 8;
2397
2398         /* append the '1' bit */
2399         md->buf[md->curlen++] = (unsigned char) 0x80;
2400
2401         /* if the length is currently above 56 bytes we append zeros
2402          * then compress.  Then we can fall back to padding zeros and length
2403          * encoding like normal.
2404          */
2405         if (md->curlen > 56) {
2406                 while (md->curlen < 64) {
2407                         md->buf[md->curlen++] = (unsigned char) 0;
2408                 }
2409                 sha256_compress(md, md->buf);
2410                 md->curlen = 0;
2411         }
2412
2413         /* pad upto 56 bytes of zeroes */
2414         while (md->curlen < 56) {
2415                 md->buf[md->curlen++] = (unsigned char) 0;
2416         }
2417
2418         /* store length */
2419         WPA_PUT_BE64(md->buf + 56, md->length);
2420         sha256_compress(md, md->buf);
2421
2422         /* copy output */
2423         for (i = 0; i < 8; i++)
2424                 WPA_PUT_BE32(out + (4 * i), md->state[i]);
2425
2426         return 0;
2427 }
2428
2429 /**
2430  * sha256_vector - SHA256 hash for data vector
2431  * @num_elem: Number of elements in the data vector
2432  * @addr: Pointers to the data areas
2433  * @len: Lengths of the data blocks
2434  * @mac: Buffer for the hash
2435  * Returns: 0 on success, -1 of failure
2436  */
2437 static int sha256_vector(size_t num_elem, u8 *addr[], size_t *len,
2438                   u8 *mac)
2439 {
2440         struct sha256_state ctx;
2441         size_t i;
2442
2443         sha256_init(&ctx);
2444         for (i = 0; i < num_elem; i++)
2445                 if (sha256_process(&ctx, addr[i], len[i]))
2446                         return -1;
2447         if (sha256_done(&ctx, mac))
2448                 return -1;
2449         return 0;
2450 }
2451
2452 static u8 os_strlen(const char *s)
2453 {
2454         const char *p = s;
2455         while (*p)
2456                 p++;
2457         return p - s;
2458 }
2459
2460 static int os_memcmp(void *s1, void *s2, u8 n)
2461 {
2462         unsigned char *p1 = s1, *p2 = s2;
2463
2464         if (n == 0)
2465                 return 0;
2466
2467         while (*p1 == *p2) {
2468                 p1++;
2469                 p2++;
2470                 n--;
2471                 if (n == 0)
2472                         return 0;
2473         }
2474
2475         return *p1 - *p2;
2476 }
2477
2478 /**
2479  * hmac_sha256_vector - HMAC-SHA256 over data vector (RFC 2104)
2480  * @key: Key for HMAC operations
2481  * @key_len: Length of the key in bytes
2482  * @num_elem: Number of elements in the data vector
2483  * @addr: Pointers to the data areas
2484  * @len: Lengths of the data blocks
2485  * @mac: Buffer for the hash (32 bytes)
2486  */
2487 static void hmac_sha256_vector(u8 *key, size_t key_len, size_t num_elem,
2488                         u8 *addr[], size_t *len, u8 *mac)
2489 {
2490         unsigned char k_pad[64]; /* padding - key XORd with ipad/opad */
2491         unsigned char tk[32];
2492         u8 *_addr[6];
2493         size_t _len[6], i;
2494
2495         if (num_elem > 5) {
2496                 /*
2497                  * Fixed limit on the number of fragments to avoid having to
2498                  * allocate memory (which could fail).
2499                  */
2500                 return;
2501         }
2502
2503         /* if key is longer than 64 bytes reset it to key = SHA256(key) */
2504         if (key_len > 64) {
2505                 sha256_vector(1, &key, &key_len, tk);
2506                 key = tk;
2507                 key_len = 32;
2508         }
2509
2510         /* the HMAC_SHA256 transform looks like:
2511          *
2512          * SHA256(K XOR opad, SHA256(K XOR ipad, text))
2513          *
2514          * where K is an n byte key
2515          * ipad is the byte 0x36 repeated 64 times
2516          * opad is the byte 0x5c repeated 64 times
2517          * and text is the data being protected */
2518
2519         /* start out by storing key in ipad */
2520         _rtw_memset(k_pad, 0, sizeof(k_pad));
2521         _rtw_memcpy(k_pad, key, key_len);
2522         /* XOR key with ipad values */
2523         for (i = 0; i < 64; i++)
2524                 k_pad[i] ^= 0x36;
2525
2526         /* perform inner SHA256 */
2527         _addr[0] = k_pad;
2528         _len[0] = 64;
2529         for (i = 0; i < num_elem; i++) {
2530                 _addr[i + 1] = addr[i];
2531                 _len[i + 1] = len[i];
2532         }
2533         sha256_vector(1 + num_elem, _addr, _len, mac);
2534
2535         _rtw_memset(k_pad, 0, sizeof(k_pad));
2536         _rtw_memcpy(k_pad, key, key_len);
2537         /* XOR key with opad values */
2538         for (i = 0; i < 64; i++)
2539                 k_pad[i] ^= 0x5c;
2540
2541         /* perform outer SHA256 */
2542         _addr[0] = k_pad;
2543         _len[0] = 64;
2544         _addr[1] = mac;
2545         _len[1] = 32;
2546         sha256_vector(2, _addr, _len, mac);
2547 }
2548 #endif //PLATFORM_FREEBSD
2549 /**
2550  * sha256_prf - SHA256-based Pseudo-Random Function (IEEE 802.11r, 8.5.1.5.2)
2551  * @key: Key for PRF
2552  * @key_len: Length of the key in bytes
2553  * @label: A unique label for each purpose of the PRF
2554  * @data: Extra data to bind into the key
2555  * @data_len: Length of the data
2556  * @buf: Buffer for the generated pseudo-random key
2557  * @buf_len: Number of bytes of key to generate
2558  *
2559  * This function is used to derive new, cryptographically separate keys from a
2560  * given key.
2561  */
2562 #ifndef PLATFORM_FREEBSD //Baron
2563 static void sha256_prf(u8 *key, size_t key_len, char *label,
2564                 u8 *data, size_t data_len, u8 *buf, size_t buf_len)
2565 {
2566         u16 counter = 1;
2567         size_t pos, plen;
2568         u8 hash[SHA256_MAC_LEN];
2569         u8 *addr[4];
2570         size_t len[4];
2571         u8 counter_le[2], length_le[2];
2572
2573         addr[0] = counter_le;
2574         len[0] = 2;
2575         addr[1] = (u8 *) label;
2576         len[1] = os_strlen(label);
2577         addr[2] = data;
2578         len[2] = data_len;
2579         addr[3] = length_le;
2580         len[3] = sizeof(length_le);
2581
2582         WPA_PUT_LE16(length_le, buf_len * 8);
2583         pos = 0;
2584         while (pos < buf_len) {
2585                 plen = buf_len - pos;
2586                 WPA_PUT_LE16(counter_le, counter);
2587                 if (plen >= SHA256_MAC_LEN) {
2588                         hmac_sha256_vector(key, key_len, 4, addr, len,
2589                                            &buf[pos]);
2590                         pos += SHA256_MAC_LEN;
2591                 } else {
2592                         hmac_sha256_vector(key, key_len, 4, addr, len, hash);
2593                         _rtw_memcpy(&buf[pos], hash, plen);
2594                         break;
2595                 }
2596                 counter++;
2597         }
2598 }
2599 #endif //PLATFORM_FREEBSD Baron
2600
2601 /* AES tables*/
2602 const u32 Te0[256] = {
2603     0xc66363a5U, 0xf87c7c84U, 0xee777799U, 0xf67b7b8dU,
2604     0xfff2f20dU, 0xd66b6bbdU, 0xde6f6fb1U, 0x91c5c554U,
2605     0x60303050U, 0x02010103U, 0xce6767a9U, 0x562b2b7dU,
2606     0xe7fefe19U, 0xb5d7d762U, 0x4dababe6U, 0xec76769aU,
2607     0x8fcaca45U, 0x1f82829dU, 0x89c9c940U, 0xfa7d7d87U,
2608     0xeffafa15U, 0xb25959ebU, 0x8e4747c9U, 0xfbf0f00bU,
2609     0x41adadecU, 0xb3d4d467U, 0x5fa2a2fdU, 0x45afafeaU,
2610     0x239c9cbfU, 0x53a4a4f7U, 0xe4727296U, 0x9bc0c05bU,
2611     0x75b7b7c2U, 0xe1fdfd1cU, 0x3d9393aeU, 0x4c26266aU,
2612     0x6c36365aU, 0x7e3f3f41U, 0xf5f7f702U, 0x83cccc4fU,
2613     0x6834345cU, 0x51a5a5f4U, 0xd1e5e534U, 0xf9f1f108U,
2614     0xe2717193U, 0xabd8d873U, 0x62313153U, 0x2a15153fU,
2615     0x0804040cU, 0x95c7c752U, 0x46232365U, 0x9dc3c35eU,
2616     0x30181828U, 0x379696a1U, 0x0a05050fU, 0x2f9a9ab5U,
2617     0x0e070709U, 0x24121236U, 0x1b80809bU, 0xdfe2e23dU,
2618     0xcdebeb26U, 0x4e272769U, 0x7fb2b2cdU, 0xea75759fU,
2619     0x1209091bU, 0x1d83839eU, 0x582c2c74U, 0x341a1a2eU,
2620     0x361b1b2dU, 0xdc6e6eb2U, 0xb45a5aeeU, 0x5ba0a0fbU,
2621     0xa45252f6U, 0x763b3b4dU, 0xb7d6d661U, 0x7db3b3ceU,
2622     0x5229297bU, 0xdde3e33eU, 0x5e2f2f71U, 0x13848497U,
2623     0xa65353f5U, 0xb9d1d168U, 0x00000000U, 0xc1eded2cU,
2624     0x40202060U, 0xe3fcfc1fU, 0x79b1b1c8U, 0xb65b5bedU,
2625     0xd46a6abeU, 0x8dcbcb46U, 0x67bebed9U, 0x7239394bU,
2626     0x944a4adeU, 0x984c4cd4U, 0xb05858e8U, 0x85cfcf4aU,
2627     0xbbd0d06bU, 0xc5efef2aU, 0x4faaaae5U, 0xedfbfb16U,
2628     0x864343c5U, 0x9a4d4dd7U, 0x66333355U, 0x11858594U,
2629     0x8a4545cfU, 0xe9f9f910U, 0x04020206U, 0xfe7f7f81U,
2630     0xa05050f0U, 0x783c3c44U, 0x259f9fbaU, 0x4ba8a8e3U,
2631     0xa25151f3U, 0x5da3a3feU, 0x804040c0U, 0x058f8f8aU,
2632     0x3f9292adU, 0x219d9dbcU, 0x70383848U, 0xf1f5f504U,
2633     0x63bcbcdfU, 0x77b6b6c1U, 0xafdada75U, 0x42212163U,
2634     0x20101030U, 0xe5ffff1aU, 0xfdf3f30eU, 0xbfd2d26dU,
2635     0x81cdcd4cU, 0x180c0c14U, 0x26131335U, 0xc3ecec2fU,
2636     0xbe5f5fe1U, 0x359797a2U, 0x884444ccU, 0x2e171739U,
2637     0x93c4c457U, 0x55a7a7f2U, 0xfc7e7e82U, 0x7a3d3d47U,
2638     0xc86464acU, 0xba5d5de7U, 0x3219192bU, 0xe6737395U,
2639     0xc06060a0U, 0x19818198U, 0x9e4f4fd1U, 0xa3dcdc7fU,
2640     0x44222266U, 0x542a2a7eU, 0x3b9090abU, 0x0b888883U,
2641     0x8c4646caU, 0xc7eeee29U, 0x6bb8b8d3U, 0x2814143cU,
2642     0xa7dede79U, 0xbc5e5ee2U, 0x160b0b1dU, 0xaddbdb76U,
2643     0xdbe0e03bU, 0x64323256U, 0x743a3a4eU, 0x140a0a1eU,
2644     0x924949dbU, 0x0c06060aU, 0x4824246cU, 0xb85c5ce4U,
2645     0x9fc2c25dU, 0xbdd3d36eU, 0x43acacefU, 0xc46262a6U,
2646     0x399191a8U, 0x319595a4U, 0xd3e4e437U, 0xf279798bU,
2647     0xd5e7e732U, 0x8bc8c843U, 0x6e373759U, 0xda6d6db7U,
2648     0x018d8d8cU, 0xb1d5d564U, 0x9c4e4ed2U, 0x49a9a9e0U,
2649     0xd86c6cb4U, 0xac5656faU, 0xf3f4f407U, 0xcfeaea25U,
2650     0xca6565afU, 0xf47a7a8eU, 0x47aeaee9U, 0x10080818U,
2651     0x6fbabad5U, 0xf0787888U, 0x4a25256fU, 0x5c2e2e72U,
2652     0x381c1c24U, 0x57a6a6f1U, 0x73b4b4c7U, 0x97c6c651U,
2653     0xcbe8e823U, 0xa1dddd7cU, 0xe874749cU, 0x3e1f1f21U,
2654     0x964b4bddU, 0x61bdbddcU, 0x0d8b8b86U, 0x0f8a8a85U,
2655     0xe0707090U, 0x7c3e3e42U, 0x71b5b5c4U, 0xcc6666aaU,
2656     0x904848d8U, 0x06030305U, 0xf7f6f601U, 0x1c0e0e12U,
2657     0xc26161a3U, 0x6a35355fU, 0xae5757f9U, 0x69b9b9d0U,
2658     0x17868691U, 0x99c1c158U, 0x3a1d1d27U, 0x279e9eb9U,
2659     0xd9e1e138U, 0xebf8f813U, 0x2b9898b3U, 0x22111133U,
2660     0xd26969bbU, 0xa9d9d970U, 0x078e8e89U, 0x339494a7U,
2661     0x2d9b9bb6U, 0x3c1e1e22U, 0x15878792U, 0xc9e9e920U,
2662     0x87cece49U, 0xaa5555ffU, 0x50282878U, 0xa5dfdf7aU,
2663     0x038c8c8fU, 0x59a1a1f8U, 0x09898980U, 0x1a0d0d17U,
2664     0x65bfbfdaU, 0xd7e6e631U, 0x844242c6U, 0xd06868b8U,
2665     0x824141c3U, 0x299999b0U, 0x5a2d2d77U, 0x1e0f0f11U,
2666     0x7bb0b0cbU, 0xa85454fcU, 0x6dbbbbd6U, 0x2c16163aU,
2667 };
2668 const u32 Td0[256] = {
2669     0x51f4a750U, 0x7e416553U, 0x1a17a4c3U, 0x3a275e96U,
2670     0x3bab6bcbU, 0x1f9d45f1U, 0xacfa58abU, 0x4be30393U,
2671     0x2030fa55U, 0xad766df6U, 0x88cc7691U, 0xf5024c25U,
2672     0x4fe5d7fcU, 0xc52acbd7U, 0x26354480U, 0xb562a38fU,
2673     0xdeb15a49U, 0x25ba1b67U, 0x45ea0e98U, 0x5dfec0e1U,
2674     0xc32f7502U, 0x814cf012U, 0x8d4697a3U, 0x6bd3f9c6U,
2675     0x038f5fe7U, 0x15929c95U, 0xbf6d7aebU, 0x955259daU,
2676     0xd4be832dU, 0x587421d3U, 0x49e06929U, 0x8ec9c844U,
2677     0x75c2896aU, 0xf48e7978U, 0x99583e6bU, 0x27b971ddU,
2678     0xbee14fb6U, 0xf088ad17U, 0xc920ac66U, 0x7dce3ab4U,
2679     0x63df4a18U, 0xe51a3182U, 0x97513360U, 0x62537f45U,
2680     0xb16477e0U, 0xbb6bae84U, 0xfe81a01cU, 0xf9082b94U,
2681     0x70486858U, 0x8f45fd19U, 0x94de6c87U, 0x527bf8b7U,
2682     0xab73d323U, 0x724b02e2U, 0xe31f8f57U, 0x6655ab2aU,
2683     0xb2eb2807U, 0x2fb5c203U, 0x86c57b9aU, 0xd33708a5U,
2684     0x302887f2U, 0x23bfa5b2U, 0x02036abaU, 0xed16825cU,
2685     0x8acf1c2bU, 0xa779b492U, 0xf307f2f0U, 0x4e69e2a1U,
2686     0x65daf4cdU, 0x0605bed5U, 0xd134621fU, 0xc4a6fe8aU,
2687     0x342e539dU, 0xa2f355a0U, 0x058ae132U, 0xa4f6eb75U,
2688     0x0b83ec39U, 0x4060efaaU, 0x5e719f06U, 0xbd6e1051U,
2689     0x3e218af9U, 0x96dd063dU, 0xdd3e05aeU, 0x4de6bd46U,
2690     0x91548db5U, 0x71c45d05U, 0x0406d46fU, 0x605015ffU,
2691     0x1998fb24U, 0xd6bde997U, 0x894043ccU, 0x67d99e77U,
2692     0xb0e842bdU, 0x07898b88U, 0xe7195b38U, 0x79c8eedbU,
2693     0xa17c0a47U, 0x7c420fe9U, 0xf8841ec9U, 0x00000000U,
2694     0x09808683U, 0x322bed48U, 0x1e1170acU, 0x6c5a724eU,
2695     0xfd0efffbU, 0x0f853856U, 0x3daed51eU, 0x362d3927U,
2696     0x0a0fd964U, 0x685ca621U, 0x9b5b54d1U, 0x24362e3aU,
2697     0x0c0a67b1U, 0x9357e70fU, 0xb4ee96d2U, 0x1b9b919eU,
2698     0x80c0c54fU, 0x61dc20a2U, 0x5a774b69U, 0x1c121a16U,
2699     0xe293ba0aU, 0xc0a02ae5U, 0x3c22e043U, 0x121b171dU,
2700     0x0e090d0bU, 0xf28bc7adU, 0x2db6a8b9U, 0x141ea9c8U,
2701     0x57f11985U, 0xaf75074cU, 0xee99ddbbU, 0xa37f60fdU,
2702     0xf701269fU, 0x5c72f5bcU, 0x44663bc5U, 0x5bfb7e34U,
2703     0x8b432976U, 0xcb23c6dcU, 0xb6edfc68U, 0xb8e4f163U,
2704     0xd731dccaU, 0x42638510U, 0x13972240U, 0x84c61120U,
2705     0x854a247dU, 0xd2bb3df8U, 0xaef93211U, 0xc729a16dU,
2706     0x1d9e2f4bU, 0xdcb230f3U, 0x0d8652ecU, 0x77c1e3d0U,
2707     0x2bb3166cU, 0xa970b999U, 0x119448faU, 0x47e96422U,
2708     0xa8fc8cc4U, 0xa0f03f1aU, 0x567d2cd8U, 0x223390efU,
2709     0x87494ec7U, 0xd938d1c1U, 0x8ccaa2feU, 0x98d40b36U,
2710     0xa6f581cfU, 0xa57ade28U, 0xdab78e26U, 0x3fadbfa4U,
2711     0x2c3a9de4U, 0x5078920dU, 0x6a5fcc9bU, 0x547e4662U,
2712     0xf68d13c2U, 0x90d8b8e8U, 0x2e39f75eU, 0x82c3aff5U,
2713     0x9f5d80beU, 0x69d0937cU, 0x6fd52da9U, 0xcf2512b3U,
2714     0xc8ac993bU, 0x10187da7U, 0xe89c636eU, 0xdb3bbb7bU,
2715     0xcd267809U, 0x6e5918f4U, 0xec9ab701U, 0x834f9aa8U,
2716     0xe6956e65U, 0xaaffe67eU, 0x21bccf08U, 0xef15e8e6U,
2717     0xbae79bd9U, 0x4a6f36ceU, 0xea9f09d4U, 0x29b07cd6U,
2718     0x31a4b2afU, 0x2a3f2331U, 0xc6a59430U, 0x35a266c0U,
2719     0x744ebc37U, 0xfc82caa6U, 0xe090d0b0U, 0x33a7d815U,
2720     0xf104984aU, 0x41ecdaf7U, 0x7fcd500eU, 0x1791f62fU,
2721     0x764dd68dU, 0x43efb04dU, 0xccaa4d54U, 0xe49604dfU,
2722     0x9ed1b5e3U, 0x4c6a881bU, 0xc12c1fb8U, 0x4665517fU,
2723     0x9d5eea04U, 0x018c355dU, 0xfa877473U, 0xfb0b412eU,
2724     0xb3671d5aU, 0x92dbd252U, 0xe9105633U, 0x6dd64713U,
2725     0x9ad7618cU, 0x37a10c7aU, 0x59f8148eU, 0xeb133c89U,
2726     0xcea927eeU, 0xb761c935U, 0xe11ce5edU, 0x7a47b13cU,
2727     0x9cd2df59U, 0x55f2733fU, 0x1814ce79U, 0x73c737bfU,
2728     0x53f7cdeaU, 0x5ffdaa5bU, 0xdf3d6f14U, 0x7844db86U,
2729     0xcaaff381U, 0xb968c43eU, 0x3824342cU, 0xc2a3405fU,
2730     0x161dc372U, 0xbce2250cU, 0x283c498bU, 0xff0d9541U,
2731     0x39a80171U, 0x080cb3deU, 0xd8b4e49cU, 0x6456c190U,
2732     0x7bcb8461U, 0xd532b670U, 0x486c5c74U, 0xd0b85742U,
2733 };
2734 const u8 Td4s[256] = {
2735     0x52U, 0x09U, 0x6aU, 0xd5U, 0x30U, 0x36U, 0xa5U, 0x38U,
2736     0xbfU, 0x40U, 0xa3U, 0x9eU, 0x81U, 0xf3U, 0xd7U, 0xfbU,
2737     0x7cU, 0xe3U, 0x39U, 0x82U, 0x9bU, 0x2fU, 0xffU, 0x87U,
2738     0x34U, 0x8eU, 0x43U, 0x44U, 0xc4U, 0xdeU, 0xe9U, 0xcbU,
2739     0x54U, 0x7bU, 0x94U, 0x32U, 0xa6U, 0xc2U, 0x23U, 0x3dU,
2740     0xeeU, 0x4cU, 0x95U, 0x0bU, 0x42U, 0xfaU, 0xc3U, 0x4eU,
2741     0x08U, 0x2eU, 0xa1U, 0x66U, 0x28U, 0xd9U, 0x24U, 0xb2U,
2742     0x76U, 0x5bU, 0xa2U, 0x49U, 0x6dU, 0x8bU, 0xd1U, 0x25U,
2743     0x72U, 0xf8U, 0xf6U, 0x64U, 0x86U, 0x68U, 0x98U, 0x16U,
2744     0xd4U, 0xa4U, 0x5cU, 0xccU, 0x5dU, 0x65U, 0xb6U, 0x92U,
2745     0x6cU, 0x70U, 0x48U, 0x50U, 0xfdU, 0xedU, 0xb9U, 0xdaU,
2746     0x5eU, 0x15U, 0x46U, 0x57U, 0xa7U, 0x8dU, 0x9dU, 0x84U,
2747     0x90U, 0xd8U, 0xabU, 0x00U, 0x8cU, 0xbcU, 0xd3U, 0x0aU,
2748     0xf7U, 0xe4U, 0x58U, 0x05U, 0xb8U, 0xb3U, 0x45U, 0x06U,
2749     0xd0U, 0x2cU, 0x1eU, 0x8fU, 0xcaU, 0x3fU, 0x0fU, 0x02U,
2750     0xc1U, 0xafU, 0xbdU, 0x03U, 0x01U, 0x13U, 0x8aU, 0x6bU,
2751     0x3aU, 0x91U, 0x11U, 0x41U, 0x4fU, 0x67U, 0xdcU, 0xeaU,
2752     0x97U, 0xf2U, 0xcfU, 0xceU, 0xf0U, 0xb4U, 0xe6U, 0x73U,
2753     0x96U, 0xacU, 0x74U, 0x22U, 0xe7U, 0xadU, 0x35U, 0x85U,
2754     0xe2U, 0xf9U, 0x37U, 0xe8U, 0x1cU, 0x75U, 0xdfU, 0x6eU,
2755     0x47U, 0xf1U, 0x1aU, 0x71U, 0x1dU, 0x29U, 0xc5U, 0x89U,
2756     0x6fU, 0xb7U, 0x62U, 0x0eU, 0xaaU, 0x18U, 0xbeU, 0x1bU,
2757     0xfcU, 0x56U, 0x3eU, 0x4bU, 0xc6U, 0xd2U, 0x79U, 0x20U,
2758     0x9aU, 0xdbU, 0xc0U, 0xfeU, 0x78U, 0xcdU, 0x5aU, 0xf4U,
2759     0x1fU, 0xddU, 0xa8U, 0x33U, 0x88U, 0x07U, 0xc7U, 0x31U,
2760     0xb1U, 0x12U, 0x10U, 0x59U, 0x27U, 0x80U, 0xecU, 0x5fU,
2761     0x60U, 0x51U, 0x7fU, 0xa9U, 0x19U, 0xb5U, 0x4aU, 0x0dU,
2762     0x2dU, 0xe5U, 0x7aU, 0x9fU, 0x93U, 0xc9U, 0x9cU, 0xefU,
2763     0xa0U, 0xe0U, 0x3bU, 0x4dU, 0xaeU, 0x2aU, 0xf5U, 0xb0U,
2764     0xc8U, 0xebU, 0xbbU, 0x3cU, 0x83U, 0x53U, 0x99U, 0x61U,
2765     0x17U, 0x2bU, 0x04U, 0x7eU, 0xbaU, 0x77U, 0xd6U, 0x26U,
2766     0xe1U, 0x69U, 0x14U, 0x63U, 0x55U, 0x21U, 0x0cU, 0x7dU,
2767 };
2768 const u8 rcons[] = {
2769         0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80, 0x1B, 0x36
2770         /* for 128-bit blocks, Rijndael never uses more than 10 rcon values */
2771 };
2772
2773 /**
2774  * Expand the cipher key into the encryption key schedule.
2775  *
2776  * @return      the number of rounds for the given cipher key size.
2777  */
2778 #ifndef PLATFORM_FREEBSD //Baron
2779 static void rijndaelKeySetupEnc(u32 rk[/*44*/], const u8 cipherKey[])
2780 {
2781         int i;
2782         u32 temp;
2783
2784         rk[0] = GETU32(cipherKey     );
2785         rk[1] = GETU32(cipherKey +  4);
2786         rk[2] = GETU32(cipherKey +  8);
2787         rk[3] = GETU32(cipherKey + 12);
2788         for (i = 0; i < 10; i++) {
2789                 temp  = rk[3];
2790                 rk[4] = rk[0] ^
2791                         TE421(temp) ^ TE432(temp) ^ TE443(temp) ^ TE414(temp) ^
2792                         RCON(i);
2793                 rk[5] = rk[1] ^ rk[4];
2794                 rk[6] = rk[2] ^ rk[5];
2795                 rk[7] = rk[3] ^ rk[6];
2796                 rk += 4;
2797         }
2798 }
2799
2800 static void rijndaelEncrypt(u32 rk[/*44*/], u8 pt[16], u8 ct[16])
2801 {
2802         u32 s0, s1, s2, s3, t0, t1, t2, t3;
2803         int Nr = 10;
2804 #ifndef FULL_UNROLL
2805         int r;
2806 #endif /* ?FULL_UNROLL */
2807
2808         /*
2809          * map byte array block to cipher state
2810          * and add initial round key:
2811          */
2812         s0 = GETU32(pt     ) ^ rk[0];
2813         s1 = GETU32(pt +  4) ^ rk[1];
2814         s2 = GETU32(pt +  8) ^ rk[2];
2815         s3 = GETU32(pt + 12) ^ rk[3];
2816
2817 #define ROUND(i,d,s) \
2818 d##0 = TE0(s##0) ^ TE1(s##1) ^ TE2(s##2) ^ TE3(s##3) ^ rk[4 * i]; \
2819 d##1 = TE0(s##1) ^ TE1(s##2) ^ TE2(s##3) ^ TE3(s##0) ^ rk[4 * i + 1]; \
2820 d##2 = TE0(s##2) ^ TE1(s##3) ^ TE2(s##0) ^ TE3(s##1) ^ rk[4 * i + 2]; \
2821 d##3 = TE0(s##3) ^ TE1(s##0) ^ TE2(s##1) ^ TE3(s##2) ^ rk[4 * i + 3]
2822
2823 #ifdef FULL_UNROLL
2824
2825         ROUND(1,t,s);
2826         ROUND(2,s,t);
2827         ROUND(3,t,s);
2828         ROUND(4,s,t);
2829         ROUND(5,t,s);
2830         ROUND(6,s,t);
2831         ROUND(7,t,s);
2832         ROUND(8,s,t);
2833         ROUND(9,t,s);
2834
2835         rk += Nr << 2;
2836
2837 #else  /* !FULL_UNROLL */
2838
2839         /* Nr - 1 full rounds: */
2840         r = Nr >> 1;
2841         for (;;) {
2842                 ROUND(1,t,s);
2843                 rk += 8;
2844                 if (--r == 0)
2845                         break;
2846                 ROUND(0,s,t);
2847         }
2848
2849 #endif /* ?FULL_UNROLL */
2850
2851 #undef ROUND
2852
2853         /*
2854          * apply last round and
2855          * map cipher state to byte array block:
2856          */
2857         s0 = TE41(t0) ^ TE42(t1) ^ TE43(t2) ^ TE44(t3) ^ rk[0];
2858         PUTU32(ct     , s0);
2859         s1 = TE41(t1) ^ TE42(t2) ^ TE43(t3) ^ TE44(t0) ^ rk[1];
2860         PUTU32(ct +  4, s1);
2861         s2 = TE41(t2) ^ TE42(t3) ^ TE43(t0) ^ TE44(t1) ^ rk[2];
2862         PUTU32(ct +  8, s2);
2863         s3 = TE41(t3) ^ TE42(t0) ^ TE43(t1) ^ TE44(t2) ^ rk[3];
2864         PUTU32(ct + 12, s3);
2865 }
2866
2867 static void * aes_encrypt_init(u8 *key, size_t len)
2868 {
2869         u32 *rk;
2870         if (len != 16)
2871                 return NULL;
2872         rk = (u32*)rtw_malloc(AES_PRIV_SIZE);
2873         if (rk == NULL)
2874                 return NULL;
2875         rijndaelKeySetupEnc(rk, key);
2876         return rk;
2877 }
2878
2879 static void aes_128_encrypt(void *ctx, u8 *plain, u8 *crypt)
2880 {
2881         rijndaelEncrypt(ctx, plain, crypt);
2882 }
2883
2884
2885 static void gf_mulx(u8 *pad)
2886 {
2887         int i, carry;
2888
2889         carry = pad[0] & 0x80;
2890         for (i = 0; i < AES_BLOCK_SIZE - 1; i++)
2891                 pad[i] = (pad[i] << 1) | (pad[i + 1] >> 7);
2892         pad[AES_BLOCK_SIZE - 1] <<= 1;
2893         if (carry)
2894                 pad[AES_BLOCK_SIZE - 1] ^= 0x87;
2895 }
2896
2897 static void aes_encrypt_deinit(void *ctx)
2898 {
2899         _rtw_memset(ctx, 0, AES_PRIV_SIZE);
2900         rtw_mfree(ctx, AES_PRIV_SIZE);
2901 }
2902
2903
2904 /**
2905  * omac1_aes_128_vector - One-Key CBC MAC (OMAC1) hash with AES-128
2906  * @key: 128-bit key for the hash operation
2907  * @num_elem: Number of elements in the data vector
2908  * @addr: Pointers to the data areas
2909  * @len: Lengths of the data blocks
2910  * @mac: Buffer for MAC (128 bits, i.e., 16 bytes)
2911  * Returns: 0 on success, -1 on failure
2912  *
2913  * This is a mode for using block cipher (AES in this case) for authentication.
2914  * OMAC1 was standardized with the name CMAC by NIST in a Special Publication
2915  * (SP) 800-38B.
2916  */
2917 static int omac1_aes_128_vector(u8 *key, size_t num_elem,
2918                                                          u8 *addr[], size_t *len, u8 *mac)
2919 {
2920         void *ctx;
2921         u8 cbc[AES_BLOCK_SIZE], pad[AES_BLOCK_SIZE];
2922         u8 *pos, *end;
2923         size_t i, e, left, total_len;
2924
2925         ctx = aes_encrypt_init(key, 16);
2926         if (ctx == NULL)
2927                 return -1;
2928         _rtw_memset(cbc, 0, AES_BLOCK_SIZE);
2929
2930         total_len = 0;
2931         for (e = 0; e < num_elem; e++)
2932                 total_len += len[e];
2933         left = total_len;
2934
2935         e = 0;
2936         pos = addr[0];
2937         end = pos + len[0];
2938
2939         while (left >= AES_BLOCK_SIZE) {
2940                 for (i = 0; i < AES_BLOCK_SIZE; i++) {
2941                         cbc[i] ^= *pos++;
2942                         if (pos >= end) {
2943                                 e++;
2944                                 pos = addr[e];
2945                                 end = pos + len[e];
2946                         }
2947                 }
2948                 if (left > AES_BLOCK_SIZE)
2949                         aes_128_encrypt(ctx, cbc, cbc);
2950                 left -= AES_BLOCK_SIZE;
2951         }
2952
2953         _rtw_memset(pad, 0, AES_BLOCK_SIZE);
2954         aes_128_encrypt(ctx, pad, pad);
2955         gf_mulx(pad);
2956
2957         if (left || total_len == 0) {
2958                 for (i = 0; i < left; i++) {
2959                         cbc[i] ^= *pos++;
2960                         if (pos >= end) {
2961                                 e++;
2962                                 pos = addr[e];
2963                                 end = pos + len[e];
2964                         }
2965                 }
2966                 cbc[left] ^= 0x80;
2967                 gf_mulx(pad);
2968         }
2969
2970         for (i = 0; i < AES_BLOCK_SIZE; i++)
2971                 pad[i] ^= cbc[i];
2972         aes_128_encrypt(ctx, pad, mac);
2973         aes_encrypt_deinit(ctx);
2974         return 0;
2975 }
2976
2977
2978 /**
2979  * omac1_aes_128 - One-Key CBC MAC (OMAC1) hash with AES-128 (aka AES-CMAC)
2980  * @key: 128-bit key for the hash operation
2981  * @data: Data buffer for which a MAC is determined
2982  * @data_len: Length of data buffer in bytes
2983  * @mac: Buffer for MAC (128 bits, i.e., 16 bytes)
2984  * Returns: 0 on success, -1 on failure
2985  *
2986  * This is a mode for using block cipher (AES in this case) for authentication.
2987  * OMAC1 was standardized with the name CMAC by NIST in a Special Publication
2988  * (SP) 800-38B.
2989  */ //modify for CONFIG_IEEE80211W
2990 int omac1_aes_128(u8 *key, u8 *data, size_t data_len, u8 *mac)
2991 {
2992         return omac1_aes_128_vector(key, 1, &data, &data_len, mac);
2993 }
2994 #endif //PLATFORM_FREEBSD Baron
2995
2996 #ifdef CONFIG_TDLS
2997 void wpa_tdls_generate_tpk(_adapter *padapter, PVOID sta)
2998 {
2999         struct sta_info *psta = (struct sta_info *)sta;
3000         struct mlme_priv        *pmlmepriv = &padapter->mlmepriv;
3001         u8 *SNonce = psta->SNonce;
3002         u8 *ANonce = psta->ANonce;
3003
3004         u8 key_input[SHA256_MAC_LEN];
3005         u8 *nonce[2];
3006         size_t len[2];
3007         u8 data[3 * ETH_ALEN];
3008
3009         /* IEEE Std 802.11z-2010 8.5.9.1:
3010          * TPK-Key-Input = SHA-256(min(SNonce, ANonce) || max(SNonce, ANonce))
3011          */
3012         len[0] = 32;
3013         len[1] = 32;
3014         if (os_memcmp(SNonce, ANonce, 32) < 0) {
3015                 nonce[0] = SNonce;
3016                 nonce[1] = ANonce;
3017         } else {
3018                 nonce[0] = ANonce;
3019                 nonce[1] = SNonce;
3020         }
3021
3022         sha256_vector(2, nonce, len, key_input);
3023
3024         /*
3025          * TPK-Key-Data = KDF-N_KEY(TPK-Key-Input, "TDLS PMK",
3026          *      min(MAC_I, MAC_R) || max(MAC_I, MAC_R) || BSSID || N_KEY)
3027          * TODO: is N_KEY really included in KDF Context and if so, in which
3028          * presentation format (little endian 16-bit?) is it used? It gets
3029          * added by the KDF anyway..
3030          */
3031
3032         if (os_memcmp(myid(&(padapter->eeprompriv)), psta->hwaddr, ETH_ALEN) < 0) {
3033                 _rtw_memcpy(data, myid(&(padapter->eeprompriv)), ETH_ALEN);
3034                 _rtw_memcpy(data + ETH_ALEN, psta->hwaddr, ETH_ALEN);
3035         } else {
3036                 _rtw_memcpy(data, psta->hwaddr, ETH_ALEN);
3037                 _rtw_memcpy(data + ETH_ALEN, myid(&(padapter->eeprompriv)), ETH_ALEN);
3038         }
3039         _rtw_memcpy(data + 2 * ETH_ALEN, get_bssid(pmlmepriv), ETH_ALEN);
3040
3041         sha256_prf(key_input, SHA256_MAC_LEN, "TDLS PMK", data, sizeof(data), (u8 *) &psta->tpk, sizeof(psta->tpk));
3042
3043
3044 }
3045
3046 /**
3047  * wpa_tdls_ftie_mic - Calculate TDLS FTIE MIC
3048  * @kck: TPK-KCK
3049  * @lnkid: Pointer to the beginning of Link Identifier IE
3050  * @rsnie: Pointer to the beginning of RSN IE used for handshake
3051  * @timeoutie: Pointer to the beginning of Timeout IE used for handshake
3052  * @ftie: Pointer to the beginning of FT IE
3053  * @mic: Pointer for writing MIC
3054  *
3055  * Calculate MIC for TDLS frame.
3056  */
3057 int wpa_tdls_ftie_mic(u8 *kck, u8 trans_seq,
3058                                                         u8 *lnkid, u8 *rsnie, u8 *timeoutie, u8 *ftie,
3059                                                         u8 *mic)
3060 {
3061         u8 *buf, *pos;
3062         struct wpa_tdls_ftie *_ftie;
3063         struct wpa_tdls_lnkid *_lnkid;
3064         int ret;
3065         int len = 2 * ETH_ALEN + 1 + 2 + lnkid[1] + 2 + rsnie[1] +
3066                 2 + timeoutie[1] + 2 + ftie[1];
3067         buf = rtw_zmalloc(len);
3068         if (!buf) {
3069                 DBG_871X("TDLS: No memory for MIC calculation\n");
3070                 return -1;
3071         }
3072
3073         pos = buf;
3074         _lnkid = (struct wpa_tdls_lnkid *) lnkid;
3075         /* 1) TDLS initiator STA MAC address */
3076         _rtw_memcpy(pos, _lnkid->init_sta, ETH_ALEN);
3077         pos += ETH_ALEN;
3078         /* 2) TDLS responder STA MAC address */
3079         _rtw_memcpy(pos, _lnkid->resp_sta, ETH_ALEN);
3080         pos += ETH_ALEN;
3081         /* 3) Transaction Sequence number */
3082         *pos++ = trans_seq;
3083         /* 4) Link Identifier IE */
3084         _rtw_memcpy(pos, lnkid, 2 + lnkid[1]);
3085         pos += 2 + lnkid[1];
3086         /* 5) RSN IE */
3087         _rtw_memcpy(pos, rsnie, 2 + rsnie[1]);
3088         pos += 2 + rsnie[1];
3089         /* 6) Timeout Interval IE */
3090         _rtw_memcpy(pos, timeoutie, 2 + timeoutie[1]);
3091         pos += 2 + timeoutie[1];
3092         /* 7) FTIE, with the MIC field of the FTIE set to 0 */
3093         _rtw_memcpy(pos, ftie, 2 + ftie[1]);
3094         _ftie = (struct wpa_tdls_ftie *) pos;
3095         _rtw_memset(_ftie->mic, 0, TDLS_MIC_LEN);
3096         pos += 2 + ftie[1];
3097
3098         ret = omac1_aes_128(kck, buf, pos - buf, mic);
3099         rtw_mfree(buf, len);
3100         return ret;
3101
3102 }
3103
3104 int tdls_verify_mic(u8 *kck, u8 trans_seq,
3105                                                         u8 *lnkid, u8 *rsnie, u8 *timeoutie, u8 *ftie)
3106 {
3107         u8 *buf, *pos;
3108         int len;
3109         u8 mic[16];
3110         int ret;
3111         u8 *rx_ftie, *tmp_ftie;
3112
3113         if (lnkid == NULL || rsnie == NULL ||
3114             timeoutie == NULL || ftie == NULL){
3115                 return 0;
3116         }
3117
3118         len = 2 * ETH_ALEN + 1 + 2 + 18 + 2 + *(rsnie+1) + 2 + *(timeoutie+1) + 2 + *(ftie+1);
3119
3120         buf = rtw_zmalloc(len);
3121         if (buf == NULL)
3122                 return 0;
3123
3124         pos = buf;
3125         /* 1) TDLS initiator STA MAC address */
3126         _rtw_memcpy(pos, lnkid + ETH_ALEN + 2, ETH_ALEN);
3127         pos += ETH_ALEN;
3128         /* 2) TDLS responder STA MAC address */
3129         _rtw_memcpy(pos, lnkid + 2 * ETH_ALEN + 2, ETH_ALEN);
3130         pos += ETH_ALEN;
3131         /* 3) Transaction Sequence number */
3132         *pos++ = trans_seq;
3133         /* 4) Link Identifier IE */
3134         _rtw_memcpy(pos, lnkid, 2 + 18);
3135         pos += 2 + 18;
3136         /* 5) RSN IE */
3137         _rtw_memcpy(pos, rsnie, 2 + *(rsnie+1));
3138         pos += 2 + *(rsnie+1);
3139         /* 6) Timeout Interval IE */
3140         _rtw_memcpy(pos, timeoutie, 2 + *(timeoutie+1));
3141         pos += 2 + *(timeoutie+1);
3142         /* 7) FTIE, with the MIC field of the FTIE set to 0 */
3143         _rtw_memcpy(pos, ftie, 2 + *(ftie+1));
3144         pos += 2;
3145         tmp_ftie = (u8 *) (pos+2);
3146         _rtw_memset(tmp_ftie, 0, 16);
3147         pos += *(ftie+1);
3148
3149         ret = omac1_aes_128(kck, buf, pos - buf, mic);
3150         rtw_mfree(buf, len);
3151         if (ret)
3152                 return 0;
3153         rx_ftie = ftie+4;
3154
3155         if (os_memcmp(mic, rx_ftie, 16) == 0) {
3156                 //Valid MIC
3157                 return 1;
3158         }
3159
3160         //Invalid MIC
3161         DBG_871X( "[%s] Invalid MIC\n", __FUNCTION__);
3162         return 0;
3163
3164 }
3165 #endif //CONFIG_TDLS
3166
3167 void rtw_use_tkipkey_handler(RTW_TIMER_HDL_ARGS)
3168 {
3169         _adapter *padapter = (_adapter *)FunctionContext;
3170
3171 _func_enter_;
3172
3173         RT_TRACE(_module_rtl871x_security_c_,_drv_err_,("^^^rtw_use_tkipkey_handler ^^^\n"));
3174
3175 /*
3176         if(padapter->bDriverStopped ||padapter->bSurpriseRemoved){
3177                         RT_TRACE(_module_rtl871x_security_c_,_drv_err_,("^^^rtw_use_tkipkey_handler (padapter->bDriverStopped %d)(padapter->bSurpriseRemoved %d)^^^\n",padapter->bDriverStopped,padapter->bSurpriseRemoved));
3178
3179                 return;
3180         }
3181         */
3182
3183         padapter->securitypriv.busetkipkey=_TRUE;
3184
3185         RT_TRACE(_module_rtl871x_security_c_,_drv_err_,("^^^rtw_use_tkipkey_handler padapter->securitypriv.busetkipkey=%d^^^\n",padapter->securitypriv.busetkipkey));
3186
3187 _func_exit_;
3188
3189 }
3190
3191 /* Restore HW wep key setting according to key_mask */
3192 void rtw_sec_restore_wep_key(_adapter *adapter)
3193 {
3194         struct security_priv* securitypriv=&(adapter->securitypriv);
3195         sint keyid;
3196
3197         if((_WEP40_ == securitypriv->dot11PrivacyAlgrthm) ||(_WEP104_ == securitypriv->dot11PrivacyAlgrthm)) {
3198                 for(keyid=0;keyid<4;keyid++){
3199                         if(securitypriv->key_mask & BIT(keyid)){
3200                                 if(keyid == securitypriv->dot11PrivacyKeyIndex)
3201                                         rtw_set_key(adapter,securitypriv, keyid, 1, _FALSE);
3202                                 else
3203                                         rtw_set_key(adapter,securitypriv, keyid, 0, _FALSE);
3204                         }
3205                 }
3206         }
3207 }
3208
3209 u8 rtw_handle_tkip_countermeasure(_adapter* adapter, const char *caller)
3210 {
3211         struct security_priv* securitypriv=&(adapter->securitypriv);
3212         u8 status = _SUCCESS;
3213
3214         if (securitypriv->btkip_countermeasure == _TRUE) {
3215                 u32 passing_ms = rtw_get_passing_time_ms(securitypriv->btkip_countermeasure_time);
3216                 if (passing_ms > 60*1000) {
3217                         DBG_871X_LEVEL(_drv_always_, "%s("ADPT_FMT") countermeasure time:%ds > 60s \n",
3218                                 caller, ADPT_ARG(adapter), passing_ms/1000);
3219                         securitypriv->btkip_countermeasure = _FALSE;
3220                         securitypriv->btkip_countermeasure_time = 0;
3221                 } else {
3222                         DBG_871X_LEVEL(_drv_always_, "%s("ADPT_FMT") countermeasure time:%ds < 60s \n",
3223                                 caller, ADPT_ARG(adapter), passing_ms/1000);
3224                         status = _FAIL;
3225                 }
3226         }
3227
3228         return status;
3229 }
3230