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