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