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

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