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