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