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