drivers/net/wireless/rockchip_wlan/rtl8723bu/core/rtw_ieee80211.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 _IEEE80211_C
  21
  22 #ifdef CONFIG_PLATFORM_INTEL_BYT
  23 #include <linux/fs.h>
  24 #endif
  25 #include <drv_types.h>
  26
  27
  28 u8 RTW_WPA_OUI_TYPE[] = { 0x00, 0x50, 0xf2, 1 };
  29 u16 RTW_WPA_VERSION = 1;
  30 u8 WPA_AUTH_KEY_MGMT_NONE[] = { 0x00, 0x50, 0xf2, 0 };
  31 u8 WPA_AUTH_KEY_MGMT_UNSPEC_802_1X[] = { 0x00, 0x50, 0xf2, 1 };
  32 u8 WPA_AUTH_KEY_MGMT_PSK_OVER_802_1X[] = { 0x00, 0x50, 0xf2, 2 };
  33 u8 WPA_CIPHER_SUITE_NONE[] = { 0x00, 0x50, 0xf2, 0 };
  34 u8 WPA_CIPHER_SUITE_WEP40[] = { 0x00, 0x50, 0xf2, 1 };
  35 u8 WPA_CIPHER_SUITE_TKIP[] = { 0x00, 0x50, 0xf2, 2 };
  36 u8 WPA_CIPHER_SUITE_WRAP[] = { 0x00, 0x50, 0xf2, 3 };
  37 u8 WPA_CIPHER_SUITE_CCMP[] = { 0x00, 0x50, 0xf2, 4 };
  38 u8 WPA_CIPHER_SUITE_WEP104[] = { 0x00, 0x50, 0xf2, 5 };
  39
  40 u16 RSN_VERSION_BSD = 1;
  41 u8 RSN_AUTH_KEY_MGMT_UNSPEC_802_1X[] = { 0x00, 0x0f, 0xac, 1 };
  42 u8 RSN_AUTH_KEY_MGMT_PSK_OVER_802_1X[] = { 0x00, 0x0f, 0xac, 2 };
  43 u8 RSN_CIPHER_SUITE_NONE[] = { 0x00, 0x0f, 0xac, 0 };
  44 u8 RSN_CIPHER_SUITE_WEP40[] = { 0x00, 0x0f, 0xac, 1 };
  45 u8 RSN_CIPHER_SUITE_TKIP[] = { 0x00, 0x0f, 0xac, 2 };
  46 u8 RSN_CIPHER_SUITE_WRAP[] = { 0x00, 0x0f, 0xac, 3 };
  47 u8 RSN_CIPHER_SUITE_CCMP[] = { 0x00, 0x0f, 0xac, 4 };
  48 u8 RSN_CIPHER_SUITE_WEP104[] = { 0x00, 0x0f, 0xac, 5 };
  49 //-----------------------------------------------------------
  50 // for adhoc-master to generate ie and provide supported-rate to fw
  51 //-----------------------------------------------------------
  52
  53 static u8       WIFI_CCKRATES[] =
  54 {(IEEE80211_CCK_RATE_1MB | IEEE80211_BASIC_RATE_MASK),
  55  (IEEE80211_CCK_RATE_2MB | IEEE80211_BASIC_RATE_MASK),
  56  (IEEE80211_CCK_RATE_5MB | IEEE80211_BASIC_RATE_MASK),
  57  (IEEE80211_CCK_RATE_11MB | IEEE80211_BASIC_RATE_MASK)};
  58
  59 static u8       WIFI_OFDMRATES[] =
  60 {(IEEE80211_OFDM_RATE_6MB),
  61  (IEEE80211_OFDM_RATE_9MB),
  62  (IEEE80211_OFDM_RATE_12MB),
  63  (IEEE80211_OFDM_RATE_18MB),
  64  (IEEE80211_OFDM_RATE_24MB),
  65  IEEE80211_OFDM_RATE_36MB,
  66  IEEE80211_OFDM_RATE_48MB,
  67  IEEE80211_OFDM_RATE_54MB};
  68
  69
  70 int rtw_get_bit_value_from_ieee_value(u8 val)
  71 {
  72         unsigned char dot11_rate_table[]={2,4,11,22,12,18,24,36,48,72,96,108,0}; // last element must be zero!!
  73
  74         int i=0;
  75         while(dot11_rate_table[i] != 0) {
  76                 if (dot11_rate_table[i] == val)
  77                         return BIT(i);
  78                 i++;
  79         }
  80         return 0;
  81 }
  82
  83 uint    rtw_is_cckrates_included(u8 *rate)
  84 {
  85                 u32     i = 0;
  86
  87                 while(rate[i]!=0)
  88                 {
  89                         if  (  (((rate[i]) & 0x7f) == 2)        || (((rate[i]) & 0x7f) == 4) ||
  90                         (((rate[i]) & 0x7f) == 11)  || (((rate[i]) & 0x7f) == 22) )
  91                         return _TRUE;
  92                         i++;
  93                 }
  94
  95                 return _FALSE;
  96 }
  97
  98 uint    rtw_is_cckratesonly_included(u8 *rate)
  99 {
 100         u32 i = 0;
 101
 102
 103         while(rate[i]!=0)
 104         {
 105                         if  (  (((rate[i]) & 0x7f) != 2) && (((rate[i]) & 0x7f) != 4) &&
 106                                 (((rate[i]) & 0x7f) != 11)  && (((rate[i]) & 0x7f) != 22) )
 107
 108                         return _FALSE;
 109
 110                         i++;
 111         }
 112
 113         return _TRUE;
 114
 115 }
 116
 117 int rtw_check_network_type(unsigned char *rate, int ratelen, int channel)
 118 {
 119         if (channel > 14)
 120         {
 121                 if ((rtw_is_cckrates_included(rate)) == _TRUE)
 122                         return WIRELESS_INVALID;
 123                 else
 124                         return WIRELESS_11A;
 125         }
 126         else  // could be pure B, pure G, or B/G
 127         {
 128                 if ((rtw_is_cckratesonly_included(rate)) == _TRUE)
 129                         return WIRELESS_11B;
 130                 else if((rtw_is_cckrates_included(rate)) == _TRUE)
 131                         return  WIRELESS_11BG;
 132                 else
 133                         return WIRELESS_11G;
 134         }
 135
 136 }
 137
 138 u8 *rtw_set_fixed_ie(unsigned char *pbuf, unsigned int len, unsigned char *source,
 139                                 unsigned int *frlen)
 140 {
 141         _rtw_memcpy((void *)pbuf, (void *)source, len);
 142         *frlen = *frlen + len;
 143         return (pbuf + len);
 144 }
 145
 146 // rtw_set_ie will update frame length
 147 u8 *rtw_set_ie
 148 (
 149         u8 *pbuf,
 150         sint index,
 151         uint len,
 152         u8 *source,
 153         uint *frlen //frame length
 154 )
 155 {
 156         *pbuf = (u8)index;
 157
 158         *(pbuf + 1) = (u8)len;
 159
 160         if (len > 0)
 161                 _rtw_memcpy((void *)(pbuf + 2), (void *)source, len);
 162
 163         *frlen = *frlen + (len + 2);
 164
 165         return (pbuf + len + 2);
 166 }
 167
 168 inline u8 *rtw_set_ie_ch_switch(u8 *buf, u32 *buf_len, u8 ch_switch_mode,
 169         u8 new_ch, u8 ch_switch_cnt)
 170 {
 171         u8 ie_data[3];
 172
 173         ie_data[0] = ch_switch_mode;
 174         ie_data[1] = new_ch;
 175         ie_data[2] = ch_switch_cnt;
 176         return rtw_set_ie(buf, WLAN_EID_CHANNEL_SWITCH,  3, ie_data, buf_len);
 177 }
 178
 179 inline u8 secondary_ch_offset_to_hal_ch_offset(u8 ch_offset)
 180 {
 181         if (ch_offset == SCN)
 182                 return HAL_PRIME_CHNL_OFFSET_DONT_CARE;
 183         else if(ch_offset == SCA)
 184                 return HAL_PRIME_CHNL_OFFSET_UPPER;
 185         else if(ch_offset == SCB)
 186                 return HAL_PRIME_CHNL_OFFSET_LOWER;
 187
 188         return HAL_PRIME_CHNL_OFFSET_DONT_CARE;
 189 }
 190
 191 inline u8 hal_ch_offset_to_secondary_ch_offset(u8 ch_offset)
 192 {
 193         if (ch_offset == HAL_PRIME_CHNL_OFFSET_DONT_CARE)
 194                 return SCN;
 195         else if(ch_offset == HAL_PRIME_CHNL_OFFSET_LOWER)
 196                 return SCB;
 197         else if(ch_offset == HAL_PRIME_CHNL_OFFSET_UPPER)
 198                 return SCA;
 199
 200         return SCN;
 201 }
 202
 203 inline u8 *rtw_set_ie_secondary_ch_offset(u8 *buf, u32 *buf_len, u8 secondary_ch_offset)
 204 {
 205         return rtw_set_ie(buf, WLAN_EID_SECONDARY_CHANNEL_OFFSET,  1, &secondary_ch_offset, buf_len);
 206 }
 207
 208 inline u8 *rtw_set_ie_mesh_ch_switch_parm(u8 *buf, u32 *buf_len, u8 ttl,
 209         u8 flags, u16 reason, u16 precedence)
 210 {
 211         u8 ie_data[6];
 212
 213         ie_data[0] = ttl;
 214         ie_data[1] = flags;
 215         RTW_PUT_LE16((u8*)&ie_data[2], reason);
 216         RTW_PUT_LE16((u8*)&ie_data[4], precedence);
 217
 218         return rtw_set_ie(buf, 0x118,  6, ie_data, buf_len);
 219 }
 220
 221 /*----------------------------------------------------------------------------
 222 index: the information element id index, limit is the limit for search
 223 -----------------------------------------------------------------------------*/
 224 u8 *rtw_get_ie(u8 *pbuf, sint index, sint *len, sint limit)
 225 {
 226         sint tmp,i;
 227         u8 *p;
 228 _func_enter_;
 229         if (limit < 1){
 230                 _func_exit_;
 231                 return NULL;
 232         }
 233
 234         p = pbuf;
 235         i = 0;
 236         *len = 0;
 237         while(1)
 238         {
 239                 if (*p == index)
 240                 {
 241                         *len = *(p + 1);
 242                         return (p);
 243                 }
 244                 else
 245                 {
 246                         tmp = *(p + 1);
 247                         p += (tmp + 2);
 248                         i += (tmp + 2);
 249                 }
 250                 if (i >= limit)
 251                         break;
 252         }
 253 _func_exit_;
 254         return NULL;
 255 }
 256
 257 /**
 258  * rtw_get_ie_ex - Search specific IE from a series of IEs
 259  * @in_ie: Address of IEs to search
 260  * @in_len: Length limit from in_ie
 261  * @eid: Element ID to match
 262  * @oui: OUI to match
 263  * @oui_len: OUI length
 264  * @ie: If not NULL and the specific IE is found, the IE will be copied to the buf starting from the specific IE
 265  * @ielen: If not NULL and the specific IE is found, will set to the length of the entire IE
 266  *
 267  * Returns: The address of the specific IE found, or NULL
 268  */
 269 u8 *rtw_get_ie_ex(u8 *in_ie, uint in_len, u8 eid, u8 *oui, u8 oui_len, u8 *ie, uint *ielen)
 270 {
 271         uint cnt;
 272         u8 *target_ie = NULL;
 273
 274
 275         if(ielen)
 276                 *ielen = 0;
 277
 278         if(!in_ie || in_len<=0)
 279                 return target_ie;
 280
 281         cnt = 0;
 282
 283         while(cnt<in_len)
 284         {
 285                 if(eid == in_ie[cnt]
 286                         && ( !oui || _rtw_memcmp(&in_ie[cnt+2], oui, oui_len) == _TRUE))
 287                 {
 288                         target_ie = &in_ie[cnt];
 289
 290                         if(ie)
 291                                 _rtw_memcpy(ie, &in_ie[cnt], in_ie[cnt+1]+2);
 292
 293                         if(ielen)
 294                                 *ielen = in_ie[cnt+1]+2;
 295
 296                         break;
 297                 }
 298                 else
 299                 {
 300                         cnt+=in_ie[cnt+1]+2; //goto next
 301                 }
 302
 303         }
 304
 305         return target_ie;
 306 }
 307
 308 /**
 309  * rtw_ies_remove_ie - Find matching IEs and remove
 310  * @ies: Address of IEs to search
 311  * @ies_len: Pointer of length of ies, will update to new length
 312  * @offset: The offset to start scarch
 313  * @eid: Element ID to match
 314  * @oui: OUI to match
 315  * @oui_len: OUI length
 316  *
 317  * Returns: _SUCCESS: ies is updated, _FAIL: not updated
 318  */
 319 int rtw_ies_remove_ie(u8 *ies, uint *ies_len, uint offset, u8 eid, u8 *oui, u8 oui_len)
 320 {
 321         int ret = _FAIL;
 322         u8 *target_ie;
 323         u32 target_ielen;
 324         u8 *start;
 325         uint search_len;
 326
 327         if(!ies || !ies_len || *ies_len <= offset)
 328                 goto exit;
 329
 330         start = ies + offset;
 331         search_len = *ies_len - offset;
 332
 333         while (1) {
 334                 target_ie = rtw_get_ie_ex(start, search_len, eid, oui, oui_len, NULL, &target_ielen);
 335                 if (target_ie && target_ielen) {
 336                         u8 buf[MAX_IE_SZ] = {0};
 337                         u8 *remain_ies = target_ie + target_ielen;
 338                         uint remain_len = search_len - (remain_ies - start);
 339
 340                         _rtw_memcpy(buf, remain_ies, remain_len);
 341                         _rtw_memcpy(target_ie, buf, remain_len);
 342                         *ies_len = *ies_len - target_ielen;
 343                         ret = _SUCCESS;
 344
 345                         start = target_ie;
 346                         search_len = remain_len;
 347                 } else {
 348                         break;
 349                 }
 350         }
 351 exit:
 352         return ret;
 353 }
 354
 355 void rtw_set_supported_rate(u8* SupportedRates, uint mode)
 356 {
 357 _func_enter_;
 358
 359         _rtw_memset(SupportedRates, 0, NDIS_802_11_LENGTH_RATES_EX);
 360
 361         switch (mode)
 362         {
 363                 case WIRELESS_11B:
 364                         _rtw_memcpy(SupportedRates, WIFI_CCKRATES, IEEE80211_CCK_RATE_LEN);
 365                         break;
 366
 367                 case WIRELESS_11G:
 368                 case WIRELESS_11A:
 369                 case WIRELESS_11_5N:
 370                 case WIRELESS_11A_5N://Todo: no basic rate for ofdm ?
 371                 case WIRELESS_11_5AC:
 372                         _rtw_memcpy(SupportedRates, WIFI_OFDMRATES, IEEE80211_NUM_OFDM_RATESLEN);
 373                         break;
 374
 375                 case WIRELESS_11BG:
 376                 case WIRELESS_11G_24N:
 377                 case WIRELESS_11_24N:
 378                 case WIRELESS_11BG_24N:
 379                         _rtw_memcpy(SupportedRates, WIFI_CCKRATES, IEEE80211_CCK_RATE_LEN);
 380                         _rtw_memcpy(SupportedRates + IEEE80211_CCK_RATE_LEN, WIFI_OFDMRATES, IEEE80211_NUM_OFDM_RATESLEN);
 381                         break;
 382
 383         }
 384 _func_exit_;
 385 }
 386
 387 uint    rtw_get_rateset_len(u8  *rateset)
 388 {
 389         uint i = 0;
 390 _func_enter_;
 391         while(1)
 392         {
 393                 if ((rateset[i]) == 0)
 394                         break;
 395
 396                 if (i > 12)
 397                         break;
 398
 399                 i++;
 400         }
 401 _func_exit_;
 402         return i;
 403 }
 404
 405 int rtw_generate_ie(struct registry_priv *pregistrypriv)
 406 {
 407         u8      wireless_mode;
 408         int     sz = 0, rateLen;
 409         WLAN_BSSID_EX*  pdev_network = &pregistrypriv->dev_network;
 410         u8*     ie = pdev_network->IEs;
 411
 412 _func_enter_;
 413
 414         //timestamp will be inserted by hardware
 415         sz += 8;
 416         ie += sz;
 417
 418         //beacon interval : 2bytes
 419         *(u16*)ie = cpu_to_le16((u16)pdev_network->Configuration.BeaconPeriod);//BCN_INTERVAL;
 420         sz += 2;
 421         ie += 2;
 422
 423         //capability info
 424         *(u16*)ie = 0;
 425
 426         *(u16*)ie |= cpu_to_le16(cap_IBSS);
 427
 428         if(pregistrypriv->preamble == PREAMBLE_SHORT)
 429                 *(u16*)ie |= cpu_to_le16(cap_ShortPremble);
 430
 431         if (pdev_network->Privacy)
 432                 *(u16*)ie |= cpu_to_le16(cap_Privacy);
 433
 434         sz += 2;
 435         ie += 2;
 436
 437         //SSID
 438         ie = rtw_set_ie(ie, _SSID_IE_, pdev_network->Ssid.SsidLength, pdev_network->Ssid.Ssid, &sz);
 439
 440         //supported rates
 441         if(pregistrypriv->wireless_mode == WIRELESS_11ABGN)
 442         {
 443                 if(pdev_network->Configuration.DSConfig > 14)
 444                         wireless_mode = WIRELESS_11A_5N;
 445                 else
 446                         wireless_mode = WIRELESS_11BG_24N;
 447         }
 448         else
 449         {
 450                 wireless_mode = pregistrypriv->wireless_mode;
 451         }
 452
 453         rtw_set_supported_rate(pdev_network->SupportedRates, wireless_mode) ;
 454
 455         rateLen = rtw_get_rateset_len(pdev_network->SupportedRates);
 456
 457         if (rateLen > 8)
 458         {
 459                 ie = rtw_set_ie(ie, _SUPPORTEDRATES_IE_, 8, pdev_network->SupportedRates, &sz);
 460                 //ie = rtw_set_ie(ie, _EXT_SUPPORTEDRATES_IE_, (rateLen - 8), (pdev_network->SupportedRates + 8), &sz);
 461         }
 462         else
 463         {
 464                 ie = rtw_set_ie(ie, _SUPPORTEDRATES_IE_, rateLen, pdev_network->SupportedRates, &sz);
 465         }
 466
 467         //DS parameter set
 468         ie = rtw_set_ie(ie, _DSSET_IE_, 1, (u8 *)&(pdev_network->Configuration.DSConfig), &sz);
 469
 470
 471         //IBSS Parameter Set
 472
 473         ie = rtw_set_ie(ie, _IBSS_PARA_IE_, 2, (u8 *)&(pdev_network->Configuration.ATIMWindow), &sz);
 474
 475         if (rateLen > 8)
 476         {
 477                 ie = rtw_set_ie(ie, _EXT_SUPPORTEDRATES_IE_, (rateLen - 8), (pdev_network->SupportedRates + 8), &sz);
 478         }
 479
 480 #ifdef CONFIG_80211N_HT
 481         //HT Cap.
 482         if(((pregistrypriv->wireless_mode&WIRELESS_11_5N)||(pregistrypriv->wireless_mode&WIRELESS_11_24N))
 483                 && (pregistrypriv->ht_enable==_TRUE))
 484         {
 485                 //todo:
 486         }
 487 #endif //CONFIG_80211N_HT
 488
 489         //pdev_network->IELength =  sz; //update IELength
 490
 491 _func_exit_;
 492
 493         //return _SUCCESS;
 494
 495         return sz;
 496
 497 }
 498
 499 unsigned char *rtw_get_wpa_ie(unsigned char *pie, int *wpa_ie_len, int limit)
 500 {
 501         int len;
 502         u16 val16;
 503         unsigned char wpa_oui_type[] = {0x00, 0x50, 0xf2, 0x01};
 504         u8 *pbuf = pie;
 505         int limit_new = limit;
 506
 507         while(1)
 508         {
 509                 pbuf = rtw_get_ie(pbuf, _WPA_IE_ID_, &len, limit_new);
 510
 511                 if (pbuf) {
 512
 513                         //check if oui matches...
 514                         if (_rtw_memcmp((pbuf + 2), wpa_oui_type, sizeof (wpa_oui_type)) == _FALSE) {
 515
 516                                 goto check_next_ie;
 517                         }
 518
 519                         //check version...
 520                         _rtw_memcpy((u8 *)&val16, (pbuf + 6), sizeof(val16));
 521
 522                         val16 = le16_to_cpu(val16);
 523                         if (val16 != 0x0001)
 524                                 goto check_next_ie;
 525
 526                         *wpa_ie_len = *(pbuf + 1);
 527
 528                         return pbuf;
 529
 530                 }
 531                 else {
 532
 533                         *wpa_ie_len = 0;
 534                         return NULL;
 535                 }
 536
 537 check_next_ie:
 538
 539                 limit_new = limit - (pbuf - pie) - 2 - len;
 540
 541                 if (limit_new <= 0)
 542                         break;
 543
 544                 pbuf += (2 + len);
 545
 546         }
 547
 548         *wpa_ie_len = 0;
 549
 550         return NULL;
 551
 552 }
 553
 554 unsigned char *rtw_get_wpa2_ie(unsigned char *pie, int *rsn_ie_len, int limit)
 555 {
 556
 557         return rtw_get_ie(pie, _WPA2_IE_ID_,rsn_ie_len, limit);
 558
 559 }
 560
 561 int rtw_get_wpa_cipher_suite(u8 *s)
 562 {
 563         if (_rtw_memcmp(s, WPA_CIPHER_SUITE_NONE, WPA_SELECTOR_LEN) == _TRUE)
 564                 return WPA_CIPHER_NONE;
 565         if (_rtw_memcmp(s, WPA_CIPHER_SUITE_WEP40, WPA_SELECTOR_LEN) == _TRUE)
 566                 return WPA_CIPHER_WEP40;
 567         if (_rtw_memcmp(s, WPA_CIPHER_SUITE_TKIP, WPA_SELECTOR_LEN) == _TRUE)
 568                 return WPA_CIPHER_TKIP;
 569         if (_rtw_memcmp(s, WPA_CIPHER_SUITE_CCMP, WPA_SELECTOR_LEN) == _TRUE)
 570                 return WPA_CIPHER_CCMP;
 571         if (_rtw_memcmp(s, WPA_CIPHER_SUITE_WEP104, WPA_SELECTOR_LEN) == _TRUE)
 572                 return WPA_CIPHER_WEP104;
 573
 574         return 0;
 575 }
 576
 577 int rtw_get_wpa2_cipher_suite(u8 *s)
 578 {
 579         if (_rtw_memcmp(s, RSN_CIPHER_SUITE_NONE, RSN_SELECTOR_LEN) == _TRUE)
 580                 return WPA_CIPHER_NONE;
 581         if (_rtw_memcmp(s, RSN_CIPHER_SUITE_WEP40, RSN_SELECTOR_LEN) == _TRUE)
 582                 return WPA_CIPHER_WEP40;
 583         if (_rtw_memcmp(s, RSN_CIPHER_SUITE_TKIP, RSN_SELECTOR_LEN) == _TRUE)
 584                 return WPA_CIPHER_TKIP;
 585         if (_rtw_memcmp(s, RSN_CIPHER_SUITE_CCMP, RSN_SELECTOR_LEN) == _TRUE)
 586                 return WPA_CIPHER_CCMP;
 587         if (_rtw_memcmp(s, RSN_CIPHER_SUITE_WEP104, RSN_SELECTOR_LEN) == _TRUE)
 588                 return WPA_CIPHER_WEP104;
 589
 590         return 0;
 591 }
 592
 593
 594 int rtw_parse_wpa_ie(u8* wpa_ie, int wpa_ie_len, int *group_cipher, int *pairwise_cipher, int *is_8021x)
 595 {
 596         int i, ret=_SUCCESS;
 597         int left, count;
 598         u8 *pos;
 599         u8 SUITE_1X[4] = {0x00, 0x50, 0xf2, 1};
 600
 601         if (wpa_ie_len <= 0) {
 602                 /* No WPA IE - fail silently */
 603                 return _FAIL;
 604         }
 605
 606
 607         if ((*wpa_ie != _WPA_IE_ID_) || (*(wpa_ie+1) != (u8)(wpa_ie_len - 2)) ||
 608            (_rtw_memcmp(wpa_ie+2, RTW_WPA_OUI_TYPE, WPA_SELECTOR_LEN) != _TRUE) )
 609         {
 610                 return _FAIL;
 611         }
 612
 613         pos = wpa_ie;
 614
 615         pos += 8;
 616         left = wpa_ie_len - 8;
 617
 618
 619         //group_cipher
 620         if (left >= WPA_SELECTOR_LEN) {
 621
 622                 *group_cipher = rtw_get_wpa_cipher_suite(pos);
 623
 624                 pos += WPA_SELECTOR_LEN;
 625                 left -= WPA_SELECTOR_LEN;
 626
 627         }
 628         else if (left > 0)
 629         {
 630                 RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("%s: ie length mismatch, %u too much", __FUNCTION__, left));
 631
 632                 return _FAIL;
 633         }
 634
 635
 636         //pairwise_cipher
 637         if (left >= 2)
 638         {
 639                 //count = le16_to_cpu(*(u16*)pos);
 640                 count = RTW_GET_LE16(pos);
 641                 pos += 2;
 642                 left -= 2;
 643
 644                 if (count == 0 || left < count * WPA_SELECTOR_LEN) {
 645                         RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("%s: ie count botch (pairwise), "
 646                                                 "count %u left %u", __FUNCTION__, count, left));
 647                         return _FAIL;
 648                 }
 649
 650                 for (i = 0; i < count; i++)
 651                 {
 652                         *pairwise_cipher |= rtw_get_wpa_cipher_suite(pos);
 653
 654                         pos += WPA_SELECTOR_LEN;
 655                         left -= WPA_SELECTOR_LEN;
 656                 }
 657
 658         }
 659         else if (left == 1)
 660         {
 661                 RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("%s: ie too short (for key mgmt)",   __FUNCTION__));
 662                 return _FAIL;
 663         }
 664
 665         if (is_8021x) {
 666                 if (left >= 6) {
 667                         pos += 2;
 668                         if (_rtw_memcmp(pos, SUITE_1X, 4) == 1) {
 669                                 RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("%s : there has 802.1x auth\n", __FUNCTION__));
 670                                 *is_8021x = 1;
 671                         }
 672                 }
 673         }
 674
 675         return ret;
 676
 677 }
 678
 679 int rtw_parse_wpa2_ie(u8* rsn_ie, int rsn_ie_len, int *group_cipher, int *pairwise_cipher, int *is_8021x)
 680 {
 681         int i, ret=_SUCCESS;
 682         int left, count;
 683         u8 *pos;
 684         u8 SUITE_1X[4] = {0x00,0x0f, 0xac, 0x01};
 685
 686         if (rsn_ie_len <= 0) {
 687                 /* No RSN IE - fail silently */
 688                 return _FAIL;
 689         }
 690
 691
 692         if ((*rsn_ie!= _WPA2_IE_ID_) || (*(rsn_ie+1) != (u8)(rsn_ie_len - 2)))
 693         {
 694                 return _FAIL;
 695         }
 696
 697         pos = rsn_ie;
 698         pos += 4;
 699         left = rsn_ie_len - 4;
 700
 701         //group_cipher
 702         if (left >= RSN_SELECTOR_LEN) {
 703
 704                 *group_cipher = rtw_get_wpa2_cipher_suite(pos);
 705
 706                 pos += RSN_SELECTOR_LEN;
 707                 left -= RSN_SELECTOR_LEN;
 708
 709         } else if (left > 0) {
 710                 RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("%s: ie length mismatch, %u too much", __FUNCTION__, left));
 711                 return _FAIL;
 712         }
 713
 714         //pairwise_cipher
 715         if (left >= 2)
 716         {
 717                 //count = le16_to_cpu(*(u16*)pos);
 718                 count = RTW_GET_LE16(pos);
 719                 pos += 2;
 720                 left -= 2;
 721
 722                 if (count == 0 || left < count * RSN_SELECTOR_LEN) {
 723                         RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("%s: ie count botch (pairwise), "
 724                                                  "count %u left %u", __FUNCTION__, count, left));
 725                         return _FAIL;
 726                 }
 727
 728                 for (i = 0; i < count; i++)
 729                 {
 730                         *pairwise_cipher |= rtw_get_wpa2_cipher_suite(pos);
 731
 732                         pos += RSN_SELECTOR_LEN;
 733                         left -= RSN_SELECTOR_LEN;
 734                 }
 735
 736         }
 737         else if (left == 1)
 738         {
 739                 RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("%s: ie too short (for key mgmt)",  __FUNCTION__));
 740
 741                 return _FAIL;
 742         }
 743
 744         if (is_8021x) {
 745                 if (left >= 6) {
 746                         pos += 2;
 747                         if (_rtw_memcmp(pos, SUITE_1X, 4) == 1) {
 748                                 RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("%s (): there has 802.1x auth\n", __FUNCTION__));
 749                                 *is_8021x = 1;
 750                         }
 751                 }
 752         }
 753
 754         return ret;
 755
 756 }
 757
 758 //#ifdef CONFIG_WAPI_SUPPORT
 759 int rtw_get_wapi_ie(u8 *in_ie,uint in_len,u8 *wapi_ie,u16 *wapi_len)
 760 {
 761         int len = 0;
 762         u8 authmode, i;
 763         uint    cnt;
 764         u8 wapi_oui1[4]={0x0,0x14,0x72,0x01};
 765         u8 wapi_oui2[4]={0x0,0x14,0x72,0x02};
 766
 767 _func_enter_;
 768
 769         if(wapi_len)
 770                 *wapi_len = 0;
 771
 772         if(!in_ie || in_len<=0)
 773                 return len;
 774
 775         cnt = (_TIMESTAMP_ + _BEACON_ITERVAL_ + _CAPABILITY_);
 776
 777         while(cnt<in_len)
 778         {
 779                 authmode=in_ie[cnt];
 780
 781                 //if(authmode==_WAPI_IE_)
 782                 if(authmode==_WAPI_IE_ && (_rtw_memcmp(&in_ie[cnt+6], wapi_oui1,4)==_TRUE ||
 783                                         _rtw_memcmp(&in_ie[cnt+6], wapi_oui2,4)==_TRUE))
 784                 {
 785                         if (wapi_ie) {
 786                                 _rtw_memcpy(wapi_ie, &in_ie[cnt],in_ie[cnt+1]+2);
 787
 788                                 for(i=0;i<(in_ie[cnt+1]+2);i=i+8){
 789                                         RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("\n %2x,%2x,%2x,%2x,%2x,%2x,%2x,%2x\n",
 790                                                                 wapi_ie[i],wapi_ie[i+1],wapi_ie[i+2],wapi_ie[i+3],wapi_ie[i+4],
 791                                                                 wapi_ie[i+5],wapi_ie[i+6],wapi_ie[i+7]));
 792                                 }
 793                         }
 794
 795                         if(wapi_len)
 796                                 *wapi_len=in_ie[cnt+1]+2;
 797
 798                         cnt+=in_ie[cnt+1]+2;  //get next
 799                 }
 800                 else
 801                 {
 802                         cnt+=in_ie[cnt+1]+2;   //get next
 803                 }
 804         }
 805
 806         if(wapi_len)
 807                 len = *wapi_len;
 808
 809 _func_exit_;
 810
 811         return len;
 812
 813 }
 814 //#endif
 815
 816 int rtw_get_sec_ie(u8 *in_ie,uint in_len,u8 *rsn_ie,u16 *rsn_len,u8 *wpa_ie,u16 *wpa_len)
 817 {
 818         u8 authmode, sec_idx, i;
 819         u8 wpa_oui[4]={0x0,0x50,0xf2,0x01};
 820         uint    cnt;
 821
 822 _func_enter_;
 823
 824         //Search required WPA or WPA2 IE and copy to sec_ie[ ]
 825
 826         cnt = (_TIMESTAMP_ + _BEACON_ITERVAL_ + _CAPABILITY_);
 827
 828         sec_idx=0;
 829
 830         while(cnt<in_len)
 831         {
 832                 authmode=in_ie[cnt];
 833
 834                 if((authmode==_WPA_IE_ID_)&&(_rtw_memcmp(&in_ie[cnt+2], &wpa_oui[0],4)==_TRUE))
 835                 {
 836                                 RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("\n rtw_get_wpa_ie: sec_idx=%d in_ie[cnt+1]+2=%d\n",sec_idx,in_ie[cnt+1]+2));
 837
 838                                 if (wpa_ie) {
 839                                 _rtw_memcpy(wpa_ie, &in_ie[cnt],in_ie[cnt+1]+2);
 840
 841                                 for(i=0;i<(in_ie[cnt+1]+2);i=i+8){
 842                                                 RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("\n %2x,%2x,%2x,%2x,%2x,%2x,%2x,%2x\n",
 843                                                                         wpa_ie[i],wpa_ie[i+1],wpa_ie[i+2],wpa_ie[i+3],wpa_ie[i+4],
 844                                                                         wpa_ie[i+5],wpa_ie[i+6],wpa_ie[i+7]));
 845                                         }
 846                                 }
 847
 848                                 *wpa_len=in_ie[cnt+1]+2;
 849                                 cnt+=in_ie[cnt+1]+2;  //get next
 850                 }
 851                 else
 852                 {
 853                         if(authmode==_WPA2_IE_ID_)
 854                         {
 855                                 RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("\n get_rsn_ie: sec_idx=%d in_ie[cnt+1]+2=%d\n",sec_idx,in_ie[cnt+1]+2));
 856
 857                                 if (rsn_ie) {
 858                                 _rtw_memcpy(rsn_ie, &in_ie[cnt],in_ie[cnt+1]+2);
 859
 860                                 for(i=0;i<(in_ie[cnt+1]+2);i=i+8){
 861                                                 RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("\n %2x,%2x,%2x,%2x,%2x,%2x,%2x,%2x\n",
 862                                                                         rsn_ie[i],rsn_ie[i+1],rsn_ie[i+2],rsn_ie[i+3],rsn_ie[i+4],
 863                                                                         rsn_ie[i+5],rsn_ie[i+6],rsn_ie[i+7]));
 864                                         }
 865                                 }
 866
 867                                 *rsn_len=in_ie[cnt+1]+2;
 868                                 cnt+=in_ie[cnt+1]+2;  //get next
 869                         }
 870                         else
 871                         {
 872                                 cnt+=in_ie[cnt+1]+2;   //get next
 873                         }
 874                 }
 875
 876         }
 877
 878 _func_exit_;
 879
 880         return (*rsn_len+*wpa_len);
 881
 882 }
 883
 884 u8 rtw_is_wps_ie(u8 *ie_ptr, uint *wps_ielen)
 885 {
 886         u8 match = _FALSE;
 887         u8 eid, wps_oui[4]={0x0,0x50,0xf2,0x04};
 888
 889         if(ie_ptr == NULL) return match;
 890
 891         eid = ie_ptr[0];
 892
 893         if((eid==_WPA_IE_ID_)&&(_rtw_memcmp(&ie_ptr[2], wps_oui, 4)==_TRUE))
 894         {
 895                 //DBG_8192C("==> found WPS_IE.....\n");
 896                 *wps_ielen = ie_ptr[1]+2;
 897                 match=_TRUE;
 898         }
 899         return match;
 900 }
 901
 902 u8 *rtw_get_wps_ie_from_scan_queue(u8 *in_ie, uint in_len, u8 *wps_ie, uint *wps_ielen, u8 frame_type)
 903 {
 904         u8*     wps = NULL;
 905
 906         DBG_871X( "[%s] frame_type = %d\n", __FUNCTION__, frame_type );
 907         switch( frame_type )
 908         {
 909                 case 1:
 910                 case 3:
 911                 {       //      Beacon or Probe Response
 912                         wps = rtw_get_wps_ie(in_ie + _PROBERSP_IE_OFFSET_, in_len - _PROBERSP_IE_OFFSET_, wps_ie, wps_ielen);
 913                         break;
 914                 }
 915                 case 2:
 916                 {       //      Probe Request
 917                         wps = rtw_get_wps_ie(in_ie + _PROBEREQ_IE_OFFSET_ , in_len - _PROBEREQ_IE_OFFSET_ , wps_ie, wps_ielen);
 918                         break;
 919                 }
 920         }
 921         return wps;
 922 }
 923
 924 /**
 925  * rtw_get_wps_ie - Search WPS IE from a series of IEs
 926  * @in_ie: Address of IEs to search
 927  * @in_len: Length limit from in_ie
 928  * @wps_ie: If not NULL and WPS IE is found, WPS IE will be copied to the buf starting from wps_ie
 929  * @wps_ielen: If not NULL and WPS IE is found, will set to the length of the entire WPS IE
 930  *
 931  * Returns: The address of the WPS IE found, or NULL
 932  */
 933 u8 *rtw_get_wps_ie(u8 *in_ie, uint in_len, u8 *wps_ie, uint *wps_ielen)
 934 {
 935         uint cnt;
 936         u8 *wpsie_ptr=NULL;
 937         u8 eid, wps_oui[4]={0x0,0x50,0xf2,0x04};
 938
 939         if(wps_ielen)
 940                 *wps_ielen = 0;
 941
 942         if(!in_ie || in_len<=0)
 943                 return wpsie_ptr;
 944
 945         cnt = 0;
 946
 947         while(cnt<in_len)
 948         {
 949                 eid = in_ie[cnt];
 950
 951                 if((eid==_WPA_IE_ID_)&&(_rtw_memcmp(&in_ie[cnt+2], wps_oui, 4)==_TRUE))
 952                 {
 953                         wpsie_ptr = &in_ie[cnt];
 954
 955                         if(wps_ie)
 956                                 _rtw_memcpy(wps_ie, &in_ie[cnt], in_ie[cnt+1]+2);
 957
 958                         if(wps_ielen)
 959                                 *wps_ielen = in_ie[cnt+1]+2;
 960
 961                         cnt+=in_ie[cnt+1]+2;
 962
 963                         break;
 964                 }
 965                 else
 966                 {
 967                         cnt+=in_ie[cnt+1]+2; //goto next
 968                 }
 969
 970         }
 971
 972         return wpsie_ptr;
 973 }
 974
 975 /**
 976  * rtw_get_wps_attr - Search a specific WPS attribute from a given WPS IE
 977  * @wps_ie: Address of WPS IE to search
 978  * @wps_ielen: Length limit from wps_ie
 979  * @target_attr_id: The attribute ID of WPS attribute to search
 980  * @buf_attr: If not NULL and the WPS attribute is found, WPS attribute will be copied to the buf starting from buf_attr
 981  * @len_attr: If not NULL and the WPS attribute is found, will set to the length of the entire WPS attribute
 982  *
 983  * Returns: the address of the specific WPS attribute found, or NULL
 984  */
 985 u8 *rtw_get_wps_attr(u8 *wps_ie, uint wps_ielen, u16 target_attr_id ,u8 *buf_attr, u32 *len_attr)
 986 {
 987         u8 *attr_ptr = NULL;
 988         u8 * target_attr_ptr = NULL;
 989         u8 wps_oui[4]={0x00,0x50,0xF2,0x04};
 990
 991         if(len_attr)
 992                 *len_attr = 0;
 993
 994         if ( ( wps_ie[0] != _VENDOR_SPECIFIC_IE_ ) ||
 995                 ( _rtw_memcmp( wps_ie + 2, wps_oui , 4 ) != _TRUE ) )
 996         {
 997                 return attr_ptr;
 998         }
 999
1000         // 6 = 1(Element ID) + 1(Length) + 4(WPS OUI)
1001         attr_ptr = wps_ie + 6; //goto first attr
1002
1003         while(attr_ptr - wps_ie < wps_ielen)
1004         {
1005                 // 4 = 2(Attribute ID) + 2(Length)
1006                 u16 attr_id = RTW_GET_BE16(attr_ptr);
1007                 u16 attr_data_len = RTW_GET_BE16(attr_ptr + 2);
1008                 u16 attr_len = attr_data_len + 4;
1009
1010                 //DBG_871X("%s attr_ptr:%p, id:%u, length:%u\n", __FUNCTION__, attr_ptr, attr_id, attr_data_len);
1011                 if( attr_id == target_attr_id )
1012                 {
1013                         target_attr_ptr = attr_ptr;
1014
1015                         if(buf_attr)
1016                                 _rtw_memcpy(buf_attr, attr_ptr, attr_len);
1017
1018                         if(len_attr)
1019                                 *len_attr = attr_len;
1020
1021                         break;
1022                 }
1023                 else
1024                 {
1025                         attr_ptr += attr_len; //goto next
1026                 }
1027
1028         }
1029
1030         return target_attr_ptr;
1031 }
1032
1033 /**
1034  * rtw_get_wps_attr_content - Search a specific WPS attribute content from a given WPS IE
1035  * @wps_ie: Address of WPS IE to search
1036  * @wps_ielen: Length limit from wps_ie
1037  * @target_attr_id: The attribute ID of WPS attribute to search
1038  * @buf_content: If not NULL and the WPS attribute is found, WPS attribute content will be copied to the buf starting from buf_content
1039  * @len_content: If not NULL and the WPS attribute is found, will set to the length of the WPS attribute content
1040  *
1041  * Returns: the address of the specific WPS attribute content found, or NULL
1042  */
1043 u8 *rtw_get_wps_attr_content(u8 *wps_ie, uint wps_ielen, u16 target_attr_id ,u8 *buf_content, uint *len_content)
1044 {
1045         u8 *attr_ptr;
1046         u32 attr_len;
1047
1048         if(len_content)
1049                 *len_content = 0;
1050
1051         attr_ptr = rtw_get_wps_attr(wps_ie, wps_ielen, target_attr_id, NULL, &attr_len);
1052
1053         if(attr_ptr && attr_len)
1054         {
1055                 if(buf_content)
1056                         _rtw_memcpy(buf_content, attr_ptr+4, attr_len-4);
1057
1058                 if(len_content)
1059                         *len_content = attr_len-4;
1060
1061                 return attr_ptr+4;
1062         }
1063
1064         return NULL;
1065 }
1066
1067 static int rtw_ieee802_11_parse_vendor_specific(u8 *pos, uint elen,
1068                                             struct rtw_ieee802_11_elems *elems,
1069                                             int show_errors)
1070 {
1071         unsigned int oui;
1072
1073         /* first 3 bytes in vendor specific information element are the IEEE
1074          * OUI of the vendor. The following byte is used a vendor specific
1075          * sub-type. */
1076         if (elen < 4) {
1077                 if (show_errors) {
1078                         DBG_871X("short vendor specific "
1079                                    "information element ignored (len=%lu)\n",
1080                                    (unsigned long) elen);
1081                 }
1082                 return -1;
1083         }
1084
1085         oui = RTW_GET_BE24(pos);
1086         switch (oui) {
1087         case OUI_MICROSOFT:
1088                 /* Microsoft/Wi-Fi information elements are further typed and
1089                  * subtyped */
1090                 switch (pos[3]) {
1091                 case 1:
1092                         /* Microsoft OUI (00:50:F2) with OUI Type 1:
1093                          * real WPA information element */
1094                         elems->wpa_ie = pos;
1095                         elems->wpa_ie_len = elen;
1096                         break;
1097                 case WME_OUI_TYPE: /* this is a Wi-Fi WME info. element */
1098                         if (elen < 5) {
1099                                 DBG_871X("short WME "
1100                                            "information element ignored "
1101                                            "(len=%lu)\n",
1102                                            (unsigned long) elen);
1103                                 return -1;
1104                         }
1105                         switch (pos[4]) {
1106                         case WME_OUI_SUBTYPE_INFORMATION_ELEMENT:
1107                         case WME_OUI_SUBTYPE_PARAMETER_ELEMENT:
1108                                 elems->wme = pos;
1109                                 elems->wme_len = elen;
1110                                 break;
1111                         case WME_OUI_SUBTYPE_TSPEC_ELEMENT:
1112                                 elems->wme_tspec = pos;
1113                                 elems->wme_tspec_len = elen;
1114                                 break;
1115                         default:
1116                                 DBG_871X_LEVEL(_drv_warning_, "unknown WME "
1117                                            "information element ignored "
1118                                            "(subtype=%d len=%lu)\n",
1119                                            pos[4], (unsigned long) elen);
1120                                 return -1;
1121                         }
1122                         break;
1123                 case 4:
1124                         /* Wi-Fi Protected Setup (WPS) IE */
1125                         elems->wps_ie = pos;
1126                         elems->wps_ie_len = elen;
1127                         break;
1128                 default:
1129                         DBG_871X_LEVEL(_drv_warning_, "Unknown Microsoft "
1130                                    "information element ignored "
1131                                    "(type=%d len=%lu)\n",
1132                                    pos[3], (unsigned long) elen);
1133                         return -1;
1134                 }
1135                 break;
1136
1137         case OUI_BROADCOM:
1138                 switch (pos[3]) {
1139                 case VENDOR_HT_CAPAB_OUI_TYPE:
1140                         elems->vendor_ht_cap = pos;
1141                         elems->vendor_ht_cap_len = elen;
1142                         break;
1143                 default:
1144                         DBG_871X_LEVEL(_drv_warning_, "Unknown Broadcom "
1145                                    "information element ignored "
1146                                    "(type=%d len=%lu)\n",
1147                                    pos[3], (unsigned long) elen);
1148                         return -1;
1149                 }
1150                 break;
1151
1152         default:
1153                 DBG_871X_LEVEL(_drv_warning_, "unknown vendor specific information "
1154                            "element ignored (vendor OUI %02x:%02x:%02x "
1155                            "len=%lu)\n",
1156                            pos[0], pos[1], pos[2], (unsigned long) elen);
1157                 return -1;
1158         }
1159
1160         return 0;
1161
1162 }
1163
1164 /**
1165  * ieee802_11_parse_elems - Parse information elements in management frames
1166  * @start: Pointer to the start of IEs
1167  * @len: Length of IE buffer in octets
1168  * @elems: Data structure for parsed elements
1169  * @show_errors: Whether to show parsing errors in debug log
1170  * Returns: Parsing result
1171  */
1172 ParseRes rtw_ieee802_11_parse_elems(u8 *start, uint len,
1173                                 struct rtw_ieee802_11_elems *elems,
1174                                 int show_errors)
1175 {
1176         uint left = len;
1177         u8 *pos = start;
1178         int unknown = 0;
1179
1180         _rtw_memset(elems, 0, sizeof(*elems));
1181
1182         while (left >= 2) {
1183                 u8 id, elen;
1184
1185                 id = *pos++;
1186                 elen = *pos++;
1187                 left -= 2;
1188
1189                 if (elen > left) {
1190                         if (show_errors) {
1191                                 DBG_871X("IEEE 802.11 element "
1192                                            "parse failed (id=%d elen=%d "
1193                                            "left=%lu)\n",
1194                                            id, elen, (unsigned long) left);
1195                         }
1196                         return ParseFailed;
1197                 }
1198
1199                 switch (id) {
1200                 case WLAN_EID_SSID:
1201                         elems->ssid = pos;
1202                         elems->ssid_len = elen;
1203                         break;
1204                 case WLAN_EID_SUPP_RATES:
1205                         elems->supp_rates = pos;
1206                         elems->supp_rates_len = elen;
1207                         break;
1208                 case WLAN_EID_FH_PARAMS:
1209                         elems->fh_params = pos;
1210                         elems->fh_params_len = elen;
1211                         break;
1212                 case WLAN_EID_DS_PARAMS:
1213                         elems->ds_params = pos;
1214                         elems->ds_params_len = elen;
1215                         break;
1216                 case WLAN_EID_CF_PARAMS:
1217                         elems->cf_params = pos;
1218                         elems->cf_params_len = elen;
1219                         break;
1220                 case WLAN_EID_TIM:
1221                         elems->tim = pos;
1222                         elems->tim_len = elen;
1223                         break;
1224                 case WLAN_EID_IBSS_PARAMS:
1225                         elems->ibss_params = pos;
1226                         elems->ibss_params_len = elen;
1227                         break;
1228                 case WLAN_EID_CHALLENGE:
1229                         elems->challenge = pos;
1230                         elems->challenge_len = elen;
1231                         break;
1232                 case WLAN_EID_ERP_INFO:
1233                         elems->erp_info = pos;
1234                         elems->erp_info_len = elen;
1235                         break;
1236                 case WLAN_EID_EXT_SUPP_RATES:
1237                         elems->ext_supp_rates = pos;
1238                         elems->ext_supp_rates_len = elen;
1239                         break;
1240                 case WLAN_EID_VENDOR_SPECIFIC:
1241                         if (rtw_ieee802_11_parse_vendor_specific(pos, elen,
1242                                                              elems,
1243                                                              show_errors))
1244                                 unknown++;
1245                         break;
1246                 case WLAN_EID_RSN:
1247                         elems->rsn_ie = pos;
1248                         elems->rsn_ie_len = elen;
1249                         break;
1250                 case WLAN_EID_PWR_CAPABILITY:
1251                         elems->power_cap = pos;
1252                         elems->power_cap_len = elen;
1253                         break;
1254                 case WLAN_EID_SUPPORTED_CHANNELS:
1255                         elems->supp_channels = pos;
1256                         elems->supp_channels_len = elen;
1257                         break;
1258                 case WLAN_EID_MOBILITY_DOMAIN:
1259                         elems->mdie = pos;
1260                         elems->mdie_len = elen;
1261                         break;
1262                 case WLAN_EID_FAST_BSS_TRANSITION:
1263                         elems->ftie = pos;
1264                         elems->ftie_len = elen;
1265                         break;
1266                 case WLAN_EID_TIMEOUT_INTERVAL:
1267                         elems->timeout_int = pos;
1268                         elems->timeout_int_len = elen;
1269                         break;
1270                 case WLAN_EID_HT_CAP:
1271                         elems->ht_capabilities = pos;
1272                         elems->ht_capabilities_len = elen;
1273                         break;
1274                 case WLAN_EID_HT_OPERATION:
1275                         elems->ht_operation = pos;
1276                         elems->ht_operation_len = elen;
1277                         break;
1278                 case WLAN_EID_VHT_CAPABILITY:
1279                         elems->vht_capabilities = pos;
1280                         elems->vht_capabilities_len = elen;
1281                         break;
1282                 case WLAN_EID_VHT_OPERATION:
1283                         elems->vht_operation = pos;
1284                         elems->vht_operation_len = elen;
1285                         break;
1286                 case WLAN_EID_VHT_OP_MODE_NOTIFY:
1287                         elems->vht_op_mode_notify = pos;
1288                         elems->vht_op_mode_notify_len = elen;
1289                         break;
1290                 default:
1291                         unknown++;
1292                         if (!show_errors)
1293                                 break;
1294                         DBG_871X_LEVEL(_drv_warning_,
1295                                 "IEEE 802.11 element parse "
1296                                 "ignored unknown element (id=%d elen=%d)\n",
1297                                 id, elen);
1298                         break;
1299                 }
1300
1301                 left -= elen;
1302                 pos += elen;
1303         }
1304
1305         if (left)
1306                 return ParseFailed;
1307
1308         return unknown ? ParseUnknown : ParseOK;
1309
1310 }
1311
1312 static u8 key_char2num(u8 ch);
1313 static u8 key_char2num(u8 ch)
1314 {
1315     if((ch>='0')&&(ch<='9'))
1316         return ch - '0';
1317     else if ((ch>='a')&&(ch<='f'))
1318         return ch - 'a' + 10;
1319     else if ((ch>='A')&&(ch<='F'))
1320         return ch - 'A' + 10;
1321     else
1322          return 0xff;
1323 }
1324
1325 u8 str_2char2num(u8 hch, u8 lch);
1326 u8 str_2char2num(u8 hch, u8 lch)
1327 {
1328     return ((key_char2num(hch) * 10 ) + key_char2num(lch));
1329 }
1330
1331 u8 key_2char2num(u8 hch, u8 lch);
1332 u8 key_2char2num(u8 hch, u8 lch)
1333 {
1334     return ((key_char2num(hch) << 4) | key_char2num(lch));
1335 }
1336
1337 void macstr2num(u8 *dst, u8 *src);
1338 void macstr2num(u8 *dst, u8 *src)
1339 {
1340         int     jj, kk;
1341         for (jj = 0, kk = 0; jj < ETH_ALEN; jj++, kk += 3)
1342         {
1343                 dst[jj] = key_2char2num(src[kk], src[kk + 1]);
1344         }
1345 }
1346
1347 u8 convert_ip_addr(u8 hch, u8 mch, u8 lch)
1348 {
1349     return ((key_char2num(hch) * 100) + (key_char2num(mch) * 10 ) + key_char2num(lch));
1350 }
1351
1352 #ifdef CONFIG_PLATFORM_INTEL_BYT
1353 #define MAC_ADDRESS_LEN 12
1354
1355 int rtw_get_mac_addr_intel(unsigned char *buf)
1356 {
1357         int ret = 0;
1358         int i;
1359         struct file *fp = NULL;
1360         mm_segment_t oldfs;
1361         unsigned char c_mac[MAC_ADDRESS_LEN];
1362         char fname[]="/config/wifi/mac.txt";
1363         int jj,kk;
1364
1365         DBG_871X("%s Enter\n", __FUNCTION__);
1366
1367         ret = rtw_retrive_from_file(fname, c_mac, MAC_ADDRESS_LEN);
1368         if(ret < MAC_ADDRESS_LEN)
1369         {
1370                 return -1;
1371         }
1372
1373         for( jj = 0, kk = 0; jj < ETH_ALEN; jj++, kk += 2 )
1374         {
1375                 buf[jj] = key_2char2num(c_mac[kk], c_mac[kk+ 1]);
1376         }
1377
1378         DBG_871X("%s: read from file mac address: "MAC_FMT"\n",
1379                  __FUNCTION__, MAC_ARG(buf));
1380
1381         return 0;
1382 }
1383 #endif //CONFIG_PLATFORM_INTEL_BYT
1384
1385 extern char* rtw_initmac;
1386 #include <linux/rfkill-wlan.h>
1387 void rtw_macaddr_cfg(u8 *mac_addr)
1388 {
1389         u8 mac[ETH_ALEN];
1390         if(mac_addr == NULL)    return;
1391
1392         if ( rtw_initmac )
1393         {       //      Users specify the mac address
1394                 int jj,kk;
1395
1396                 for( jj = 0, kk = 0; jj < ETH_ALEN; jj++, kk += 3 )
1397                 {
1398                         mac[jj] = key_2char2num(rtw_initmac[kk], rtw_initmac[kk+ 1]);
1399                 }
1400                 _rtw_memcpy(mac_addr, mac, ETH_ALEN);
1401         }
1402 #ifdef CONFIG_PLATFORM_INTEL_BYT
1403         else if (0 == rtw_get_mac_addr_intel(mac))
1404         {
1405                 _rtw_memcpy(mac_addr, mac, ETH_ALEN);
1406         }
1407 #endif //CONFIG_PLATFORM_INTEL_BYT
1408         else
1409     {
1410         printk("Wifi Efuse Mac => %02x:%02x:%02x:%02x:%02x:%02x\n", mac_addr[0], mac_addr[1],
1411             mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]);
1412         if (!rockchip_wifi_mac_addr(mac)) {
1413             printk("=========> get mac address from flash=[%02x:%02x:%02x:%02x:%02x:%02x]\n", mac[0], mac[1],
1414                 mac[2], mac[3], mac[4], mac[5]);
1415             _rtw_memcpy(mac_addr, mac, ETH_ALEN);
1416         } else {
1417             //  Use the mac address stored in the Efuse
1418             _rtw_memcpy(mac, mac_addr, ETH_ALEN);
1419         }
1420     }
1421
1422         if (((mac[0]==0xff) &&(mac[1]==0xff) && (mac[2]==0xff) &&
1423              (mac[3]==0xff) && (mac[4]==0xff) &&(mac[5]==0xff)) ||
1424             ((mac[0]==0x0) && (mac[1]==0x0) && (mac[2]==0x0) &&
1425              (mac[3]==0x0) && (mac[4]==0x0) &&(mac[5]==0x0)))
1426         {
1427                 mac[0] = 0x00;
1428                 mac[1] = 0xe0;
1429                 mac[2] = 0x4c;
1430                 mac[3] = 0x87;
1431                 mac[4] = 0x00;
1432                 mac[5] = 0x00;
1433                 // use default mac addresss
1434                 _rtw_memcpy(mac_addr, mac, ETH_ALEN);
1435                 DBG_871X("MAC Address from efuse error, assign default one !!!\n");
1436         }
1437
1438         DBG_871X("rtw_macaddr_cfg MAC Address  = "MAC_FMT"\n", MAC_ARG(mac_addr));
1439 }
1440
1441 #ifdef CONFIG_80211N_HT
1442 void dump_ht_cap_ie_content(void *sel, u8 *buf, u32 buf_len)
1443 {
1444         if (buf_len != 26) {
1445                 DBG_871X_SEL_NL(sel, "Invalid HT capability IE len:%d != %d\n", buf_len, 26);
1446                 return;
1447         }
1448
1449         DBG_871X_SEL_NL(sel, "HT Capabilities Info:%02x%02x\n", *(buf), *(buf+1));
1450         DBG_871X_SEL_NL(sel, "A-MPDU Parameters:"HT_AMPDU_PARA_FMT"\n"
1451                 , HT_AMPDU_PARA_ARG(HT_CAP_ELE_AMPDU_PARA(buf)));
1452         DBG_871X_SEL_NL(sel, "Supported MCS Set:"HT_SUP_MCS_SET_FMT"\n"
1453                 , HT_SUP_MCS_SET_ARG(HT_CAP_ELE_SUP_MCS_SET(buf)));
1454 }
1455
1456 void dump_ht_cap_ie(void *sel, u8 *ie, u32 ie_len)
1457 {
1458         u8* pos = (u8*)ie;
1459         u16 id;
1460         u16 len;
1461
1462         u8 *ht_cap_ie;
1463         sint ht_cap_ielen;
1464
1465         ht_cap_ie = rtw_get_ie(ie, _HT_CAPABILITY_IE_, &ht_cap_ielen, ie_len);
1466         if(!ie || ht_cap_ie != ie)
1467                 return;
1468
1469         dump_ht_cap_ie_content(sel, ht_cap_ie+2, ht_cap_ielen);
1470 }
1471 #endif /* CONFIG_80211N_HT */
1472
1473 void dump_ies(void *sel, u8 *buf, u32 buf_len)
1474 {
1475         u8* pos = (u8*)buf;
1476         u8 id, len;
1477
1478         while(pos-buf+1<buf_len){
1479                 id = *pos;
1480                 len = *(pos+1);
1481
1482                 DBG_871X_SEL_NL(sel, "%s ID:%u, LEN:%u\n", __FUNCTION__, id, len);
1483                 #ifdef CONFIG_80211N_HT
1484                 dump_ht_cap_ie(sel, pos, len);
1485                 #endif
1486                 dump_wps_ie(sel, pos, len);
1487                 #ifdef CONFIG_P2P
1488                 dump_p2p_ie(sel, pos, len);
1489                 #ifdef CONFIG_WFD
1490                 dump_wfd_ie(sel, pos, len);
1491                 #endif
1492                 #endif
1493
1494                 pos+=(2+len);
1495         }
1496 }
1497
1498 void dump_wps_ie(void *sel, u8 *ie, u32 ie_len)
1499 {
1500         u8* pos = (u8*)ie;
1501         u16 id;
1502         u16 len;
1503
1504         u8 *wps_ie;
1505         uint wps_ielen;
1506
1507         wps_ie = rtw_get_wps_ie(ie, ie_len, NULL, &wps_ielen);
1508         if(wps_ie != ie || wps_ielen == 0)
1509                 return;
1510
1511         pos+=6;
1512         while(pos-ie < ie_len){
1513                 id = RTW_GET_BE16(pos);
1514                 len = RTW_GET_BE16(pos + 2);
1515
1516                 DBG_871X_SEL_NL(sel, "%s ID:0x%04x, LEN:%u\n", __FUNCTION__, id, len);
1517
1518                 pos+=(4+len);
1519         }
1520 }
1521
1522 #ifdef CONFIG_P2P
1523 /**
1524  * rtw_get_p2p_merged_len - Get merged ie length from muitiple p2p ies.
1525  * @in_ie: Pointer of the first p2p ie
1526  * @in_len: Total len of muiltiple p2p ies
1527  * Returns: Length of merged p2p ie length
1528  */
1529 u32 rtw_get_p2p_merged_ies_len(u8 *in_ie, u32 in_len)
1530 {
1531         PNDIS_802_11_VARIABLE_IEs       pIE;
1532         u8 OUI[4] = { 0x50, 0x6f, 0x9a, 0x09 };
1533         int i=0;
1534         int j=0, len=0;
1535
1536         while( i < in_len)
1537         {
1538                 pIE = (PNDIS_802_11_VARIABLE_IEs)(in_ie+ i);
1539
1540                 if( pIE->ElementID == _VENDOR_SPECIFIC_IE_ && _rtw_memcmp(pIE->data, OUI, 4) )
1541                 {
1542                         len += pIE->Length-4; // 4 is P2P OUI length, don't count it in this loop
1543                 }
1544
1545                 i += (pIE->Length + 2);
1546         }
1547
1548         return len + 4; // Append P2P OUI length at last.
1549 }
1550
1551 /**
1552  * rtw_p2p_merge_ies - Merge muitiple p2p ies into one
1553  * @in_ie: Pointer of the first p2p ie
1554  * @in_len: Total len of muiltiple p2p ies
1555  * @merge_ie: Pointer of merged ie
1556  * Returns: Length of merged p2p ie
1557  */
1558 int rtw_p2p_merge_ies(u8 *in_ie, u32 in_len, u8 *merge_ie)
1559 {
1560         PNDIS_802_11_VARIABLE_IEs       pIE;
1561         u8 len = 0;
1562         u8 OUI[4] = { 0x50, 0x6f, 0x9a, 0x09 };
1563         u8 ELOUI[6] = { 0xDD, 0x00, 0x50, 0x6f, 0x9a, 0x09 };   //EID;Len;OUI, Len would copy at the end of function
1564         int i=0;
1565
1566         if( merge_ie != NULL)
1567         {
1568                 //Set first P2P OUI
1569                 _rtw_memcpy(merge_ie, ELOUI, 6);
1570                 merge_ie += 6;
1571
1572                 while( i < in_len)
1573                 {
1574                         pIE = (PNDIS_802_11_VARIABLE_IEs)(in_ie+ i);
1575
1576                         // Take out the rest of P2P OUIs
1577                         if( pIE->ElementID == _VENDOR_SPECIFIC_IE_ && _rtw_memcmp(pIE->data, OUI, 4) )
1578                         {
1579                                 _rtw_memcpy( merge_ie, pIE->data +4, pIE->Length -4);
1580                                 len += pIE->Length-4;
1581                                 merge_ie += pIE->Length-4;
1582                         }
1583
1584                         i += (pIE->Length + 2);
1585                 }
1586
1587                 return len + 4; // 4 is for P2P OUI
1588
1589         }
1590
1591         return 0;
1592 }
1593
1594 void dump_p2p_ie(void *sel, u8 *ie, u32 ie_len) {
1595         u8* pos = (u8*)ie;
1596         u8 id;
1597         u16 len;
1598
1599         u8 *p2p_ie;
1600         uint p2p_ielen;
1601
1602         p2p_ie = rtw_get_p2p_ie(ie, ie_len, NULL, &p2p_ielen);
1603         if(p2p_ie != ie || p2p_ielen == 0)
1604                 return;
1605
1606         pos+=6;
1607         while(pos-ie < ie_len){
1608                 id = *pos;
1609                 len = RTW_GET_LE16(pos+1);
1610
1611                 DBG_871X_SEL_NL(sel, "%s ID:%u, LEN:%u\n", __FUNCTION__, id, len);
1612
1613                 pos+=(3+len);
1614         }
1615 }
1616
1617 u8 *rtw_get_p2p_ie_from_scan_queue(u8 *in_ie, int in_len, u8 *p2p_ie, uint *p2p_ielen, u8 frame_type)
1618 {
1619         u8*     p2p = NULL;
1620
1621         DBG_871X( "[%s] frame_type = %d\n", __FUNCTION__, frame_type );
1622         switch( frame_type )
1623         {
1624                 case 1:
1625                 case 3:
1626                 {       //      Beacon or Probe Response
1627                         p2p = rtw_get_p2p_ie(in_ie + _PROBERSP_IE_OFFSET_, in_len - _PROBERSP_IE_OFFSET_, p2p_ie, p2p_ielen);
1628                         break;
1629                 }
1630                 case 2:
1631                 {       //      Probe Request
1632                         p2p = rtw_get_p2p_ie(in_ie + _PROBEREQ_IE_OFFSET_ , in_len - _PROBEREQ_IE_OFFSET_ , p2p_ie, p2p_ielen);
1633                         break;
1634                 }
1635         }
1636         return p2p;
1637 }
1638
1639 /**
1640  * rtw_get_p2p_ie - Search P2P IE from a series of IEs
1641  * @in_ie: Address of IEs to search
1642  * @in_len: Length limit from in_ie
1643  * @p2p_ie: If not NULL and P2P IE is found, P2P IE will be copied to the buf starting from p2p_ie
1644  * @p2p_ielen: If not NULL and P2P IE is found, will set to the length of the entire P2P IE
1645  *
1646  * Returns: The address of the P2P IE found, or NULL
1647  */
1648 u8 *rtw_get_p2p_ie(u8 *in_ie, int in_len, u8 *p2p_ie, uint *p2p_ielen)
1649 {
1650         uint cnt = 0;
1651         u8 *p2p_ie_ptr;
1652         u8 eid, p2p_oui[4]={0x50,0x6F,0x9A,0x09};
1653
1654         if ( p2p_ielen != NULL )
1655                 *p2p_ielen = 0;
1656
1657         while(cnt<in_len)
1658         {
1659                 eid = in_ie[cnt];
1660                 if ((in_len < 0) || (cnt > MAX_IE_SZ)) {
1661                         rtw_dump_stack();
1662                         return NULL;
1663                 }
1664                 if( ( eid == _VENDOR_SPECIFIC_IE_ ) && ( _rtw_memcmp( &in_ie[cnt+2], p2p_oui, 4) == _TRUE ) )
1665                 {
1666                         p2p_ie_ptr = in_ie + cnt;
1667
1668                         if ( p2p_ie != NULL )
1669                         {
1670                                 _rtw_memcpy( p2p_ie, &in_ie[ cnt ], in_ie[ cnt + 1 ] + 2 );
1671                         }
1672
1673                         if ( p2p_ielen != NULL )
1674                         {
1675                                 *p2p_ielen = in_ie[ cnt + 1 ] + 2;
1676                         }
1677
1678                         return p2p_ie_ptr;
1679
1680                         break;
1681                 }
1682                 else
1683                 {
1684                         cnt += in_ie[ cnt + 1 ] +2; //goto next
1685                 }
1686
1687         }
1688
1689         return NULL;
1690
1691 }
1692
1693 /**
1694  * rtw_get_p2p_attr - Search a specific P2P attribute from a given P2P IE
1695  * @p2p_ie: Address of P2P IE to search
1696  * @p2p_ielen: Length limit from p2p_ie
1697  * @target_attr_id: The attribute ID of P2P attribute to search
1698  * @buf_attr: If not NULL and the P2P attribute is found, P2P attribute will be copied to the buf starting from buf_attr
1699  * @len_attr: If not NULL and the P2P attribute is found, will set to the length of the entire P2P attribute
1700  *
1701  * Returns: the address of the specific WPS attribute found, or NULL
1702  */
1703 u8 *rtw_get_p2p_attr(u8 *p2p_ie, uint p2p_ielen, u8 target_attr_id ,u8 *buf_attr, u32 *len_attr)
1704 {
1705         u8 *attr_ptr = NULL;
1706         u8 *target_attr_ptr = NULL;
1707         u8 p2p_oui[4]={0x50,0x6F,0x9A,0x09};
1708
1709         if(len_attr)
1710                 *len_attr = 0;
1711
1712         if ( !p2p_ie || ( p2p_ie[0] != _VENDOR_SPECIFIC_IE_ ) ||
1713                 ( _rtw_memcmp( p2p_ie + 2, p2p_oui , 4 ) != _TRUE ) )
1714         {
1715                 return attr_ptr;
1716         }
1717
1718         // 6 = 1(Element ID) + 1(Length) + 3 (OUI) + 1(OUI Type)
1719         attr_ptr = p2p_ie + 6; //goto first attr
1720
1721         while(attr_ptr - p2p_ie < p2p_ielen)
1722         {
1723                 // 3 = 1(Attribute ID) + 2(Length)
1724                 u8 attr_id = *attr_ptr;
1725                 u16 attr_data_len = RTW_GET_LE16(attr_ptr + 1);
1726                 u16 attr_len = attr_data_len + 3;
1727
1728                 //DBG_871X("%s attr_ptr:%p, id:%u, length:%u\n", __FUNCTION__, attr_ptr, attr_id, attr_data_len);
1729                 if( attr_id == target_attr_id )
1730                 {
1731                         target_attr_ptr = attr_ptr;
1732
1733                         if(buf_attr)
1734                                 _rtw_memcpy(buf_attr, attr_ptr, attr_len);
1735
1736                         if(len_attr)
1737                                 *len_attr = attr_len;
1738
1739                         break;
1740                 }
1741                 else
1742                 {
1743                         attr_ptr += attr_len; //goto next
1744                 }
1745
1746         }
1747
1748         return target_attr_ptr;
1749 }
1750
1751 /**
1752  * rtw_get_p2p_attr_content - Search a specific P2P attribute content from a given P2P IE
1753  * @p2p_ie: Address of P2P IE to search
1754  * @p2p_ielen: Length limit from p2p_ie
1755  * @target_attr_id: The attribute ID of P2P attribute to search
1756  * @buf_content: If not NULL and the P2P attribute is found, P2P attribute content will be copied to the buf starting from buf_content
1757  * @len_content: If not NULL and the P2P attribute is found, will set to the length of the P2P attribute content
1758  *
1759  * Returns: the address of the specific P2P attribute content found, or NULL
1760  */
1761 u8 *rtw_get_p2p_attr_content(u8 *p2p_ie, uint p2p_ielen, u8 target_attr_id ,u8 *buf_content, uint *len_content)
1762 {
1763         u8 *attr_ptr;
1764         u32 attr_len;
1765
1766         if(len_content)
1767                 *len_content = 0;
1768
1769         attr_ptr = rtw_get_p2p_attr(p2p_ie, p2p_ielen, target_attr_id, NULL, &attr_len);
1770
1771         if(attr_ptr && attr_len)
1772         {
1773                 if(buf_content)
1774                         _rtw_memcpy(buf_content, attr_ptr+3, attr_len-3);
1775
1776                 if(len_content)
1777                         *len_content = attr_len-3;
1778
1779                 return attr_ptr+3;
1780         }
1781
1782         return NULL;
1783 }
1784
1785 u32 rtw_set_p2p_attr_content(u8 *pbuf, u8 attr_id, u16 attr_len, u8 *pdata_attr)
1786 {
1787         u32 a_len;
1788
1789         *pbuf = attr_id;
1790
1791         //*(u16*)(pbuf + 1) = cpu_to_le16(attr_len);
1792         RTW_PUT_LE16(pbuf + 1, attr_len);
1793
1794         if(pdata_attr)
1795                 _rtw_memcpy(pbuf + 3, pdata_attr, attr_len);
1796
1797         a_len = attr_len + 3;
1798
1799         return a_len;
1800 }
1801
1802 static uint rtw_p2p_attr_remove(u8 *ie, uint ielen_ori, u8 attr_id)
1803 {
1804         u8 *target_attr;
1805         u32 target_attr_len;
1806         uint ielen = ielen_ori;
1807         int index=0;
1808
1809         while(1) {
1810                 target_attr=rtw_get_p2p_attr(ie, ielen, attr_id, NULL, &target_attr_len);
1811                 if(target_attr && target_attr_len)
1812                 {
1813                         u8 *next_attr = target_attr+target_attr_len;
1814                         uint remain_len = ielen-(next_attr-ie);
1815                         //dump_ies(RTW_DBGDUMP, ie, ielen);
1816                         #if 0
1817                         DBG_871X("[%d] ie:%p, ielen:%u\n"
1818                                 "target_attr:%p, target_attr_len:%u\n"
1819                                 "next_attr:%p, remain_len:%u\n"
1820                                 , index++
1821                                 , ie, ielen
1822                                 , target_attr, target_attr_len
1823                                 , next_attr, remain_len
1824                         );
1825                         #endif
1826
1827                         _rtw_memset(target_attr, 0, target_attr_len);
1828                         _rtw_memcpy(target_attr, next_attr, remain_len);
1829                         _rtw_memset(target_attr+remain_len, 0, target_attr_len);
1830                         *(ie+1) -= target_attr_len;
1831                         ielen-=target_attr_len;
1832                 }
1833                 else
1834                 {
1835                         //if(index>0)
1836                         //      dump_ies(RTW_DBGDUMP, ie, ielen);
1837                         break;
1838                 }
1839         }
1840
1841         return ielen;
1842 }
1843
1844 void rtw_WLAN_BSSID_EX_remove_p2p_attr(WLAN_BSSID_EX *bss_ex, u8 attr_id)
1845 {
1846         u8 *p2p_ie;
1847         uint p2p_ielen, p2p_ielen_ori;
1848         int cnt;
1849
1850         if( (p2p_ie=rtw_get_p2p_ie(bss_ex->IEs+_FIXED_IE_LENGTH_, bss_ex->IELength-_FIXED_IE_LENGTH_, NULL, &p2p_ielen_ori)) )
1851         {
1852                 if (0)
1853                 if(rtw_get_p2p_attr(p2p_ie, p2p_ielen_ori, attr_id, NULL, NULL)) {
1854                         DBG_871X("rtw_get_p2p_attr: GOT P2P_ATTR:%u!!!!!!!!\n", attr_id);
1855                         dump_ies(RTW_DBGDUMP, bss_ex->IEs+_FIXED_IE_LENGTH_, bss_ex->IELength-_FIXED_IE_LENGTH_);
1856                 }
1857
1858                 p2p_ielen=rtw_p2p_attr_remove(p2p_ie, p2p_ielen_ori, attr_id);
1859                 if(p2p_ielen != p2p_ielen_ori) {
1860
1861                         u8 *next_ie_ori = p2p_ie+p2p_ielen_ori;
1862                         u8 *next_ie = p2p_ie+p2p_ielen;
1863                         uint remain_len = bss_ex->IELength-(next_ie_ori-bss_ex->IEs);
1864
1865                         _rtw_memcpy(next_ie, next_ie_ori, remain_len);
1866                         _rtw_memset(next_ie+remain_len, 0, p2p_ielen_ori-p2p_ielen);
1867                         bss_ex->IELength -= p2p_ielen_ori-p2p_ielen;
1868
1869                         if (0) {
1870                                 DBG_871X("remove P2P_ATTR:%u!\n", attr_id);
1871                                 dump_ies(RTW_DBGDUMP, bss_ex->IEs+_FIXED_IE_LENGTH_, bss_ex->IELength-_FIXED_IE_LENGTH_);
1872                         }
1873                 }
1874         }
1875 }
1876
1877 #endif //CONFIG_P2P
1878
1879 #ifdef CONFIG_WFD
1880 void dump_wfd_ie(void *sel, u8 *ie, u32 ie_len)
1881 {
1882         u8* pos = (u8*)ie;
1883         u8 id;
1884         u16 len;
1885
1886         u8 *wfd_ie;
1887         uint wfd_ielen;
1888
1889         if(rtw_get_wfd_ie(ie, ie_len, NULL, &wfd_ielen) == _FALSE)
1890                 return;
1891
1892         pos+=6;
1893         while(pos-ie < ie_len){
1894                 id = *pos;
1895                 len = RTW_GET_BE16(pos+1);
1896
1897                 DBG_871X_SEL_NL(sel, "%s ID:%u, LEN:%u\n", __FUNCTION__, id, len);
1898
1899                 pos+=(3+len);
1900         }
1901 }
1902
1903 int rtw_get_wfd_ie(u8 *in_ie, int in_len, u8 *wfd_ie, uint *wfd_ielen)
1904 {
1905         int match;
1906         uint cnt = 0;
1907         u8 eid, wfd_oui[4]={0x50,0x6F,0x9A,0x0A};
1908
1909
1910         match=_FALSE;
1911
1912         if ( in_len < 0 )
1913         {
1914                 return match;
1915         }
1916
1917         while(cnt<in_len)
1918         {
1919                 eid = in_ie[cnt];
1920
1921                 if( ( eid == _VENDOR_SPECIFIC_IE_ ) && ( _rtw_memcmp( &in_ie[cnt+2], wfd_oui, 4) == _TRUE ) )
1922                 {
1923                         if ( wfd_ie != NULL )
1924                         {
1925                                 _rtw_memcpy( wfd_ie, &in_ie[ cnt ], in_ie[ cnt + 1 ] + 2 );
1926
1927                         }
1928                         else
1929                         {
1930                                 if ( wfd_ielen != NULL )
1931                                 {
1932                                         *wfd_ielen = 0;
1933                                 }
1934                         }
1935
1936                         if ( wfd_ielen != NULL )
1937                         {
1938                                 *wfd_ielen = in_ie[ cnt + 1 ] + 2;
1939                         }
1940
1941                         cnt += in_ie[ cnt + 1 ] + 2;
1942
1943                         match = _TRUE;
1944                         break;
1945                 }
1946                 else
1947                 {
1948                         cnt += in_ie[ cnt + 1 ] +2; //goto next
1949                 }
1950
1951         }
1952
1953         if ( match == _TRUE )
1954         {
1955                 match = cnt;
1956         }
1957
1958         return match;
1959
1960 }
1961
1962 int rtw_get_wfd_ie_from_scan_queue(u8 *in_ie, int in_len, u8 *wfd_ie, uint *wfd_ielen, u8 frame_type)
1963 {
1964         int match;
1965
1966         match=_FALSE;
1967
1968         DBG_871X( "[%s] frame_type = %d\n", __FUNCTION__, frame_type );
1969         switch( frame_type )
1970         {
1971                 case 1:
1972                 case 3:
1973                 {       //      Beacon or Probe Response
1974                         match = rtw_get_wfd_ie(in_ie + _PROBERSP_IE_OFFSET_, in_len - _PROBERSP_IE_OFFSET_, wfd_ie, wfd_ielen);
1975                         break;
1976                 }
1977                 case 2:
1978                 {       //      Probe Request
1979                         match = rtw_get_wfd_ie(in_ie + _PROBEREQ_IE_OFFSET_ , in_len - _PROBEREQ_IE_OFFSET_ , wfd_ie, wfd_ielen);
1980                         break;
1981                 }
1982         }
1983         return match;
1984 }
1985
1986 //      attr_content: The output buffer, contains the "body field" of WFD attribute.
1987 //      attr_contentlen: The data length of the "body field" of WFD attribute.
1988 int rtw_get_wfd_attr_content(u8 *wfd_ie, uint wfd_ielen, u8 target_attr_id ,u8 *attr_content, uint *attr_contentlen)
1989 {
1990         int match;
1991         uint cnt = 0;
1992         u8 attr_id, wfd_oui[4]={0x50,0x6F,0x9A,0x0A};
1993
1994
1995         match=_FALSE;
1996
1997         if ( ( wfd_ie[ 0 ] != _VENDOR_SPECIFIC_IE_ ) ||
1998                 ( _rtw_memcmp( wfd_ie + 2, wfd_oui , 4 ) != _TRUE ) )
1999         {
2000                 return( match );
2001         }
2002
2003         //      1 ( WFD IE ) + 1 ( Length ) + 3 ( OUI ) + 1 ( OUI Type )
2004         cnt = 6;
2005         while( cnt < wfd_ielen )
2006         {
2007                 u16 attrlen = RTW_GET_BE16(wfd_ie + cnt + 1);
2008
2009                 attr_id = wfd_ie[cnt];
2010                 if( attr_id == target_attr_id )
2011                 {
2012                         //      3 -> 1 byte for attribute ID field, 2 bytes for length field
2013                         if(attr_content)
2014                                 _rtw_memcpy( attr_content, &wfd_ie[ cnt + 3 ], attrlen );
2015
2016                         if(attr_contentlen)
2017                                 *attr_contentlen = attrlen;
2018
2019                         cnt += attrlen + 3;
2020
2021                         match = _TRUE;
2022                         break;
2023                 }
2024                 else
2025                 {
2026                         cnt += attrlen + 3; //goto next
2027                 }
2028
2029         }
2030
2031         return match;
2032
2033 }
2034 #endif // CONFIG_WFD
2035
2036 //Baron adds to avoid FreeBSD warning
2037 int ieee80211_is_empty_essid(const char *essid, int essid_len)
2038 {
2039         /* Single white space is for Linksys APs */
2040         if (essid_len == 1 && essid[0] == ' ')
2041                 return 1;
2042
2043         /* Otherwise, if the entire essid is 0, we assume it is hidden */
2044         while (essid_len) {
2045                 essid_len--;
2046                 if (essid[essid_len] != '\0')
2047                         return 0;
2048         }
2049
2050         return 1;
2051 }
2052
2053 int ieee80211_get_hdrlen(u16 fc)
2054 {
2055         int hdrlen = 24;
2056
2057         switch (WLAN_FC_GET_TYPE(fc)) {
2058         case RTW_IEEE80211_FTYPE_DATA:
2059                 if (fc & RTW_IEEE80211_STYPE_QOS_DATA)
2060                         hdrlen += 2;
2061                 if ((fc & RTW_IEEE80211_FCTL_FROMDS) && (fc & RTW_IEEE80211_FCTL_TODS))
2062                         hdrlen += 6; /* Addr4 */
2063                 break;
2064         case RTW_IEEE80211_FTYPE_CTL:
2065                 switch (WLAN_FC_GET_STYPE(fc)) {
2066                 case RTW_IEEE80211_STYPE_CTS:
2067                 case RTW_IEEE80211_STYPE_ACK:
2068                         hdrlen = 10;
2069                         break;
2070                 default:
2071                         hdrlen = 16;
2072                         break;
2073                 }
2074                 break;
2075         }
2076
2077         return hdrlen;
2078 }
2079
2080 int rtw_get_cipher_info(struct wlan_network *pnetwork)
2081 {
2082         u32 wpa_ielen;
2083         unsigned char *pbuf;
2084         int group_cipher = 0, pairwise_cipher = 0, is8021x = 0;
2085         int ret = _FAIL;
2086         pbuf = rtw_get_wpa_ie(&pnetwork->network.IEs[12], &wpa_ielen, pnetwork->network.IELength-12);
2087
2088         if(pbuf && (wpa_ielen>0)) {
2089                 RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("rtw_get_cipher_info: wpa_ielen: %d", wpa_ielen));
2090                 if (_SUCCESS == rtw_parse_wpa_ie(pbuf, wpa_ielen+2, &group_cipher, &pairwise_cipher, &is8021x)) {
2091
2092                         pnetwork->BcnInfo.pairwise_cipher = pairwise_cipher;
2093                         pnetwork->BcnInfo.group_cipher = group_cipher;
2094                         pnetwork->BcnInfo.is_8021x = is8021x;
2095                         RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("%s: pnetwork->pairwise_cipher: %d, is_8021x is %d",
2096                                                 __func__, pnetwork->BcnInfo.pairwise_cipher, pnetwork->BcnInfo.is_8021x));
2097                         ret = _SUCCESS;
2098                 }
2099         } else {
2100
2101                 pbuf = rtw_get_wpa2_ie(&pnetwork->network.IEs[12], &wpa_ielen, pnetwork->network.IELength-12);
2102
2103                 if(pbuf && (wpa_ielen>0)) {
2104                         RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("get RSN IE\n"));
2105                         if (_SUCCESS == rtw_parse_wpa2_ie(pbuf, wpa_ielen+2, &group_cipher, &pairwise_cipher, &is8021x)) {
2106                                 RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("get RSN IE  OK!!!\n"));
2107                                 pnetwork->BcnInfo.pairwise_cipher = pairwise_cipher;
2108                                 pnetwork->BcnInfo.group_cipher = group_cipher;
2109                                 pnetwork->BcnInfo.is_8021x = is8021x;
2110                                 RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("%s: pnetwork->pairwise_cipher: %d,"
2111                                                         "pnetwork->group_cipher is %d, is_8021x is %d", __func__, pnetwork->BcnInfo.pairwise_cipher,
2112                                                         pnetwork->BcnInfo.group_cipher,pnetwork->BcnInfo.is_8021x));
2113                                 ret = _SUCCESS;
2114                         }
2115                 }
2116         }
2117
2118         return ret;
2119 }
2120
2121 void rtw_get_bcn_info(struct wlan_network *pnetwork)
2122 {
2123         unsigned short cap = 0;
2124         u8 bencrypt = 0;
2125         //u8 wpa_ie[255],rsn_ie[255];
2126         u16 wpa_len=0,rsn_len=0;
2127         struct HT_info_element *pht_info = NULL;
2128         struct rtw_ieee80211_ht_cap *pht_cap = NULL;
2129         unsigned int            len;
2130         unsigned char           *p;
2131
2132         _rtw_memcpy((u8 *)&cap, rtw_get_capability_from_ie(pnetwork->network.IEs), 2);
2133         cap = le16_to_cpu(cap);
2134         if (cap & WLAN_CAPABILITY_PRIVACY) {
2135                 bencrypt = 1;
2136                 pnetwork->network.Privacy = 1;
2137         } else {
2138                 pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_OPENSYS;
2139         }
2140         rtw_get_sec_ie(pnetwork->network.IEs ,pnetwork->network.IELength,NULL,&rsn_len,NULL,&wpa_len);
2141         RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("rtw_get_bcn_info: ssid=%s\n",pnetwork->network.Ssid.Ssid));
2142         RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("rtw_get_bcn_info: wpa_len=%d rsn_len=%d\n",wpa_len,rsn_len));
2143         RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("rtw_get_bcn_info: ssid=%s\n",pnetwork->network.Ssid.Ssid));
2144         RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("rtw_get_bcn_info: wpa_len=%d rsn_len=%d\n",wpa_len,rsn_len));
2145
2146         if (rsn_len > 0) {
2147                 pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_WPA2;
2148         } else if (wpa_len > 0) {
2149                 pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_WPA;
2150         } else {
2151                 if (bencrypt)
2152                         pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_WEP;
2153         }
2154         RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("rtw_get_bcn_info: pnetwork->encryp_protocol is %x\n",
2155                                 pnetwork->BcnInfo.encryp_protocol));
2156         RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("rtw_get_bcn_info: pnetwork->encryp_protocol is %x\n",
2157                                 pnetwork->BcnInfo.encryp_protocol));
2158         rtw_get_cipher_info(pnetwork);
2159
2160         /* get bwmode and ch_offset */
2161         /* parsing HT_CAP_IE */
2162         p = rtw_get_ie(pnetwork->network.IEs + _FIXED_IE_LENGTH_, _HT_CAPABILITY_IE_, &len, pnetwork->network.IELength - _FIXED_IE_LENGTH_);
2163         if(p && len>0) {
2164                         pht_cap = (struct rtw_ieee80211_ht_cap *)(p + 2);
2165                         pnetwork->BcnInfo.ht_cap_info = pht_cap->cap_info;
2166         } else {
2167                         pnetwork->BcnInfo.ht_cap_info = 0;
2168         }
2169         /* parsing HT_INFO_IE */
2170         p = rtw_get_ie(pnetwork->network.IEs + _FIXED_IE_LENGTH_, _HT_ADD_INFO_IE_, &len, pnetwork->network.IELength - _FIXED_IE_LENGTH_);
2171         if(p && len>0) {
2172                         pht_info = (struct HT_info_element *)(p + 2);
2173                         pnetwork->BcnInfo.ht_info_infos_0 = pht_info->infos[0];
2174         } else {
2175                         pnetwork->BcnInfo.ht_info_infos_0 = 0;
2176         }
2177 }
2178
2179 //show MCS rate, unit: 100Kbps
2180 u16 rtw_mcs_rate(u8 rf_type, u8 bw_40MHz, u8 short_GI, unsigned char * MCS_rate)
2181 {
2182         u16 max_rate = 0;
2183
2184         if(rf_type == RF_1T1R)
2185         {
2186                 if(MCS_rate[0] & BIT(7))
2187                         max_rate = (bw_40MHz) ? ((short_GI)?1500:1350):((short_GI)?722:650);
2188                 else if(MCS_rate[0] & BIT(6))
2189                         max_rate = (bw_40MHz) ? ((short_GI)?1350:1215):((short_GI)?650:585);
2190                 else if(MCS_rate[0] & BIT(5))
2191                         max_rate = (bw_40MHz) ? ((short_GI)?1200:1080):((short_GI)?578:520);
2192                 else if(MCS_rate[0] & BIT(4))
2193                         max_rate = (bw_40MHz) ? ((short_GI)?900:810):((short_GI)?433:390);
2194                 else if(MCS_rate[0] & BIT(3))
2195                         max_rate = (bw_40MHz) ? ((short_GI)?600:540):((short_GI)?289:260);
2196                 else if(MCS_rate[0] & BIT(2))
2197                         max_rate = (bw_40MHz) ? ((short_GI)?450:405):((short_GI)?217:195);
2198                 else if(MCS_rate[0] & BIT(1))
2199                         max_rate = (bw_40MHz) ? ((short_GI)?300:270):((short_GI)?144:130);
2200                 else if(MCS_rate[0] & BIT(0))
2201                         max_rate = (bw_40MHz) ? ((short_GI)?150:135):((short_GI)?72:65);
2202         }
2203         else
2204         {
2205                 if(MCS_rate[1])
2206                 {
2207                         if(MCS_rate[1] & BIT(7))
2208                                 max_rate = (bw_40MHz) ? ((short_GI)?3000:2700):((short_GI)?1444:1300);
2209                         else if(MCS_rate[1] & BIT(6))
2210                                 max_rate = (bw_40MHz) ? ((short_GI)?2700:2430):((short_GI)?1300:1170);
2211                         else if(MCS_rate[1] & BIT(5))
2212                                 max_rate = (bw_40MHz) ? ((short_GI)?2400:2160):((short_GI)?1156:1040);
2213                         else if(MCS_rate[1] & BIT(4))
2214                                 max_rate = (bw_40MHz) ? ((short_GI)?1800:1620):((short_GI)?867:780);
2215                         else if(MCS_rate[1] & BIT(3))
2216                                 max_rate = (bw_40MHz) ? ((short_GI)?1200:1080):((short_GI)?578:520);
2217                         else if(MCS_rate[1] & BIT(2))
2218                                 max_rate = (bw_40MHz) ? ((short_GI)?900:810):((short_GI)?433:390);
2219                         else if(MCS_rate[1] & BIT(1))
2220                                 max_rate = (bw_40MHz) ? ((short_GI)?600:540):((short_GI)?289:260);
2221                         else if(MCS_rate[1] & BIT(0))
2222                                 max_rate = (bw_40MHz) ? ((short_GI)?300:270):((short_GI)?144:130);
2223                 }
2224                 else
2225                 {
2226                         if(MCS_rate[0] & BIT(7))
2227                                 max_rate = (bw_40MHz) ? ((short_GI)?1500:1350):((short_GI)?722:650);
2228                         else if(MCS_rate[0] & BIT(6))
2229                                 max_rate = (bw_40MHz) ? ((short_GI)?1350:1215):((short_GI)?650:585);
2230                         else if(MCS_rate[0] & BIT(5))
2231                                 max_rate = (bw_40MHz) ? ((short_GI)?1200:1080):((short_GI)?578:520);
2232                         else if(MCS_rate[0] & BIT(4))
2233                                 max_rate = (bw_40MHz) ? ((short_GI)?900:810):((short_GI)?433:390);
2234                         else if(MCS_rate[0] & BIT(3))
2235                                 max_rate = (bw_40MHz) ? ((short_GI)?600:540):((short_GI)?289:260);
2236                         else if(MCS_rate[0] & BIT(2))
2237                                 max_rate = (bw_40MHz) ? ((short_GI)?450:405):((short_GI)?217:195);
2238                         else if(MCS_rate[0] & BIT(1))
2239                                 max_rate = (bw_40MHz) ? ((short_GI)?300:270):((short_GI)?144:130);
2240                         else if(MCS_rate[0] & BIT(0))
2241                                 max_rate = (bw_40MHz) ? ((short_GI)?150:135):((short_GI)?72:65);
2242                 }
2243         }
2244         return max_rate;
2245 }
2246
2247 int rtw_action_frame_parse(const u8 *frame, u32 frame_len, u8* category, u8 *action)
2248 {
2249         const u8 *frame_body = frame + sizeof(struct rtw_ieee80211_hdr_3addr);
2250         u16 fc;
2251         u8 c;
2252         u8 a = ACT_PUBLIC_MAX;
2253
2254         fc = le16_to_cpu(((struct rtw_ieee80211_hdr_3addr *)frame)->frame_ctl);
2255
2256         if ((fc & (RTW_IEEE80211_FCTL_FTYPE|RTW_IEEE80211_FCTL_STYPE))
2257                 != (RTW_IEEE80211_FTYPE_MGMT|RTW_IEEE80211_STYPE_ACTION)
2258         )
2259         {
2260                 return _FALSE;
2261         }
2262
2263         c = frame_body[0];
2264
2265         switch(c) {
2266         case RTW_WLAN_CATEGORY_P2P: /* vendor-specific */
2267                 break;
2268         default:
2269                 a = frame_body[1];
2270         }
2271
2272         if (category)
2273                 *category = c;
2274         if (action)
2275                 *action = a;
2276
2277         return _TRUE;
2278 }
2279
2280 static const char *_action_public_str[] = {
2281         "ACT_PUB_BSSCOEXIST",
2282         "ACT_PUB_DSE_ENABLE",
2283         "ACT_PUB_DSE_DEENABLE",
2284         "ACT_PUB_DSE_REG_LOCATION",
2285         "ACT_PUB_EXT_CHL_SWITCH",
2286         "ACT_PUB_DSE_MSR_REQ",
2287         "ACT_PUB_DSE_MSR_RPRT",
2288         "ACT_PUB_MP",
2289         "ACT_PUB_DSE_PWR_CONSTRAINT",
2290         "ACT_PUB_VENDOR",
2291         "ACT_PUB_GAS_INITIAL_REQ",
2292         "ACT_PUB_GAS_INITIAL_RSP",
2293         "ACT_PUB_GAS_COMEBACK_REQ",
2294         "ACT_PUB_GAS_COMEBACK_RSP",
2295         "ACT_PUB_TDLS_DISCOVERY_RSP",
2296         "ACT_PUB_LOCATION_TRACK",
2297         "ACT_PUB_RSVD",
2298 };
2299
2300 const char *action_public_str(u8 action)
2301 {
2302         action = (action >= ACT_PUBLIC_MAX) ? ACT_PUBLIC_MAX : action;
2303         return _action_public_str[action];
2304 }
2305