drivers/net/wireless/rockchip_wlan/rtl8723cs/core/rtw_ieee80211.c

   1 /******************************************************************************
   2  *
   3  * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
   4  *
   5  * This program is free software; you can redistribute it and/or modify it
   6  * under the terms of version 2 of the GNU General Public License as
   7  * published by the Free Software Foundation.
   8  *
   9  * This program is distributed in the hope that it will be useful, but WITHOUT
  10  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  11  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
  12  * more details.
  13  *
  14  * You should have received a copy of the GNU General Public License along with
  15  * this program; if not, write to the Free Software Foundation, Inc.,
  16  * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
  17  *
  18  *
  19  ******************************************************************************/
  20 #define _IEEE80211_C
  21
  22 #ifdef CONFIG_PLATFORM_INTEL_BYT
  23 #include <linux/fs.h>
  24 #endif
  25 #include <drv_types.h>
  26 #include <linux/rfkill-wlan.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
  60 static u8       WIFI_OFDMRATES[] = {
  61         (IEEE80211_OFDM_RATE_6MB),
  62         (IEEE80211_OFDM_RATE_9MB),
  63         (IEEE80211_OFDM_RATE_12MB),
  64         (IEEE80211_OFDM_RATE_18MB),
  65         (IEEE80211_OFDM_RATE_24MB),
  66         IEEE80211_OFDM_RATE_36MB,
  67         IEEE80211_OFDM_RATE_48MB,
  68         IEEE80211_OFDM_RATE_54MB
  69 };
  70
  71 u8 mgn_rates_cck[4] = {MGN_1M, MGN_2M, MGN_5_5M, MGN_11M};
  72 u8 mgn_rates_ofdm[8] = {MGN_6M, MGN_9M, MGN_12M, MGN_18M, MGN_24M, MGN_36M, MGN_48M, MGN_54M};
  73 u8 mgn_rates_mcs0_7[8] = {MGN_MCS0, MGN_MCS1, MGN_MCS2, MGN_MCS3, MGN_MCS4, MGN_MCS5, MGN_MCS6, MGN_MCS7};
  74 u8 mgn_rates_mcs8_15[8] = {MGN_MCS8, MGN_MCS9, MGN_MCS10, MGN_MCS11, MGN_MCS12, MGN_MCS13, MGN_MCS14, MGN_MCS15};
  75 u8 mgn_rates_mcs16_23[8] = {MGN_MCS16, MGN_MCS17, MGN_MCS18, MGN_MCS19, MGN_MCS20, MGN_MCS21, MGN_MCS22, MGN_MCS23};
  76 u8 mgn_rates_mcs24_31[8] = {MGN_MCS24, MGN_MCS25, MGN_MCS26, MGN_MCS27, MGN_MCS28, MGN_MCS29, MGN_MCS30, MGN_MCS31};
  77 u8 mgn_rates_vht1ss[10] = {MGN_VHT1SS_MCS0, MGN_VHT1SS_MCS1, MGN_VHT1SS_MCS2, MGN_VHT1SS_MCS3, MGN_VHT1SS_MCS4
  78         , MGN_VHT1SS_MCS5, MGN_VHT1SS_MCS6, MGN_VHT1SS_MCS7, MGN_VHT1SS_MCS8, MGN_VHT1SS_MCS9
  79                           };
  80 u8 mgn_rates_vht2ss[10] = {MGN_VHT2SS_MCS0, MGN_VHT2SS_MCS1, MGN_VHT2SS_MCS2, MGN_VHT2SS_MCS3, MGN_VHT2SS_MCS4
  81         , MGN_VHT2SS_MCS5, MGN_VHT2SS_MCS6, MGN_VHT2SS_MCS7, MGN_VHT2SS_MCS8, MGN_VHT2SS_MCS9
  82                           };
  83 u8 mgn_rates_vht3ss[10] = {MGN_VHT3SS_MCS0, MGN_VHT3SS_MCS1, MGN_VHT3SS_MCS2, MGN_VHT3SS_MCS3, MGN_VHT3SS_MCS4
  84         , MGN_VHT3SS_MCS5, MGN_VHT3SS_MCS6, MGN_VHT3SS_MCS7, MGN_VHT3SS_MCS8, MGN_VHT3SS_MCS9
  85                           };
  86 u8 mgn_rates_vht4ss[10] = {MGN_VHT4SS_MCS0, MGN_VHT4SS_MCS1, MGN_VHT4SS_MCS2, MGN_VHT4SS_MCS3, MGN_VHT4SS_MCS4
  87         , MGN_VHT4SS_MCS5, MGN_VHT4SS_MCS6, MGN_VHT4SS_MCS7, MGN_VHT4SS_MCS8, MGN_VHT4SS_MCS9
  88                           };
  89
  90 static const char *const _rate_section_str[] = {
  91         "CCK",
  92         "OFDM",
  93         "HT_1SS",
  94         "HT_2SS",
  95         "HT_3SS",
  96         "HT_4SS",
  97         "VHT_1SS",
  98         "VHT_2SS",
  99         "VHT_3SS",
 100         "VHT_4SS",
 101         "RATE_SECTION_UNKNOWN",
 102 };
 103
 104 const char *rate_section_str(u8 section)
 105 {
 106         section = (section >= RATE_SECTION_NUM) ? RATE_SECTION_NUM : section;
 107         return _rate_section_str[section];
 108 }
 109
 110 struct rate_section_ent rates_by_sections[RATE_SECTION_NUM] = {
 111         {RF_1TX, 4, mgn_rates_cck},
 112         {RF_1TX, 8, mgn_rates_ofdm},
 113         {RF_1TX, 8, mgn_rates_mcs0_7},
 114         {RF_2TX, 8, mgn_rates_mcs8_15},
 115         {RF_3TX, 8, mgn_rates_mcs16_23},
 116         {RF_4TX, 8, mgn_rates_mcs24_31},
 117         {RF_1TX, 10, mgn_rates_vht1ss},
 118         {RF_2TX, 10, mgn_rates_vht2ss},
 119         {RF_3TX, 10, mgn_rates_vht3ss},
 120         {RF_4TX, 10, mgn_rates_vht4ss},
 121 };
 122
 123 int rtw_get_bit_value_from_ieee_value(u8 val)
 124 {
 125         unsigned char dot11_rate_table[] = {2, 4, 11, 22, 12, 18, 24, 36, 48, 72, 96, 108, 0}; /* last element must be zero!! */
 126
 127         int i = 0;
 128         while (dot11_rate_table[i] != 0) {
 129                 if (dot11_rate_table[i] == val)
 130                         return BIT(i);
 131                 i++;
 132         }
 133         return 0;
 134 }
 135
 136 uint    rtw_is_cckrates_included(u8 *rate)
 137 {
 138         u32     i = 0;
 139
 140         while (rate[i] != 0) {
 141                 if ((((rate[i]) & 0x7f) == 2)   || (((rate[i]) & 0x7f) == 4) ||
 142                     (((rate[i]) & 0x7f) == 11)  || (((rate[i]) & 0x7f) == 22))
 143                         return _TRUE;
 144                 i++;
 145         }
 146
 147         return _FALSE;
 148 }
 149
 150 uint    rtw_is_cckratesonly_included(u8 *rate)
 151 {
 152         u32 i = 0;
 153
 154
 155         while (rate[i] != 0) {
 156                 if ((((rate[i]) & 0x7f) != 2) && (((rate[i]) & 0x7f) != 4) &&
 157                     (((rate[i]) & 0x7f) != 11)  && (((rate[i]) & 0x7f) != 22))
 158                         return _FALSE;
 159
 160                 i++;
 161         }
 162
 163         return _TRUE;
 164
 165 }
 166
 167 int rtw_check_network_type(unsigned char *rate, int ratelen, int channel)
 168 {
 169         if (channel > 14) {
 170                 if ((rtw_is_cckrates_included(rate)) == _TRUE)
 171                         return WIRELESS_INVALID;
 172                 else
 173                         return WIRELESS_11A;
 174         } else { /* could be pure B, pure G, or B/G */
 175                 if ((rtw_is_cckratesonly_included(rate)) == _TRUE)
 176                         return WIRELESS_11B;
 177                 else if ((rtw_is_cckrates_included(rate)) == _TRUE)
 178                         return  WIRELESS_11BG;
 179                 else
 180                         return WIRELESS_11G;
 181         }
 182
 183 }
 184
 185 u8 *rtw_set_fixed_ie(unsigned char *pbuf, unsigned int len, unsigned char *source,
 186                      unsigned int *frlen)
 187 {
 188         _rtw_memcpy((void *)pbuf, (void *)source, len);
 189         *frlen = *frlen + len;
 190         return pbuf + len;
 191 }
 192
 193 /* rtw_set_ie will update frame length */
 194 u8 *rtw_set_ie
 195 (
 196         u8 *pbuf,
 197         sint index,
 198         uint len,
 199         u8 *source,
 200         uint *frlen /* frame length */
 201 )
 202 {
 203         *pbuf = (u8)index;
 204
 205         *(pbuf + 1) = (u8)len;
 206
 207         if (len > 0)
 208                 _rtw_memcpy((void *)(pbuf + 2), (void *)source, len);
 209
 210         *frlen = *frlen + (len + 2);
 211
 212         return pbuf + len + 2;
 213 }
 214
 215 inline u8 *rtw_set_ie_ch_switch(u8 *buf, u32 *buf_len, u8 ch_switch_mode,
 216                                 u8 new_ch, u8 ch_switch_cnt)
 217 {
 218         u8 ie_data[3];
 219
 220         ie_data[0] = ch_switch_mode;
 221         ie_data[1] = new_ch;
 222         ie_data[2] = ch_switch_cnt;
 223         return rtw_set_ie(buf, WLAN_EID_CHANNEL_SWITCH,  3, ie_data, buf_len);
 224 }
 225
 226 inline u8 secondary_ch_offset_to_hal_ch_offset(u8 ch_offset)
 227 {
 228         if (ch_offset == SCN)
 229                 return HAL_PRIME_CHNL_OFFSET_DONT_CARE;
 230         else if (ch_offset == SCA)
 231                 return HAL_PRIME_CHNL_OFFSET_UPPER;
 232         else if (ch_offset == SCB)
 233                 return HAL_PRIME_CHNL_OFFSET_LOWER;
 234
 235         return HAL_PRIME_CHNL_OFFSET_DONT_CARE;
 236 }
 237
 238 inline u8 hal_ch_offset_to_secondary_ch_offset(u8 ch_offset)
 239 {
 240         if (ch_offset == HAL_PRIME_CHNL_OFFSET_DONT_CARE)
 241                 return SCN;
 242         else if (ch_offset == HAL_PRIME_CHNL_OFFSET_LOWER)
 243                 return SCB;
 244         else if (ch_offset == HAL_PRIME_CHNL_OFFSET_UPPER)
 245                 return SCA;
 246
 247         return SCN;
 248 }
 249
 250 inline u8 *rtw_set_ie_secondary_ch_offset(u8 *buf, u32 *buf_len, u8 secondary_ch_offset)
 251 {
 252         return rtw_set_ie(buf, WLAN_EID_SECONDARY_CHANNEL_OFFSET,  1, &secondary_ch_offset, buf_len);
 253 }
 254
 255 inline u8 *rtw_set_ie_mesh_ch_switch_parm(u8 *buf, u32 *buf_len, u8 ttl,
 256                 u8 flags, u16 reason, u16 precedence)
 257 {
 258         u8 ie_data[6];
 259
 260         ie_data[0] = ttl;
 261         ie_data[1] = flags;
 262         RTW_PUT_LE16((u8 *)&ie_data[2], reason);
 263         RTW_PUT_LE16((u8 *)&ie_data[4], precedence);
 264
 265         return rtw_set_ie(buf, 0x118,  6, ie_data, buf_len);
 266 }
 267
 268 /*----------------------------------------------------------------------------
 269 index: the information element id index, limit is the limit for search
 270 -----------------------------------------------------------------------------*/
 271 u8 *rtw_get_ie(u8 *pbuf, sint index, sint *len, sint limit)
 272 {
 273         sint tmp, i;
 274         u8 *p;
 275         if (limit < 1) {
 276                 return NULL;
 277         }
 278
 279         p = pbuf;
 280         i = 0;
 281         *len = 0;
 282         while (1) {
 283                 if (*p == index) {
 284                         *len = *(p + 1);
 285                         return p;
 286                 } else {
 287                         tmp = *(p + 1);
 288                         p += (tmp + 2);
 289                         i += (tmp + 2);
 290                 }
 291                 if (i >= limit)
 292                         break;
 293         }
 294         return NULL;
 295 }
 296
 297 /**
 298  * rtw_get_ie_ex - Search specific IE from a series of IEs
 299  * @in_ie: Address of IEs to search
 300  * @in_len: Length limit from in_ie
 301  * @eid: Element ID to match
 302  * @oui: OUI to match
 303  * @oui_len: OUI length
 304  * @ie: If not NULL and the specific IE is found, the IE will be copied to the buf starting from the specific IE
 305  * @ielen: If not NULL and the specific IE is found, will set to the length of the entire IE
 306  *
 307  * Returns: The address of the specific IE found, or NULL
 308  */
 309 u8 *rtw_get_ie_ex(u8 *in_ie, uint in_len, u8 eid, u8 *oui, u8 oui_len, u8 *ie, uint *ielen)
 310 {
 311         uint cnt;
 312         u8 *target_ie = NULL;
 313
 314
 315         if (ielen)
 316                 *ielen = 0;
 317
 318         if (!in_ie || in_len <= 0)
 319                 return target_ie;
 320
 321         cnt = 0;
 322
 323         while (cnt < in_len) {
 324                 if (eid == in_ie[cnt]
 325                     && (!oui || _rtw_memcmp(&in_ie[cnt + 2], oui, oui_len) == _TRUE)) {
 326                         target_ie = &in_ie[cnt];
 327
 328                         if (ie)
 329                                 _rtw_memcpy(ie, &in_ie[cnt], in_ie[cnt + 1] + 2);
 330
 331                         if (ielen)
 332                                 *ielen = in_ie[cnt + 1] + 2;
 333
 334                         break;
 335                 } else {
 336                         cnt += in_ie[cnt + 1] + 2; /* goto next  */
 337                 }
 338
 339         }
 340
 341         return target_ie;
 342 }
 343
 344 /**
 345  * rtw_ies_remove_ie - Find matching IEs and remove
 346  * @ies: Address of IEs to search
 347  * @ies_len: Pointer of length of ies, will update to new length
 348  * @offset: The offset to start scarch
 349  * @eid: Element ID to match
 350  * @oui: OUI to match
 351  * @oui_len: OUI length
 352  *
 353  * Returns: _SUCCESS: ies is updated, _FAIL: not updated
 354  */
 355 int rtw_ies_remove_ie(u8 *ies, uint *ies_len, uint offset, u8 eid, u8 *oui, u8 oui_len)
 356 {
 357         int ret = _FAIL;
 358         u8 *target_ie;
 359         u32 target_ielen;
 360         u8 *start;
 361         uint search_len;
 362
 363         if (!ies || !ies_len || *ies_len <= offset)
 364                 goto exit;
 365
 366         start = ies + offset;
 367         search_len = *ies_len - offset;
 368
 369         while (1) {
 370                 target_ie = rtw_get_ie_ex(start, search_len, eid, oui, oui_len, NULL, &target_ielen);
 371                 if (target_ie && target_ielen) {
 372                         u8 *remain_ies = target_ie + target_ielen;
 373                         uint remain_len = search_len - (remain_ies - start);
 374
 375                         _rtw_memmove(target_ie, remain_ies, remain_len);
 376                         *ies_len = *ies_len - target_ielen;
 377                         ret = _SUCCESS;
 378
 379                         start = target_ie;
 380                         search_len = remain_len;
 381                 } else
 382                         break;
 383         }
 384 exit:
 385         return ret;
 386 }
 387
 388 void rtw_set_supported_rate(u8 *SupportedRates, uint mode)
 389 {
 390
 391         _rtw_memset(SupportedRates, 0, NDIS_802_11_LENGTH_RATES_EX);
 392
 393         switch (mode) {
 394         case WIRELESS_11B:
 395                 _rtw_memcpy(SupportedRates, WIFI_CCKRATES, IEEE80211_CCK_RATE_LEN);
 396                 break;
 397
 398         case WIRELESS_11G:
 399         case WIRELESS_11A:
 400         case WIRELESS_11_5N:
 401         case WIRELESS_11A_5N: /* Todo: no basic rate for ofdm ? */
 402         case WIRELESS_11_5AC:
 403                 _rtw_memcpy(SupportedRates, WIFI_OFDMRATES, IEEE80211_NUM_OFDM_RATESLEN);
 404                 break;
 405
 406         case WIRELESS_11BG:
 407         case WIRELESS_11G_24N:
 408         case WIRELESS_11_24N:
 409         case WIRELESS_11BG_24N:
 410                 _rtw_memcpy(SupportedRates, WIFI_CCKRATES, IEEE80211_CCK_RATE_LEN);
 411                 _rtw_memcpy(SupportedRates + IEEE80211_CCK_RATE_LEN, WIFI_OFDMRATES, IEEE80211_NUM_OFDM_RATESLEN);
 412                 break;
 413
 414         }
 415 }
 416
 417 uint    rtw_get_rateset_len(u8  *rateset)
 418 {
 419         uint i = 0;
 420         while (1) {
 421                 if ((rateset[i]) == 0)
 422                         break;
 423
 424                 if (i > 12)
 425                         break;
 426
 427                 i++;
 428         }
 429         return i;
 430 }
 431
 432 int rtw_generate_ie(struct registry_priv *pregistrypriv)
 433 {
 434         u8      wireless_mode;
 435         int     sz = 0, rateLen;
 436         WLAN_BSSID_EX   *pdev_network = &pregistrypriv->dev_network;
 437         u8      *ie = pdev_network->IEs;
 438
 439
 440         /* timestamp will be inserted by hardware */
 441         sz += 8;
 442         ie += sz;
 443
 444         /* beacon interval : 2bytes */
 445         *(u16 *)ie = cpu_to_le16((u16)pdev_network->Configuration.BeaconPeriod); /* BCN_INTERVAL; */
 446         sz += 2;
 447         ie += 2;
 448
 449         /* capability info */
 450         *(u16 *)ie = 0;
 451
 452         *(u16 *)ie |= cpu_to_le16(cap_IBSS);
 453
 454         if (pregistrypriv->preamble == PREAMBLE_SHORT)
 455                 *(u16 *)ie |= cpu_to_le16(cap_ShortPremble);
 456
 457         if (pdev_network->Privacy)
 458                 *(u16 *)ie |= cpu_to_le16(cap_Privacy);
 459
 460         sz += 2;
 461         ie += 2;
 462
 463         /* SSID */
 464         ie = rtw_set_ie(ie, _SSID_IE_, pdev_network->Ssid.SsidLength, pdev_network->Ssid.Ssid, &sz);
 465
 466         /* supported rates */
 467         if (pregistrypriv->wireless_mode == WIRELESS_11ABGN) {
 468                 if (pdev_network->Configuration.DSConfig > 14)
 469                         wireless_mode = WIRELESS_11A_5N;
 470                 else
 471                         wireless_mode = WIRELESS_11BG_24N;
 472         } else if (pregistrypriv->wireless_mode == WIRELESS_MODE_MAX) { /* WIRELESS_11ABGN | WIRELESS_11AC */
 473                 if (pdev_network->Configuration.DSConfig > 14)
 474                         wireless_mode = WIRELESS_11_5AC;
 475                 else
 476                         wireless_mode = WIRELESS_11BG_24N;
 477         } else
 478                 wireless_mode = pregistrypriv->wireless_mode;
 479
 480         rtw_set_supported_rate(pdev_network->SupportedRates, wireless_mode) ;
 481
 482         rateLen = rtw_get_rateset_len(pdev_network->SupportedRates);
 483
 484         if (rateLen > 8) {
 485                 ie = rtw_set_ie(ie, _SUPPORTEDRATES_IE_, 8, pdev_network->SupportedRates, &sz);
 486                 /* ie = rtw_set_ie(ie, _EXT_SUPPORTEDRATES_IE_, (rateLen - 8), (pdev_network->SupportedRates + 8), &sz); */
 487         } else
 488                 ie = rtw_set_ie(ie, _SUPPORTEDRATES_IE_, rateLen, pdev_network->SupportedRates, &sz);
 489
 490         /* DS parameter set */
 491         ie = rtw_set_ie(ie, _DSSET_IE_, 1, (u8 *)&(pdev_network->Configuration.DSConfig), &sz);
 492
 493
 494         /* IBSS Parameter Set */
 495
 496         ie = rtw_set_ie(ie, _IBSS_PARA_IE_, 2, (u8 *)&(pdev_network->Configuration.ATIMWindow), &sz);
 497
 498         if (rateLen > 8)
 499                 ie = rtw_set_ie(ie, _EXT_SUPPORTEDRATES_IE_, (rateLen - 8), (pdev_network->SupportedRates + 8), &sz);
 500
 501 #ifdef CONFIG_80211N_HT
 502         /* HT Cap. */
 503         if (((pregistrypriv->wireless_mode & WIRELESS_11_5N) || (pregistrypriv->wireless_mode & WIRELESS_11_24N))
 504             && (pregistrypriv->ht_enable == _TRUE)) {
 505                 /* todo: */
 506         }
 507 #endif /* CONFIG_80211N_HT */
 508
 509         /* pdev_network->IELength =  sz; */ /* update IELength */
 510
 511
 512         /* return _SUCCESS; */
 513
 514         return sz;
 515
 516 }
 517
 518 unsigned char *rtw_get_wpa_ie(unsigned char *pie, int *wpa_ie_len, int limit)
 519 {
 520         int len;
 521         u16 val16;
 522         unsigned char wpa_oui_type[] = {0x00, 0x50, 0xf2, 0x01};
 523         u8 *pbuf = pie;
 524         int limit_new = limit;
 525
 526         while (1) {
 527                 pbuf = rtw_get_ie(pbuf, _WPA_IE_ID_, &len, limit_new);
 528
 529                 if (pbuf) {
 530
 531                         /* check if oui matches... */
 532                         if (_rtw_memcmp((pbuf + 2), wpa_oui_type, sizeof(wpa_oui_type)) == _FALSE)
 533
 534                                 goto check_next_ie;
 535
 536                         /* check version... */
 537                         _rtw_memcpy((u8 *)&val16, (pbuf + 6), sizeof(val16));
 538
 539                         val16 = le16_to_cpu(val16);
 540                         if (val16 != 0x0001)
 541                                 goto check_next_ie;
 542
 543                         *wpa_ie_len = *(pbuf + 1);
 544
 545                         return pbuf;
 546
 547                 } else {
 548
 549                         *wpa_ie_len = 0;
 550                         return NULL;
 551                 }
 552
 553 check_next_ie:
 554
 555                 limit_new = limit - (pbuf - pie) - 2 - len;
 556
 557                 if (limit_new <= 0)
 558                         break;
 559
 560                 pbuf += (2 + len);
 561
 562         }
 563
 564         *wpa_ie_len = 0;
 565
 566         return NULL;
 567
 568 }
 569
 570 unsigned char *rtw_get_wpa2_ie(unsigned char *pie, int *rsn_ie_len, int limit)
 571 {
 572
 573         return rtw_get_ie(pie, _WPA2_IE_ID_, rsn_ie_len, limit);
 574
 575 }
 576
 577 int rtw_get_wpa_cipher_suite(u8 *s)
 578 {
 579         if (_rtw_memcmp(s, WPA_CIPHER_SUITE_NONE, WPA_SELECTOR_LEN) == _TRUE)
 580                 return WPA_CIPHER_NONE;
 581         if (_rtw_memcmp(s, WPA_CIPHER_SUITE_WEP40, WPA_SELECTOR_LEN) == _TRUE)
 582                 return WPA_CIPHER_WEP40;
 583         if (_rtw_memcmp(s, WPA_CIPHER_SUITE_TKIP, WPA_SELECTOR_LEN) == _TRUE)
 584                 return WPA_CIPHER_TKIP;
 585         if (_rtw_memcmp(s, WPA_CIPHER_SUITE_CCMP, WPA_SELECTOR_LEN) == _TRUE)
 586                 return WPA_CIPHER_CCMP;
 587         if (_rtw_memcmp(s, WPA_CIPHER_SUITE_WEP104, WPA_SELECTOR_LEN) == _TRUE)
 588                 return WPA_CIPHER_WEP104;
 589
 590         return 0;
 591 }
 592
 593 int rtw_get_wpa2_cipher_suite(u8 *s)
 594 {
 595         if (_rtw_memcmp(s, RSN_CIPHER_SUITE_NONE, RSN_SELECTOR_LEN) == _TRUE)
 596                 return WPA_CIPHER_NONE;
 597         if (_rtw_memcmp(s, RSN_CIPHER_SUITE_WEP40, RSN_SELECTOR_LEN) == _TRUE)
 598                 return WPA_CIPHER_WEP40;
 599         if (_rtw_memcmp(s, RSN_CIPHER_SUITE_TKIP, RSN_SELECTOR_LEN) == _TRUE)
 600                 return WPA_CIPHER_TKIP;
 601         if (_rtw_memcmp(s, RSN_CIPHER_SUITE_CCMP, RSN_SELECTOR_LEN) == _TRUE)
 602                 return WPA_CIPHER_CCMP;
 603         if (_rtw_memcmp(s, RSN_CIPHER_SUITE_WEP104, RSN_SELECTOR_LEN) == _TRUE)
 604                 return WPA_CIPHER_WEP104;
 605
 606         return 0;
 607 }
 608
 609
 610 int rtw_parse_wpa_ie(u8 *wpa_ie, int wpa_ie_len, int *group_cipher, int *pairwise_cipher, int *is_8021x)
 611 {
 612         int i, ret = _SUCCESS;
 613         int left, count;
 614         u8 *pos;
 615         u8 SUITE_1X[4] = {0x00, 0x50, 0xf2, 1};
 616
 617         if (wpa_ie_len <= 0) {
 618                 /* No WPA IE - fail silently */
 619                 return _FAIL;
 620         }
 621
 622
 623         if ((*wpa_ie != _WPA_IE_ID_) || (*(wpa_ie + 1) != (u8)(wpa_ie_len - 2)) ||
 624             (_rtw_memcmp(wpa_ie + 2, RTW_WPA_OUI_TYPE, WPA_SELECTOR_LEN) != _TRUE))
 625                 return _FAIL;
 626
 627         pos = wpa_ie;
 628
 629         pos += 8;
 630         left = wpa_ie_len - 8;
 631
 632
 633         /* group_cipher */
 634         if (left >= WPA_SELECTOR_LEN) {
 635
 636                 *group_cipher = rtw_get_wpa_cipher_suite(pos);
 637
 638                 pos += WPA_SELECTOR_LEN;
 639                 left -= WPA_SELECTOR_LEN;
 640
 641         } else if (left > 0) {
 642
 643                 return _FAIL;
 644         }
 645
 646
 647         /* pairwise_cipher */
 648         if (left >= 2) {
 649                 /* count = le16_to_cpu(*(u16*)pos);      */
 650                 count = RTW_GET_LE16(pos);
 651                 pos += 2;
 652                 left -= 2;
 653
 654                 if (count == 0 || left < count * WPA_SELECTOR_LEN) {
 655                         return _FAIL;
 656                 }
 657
 658                 for (i = 0; i < count; i++) {
 659                         *pairwise_cipher |= rtw_get_wpa_cipher_suite(pos);
 660
 661                         pos += WPA_SELECTOR_LEN;
 662                         left -= WPA_SELECTOR_LEN;
 663                 }
 664
 665         } else if (left == 1) {
 666                 return _FAIL;
 667         }
 668
 669         if (is_8021x) {
 670                 if (left >= 6) {
 671                         pos += 2;
 672                         if (_rtw_memcmp(pos, SUITE_1X, 4) == 1) {
 673                                 *is_8021x = 1;
 674                         }
 675                 }
 676         }
 677
 678         return ret;
 679
 680 }
 681
 682 int rtw_parse_wpa2_ie(u8 *rsn_ie, int rsn_ie_len, int *group_cipher, int *pairwise_cipher, int *is_8021x)
 683 {
 684         int i, ret = _SUCCESS;
 685         int left, count;
 686         u8 *pos;
 687         u8 SUITE_1X[4] = {0x00, 0x0f, 0xac, 0x01};
 688
 689         if (rsn_ie_len <= 0) {
 690                 /* No RSN IE - fail silently */
 691                 return _FAIL;
 692         }
 693
 694
 695         if ((*rsn_ie != _WPA2_IE_ID_) || (*(rsn_ie + 1) != (u8)(rsn_ie_len - 2)))
 696                 return _FAIL;
 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                 return _FAIL;
 712         }
 713
 714         /* pairwise_cipher */
 715         if (left >= 2) {
 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                         return _FAIL;
 723                 }
 724
 725                 for (i = 0; i < count; i++) {
 726                         *pairwise_cipher |= rtw_get_wpa2_cipher_suite(pos);
 727
 728                         pos += RSN_SELECTOR_LEN;
 729                         left -= RSN_SELECTOR_LEN;
 730                 }
 731
 732         } else if (left == 1) {
 733
 734                 return _FAIL;
 735         }
 736
 737         if (is_8021x) {
 738                 if (left >= 6) {
 739                         pos += 2;
 740                         if (_rtw_memcmp(pos, SUITE_1X, 4) == 1) {
 741                                 *is_8021x = 1;
 742                         }
 743                 }
 744         }
 745
 746         return ret;
 747
 748 }
 749
 750 /* #ifdef CONFIG_WAPI_SUPPORT */
 751 int rtw_get_wapi_ie(u8 *in_ie, uint in_len, u8 *wapi_ie, u16 *wapi_len)
 752 {
 753         int len = 0;
 754         u8 authmode, i;
 755         uint    cnt;
 756         u8 wapi_oui1[4] = {0x0, 0x14, 0x72, 0x01};
 757         u8 wapi_oui2[4] = {0x0, 0x14, 0x72, 0x02};
 758
 759
 760         if (wapi_len)
 761                 *wapi_len = 0;
 762
 763         if (!in_ie || in_len <= 0)
 764                 return len;
 765
 766         cnt = (_TIMESTAMP_ + _BEACON_ITERVAL_ + _CAPABILITY_);
 767
 768         while (cnt < in_len) {
 769                 authmode = in_ie[cnt];
 770
 771                 /* if(authmode==_WAPI_IE_) */
 772                 if (authmode == _WAPI_IE_ && (_rtw_memcmp(&in_ie[cnt + 6], wapi_oui1, 4) == _TRUE ||
 773                         _rtw_memcmp(&in_ie[cnt + 6], wapi_oui2, 4) == _TRUE)) {
 774                         if (wapi_ie)
 775                                 _rtw_memcpy(wapi_ie, &in_ie[cnt], in_ie[cnt + 1] + 2);
 776
 777                         if (wapi_len)
 778                                 *wapi_len = in_ie[cnt + 1] + 2;
 779
 780                         cnt += in_ie[cnt + 1] + 2; /* get next */
 781                 } else {
 782                         cnt += in_ie[cnt + 1] + 2; /* get next */
 783                 }
 784         }
 785
 786         if (wapi_len)
 787                 len = *wapi_len;
 788
 789
 790         return len;
 791
 792 }
 793 /* #endif */
 794
 795 int rtw_get_sec_ie(u8 *in_ie, uint in_len, u8 *rsn_ie, u16 *rsn_len, u8 *wpa_ie, u16 *wpa_len)
 796 {
 797         u8 authmode, sec_idx, i;
 798         u8 wpa_oui[4] = {0x0, 0x50, 0xf2, 0x01};
 799         uint    cnt;
 800
 801
 802         /* Search required WPA or WPA2 IE and copy to sec_ie[ ] */
 803
 804         cnt = (_TIMESTAMP_ + _BEACON_ITERVAL_ + _CAPABILITY_);
 805
 806         sec_idx = 0;
 807
 808         while (cnt < in_len) {
 809                 authmode = in_ie[cnt];
 810
 811                 if ((authmode == _WPA_IE_ID_) && (_rtw_memcmp(&in_ie[cnt + 2], &wpa_oui[0], 4) == _TRUE)) {
 812
 813                         if (wpa_ie)
 814                                 _rtw_memcpy(wpa_ie, &in_ie[cnt], in_ie[cnt + 1] + 2);
 815
 816                         *wpa_len = in_ie[cnt + 1] + 2;
 817                         cnt += in_ie[cnt + 1] + 2; /* get next */
 818                 } else {
 819                         if (authmode == _WPA2_IE_ID_) {
 820
 821                                 if (rsn_ie)
 822                                         _rtw_memcpy(rsn_ie, &in_ie[cnt], in_ie[cnt + 1] + 2);
 823
 824                                 *rsn_len = in_ie[cnt + 1] + 2;
 825                                 cnt += in_ie[cnt + 1] + 2; /* get next */
 826                         } else {
 827                                 cnt += in_ie[cnt + 1] + 2; /* get next */
 828                         }
 829                 }
 830
 831         }
 832
 833
 834         return *rsn_len + *wpa_len;
 835
 836 }
 837
 838 u8 rtw_is_wps_ie(u8 *ie_ptr, uint *wps_ielen)
 839 {
 840         u8 match = _FALSE;
 841         u8 eid, wps_oui[4] = {0x0, 0x50, 0xf2, 0x04};
 842
 843         if (ie_ptr == NULL)
 844                 return match;
 845
 846         eid = ie_ptr[0];
 847
 848         if ((eid == _WPA_IE_ID_) && (_rtw_memcmp(&ie_ptr[2], wps_oui, 4) == _TRUE)) {
 849                 /* RTW_INFO("==> found WPS_IE.....\n"); */
 850                 *wps_ielen = ie_ptr[1] + 2;
 851                 match = _TRUE;
 852         }
 853         return match;
 854 }
 855
 856 u8 *rtw_get_wps_ie_from_scan_queue(u8 *in_ie, uint in_len, u8 *wps_ie, uint *wps_ielen, u8 frame_type)
 857 {
 858         u8      *wps = NULL;
 859
 860         RTW_INFO("[%s] frame_type = %d\n", __FUNCTION__, frame_type);
 861         switch (frame_type) {
 862         case 1:
 863         case 3: {
 864                 /*      Beacon or Probe Response */
 865                 wps = rtw_get_wps_ie(in_ie + _PROBERSP_IE_OFFSET_, in_len - _PROBERSP_IE_OFFSET_, wps_ie, wps_ielen);
 866                 break;
 867         }
 868         case 2: {
 869                 /*      Probe Request */
 870                 wps = rtw_get_wps_ie(in_ie + _PROBEREQ_IE_OFFSET_ , in_len - _PROBEREQ_IE_OFFSET_ , wps_ie, wps_ielen);
 871                 break;
 872         }
 873         }
 874         return wps;
 875 }
 876
 877 /**
 878  * rtw_get_wps_ie - Search WPS IE from a series of IEs
 879  * @in_ie: Address of IEs to search
 880  * @in_len: Length limit from in_ie
 881  * @wps_ie: If not NULL and WPS IE is found, WPS IE will be copied to the buf starting from wps_ie
 882  * @wps_ielen: If not NULL and WPS IE is found, will set to the length of the entire WPS IE
 883  *
 884  * Returns: The address of the WPS IE found, or NULL
 885  */
 886 u8 *rtw_get_wps_ie(u8 *in_ie, uint in_len, u8 *wps_ie, uint *wps_ielen)
 887 {
 888         uint cnt;
 889         u8 *wpsie_ptr = NULL;
 890         u8 eid, wps_oui[4] = {0x00, 0x50, 0xf2, 0x04};
 891
 892         if (wps_ielen)
 893                 *wps_ielen = 0;
 894
 895         if (!in_ie) {
 896                 rtw_warn_on(1);
 897                 return wpsie_ptr;
 898         }
 899
 900         if (in_len <= 0)
 901                 return wpsie_ptr;
 902
 903         cnt = 0;
 904
 905         while (cnt + 1 + 4 < in_len) {
 906                 eid = in_ie[cnt];
 907
 908                 if (cnt + 1 + 4 >= MAX_IE_SZ) {
 909                         rtw_warn_on(1);
 910                         return NULL;
 911                 }
 912
 913                 if (eid == WLAN_EID_VENDOR_SPECIFIC && _rtw_memcmp(&in_ie[cnt + 2], wps_oui, 4) == _TRUE) {
 914                         wpsie_ptr = in_ie + cnt;
 915
 916                         if (wps_ie)
 917                                 _rtw_memcpy(wps_ie, &in_ie[cnt], in_ie[cnt + 1] + 2);
 918
 919                         if (wps_ielen)
 920                                 *wps_ielen = in_ie[cnt + 1] + 2;
 921
 922                         break;
 923                 } else
 924                         cnt += in_ie[cnt + 1] + 2;
 925
 926         }
 927
 928         return wpsie_ptr;
 929 }
 930
 931 /**
 932  * rtw_get_wps_attr - Search a specific WPS attribute from a given WPS IE
 933  * @wps_ie: Address of WPS IE to search
 934  * @wps_ielen: Length limit from wps_ie
 935  * @target_attr_id: The attribute ID of WPS attribute to search
 936  * @buf_attr: If not NULL and the WPS attribute is found, WPS attribute will be copied to the buf starting from buf_attr
 937  * @len_attr: If not NULL and the WPS attribute is found, will set to the length of the entire WPS attribute
 938  *
 939  * Returns: the address of the specific WPS attribute found, or NULL
 940  */
 941 u8 *rtw_get_wps_attr(u8 *wps_ie, uint wps_ielen, u16 target_attr_id , u8 *buf_attr, u32 *len_attr)
 942 {
 943         u8 *attr_ptr = NULL;
 944         u8 *target_attr_ptr = NULL;
 945         u8 wps_oui[4] = {0x00, 0x50, 0xF2, 0x04};
 946
 947         if (len_attr)
 948                 *len_attr = 0;
 949
 950         if ((wps_ie[0] != _VENDOR_SPECIFIC_IE_) ||
 951             (_rtw_memcmp(wps_ie + 2, wps_oui , 4) != _TRUE))
 952                 return attr_ptr;
 953
 954         /* 6 = 1(Element ID) + 1(Length) + 4(WPS OUI) */
 955         attr_ptr = wps_ie + 6; /* goto first attr */
 956
 957         while (attr_ptr - wps_ie < wps_ielen) {
 958                 /* 4 = 2(Attribute ID) + 2(Length) */
 959                 u16 attr_id = RTW_GET_BE16(attr_ptr);
 960                 u16 attr_data_len = RTW_GET_BE16(attr_ptr + 2);
 961                 u16 attr_len = attr_data_len + 4;
 962
 963                 /* RTW_INFO("%s attr_ptr:%p, id:%u, length:%u\n", __FUNCTION__, attr_ptr, attr_id, attr_data_len); */
 964                 if (attr_id == target_attr_id) {
 965                         target_attr_ptr = attr_ptr;
 966
 967                         if (buf_attr)
 968                                 _rtw_memcpy(buf_attr, attr_ptr, attr_len);
 969
 970                         if (len_attr)
 971                                 *len_attr = attr_len;
 972
 973                         break;
 974                 } else {
 975                         attr_ptr += attr_len; /* goto next */
 976                 }
 977
 978         }
 979
 980         return target_attr_ptr;
 981 }
 982
 983 /**
 984  * rtw_get_wps_attr_content - Search a specific WPS attribute content from a given WPS IE
 985  * @wps_ie: Address of WPS IE to search
 986  * @wps_ielen: Length limit from wps_ie
 987  * @target_attr_id: The attribute ID of WPS attribute to search
 988  * @buf_content: If not NULL and the WPS attribute is found, WPS attribute content will be copied to the buf starting from buf_content
 989  * @len_content: If not NULL and the WPS attribute is found, will set to the length of the WPS attribute content
 990  *
 991  * Returns: the address of the specific WPS attribute content found, or NULL
 992  */
 993 u8 *rtw_get_wps_attr_content(u8 *wps_ie, uint wps_ielen, u16 target_attr_id , u8 *buf_content, uint *len_content)
 994 {
 995         u8 *attr_ptr;
 996         u32 attr_len;
 997
 998         if (len_content)
 999                 *len_content = 0;
1000
1001         attr_ptr = rtw_get_wps_attr(wps_ie, wps_ielen, target_attr_id, NULL, &attr_len);
1002
1003         if (attr_ptr && attr_len) {
1004                 if (buf_content)
1005                         _rtw_memcpy(buf_content, attr_ptr + 4, attr_len - 4);
1006
1007                 if (len_content)
1008                         *len_content = attr_len - 4;
1009
1010                 return attr_ptr + 4;
1011         }
1012
1013         return NULL;
1014 }
1015
1016 static int rtw_ieee802_11_parse_vendor_specific(u8 *pos, uint elen,
1017                 struct rtw_ieee802_11_elems *elems,
1018                 int show_errors)
1019 {
1020         unsigned int oui;
1021
1022         /* first 3 bytes in vendor specific information element are the IEEE
1023          * OUI of the vendor. The following byte is used a vendor specific
1024          * sub-type. */
1025         if (elen < 4) {
1026                 if (show_errors) {
1027                         RTW_INFO("short vendor specific "
1028                                  "information element ignored (len=%lu)\n",
1029                                  (unsigned long) elen);
1030                 }
1031                 return -1;
1032         }
1033
1034         oui = RTW_GET_BE24(pos);
1035         switch (oui) {
1036         case OUI_MICROSOFT:
1037                 /* Microsoft/Wi-Fi information elements are further typed and
1038                  * subtyped */
1039                 switch (pos[3]) {
1040                 case 1:
1041                         /* Microsoft OUI (00:50:F2) with OUI Type 1:
1042                          * real WPA information element */
1043                         elems->wpa_ie = pos;
1044                         elems->wpa_ie_len = elen;
1045                         break;
1046                 case WME_OUI_TYPE: /* this is a Wi-Fi WME info. element */
1047                         if (elen < 5) {
1048                                 RTW_DBG("short WME "
1049                                         "information element ignored "
1050                                         "(len=%lu)\n",
1051                                         (unsigned long) elen);
1052                                 return -1;
1053                         }
1054                         switch (pos[4]) {
1055                         case WME_OUI_SUBTYPE_INFORMATION_ELEMENT:
1056                         case WME_OUI_SUBTYPE_PARAMETER_ELEMENT:
1057                                 elems->wme = pos;
1058                                 elems->wme_len = elen;
1059                                 break;
1060                         case WME_OUI_SUBTYPE_TSPEC_ELEMENT:
1061                                 elems->wme_tspec = pos;
1062                                 elems->wme_tspec_len = elen;
1063                                 break;
1064                         default:
1065                                 RTW_DBG("unknown WME "
1066                                         "information element ignored "
1067                                         "(subtype=%d len=%lu)\n",
1068                                         pos[4], (unsigned long) elen);
1069                                 return -1;
1070                         }
1071                         break;
1072                 case 4:
1073                         /* Wi-Fi Protected Setup (WPS) IE */
1074                         elems->wps_ie = pos;
1075                         elems->wps_ie_len = elen;
1076                         break;
1077                 default:
1078                         RTW_DBG("Unknown Microsoft "
1079                                 "information element ignored "
1080                                 "(type=%d len=%lu)\n",
1081                                 pos[3], (unsigned long) elen);
1082                         return -1;
1083                 }
1084                 break;
1085
1086         case OUI_BROADCOM:
1087                 switch (pos[3]) {
1088                 case VENDOR_HT_CAPAB_OUI_TYPE:
1089                         elems->vendor_ht_cap = pos;
1090                         elems->vendor_ht_cap_len = elen;
1091                         break;
1092                 default:
1093                         RTW_DBG("Unknown Broadcom "
1094                                 "information element ignored "
1095                                 "(type=%d len=%lu)\n",
1096                                 pos[3], (unsigned long) elen);
1097                         return -1;
1098                 }
1099                 break;
1100
1101         default:
1102                 RTW_DBG("unknown vendor specific information "
1103                         "element ignored (vendor OUI %02x:%02x:%02x "
1104                         "len=%lu)\n",
1105                         pos[0], pos[1], pos[2], (unsigned long) elen);
1106                 return -1;
1107         }
1108
1109         return 0;
1110
1111 }
1112
1113 /**
1114  * ieee802_11_parse_elems - Parse information elements in management frames
1115  * @start: Pointer to the start of IEs
1116  * @len: Length of IE buffer in octets
1117  * @elems: Data structure for parsed elements
1118  * @show_errors: Whether to show parsing errors in debug log
1119  * Returns: Parsing result
1120  */
1121 ParseRes rtw_ieee802_11_parse_elems(u8 *start, uint len,
1122                                     struct rtw_ieee802_11_elems *elems,
1123                                     int show_errors)
1124 {
1125         uint left = len;
1126         u8 *pos = start;
1127         int unknown = 0;
1128
1129         _rtw_memset(elems, 0, sizeof(*elems));
1130
1131         while (left >= 2) {
1132                 u8 id, elen;
1133
1134                 id = *pos++;
1135                 elen = *pos++;
1136                 left -= 2;
1137
1138                 if (elen > left) {
1139                         if (show_errors) {
1140                                 RTW_INFO("IEEE 802.11 element "
1141                                          "parse failed (id=%d elen=%d "
1142                                          "left=%lu)\n",
1143                                          id, elen, (unsigned long) left);
1144                         }
1145                         return ParseFailed;
1146                 }
1147
1148                 switch (id) {
1149                 case WLAN_EID_SSID:
1150                         elems->ssid = pos;
1151                         elems->ssid_len = elen;
1152                         break;
1153                 case WLAN_EID_SUPP_RATES:
1154                         elems->supp_rates = pos;
1155                         elems->supp_rates_len = elen;
1156                         break;
1157                 case WLAN_EID_FH_PARAMS:
1158                         elems->fh_params = pos;
1159                         elems->fh_params_len = elen;
1160                         break;
1161                 case WLAN_EID_DS_PARAMS:
1162                         elems->ds_params = pos;
1163                         elems->ds_params_len = elen;
1164                         break;
1165                 case WLAN_EID_CF_PARAMS:
1166                         elems->cf_params = pos;
1167                         elems->cf_params_len = elen;
1168                         break;
1169                 case WLAN_EID_TIM:
1170                         elems->tim = pos;
1171                         elems->tim_len = elen;
1172                         break;
1173                 case WLAN_EID_IBSS_PARAMS:
1174                         elems->ibss_params = pos;
1175                         elems->ibss_params_len = elen;
1176                         break;
1177                 case WLAN_EID_CHALLENGE:
1178                         elems->challenge = pos;
1179                         elems->challenge_len = elen;
1180                         break;
1181                 case WLAN_EID_ERP_INFO:
1182                         elems->erp_info = pos;
1183                         elems->erp_info_len = elen;
1184                         break;
1185                 case WLAN_EID_EXT_SUPP_RATES:
1186                         elems->ext_supp_rates = pos;
1187                         elems->ext_supp_rates_len = elen;
1188                         break;
1189                 case WLAN_EID_VENDOR_SPECIFIC:
1190                         if (rtw_ieee802_11_parse_vendor_specific(pos, elen,
1191                                         elems,
1192                                         show_errors))
1193                                 unknown++;
1194                         break;
1195                 case WLAN_EID_RSN:
1196                         elems->rsn_ie = pos;
1197                         elems->rsn_ie_len = elen;
1198                         break;
1199                 case WLAN_EID_PWR_CAPABILITY:
1200                         elems->power_cap = pos;
1201                         elems->power_cap_len = elen;
1202                         break;
1203                 case WLAN_EID_SUPPORTED_CHANNELS:
1204                         elems->supp_channels = pos;
1205                         elems->supp_channels_len = elen;
1206                         break;
1207                 case WLAN_EID_MOBILITY_DOMAIN:
1208                         elems->mdie = pos;
1209                         elems->mdie_len = elen;
1210                         break;
1211                 case WLAN_EID_FAST_BSS_TRANSITION:
1212                         elems->ftie = pos;
1213                         elems->ftie_len = elen;
1214                         break;
1215                 case WLAN_EID_TIMEOUT_INTERVAL:
1216                         elems->timeout_int = pos;
1217                         elems->timeout_int_len = elen;
1218                         break;
1219                 case WLAN_EID_HT_CAP:
1220                         elems->ht_capabilities = pos;
1221                         elems->ht_capabilities_len = elen;
1222                         break;
1223                 case WLAN_EID_HT_OPERATION:
1224                         elems->ht_operation = pos;
1225                         elems->ht_operation_len = elen;
1226                         break;
1227                 case WLAN_EID_VHT_CAPABILITY:
1228                         elems->vht_capabilities = pos;
1229                         elems->vht_capabilities_len = elen;
1230                         break;
1231                 case WLAN_EID_VHT_OPERATION:
1232                         elems->vht_operation = pos;
1233                         elems->vht_operation_len = elen;
1234                         break;
1235                 case WLAN_EID_VHT_OP_MODE_NOTIFY:
1236                         elems->vht_op_mode_notify = pos;
1237                         elems->vht_op_mode_notify_len = elen;
1238                         break;
1239                 default:
1240                         unknown++;
1241                         if (!show_errors)
1242                                 break;
1243                         RTW_DBG("IEEE 802.11 element parse "
1244                                 "ignored unknown element (id=%d elen=%d)\n",
1245                                 id, elen);
1246                         break;
1247                 }
1248
1249                 left -= elen;
1250                 pos += elen;
1251         }
1252
1253         if (left)
1254                 return ParseFailed;
1255
1256         return unknown ? ParseUnknown : ParseOK;
1257
1258 }
1259
1260 static u8 key_char2num(u8 ch);
1261 static u8 key_char2num(u8 ch)
1262 {
1263         if ((ch >= '0') && (ch <= '9'))
1264                 return ch - '0';
1265         else if ((ch >= 'a') && (ch <= 'f'))
1266                 return ch - 'a' + 10;
1267         else if ((ch >= 'A') && (ch <= 'F'))
1268                 return ch - 'A' + 10;
1269         else
1270                 return 0xff;
1271 }
1272
1273 u8 str_2char2num(u8 hch, u8 lch);
1274 u8 str_2char2num(u8 hch, u8 lch)
1275 {
1276         return (key_char2num(hch) * 10) + key_char2num(lch);
1277 }
1278
1279 u8 key_2char2num(u8 hch, u8 lch);
1280 u8 key_2char2num(u8 hch, u8 lch)
1281 {
1282         return (key_char2num(hch) << 4) | key_char2num(lch);
1283 }
1284
1285 void macstr2num(u8 *dst, u8 *src);
1286 void macstr2num(u8 *dst, u8 *src)
1287 {
1288         int     jj, kk;
1289         for (jj = 0, kk = 0; jj < ETH_ALEN; jj++, kk += 3)
1290                 dst[jj] = key_2char2num(src[kk], src[kk + 1]);
1291 }
1292
1293 u8 convert_ip_addr(u8 hch, u8 mch, u8 lch)
1294 {
1295         return (key_char2num(hch) * 100) + (key_char2num(mch) * 10) + key_char2num(lch);
1296 }
1297
1298 #ifdef CONFIG_PLATFORM_INTEL_BYT
1299 #define MAC_ADDRESS_LEN 12
1300
1301 int rtw_get_mac_addr_intel(unsigned char *buf)
1302 {
1303         int ret = 0;
1304         int i;
1305         struct file *fp = NULL;
1306         mm_segment_t oldfs;
1307         unsigned char c_mac[MAC_ADDRESS_LEN];
1308         char fname[] = "/config/wifi/mac.txt";
1309         int jj, kk;
1310
1311         RTW_INFO("%s Enter\n", __FUNCTION__);
1312
1313         ret = rtw_retrieve_from_file(fname, c_mac, MAC_ADDRESS_LEN);
1314         if (ret < MAC_ADDRESS_LEN)
1315                 return -1;
1316
1317         for (jj = 0, kk = 0; jj < ETH_ALEN; jj++, kk += 2)
1318                 buf[jj] = key_2char2num(c_mac[kk], c_mac[kk + 1]);
1319
1320         RTW_INFO("%s: read from file mac address: "MAC_FMT"\n",
1321                  __FUNCTION__, MAC_ARG(buf));
1322
1323         return 0;
1324 }
1325 #endif /* CONFIG_PLATFORM_INTEL_BYT */
1326
1327 /*
1328  * Description:
1329  * rtw_check_invalid_mac_address:
1330  * This is only used for checking mac address valid or not.
1331  *
1332  * Input:
1333  * adapter: mac_address pointer.
1334  * check_local_bit: check locally bit or not.
1335  *
1336  * Output:
1337  * _TRUE: The mac address is invalid.
1338  * _FALSE: The mac address is valid.
1339  *
1340  * Auther: Isaac.Li
1341  */
1342 u8 rtw_check_invalid_mac_address(u8 *mac_addr, u8 check_local_bit)
1343 {
1344         u8 null_mac_addr[ETH_ALEN] = {0, 0, 0, 0, 0, 0};
1345         u8 multi_mac_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
1346         u8 res = _FALSE;
1347
1348         if (_rtw_memcmp(mac_addr, null_mac_addr, ETH_ALEN)) {
1349                 res = _TRUE;
1350                 goto func_exit;
1351         }
1352
1353         if (_rtw_memcmp(mac_addr, multi_mac_addr, ETH_ALEN)) {
1354                 res = _TRUE;
1355                 goto func_exit;
1356         }
1357
1358         if (mac_addr[0] & BIT0) {
1359                 res = _TRUE;
1360                 goto func_exit;
1361         }
1362
1363         if (check_local_bit == _TRUE) {
1364                 if (mac_addr[0] & BIT1) {
1365                         res = _TRUE;
1366                         goto func_exit;
1367                 }
1368         }
1369
1370 func_exit:
1371         return res;
1372 }
1373
1374 extern char *rtw_initmac;
1375 /**
1376  * rtw_macaddr_cfg - Decide the mac address used
1377  * @out: buf to store mac address decided
1378  * @hw_mac_addr: mac address from efuse/epprom
1379  */
1380 void rtw_macaddr_cfg(u8 *out, const u8 *hw_mac_addr)
1381 {
1382 #define DEFAULT_RANDOM_MACADDR 1
1383         u8 mac[ETH_ALEN];
1384
1385         if (out == NULL) {
1386                 rtw_warn_on(1);
1387                 return;
1388         }
1389
1390         /* Users specify the mac address */
1391         if (rtw_initmac) {
1392                 int jj, kk;
1393
1394                 for (jj = 0, kk = 0; jj < ETH_ALEN; jj++, kk += 3)
1395                         mac[jj] = key_2char2num(rtw_initmac[kk], rtw_initmac[kk + 1]);
1396
1397                 goto err_chk;
1398         }
1399
1400         /* platform specified */
1401 #ifdef CONFIG_PLATFORM_INTEL_BYT
1402         if (rtw_get_mac_addr_intel(mac) == 0)
1403                 goto err_chk;
1404 #endif
1405
1406         /* Use the mac address stored in the Efuse */
1407         if (hw_mac_addr) {
1408                 _rtw_memcpy(mac, hw_mac_addr, ETH_ALEN);
1409                 goto err_chk;
1410         }
1411
1412 err_chk:
1413     if (!rockchip_wifi_mac_addr(mac)) {
1414                 printk("get mac address from flash=[%02x:%02x:%02x:%02x:%02x:%02x]\n", mac[0], mac[1],
1415                 mac[2], mac[3], mac[4], mac[5]);
1416         }
1417         if (rtw_check_invalid_mac_address(mac, _TRUE) == _TRUE) {
1418 #if DEFAULT_RANDOM_MACADDR
1419                 RTW_ERR("invalid mac addr:"MAC_FMT", assign random MAC\n", MAC_ARG(mac));
1420                 *((u32 *)(&mac[2])) = rtw_random32();
1421                 mac[0] = 0x00;
1422                 mac[1] = 0xe0;
1423                 mac[2] = 0x4c;
1424 #else
1425                 RTW_ERR("invalid mac addr:"MAC_FMT", assign default one\n", MAC_ARG(mac));
1426                 mac[0] = 0x00;
1427                 mac[1] = 0xe0;
1428                 mac[2] = 0x4c;
1429                 mac[3] = 0x87;
1430                 mac[4] = 0x00;
1431                 mac[5] = 0x00;
1432 #endif
1433         }
1434
1435         _rtw_memcpy(out, mac, ETH_ALEN);
1436         RTW_INFO("%s mac addr:"MAC_FMT"\n", __func__, MAC_ARG(out));
1437 }
1438
1439 #ifdef CONFIG_80211N_HT
1440 void dump_ht_cap_ie_content(void *sel, u8 *buf, u32 buf_len)
1441 {
1442         if (buf_len != 26) {
1443                 RTW_PRINT_SEL(sel, "Invalid HT capability IE len:%d != %d\n", buf_len, 26);
1444                 return;
1445         }
1446
1447         RTW_PRINT_SEL(sel, "HT Capabilities Info:%02x%02x\n", *(buf), *(buf + 1));
1448         RTW_PRINT_SEL(sel, "A-MPDU Parameters:"HT_AMPDU_PARA_FMT"\n"
1449                       , HT_AMPDU_PARA_ARG(HT_CAP_ELE_AMPDU_PARA(buf)));
1450         RTW_PRINT_SEL(sel, "Supported MCS Set:"HT_SUP_MCS_SET_FMT"\n"
1451                       , HT_SUP_MCS_SET_ARG(HT_CAP_ELE_SUP_MCS_SET(buf)));
1452 }
1453
1454 void dump_ht_cap_ie(void *sel, u8 *ie, u32 ie_len)
1455 {
1456         u8 *pos = (u8 *)ie;
1457         u16 id;
1458         u16 len;
1459
1460         u8 *ht_cap_ie;
1461         sint ht_cap_ielen;
1462
1463         ht_cap_ie = rtw_get_ie(ie, _HT_CAPABILITY_IE_, &ht_cap_ielen, ie_len);
1464         if (!ie || ht_cap_ie != ie)
1465                 return;
1466
1467         dump_ht_cap_ie_content(sel, ht_cap_ie + 2, ht_cap_ielen);
1468 }
1469 #endif /* CONFIG_80211N_HT */
1470
1471 void dump_ies(void *sel, u8 *buf, u32 buf_len)
1472 {
1473         u8 *pos = (u8 *)buf;
1474         u8 id, len;
1475
1476         while (pos - buf + 1 < buf_len) {
1477                 id = *pos;
1478                 len = *(pos + 1);
1479
1480                 RTW_PRINT_SEL(sel, "%s ID:%u, LEN:%u\n", __FUNCTION__, id, len);
1481 #ifdef CONFIG_80211N_HT
1482                 dump_ht_cap_ie(sel, pos, len + 2);
1483 #endif
1484                 dump_wps_ie(sel, pos, len + 2);
1485 #ifdef CONFIG_P2P
1486                 dump_p2p_ie(sel, pos, len + 2);
1487 #ifdef CONFIG_WFD
1488                 dump_wfd_ie(sel, pos, len + 2);
1489 #endif
1490 #endif
1491
1492                 pos += (2 + len);
1493         }
1494 }
1495
1496 void dump_wps_ie(void *sel, u8 *ie, u32 ie_len)
1497 {
1498         u8 *pos = (u8 *)ie;
1499         u16 id;
1500         u16 len;
1501
1502         u8 *wps_ie;
1503         uint wps_ielen;
1504
1505         wps_ie = rtw_get_wps_ie(ie, ie_len, NULL, &wps_ielen);
1506         if (wps_ie != ie || wps_ielen == 0)
1507                 return;
1508
1509         pos += 6;
1510         while (pos - ie + 4 <= ie_len) {
1511                 id = RTW_GET_BE16(pos);
1512                 len = RTW_GET_BE16(pos + 2);
1513
1514                 RTW_PRINT_SEL(sel, "%s ID:0x%04x, LEN:%u%s\n", __func__, id, len
1515                         , ((pos - ie + 4 + len) <= ie_len) ? "" : "(exceed ie_len)");
1516
1517                 pos += (4 + len);
1518         }
1519 }
1520
1521 /**
1522  * rtw_ies_get_chbw - get operation ch, bw, offset from IEs of BSS.
1523  * @ies: pointer of the first tlv IE
1524  * @ies_len: length of @ies
1525  * @ch: pointer of ch, used as output
1526  * @bw: pointer of bw, used as output
1527  * @offset: pointer of offset, used as output
1528  */
1529 void rtw_ies_get_chbw(u8 *ies, int ies_len, u8 *ch, u8 *bw, u8 *offset)
1530 {
1531         u8 *p;
1532         int     ie_len;
1533
1534         *ch = 0;
1535         *bw = CHANNEL_WIDTH_20;
1536         *offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
1537
1538         p = rtw_get_ie(ies, _DSSET_IE_, &ie_len, ies_len);
1539         if (p && ie_len > 0)
1540                 *ch = *(p + 2);
1541
1542 #ifdef CONFIG_80211N_HT
1543         {
1544                 u8 *ht_cap_ie, *ht_op_ie;
1545                 int ht_cap_ielen, ht_op_ielen;
1546
1547                 ht_cap_ie = rtw_get_ie(ies, EID_HTCapability, &ht_cap_ielen, ies_len);
1548                 if (ht_cap_ie && ht_cap_ielen) {
1549                         if (GET_HT_CAP_ELE_CHL_WIDTH(ht_cap_ie + 2))
1550                                 *bw = CHANNEL_WIDTH_40;
1551                 }
1552
1553                 ht_op_ie = rtw_get_ie(ies, EID_HTInfo, &ht_op_ielen, ies_len);
1554                 if (ht_op_ie && ht_op_ielen) {
1555                         if (*ch == 0)
1556                                 *ch = GET_HT_OP_ELE_PRI_CHL(ht_op_ie + 2);
1557                         else if (*ch != 0 && *ch != GET_HT_OP_ELE_PRI_CHL(ht_op_ie + 2)) {
1558                                 RTW_INFO("%s ch inconsistent, DSSS:%u, HT primary:%u\n"
1559                                         , __func__, *ch, GET_HT_OP_ELE_PRI_CHL(ht_op_ie + 2));
1560                         }
1561
1562                         if (!GET_HT_OP_ELE_STA_CHL_WIDTH(ht_op_ie + 2))
1563                                 *bw = CHANNEL_WIDTH_20;
1564
1565                         if (*bw == CHANNEL_WIDTH_40) {
1566                                 switch (GET_HT_OP_ELE_2ND_CHL_OFFSET(ht_op_ie + 2)) {
1567                                 case SCA:
1568                                         *offset = HAL_PRIME_CHNL_OFFSET_LOWER;
1569                                         break;
1570                                 case SCB:
1571                                         *offset = HAL_PRIME_CHNL_OFFSET_UPPER;
1572                                         break;
1573                                 }
1574                         }
1575                 }
1576         }
1577 #endif /* CONFIG_80211N_HT */
1578 #ifdef CONFIG_80211AC_VHT
1579         {
1580                 u8 *vht_op_ie;
1581                 int vht_op_ielen;
1582
1583                 vht_op_ie = rtw_get_ie(ies, EID_VHTOperation, &vht_op_ielen, ies_len);
1584                 if (vht_op_ie && vht_op_ielen) {
1585                         /* enable VHT 80 before check enable HT40 or not */
1586                         if (GET_VHT_OPERATION_ELE_CHL_WIDTH(vht_op_ie + 2)  >=  1) {
1587                                 /* for HT40, enable VHT80 */
1588                                 if (*bw == CHANNEL_WIDTH_40)
1589                                         *bw = CHANNEL_WIDTH_80;
1590                                 /* for HT20, enable VHT20 */
1591                                 else if (*bw == CHANNEL_WIDTH_20) {
1592                                         /* modify VHT OP IE */
1593                                         SET_VHT_OPERATION_ELE_CHL_WIDTH(vht_op_ie + 2, 0);
1594                                         /* reset to 0 for VHT20 */
1595                                         SET_VHT_OPERATION_ELE_CHL_CENTER_FREQ1(vht_op_ie + 2, 0);
1596                                         SET_VHT_OPERATION_ELE_CHL_CENTER_FREQ2(vht_op_ie + 2, 0);
1597                                 }
1598                         } else {
1599                                 /*
1600                                   VHT OP WIDTH = 0  under HT20/HT40
1601                                   if REGSTY_BW_5G(pregistrypriv) < CHANNEL_WIDTH_80 in rtw_build_vht_operation_ie
1602                                 */
1603                         }
1604                 }
1605         }
1606 #endif
1607 }
1608
1609 void rtw_bss_get_chbw(WLAN_BSSID_EX *bss, u8 *ch, u8 *bw, u8 *offset)
1610 {
1611         rtw_ies_get_chbw(bss->IEs + sizeof(NDIS_802_11_FIXED_IEs)
1612                 , bss->IELength - sizeof(NDIS_802_11_FIXED_IEs)
1613                 , ch, bw, offset);
1614
1615         if (*ch == 0)
1616                 *ch = bss->Configuration.DSConfig;
1617         else if (*ch != bss->Configuration.DSConfig) {
1618                 RTW_INFO("inconsistent ch - ies:%u bss->Configuration.DSConfig:%u\n"
1619                          , *ch, bss->Configuration.DSConfig);
1620                 *ch = bss->Configuration.DSConfig;
1621                 rtw_warn_on(1);
1622         }
1623 }
1624
1625 /**
1626  * rtw_is_chbw_grouped - test if the two ch settings can be grouped together
1627  * @ch_a: ch of set a
1628  * @bw_a: bw of set a
1629  * @offset_a: offset of set a
1630  * @ch_b: ch of set b
1631  * @bw_b: bw of set b
1632  * @offset_b: offset of set b
1633  */
1634 bool rtw_is_chbw_grouped(u8 ch_a, u8 bw_a, u8 offset_a
1635                          , u8 ch_b, u8 bw_b, u8 offset_b)
1636 {
1637         bool is_grouped = _FALSE;
1638
1639         if (ch_a != ch_b) {
1640                 /* ch is different */
1641                 goto exit;
1642         } else if ((bw_a == CHANNEL_WIDTH_40 || bw_a == CHANNEL_WIDTH_80)
1643                    && (bw_b == CHANNEL_WIDTH_40 || bw_b == CHANNEL_WIDTH_80)
1644                   ) {
1645                 if (offset_a != offset_b)
1646                         goto exit;
1647         }
1648
1649         is_grouped = _TRUE;
1650
1651 exit:
1652         return is_grouped;
1653 }
1654
1655 /**
1656  * rtw_sync_chbw - obey g_ch, adjust g_bw, g_offset, bw, offset
1657  * @req_ch: pointer of the request ch, may be modified further
1658  * @req_bw: pointer of the request bw, may be modified further
1659  * @req_offset: pointer of the request offset, may be modified further
1660  * @g_ch: pointer of the ongoing group ch
1661  * @g_bw: pointer of the ongoing group bw, may be modified further
1662  * @g_offset: pointer of the ongoing group offset, may be modified further
1663  */
1664 void rtw_sync_chbw(u8 *req_ch, u8 *req_bw, u8 *req_offset
1665                    , u8 *g_ch, u8 *g_bw, u8 *g_offset)
1666 {
1667
1668         *req_ch = *g_ch;
1669
1670         if (*req_bw == CHANNEL_WIDTH_80 && *g_ch <= 14) {
1671                 /*2.4G ch, downgrade to 40Mhz */
1672                 *req_bw = CHANNEL_WIDTH_40;
1673         }
1674
1675         switch (*req_bw) {
1676         case CHANNEL_WIDTH_80:
1677                 if (*g_bw == CHANNEL_WIDTH_40 || *g_bw == CHANNEL_WIDTH_80)
1678                         *req_offset = *g_offset;
1679                 else if (*g_bw == CHANNEL_WIDTH_20)
1680                         *req_offset = rtw_get_offset_by_ch(*req_ch);
1681
1682                 if (*req_offset == HAL_PRIME_CHNL_OFFSET_DONT_CARE) {
1683                         RTW_ERR("%s req 80MHz BW without offset, down to 20MHz\n", __func__);
1684                         rtw_warn_on(1);
1685                         *req_bw = CHANNEL_WIDTH_20;
1686                 }
1687                 break;
1688         case CHANNEL_WIDTH_40:
1689                 if (*g_bw == CHANNEL_WIDTH_40 || *g_bw == CHANNEL_WIDTH_80)
1690                         *req_offset = *g_offset;
1691                 else if (*g_bw == CHANNEL_WIDTH_20)
1692                         *req_offset = rtw_get_offset_by_ch(*req_ch);
1693
1694                 if (*req_offset == HAL_PRIME_CHNL_OFFSET_DONT_CARE) {
1695                         RTW_ERR("%s req 40MHz BW without offset, down to 20MHz\n", __func__);
1696                         rtw_warn_on(1);
1697                         *req_bw = CHANNEL_WIDTH_20;
1698                 }
1699                 break;
1700         case CHANNEL_WIDTH_20:
1701                 *req_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
1702                 break;
1703         default:
1704                 RTW_ERR("%s req unsupported BW:%u\n", __func__, *req_bw);
1705                 rtw_warn_on(1);
1706         }
1707
1708         if (*req_bw > *g_bw) {
1709                 *g_bw = *req_bw;
1710                 *g_offset = *req_offset;
1711         }
1712 }
1713
1714 /**
1715  * rtw_get_p2p_merged_len - Get merged ie length from muitiple p2p ies.
1716  * @in_ie: Pointer of the first p2p ie
1717  * @in_len: Total len of muiltiple p2p ies
1718  * Returns: Length of merged p2p ie length
1719  */
1720 u32 rtw_get_p2p_merged_ies_len(u8 *in_ie, u32 in_len)
1721 {
1722         PNDIS_802_11_VARIABLE_IEs       pIE;
1723         u8 OUI[4] = { 0x50, 0x6f, 0x9a, 0x09 };
1724         int i = 0;
1725         int j = 0, len = 0;
1726
1727         while (i < in_len) {
1728                 pIE = (PNDIS_802_11_VARIABLE_IEs)(in_ie + i);
1729
1730                 if (pIE->ElementID == _VENDOR_SPECIFIC_IE_ && _rtw_memcmp(pIE->data, OUI, 4)) {
1731                         len += pIE->Length - 4; /* 4 is P2P OUI length, don't count it in this loop */
1732                 }
1733
1734                 i += (pIE->Length + 2);
1735         }
1736
1737         return len + 4; /* Append P2P OUI length at last. */
1738 }
1739
1740 /**
1741  * rtw_p2p_merge_ies - Merge muitiple p2p ies into one
1742  * @in_ie: Pointer of the first p2p ie
1743  * @in_len: Total len of muiltiple p2p ies
1744  * @merge_ie: Pointer of merged ie
1745  * Returns: Length of merged p2p ie
1746  */
1747 int rtw_p2p_merge_ies(u8 *in_ie, u32 in_len, u8 *merge_ie)
1748 {
1749         PNDIS_802_11_VARIABLE_IEs       pIE;
1750         u8 len = 0;
1751         u8 OUI[4] = { 0x50, 0x6f, 0x9a, 0x09 };
1752         u8 ELOUI[6] = { 0xDD, 0x00, 0x50, 0x6f, 0x9a, 0x09 };   /* EID;Len;OUI, Len would copy at the end of function */
1753         int i = 0;
1754
1755         if (merge_ie != NULL) {
1756                 /* Set first P2P OUI */
1757                 _rtw_memcpy(merge_ie, ELOUI, 6);
1758                 merge_ie += 6;
1759
1760                 while (i < in_len) {
1761                         pIE = (PNDIS_802_11_VARIABLE_IEs)(in_ie + i);
1762
1763                         /* Take out the rest of P2P OUIs */
1764                         if (pIE->ElementID == _VENDOR_SPECIFIC_IE_ && _rtw_memcmp(pIE->data, OUI, 4)) {
1765                                 _rtw_memcpy(merge_ie, pIE->data + 4, pIE->Length - 4);
1766                                 len += pIE->Length - 4;
1767                                 merge_ie += pIE->Length - 4;
1768                         }
1769
1770                         i += (pIE->Length + 2);
1771                 }
1772
1773                 return len + 4; /* 4 is for P2P OUI */
1774
1775         }
1776
1777         return 0;
1778 }
1779
1780 void dump_p2p_ie(void *sel, u8 *ie, u32 ie_len)
1781 {
1782         u8 *pos = (u8 *)ie;
1783         u8 id;
1784         u16 len;
1785
1786         u8 *p2p_ie;
1787         uint p2p_ielen;
1788
1789         p2p_ie = rtw_get_p2p_ie(ie, ie_len, NULL, &p2p_ielen);
1790         if (p2p_ie != ie || p2p_ielen == 0)
1791                 return;
1792
1793         pos += 6;
1794         while (pos - ie + 3 <= ie_len) {
1795                 id = *pos;
1796                 len = RTW_GET_LE16(pos + 1);
1797
1798                 RTW_PRINT_SEL(sel, "%s ID:%u, LEN:%u%s\n", __func__, id, len
1799                         , ((pos - ie + 3 + len) <= ie_len) ? "" : "(exceed ie_len)");
1800
1801                 pos += (3 + len);
1802         }
1803 }
1804
1805 /**
1806  * rtw_get_p2p_ie - Search P2P IE from a series of IEs
1807  * @in_ie: Address of IEs to search
1808  * @in_len: Length limit from in_ie
1809  * @p2p_ie: If not NULL and P2P IE is found, P2P IE will be copied to the buf starting from p2p_ie
1810  * @p2p_ielen: If not NULL and P2P IE is found, will set to the length of the entire P2P IE
1811  *
1812  * Returns: The address of the P2P IE found, or NULL
1813  */
1814 u8 *rtw_get_p2p_ie(u8 *in_ie, int in_len, u8 *p2p_ie, uint *p2p_ielen)
1815 {
1816         uint cnt;
1817         u8 *p2p_ie_ptr = NULL;
1818         u8 eid, p2p_oui[4] = {0x50, 0x6F, 0x9A, 0x09};
1819
1820         if (p2p_ielen)
1821                 *p2p_ielen = 0;
1822
1823         if (!in_ie || in_len < 0) {
1824                 rtw_warn_on(1);
1825                 return p2p_ie_ptr;
1826         }
1827
1828         if (in_len <= 0)
1829                 return p2p_ie_ptr;
1830
1831         cnt = 0;
1832
1833         while (cnt + 1 + 4 < in_len) {
1834                 eid = in_ie[cnt];
1835
1836                 if (cnt + 1 + 4 >= MAX_IE_SZ) {
1837                         rtw_warn_on(1);
1838                         return NULL;
1839                 }
1840
1841                 if (eid == WLAN_EID_VENDOR_SPECIFIC && _rtw_memcmp(&in_ie[cnt + 2], p2p_oui, 4) == _TRUE) {
1842                         p2p_ie_ptr = in_ie + cnt;
1843
1844                         if (p2p_ie)
1845                                 _rtw_memcpy(p2p_ie, &in_ie[cnt], in_ie[cnt + 1] + 2);
1846
1847                         if (p2p_ielen)
1848                                 *p2p_ielen = in_ie[cnt + 1] + 2;
1849
1850                         break;
1851                 } else
1852                         cnt += in_ie[cnt + 1] + 2;
1853
1854         }
1855
1856         return p2p_ie_ptr;
1857 }
1858
1859 /**
1860  * rtw_get_p2p_attr - Search a specific P2P attribute from a given P2P IE
1861  * @p2p_ie: Address of P2P IE to search
1862  * @p2p_ielen: Length limit from p2p_ie
1863  * @target_attr_id: The attribute ID of P2P attribute to search
1864  * @buf_attr: If not NULL and the P2P attribute is found, P2P attribute will be copied to the buf starting from buf_attr
1865  * @len_attr: If not NULL and the P2P attribute is found, will set to the length of the entire P2P attribute
1866  *
1867  * Returns: the address of the specific WPS attribute found, or NULL
1868  */
1869 u8 *rtw_get_p2p_attr(u8 *p2p_ie, uint p2p_ielen, u8 target_attr_id , u8 *buf_attr, u32 *len_attr)
1870 {
1871         u8 *attr_ptr = NULL;
1872         u8 *target_attr_ptr = NULL;
1873         u8 p2p_oui[4] = {0x50, 0x6F, 0x9A, 0x09};
1874
1875         if (len_attr)
1876                 *len_attr = 0;
1877
1878         if (!p2p_ie
1879             || p2p_ielen <= 6
1880             || (p2p_ie[0] != WLAN_EID_VENDOR_SPECIFIC)
1881             || (_rtw_memcmp(p2p_ie + 2, p2p_oui, 4) != _TRUE))
1882                 return attr_ptr;
1883
1884         /* 6 = 1(Element ID) + 1(Length) + 3 (OUI) + 1(OUI Type) */
1885         attr_ptr = p2p_ie + 6; /* goto first attr */
1886
1887         while ((attr_ptr - p2p_ie + 3) <= p2p_ielen) {
1888                 /* 3 = 1(Attribute ID) + 2(Length) */
1889                 u8 attr_id = *attr_ptr;
1890                 u16 attr_data_len = RTW_GET_LE16(attr_ptr + 1);
1891                 u16 attr_len = attr_data_len + 3;
1892
1893                 if (0)
1894                         RTW_INFO("%s attr_ptr:%p, id:%u, length:%u\n", __func__, attr_ptr, attr_id, attr_data_len);
1895
1896                 if ((attr_ptr - p2p_ie + attr_len) > p2p_ielen)
1897                         break;
1898
1899                 if (attr_id == target_attr_id) {
1900                         target_attr_ptr = attr_ptr;
1901
1902                         if (buf_attr)
1903                                 _rtw_memcpy(buf_attr, attr_ptr, attr_len);
1904
1905                         if (len_attr)
1906                                 *len_attr = attr_len;
1907
1908                         break;
1909                 } else
1910                         attr_ptr += attr_len;
1911         }
1912
1913         return target_attr_ptr;
1914 }
1915
1916 /**
1917  * rtw_get_p2p_attr_content - Search a specific P2P attribute content from a given P2P IE
1918  * @p2p_ie: Address of P2P IE to search
1919  * @p2p_ielen: Length limit from p2p_ie
1920  * @target_attr_id: The attribute ID of P2P attribute to search
1921  * @buf_content: If not NULL and the P2P attribute is found, P2P attribute content will be copied to the buf starting from buf_content
1922  * @len_content: If not NULL and the P2P attribute is found, will set to the length of the P2P attribute content
1923  *
1924  * Returns: the address of the specific P2P attribute content found, or NULL
1925  */
1926 u8 *rtw_get_p2p_attr_content(u8 *p2p_ie, uint p2p_ielen, u8 target_attr_id , u8 *buf_content, uint *len_content)
1927 {
1928         u8 *attr_ptr;
1929         u32 attr_len;
1930
1931         if (len_content)
1932                 *len_content = 0;
1933
1934         attr_ptr = rtw_get_p2p_attr(p2p_ie, p2p_ielen, target_attr_id, NULL, &attr_len);
1935
1936         if (attr_ptr && attr_len) {
1937                 if (buf_content)
1938                         _rtw_memcpy(buf_content, attr_ptr + 3, attr_len - 3);
1939
1940                 if (len_content)
1941                         *len_content = attr_len - 3;
1942
1943                 return attr_ptr + 3;
1944         }
1945
1946         return NULL;
1947 }
1948
1949 u32 rtw_set_p2p_attr_content(u8 *pbuf, u8 attr_id, u16 attr_len, u8 *pdata_attr)
1950 {
1951         u32 a_len;
1952
1953         *pbuf = attr_id;
1954
1955         /* *(u16*)(pbuf + 1) = cpu_to_le16(attr_len); */
1956         RTW_PUT_LE16(pbuf + 1, attr_len);
1957
1958         if (pdata_attr)
1959                 _rtw_memcpy(pbuf + 3, pdata_attr, attr_len);
1960
1961         a_len = attr_len + 3;
1962
1963         return a_len;
1964 }
1965
1966 uint rtw_del_p2p_ie(u8 *ies, uint ies_len_ori, const char *msg)
1967 {
1968 #define DBG_DEL_P2P_IE 0
1969
1970         u8 *target_ie;
1971         u32 target_ie_len;
1972         uint ies_len = ies_len_ori;
1973         int index = 0;
1974
1975         while (1) {
1976                 target_ie = rtw_get_p2p_ie(ies, ies_len, NULL, &target_ie_len);
1977                 if (target_ie && target_ie_len) {
1978                         u8 *next_ie = target_ie + target_ie_len;
1979                         uint remain_len = ies_len - (next_ie - ies);
1980
1981                         if (DBG_DEL_P2P_IE && msg) {
1982                                 RTW_INFO("%s %d before\n", __func__, index);
1983                                 dump_ies(RTW_DBGDUMP, ies, ies_len);
1984
1985                                 RTW_INFO("ies:%p, ies_len:%u\n", ies, ies_len);
1986                                 RTW_INFO("target_ie:%p, target_ie_len:%u\n", target_ie, target_ie_len);
1987                                 RTW_INFO("next_ie:%p, remain_len:%u\n", next_ie, remain_len);
1988                         }
1989
1990                         _rtw_memmove(target_ie, next_ie, remain_len);
1991                         _rtw_memset(target_ie + remain_len, 0, target_ie_len);
1992                         ies_len -= target_ie_len;
1993
1994                         if (DBG_DEL_P2P_IE && msg) {
1995                                 RTW_INFO("%s %d after\n", __func__, index);
1996                                 dump_ies(RTW_DBGDUMP, ies, ies_len);
1997                         }
1998
1999                         index++;
2000                 } else
2001                         break;
2002         }
2003
2004         return ies_len;
2005 }
2006
2007 uint rtw_del_p2p_attr(u8 *ie, uint ielen_ori, u8 attr_id)
2008 {
2009 #define DBG_DEL_P2P_ATTR 0
2010
2011         u8 *target_attr;
2012         u32 target_attr_len;
2013         uint ielen = ielen_ori;
2014         int index = 0;
2015
2016         while (1) {
2017                 target_attr = rtw_get_p2p_attr(ie, ielen, attr_id, NULL, &target_attr_len);
2018                 if (target_attr && target_attr_len) {
2019                         u8 *next_attr = target_attr + target_attr_len;
2020                         uint remain_len = ielen - (next_attr - ie);
2021
2022                         if (DBG_DEL_P2P_ATTR) {
2023                                 RTW_INFO("%s %d before\n", __func__, index);
2024                                 dump_ies(RTW_DBGDUMP, ie, ielen);
2025
2026                                 RTW_INFO("ie:%p, ielen:%u\n", ie, ielen);
2027                                 RTW_INFO("target_attr:%p, target_attr_len:%u\n", target_attr, target_attr_len);
2028                                 RTW_INFO("next_attr:%p, remain_len:%u\n", next_attr, remain_len);
2029                         }
2030
2031                         _rtw_memmove(target_attr, next_attr, remain_len);
2032                         _rtw_memset(target_attr + remain_len, 0, target_attr_len);
2033                         *(ie + 1) -= target_attr_len;
2034                         ielen -= target_attr_len;
2035
2036                         if (DBG_DEL_P2P_ATTR) {
2037                                 RTW_INFO("%s %d after\n", __func__, index);
2038                                 dump_ies(RTW_DBGDUMP, ie, ielen);
2039                         }
2040
2041                         index++;
2042                 } else
2043                         break;
2044         }
2045
2046         return ielen;
2047 }
2048
2049 inline u8 *rtw_bss_ex_get_p2p_ie(WLAN_BSSID_EX *bss_ex, u8 *p2p_ie, uint *p2p_ielen)
2050 {
2051         return rtw_get_p2p_ie(BSS_EX_TLV_IES(bss_ex), BSS_EX_TLV_IES_LEN(bss_ex), p2p_ie, p2p_ielen);
2052 }
2053
2054 void rtw_bss_ex_del_p2p_ie(WLAN_BSSID_EX *bss_ex)
2055 {
2056 #define DBG_BSS_EX_DEL_P2P_IE 0
2057
2058         u8 *ies = BSS_EX_TLV_IES(bss_ex);
2059         uint ies_len_ori = BSS_EX_TLV_IES_LEN(bss_ex);
2060         uint ies_len;
2061
2062         ies_len = rtw_del_p2p_ie(ies, ies_len_ori, DBG_BSS_EX_DEL_P2P_IE ? __func__ : NULL);
2063         bss_ex->IELength -= ies_len_ori - ies_len;
2064 }
2065
2066 void rtw_bss_ex_del_p2p_attr(WLAN_BSSID_EX *bss_ex, u8 attr_id)
2067 {
2068 #define DBG_BSS_EX_DEL_P2P_ATTR 0
2069
2070         u8 *ies = BSS_EX_TLV_IES(bss_ex);
2071         uint ies_len = BSS_EX_TLV_IES_LEN(bss_ex);
2072
2073         u8 *ie;
2074         uint ie_len, ie_len_ori;
2075
2076         int index = 0;
2077
2078         while (1) {
2079                 ie = rtw_get_p2p_ie(ies, ies_len, NULL, &ie_len_ori);
2080                 if (ie) {
2081                         u8 *next_ie_ori = ie + ie_len_ori;
2082                         uint remain_len = bss_ex->IELength - (next_ie_ori - bss_ex->IEs);
2083                         u8 has_target_attr = 0;
2084
2085                         if (DBG_BSS_EX_DEL_P2P_ATTR) {
2086                                 if (rtw_get_p2p_attr(ie, ie_len_ori, attr_id, NULL, NULL)) {
2087                                         RTW_INFO("%s %d before\n", __func__, index);
2088                                         dump_ies(RTW_DBGDUMP, BSS_EX_TLV_IES(bss_ex), BSS_EX_TLV_IES_LEN(bss_ex));
2089
2090                                         RTW_INFO("ies:%p, ies_len:%u\n", ies, ies_len);
2091                                         RTW_INFO("ie:%p, ie_len_ori:%u\n", ie, ie_len_ori);
2092                                         RTW_INFO("next_ie_ori:%p, remain_len:%u\n", next_ie_ori, remain_len);
2093                                         has_target_attr = 1;
2094                                 }
2095                         }
2096
2097                         ie_len = rtw_del_p2p_attr(ie, ie_len_ori, attr_id);
2098                         if (ie_len != ie_len_ori) {
2099                                 u8 *next_ie = ie + ie_len;
2100
2101                                 _rtw_memmove(next_ie, next_ie_ori, remain_len);
2102                                 _rtw_memset(next_ie + remain_len, 0, ie_len_ori - ie_len);
2103                                 bss_ex->IELength -= ie_len_ori - ie_len;
2104
2105                                 ies = next_ie;
2106                         } else
2107                                 ies = next_ie_ori;
2108
2109                         if (DBG_BSS_EX_DEL_P2P_ATTR) {
2110                                 if (has_target_attr) {
2111                                         RTW_INFO("%s %d after\n", __func__, index);
2112                                         dump_ies(RTW_DBGDUMP, BSS_EX_TLV_IES(bss_ex), BSS_EX_TLV_IES_LEN(bss_ex));
2113                                 }
2114                         }
2115
2116                         ies_len = remain_len;
2117
2118                         index++;
2119                 } else
2120                         break;
2121         }
2122 }
2123
2124 void dump_wfd_ie(void *sel, u8 *ie, u32 ie_len)
2125 {
2126         u8 *pos = (u8 *)ie;
2127         u8 id;
2128         u16 len;
2129
2130         u8 *wfd_ie;
2131         uint wfd_ielen;
2132
2133         wfd_ie = rtw_get_wfd_ie(ie, ie_len, NULL, &wfd_ielen);
2134         if (wfd_ie != ie || wfd_ielen == 0)
2135                 return;
2136
2137         pos += 6;
2138         while (pos - ie + 3 <= ie_len) {
2139                 id = *pos;
2140                 len = RTW_GET_BE16(pos + 1);
2141
2142                 RTW_PRINT_SEL(sel, "%s ID:%u, LEN:%u%s\n", __func__, id, len
2143                         , ((pos - ie + 3 + len) <= ie_len) ? "" : "(exceed ie_len)");
2144
2145                 pos += (3 + len);
2146         }
2147 }
2148
2149 /**
2150  * rtw_get_wfd_ie - Search WFD IE from a series of IEs
2151  * @in_ie: Address of IEs to search
2152  * @in_len: Length limit from in_ie
2153  * @wfd_ie: If not NULL and WFD IE is found, WFD IE will be copied to the buf starting from wfd_ie
2154  * @wfd_ielen: If not NULL and WFD IE is found, will set to the length of the entire WFD IE
2155  *
2156  * Returns: The address of the P2P IE found, or NULL
2157  */
2158 u8 *rtw_get_wfd_ie(u8 *in_ie, int in_len, u8 *wfd_ie, uint *wfd_ielen)
2159 {
2160         uint cnt;
2161         u8 *wfd_ie_ptr = NULL;
2162         u8 eid, wfd_oui[4] = {0x50, 0x6F, 0x9A, 0x0A};
2163
2164         if (wfd_ielen)
2165                 *wfd_ielen = 0;
2166
2167         if (!in_ie || in_len < 0) {
2168                 rtw_warn_on(1);
2169                 return wfd_ie_ptr;
2170         }
2171
2172         if (in_len <= 0)
2173                 return wfd_ie_ptr;
2174
2175         cnt = 0;
2176
2177         while (cnt + 1 + 4 < in_len) {
2178                 eid = in_ie[cnt];
2179
2180                 if (cnt + 1 + 4 >= MAX_IE_SZ) {
2181                         rtw_warn_on(1);
2182                         return NULL;
2183                 }
2184
2185                 if (eid == WLAN_EID_VENDOR_SPECIFIC && _rtw_memcmp(&in_ie[cnt + 2], wfd_oui, 4) == _TRUE) {
2186                         wfd_ie_ptr = in_ie + cnt;
2187
2188                         if (wfd_ie)
2189                                 _rtw_memcpy(wfd_ie, &in_ie[cnt], in_ie[cnt + 1] + 2);
2190
2191                         if (wfd_ielen)
2192                                 *wfd_ielen = in_ie[cnt + 1] + 2;
2193
2194                         break;
2195                 } else
2196                         cnt += in_ie[cnt + 1] + 2;
2197
2198         }
2199
2200         return wfd_ie_ptr;
2201 }
2202
2203 /**
2204  * rtw_get_wfd_attr - Search a specific WFD attribute from a given WFD IE
2205  * @wfd_ie: Address of WFD IE to search
2206  * @wfd_ielen: Length limit from wfd_ie
2207  * @target_attr_id: The attribute ID of WFD attribute to search
2208  * @buf_attr: If not NULL and the WFD attribute is found, WFD attribute will be copied to the buf starting from buf_attr
2209  * @len_attr: If not NULL and the WFD attribute is found, will set to the length of the entire WFD attribute
2210  *
2211  * Returns: the address of the specific WPS attribute found, or NULL
2212  */
2213 u8 *rtw_get_wfd_attr(u8 *wfd_ie, uint wfd_ielen, u8 target_attr_id, u8 *buf_attr, u32 *len_attr)
2214 {
2215         u8 *attr_ptr = NULL;
2216         u8 *target_attr_ptr = NULL;
2217         u8 wfd_oui[4] = {0x50, 0x6F, 0x9A, 0x0A};
2218
2219         if (len_attr)
2220                 *len_attr = 0;
2221
2222         if (!wfd_ie
2223             || wfd_ielen <= 6
2224             || (wfd_ie[0] != WLAN_EID_VENDOR_SPECIFIC)
2225             || (_rtw_memcmp(wfd_ie + 2, wfd_oui, 4) != _TRUE))
2226                 return attr_ptr;
2227
2228         /* 6 = 1(Element ID) + 1(Length) + 3 (OUI) + 1(OUI Type) */
2229         attr_ptr = wfd_ie + 6; /* goto first attr */
2230
2231         while ((attr_ptr - wfd_ie + 3) <= wfd_ielen) {
2232                 /* 3 = 1(Attribute ID) + 2(Length) */
2233                 u8 attr_id = *attr_ptr;
2234                 u16 attr_data_len = RTW_GET_BE16(attr_ptr + 1);
2235                 u16 attr_len = attr_data_len + 3;
2236
2237                 if (0)
2238                         RTW_INFO("%s attr_ptr:%p, id:%u, length:%u\n", __func__, attr_ptr, attr_id, attr_data_len);
2239
2240                 if ((attr_ptr - wfd_ie + attr_len) > wfd_ielen)
2241                         break;
2242
2243                 if (attr_id == target_attr_id) {
2244                         target_attr_ptr = attr_ptr;
2245
2246                         if (buf_attr)
2247                                 _rtw_memcpy(buf_attr, attr_ptr, attr_len);
2248
2249                         if (len_attr)
2250                                 *len_attr = attr_len;
2251
2252                         break;
2253                 } else
2254                         attr_ptr += attr_len;
2255         }
2256
2257         return target_attr_ptr;
2258 }
2259
2260 /**
2261  * rtw_get_wfd_attr_content - Search a specific WFD attribute content from a given WFD IE
2262  * @wfd_ie: Address of WFD IE to search
2263  * @wfd_ielen: Length limit from wfd_ie
2264  * @target_attr_id: The attribute ID of WFD attribute to search
2265  * @buf_content: If not NULL and the WFD attribute is found, WFD attribute content will be copied to the buf starting from buf_content
2266  * @len_content: If not NULL and the WFD attribute is found, will set to the length of the WFD attribute content
2267  *
2268  * Returns: the address of the specific WFD attribute content found, or NULL
2269  */
2270 u8 *rtw_get_wfd_attr_content(u8 *wfd_ie, uint wfd_ielen, u8 target_attr_id, u8 *buf_content, uint *len_content)
2271 {
2272         u8 *attr_ptr;
2273         u32 attr_len;
2274
2275         if (len_content)
2276                 *len_content = 0;
2277
2278         attr_ptr = rtw_get_wfd_attr(wfd_ie, wfd_ielen, target_attr_id, NULL, &attr_len);
2279
2280         if (attr_ptr && attr_len) {
2281                 if (buf_content)
2282                         _rtw_memcpy(buf_content, attr_ptr + 3, attr_len - 3);
2283
2284                 if (len_content)
2285                         *len_content = attr_len - 3;
2286
2287                 return attr_ptr + 3;
2288         }
2289
2290         return NULL;
2291 }
2292
2293 uint rtw_del_wfd_ie(u8 *ies, uint ies_len_ori, const char *msg)
2294 {
2295 #define DBG_DEL_WFD_IE 0
2296
2297         u8 *target_ie;
2298         u32 target_ie_len;
2299         uint ies_len = ies_len_ori;
2300         int index = 0;
2301
2302         while (1) {
2303                 target_ie = rtw_get_wfd_ie(ies, ies_len, NULL, &target_ie_len);
2304                 if (target_ie && target_ie_len) {
2305                         u8 *next_ie = target_ie + target_ie_len;
2306                         uint remain_len = ies_len - (next_ie - ies);
2307
2308                         if (DBG_DEL_WFD_IE && msg) {
2309                                 RTW_INFO("%s %d before\n", __func__, index);
2310                                 dump_ies(RTW_DBGDUMP, ies, ies_len);
2311
2312                                 RTW_INFO("ies:%p, ies_len:%u\n", ies, ies_len);
2313                                 RTW_INFO("target_ie:%p, target_ie_len:%u\n", target_ie, target_ie_len);
2314                                 RTW_INFO("next_ie:%p, remain_len:%u\n", next_ie, remain_len);
2315                         }
2316
2317                         _rtw_memmove(target_ie, next_ie, remain_len);
2318                         _rtw_memset(target_ie + remain_len, 0, target_ie_len);
2319                         ies_len -= target_ie_len;
2320
2321                         if (DBG_DEL_WFD_IE && msg) {
2322                                 RTW_INFO("%s %d after\n", __func__, index);
2323                                 dump_ies(RTW_DBGDUMP, ies, ies_len);
2324                         }
2325
2326                         index++;
2327                 } else
2328                         break;
2329         }
2330
2331         return ies_len;
2332 }
2333
2334 uint rtw_del_wfd_attr(u8 *ie, uint ielen_ori, u8 attr_id)
2335 {
2336 #define DBG_DEL_WFD_ATTR 0
2337
2338         u8 *target_attr;
2339         u32 target_attr_len;
2340         uint ielen = ielen_ori;
2341         int index = 0;
2342
2343         while (1) {
2344                 target_attr = rtw_get_wfd_attr(ie, ielen, attr_id, NULL, &target_attr_len);
2345                 if (target_attr && target_attr_len) {
2346                         u8 *next_attr = target_attr + target_attr_len;
2347                         uint remain_len = ielen - (next_attr - ie);
2348
2349                         if (DBG_DEL_WFD_ATTR) {
2350                                 RTW_INFO("%s %d before\n", __func__, index);
2351                                 dump_ies(RTW_DBGDUMP, ie, ielen);
2352
2353                                 RTW_INFO("ie:%p, ielen:%u\n", ie, ielen);
2354                                 RTW_INFO("target_attr:%p, target_attr_len:%u\n", target_attr, target_attr_len);
2355                                 RTW_INFO("next_attr:%p, remain_len:%u\n", next_attr, remain_len);
2356                         }
2357
2358                         _rtw_memmove(target_attr, next_attr, remain_len);
2359                         _rtw_memset(target_attr + remain_len, 0, target_attr_len);
2360                         *(ie + 1) -= target_attr_len;
2361                         ielen -= target_attr_len;
2362
2363                         if (DBG_DEL_WFD_ATTR) {
2364                                 RTW_INFO("%s %d after\n", __func__, index);
2365                                 dump_ies(RTW_DBGDUMP, ie, ielen);
2366                         }
2367
2368                         index++;
2369                 } else
2370                         break;
2371         }
2372
2373         return ielen;
2374 }
2375
2376 inline u8 *rtw_bss_ex_get_wfd_ie(WLAN_BSSID_EX *bss_ex, u8 *wfd_ie, uint *wfd_ielen)
2377 {
2378         return rtw_get_wfd_ie(BSS_EX_TLV_IES(bss_ex), BSS_EX_TLV_IES_LEN(bss_ex), wfd_ie, wfd_ielen);
2379 }
2380
2381 void rtw_bss_ex_del_wfd_ie(WLAN_BSSID_EX *bss_ex)
2382 {
2383 #define DBG_BSS_EX_DEL_WFD_IE 0
2384         u8 *ies = BSS_EX_TLV_IES(bss_ex);
2385         uint ies_len_ori = BSS_EX_TLV_IES_LEN(bss_ex);
2386         uint ies_len;
2387
2388         ies_len = rtw_del_wfd_ie(ies, ies_len_ori, DBG_BSS_EX_DEL_WFD_IE ? __func__ : NULL);
2389         bss_ex->IELength -= ies_len_ori - ies_len;
2390 }
2391
2392 void rtw_bss_ex_del_wfd_attr(WLAN_BSSID_EX *bss_ex, u8 attr_id)
2393 {
2394 #define DBG_BSS_EX_DEL_WFD_ATTR 0
2395
2396         u8 *ies = BSS_EX_TLV_IES(bss_ex);
2397         uint ies_len = BSS_EX_TLV_IES_LEN(bss_ex);
2398
2399         u8 *ie;
2400         uint ie_len, ie_len_ori;
2401
2402         int index = 0;
2403
2404         while (1) {
2405                 ie = rtw_get_wfd_ie(ies, ies_len, NULL, &ie_len_ori);
2406                 if (ie) {
2407                         u8 *next_ie_ori = ie + ie_len_ori;
2408                         uint remain_len = bss_ex->IELength - (next_ie_ori - bss_ex->IEs);
2409                         u8 has_target_attr = 0;
2410
2411                         if (DBG_BSS_EX_DEL_WFD_ATTR) {
2412                                 if (rtw_get_wfd_attr(ie, ie_len_ori, attr_id, NULL, NULL)) {
2413                                         RTW_INFO("%s %d before\n", __func__, index);
2414                                         dump_ies(RTW_DBGDUMP, BSS_EX_TLV_IES(bss_ex), BSS_EX_TLV_IES_LEN(bss_ex));
2415
2416                                         RTW_INFO("ies:%p, ies_len:%u\n", ies, ies_len);
2417                                         RTW_INFO("ie:%p, ie_len_ori:%u\n", ie, ie_len_ori);
2418                                         RTW_INFO("next_ie_ori:%p, remain_len:%u\n", next_ie_ori, remain_len);
2419                                         has_target_attr = 1;
2420                                 }
2421                         }
2422
2423                         ie_len = rtw_del_wfd_attr(ie, ie_len_ori, attr_id);
2424                         if (ie_len != ie_len_ori) {
2425                                 u8 *next_ie = ie + ie_len;
2426
2427                                 _rtw_memmove(next_ie, next_ie_ori, remain_len);
2428                                 _rtw_memset(next_ie + remain_len, 0, ie_len_ori - ie_len);
2429                                 bss_ex->IELength -= ie_len_ori - ie_len;
2430
2431                                 ies = next_ie;
2432                         } else
2433                                 ies = next_ie_ori;
2434
2435                         if (DBG_BSS_EX_DEL_WFD_ATTR) {
2436                                 if (has_target_attr) {
2437                                         RTW_INFO("%s %d after\n", __func__, index);
2438                                         dump_ies(RTW_DBGDUMP, BSS_EX_TLV_IES(bss_ex), BSS_EX_TLV_IES_LEN(bss_ex));
2439                                 }
2440                         }
2441
2442                         ies_len = remain_len;
2443
2444                         index++;
2445                 } else
2446                         break;
2447         }
2448 }
2449
2450 /* Baron adds to avoid FreeBSD warning */
2451 int ieee80211_is_empty_essid(const char *essid, int essid_len)
2452 {
2453         /* Single white space is for Linksys APs */
2454         if (essid_len == 1 && essid[0] == ' ')
2455                 return 1;
2456
2457         /* Otherwise, if the entire essid is 0, we assume it is hidden */
2458         while (essid_len) {
2459                 essid_len--;
2460                 if (essid[essid_len] != '\0')
2461                         return 0;
2462         }
2463
2464         return 1;
2465 }
2466
2467 int ieee80211_get_hdrlen(u16 fc)
2468 {
2469         int hdrlen = 24;
2470
2471         switch (WLAN_FC_GET_TYPE(fc)) {
2472         case RTW_IEEE80211_FTYPE_DATA:
2473                 if (fc & RTW_IEEE80211_STYPE_QOS_DATA)
2474                         hdrlen += 2;
2475                 if ((fc & RTW_IEEE80211_FCTL_FROMDS) && (fc & RTW_IEEE80211_FCTL_TODS))
2476                         hdrlen += 6; /* Addr4 */
2477                 break;
2478         case RTW_IEEE80211_FTYPE_CTL:
2479                 switch (WLAN_FC_GET_STYPE(fc)) {
2480                 case RTW_IEEE80211_STYPE_CTS:
2481                 case RTW_IEEE80211_STYPE_ACK:
2482                         hdrlen = 10;
2483                         break;
2484                 default:
2485                         hdrlen = 16;
2486                         break;
2487                 }
2488                 break;
2489         }
2490
2491         return hdrlen;
2492 }
2493
2494 int rtw_get_cipher_info(struct wlan_network *pnetwork)
2495 {
2496         u32 wpa_ielen;
2497         unsigned char *pbuf;
2498         int group_cipher = 0, pairwise_cipher = 0, is8021x = 0;
2499         int ret = _FAIL;
2500         pbuf = rtw_get_wpa_ie(&pnetwork->network.IEs[12], &wpa_ielen, pnetwork->network.IELength - 12);
2501
2502         if (pbuf && (wpa_ielen > 0)) {
2503                 if (_SUCCESS == rtw_parse_wpa_ie(pbuf, wpa_ielen + 2, &group_cipher, &pairwise_cipher, &is8021x)) {
2504
2505                         pnetwork->BcnInfo.pairwise_cipher = pairwise_cipher;
2506                         pnetwork->BcnInfo.group_cipher = group_cipher;
2507                         pnetwork->BcnInfo.is_8021x = is8021x;
2508                         ret = _SUCCESS;
2509                 }
2510         } else {
2511
2512                 pbuf = rtw_get_wpa2_ie(&pnetwork->network.IEs[12], &wpa_ielen, pnetwork->network.IELength - 12);
2513
2514                 if (pbuf && (wpa_ielen > 0)) {
2515                         if (_SUCCESS == rtw_parse_wpa2_ie(pbuf, wpa_ielen + 2, &group_cipher, &pairwise_cipher, &is8021x)) {
2516                                 pnetwork->BcnInfo.pairwise_cipher = pairwise_cipher;
2517                                 pnetwork->BcnInfo.group_cipher = group_cipher;
2518                                 pnetwork->BcnInfo.is_8021x = is8021x;
2519                                 ret = _SUCCESS;
2520                         }
2521                 }
2522         }
2523
2524         return ret;
2525 }
2526
2527 void rtw_get_bcn_info(struct wlan_network *pnetwork)
2528 {
2529         unsigned short cap = 0;
2530         u8 bencrypt = 0;
2531         /* u8 wpa_ie[255],rsn_ie[255]; */
2532         u16 wpa_len = 0, rsn_len = 0;
2533         struct HT_info_element *pht_info = NULL;
2534         struct rtw_ieee80211_ht_cap *pht_cap = NULL;
2535         unsigned int            len;
2536         unsigned char           *p;
2537
2538         _rtw_memcpy((u8 *)&cap, rtw_get_capability_from_ie(pnetwork->network.IEs), 2);
2539         cap = le16_to_cpu(cap);
2540         if (cap & WLAN_CAPABILITY_PRIVACY) {
2541                 bencrypt = 1;
2542                 pnetwork->network.Privacy = 1;
2543         } else
2544                 pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_OPENSYS;
2545         rtw_get_sec_ie(pnetwork->network.IEs , pnetwork->network.IELength, NULL, &rsn_len, NULL, &wpa_len);
2546
2547         if (rsn_len > 0)
2548                 pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_WPA2;
2549         else if (wpa_len > 0)
2550                 pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_WPA;
2551         else {
2552                 if (bencrypt)
2553                         pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_WEP;
2554         }
2555         rtw_get_cipher_info(pnetwork);
2556
2557         /* get bwmode and ch_offset */
2558         /* parsing HT_CAP_IE */
2559         p = rtw_get_ie(pnetwork->network.IEs + _FIXED_IE_LENGTH_, _HT_CAPABILITY_IE_, &len, pnetwork->network.IELength - _FIXED_IE_LENGTH_);
2560         if (p && len > 0) {
2561                 pht_cap = (struct rtw_ieee80211_ht_cap *)(p + 2);
2562                 pnetwork->BcnInfo.ht_cap_info = pht_cap->cap_info;
2563         } else
2564                 pnetwork->BcnInfo.ht_cap_info = 0;
2565         /* parsing HT_INFO_IE */
2566         p = rtw_get_ie(pnetwork->network.IEs + _FIXED_IE_LENGTH_, _HT_ADD_INFO_IE_, &len, pnetwork->network.IELength - _FIXED_IE_LENGTH_);
2567         if (p && len > 0) {
2568                 pht_info = (struct HT_info_element *)(p + 2);
2569                 pnetwork->BcnInfo.ht_info_infos_0 = pht_info->infos[0];
2570         } else
2571                 pnetwork->BcnInfo.ht_info_infos_0 = 0;
2572 }
2573
2574 u8      rtw_ht_mcsset_to_nss(u8 *supp_mcs_set)
2575 {
2576         u8 nss = 1;
2577
2578         if (supp_mcs_set[3])
2579                 nss = 4;
2580         else if (supp_mcs_set[2])
2581                 nss = 3;
2582         else if (supp_mcs_set[1])
2583                 nss = 2;
2584         else if (supp_mcs_set[0])
2585                 nss = 1;
2586         else
2587                 RTW_INFO("%s,%d, warning! supp_mcs_set is zero\n", __func__, __LINE__);
2588         /* RTW_INFO("%s HT: %dSS\n", __FUNCTION__, nss); */
2589         return nss;
2590 }
2591
2592 u32     rtw_ht_mcs_set_to_bitmap(u8 *mcs_set, u8 nss)
2593 {
2594         u8 i;
2595         u32 bitmap = 0;
2596
2597         for (i = 0; i < nss; i++)
2598                 bitmap |= mcs_set[i] << (i * 8);
2599
2600         RTW_INFO("ht_mcs_set=%02x %02x %02x %02x, nss=%u, bitmap=%08x\n"
2601                 , mcs_set[0], mcs_set[1], mcs_set[2], mcs_set[3], nss, bitmap);
2602
2603         return bitmap;
2604 }
2605
2606 /* show MCS rate, unit: 100Kbps */
2607 u16 rtw_mcs_rate(u8 rf_type, u8 bw_40MHz, u8 short_GI, unsigned char *MCS_rate)
2608 {
2609         u16 max_rate = 0;
2610
2611         if (MCS_rate[3]) {
2612                 if (MCS_rate[3] & BIT(7))
2613                         max_rate = (bw_40MHz) ? ((short_GI) ? 6000 : 5400) : ((short_GI) ? 2889 : 2600);
2614                 else if (MCS_rate[3] & BIT(6))
2615                         max_rate = (bw_40MHz) ? ((short_GI) ? 5400 : 4860) : ((short_GI) ? 2600 : 2340);
2616                 else if (MCS_rate[3] & BIT(5))
2617                         max_rate = (bw_40MHz) ? ((short_GI) ? 4800 : 4320) : ((short_GI) ? 2311 : 2080);
2618                 else if (MCS_rate[3] & BIT(4))
2619                         max_rate = (bw_40MHz) ? ((short_GI) ? 3600 : 3240) : ((short_GI) ? 1733 : 1560);
2620                 else if (MCS_rate[3] & BIT(3))
2621                         max_rate = (bw_40MHz) ? ((short_GI) ? 2400 : 2160) : ((short_GI) ? 1156 : 1040);
2622                 else if (MCS_rate[3] & BIT(2))
2623                         max_rate = (bw_40MHz) ? ((short_GI) ? 1800 : 1620) : ((short_GI) ? 867 : 780);
2624                 else if (MCS_rate[3] & BIT(1))
2625                         max_rate = (bw_40MHz) ? ((short_GI) ? 1200 : 1080) : ((short_GI) ? 578 : 520);
2626                 else if (MCS_rate[3] & BIT(0))
2627                         max_rate = (bw_40MHz) ? ((short_GI) ? 600 : 540) : ((short_GI) ? 289 : 260);
2628         } else if (MCS_rate[2]) {
2629                 if (MCS_rate[2] & BIT(7))
2630                         max_rate = (bw_40MHz) ? ((short_GI) ? 4500 : 4050) : ((short_GI) ? 2167 : 1950);
2631                 else if (MCS_rate[2] & BIT(6))
2632                         max_rate = (bw_40MHz) ? ((short_GI) ? 4050 : 3645) : ((short_GI) ? 1950 : 1750);
2633                 else if (MCS_rate[2] & BIT(5))
2634                         max_rate = (bw_40MHz) ? ((short_GI) ? 3600 : 3240) : ((short_GI) ? 1733 : 1560);
2635                 else if (MCS_rate[2] & BIT(4))
2636                         max_rate = (bw_40MHz) ? ((short_GI) ? 2700 : 2430) : ((short_GI) ? 1300 : 1170);
2637                 else if (MCS_rate[2] & BIT(3))
2638                         max_rate = (bw_40MHz) ? ((short_GI) ? 1800 : 1620) : ((short_GI) ? 867 : 780);
2639                 else if (MCS_rate[2] & BIT(2))
2640                         max_rate = (bw_40MHz) ? ((short_GI) ? 1350 : 1215) : ((short_GI) ? 650 : 585);
2641                 else if (MCS_rate[2] & BIT(1))
2642                         max_rate = (bw_40MHz) ? ((short_GI) ? 900 : 810) : ((short_GI) ? 433 : 390);
2643                 else if (MCS_rate[2] & BIT(0))
2644                         max_rate = (bw_40MHz) ? ((short_GI) ? 450 : 405) : ((short_GI) ? 217 : 195);
2645         } else if (MCS_rate[1]) {
2646                 if (MCS_rate[1] & BIT(7))
2647                         max_rate = (bw_40MHz) ? ((short_GI) ? 3000 : 2700) : ((short_GI) ? 1444 : 1300);
2648                 else if (MCS_rate[1] & BIT(6))
2649                         max_rate = (bw_40MHz) ? ((short_GI) ? 2700 : 2430) : ((short_GI) ? 1300 : 1170);
2650                 else if (MCS_rate[1] & BIT(5))
2651                         max_rate = (bw_40MHz) ? ((short_GI) ? 2400 : 2160) : ((short_GI) ? 1156 : 1040);
2652                 else if (MCS_rate[1] & BIT(4))
2653                         max_rate = (bw_40MHz) ? ((short_GI) ? 1800 : 1620) : ((short_GI) ? 867 : 780);
2654                 else if (MCS_rate[1] & BIT(3))
2655                         max_rate = (bw_40MHz) ? ((short_GI) ? 1200 : 1080) : ((short_GI) ? 578 : 520);
2656                 else if (MCS_rate[1] & BIT(2))
2657                         max_rate = (bw_40MHz) ? ((short_GI) ? 900 : 810) : ((short_GI) ? 433 : 390);
2658                 else if (MCS_rate[1] & BIT(1))
2659                         max_rate = (bw_40MHz) ? ((short_GI) ? 600 : 540) : ((short_GI) ? 289 : 260);
2660                 else if (MCS_rate[1] & BIT(0))
2661                         max_rate = (bw_40MHz) ? ((short_GI) ? 300 : 270) : ((short_GI) ? 144 : 130);
2662         } else {
2663                 if (MCS_rate[0] & BIT(7))
2664                         max_rate = (bw_40MHz) ? ((short_GI) ? 1500 : 1350) : ((short_GI) ? 722 : 650);
2665                 else if (MCS_rate[0] & BIT(6))
2666                         max_rate = (bw_40MHz) ? ((short_GI) ? 1350 : 1215) : ((short_GI) ? 650 : 585);
2667                 else if (MCS_rate[0] & BIT(5))
2668                         max_rate = (bw_40MHz) ? ((short_GI) ? 1200 : 1080) : ((short_GI) ? 578 : 520);
2669                 else if (MCS_rate[0] & BIT(4))
2670                         max_rate = (bw_40MHz) ? ((short_GI) ? 900 : 810) : ((short_GI) ? 433 : 390);
2671                 else if (MCS_rate[0] & BIT(3))
2672                         max_rate = (bw_40MHz) ? ((short_GI) ? 600 : 540) : ((short_GI) ? 289 : 260);
2673                 else if (MCS_rate[0] & BIT(2))
2674                         max_rate = (bw_40MHz) ? ((short_GI) ? 450 : 405) : ((short_GI) ? 217 : 195);
2675                 else if (MCS_rate[0] & BIT(1))
2676                         max_rate = (bw_40MHz) ? ((short_GI) ? 300 : 270) : ((short_GI) ? 144 : 130);
2677                 else if (MCS_rate[0] & BIT(0))
2678                         max_rate = (bw_40MHz) ? ((short_GI) ? 150 : 135) : ((short_GI) ? 72 : 65);
2679         }
2680
2681         return max_rate;
2682 }
2683
2684 int rtw_action_frame_parse(const u8 *frame, u32 frame_len, u8 *category, u8 *action)
2685 {
2686         const u8 *frame_body = frame + sizeof(struct rtw_ieee80211_hdr_3addr);
2687         u16 fc;
2688         u8 c;
2689         u8 a = ACT_PUBLIC_MAX;
2690
2691         fc = le16_to_cpu(((struct rtw_ieee80211_hdr_3addr *)frame)->frame_ctl);
2692
2693         if ((fc & (RTW_IEEE80211_FCTL_FTYPE | RTW_IEEE80211_FCTL_STYPE))
2694             != (RTW_IEEE80211_FTYPE_MGMT | RTW_IEEE80211_STYPE_ACTION)
2695            )
2696                 return _FALSE;
2697
2698         c = frame_body[0];
2699
2700         switch (c) {
2701         case RTW_WLAN_CATEGORY_P2P: /* vendor-specific */
2702                 break;
2703         default:
2704                 a = frame_body[1];
2705         }
2706
2707         if (category)
2708                 *category = c;
2709         if (action)
2710                 *action = a;
2711
2712         return _TRUE;
2713 }
2714
2715 static const char *_action_public_str[] = {
2716         "ACT_PUB_BSSCOEXIST",
2717         "ACT_PUB_DSE_ENABLE",
2718         "ACT_PUB_DSE_DEENABLE",
2719         "ACT_PUB_DSE_REG_LOCATION",
2720         "ACT_PUB_EXT_CHL_SWITCH",
2721         "ACT_PUB_DSE_MSR_REQ",
2722         "ACT_PUB_DSE_MSR_RPRT",
2723         "ACT_PUB_MP",
2724         "ACT_PUB_DSE_PWR_CONSTRAINT",
2725         "ACT_PUB_VENDOR",
2726         "ACT_PUB_GAS_INITIAL_REQ",
2727         "ACT_PUB_GAS_INITIAL_RSP",
2728         "ACT_PUB_GAS_COMEBACK_REQ",
2729         "ACT_PUB_GAS_COMEBACK_RSP",
2730         "ACT_PUB_TDLS_DISCOVERY_RSP",
2731         "ACT_PUB_LOCATION_TRACK",
2732         "ACT_PUB_RSVD",
2733 };
2734
2735 const char *action_public_str(u8 action)
2736 {
2737         action = (action >= ACT_PUBLIC_MAX) ? ACT_PUBLIC_MAX : action;
2738         return _action_public_str[action];
2739 }
2740