kernel/drivers/staging/rtl8188eu/core/rtw_ieee80211.c

   1 /******************************************************************************
   2  *
   3  * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
   4  *
   5  * This program is free software; you can redistribute it and/or modify it
   6  * under the terms of version 2 of the GNU General Public License as
   7  * published by the Free Software Foundation.
   8  *
   9  * This program is distributed in the hope that it will be useful, but WITHOUT
  10  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  11  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
  12  * more details.
  13  *
  14  * You should have received a copy of the GNU General Public License along with
  15  * this program; if not, write to the Free Software Foundation, Inc.,
  16  * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
  17  *
  18  *
  19  ******************************************************************************/
  20 #define _IEEE80211_C
  21
  22 #include <linux/ieee80211.h>
  23
  24 #include <drv_types.h>
  25 #include <osdep_intf.h>
  26 #include <ieee80211.h>
  27 #include <wifi.h>
  28 #include <osdep_service.h>
  29 #include <wlan_bssdef.h>
  30
  31 u8 RTW_WPA_OUI_TYPE[] = { 0x00, 0x50, 0xf2, 1 };
  32 u8 WPA_AUTH_KEY_MGMT_NONE[] = { 0x00, 0x50, 0xf2, 0 };
  33 u8 WPA_AUTH_KEY_MGMT_UNSPEC_802_1X[] = { 0x00, 0x50, 0xf2, 1 };
  34 u8 WPA_AUTH_KEY_MGMT_PSK_OVER_802_1X[] = { 0x00, 0x50, 0xf2, 2 };
  35 u8 WPA_CIPHER_SUITE_NONE[] = { 0x00, 0x50, 0xf2, 0 };
  36 u8 WPA_CIPHER_SUITE_WEP40[] = { 0x00, 0x50, 0xf2, 1 };
  37 u8 WPA_CIPHER_SUITE_TKIP[] = { 0x00, 0x50, 0xf2, 2 };
  38 u8 WPA_CIPHER_SUITE_WRAP[] = { 0x00, 0x50, 0xf2, 3 };
  39 u8 WPA_CIPHER_SUITE_CCMP[] = { 0x00, 0x50, 0xf2, 4 };
  40 u8 WPA_CIPHER_SUITE_WEP104[] = { 0x00, 0x50, 0xf2, 5 };
  41
  42 u16 RSN_VERSION_BSD = 1;
  43 u8 RSN_AUTH_KEY_MGMT_UNSPEC_802_1X[] = { 0x00, 0x0f, 0xac, 1 };
  44 u8 RSN_AUTH_KEY_MGMT_PSK_OVER_802_1X[] = { 0x00, 0x0f, 0xac, 2 };
  45 u8 RSN_CIPHER_SUITE_NONE[] = { 0x00, 0x0f, 0xac, 0 };
  46 u8 RSN_CIPHER_SUITE_WEP40[] = { 0x00, 0x0f, 0xac, 1 };
  47 u8 RSN_CIPHER_SUITE_TKIP[] = { 0x00, 0x0f, 0xac, 2 };
  48 u8 RSN_CIPHER_SUITE_WRAP[] = { 0x00, 0x0f, 0xac, 3 };
  49 u8 RSN_CIPHER_SUITE_CCMP[] = { 0x00, 0x0f, 0xac, 4 };
  50 u8 RSN_CIPHER_SUITE_WEP104[] = { 0x00, 0x0f, 0xac, 5 };
  51 /*  */
  52 /*  for adhoc-master to generate ie and provide supported-rate to fw */
  53 /*  */
  54
  55 static u8       WIFI_CCKRATES[] = {
  56         (IEEE80211_CCK_RATE_1MB | IEEE80211_BASIC_RATE_MASK),
  57         (IEEE80211_CCK_RATE_2MB | IEEE80211_BASIC_RATE_MASK),
  58         (IEEE80211_CCK_RATE_5MB | IEEE80211_BASIC_RATE_MASK),
  59         (IEEE80211_CCK_RATE_11MB | IEEE80211_BASIC_RATE_MASK)
  60         };
  61
  62 static u8       WIFI_OFDMRATES[] = {
  63          (IEEE80211_OFDM_RATE_6MB),
  64          (IEEE80211_OFDM_RATE_9MB),
  65          (IEEE80211_OFDM_RATE_12MB),
  66          (IEEE80211_OFDM_RATE_18MB),
  67          (IEEE80211_OFDM_RATE_24MB),
  68          IEEE80211_OFDM_RATE_36MB,
  69          IEEE80211_OFDM_RATE_48MB,
  70          IEEE80211_OFDM_RATE_54MB
  71         };
  72
  73
  74 int rtw_get_bit_value_from_ieee_value(u8 val)
  75 {
  76         unsigned char dot11_rate_table[] = {
  77                 2, 4, 11, 22, 12, 18, 24, 36, 48,
  78                 72, 96, 108, 0}; /*  last element must be zero!! */
  79
  80         int i = 0;
  81         while (dot11_rate_table[i] != 0) {
  82                 if (dot11_rate_table[i] == val)
  83                         return BIT(i);
  84                 i++;
  85         }
  86         return 0;
  87 }
  88
  89 uint    rtw_is_cckrates_included(u8 *rate)
  90 {
  91         u32     i = 0;
  92
  93         while (rate[i] != 0) {
  94                 if  ((((rate[i]) & 0x7f) == 2) || (((rate[i]) & 0x7f) == 4) ||
  95                      (((rate[i]) & 0x7f) == 11)  || (((rate[i]) & 0x7f) == 22))
  96                         return true;
  97                 i++;
  98         }
  99         return false;
 100 }
 101
 102 uint    rtw_is_cckratesonly_included(u8 *rate)
 103 {
 104         u32 i = 0;
 105
 106         while (rate[i] != 0) {
 107                 if  ((((rate[i]) & 0x7f) != 2) && (((rate[i]) & 0x7f) != 4) &&
 108                      (((rate[i]) & 0x7f) != 11)  && (((rate[i]) & 0x7f) != 22))
 109                         return false;
 110                 i++;
 111         }
 112
 113         return true;
 114 }
 115
 116 int rtw_check_network_type(unsigned char *rate, int ratelen, int channel)
 117 {
 118         if (channel > 14) {
 119                 if ((rtw_is_cckrates_included(rate)) == true)
 120                         return WIRELESS_INVALID;
 121                 else
 122                         return WIRELESS_11A;
 123         } else {  /*  could be pure B, pure G, or B/G */
 124                 if ((rtw_is_cckratesonly_included(rate)) == true)
 125                         return WIRELESS_11B;
 126                 else if ((rtw_is_cckrates_included(rate)) == true)
 127                         return  WIRELESS_11BG;
 128                 else
 129                         return WIRELESS_11G;
 130         }
 131 }
 132
 133 u8 *rtw_set_fixed_ie(void *pbuf, unsigned int len, void *source,
 134                      unsigned int *frlen)
 135 {
 136         memcpy(pbuf, source, len);
 137         *frlen = *frlen + len;
 138         return ((u8 *)pbuf) + len;
 139 }
 140
 141 /*  rtw_set_ie will update frame length */
 142 u8 *rtw_set_ie
 143 (
 144         u8 *pbuf,
 145         int index,
 146         uint len,
 147         u8 *source,
 148         uint *frlen /* frame length */
 149 )
 150 {
 151         *pbuf = (u8)index;
 152
 153         *(pbuf + 1) = (u8)len;
 154
 155         if (len > 0)
 156                 memcpy((void *)(pbuf + 2), (void *)source, len);
 157
 158         *frlen = *frlen + (len + 2);
 159
 160         return pbuf + len + 2;
 161 }
 162
 163 inline u8 *rtw_set_ie_ch_switch(u8 *buf, u32 *buf_len, u8 ch_switch_mode,
 164         u8 new_ch, u8 ch_switch_cnt)
 165 {
 166         u8 ie_data[3];
 167
 168         ie_data[0] = ch_switch_mode;
 169         ie_data[1] = new_ch;
 170         ie_data[2] = ch_switch_cnt;
 171         return rtw_set_ie(buf, WLAN_EID_CHANNEL_SWITCH,  3, ie_data, buf_len);
 172 }
 173
 174 inline u8 secondary_ch_offset_to_hal_ch_offset(u8 ch_offset)
 175 {
 176         if (ch_offset == SCN)
 177                 return HAL_PRIME_CHNL_OFFSET_DONT_CARE;
 178         else if (ch_offset == SCA)
 179                 return HAL_PRIME_CHNL_OFFSET_UPPER;
 180         else if (ch_offset == SCB)
 181                 return HAL_PRIME_CHNL_OFFSET_LOWER;
 182
 183         return HAL_PRIME_CHNL_OFFSET_DONT_CARE;
 184 }
 185
 186 inline u8 hal_ch_offset_to_secondary_ch_offset(u8 ch_offset)
 187 {
 188         if (ch_offset == HAL_PRIME_CHNL_OFFSET_DONT_CARE)
 189                 return SCN;
 190         else if (ch_offset == HAL_PRIME_CHNL_OFFSET_LOWER)
 191                 return SCB;
 192         else if (ch_offset == HAL_PRIME_CHNL_OFFSET_UPPER)
 193                 return SCA;
 194
 195         return SCN;
 196 }
 197
 198 inline u8 *rtw_set_ie_secondary_ch_offset(u8 *buf, u32 *buf_len, u8 secondary_ch_offset)
 199 {
 200         return rtw_set_ie(buf, WLAN_EID_SECONDARY_CHANNEL_OFFSET,  1, &secondary_ch_offset, buf_len);
 201 }
 202
 203 inline u8 *rtw_set_ie_mesh_ch_switch_parm(u8 *buf, u32 *buf_len, u8 ttl,
 204         u8 flags, u16 reason, u16 precedence)
 205 {
 206         u8 ie_data[6];
 207
 208         ie_data[0] = ttl;
 209         ie_data[1] = flags;
 210         *(u16 *)(ie_data+2) = cpu_to_le16(reason);
 211         *(u16 *)(ie_data+4) = cpu_to_le16(precedence);
 212
 213         return rtw_set_ie(buf, 0x118,  6, ie_data, buf_len);
 214 }
 215
 216 /*----------------------------------------------------------------------------
 217 index: the information element id index, limit is the limit for search
 218 -----------------------------------------------------------------------------*/
 219 u8 *rtw_get_ie(u8 *pbuf, int index, int *len, int limit)
 220 {
 221         int tmp, i;
 222         u8 *p;
 223         if (limit < 1)
 224                 return NULL;
 225
 226         p = pbuf;
 227         i = 0;
 228         *len = 0;
 229         while (1) {
 230                 if (*p == index) {
 231                         *len = *(p + 1);
 232                         return p;
 233                 } else {
 234                         tmp = *(p + 1);
 235                         p += (tmp + 2);
 236                         i += (tmp + 2);
 237                 }
 238                 if (i >= limit)
 239                         break;
 240         }
 241         return NULL;
 242 }
 243
 244 /**
 245  * rtw_get_ie_ex - Search specific IE from a series of IEs
 246  * @in_ie: Address of IEs to search
 247  * @in_len: Length limit from in_ie
 248  * @eid: Element ID to match
 249  * @oui: OUI to match
 250  * @oui_len: OUI length
 251  * @ie: If not NULL and the specific IE is found, the IE will be copied to the buf starting from the specific IE
 252  * @ielen: If not NULL and the specific IE is found, will set to the length of the entire IE
 253  *
 254  * Returns: The address of the specific IE found, or NULL
 255  */
 256 u8 *rtw_get_ie_ex(u8 *in_ie, uint in_len, u8 eid, u8 *oui, u8 oui_len, u8 *ie, uint *ielen)
 257 {
 258         uint cnt;
 259         u8 *target_ie = NULL;
 260
 261
 262         if (ielen)
 263                 *ielen = 0;
 264
 265         if (!in_ie || in_len <= 0)
 266                 return target_ie;
 267
 268         cnt = 0;
 269
 270         while (cnt < in_len) {
 271                 if (eid == in_ie[cnt] && (!oui || !memcmp(&in_ie[cnt+2], oui, oui_len))) {
 272                         target_ie = &in_ie[cnt];
 273
 274                         if (ie)
 275                                 memcpy(ie, &in_ie[cnt], in_ie[cnt+1]+2);
 276
 277                         if (ielen)
 278                                 *ielen = in_ie[cnt+1]+2;
 279
 280                         break;
 281                 } else {
 282                         cnt += in_ie[cnt+1]+2; /* goto next */
 283                 }
 284         }
 285         return target_ie;
 286 }
 287
 288 /**
 289  * rtw_ies_remove_ie - Find matching IEs and remove
 290  * @ies: Address of IEs to search
 291  * @ies_len: Pointer of length of ies, will update to new length
 292  * @offset: The offset to start scarch
 293  * @eid: Element ID to match
 294  * @oui: OUI to match
 295  * @oui_len: OUI length
 296  *
 297  * Returns: _SUCCESS: ies is updated, _FAIL: not updated
 298  */
 299 int rtw_ies_remove_ie(u8 *ies, uint *ies_len, uint offset, u8 eid, u8 *oui, u8 oui_len)
 300 {
 301         int ret = _FAIL;
 302         u8 *target_ie;
 303         u32 target_ielen;
 304         u8 *start;
 305         uint search_len;
 306
 307         if (!ies || !ies_len || *ies_len <= offset)
 308                 goto exit;
 309
 310         start = ies + offset;
 311         search_len = *ies_len - offset;
 312
 313         while (1) {
 314                 target_ie = rtw_get_ie_ex(start, search_len, eid, oui, oui_len, NULL, &target_ielen);
 315                 if (target_ie && target_ielen) {
 316                         u8 buf[MAX_IE_SZ] = {0};
 317                         u8 *remain_ies = target_ie + target_ielen;
 318                         uint remain_len = search_len - (remain_ies - start);
 319
 320                         memcpy(buf, remain_ies, remain_len);
 321                         memcpy(target_ie, buf, remain_len);
 322                         *ies_len = *ies_len - target_ielen;
 323                         ret = _SUCCESS;
 324
 325                         start = target_ie;
 326                         search_len = remain_len;
 327                 } else {
 328                         break;
 329                 }
 330         }
 331 exit:
 332         return ret;
 333 }
 334
 335 void rtw_set_supported_rate(u8 *SupportedRates, uint mode)
 336 {
 337
 338         memset(SupportedRates, 0, NDIS_802_11_LENGTH_RATES_EX);
 339
 340         switch (mode) {
 341         case WIRELESS_11B:
 342                 memcpy(SupportedRates, WIFI_CCKRATES, IEEE80211_CCK_RATE_LEN);
 343                 break;
 344         case WIRELESS_11G:
 345         case WIRELESS_11A:
 346         case WIRELESS_11_5N:
 347         case WIRELESS_11A_5N:/* Todo: no basic rate for ofdm ? */
 348                 memcpy(SupportedRates, WIFI_OFDMRATES, IEEE80211_NUM_OFDM_RATESLEN);
 349                 break;
 350         case WIRELESS_11BG:
 351         case WIRELESS_11G_24N:
 352         case WIRELESS_11_24N:
 353         case WIRELESS_11BG_24N:
 354                 memcpy(SupportedRates, WIFI_CCKRATES, IEEE80211_CCK_RATE_LEN);
 355                 memcpy(SupportedRates + IEEE80211_CCK_RATE_LEN, WIFI_OFDMRATES, IEEE80211_NUM_OFDM_RATESLEN);
 356                 break;
 357         }
 358 }
 359
 360 uint    rtw_get_rateset_len(u8  *rateset)
 361 {
 362         uint i = 0;
 363         while (1) {
 364                 if ((rateset[i]) == 0)
 365                         break;
 366                 if (i > 12)
 367                         break;
 368                 i++;
 369         }
 370         return i;
 371 }
 372
 373 int rtw_generate_ie(struct registry_priv *pregistrypriv)
 374 {
 375         u8      wireless_mode;
 376         int     sz = 0, rateLen;
 377         struct wlan_bssid_ex *pdev_network = &pregistrypriv->dev_network;
 378         u8 *ie = pdev_network->IEs;
 379
 380
 381         /* timestamp will be inserted by hardware */
 382         sz += 8;
 383         ie += sz;
 384
 385         /* beacon interval : 2bytes */
 386         *(__le16 *)ie = cpu_to_le16((u16)pdev_network->Configuration.BeaconPeriod);/* BCN_INTERVAL; */
 387         sz += 2;
 388         ie += 2;
 389
 390         /* capability info */
 391         *(u16 *)ie = 0;
 392
 393         *(__le16 *)ie |= cpu_to_le16(cap_IBSS);
 394
 395         if (pregistrypriv->preamble == PREAMBLE_SHORT)
 396                 *(__le16 *)ie |= cpu_to_le16(cap_ShortPremble);
 397
 398         if (pdev_network->Privacy)
 399                 *(__le16 *)ie |= cpu_to_le16(cap_Privacy);
 400
 401         sz += 2;
 402         ie += 2;
 403
 404         /* SSID */
 405         ie = rtw_set_ie(ie, _SSID_IE_, pdev_network->Ssid.SsidLength, pdev_network->Ssid.Ssid, &sz);
 406
 407         /* supported rates */
 408         if (pregistrypriv->wireless_mode == WIRELESS_11ABGN) {
 409                 if (pdev_network->Configuration.DSConfig > 14)
 410                         wireless_mode = WIRELESS_11A_5N;
 411                 else
 412                         wireless_mode = WIRELESS_11BG_24N;
 413         } else {
 414                 wireless_mode = pregistrypriv->wireless_mode;
 415         }
 416
 417                 rtw_set_supported_rate(pdev_network->SupportedRates, wireless_mode);
 418
 419         rateLen = rtw_get_rateset_len(pdev_network->SupportedRates);
 420
 421         if (rateLen > 8) {
 422                 ie = rtw_set_ie(ie, _SUPPORTEDRATES_IE_, 8, pdev_network->SupportedRates, &sz);
 423                 /* ie = rtw_set_ie(ie, _EXT_SUPPORTEDRATES_IE_, (rateLen - 8), (pdev_network->SupportedRates + 8), &sz); */
 424         } else {
 425                 ie = rtw_set_ie(ie, _SUPPORTEDRATES_IE_, rateLen, pdev_network->SupportedRates, &sz);
 426         }
 427
 428         /* DS parameter set */
 429         ie = rtw_set_ie(ie, _DSSET_IE_, 1, (u8 *)&(pdev_network->Configuration.DSConfig), &sz);
 430
 431         /* IBSS Parameter Set */
 432
 433         ie = rtw_set_ie(ie, _IBSS_PARA_IE_, 2, (u8 *)&(pdev_network->Configuration.ATIMWindow), &sz);
 434
 435         if (rateLen > 8)
 436                 ie = rtw_set_ie(ie, _EXT_SUPPORTEDRATES_IE_, (rateLen - 8), (pdev_network->SupportedRates + 8), &sz);
 437
 438         return sz;
 439 }
 440
 441 unsigned char *rtw_get_wpa_ie(unsigned char *pie, int *wpa_ie_len, int limit)
 442 {
 443         int len;
 444         u16 val16;
 445         __le16 le_tmp;
 446         unsigned char wpa_oui_type[] = {0x00, 0x50, 0xf2, 0x01};
 447         u8 *pbuf = pie;
 448         int limit_new = limit;
 449
 450         while (1) {
 451                 pbuf = rtw_get_ie(pbuf, _WPA_IE_ID_, &len, limit_new);
 452
 453                 if (pbuf) {
 454                         /* check if oui matches... */
 455                         if (!memcmp((pbuf + 2), wpa_oui_type, sizeof(wpa_oui_type)) == false)
 456                                 goto check_next_ie;
 457
 458                         /* check version... */
 459                         memcpy((u8 *)&le_tmp, (pbuf + 6), sizeof(val16));
 460
 461                         val16 = le16_to_cpu(le_tmp);
 462                         if (val16 != 0x0001)
 463                                 goto check_next_ie;
 464                         *wpa_ie_len = *(pbuf + 1);
 465                         return pbuf;
 466                 } else {
 467                         *wpa_ie_len = 0;
 468                         return NULL;
 469                 }
 470
 471 check_next_ie:
 472                 limit_new = limit - (pbuf - pie) - 2 - len;
 473                 if (limit_new <= 0)
 474                         break;
 475                 pbuf += (2 + len);
 476         }
 477         *wpa_ie_len = 0;
 478         return NULL;
 479 }
 480
 481 unsigned char *rtw_get_wpa2_ie(unsigned char *pie, int *rsn_ie_len, int limit)
 482 {
 483
 484         return rtw_get_ie(pie, _WPA2_IE_ID_, rsn_ie_len, limit);
 485 }
 486
 487 int rtw_get_wpa_cipher_suite(u8 *s)
 488 {
 489         if (!memcmp(s, WPA_CIPHER_SUITE_NONE, WPA_SELECTOR_LEN))
 490                 return WPA_CIPHER_NONE;
 491         if (!memcmp(s, WPA_CIPHER_SUITE_WEP40, WPA_SELECTOR_LEN))
 492                 return WPA_CIPHER_WEP40;
 493         if (!memcmp(s, WPA_CIPHER_SUITE_TKIP, WPA_SELECTOR_LEN))
 494                 return WPA_CIPHER_TKIP;
 495         if (!memcmp(s, WPA_CIPHER_SUITE_CCMP, WPA_SELECTOR_LEN))
 496                 return WPA_CIPHER_CCMP;
 497         if (!memcmp(s, WPA_CIPHER_SUITE_WEP104, WPA_SELECTOR_LEN))
 498                 return WPA_CIPHER_WEP104;
 499
 500         return 0;
 501 }
 502
 503 int rtw_get_wpa2_cipher_suite(u8 *s)
 504 {
 505         if (!memcmp(s, RSN_CIPHER_SUITE_NONE, RSN_SELECTOR_LEN))
 506                 return WPA_CIPHER_NONE;
 507         if (!memcmp(s, RSN_CIPHER_SUITE_WEP40, RSN_SELECTOR_LEN))
 508                 return WPA_CIPHER_WEP40;
 509         if (!memcmp(s, RSN_CIPHER_SUITE_TKIP, RSN_SELECTOR_LEN))
 510                 return WPA_CIPHER_TKIP;
 511         if (!memcmp(s, RSN_CIPHER_SUITE_CCMP, RSN_SELECTOR_LEN))
 512                 return WPA_CIPHER_CCMP;
 513         if (!memcmp(s, RSN_CIPHER_SUITE_WEP104, RSN_SELECTOR_LEN))
 514                 return WPA_CIPHER_WEP104;
 515
 516         return 0;
 517 }
 518
 519
 520 int rtw_parse_wpa_ie(u8 *wpa_ie, int wpa_ie_len, int *group_cipher, int *pairwise_cipher, int *is_8021x)
 521 {
 522         int i, ret = _SUCCESS;
 523         int left, count;
 524         u8 *pos;
 525         u8 SUITE_1X[4] = {0x00, 0x50, 0xf2, 1};
 526
 527         if (wpa_ie_len <= 0) {
 528                 /* No WPA IE - fail silently */
 529                 return _FAIL;
 530         }
 531
 532
 533         if ((*wpa_ie != _WPA_IE_ID_) || (*(wpa_ie+1) != (u8)(wpa_ie_len - 2)) ||
 534             (memcmp(wpa_ie+2, RTW_WPA_OUI_TYPE, WPA_SELECTOR_LEN)))
 535                 return _FAIL;
 536
 537         pos = wpa_ie;
 538
 539         pos += 8;
 540         left = wpa_ie_len - 8;
 541
 542
 543         /* group_cipher */
 544         if (left >= WPA_SELECTOR_LEN) {
 545                 *group_cipher = rtw_get_wpa_cipher_suite(pos);
 546                 pos += WPA_SELECTOR_LEN;
 547                 left -= WPA_SELECTOR_LEN;
 548         } else if (left > 0) {
 549                 RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("%s: ie length mismatch, %u too much", __func__, left));
 550                 return _FAIL;
 551         }
 552
 553         /* pairwise_cipher */
 554         if (left >= 2) {
 555                 count = get_unaligned_le16(pos);
 556                 pos += 2;
 557                 left -= 2;
 558
 559                 if (count == 0 || left < count * WPA_SELECTOR_LEN) {
 560                         RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("%s: ie count botch (pairwise), "
 561                                                 "count %u left %u", __func__, count, left));
 562                         return _FAIL;
 563                 }
 564
 565                 for (i = 0; i < count; i++) {
 566                         *pairwise_cipher |= rtw_get_wpa_cipher_suite(pos);
 567
 568                         pos += WPA_SELECTOR_LEN;
 569                         left -= WPA_SELECTOR_LEN;
 570                 }
 571         } else if (left == 1) {
 572                 RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("%s: ie too short (for key mgmt)",   __func__));
 573                 return _FAIL;
 574         }
 575
 576         if (is_8021x) {
 577                 if (left >= 6) {
 578                         pos += 2;
 579                         if (!memcmp(pos, SUITE_1X, 4)) {
 580                                 RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("%s : there has 802.1x auth\n", __func__));
 581                                 *is_8021x = 1;
 582                         }
 583                 }
 584         }
 585
 586         return ret;
 587 }
 588
 589 int rtw_parse_wpa2_ie(u8 *rsn_ie, int rsn_ie_len, int *group_cipher, int *pairwise_cipher, int *is_8021x)
 590 {
 591         int i, ret = _SUCCESS;
 592         int left, count;
 593         u8 *pos;
 594         u8 SUITE_1X[4] = {0x00, 0x0f, 0xac, 0x01};
 595
 596         if (rsn_ie_len <= 0) {
 597                 /* No RSN IE - fail silently */
 598                 return _FAIL;
 599         }
 600
 601
 602         if ((*rsn_ie != _WPA2_IE_ID_) || (*(rsn_ie+1) != (u8)(rsn_ie_len - 2)))
 603                 return _FAIL;
 604
 605         pos = rsn_ie;
 606         pos += 4;
 607         left = rsn_ie_len - 4;
 608
 609         /* group_cipher */
 610         if (left >= RSN_SELECTOR_LEN) {
 611                 *group_cipher = rtw_get_wpa2_cipher_suite(pos);
 612
 613                 pos += RSN_SELECTOR_LEN;
 614                 left -= RSN_SELECTOR_LEN;
 615
 616         } else if (left > 0) {
 617                 RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("%s: ie length mismatch, %u too much", __func__, left));
 618                 return _FAIL;
 619         }
 620
 621         /* pairwise_cipher */
 622         if (left >= 2) {
 623                 count = get_unaligned_le16(pos);
 624                 pos += 2;
 625                 left -= 2;
 626
 627                 if (count == 0 || left < count * RSN_SELECTOR_LEN) {
 628                         RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("%s: ie count botch (pairwise), "
 629                                                  "count %u left %u", __func__, count, left));
 630                         return _FAIL;
 631                 }
 632
 633                 for (i = 0; i < count; i++) {
 634                         *pairwise_cipher |= rtw_get_wpa2_cipher_suite(pos);
 635
 636                         pos += RSN_SELECTOR_LEN;
 637                         left -= RSN_SELECTOR_LEN;
 638                 }
 639
 640         } else if (left == 1) {
 641                 RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("%s: ie too short (for key mgmt)",  __func__));
 642
 643                 return _FAIL;
 644         }
 645
 646         if (is_8021x) {
 647                 if (left >= 6) {
 648                         pos += 2;
 649                         if (!memcmp(pos, SUITE_1X, 4)) {
 650                                 RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("%s (): there has 802.1x auth\n", __func__));
 651                                 *is_8021x = 1;
 652                         }
 653                 }
 654         }
 655         return ret;
 656 }
 657
 658 int rtw_get_sec_ie(u8 *in_ie, uint in_len, u8 *rsn_ie, u16 *rsn_len, u8 *wpa_ie, u16 *wpa_len)
 659 {
 660         u8 authmode, sec_idx, i;
 661         u8 wpa_oui[4] = {0x0, 0x50, 0xf2, 0x01};
 662         uint    cnt;
 663
 664
 665         /* Search required WPA or WPA2 IE and copy to sec_ie[] */
 666
 667         cnt = _TIMESTAMP_ + _BEACON_ITERVAL_ + _CAPABILITY_;
 668
 669         sec_idx = 0;
 670
 671         while (cnt < in_len) {
 672                 authmode = in_ie[cnt];
 673
 674                 if ((authmode == _WPA_IE_ID_) && (!memcmp(&in_ie[cnt+2], &wpa_oui[0], 4))) {
 675                                 RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
 676                                          ("\n rtw_get_wpa_ie: sec_idx =%d in_ie[cnt+1]+2 =%d\n",
 677                                          sec_idx, in_ie[cnt+1]+2));
 678
 679                                 if (wpa_ie) {
 680                                         memcpy(wpa_ie, &in_ie[cnt], in_ie[cnt+1]+2);
 681
 682                                         for (i = 0; i < (in_ie[cnt+1]+2); i += 8) {
 683                                                 RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
 684                                                          ("\n %2x,%2x,%2x,%2x,%2x,%2x,%2x,%2x\n",
 685                                                          wpa_ie[i], wpa_ie[i+1], wpa_ie[i+2], wpa_ie[i+3], wpa_ie[i+4],
 686                                                          wpa_ie[i+5], wpa_ie[i+6], wpa_ie[i+7]));
 687                                         }
 688                                 }
 689
 690                                 *wpa_len = in_ie[cnt+1]+2;
 691                                 cnt += in_ie[cnt+1]+2;  /* get next */
 692                 } else {
 693                         if (authmode == _WPA2_IE_ID_) {
 694                                 RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
 695                                          ("\n get_rsn_ie: sec_idx =%d in_ie[cnt+1]+2 =%d\n",
 696                                          sec_idx, in_ie[cnt+1]+2));
 697
 698                                 if (rsn_ie) {
 699                                         memcpy(rsn_ie, &in_ie[cnt], in_ie[cnt+1]+2);
 700
 701                                         for (i = 0; i < (in_ie[cnt+1]+2); i += 8) {
 702                                                 RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
 703                                                          ("\n %2x,%2x,%2x,%2x,%2x,%2x,%2x,%2x\n",
 704                                                          rsn_ie[i], rsn_ie[i+1], rsn_ie[i+2], rsn_ie[i+3], rsn_ie[i+4],
 705                                                          rsn_ie[i+5], rsn_ie[i+6], rsn_ie[i+7]));
 706                                                 }
 707                                 }
 708
 709                                 *rsn_len = in_ie[cnt+1]+2;
 710                                 cnt += in_ie[cnt+1]+2;  /* get next */
 711                         } else {
 712                                 cnt += in_ie[cnt+1]+2;   /* get next */
 713                         }
 714                 }
 715         }
 716
 717
 718         return *rsn_len + *wpa_len;
 719 }
 720
 721 u8 rtw_is_wps_ie(u8 *ie_ptr, uint *wps_ielen)
 722 {
 723         u8 match = false;
 724         u8 eid, wps_oui[4] = {0x0, 0x50, 0xf2, 0x04};
 725
 726         if (ie_ptr == NULL)
 727                 return match;
 728
 729         eid = ie_ptr[0];
 730
 731         if ((eid == _WPA_IE_ID_) && (!memcmp(&ie_ptr[2], wps_oui, 4))) {
 732                 *wps_ielen = ie_ptr[1]+2;
 733                 match = true;
 734         }
 735         return match;
 736 }
 737
 738 /**
 739  * rtw_get_wps_ie - Search WPS IE from a series of IEs
 740  * @in_ie: Address of IEs to search
 741  * @in_len: Length limit from in_ie
 742  * @wps_ie: If not NULL and WPS IE is found, WPS IE will be copied to the buf starting from wps_ie
 743  * @wps_ielen: If not NULL and WPS IE is found, will set to the length of the entire WPS IE
 744  *
 745  * Returns: The address of the WPS IE found, or NULL
 746  */
 747 u8 *rtw_get_wps_ie(u8 *in_ie, uint in_len, u8 *wps_ie, uint *wps_ielen)
 748 {
 749         uint cnt;
 750         u8 *wpsie_ptr = NULL;
 751         u8 eid, wps_oui[4] = {0x0, 0x50, 0xf2, 0x04};
 752
 753         if (wps_ielen)
 754                 *wps_ielen = 0;
 755
 756         if (!in_ie || in_len <= 0)
 757                 return wpsie_ptr;
 758
 759         cnt = 0;
 760
 761         while (cnt < in_len) {
 762                 eid = in_ie[cnt];
 763
 764                 if ((eid == _WPA_IE_ID_) && (!memcmp(&in_ie[cnt+2], wps_oui, 4))) {
 765                         wpsie_ptr = &in_ie[cnt];
 766
 767                         if (wps_ie)
 768                                 memcpy(wps_ie, &in_ie[cnt], in_ie[cnt+1]+2);
 769
 770                         if (wps_ielen)
 771                                 *wps_ielen = in_ie[cnt+1]+2;
 772
 773                         cnt += in_ie[cnt+1]+2;
 774
 775                         break;
 776                 } else {
 777                         cnt += in_ie[cnt+1]+2; /* goto next */
 778                 }
 779         }
 780         return wpsie_ptr;
 781 }
 782
 783 /**
 784  * rtw_get_wps_attr - Search a specific WPS attribute from a given WPS IE
 785  * @wps_ie: Address of WPS IE to search
 786  * @wps_ielen: Length limit from wps_ie
 787  * @target_attr_id: The attribute ID of WPS attribute to search
 788  * @buf_attr: If not NULL and the WPS attribute is found, WPS attribute will be copied to the buf starting from buf_attr
 789  * @len_attr: If not NULL and the WPS attribute is found, will set to the length of the entire WPS attribute
 790  *
 791  * Returns: the address of the specific WPS attribute found, or NULL
 792  */
 793 u8 *rtw_get_wps_attr(u8 *wps_ie, uint wps_ielen, u16 target_attr_id, u8 *buf_attr, u32 *len_attr)
 794 {
 795         u8 *attr_ptr = NULL;
 796         u8 *target_attr_ptr = NULL;
 797         u8 wps_oui[4] = {0x00, 0x50, 0xF2, 0x04};
 798
 799         if (len_attr)
 800                 *len_attr = 0;
 801
 802         if ((wps_ie[0] != _VENDOR_SPECIFIC_IE_) ||
 803             (memcmp(wps_ie + 2, wps_oui, 4)))
 804                 return attr_ptr;
 805
 806         /*  6 = 1(Element ID) + 1(Length) + 4(WPS OUI) */
 807         attr_ptr = wps_ie + 6; /* goto first attr */
 808
 809         while (attr_ptr - wps_ie < wps_ielen) {
 810                 /*  4 = 2(Attribute ID) + 2(Length) */
 811                 u16 attr_id = get_unaligned_be16(attr_ptr);
 812                 u16 attr_data_len = get_unaligned_be16(attr_ptr + 2);
 813                 u16 attr_len = attr_data_len + 4;
 814
 815                 if (attr_id == target_attr_id) {
 816                         target_attr_ptr = attr_ptr;
 817                         if (buf_attr)
 818                                 memcpy(buf_attr, attr_ptr, attr_len);
 819                         if (len_attr)
 820                                 *len_attr = attr_len;
 821                         break;
 822                 } else {
 823                         attr_ptr += attr_len; /* goto next */
 824                 }
 825         }
 826         return target_attr_ptr;
 827 }
 828
 829 /**
 830  * rtw_get_wps_attr_content - Search a specific WPS attribute content from a given WPS IE
 831  * @wps_ie: Address of WPS IE to search
 832  * @wps_ielen: Length limit from wps_ie
 833  * @target_attr_id: The attribute ID of WPS attribute to search
 834  * @buf_content: If not NULL and the WPS attribute is found, WPS attribute content will be copied to the buf starting from buf_content
 835  * @len_content: If not NULL and the WPS attribute is found, will set to the length of the WPS attribute content
 836  *
 837  * Returns: the address of the specific WPS attribute content found, or NULL
 838  */
 839 u8 *rtw_get_wps_attr_content(u8 *wps_ie, uint wps_ielen, u16 target_attr_id, u8 *buf_content, uint *len_content)
 840 {
 841         u8 *attr_ptr;
 842         u32 attr_len;
 843
 844         if (len_content)
 845                 *len_content = 0;
 846
 847         attr_ptr = rtw_get_wps_attr(wps_ie, wps_ielen, target_attr_id, NULL, &attr_len);
 848
 849         if (attr_ptr && attr_len) {
 850                 if (buf_content)
 851                         memcpy(buf_content, attr_ptr+4, attr_len-4);
 852
 853                 if (len_content)
 854                         *len_content = attr_len-4;
 855
 856                 return attr_ptr+4;
 857         }
 858
 859         return NULL;
 860 }
 861
 862 static int rtw_ieee802_11_parse_vendor_specific(u8 *pos, uint elen,
 863                                             struct rtw_ieee802_11_elems *elems,
 864                                             int show_errors)
 865 {
 866         unsigned int oui;
 867
 868         /* first 3 bytes in vendor specific information element are the IEEE
 869          * OUI of the vendor. The following byte is used a vendor specific
 870          * sub-type. */
 871         if (elen < 4) {
 872                 if (show_errors) {
 873                         DBG_88E("short vendor specific information element ignored (len=%lu)\n",
 874                                 (unsigned long)elen);
 875                 }
 876                 return -1;
 877         }
 878
 879         oui = RTW_GET_BE24(pos);
 880         switch (oui) {
 881         case OUI_MICROSOFT:
 882                 /* Microsoft/Wi-Fi information elements are further typed and
 883                  * subtyped */
 884                 switch (pos[3]) {
 885                 case 1:
 886                         /* Microsoft OUI (00:50:F2) with OUI Type 1:
 887                          * real WPA information element */
 888                         elems->wpa_ie = pos;
 889                         elems->wpa_ie_len = elen;
 890                         break;
 891                 case WME_OUI_TYPE: /* this is a Wi-Fi WME info. element */
 892                         if (elen < 5) {
 893                                 DBG_88E("short WME information element ignored (len=%lu)\n",
 894                                         (unsigned long)elen);
 895                                 return -1;
 896                         }
 897                         switch (pos[4]) {
 898                         case WME_OUI_SUBTYPE_INFORMATION_ELEMENT:
 899                         case WME_OUI_SUBTYPE_PARAMETER_ELEMENT:
 900                                 elems->wme = pos;
 901                                 elems->wme_len = elen;
 902                                 break;
 903                         case WME_OUI_SUBTYPE_TSPEC_ELEMENT:
 904                                 elems->wme_tspec = pos;
 905                                 elems->wme_tspec_len = elen;
 906                                 break;
 907                         default:
 908                                 DBG_88E("unknown WME information element ignored (subtype=%d len=%lu)\n",
 909                                         pos[4], (unsigned long)elen);
 910                                 return -1;
 911                         }
 912                         break;
 913                 case 4:
 914                         /* Wi-Fi Protected Setup (WPS) IE */
 915                         elems->wps_ie = pos;
 916                         elems->wps_ie_len = elen;
 917                         break;
 918                 default:
 919                         DBG_88E("Unknown Microsoft information element ignored (type=%d len=%lu)\n",
 920                                 pos[3], (unsigned long)elen);
 921                         return -1;
 922                 }
 923                 break;
 924
 925         case OUI_BROADCOM:
 926                 switch (pos[3]) {
 927                 case VENDOR_HT_CAPAB_OUI_TYPE:
 928                         elems->vendor_ht_cap = pos;
 929                         elems->vendor_ht_cap_len = elen;
 930                         break;
 931                 default:
 932                         DBG_88E("Unknown Broadcom information element ignored (type=%d len=%lu)\n",
 933                                 pos[3], (unsigned long)elen);
 934                         return -1;
 935                 }
 936                 break;
 937         default:
 938                 DBG_88E("unknown vendor specific information element ignored (vendor OUI %02x:%02x:%02x len=%lu)\n",
 939                         pos[0], pos[1], pos[2], (unsigned long)elen);
 940                 return -1;
 941         }
 942         return 0;
 943 }
 944
 945 /**
 946  * ieee802_11_parse_elems - Parse information elements in management frames
 947  * @start: Pointer to the start of IEs
 948  * @len: Length of IE buffer in octets
 949  * @elems: Data structure for parsed elements
 950  * @show_errors: Whether to show parsing errors in debug log
 951  * Returns: Parsing result
 952  */
 953 enum parse_res rtw_ieee802_11_parse_elems(u8 *start, uint len,
 954                                 struct rtw_ieee802_11_elems *elems,
 955                                 int show_errors)
 956 {
 957         uint left = len;
 958         u8 *pos = start;
 959         int unknown = 0;
 960
 961         memset(elems, 0, sizeof(*elems));
 962
 963         while (left >= 2) {
 964                 u8 id, elen;
 965
 966                 id = *pos++;
 967                 elen = *pos++;
 968                 left -= 2;
 969
 970                 if (elen > left) {
 971                         if (show_errors) {
 972                                 DBG_88E("IEEE 802.11 element parse failed (id=%d elen=%d left=%lu)\n",
 973                                         id, elen, (unsigned long)left);
 974                         }
 975                         return ParseFailed;
 976                 }
 977
 978                 switch (id) {
 979                 case WLAN_EID_SSID:
 980                         elems->ssid = pos;
 981                         elems->ssid_len = elen;
 982                         break;
 983                 case WLAN_EID_SUPP_RATES:
 984                         elems->supp_rates = pos;
 985                         elems->supp_rates_len = elen;
 986                         break;
 987                 case WLAN_EID_FH_PARAMS:
 988                         elems->fh_params = pos;
 989                         elems->fh_params_len = elen;
 990                         break;
 991                 case WLAN_EID_DS_PARAMS:
 992                         elems->ds_params = pos;
 993                         elems->ds_params_len = elen;
 994                         break;
 995                 case WLAN_EID_CF_PARAMS:
 996                         elems->cf_params = pos;
 997                         elems->cf_params_len = elen;
 998                         break;
 999                 case WLAN_EID_TIM:
1000                         elems->tim = pos;
1001                         elems->tim_len = elen;
1002                         break;
1003                 case WLAN_EID_IBSS_PARAMS:
1004                         elems->ibss_params = pos;
1005                         elems->ibss_params_len = elen;
1006                         break;
1007                 case WLAN_EID_CHALLENGE:
1008                         elems->challenge = pos;
1009                         elems->challenge_len = elen;
1010                         break;
1011                 case WLAN_EID_ERP_INFO:
1012                         elems->erp_info = pos;
1013                         elems->erp_info_len = elen;
1014                         break;
1015                 case WLAN_EID_EXT_SUPP_RATES:
1016                         elems->ext_supp_rates = pos;
1017                         elems->ext_supp_rates_len = elen;
1018                         break;
1019                 case WLAN_EID_VENDOR_SPECIFIC:
1020                         if (rtw_ieee802_11_parse_vendor_specific(pos, elen, elems, show_errors))
1021                                 unknown++;
1022                         break;
1023                 case WLAN_EID_RSN:
1024                         elems->rsn_ie = pos;
1025                         elems->rsn_ie_len = elen;
1026                         break;
1027                 case WLAN_EID_PWR_CAPABILITY:
1028                         elems->power_cap = pos;
1029                         elems->power_cap_len = elen;
1030                         break;
1031                 case WLAN_EID_SUPPORTED_CHANNELS:
1032                         elems->supp_channels = pos;
1033                         elems->supp_channels_len = elen;
1034                         break;
1035                 case WLAN_EID_MOBILITY_DOMAIN:
1036                         elems->mdie = pos;
1037                         elems->mdie_len = elen;
1038                         break;
1039                 case WLAN_EID_FAST_BSS_TRANSITION:
1040                         elems->ftie = pos;
1041                         elems->ftie_len = elen;
1042                         break;
1043                 case WLAN_EID_TIMEOUT_INTERVAL:
1044                         elems->timeout_int = pos;
1045                         elems->timeout_int_len = elen;
1046                         break;
1047                 case WLAN_EID_HT_CAPABILITY:
1048                         elems->ht_capabilities = pos;
1049                         elems->ht_capabilities_len = elen;
1050                         break;
1051                 case WLAN_EID_HT_OPERATION:
1052                         elems->ht_operation = pos;
1053                         elems->ht_operation_len = elen;
1054                         break;
1055                 default:
1056                         unknown++;
1057                         if (!show_errors)
1058                                 break;
1059                         DBG_88E("IEEE 802.11 element parse ignored unknown element (id=%d elen=%d)\n",
1060                                 id, elen);
1061                         break;
1062                 }
1063                 left -= elen;
1064                 pos += elen;
1065         }
1066         if (left)
1067                 return ParseFailed;
1068         return unknown ? ParseUnknown : ParseOK;
1069 }
1070
1071 void rtw_macaddr_cfg(u8 *mac_addr)
1072 {
1073         u8 mac[ETH_ALEN];
1074
1075         if (mac_addr == NULL)
1076                 return;
1077
1078         if (rtw_initmac && mac_pton(rtw_initmac, mac)) {
1079                 /* Users specify the mac address */
1080                 memcpy(mac_addr, mac, ETH_ALEN);
1081         } else {
1082                 /* Use the mac address stored in the Efuse */
1083                 memcpy(mac, mac_addr, ETH_ALEN);
1084         }
1085
1086         if (((mac[0] == 0xff) && (mac[1] == 0xff) && (mac[2] == 0xff) &&
1087              (mac[3] == 0xff) && (mac[4] == 0xff) && (mac[5] == 0xff)) ||
1088             ((mac[0] == 0x0) && (mac[1] == 0x0) && (mac[2] == 0x0) &&
1089              (mac[3] == 0x0) && (mac[4] == 0x0) && (mac[5] == 0x0))) {
1090                 mac[0] = 0x00;
1091                 mac[1] = 0xe0;
1092                 mac[2] = 0x4c;
1093                 mac[3] = 0x87;
1094                 mac[4] = 0x00;
1095                 mac[5] = 0x00;
1096                 /*  use default mac address */
1097                 memcpy(mac_addr, mac, ETH_ALEN);
1098                 DBG_88E("MAC Address from efuse error, assign default one !!!\n");
1099         }
1100
1101         DBG_88E("rtw_macaddr_cfg MAC Address  = %pM\n", (mac_addr));
1102 }
1103
1104 void dump_ies(u8 *buf, u32 buf_len)
1105 {
1106         u8 *pos = buf;
1107         u8 id, len;
1108
1109         while (pos-buf <= buf_len) {
1110                 id = *pos;
1111                 len = *(pos+1);
1112
1113                 DBG_88E("%s ID:%u, LEN:%u\n", __func__, id, len);
1114                 dump_wps_ie(pos, len);
1115
1116                 pos += (2 + len);
1117         }
1118 }
1119
1120 void dump_wps_ie(u8 *ie, u32 ie_len)
1121 {
1122         u8 *pos = ie;
1123         u16 id;
1124         u16 len;
1125         u8 *wps_ie;
1126         uint wps_ielen;
1127
1128         wps_ie = rtw_get_wps_ie(ie, ie_len, NULL, &wps_ielen);
1129         if (wps_ie != ie || wps_ielen == 0)
1130                 return;
1131
1132         pos += 6;
1133         while (pos-ie < ie_len) {
1134                 id = get_unaligned_be16(pos);
1135                 len = get_unaligned_be16(pos + 2);
1136                 DBG_88E("%s ID:0x%04x, LEN:%u\n", __func__, id, len);
1137                 pos += (4+len);
1138         }
1139 }
1140
1141 /* Baron adds to avoid FreeBSD warning */
1142 int ieee80211_is_empty_essid(const char *essid, int essid_len)
1143 {
1144         /* Single white space is for Linksys APs */
1145         if (essid_len == 1 && essid[0] == ' ')
1146                 return 1;
1147
1148         /* Otherwise, if the entire essid is 0, we assume it is hidden */
1149         while (essid_len) {
1150                 essid_len--;
1151                 if (essid[essid_len] != '\0')
1152                         return 0;
1153         }
1154
1155         return 1;
1156 }
1157
1158 int ieee80211_get_hdrlen(u16 fc)
1159 {
1160         int hdrlen = 24;
1161
1162         switch (WLAN_FC_GET_TYPE(fc)) {
1163         case RTW_IEEE80211_FTYPE_DATA:
1164                 if (fc & RTW_IEEE80211_STYPE_QOS_DATA)
1165                         hdrlen += 2;
1166                 if ((fc & RTW_IEEE80211_FCTL_FROMDS) && (fc & RTW_IEEE80211_FCTL_TODS))
1167                         hdrlen += 6; /* Addr4 */
1168                 break;
1169         case RTW_IEEE80211_FTYPE_CTL:
1170                 switch (WLAN_FC_GET_STYPE(fc)) {
1171                 case RTW_IEEE80211_STYPE_CTS:
1172                 case RTW_IEEE80211_STYPE_ACK:
1173                         hdrlen = 10;
1174                         break;
1175                 default:
1176                         hdrlen = 16;
1177                         break;
1178                 }
1179                 break;
1180         }
1181
1182         return hdrlen;
1183 }
1184
1185 static int rtw_get_cipher_info(struct wlan_network *pnetwork)
1186 {
1187         u32 wpa_ielen;
1188         unsigned char *pbuf;
1189         int group_cipher = 0, pairwise_cipher = 0, is8021x = 0;
1190         int ret = _FAIL;
1191         pbuf = rtw_get_wpa_ie(&pnetwork->network.IEs[12], &wpa_ielen, pnetwork->network.IELength-12);
1192
1193         if (pbuf && (wpa_ielen > 0)) {
1194                 RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_get_cipher_info: wpa_ielen: %d", wpa_ielen));
1195                 if (_SUCCESS == rtw_parse_wpa_ie(pbuf, wpa_ielen+2, &group_cipher, &pairwise_cipher, &is8021x)) {
1196                         pnetwork->BcnInfo.pairwise_cipher = pairwise_cipher;
1197                         pnetwork->BcnInfo.group_cipher = group_cipher;
1198                         pnetwork->BcnInfo.is_8021x = is8021x;
1199                         RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("%s: pnetwork->pairwise_cipher: %d, is_8021x is %d",
1200                                  __func__, pnetwork->BcnInfo.pairwise_cipher, pnetwork->BcnInfo.is_8021x));
1201                         ret = _SUCCESS;
1202                 }
1203         } else {
1204                 pbuf = rtw_get_wpa2_ie(&pnetwork->network.IEs[12], &wpa_ielen, pnetwork->network.IELength-12);
1205
1206                 if (pbuf && (wpa_ielen > 0)) {
1207                         RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("get RSN IE\n"));
1208                         if (_SUCCESS == rtw_parse_wpa2_ie(pbuf, wpa_ielen+2, &group_cipher, &pairwise_cipher, &is8021x)) {
1209                                 RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("get RSN IE  OK!!!\n"));
1210                                 pnetwork->BcnInfo.pairwise_cipher = pairwise_cipher;
1211                                 pnetwork->BcnInfo.group_cipher = group_cipher;
1212                                 pnetwork->BcnInfo.is_8021x = is8021x;
1213                                 RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("%s: pnetwork->pairwise_cipher: %d,"
1214                                                         "pnetwork->group_cipher is %d, is_8021x is %d", __func__, pnetwork->BcnInfo.pairwise_cipher,
1215                                                         pnetwork->BcnInfo.group_cipher, pnetwork->BcnInfo.is_8021x));
1216                                 ret = _SUCCESS;
1217                         }
1218                 }
1219         }
1220
1221         return ret;
1222 }
1223
1224 void rtw_get_bcn_info(struct wlan_network *pnetwork)
1225 {
1226         unsigned short cap = 0;
1227         u8 bencrypt = 0;
1228         __le16 le_tmp;
1229         u16 wpa_len = 0, rsn_len = 0;
1230         struct HT_info_element *pht_info = NULL;
1231         struct rtw_ieee80211_ht_cap *pht_cap = NULL;
1232         unsigned int            len;
1233         unsigned char           *p;
1234
1235         memcpy(&le_tmp, rtw_get_capability_from_ie(pnetwork->network.IEs), 2);
1236         cap = le16_to_cpu(le_tmp);
1237         if (cap & WLAN_CAPABILITY_PRIVACY) {
1238                 bencrypt = 1;
1239                 pnetwork->network.Privacy = 1;
1240         } else {
1241                 pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_OPENSYS;
1242         }
1243         rtw_get_sec_ie(pnetwork->network.IEs, pnetwork->network.IELength, NULL, &rsn_len, NULL, &wpa_len);
1244         RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_get_bcn_info: ssid =%s\n", pnetwork->network.Ssid.Ssid));
1245         RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_get_bcn_info: wpa_len =%d rsn_len =%d\n", wpa_len, rsn_len));
1246         RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_get_bcn_info: ssid =%s\n", pnetwork->network.Ssid.Ssid));
1247         RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_get_bcn_info: wpa_len =%d rsn_len =%d\n", wpa_len, rsn_len));
1248
1249         if (rsn_len > 0) {
1250                 pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_WPA2;
1251         } else if (wpa_len > 0) {
1252                 pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_WPA;
1253         } else {
1254                 if (bencrypt)
1255                         pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_WEP;
1256         }
1257         RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_get_bcn_info: pnetwork->encryp_protocol is %x\n",
1258                  pnetwork->BcnInfo.encryp_protocol));
1259         RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_get_bcn_info: pnetwork->encryp_protocol is %x\n",
1260                  pnetwork->BcnInfo.encryp_protocol));
1261         rtw_get_cipher_info(pnetwork);
1262
1263         /* get bwmode and ch_offset */
1264         /* parsing HT_CAP_IE */
1265         p = rtw_get_ie(pnetwork->network.IEs + _FIXED_IE_LENGTH_, _HT_CAPABILITY_IE_, &len, pnetwork->network.IELength - _FIXED_IE_LENGTH_);
1266         if (p && len > 0) {
1267                         pht_cap = (struct rtw_ieee80211_ht_cap *)(p + 2);
1268                         pnetwork->BcnInfo.ht_cap_info = pht_cap->cap_info;
1269         } else {
1270                         pnetwork->BcnInfo.ht_cap_info = 0;
1271         }
1272         /* parsing HT_INFO_IE */
1273         p = rtw_get_ie(pnetwork->network.IEs + _FIXED_IE_LENGTH_, _HT_ADD_INFO_IE_, &len, pnetwork->network.IELength - _FIXED_IE_LENGTH_);
1274         if (p && len > 0) {
1275                         pht_info = (struct HT_info_element *)(p + 2);
1276                         pnetwork->BcnInfo.ht_info_infos_0 = pht_info->infos[0];
1277         } else {
1278                         pnetwork->BcnInfo.ht_info_infos_0 = 0;
1279         }
1280 }
1281
1282 /* show MCS rate, unit: 100Kbps */
1283 u16 rtw_mcs_rate(u8 rf_type, u8 bw_40MHz, u8 short_GI_20, u8 short_GI_40, unsigned char *MCS_rate)
1284 {
1285         u16 max_rate = 0;
1286
1287         if (rf_type == RF_1T1R) {
1288                 if (MCS_rate[0] & BIT(7))
1289                         max_rate = (bw_40MHz) ? ((short_GI_40) ? 1500 : 1350) : ((short_GI_20) ? 722 : 650);
1290                 else if (MCS_rate[0] & BIT(6))
1291                         max_rate = (bw_40MHz) ? ((short_GI_40) ? 1350 : 1215) : ((short_GI_20) ? 650 : 585);
1292                 else if (MCS_rate[0] & BIT(5))
1293                         max_rate = (bw_40MHz) ? ((short_GI_40) ? 1200 : 1080) : ((short_GI_20) ? 578 : 520);
1294                 else if (MCS_rate[0] & BIT(4))
1295                         max_rate = (bw_40MHz) ? ((short_GI_40) ? 900 : 810) : ((short_GI_20) ? 433 : 390);
1296                 else if (MCS_rate[0] & BIT(3))
1297                         max_rate = (bw_40MHz) ? ((short_GI_40) ? 600 : 540) : ((short_GI_20) ? 289 : 260);
1298                 else if (MCS_rate[0] & BIT(2))
1299                         max_rate = (bw_40MHz) ? ((short_GI_40) ? 450 : 405) : ((short_GI_20) ? 217 : 195);
1300                 else if (MCS_rate[0] & BIT(1))
1301                         max_rate = (bw_40MHz) ? ((short_GI_40) ? 300 : 270) : ((short_GI_20) ? 144 : 130);
1302                 else if (MCS_rate[0] & BIT(0))
1303                         max_rate = (bw_40MHz) ? ((short_GI_40) ? 150 : 135) : ((short_GI_20) ? 72 : 65);
1304         } else {
1305                 if (MCS_rate[1]) {
1306                         if (MCS_rate[1] & BIT(7))
1307                                 max_rate = (bw_40MHz) ? ((short_GI_40) ? 3000 : 2700) : ((short_GI_20) ? 1444 : 1300);
1308                         else if (MCS_rate[1] & BIT(6))
1309                                 max_rate = (bw_40MHz) ? ((short_GI_40) ? 2700 : 2430) : ((short_GI_20) ? 1300 : 1170);
1310                         else if (MCS_rate[1] & BIT(5))
1311                                 max_rate = (bw_40MHz) ? ((short_GI_40) ? 2400 : 2160) : ((short_GI_20) ? 1156 : 1040);
1312                         else if (MCS_rate[1] & BIT(4))
1313                                 max_rate = (bw_40MHz) ? ((short_GI_40) ? 1800 : 1620) : ((short_GI_20) ? 867 : 780);
1314                         else if (MCS_rate[1] & BIT(3))
1315                                 max_rate = (bw_40MHz) ? ((short_GI_40) ? 1200 : 1080) : ((short_GI_20) ? 578 : 520);
1316                         else if (MCS_rate[1] & BIT(2))
1317                                 max_rate = (bw_40MHz) ? ((short_GI_40) ? 900 : 810) : ((short_GI_20) ? 433 : 390);
1318                         else if (MCS_rate[1] & BIT(1))
1319                                 max_rate = (bw_40MHz) ? ((short_GI_40) ? 600 : 540) : ((short_GI_20) ? 289 : 260);
1320                         else if (MCS_rate[1] & BIT(0))
1321                                 max_rate = (bw_40MHz) ? ((short_GI_40) ? 300 : 270) : ((short_GI_20) ? 144 : 130);
1322                 } else {
1323                         if (MCS_rate[0] & BIT(7))
1324                                 max_rate = (bw_40MHz) ? ((short_GI_40) ? 1500 : 1350) : ((short_GI_20) ? 722 : 650);
1325                         else if (MCS_rate[0] & BIT(6))
1326                                 max_rate = (bw_40MHz) ? ((short_GI_40) ? 1350 : 1215) : ((short_GI_20) ? 650 : 585);
1327                         else if (MCS_rate[0] & BIT(5))
1328                                 max_rate = (bw_40MHz) ? ((short_GI_40) ? 1200 : 1080) : ((short_GI_20) ? 578 : 520);
1329                         else if (MCS_rate[0] & BIT(4))
1330                                 max_rate = (bw_40MHz) ? ((short_GI_40) ? 900 : 810) : ((short_GI_20) ? 433 : 390);
1331                         else if (MCS_rate[0] & BIT(3))
1332                                 max_rate = (bw_40MHz) ? ((short_GI_40) ? 600 : 540) : ((short_GI_20) ? 289 : 260);
1333                         else if (MCS_rate[0] & BIT(2))
1334                                 max_rate = (bw_40MHz) ? ((short_GI_40) ? 450 : 405) : ((short_GI_20) ? 217 : 195);
1335                         else if (MCS_rate[0] & BIT(1))
1336                                 max_rate = (bw_40MHz) ? ((short_GI_40) ? 300 : 270) : ((short_GI_20) ? 144 : 130);
1337                         else if (MCS_rate[0] & BIT(0))
1338                                 max_rate = (bw_40MHz) ? ((short_GI_40) ? 150 : 135) : ((short_GI_20) ? 72 : 65);
1339                 }
1340         }
1341         return max_rate;
1342 }
1343
1344 int rtw_action_frame_parse(const u8 *frame, u32 frame_len, u8 *category, u8 *action)
1345 {
1346         const u8 *frame_body = frame + sizeof(struct rtw_ieee80211_hdr_3addr);
1347         u16 fc;
1348         u8 c, a = 0;
1349
1350         fc = le16_to_cpu(((struct rtw_ieee80211_hdr_3addr *)frame)->frame_ctl);
1351
1352         if ((fc & (RTW_IEEE80211_FCTL_FTYPE|RTW_IEEE80211_FCTL_STYPE)) !=
1353             (RTW_IEEE80211_FTYPE_MGMT|RTW_IEEE80211_STYPE_ACTION))
1354                 return false;
1355
1356         c = frame_body[0];
1357
1358         switch (c) {
1359         case RTW_WLAN_CATEGORY_P2P: /* vendor-specific */
1360                 break;
1361         default:
1362                 a = frame_body[1];
1363         }
1364
1365         if (category)
1366                 *category = c;
1367         if (action)
1368                 *action = a;
1369
1370         return true;
1371 }
1372
1373 static const char *_action_public_str[] = {
1374         "ACT_PUB_BSSCOEXIST",
1375         "ACT_PUB_DSE_ENABLE",
1376         "ACT_PUB_DSE_DEENABLE",
1377         "ACT_PUB_DSE_REG_LOCATION",
1378         "ACT_PUB_EXT_CHL_SWITCH",
1379         "ACT_PUB_DSE_MSR_REQ",
1380         "ACT_PUB_DSE_MSR_RPRT",
1381         "ACT_PUB_MP",
1382         "ACT_PUB_DSE_PWR_CONSTRAINT",
1383         "ACT_PUB_VENDOR",
1384         "ACT_PUB_GAS_INITIAL_REQ",
1385         "ACT_PUB_GAS_INITIAL_RSP",
1386         "ACT_PUB_GAS_COMEBACK_REQ",
1387         "ACT_PUB_GAS_COMEBACK_RSP",
1388         "ACT_PUB_TDLS_DISCOVERY_RSP",
1389         "ACT_PUB_LOCATION_TRACK",
1390         "ACT_PUB_RSVD",
1391 };
1392
1393 const char *action_public_str(u8 action)
1394 {
1395         action = min_t(u8, action, ACT_PUBLIC_MAX);
1396         return _action_public_str[action];
1397 }