]> git.proxmox.com Git - mirror_ubuntu-artful-kernel.git/blob - drivers/staging/rtl8723au/core/rtw_ieee80211.c
projects / mirror_ubuntu-artful-kernel.git / blob
? search:
summary | shortlog | log | commit | commitdiff | tree
blame | history | raw | HEAD
staging: rtl8723au: make source * argument const
[mirror_ubuntu-artful-kernel.git] / drivers / staging / rtl8723au / 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 <linux/ieee80211.h>
19 #include <ieee80211.h>
20 #include <wifi.h>
21 #include <osdep_service.h>
22 #include <wlan_bssdef.h>
23
24 u8 RTW_WPA_OUI23A_TYPE[] = { 0x00, 0x50, 0xf2, 1 };
25 u16 RTW_WPA_VERSION23A = 1;
26 u8 WPA_AUTH_KEY_MGMT_NONE23A[] = { 0x00, 0x50, 0xf2, 0 };
27 u8 WPA_AUTH_KEY_MGMT_UNSPEC_802_1X23A[] = { 0x00, 0x50, 0xf2, 1 };
28 u8 WPA_AUTH_KEY_MGMT_PSK_OVER_802_1X23A[] = { 0x00, 0x50, 0xf2, 2 };
29 u8 WPA_CIPHER_SUITE_NONE23A[] = { 0x00, 0x50, 0xf2, 0 };
30 u8 WPA_CIPHER_SUITE_WEP4023A[] = { 0x00, 0x50, 0xf2, 1 };
31 u8 WPA_CIPHER_SUITE_TKIP23A[] = { 0x00, 0x50, 0xf2, 2 };
32 u8 WPA_CIPHER_SUITE_WRAP23A[] = { 0x00, 0x50, 0xf2, 3 };
33 u8 WPA_CIPHER_SUITE_CCMP23A[] = { 0x00, 0x50, 0xf2, 4 };
34 u8 WPA_CIPHER_SUITE_WEP10423A[] = { 0x00, 0x50, 0xf2, 5 };
35
36 u16 RSN_VERSION_BSD23A = 1;
37 u8 RSN_AUTH_KEY_MGMT_UNSPEC_802_1X23A[] = { 0x00, 0x0f, 0xac, 1 };
38 u8 RSN_AUTH_KEY_MGMT_PSK_OVER_802_1X23A[] = { 0x00, 0x0f, 0xac, 2 };
39 u8 RSN_CIPHER_SUITE_NONE23A[] = { 0x00, 0x0f, 0xac, 0 };
40 u8 RSN_CIPHER_SUITE_WEP4023A[] = { 0x00, 0x0f, 0xac, 1 };
41 u8 RSN_CIPHER_SUITE_TKIP23A[] = { 0x00, 0x0f, 0xac, 2 };
42 u8 RSN_CIPHER_SUITE_WRAP23A[] = { 0x00, 0x0f, 0xac, 3 };
43 u8 RSN_CIPHER_SUITE_CCMP23A[] = { 0x00, 0x0f, 0xac, 4 };
44 u8 RSN_CIPHER_SUITE_WEP10423A[] = { 0x00, 0x0f, 0xac, 5 };
45 /* */
46 /* for adhoc-master to generate ie and provide supported-rate to fw */
47 /* */
48
49 static u8 WIFI_CCKRATES[] =
50 {(IEEE80211_CCK_RATE_1MB | IEEE80211_BASIC_RATE_MASK),
51 (IEEE80211_CCK_RATE_2MB | IEEE80211_BASIC_RATE_MASK),
52 (IEEE80211_CCK_RATE_5MB | IEEE80211_BASIC_RATE_MASK),
53 (IEEE80211_CCK_RATE_11MB | IEEE80211_BASIC_RATE_MASK)};
54
55 static u8 WIFI_OFDMRATES[] =
56 {(IEEE80211_OFDM_RATE_6MB),
57 (IEEE80211_OFDM_RATE_9MB),
58 (IEEE80211_OFDM_RATE_12MB),
59 (IEEE80211_OFDM_RATE_18MB),
60 (IEEE80211_OFDM_RATE_24MB),
61 IEEE80211_OFDM_RATE_36MB,
62 IEEE80211_OFDM_RATE_48MB,
63 IEEE80211_OFDM_RATE_54MB};
64
65 int rtw_get_bit_value_from_ieee_value23a(u8 val)
66 {
67 unsigned char dot11_rate_table[]=
68 {2, 4, 11, 22, 12, 18, 24, 36, 48, 72, 96, 108, 0};
69
70 int i = 0;
71 while (dot11_rate_table[i] != 0) {
72 if (dot11_rate_table[i] == val)
73 return BIT(i);
74 i++;
75 }
76 return 0;
77 }
78
79 uint rtw_is_cckrates_included23a(u8 *rate)
80 {
81 u32 i = 0;
82
83 while (rate[i] != 0) {
84 if ((((rate[i]) & 0x7f) == 2) || (((rate[i]) & 0x7f) == 4) ||
85 (((rate[i]) & 0x7f) == 11) || (((rate[i]) & 0x7f) == 22))
86 return true;
87 i++;
88 }
89
90 return false;
91 }
92
93 uint rtw_is_cckratesonly_included23a(u8 *rate)
94 {
95 u32 i = 0;
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
102 i++;
103 }
104
105 return true;
106 }
107
108 int rtw_check_network_type23a(unsigned char *rate, int ratelen, int channel)
109 {
110 if (channel > 14) {
111 if ((rtw_is_cckrates_included23a(rate)) == true)
112 return WIRELESS_INVALID;
113 else
114 return WIRELESS_11A;
115 } else { /* could be pure B, pure G, or B/G */
116 if ((rtw_is_cckratesonly_included23a(rate)) == true)
117 return WIRELESS_11B;
118 else if ((rtw_is_cckrates_included23a(rate)) == true)
119 return WIRELESS_11BG;
120 else
121 return WIRELESS_11G;
122 }
123 }
124
125 u8 *rtw_set_fixed_ie23a(unsigned char *pbuf, unsigned int len,
126 unsigned char *source, unsigned int *frlen)
127 {
128 memcpy((void *)pbuf, (void *)source, len);
129 *frlen = *frlen + len;
130 return pbuf + len;
131 }
132
133 /* rtw_set_ie23a will update frame length */
134 u8 *rtw_set_ie23a(u8 *pbuf, int index, uint len, const u8 *source, uint *frlen)
135 {
136
137 *pbuf = (u8)index;
138
139 *(pbuf + 1) = (u8)len;
140
141 if (len > 0)
142 memcpy((void *)(pbuf + 2), (void *)source, len);
143
144 *frlen = *frlen + (len + 2);
145
146
147 return pbuf + len + 2;
148 }
149
150 inline u8 *rtw_set_ie23a_ch_switch (u8 *buf, u32 *buf_len, u8 ch_switch_mode,
151 u8 new_ch, u8 ch_switch_cnt)
152 {
153 u8 ie_data[3];
154
155 ie_data[0] = ch_switch_mode;
156 ie_data[1] = new_ch;
157 ie_data[2] = ch_switch_cnt;
158 return rtw_set_ie23a(buf, WLAN_EID_CHANNEL_SWITCH, 3, ie_data, buf_len);
159 }
160
161 inline u8 secondary_ch_offset_to_hal_ch_offset23a(u8 ch_offset)
162 {
163 if (ch_offset == SCN)
164 return HAL_PRIME_CHNL_OFFSET_DONT_CARE;
165 else if (ch_offset == SCA)
166 return HAL_PRIME_CHNL_OFFSET_UPPER;
167 else if (ch_offset == SCB)
168 return HAL_PRIME_CHNL_OFFSET_LOWER;
169
170 return HAL_PRIME_CHNL_OFFSET_DONT_CARE;
171 }
172
173 inline u8 hal_ch_offset_to_secondary_ch_offset23a(u8 ch_offset)
174 {
175 if (ch_offset == HAL_PRIME_CHNL_OFFSET_DONT_CARE)
176 return SCN;
177 else if (ch_offset == HAL_PRIME_CHNL_OFFSET_LOWER)
178 return SCB;
179 else if (ch_offset == HAL_PRIME_CHNL_OFFSET_UPPER)
180 return SCA;
181
182 return SCN;
183 }
184
185 inline u8 *rtw_set_ie23a_secondary_ch_offset(u8 *buf, u32 *buf_len,
186 u8 secondary_ch_offset)
187 {
188 return rtw_set_ie23a(buf, WLAN_EID_SECONDARY_CHANNEL_OFFSET,
189 1, &secondary_ch_offset, buf_len);
190 }
191
192 inline u8 *rtw_set_ie23a_mesh_ch_switch_parm(u8 *buf, u32 *buf_len, u8 ttl,
193 u8 flags, u16 reason, u16 precedence)
194 {
195 u8 ie_data[6];
196
197 ie_data[0] = ttl;
198 ie_data[1] = flags;
199 put_unaligned_le16(reason, (u8*)&ie_data[2]);
200 put_unaligned_le16(precedence, (u8*)&ie_data[4]);
201
202 return rtw_set_ie23a(buf, 0x118, 6, ie_data, buf_len);
203 }
204
205 /*----------------------------------------------------------------------------
206 index: the information element id index, limit is the limit for search
207 -----------------------------------------------------------------------------*/
208 u8 *rtw_get_ie23a(u8 *pbuf, int index, int *len, int limit)
209 {
210 int tmp, i;
211 u8 *p;
212
213 if (limit < 1) {
214
215 return NULL;
216 }
217
218 p = pbuf;
219 i = 0;
220 *len = 0;
221 while (1) {
222 if (*p == index) {
223 *len = *(p + 1);
224 return p;
225 } else {
226 tmp = *(p + 1);
227 p += (tmp + 2);
228 i += (tmp + 2);
229 }
230 if (i >= limit)
231 break;
232 }
233
234 return NULL;
235 }
236
237 /**
238 * rtw_get_ie23a_ex - Search specific IE from a series of IEs
239 * @in_ie: Address of IEs to search
240 * @in_len: Length limit from in_ie
241 * @eid: Element ID to match
242 * @oui: OUI to match
243 * @oui_len: OUI length
244 * @ie: If not NULL and the specific IE is found, the IE will be copied
245 * to the buf starting from the specific IE
246 * @ielen: If not NULL and the specific IE is found, will set to the length
247 * of the entire IE
248 *
249 * Returns: The address of the specific IE found, or NULL
250 */
251 u8 *rtw_get_ie23a_ex(u8 *in_ie, uint in_len, u8 eid, u8 *oui, u8 oui_len,
252 u8 *ie, uint *ielen)
253 {
254 uint cnt;
255 u8 *target_ie = NULL;
256
257 if (ielen)
258 *ielen = 0;
259
260 if (!in_ie || in_len <= 0)
261 return target_ie;
262
263 cnt = 0;
264
265 while (cnt < in_len) {
266 if (eid == in_ie[cnt] &&
267 (!oui || !memcmp(&in_ie[cnt+2], oui, oui_len))) {
268 target_ie = &in_ie[cnt];
269
270 if (ie)
271 memcpy(ie, &in_ie[cnt], in_ie[cnt+1]+2);
272
273 if (ielen)
274 *ielen = in_ie[cnt+1]+2;
275 break;
276 } else {
277 cnt += in_ie[cnt + 1] + 2; /* goto next */
278 }
279 }
280
281 return target_ie;
282 }
283
284 /**
285 * rtw_ies_remove_ie23a - Find matching IEs and remove
286 * @ies: Address of IEs to search
287 * @ies_len: Pointer of length of ies, will update to new length
288 * @offset: The offset to start scarch
289 * @eid: Element ID to match
290 * @oui: OUI to match
291 * @oui_len: OUI length
292 *
293 * Returns: _SUCCESS: ies is updated, _FAIL: not updated
294 */
295 int rtw_ies_remove_ie23a(u8 *ies, uint *ies_len, uint offset, u8 eid,
296 u8 *oui, u8 oui_len)
297 {
298 int ret = _FAIL;
299 u8 *target_ie;
300 u32 target_ielen;
301 u8 *start;
302 uint search_len;
303
304 if (!ies || !ies_len || *ies_len <= offset)
305 goto exit;
306
307 start = ies + offset;
308 search_len = *ies_len - offset;
309
310 while (1) {
311 target_ie = rtw_get_ie23a_ex(start, search_len, eid, oui, oui_len,
312 NULL, &target_ielen);
313 if (target_ie && target_ielen) {
314 u8 buf[MAX_IE_SZ] = {0};
315 u8 *remain_ies = target_ie + target_ielen;
316 uint remain_len = search_len - (remain_ies - start);
317
318 memcpy(buf, remain_ies, remain_len);
319 memcpy(target_ie, buf, remain_len);
320 *ies_len = *ies_len - target_ielen;
321 ret = _SUCCESS;
322
323 start = target_ie;
324 search_len = remain_len;
325 } else {
326 break;
327 }
328 }
329 exit:
330 return ret;
331 }
332
333 void rtw_set_supported_rate23a(u8* SupportedRates, uint mode)
334 {
335
336
337 memset(SupportedRates, 0, NDIS_802_11_LENGTH_RATES_EX);
338
339 switch (mode)
340 {
341 case WIRELESS_11B:
342 memcpy(SupportedRates, WIFI_CCKRATES, IEEE80211_CCK_RATE_LEN);
343 break;
344
345 case WIRELESS_11G:
346 case WIRELESS_11A:
347 case WIRELESS_11_5N:
348 case WIRELESS_11A_5N:/* Todo: no basic rate for ofdm ? */
349 memcpy(SupportedRates, WIFI_OFDMRATES,
350 IEEE80211_NUM_OFDM_RATESLEN);
351 break;
352
353 case WIRELESS_11BG:
354 case WIRELESS_11G_24N:
355 case WIRELESS_11_24N:
356 case WIRELESS_11BG_24N:
357 memcpy(SupportedRates, WIFI_CCKRATES, IEEE80211_CCK_RATE_LEN);
358 memcpy(SupportedRates + IEEE80211_CCK_RATE_LEN, WIFI_OFDMRATES,
359 IEEE80211_NUM_OFDM_RATESLEN);
360 break;
361 }
362
363 }
364
365 uint rtw_get_rateset_len23a(u8 *rateset)
366 {
367 uint i = 0;
368
369 while(1) {
370 if ((rateset[i]) == 0)
371 break;
372
373 if (i > 12)
374 break;
375
376 i++;
377 }
378
379 return i;
380 }
381
382 int rtw_generate_ie23a(struct registry_priv *pregistrypriv)
383 {
384 u8 wireless_mode;
385 int sz = 0, rateLen;
386 struct wlan_bssid_ex* pdev_network = &pregistrypriv->dev_network;
387 u8* ie = pdev_network->IEs;
388
389
390
391 /* timestamp will be inserted by hardware */
392 sz += 8;
393 ie += sz;
394
395 /* beacon interval : 2bytes */
396 /* BCN_INTERVAL; */
397 *(u16*)ie = cpu_to_le16((u16)pdev_network->Configuration.BeaconPeriod);
398 sz += 2;
399 ie += 2;
400
401 /* capability info */
402 *(u16*)ie = 0;
403
404 *(u16*)ie |= cpu_to_le16(cap_IBSS);
405
406 if (pregistrypriv->preamble == PREAMBLE_SHORT)
407 *(u16*)ie |= cpu_to_le16(cap_ShortPremble);
408
409 if (pdev_network->Privacy)
410 *(u16*)ie |= cpu_to_le16(cap_Privacy);
411
412 sz += 2;
413 ie += 2;
414
415 /* SSID */
416 ie = rtw_set_ie23a(ie, _SSID_IE_, pdev_network->Ssid.ssid_len,
417 pdev_network->Ssid.ssid, &sz);
418
419 /* supported rates */
420 if (pregistrypriv->wireless_mode == WIRELESS_11ABGN) {
421 if (pdev_network->Configuration.DSConfig > 14)
422 wireless_mode = WIRELESS_11A_5N;
423 else
424 wireless_mode = WIRELESS_11BG_24N;
425 } else {
426 wireless_mode = pregistrypriv->wireless_mode;
427 }
428
429 rtw_set_supported_rate23a(pdev_network->SupportedRates, wireless_mode) ;
430
431 rateLen = rtw_get_rateset_len23a(pdev_network->SupportedRates);
432
433 if (rateLen > 8) {
434 ie = rtw_set_ie23a(ie, _SUPPORTEDRATES_IE_, 8,
435 pdev_network->SupportedRates, &sz);
436 /* ie = rtw_set_ie23a(ie, _EXT_SUPPORTEDRATES_IE_, (rateLen - 8), (pdev_network->SupportedRates + 8), &sz); */
437 } else {
438 ie = rtw_set_ie23a(ie, _SUPPORTEDRATES_IE_, rateLen,
439 pdev_network->SupportedRates, &sz);
440 }
441
442 /* DS parameter set */
443 ie = rtw_set_ie23a(ie, _DSSET_IE_, 1,
444 (u8 *)&pdev_network->Configuration.DSConfig, &sz);
445
446 /* IBSS Parameter Set */
447
448 ie = rtw_set_ie23a(ie, _IBSS_PARA_IE_, 2,
449 (u8 *)&pdev_network->Configuration.ATIMWindow, &sz);
450
451 if (rateLen > 8) {
452 ie = rtw_set_ie23a(ie, _EXT_SUPPORTEDRATES_IE_, (rateLen - 8),
453 (pdev_network->SupportedRates + 8), &sz);
454 }
455
456
457
458 /* return _SUCCESS; */
459
460 return sz;
461 }
462
463 unsigned char *rtw_get_wpa_ie23a(unsigned char *pie, int *wpa_ie_len, int limit)
464 {
465 int len;
466 u16 val16;
467 unsigned char wpa_oui_type[] = {0x00, 0x50, 0xf2, 0x01};
468 u8 *pbuf = pie;
469 int limit_new = limit;
470
471 while(1) {
472 pbuf = rtw_get_ie23a(pbuf, WLAN_EID_VENDOR_SPECIFIC,
473 &len, limit_new);
474
475 if (pbuf) {
476 /* check if oui matches... */
477 if (memcmp((pbuf + 2), wpa_oui_type,
478 sizeof(wpa_oui_type))) {
479 goto check_next_ie;
480 }
481
482 /* check version... */
483 memcpy((u8 *)&val16, (pbuf + 6), sizeof(val16));
484
485 val16 = le16_to_cpu(val16);
486 if (val16 != 0x0001)
487 goto check_next_ie;
488
489 *wpa_ie_len = *(pbuf + 1);
490
491 return pbuf;
492 } else {
493 *wpa_ie_len = 0;
494 return NULL;
495 }
496
497 check_next_ie:
498
499 limit_new = limit - (pbuf - pie) - 2 - len;
500
501 if (limit_new <= 0)
502 break;
503
504 pbuf += (2 + len);
505 }
506
507 *wpa_ie_len = 0;
508
509 return NULL;
510 }
511
512 unsigned char *rtw_get_wpa2_ie23a(unsigned char *pie, int *rsn_ie_len, int limit)
513 {
514 return rtw_get_ie23a(pie, _WPA2_IE_ID_, rsn_ie_len, limit);
515 }
516
517 int rtw_get_wpa_cipher_suite23a(u8 *s)
518 {
519 if (!memcmp(s, WPA_CIPHER_SUITE_NONE23A, WPA_SELECTOR_LEN))
520 return WPA_CIPHER_NONE;
521 if (!memcmp(s, WPA_CIPHER_SUITE_WEP4023A, WPA_SELECTOR_LEN))
522 return WPA_CIPHER_WEP40;
523 if (!memcmp(s, WPA_CIPHER_SUITE_TKIP23A, WPA_SELECTOR_LEN))
524 return WPA_CIPHER_TKIP;
525 if (!memcmp(s, WPA_CIPHER_SUITE_CCMP23A, WPA_SELECTOR_LEN))
526 return WPA_CIPHER_CCMP;
527 if (!memcmp(s, WPA_CIPHER_SUITE_WEP10423A, WPA_SELECTOR_LEN))
528 return WPA_CIPHER_WEP104;
529
530 return 0;
531 }
532
533 int rtw_get_wpa2_cipher_suite23a(u8 *s)
534 {
535 if (!memcmp(s, RSN_CIPHER_SUITE_NONE23A, RSN_SELECTOR_LEN))
536 return WPA_CIPHER_NONE;
537 if (!memcmp(s, RSN_CIPHER_SUITE_WEP4023A, RSN_SELECTOR_LEN))
538 return WPA_CIPHER_WEP40;
539 if (!memcmp(s, RSN_CIPHER_SUITE_TKIP23A, RSN_SELECTOR_LEN))
540 return WPA_CIPHER_TKIP;
541 if (!memcmp(s, RSN_CIPHER_SUITE_CCMP23A, RSN_SELECTOR_LEN))
542 return WPA_CIPHER_CCMP;
543 if (!memcmp(s, RSN_CIPHER_SUITE_WEP10423A, RSN_SELECTOR_LEN))
544 return WPA_CIPHER_WEP104;
545
546 return 0;
547 }
548
549 int rtw_parse_wpa_ie23a(u8* wpa_ie, int wpa_ie_len, int *group_cipher, int *pairwise_cipher, int *is_8021x)
550 {
551 int i, ret = _SUCCESS;
552 int left, count;
553 u8 *pos;
554 u8 SUITE_1X[4] = {0x00, 0x50, 0xf2, 1};
555
556 if (wpa_ie_len <= 0) {
557 /* No WPA IE - fail silently */
558 return _FAIL;
559 }
560
561 if ((*wpa_ie != WLAN_EID_VENDOR_SPECIFIC) ||
562 (*(wpa_ie+1) != (u8)(wpa_ie_len - 2)) ||
563 memcmp(wpa_ie + 2, RTW_WPA_OUI23A_TYPE, WPA_SELECTOR_LEN)) {
564 return _FAIL;
565 }
566
567 pos = wpa_ie;
568
569 pos += 8;
570 left = wpa_ie_len - 8;
571
572 /* group_cipher */
573 if (left >= WPA_SELECTOR_LEN) {
574
575 *group_cipher = rtw_get_wpa_cipher_suite23a(pos);
576
577 pos += WPA_SELECTOR_LEN;
578 left -= WPA_SELECTOR_LEN;
579 } else if (left > 0) {
580 RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
581 ("%s: ie length mismatch, %u too much",
582 __func__, left));
583
584 return _FAIL;
585 }
586
587 /* pairwise_cipher */
588 if (left >= 2) {
589 /* count = le16_to_cpu(*(u16*)pos); */
590 count = get_unaligned_le16(pos);
591 pos += 2;
592 left -= 2;
593
594 if (count == 0 || left < count * WPA_SELECTOR_LEN) {
595 RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
596 ("%s: ie count botch (pairwise), "
597 "count %u left %u", __func__,
598 count, left));
599 return _FAIL;
600 }
601
602 for (i = 0; i < count; i++) {
603 *pairwise_cipher |= rtw_get_wpa_cipher_suite23a(pos);
604
605 pos += WPA_SELECTOR_LEN;
606 left -= WPA_SELECTOR_LEN;
607 }
608 } else if (left == 1) {
609 RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
610 ("%s: ie too short (for key mgmt)", __func__));
611 return _FAIL;
612 }
613
614 if (is_8021x) {
615 if (left >= 6) {
616 pos += 2;
617 if (!memcmp(pos, SUITE_1X, 4)) {
618 RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
619 ("%s : there has 802.1x auth\n",
620 __func__));
621 *is_8021x = 1;
622 }
623 }
624 }
625
626 return ret;
627 }
628
629 int rtw_parse_wpa2_ie23a(u8* rsn_ie, int rsn_ie_len, int *group_cipher,
630 int *pairwise_cipher, int *is_8021x)
631 {
632 int i, ret = _SUCCESS;
633 int left, count;
634 u8 *pos;
635 u8 SUITE_1X[4] = {0x00, 0x0f, 0xac, 0x01};
636
637 if (rsn_ie_len <= 0) {
638 /* No RSN IE - fail silently */
639 return _FAIL;
640 }
641
642 if ((*rsn_ie!= _WPA2_IE_ID_) || (*(rsn_ie+1) != (u8)(rsn_ie_len - 2))) {
643 return _FAIL;
644 }
645
646 pos = rsn_ie;
647 pos += 4;
648 left = rsn_ie_len - 4;
649
650 /* group_cipher */
651 if (left >= RSN_SELECTOR_LEN) {
652 *group_cipher = rtw_get_wpa2_cipher_suite23a(pos);
653
654 pos += RSN_SELECTOR_LEN;
655 left -= RSN_SELECTOR_LEN;
656 } else if (left > 0) {
657 RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
658 ("%s: ie length mismatch, %u too much",
659 __func__, left));
660 return _FAIL;
661 }
662
663 /* pairwise_cipher */
664 if (left >= 2) {
665 /* count = le16_to_cpu(*(u16*)pos); */
666 count = get_unaligned_le16(pos);
667 pos += 2;
668 left -= 2;
669
670 if (count == 0 || left < count * RSN_SELECTOR_LEN) {
671 RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
672 ("%s: ie count botch (pairwise), "
673 "count %u left %u",
674 __func__, count, left));
675 return _FAIL;
676 }
677
678 for (i = 0; i < count; i++) {
679 *pairwise_cipher |= rtw_get_wpa2_cipher_suite23a(pos);
680
681 pos += RSN_SELECTOR_LEN;
682 left -= RSN_SELECTOR_LEN;
683 }
684 } else if (left == 1) {
685 RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
686 ("%s: ie too short (for key mgmt)", __func__));
687
688 return _FAIL;
689 }
690
691 if (is_8021x) {
692 if (left >= 6) {
693 pos += 2;
694 if (!memcmp(pos, SUITE_1X, 4)) {
695 RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
696 ("%s (): there has 802.1x auth\n",
697 __func__));
698 *is_8021x = 1;
699 }
700 }
701 }
702
703 return ret;
704 }
705
706 int rtw_get_sec_ie23a(u8 *in_ie, uint in_len, u8 *rsn_ie, u16 *rsn_len,
707 u8 *wpa_ie, u16 *wpa_len)
708 {
709 u8 authmode, sec_idx, i;
710 u8 wpa_oui[4] = {0x0, 0x50, 0xf2, 0x01};
711 uint cnt;
712
713
714
715 /* Search required WPA or WPA2 IE and copy to sec_ie[ ] */
716
717 cnt = (_TIMESTAMP_ + _BEACON_ITERVAL_ + _CAPABILITY_);
718
719 sec_idx = 0;
720
721 while(cnt < in_len) {
722 authmode = in_ie[cnt];
723
724 if ((authmode == WLAN_EID_VENDOR_SPECIFIC) &&
725 !memcmp(&in_ie[cnt+2], &wpa_oui[0], 4)) {
726 RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
727 ("\n rtw_get_wpa_ie23a: sec_idx =%d "
728 "in_ie[cnt+1]+2 =%d\n",
729 sec_idx, in_ie[cnt + 1] + 2));
730
731 if (wpa_ie) {
732 memcpy(wpa_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_,
736 _drv_info_,
737 ("\n %2x,%2x,%2x,%2x,%2x,%2x,"
738 "%2x,%2x\n", wpa_ie[i],
739 wpa_ie[i + 1], wpa_ie[i + 2],
740 wpa_ie[i + 3], wpa_ie[i + 4],
741 wpa_ie[i + 5], wpa_ie[i + 6],
742 wpa_ie[i + 7]));
743 }
744 }
745
746 *wpa_len = in_ie[cnt + 1] + 2;
747 cnt += in_ie[cnt + 1] + 2; /* get next */
748 } else {
749 if (authmode == _WPA2_IE_ID_) {
750 RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
751 ("\n get_rsn_ie: sec_idx =%d in_ie"
752 "[cnt+1]+2 =%d\n", sec_idx,
753 in_ie[cnt + 1] + 2));
754
755 if (rsn_ie) {
756 memcpy(rsn_ie, &in_ie[cnt], in_ie[cnt + 1] + 2);
757
758 for (i = 0; i < (in_ie[cnt + 1] + 2); i = i + 8) {
759 RT_TRACE(_module_rtl871x_mlme_c_,
760 _drv_info_,
761 ("\n %2x,%2x,%2x,%2x,%2x,%2x,"
762 "%2x,%2x\n", rsn_ie[i],
763 rsn_ie[i + 1], rsn_ie[i + 2],
764 rsn_ie[i + 3], rsn_ie[i + 4],
765 rsn_ie[i + 5], rsn_ie[i + 6],
766 rsn_ie[i + 7]));
767 }
768 }
769
770 *rsn_len = in_ie[cnt + 1] + 2;
771 cnt += in_ie[cnt + 1] + 2; /* get next */
772 } else {
773 cnt += in_ie[cnt + 1] + 2; /* get next */
774 }
775 }
776 }
777
778
779
780 return *rsn_len + *wpa_len;
781 }
782
783 u8 rtw_is_wps_ie23a(u8 *ie_ptr, uint *wps_ielen)
784 {
785 u8 match = false;
786 u8 eid, wps_oui[4]= {0x0, 0x50, 0xf2, 0x04};
787
788 if (!ie_ptr)
789 return match;
790
791 eid = ie_ptr[0];
792
793 if ((eid == WLAN_EID_VENDOR_SPECIFIC) &&
794 !memcmp(&ie_ptr[2], wps_oui, 4)) {
795 /* DBG_8723A("==> found WPS_IE.....\n"); */
796 *wps_ielen = ie_ptr[1] + 2;
797 match = true;
798 }
799 return match;
800 }
801
802 /**
803 * rtw_get_wps_ie23a - Search WPS IE from a series of IEs
804 * @in_ie: Address of IEs to search
805 * @in_len: Length limit from in_ie
806 * @wps_ie: If not NULL and WPS IE is found, WPS IE will be copied to the
807 * buf starting from wps_ie
808 * @wps_ielen: If not NULL and WPS IE is found, will set to the length of
809 * the entire WPS IE
810 *
811 * Returns: The address of the WPS IE found, or NULL
812 */
813 u8 *rtw_get_wps_ie23a(u8 *in_ie, uint in_len, u8 *wps_ie, uint *wps_ielen)
814 {
815 uint cnt;
816 u8 *wpsie_ptr = NULL;
817 u8 eid, wps_oui[4] = {0x0, 0x50, 0xf2, 0x04};
818
819 if (wps_ielen)
820 *wps_ielen = 0;
821
822 if (!in_ie || in_len <= 0)
823 return wpsie_ptr;
824
825 cnt = 0;
826
827 while (cnt < in_len) {
828 eid = in_ie[cnt];
829
830 if ((eid == WLAN_EID_VENDOR_SPECIFIC) &&
831 !memcmp(&in_ie[cnt+2], wps_oui, 4)) {
832 wpsie_ptr = &in_ie[cnt];
833
834 if (wps_ie)
835 memcpy(wps_ie, &in_ie[cnt], in_ie[cnt + 1] + 2);
836
837 if (wps_ielen)
838 *wps_ielen = in_ie[cnt + 1] + 2;
839
840 cnt += in_ie[cnt + 1] + 2;
841
842 break;
843 } else {
844 cnt += in_ie[cnt + 1] + 2; /* goto next */
845 }
846 }
847
848 return wpsie_ptr;
849 }
850
851 /**
852 * rtw_get_wps_attr23a - Search a specific WPS attribute from a given WPS IE
853 * @wps_ie: Address of WPS IE to search
854 * @wps_ielen: Length limit from wps_ie
855 * @target_attr_id: The attribute ID of WPS attribute to search
856 * @buf_attr: If not NULL and the WPS attribute is found, WPS attribute
857 * will be copied to the buf starting from buf_attr
858 * @len_attr: If not NULL and the WPS attribute is found, will set to the
859 * length of the entire WPS attribute
860 *
861 * Returns: the address of the specific WPS attribute found, or NULL
862 */
863 u8 *rtw_get_wps_attr23a(u8 *wps_ie, uint wps_ielen, u16 target_attr_id,
864 u8 *buf_attr, u32 *len_attr)
865 {
866 u8 *attr_ptr = NULL;
867 u8 * target_attr_ptr = NULL;
868 u8 wps_oui[4] = {0x00, 0x50, 0xF2, 0x04};
869
870 if (len_attr)
871 *len_attr = 0;
872
873 if ((wps_ie[0] != WLAN_EID_VENDOR_SPECIFIC) ||
874 memcmp(wps_ie + 2, wps_oui, 4)) {
875 return attr_ptr;
876 }
877
878 /* 6 = 1(Element ID) + 1(Length) + 4(WPS OUI) */
879 attr_ptr = wps_ie + 6; /* goto first attr */
880
881 while (attr_ptr - wps_ie < wps_ielen) {
882 /* 4 = 2(Attribute ID) + 2(Length) */
883 u16 attr_id = get_unaligned_be16(attr_ptr);
884 u16 attr_data_len = get_unaligned_be16(attr_ptr + 2);
885 u16 attr_len = attr_data_len + 4;
886
887 /* DBG_8723A("%s attr_ptr:%p, id:%u, length:%u\n", __func__, attr_ptr, attr_id, attr_data_len); */
888 if (attr_id == target_attr_id) {
889 target_attr_ptr = attr_ptr;
890
891 if (buf_attr)
892 memcpy(buf_attr, attr_ptr, attr_len);
893
894 if (len_attr)
895 *len_attr = attr_len;
896
897 break;
898 } else {
899 attr_ptr += attr_len; /* goto next */
900 }
901 }
902
903 return target_attr_ptr;
904 }
905
906 /**
907 * rtw_get_wps_attr_content23a - Search a specific WPS attribute content
908 * from a given WPS IE
909 * @wps_ie: Address of WPS IE to search
910 * @wps_ielen: Length limit from wps_ie
911 * @target_attr_id: The attribute ID of WPS attribute to search
912 * @buf_content: If not NULL and the WPS attribute is found, WPS attribute
913 * content will be copied to the buf starting from buf_content
914 * @len_content: If not NULL and the WPS attribute is found, will set to the
915 * length of the WPS attribute content
916 *
917 * Returns: the address of the specific WPS attribute content found, or NULL
918 */
919 u8 *rtw_get_wps_attr_content23a(u8 *wps_ie, uint wps_ielen, u16 target_attr_id,
920 u8 *buf_content, uint *len_content)
921 {
922 u8 *attr_ptr;
923 u32 attr_len;
924
925 if (len_content)
926 *len_content = 0;
927
928 attr_ptr = rtw_get_wps_attr23a(wps_ie, wps_ielen, target_attr_id,
929 NULL, &attr_len);
930
931 if (attr_ptr && attr_len) {
932 if (buf_content)
933 memcpy(buf_content, attr_ptr + 4, attr_len - 4);
934
935 if (len_content)
936 *len_content = attr_len - 4;
937
938 return attr_ptr + 4;
939 }
940
941 return NULL;
942 }
943
944 static int
945 rtw_ieee802_11_parse_vendor_specific(u8 *pos, uint elen,
946 struct rtw_ieee802_11_elems *elems,
947 int show_errors)
948 {
949 unsigned int oui;
950
951 /* first 3 bytes in vendor specific information element are the IEEE
952 * OUI of the vendor. The following byte is used a vendor specific
953 * sub-type. */
954 if (elen < 4) {
955 if (show_errors) {
956 DBG_8723A("short vendor specific "
957 "information element ignored (len =%lu)\n",
958 (unsigned long) elen);
959 }
960 return -1;
961 }
962
963 oui = RTW_GET_BE24(pos);
964 switch (oui) {
965 case WLAN_OUI_MICROSOFT:
966 /* Microsoft/Wi-Fi information elements are further typed and
967 * subtyped */
968 switch (pos[3]) {
969 case 1:
970 /* Microsoft OUI (00:50:F2) with OUI Type 1:
971 * real WPA information element */
972 elems->wpa_ie = pos;
973 elems->wpa_ie_len = elen;
974 break;
975 case WME_OUI_TYPE: /* this is a Wi-Fi WME info. element */
976 if (elen < 5) {
977 DBG_8723A("short WME "
978 "information element ignored "
979 "(len =%lu)\n",
980 (unsigned long) elen);
981 return -1;
982 }
983 switch (pos[4]) {
984 case WME_OUI_SUBTYPE_INFORMATION_ELEMENT:
985 case WME_OUI_SUBTYPE_PARAMETER_ELEMENT:
986 elems->wme = pos;
987 elems->wme_len = elen;
988 break;
989 case WME_OUI_SUBTYPE_TSPEC_ELEMENT:
990 elems->wme_tspec = pos;
991 elems->wme_tspec_len = elen;
992 break;
993 default:
994 DBG_8723A("unknown WME "
995 "information element ignored "
996 "(subtype =%d len =%lu)\n",
997 pos[4], (unsigned long) elen);
998 return -1;
999 }
1000 break;
1001 case 4:
1002 /* Wi-Fi Protected Setup (WPS) IE */
1003 elems->wps_ie = pos;
1004 elems->wps_ie_len = elen;
1005 break;
1006 default:
1007 DBG_8723A("Unknown Microsoft "
1008 "information element ignored "
1009 "(type =%d len =%lu)\n",
1010 pos[3], (unsigned long) elen);
1011 return -1;
1012 }
1013 break;
1014
1015 case OUI_BROADCOM:
1016 switch (pos[3]) {
1017 case VENDOR_HT_CAPAB_OUI_TYPE:
1018 elems->vendor_ht_cap = pos;
1019 elems->vendor_ht_cap_len = elen;
1020 break;
1021 default:
1022 DBG_8723A("Unknown Broadcom "
1023 "information element ignored "
1024 "(type =%d len =%lu)\n",
1025 pos[3], (unsigned long) elen);
1026 return -1;
1027 }
1028 break;
1029
1030 default:
1031 DBG_8723A("unknown vendor specific information "
1032 "element ignored (vendor OUI %02x:%02x:%02x "
1033 "len =%lu)\n",
1034 pos[0], pos[1], pos[2], (unsigned long) elen);
1035 return -1;
1036 }
1037
1038 return 0;
1039 }
1040
1041 /**
1042 * ieee802_11_parse_elems - Parse information elements in management frames
1043 * @start: Pointer to the start of IEs
1044 * @len: Length of IE buffer in octets
1045 * @elems: Data structure for parsed elements
1046 * @show_errors: Whether to show parsing errors in debug log
1047 * Returns: Parsing result
1048 */
1049 enum parse_res rtw_ieee802_11_parse_elems23a(u8 *start, uint len,
1050 struct rtw_ieee802_11_elems *elems,
1051 int show_errors)
1052 {
1053 uint left = len;
1054 u8 *pos = start;
1055 int unknown = 0;
1056
1057 memset(elems, 0, sizeof(*elems));
1058
1059 while (left >= 2) {
1060 u8 id, elen;
1061
1062 id = *pos++;
1063 elen = *pos++;
1064 left -= 2;
1065
1066 if (elen > left) {
1067 if (show_errors) {
1068 DBG_8723A("IEEE 802.11 element "
1069 "parse failed (id =%d elen =%d "
1070 "left =%lu)\n",
1071 id, elen, (unsigned long) left);
1072 }
1073 return ParseFailed;
1074 }
1075
1076 switch (id) {
1077 case WLAN_EID_SSID:
1078 elems->ssid = pos;
1079 elems->ssid_len = elen;
1080 break;
1081 case WLAN_EID_SUPP_RATES:
1082 elems->supp_rates = pos;
1083 elems->supp_rates_len = elen;
1084 break;
1085 case WLAN_EID_FH_PARAMS:
1086 elems->fh_params = pos;
1087 elems->fh_params_len = elen;
1088 break;
1089 case WLAN_EID_DS_PARAMS:
1090 elems->ds_params = pos;
1091 elems->ds_params_len = elen;
1092 break;
1093 case WLAN_EID_CF_PARAMS:
1094 elems->cf_params = pos;
1095 elems->cf_params_len = elen;
1096 break;
1097 case WLAN_EID_TIM:
1098 elems->tim = pos;
1099 elems->tim_len = elen;
1100 break;
1101 case WLAN_EID_IBSS_PARAMS:
1102 elems->ibss_params = pos;
1103 elems->ibss_params_len = elen;
1104 break;
1105 case WLAN_EID_CHALLENGE:
1106 elems->challenge = pos;
1107 elems->challenge_len = elen;
1108 break;
1109 case WLAN_EID_ERP_INFO:
1110 elems->erp_info = pos;
1111 elems->erp_info_len = elen;
1112 break;
1113 case WLAN_EID_EXT_SUPP_RATES:
1114 elems->ext_supp_rates = pos;
1115 elems->ext_supp_rates_len = elen;
1116 break;
1117 case WLAN_EID_VENDOR_SPECIFIC:
1118 if (rtw_ieee802_11_parse_vendor_specific(pos, elen,
1119 elems,
1120 show_errors))
1121 unknown++;
1122 break;
1123 case WLAN_EID_RSN:
1124 elems->rsn_ie = pos;
1125 elems->rsn_ie_len = elen;
1126 break;
1127 case WLAN_EID_PWR_CAPABILITY:
1128 elems->power_cap = pos;
1129 elems->power_cap_len = elen;
1130 break;
1131 case WLAN_EID_SUPPORTED_CHANNELS:
1132 elems->supp_channels = pos;
1133 elems->supp_channels_len = elen;
1134 break;
1135 case WLAN_EID_MOBILITY_DOMAIN:
1136 elems->mdie = pos;
1137 elems->mdie_len = elen;
1138 break;
1139 case WLAN_EID_FAST_BSS_TRANSITION:
1140 elems->ftie = pos;
1141 elems->ftie_len = elen;
1142 break;
1143 case WLAN_EID_TIMEOUT_INTERVAL:
1144 elems->timeout_int = pos;
1145 elems->timeout_int_len = elen;
1146 break;
1147 case WLAN_EID_HT_CAPABILITY:
1148 elems->ht_capabilities = pos;
1149 elems->ht_capabilities_len = elen;
1150 break;
1151 case WLAN_EID_HT_OPERATION:
1152 elems->ht_operation = pos;
1153 elems->ht_operation_len = elen;
1154 break;
1155 default:
1156 unknown++;
1157 if (!show_errors)
1158 break;
1159 DBG_8723A("IEEE 802.11 element parse "
1160 "ignored unknown element (id =%d elen =%d)\n",
1161 id, elen);
1162 break;
1163 }
1164
1165 left -= elen;
1166 pos += elen;
1167 }
1168
1169 if (left)
1170 return ParseFailed;
1171
1172 return unknown ? ParseUnknown : ParseOK;
1173 }
1174
1175 static u8 key_char2num(u8 ch)
1176 {
1177 if ((ch >= '0') && (ch <= '9'))
1178 return ch - '0';
1179 else if ((ch >= 'a') && (ch <= 'f'))
1180 return ch - 'a' + 10;
1181 else if ((ch >= 'A') && (ch <= 'F'))
1182 return ch - 'A' + 10;
1183 else
1184 return 0xff;
1185 }
1186
1187 u8 str_2char2num23a(u8 hch, u8 lch)
1188 {
1189 return (key_char2num(hch) * 10) + key_char2num(lch);
1190 }
1191
1192 u8 key_2char2num23a(u8 hch, u8 lch)
1193 {
1194 return (key_char2num(hch) << 4) | key_char2num(lch);
1195 }
1196
1197 void rtw_macaddr_cfg23a(u8 *mac_addr)
1198 {
1199 u8 mac[ETH_ALEN];
1200 if (!mac_addr)
1201 return;
1202
1203 memcpy(mac, mac_addr, ETH_ALEN);
1204
1205 if (is_broadcast_ether_addr(mac) || is_zero_ether_addr(mac)) {
1206 mac[0] = 0x00;
1207 mac[1] = 0xe0;
1208 mac[2] = 0x4c;
1209 mac[3] = 0x87;
1210 mac[4] = 0x00;
1211 mac[5] = 0x00;
1212 /* use default mac addresss */
1213 memcpy(mac_addr, mac, ETH_ALEN);
1214 DBG_8723A("MAC Address from efuse error, assign default "
1215 "one !!!\n");
1216 }
1217 DBG_8723A("rtw_macaddr_cfg23a MAC Address = "MAC_FMT"\n",
1218 MAC_ARG(mac_addr));
1219 }
1220
1221 void dump_ies23a(u8 *buf, u32 buf_len) {
1222 u8* pos = (u8*)buf;
1223 u8 id, len;
1224
1225 while (pos-buf <= buf_len) {
1226 id = *pos;
1227 len = *(pos + 1);
1228
1229 DBG_8723A("%s ID:%u, LEN:%u\n", __func__, id, len);
1230 #ifdef CONFIG_8723AU_P2P
1231 dump_p2p_ie23a(pos, len);
1232 #endif
1233 dump_wps_ie23a(pos, len);
1234
1235 pos += (2 + len);
1236 }
1237 }
1238
1239 void dump_wps_ie23a(u8 *ie, u32 ie_len) {
1240 u8* pos = (u8*)ie;
1241 u16 id;
1242 u16 len;
1243
1244 u8 *wps_ie;
1245 uint wps_ielen;
1246
1247 wps_ie = rtw_get_wps_ie23a(ie, ie_len, NULL, &wps_ielen);
1248 if (wps_ie != ie || wps_ielen == 0)
1249 return;
1250
1251 pos+= 6;
1252 while (pos-ie < ie_len) {
1253 id = get_unaligned_be16(pos);
1254 len = get_unaligned_be16(pos + 2);
1255
1256 DBG_8723A("%s ID:0x%04x, LEN:%u\n", __func__, id, len);
1257
1258 pos += (4 + len);
1259 }
1260 }
1261
1262 #ifdef CONFIG_8723AU_P2P
1263 void dump_p2p_ie23a(u8 *ie, u32 ie_len) {
1264 u8* pos = (u8*)ie;
1265 u8 id;
1266 u16 len;
1267
1268 u8 *p2p_ie;
1269 uint p2p_ielen;
1270
1271 p2p_ie = rtw_get_p2p_ie23a(ie, ie_len, NULL, &p2p_ielen);
1272 if (p2p_ie != ie || p2p_ielen == 0)
1273 return;
1274
1275 pos += 6;
1276 while (pos-ie < ie_len) {
1277 id = *pos;
1278 len = get_unaligned_le16(pos+1);
1279
1280 DBG_8723A("%s ID:%u, LEN:%u\n", __func__, id, len);
1281
1282 pos+= (3+len);
1283 }
1284 }
1285
1286 /**
1287 * rtw_get_p2p_ie23a - Search P2P IE from a series of IEs
1288 * @in_ie: Address of IEs to search
1289 * @in_len: Length limit from in_ie
1290 * @p2p_ie: If not NULL and P2P IE is found, P2P IE will be copied to the
1291 * buf starting from p2p_ie
1292 * @p2p_ielen: If not NULL and P2P IE is found, will set to the length of
1293 * the entire P2P IE
1294 *
1295 * Returns: The address of the P2P IE found, or NULL
1296 */
1297 u8 *rtw_get_p2p_ie23a(u8 *in_ie, int in_len, u8 *p2p_ie, uint *p2p_ielen)
1298 {
1299 uint cnt = 0;
1300 u8 *p2p_ie_ptr;
1301 u8 eid, p2p_oui[4]={0x50, 0x6F, 0x9A, 0x09};
1302
1303 if (p2p_ielen)
1304 *p2p_ielen = 0;
1305
1306 while (cnt<in_len) {
1307 eid = in_ie[cnt];
1308 if ((in_len < 0) || (cnt > MAX_IE_SZ)) {
1309 dump_stack();
1310 return NULL;
1311 }
1312 if ((eid == WLAN_EID_VENDOR_SPECIFIC) &&
1313 !memcmp(&in_ie[cnt + 2], p2p_oui, 4)) {
1314 p2p_ie_ptr = in_ie + cnt;
1315
1316 if (p2p_ie != NULL) {
1317 memcpy(p2p_ie, &in_ie[cnt],
1318 in_ie[cnt + 1] + 2);
1319 }
1320
1321 if (p2p_ielen != NULL) {
1322 *p2p_ielen = in_ie[cnt + 1] + 2;
1323 }
1324
1325 return p2p_ie_ptr;
1326
1327 break;
1328 } else {
1329 cnt += in_ie[cnt + 1] + 2; /* goto next */
1330 }
1331 }
1332
1333 return NULL;
1334 }
1335
1336 /**
1337 * rtw_get_p2p_attr23a - Search a specific P2P attribute from a given P2P IE
1338 * @p2p_ie: Address of P2P IE to search
1339 * @p2p_ielen: Length limit from p2p_ie
1340 * @target_attr_id: The attribute ID of P2P attribute to search
1341 * @buf_attr: If not NULL and the P2P attribute is found, P2P attribute will
1342 * be copied to the buf starting from buf_attr
1343 * @len_attr: If not NULL and the P2P attribute is found, will set to the
1344 * length of the entire P2P attribute
1345 *
1346 * Returns: the address of the specific WPS attribute found, or NULL
1347 */
1348 u8 *rtw_get_p2p_attr23a(u8 *p2p_ie, uint p2p_ielen, u8 target_attr_id,
1349 u8 *buf_attr, u32 *len_attr)
1350 {
1351 u8 *attr_ptr = NULL;
1352 u8 *target_attr_ptr = NULL;
1353 u8 p2p_oui[4]={0x50, 0x6F, 0x9A, 0x09};
1354
1355 if (len_attr)
1356 *len_attr = 0;
1357
1358 if (!p2p_ie || (p2p_ie[0] != WLAN_EID_VENDOR_SPECIFIC) ||
1359 memcmp(p2p_ie + 2, p2p_oui, 4)) {
1360 return attr_ptr;
1361 }
1362
1363 /* 6 = 1(Element ID) + 1(Length) + 3 (OUI) + 1(OUI Type) */
1364 attr_ptr = p2p_ie + 6; /* goto first attr */
1365
1366 while (attr_ptr - p2p_ie < p2p_ielen) {
1367 /* 3 = 1(Attribute ID) + 2(Length) */
1368 u8 attr_id = *attr_ptr;
1369 u16 attr_data_len = get_unaligned_le16(attr_ptr + 1);
1370 u16 attr_len = attr_data_len + 3;
1371
1372 /* DBG_8723A("%s attr_ptr:%p, id:%u, length:%u\n", __func__, attr_ptr, attr_id, attr_data_len); */
1373 if (attr_id == target_attr_id) {
1374 target_attr_ptr = attr_ptr;
1375
1376 if (buf_attr)
1377 memcpy(buf_attr, attr_ptr, attr_len);
1378
1379 if (len_attr)
1380 *len_attr = attr_len;
1381
1382 break;
1383 } else {
1384 attr_ptr += attr_len; /* goto next */
1385 }
1386 }
1387
1388 return target_attr_ptr;
1389 }
1390
1391 /**
1392 * rtw_get_p2p_attr23a_content - Search a specific P2P attribute content from
1393 * a given P2P IE
1394 * @p2p_ie: Address of P2P IE to search
1395 * @p2p_ielen: Length limit from p2p_ie
1396 * @target_attr_id: The attribute ID of P2P attribute to search
1397 * @buf_content: If not NULL and the P2P attribute is found, P2P attribute
1398 * content will be copied to the buf starting from buf_content
1399 * @len_content: If not NULL and the P2P attribute is found, will set to the
1400 * length of the P2P attribute content
1401 *
1402 * Returns: the address of the specific P2P attribute content found, or NULL
1403 */
1404 u8 *rtw_get_p2p_attr23a_content(u8 *p2p_ie, uint p2p_ielen, u8 target_attr_id,
1405 u8 *buf_content, uint *len_content)
1406 {
1407 u8 *attr_ptr;
1408 u32 attr_len;
1409
1410 if (len_content)
1411 *len_content = 0;
1412
1413 attr_ptr = rtw_get_p2p_attr23a(p2p_ie, p2p_ielen, target_attr_id,
1414 NULL, &attr_len);
1415
1416 if (attr_ptr && attr_len) {
1417 if (buf_content)
1418 memcpy(buf_content, attr_ptr + 3, attr_len - 3);
1419
1420 if (len_content)
1421 *len_content = attr_len - 3;
1422
1423 return attr_ptr+3;
1424 }
1425
1426 return NULL;
1427 }
1428
1429 u32 rtw_set_p2p_attr_content23a(u8 *pbuf, u8 attr_id, u16 attr_len, u8 *pdata_attr)
1430 {
1431 u32 a_len;
1432
1433 *pbuf = attr_id;
1434
1435 /* u16*)(pbuf + 1) = cpu_to_le16(attr_len); */
1436 put_unaligned_le16(attr_len, pbuf + 1);
1437
1438 if (pdata_attr)
1439 memcpy(pbuf + 3, pdata_attr, attr_len);
1440
1441 a_len = attr_len + 3;
1442
1443 return a_len;
1444 }
1445
1446 static uint rtw_p2p_attr_remove(u8 *ie, uint ielen_ori, u8 attr_id)
1447 {
1448 u8 *target_attr;
1449 u32 target_attr_len;
1450 uint ielen = ielen_ori;
1451
1452 while(1) {
1453 target_attr = rtw_get_p2p_attr23a(ie, ielen, attr_id, NULL,
1454 &target_attr_len);
1455 if (target_attr && target_attr_len) {
1456 u8 *next_attr = target_attr+target_attr_len;
1457 uint remain_len = ielen-(next_attr-ie);
1458 /* dump_ies23a(ie, ielen); */
1459
1460 memset(target_attr, 0, target_attr_len);
1461 memcpy(target_attr, next_attr, remain_len);
1462 memset(target_attr+remain_len, 0, target_attr_len);
1463 *(ie + 1) -= target_attr_len;
1464 ielen -= target_attr_len;
1465 } else {
1466 /* if (index>0) */
1467 /* dump_ies23a(ie, ielen); */
1468 break;
1469 }
1470 }
1471
1472 return ielen;
1473 }
1474
1475 void rtw_wlan_bssid_ex_remove_p2p_attr23a(struct wlan_bssid_ex *bss_ex, u8 attr_id)
1476 {
1477 u8 *p2p_ie;
1478 uint p2p_ielen, p2p_ielen_ori;
1479
1480 if ((p2p_ie = rtw_get_p2p_ie23a(bss_ex->IEs + _FIXED_IE_LENGTH_,
1481 bss_ex->IELength - _FIXED_IE_LENGTH_,
1482 NULL, &p2p_ielen_ori))) {
1483 p2p_ielen = rtw_p2p_attr_remove(p2p_ie, p2p_ielen_ori, attr_id);
1484 if (p2p_ielen != p2p_ielen_ori) {
1485 u8 *next_ie_ori = p2p_ie+p2p_ielen_ori;
1486 u8 *next_ie = p2p_ie+p2p_ielen;
1487 uint remain_len;
1488 remain_len = bss_ex->IELength-(next_ie_ori-bss_ex->IEs);
1489
1490 memcpy(next_ie, next_ie_ori, remain_len);
1491 memset(next_ie+remain_len, 0, p2p_ielen_ori-p2p_ielen);
1492 bss_ex->IELength -= p2p_ielen_ori-p2p_ielen;
1493 }
1494 }
1495 }
1496
1497 #endif /* CONFIG_8723AU_P2P */
1498
1499 #ifdef CONFIG_8723AU_P2P
1500 int rtw_get_wfd_ie(u8 *in_ie, int in_len, u8 *wfd_ie, uint *wfd_ielen)
1501 {
1502 int match;
1503 const u8 *ie;
1504
1505 match = 0;
1506
1507 if (in_len < 0)
1508 return match;
1509
1510 ie = cfg80211_find_vendor_ie(0x506F9A, 0x0A, in_ie, in_len);
1511 if (ie && (ie[1] <= (MAX_WFD_IE_LEN - 2))) {
1512 if (wfd_ie) {
1513 *wfd_ielen = ie[1] + 2;
1514 memcpy(wfd_ie, ie, ie[1] + 2);
1515 } else
1516 if (wfd_ielen)
1517 *wfd_ielen = 0;
1518
1519 match = 1;
1520 }
1521
1522 return match;
1523 }
1524
1525 /* attr_content: The output buffer, contains the "body field" of
1526 WFD attribute. */
1527 /* attr_contentlen: The data length of the "body field" of WFD
1528 attribute. */
1529 int rtw_get_wfd_attr_content(u8 *wfd_ie, uint wfd_ielen, u8 target_attr_id,
1530 u8 *attr_content, uint *attr_contentlen)
1531 {
1532 int match;
1533 uint cnt = 0;
1534 u8 attr_id, wfd_oui[4] = {0x50, 0x6F, 0x9A, 0x0A};
1535
1536 match = false;
1537
1538 if ((wfd_ie[0] != WLAN_EID_VENDOR_SPECIFIC) ||
1539 memcmp(wfd_ie + 2, wfd_oui, 4)) {
1540 return match;
1541 }
1542
1543 /* 1 (WFD IE) + 1 (Length) + 3 (OUI) + 1 (OUI Type) */
1544 cnt = 6;
1545 while (cnt < wfd_ielen) {
1546 u16 attrlen = get_unaligned_be16(wfd_ie + cnt + 1);
1547
1548 attr_id = wfd_ie[cnt];
1549 if (attr_id == target_attr_id) {
1550 /* 3 -> 1 byte for attribute ID field, 2
1551 bytes for length field */
1552 if (attr_content)
1553 memcpy(attr_content, &wfd_ie[cnt + 3], attrlen);
1554
1555 if (attr_contentlen)
1556 *attr_contentlen = attrlen;
1557
1558 cnt += attrlen + 3;
1559
1560 match = true;
1561 break;
1562 } else {
1563 cnt += attrlen + 3; /* goto next */
1564 }
1565 }
1566
1567 return match;
1568 }
1569 #endif /* CONFIG_8723AU_P2P */
1570
1571 /* Baron adds to avoid FreeBSD warning */
1572 int ieee80211_is_empty_essid23a(const char *essid, int essid_len)
1573 {
1574 /* Single white space is for Linksys APs */
1575 if (essid_len == 1 && essid[0] == ' ')
1576 return 1;
1577
1578 /* Otherwise, if the entire essid is 0, we assume it is hidden */
1579 while (essid_len) {
1580 essid_len--;
1581 if (essid[essid_len] != '\0')
1582 return 0;
1583 }
1584
1585 return 1;
1586 }
1587
1588 static int rtw_get_cipher_info(struct wlan_network *pnetwork)
1589 {
1590 u32 wpa_ielen;
1591 unsigned char *pbuf;
1592 int group_cipher = 0, pairwise_cipher = 0, is8021x = 0;
1593 int ret = _FAIL;
1594 int r;
1595 pbuf = rtw_get_wpa_ie23a(&pnetwork->network.IEs[12], &wpa_ielen,
1596 pnetwork->network.IELength - 12);
1597
1598 if (pbuf && (wpa_ielen > 0)) {
1599 RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
1600 ("rtw_get_cipher_info: wpa_ielen: %d", wpa_ielen));
1601 r = rtw_parse_wpa_ie23a(pbuf, wpa_ielen + 2, &group_cipher,
1602 &pairwise_cipher, &is8021x);
1603 if (r == _SUCCESS) {
1604 pnetwork->BcnInfo.pairwise_cipher = pairwise_cipher;
1605 pnetwork->BcnInfo.group_cipher = group_cipher;
1606 pnetwork->BcnInfo.is_8021x = is8021x;
1607 RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
1608 ("%s: pnetwork->pairwise_cipher: %d, is_"
1609 "8021x is %d", __func__,
1610 pnetwork->BcnInfo.pairwise_cipher,
1611 pnetwork->BcnInfo.is_8021x));
1612 ret = _SUCCESS;
1613 }
1614 } else {
1615 pbuf = rtw_get_wpa2_ie23a(&pnetwork->network.IEs[12], &wpa_ielen,
1616 pnetwork->network.IELength - 12);
1617
1618 if (pbuf && (wpa_ielen > 0)) {
1619 RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
1620 ("get RSN IE\n"));
1621 r = rtw_parse_wpa2_ie23a(pbuf, wpa_ielen + 2,
1622 &group_cipher, &pairwise_cipher,
1623 &is8021x);
1624 if (r == _SUCCESS) {
1625 RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
1626 ("get RSN IE OK!!!\n"));
1627 pnetwork->BcnInfo.pairwise_cipher =
1628 pairwise_cipher;
1629 pnetwork->BcnInfo.group_cipher = group_cipher;
1630 pnetwork->BcnInfo.is_8021x = is8021x;
1631 RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
1632 ("%s: pnetwork->pairwise_cipher: %d,"
1633 "pnetwork->group_cipher is %d, "
1634 "is_8021x is %d", __func__,
1635 pnetwork->BcnInfo.pairwise_cipher,
1636 pnetwork->BcnInfo.group_cipher,
1637 pnetwork->BcnInfo.is_8021x));
1638 ret = _SUCCESS;
1639 }
1640 }
1641 }
1642
1643 return ret;
1644 }
1645
1646 void rtw_get_bcn_info23a(struct wlan_network *pnetwork)
1647 {
1648 unsigned short cap = 0;
1649 u8 bencrypt = 0;
1650 /* u8 wpa_ie[255], rsn_ie[255]; */
1651 u16 wpa_len = 0, rsn_len = 0;
1652 struct HT_info_element *pht_info = NULL;
1653 struct ieee80211_ht_cap *pht_cap = NULL;
1654 unsigned int len;
1655 unsigned char *p;
1656
1657 memcpy(&cap, rtw_get_capability23a_from_ie(pnetwork->network.IEs), 2);
1658 cap = le16_to_cpu(cap);
1659 if (cap & WLAN_CAPABILITY_PRIVACY) {
1660 bencrypt = 1;
1661 pnetwork->network.Privacy = 1;
1662 } else {
1663 pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_OPENSYS;
1664 }
1665 rtw_get_sec_ie23a(pnetwork->network.IEs, pnetwork->network.IELength,
1666 NULL, &rsn_len, NULL, &wpa_len);
1667 RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
1668 ("rtw_get_bcn_info23a: ssid =%s\n", pnetwork->network.Ssid.ssid));
1669 RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
1670 ("rtw_get_bcn_info23a: wpa_len =%d rsn_len =%d\n",
1671 wpa_len, rsn_len));
1672 RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
1673 ("rtw_get_bcn_info23a: ssid =%s\n", pnetwork->network.Ssid.ssid));
1674 RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
1675 ("rtw_get_bcn_info23a: wpa_len =%d rsn_len =%d\n",
1676 wpa_len, rsn_len));
1677
1678 if (rsn_len > 0) {
1679 pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_WPA2;
1680 } else if (wpa_len > 0) {
1681 pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_WPA;
1682 } else {
1683 if (bencrypt)
1684 pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_WEP;
1685 }
1686 RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
1687 ("rtw_get_bcn_info23a: pnetwork->encryp_protocol is %x\n",
1688 pnetwork->BcnInfo.encryp_protocol));
1689 RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
1690 ("rtw_get_bcn_info23a: pnetwork->encryp_protocol is %x\n",
1691 pnetwork->BcnInfo.encryp_protocol));
1692 rtw_get_cipher_info(pnetwork);
1693
1694 /* get bwmode and ch_offset */
1695 /* parsing HT_CAP_IE */
1696 p = rtw_get_ie23a(pnetwork->network.IEs + _FIXED_IE_LENGTH_,
1697 _HT_CAPABILITY_IE_, &len,
1698 pnetwork->network.IELength - _FIXED_IE_LENGTH_);
1699 if (p && len > 0) {
1700 pht_cap = (struct ieee80211_ht_cap *)(p + 2);
1701 pnetwork->BcnInfo.ht_cap_info = pht_cap->cap_info;
1702 } else {
1703 pnetwork->BcnInfo.ht_cap_info = 0;
1704 }
1705 /* parsing HT_INFO_IE */
1706 p = rtw_get_ie23a(pnetwork->network.IEs + _FIXED_IE_LENGTH_,
1707 _HT_ADD_INFO_IE_, &len,
1708 pnetwork->network.IELength - _FIXED_IE_LENGTH_);
1709 if (p && len > 0) {
1710 pht_info = (struct HT_info_element *)(p + 2);
1711 pnetwork->BcnInfo.ht_info_infos_0 = pht_info->infos[0];
1712 } else {
1713 pnetwork->BcnInfo.ht_info_infos_0 = 0;
1714 }
1715 }
1716
1717 /* show MCS rate, unit: 100Kbps */
1718 u16 rtw_mcs_rate23a(u8 rf_type, u8 bw_40MHz, u8 short_GI_20, u8 short_GI_40,
1719 unsigned char * MCS_rate)
1720 {
1721 u16 max_rate = 0;
1722
1723 if (rf_type == RF_1T1R) {
1724 if (MCS_rate[0] & BIT(7))
1725 max_rate = (bw_40MHz) ? ((short_GI_40)?1500:1350):
1726 ((short_GI_20)?722:650);
1727 else if (MCS_rate[0] & BIT(6))
1728 max_rate = (bw_40MHz) ? ((short_GI_40)?1350:1215):
1729 ((short_GI_20)?650:585);
1730 else if (MCS_rate[0] & BIT(5))
1731 max_rate = (bw_40MHz) ? ((short_GI_40)?1200:1080):
1732 ((short_GI_20)?578:520);
1733 else if (MCS_rate[0] & BIT(4))
1734 max_rate = (bw_40MHz) ? ((short_GI_40)?900:810):
1735 ((short_GI_20)?433:390);
1736 else if (MCS_rate[0] & BIT(3))
1737 max_rate = (bw_40MHz) ? ((short_GI_40)?600:540):
1738 ((short_GI_20)?289:260);
1739 else if (MCS_rate[0] & BIT(2))
1740 max_rate = (bw_40MHz) ? ((short_GI_40)?450:405):
1741 ((short_GI_20)?217:195);
1742 else if (MCS_rate[0] & BIT(1))
1743 max_rate = (bw_40MHz) ? ((short_GI_40)?300:270):
1744 ((short_GI_20)?144:130);
1745 else if (MCS_rate[0] & BIT(0))
1746 max_rate = (bw_40MHz) ? ((short_GI_40)?150:135):
1747 ((short_GI_20)?72:65);
1748 } else {
1749 if (MCS_rate[1]) {
1750 if (MCS_rate[1] & BIT(7))
1751 max_rate = (bw_40MHz) ? ((short_GI_40)?3000:2700):((short_GI_20)?1444:1300);
1752 else if (MCS_rate[1] & BIT(6))
1753 max_rate = (bw_40MHz) ? ((short_GI_40)?2700:2430):((short_GI_20)?1300:1170);
1754 else if (MCS_rate[1] & BIT(5))
1755 max_rate = (bw_40MHz) ? ((short_GI_40)?2400:2160):((short_GI_20)?1156:1040);
1756 else if (MCS_rate[1] & BIT(4))
1757 max_rate = (bw_40MHz) ? ((short_GI_40)?1800:1620):((short_GI_20)?867:780);
1758 else if (MCS_rate[1] & BIT(3))
1759 max_rate = (bw_40MHz) ? ((short_GI_40)?1200:1080):((short_GI_20)?578:520);
1760 else if (MCS_rate[1] & BIT(2))
1761 max_rate = (bw_40MHz) ? ((short_GI_40)?900:810):((short_GI_20)?433:390);
1762 else if (MCS_rate[1] & BIT(1))
1763 max_rate = (bw_40MHz) ? ((short_GI_40)?600:540):((short_GI_20)?289:260);
1764 else if (MCS_rate[1] & BIT(0))
1765 max_rate = (bw_40MHz) ? ((short_GI_40)?300:270):((short_GI_20)?144:130);
1766 } else {
1767 if (MCS_rate[0] & BIT(7))
1768 max_rate = (bw_40MHz) ? ((short_GI_40)?1500:1350):((short_GI_20)?722:650);
1769 else if (MCS_rate[0] & BIT(6))
1770 max_rate = (bw_40MHz) ? ((short_GI_40)?1350:1215):((short_GI_20)?650:585);
1771 else if (MCS_rate[0] & BIT(5))
1772 max_rate = (bw_40MHz) ? ((short_GI_40)?1200:1080):((short_GI_20)?578:520);
1773 else if (MCS_rate[0] & BIT(4))
1774 max_rate = (bw_40MHz) ? ((short_GI_40)?900:810):((short_GI_20)?433:390);
1775 else if (MCS_rate[0] & BIT(3))
1776 max_rate = (bw_40MHz) ? ((short_GI_40)?600:540):((short_GI_20)?289:260);
1777 else if (MCS_rate[0] & BIT(2))
1778 max_rate = (bw_40MHz) ? ((short_GI_40)?450:405):((short_GI_20)?217:195);
1779 else if (MCS_rate[0] & BIT(1))
1780 max_rate = (bw_40MHz) ? ((short_GI_40)?300:270):((short_GI_20)?144:130);
1781 else if (MCS_rate[0] & BIT(0))
1782 max_rate = (bw_40MHz) ? ((short_GI_40)?150:135):((short_GI_20)?72:65);
1783 }
1784 }
1785 return max_rate;
1786 }
1787
1788 int rtw_action_frame_parse23a(const u8 *frame, u32 frame_len, u8* category,
1789 u8 *action)
1790 {
1791 const u8 *frame_body = frame + sizeof(struct ieee80211_hdr_3addr);
1792 u16 fc;
1793 u8 c, a = 0;
1794
1795 fc = le16_to_cpu(((struct ieee80211_hdr_3addr *)frame)->frame_control);
1796
1797 if ((fc & (IEEE80211_FCTL_FTYPE|IEEE80211_FCTL_STYPE)) !=
1798 (IEEE80211_FTYPE_MGMT|IEEE80211_STYPE_ACTION)) {
1799 return false;
1800 }
1801
1802 c = frame_body[0];
1803
1804 switch (c) {
1805 case WLAN_CATEGORY_VENDOR_SPECIFIC: /* vendor-specific */
1806 break;
1807 default:
1808 a = frame_body[1];
1809 }
1810
1811 if (category)
1812 *category = c;
1813 if (action)
1814 *action = a;
1815
1816 return true;
1817 }
1818
1819 static const char *_action_public_str23a[] = {
1820 "ACT_PUB_BSSCOEXIST",
1821 "ACT_PUB_DSE_ENABLE",
1822 "ACT_PUB_DSE_DEENABLE",
1823 "ACT_PUB_DSE_REG_LOCATION",
1824 "ACT_PUB_EXT_CHL_SWITCH",
1825 "ACT_PUB_DSE_MSR_REQ",
1826 "ACT_PUB_DSE_MSR_RPRT",
1827 "ACT_PUB_MP",
1828 "ACT_PUB_DSE_PWR_CONSTRAINT",
1829 "ACT_PUB_VENDOR",
1830 "ACT_PUB_GAS_INITIAL_REQ",
1831 "ACT_PUB_GAS_INITIAL_RSP",
1832 "ACT_PUB_GAS_COMEBACK_REQ",
1833 "ACT_PUB_GAS_COMEBACK_RSP",
1834 "ACT_PUB_TDLS_DISCOVERY_RSP",
1835 "ACT_PUB_LOCATION_TRACK",
1836 "ACT_PUB_RSVD",
1837 };
1838
1839 const char *action_public_str23a(u8 action)
1840 {
1841 action = (action >= ACT_PUBLIC_MAX) ? ACT_PUBLIC_MAX : action;
1842 return _action_public_str23a[action];
1843 }
Ubuntu Artful kernel mirror