1 /******************************************************************************
2 *
3 * Copyright(c) 2007 - 2017 Realtek Corporation.
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 #ifdef CONFIG_PLATFORM_INTEL_BYT
18 #include <linux/fs.h>
19 #endif
20 #include <drv_types.h>
21
22
23 u8 RTW_WPA_OUI_TYPE[] = { 0x00, 0x50, 0xf2, 1 };
24 u16 RTW_WPA_VERSION = 1;
25 u8 WPA_AUTH_KEY_MGMT_NONE[] = { 0x00, 0x50, 0xf2, 0 };
26 u8 WPA_AUTH_KEY_MGMT_UNSPEC_802_1X[] = { 0x00, 0x50, 0xf2, 1 };
27 u8 WPA_AUTH_KEY_MGMT_PSK_OVER_802_1X[] = { 0x00, 0x50, 0xf2, 2 };
28 u8 WPA_CIPHER_SUITE_NONE[] = { 0x00, 0x50, 0xf2, 0 };
29 u8 WPA_CIPHER_SUITE_WEP40[] = { 0x00, 0x50, 0xf2, 1 };
30 u8 WPA_CIPHER_SUITE_TKIP[] = { 0x00, 0x50, 0xf2, 2 };
31 u8 WPA_CIPHER_SUITE_WRAP[] = { 0x00, 0x50, 0xf2, 3 };
32 u8 WPA_CIPHER_SUITE_CCMP[] = { 0x00, 0x50, 0xf2, 4 };
33 u8 WPA_CIPHER_SUITE_WEP104[] = { 0x00, 0x50, 0xf2, 5 };
34
35 u16 RSN_VERSION_BSD = 1;
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_AES_128_CMAC[] = { 0x00, 0x0f, 0xac, 6 };
42 u8 RSN_CIPHER_SUITE_GCMP[] = { 0x00, 0x0f, 0xac, 8 };
43 u8 RSN_CIPHER_SUITE_GCMP_256[] = { 0x00, 0x0f, 0xac, 9 };
44 u8 RSN_CIPHER_SUITE_CCMP_256[] = { 0x00, 0x0f, 0xac, 10 };
45 u8 RSN_CIPHER_SUITE_BIP_GMAC_128[] = { 0x00, 0x0f, 0xac, 11 };
46 u8 RSN_CIPHER_SUITE_BIP_GMAC_256[] = { 0x00, 0x0f, 0xac, 12 };
47 u8 RSN_CIPHER_SUITE_BIP_CMAC_256[] = { 0x00, 0x0f, 0xac, 13 };
48 u8 RSN_CIPHER_SUITE_WEP104[] = { 0x00, 0x0f, 0xac, 5 };
49
50 u8 WLAN_AKM_8021X[] = {0x00, 0x0f, 0xac, 1};
51 u8 WLAN_AKM_PSK[] = {0x00, 0x0f, 0xac, 2};
52 u8 WLAN_AKM_FT_8021X[] = {0x00, 0x0f, 0xac, 3};
53 u8 WLAN_AKM_FT_PSK[] = {0x00, 0x0f, 0xac, 4};
54 u8 WLAN_AKM_8021X_SHA256[] = {0x00, 0x0f, 0xac, 5};
55 u8 WLAN_AKM_PSK_SHA256[] = {0x00, 0x0f, 0xac, 6};
56 u8 WLAN_AKM_TDLS[] = {0x00, 0x0f, 0xac, 7};
57 u8 WLAN_AKM_SAE[] = {0x00, 0x0f, 0xac, 8};
58 u8 WLAN_AKM_FT_OVER_SAE[] = {0x00, 0x0f, 0xac, 9};
59 u8 WLAN_AKM_8021X_SUITE_B[] = {0x00, 0x0f, 0xac, 11};
60 u8 WLAN_AKM_8021X_SUITE_B_192[] = {0x00, 0x0f, 0xac, 12};
61 u8 WLAN_AKM_FILS_SHA256[] = {0x00, 0x0f, 0xac, 14};
62 u8 WLAN_AKM_FILS_SHA384[] = {0x00, 0x0f, 0xac, 15};
63 u8 WLAN_AKM_FT_FILS_SHA256[] = {0x00, 0x0f, 0xac, 16};
64 u8 WLAN_AKM_FT_FILS_SHA384[] = {0x00, 0x0f, 0xac, 17};
65 /* -----------------------------------------------------------
66 * for adhoc-master to generate ie and provide supported-rate to fw
67 * ----------------------------------------------------------- */
68
69 u8 WIFI_CCKRATES[] = {
70 (IEEE80211_CCK_RATE_1MB | IEEE80211_BASIC_RATE_MASK),
71 (IEEE80211_CCK_RATE_2MB | IEEE80211_BASIC_RATE_MASK),
72 (IEEE80211_CCK_RATE_5MB | IEEE80211_BASIC_RATE_MASK),
73 (IEEE80211_CCK_RATE_11MB | IEEE80211_BASIC_RATE_MASK)
74 };
75
76 u8 WIFI_OFDMRATES[] = {
77 (IEEE80211_OFDM_RATE_6MB),
78 (IEEE80211_OFDM_RATE_9MB),
79 (IEEE80211_OFDM_RATE_12MB),
80 (IEEE80211_OFDM_RATE_18MB),
81 (IEEE80211_OFDM_RATE_24MB),
82 IEEE80211_OFDM_RATE_36MB,
83 IEEE80211_OFDM_RATE_48MB,
84 IEEE80211_OFDM_RATE_54MB
85 };
86
87 u8 mgn_rates_cck[4] = {MGN_1M, MGN_2M, MGN_5_5M, MGN_11M};
88 u8 mgn_rates_ofdm[8] = {MGN_6M, MGN_9M, MGN_12M, MGN_18M, MGN_24M, MGN_36M, MGN_48M, MGN_54M};
89 u8 mgn_rates_mcs0_7[8] = {MGN_MCS0, MGN_MCS1, MGN_MCS2, MGN_MCS3, MGN_MCS4, MGN_MCS5, MGN_MCS6, MGN_MCS7};
90 u8 mgn_rates_mcs8_15[8] = {MGN_MCS8, MGN_MCS9, MGN_MCS10, MGN_MCS11, MGN_MCS12, MGN_MCS13, MGN_MCS14, MGN_MCS15};
91 u8 mgn_rates_mcs16_23[8] = {MGN_MCS16, MGN_MCS17, MGN_MCS18, MGN_MCS19, MGN_MCS20, MGN_MCS21, MGN_MCS22, MGN_MCS23};
92 u8 mgn_rates_mcs24_31[8] = {MGN_MCS24, MGN_MCS25, MGN_MCS26, MGN_MCS27, MGN_MCS28, MGN_MCS29, MGN_MCS30, MGN_MCS31};
93 u8 mgn_rates_vht1ss[10] = {MGN_VHT1SS_MCS0, MGN_VHT1SS_MCS1, MGN_VHT1SS_MCS2, MGN_VHT1SS_MCS3, MGN_VHT1SS_MCS4
94 , MGN_VHT1SS_MCS5, MGN_VHT1SS_MCS6, MGN_VHT1SS_MCS7, MGN_VHT1SS_MCS8, MGN_VHT1SS_MCS9
95 };
96 u8 mgn_rates_vht2ss[10] = {MGN_VHT2SS_MCS0, MGN_VHT2SS_MCS1, MGN_VHT2SS_MCS2, MGN_VHT2SS_MCS3, MGN_VHT2SS_MCS4
97 , MGN_VHT2SS_MCS5, MGN_VHT2SS_MCS6, MGN_VHT2SS_MCS7, MGN_VHT2SS_MCS8, MGN_VHT2SS_MCS9
98 };
99 u8 mgn_rates_vht3ss[10] = {MGN_VHT3SS_MCS0, MGN_VHT3SS_MCS1, MGN_VHT3SS_MCS2, MGN_VHT3SS_MCS3, MGN_VHT3SS_MCS4
100 , MGN_VHT3SS_MCS5, MGN_VHT3SS_MCS6, MGN_VHT3SS_MCS7, MGN_VHT3SS_MCS8, MGN_VHT3SS_MCS9
101 };
102 u8 mgn_rates_vht4ss[10] = {MGN_VHT4SS_MCS0, MGN_VHT4SS_MCS1, MGN_VHT4SS_MCS2, MGN_VHT4SS_MCS3, MGN_VHT4SS_MCS4
103 , MGN_VHT4SS_MCS5, MGN_VHT4SS_MCS6, MGN_VHT4SS_MCS7, MGN_VHT4SS_MCS8, MGN_VHT4SS_MCS9
104 };
105
mgn_rate_to_rs(enum MGN_RATE rate)106 RATE_SECTION mgn_rate_to_rs(enum MGN_RATE rate)
107 {
108 RATE_SECTION rs = RATE_SECTION_NUM;
109
110 if (IS_CCK_RATE(rate))
111 rs = CCK;
112 else if (IS_OFDM_RATE(rate))
113 rs = OFDM;
114 else if (IS_HT1SS_RATE(rate))
115 rs = HT_1SS;
116 else if (IS_HT2SS_RATE(rate))
117 rs = HT_2SS;
118 else if (IS_HT3SS_RATE(rate))
119 rs = HT_3SS;
120 else if (IS_HT4SS_RATE(rate))
121 rs = HT_4SS;
122 else if (IS_VHT1SS_RATE(rate))
123 rs = VHT_1SS;
124 else if (IS_VHT2SS_RATE(rate))
125 rs = VHT_2SS;
126 else if (IS_VHT3SS_RATE(rate))
127 rs = VHT_3SS;
128 else if (IS_VHT4SS_RATE(rate))
129 rs = VHT_4SS;
130
131 return rs;
132 }
133
134 static const char *const _rate_section_str[] = {
135 "CCK",
136 "OFDM",
137 "HT_1SS",
138 "HT_2SS",
139 "HT_3SS",
140 "HT_4SS",
141 "VHT_1SS",
142 "VHT_2SS",
143 "VHT_3SS",
144 "VHT_4SS",
145 "RATE_SECTION_UNKNOWN",
146 };
147
rate_section_str(u8 section)148 const char *rate_section_str(u8 section)
149 {
150 section = (section >= RATE_SECTION_NUM) ? RATE_SECTION_NUM : section;
151 return _rate_section_str[section];
152 }
153
154 struct rate_section_ent rates_by_sections[RATE_SECTION_NUM] = {
155 {RF_1TX, 4, mgn_rates_cck},
156 {RF_1TX, 8, mgn_rates_ofdm},
157 {RF_1TX, 8, mgn_rates_mcs0_7},
158 {RF_2TX, 8, mgn_rates_mcs8_15},
159 {RF_3TX, 8, mgn_rates_mcs16_23},
160 {RF_4TX, 8, mgn_rates_mcs24_31},
161 {RF_1TX, 10, mgn_rates_vht1ss},
162 {RF_2TX, 10, mgn_rates_vht2ss},
163 {RF_3TX, 10, mgn_rates_vht3ss},
164 {RF_4TX, 10, mgn_rates_vht4ss},
165 };
166
rtw_get_bit_value_from_ieee_value(u8 val)167 int rtw_get_bit_value_from_ieee_value(u8 val)
168 {
169 unsigned char dot11_rate_table[] = {2, 4, 11, 22, 12, 18, 24, 36, 48, 72, 96, 108, 0}; /* last element must be zero!! */
170
171 int i = 0;
172 while (dot11_rate_table[i] != 0) {
173 if (dot11_rate_table[i] == val)
174 return BIT(i);
175 i++;
176 }
177 return 0;
178 }
rtw_get_cckrate_size(u8 * rate,u32 rate_length)179 uint rtw_get_cckrate_size(u8 *rate, u32 rate_length)
180 {
181 int i = 0;
182 while(i < rate_length){
183 RTW_DBG("%s, rate[%d]=%u\n", __FUNCTION__, i, rate[i]);
184 if (((rate[i] & 0x7f) == 2) || ((rate[i] & 0x7f) == 4) ||
185 ((rate[i] & 0x7f) == 11) || ((rate[i] & 0x7f) == 22))
186 i++;
187 else
188 break;
189 }
190 return i;
191 }
192
rtw_is_cckrates_included(u8 * rate)193 uint rtw_is_cckrates_included(u8 *rate)
194 {
195 u32 i = 0;
196
197 while (rate[i] != 0) {
198 if ((((rate[i]) & 0x7f) == 2) || (((rate[i]) & 0x7f) == 4) ||
199 (((rate[i]) & 0x7f) == 11) || (((rate[i]) & 0x7f) == 22))
200 return _TRUE;
201 i++;
202 }
203
204 return _FALSE;
205 }
206
rtw_is_cckratesonly_included(u8 * rate)207 uint rtw_is_cckratesonly_included(u8 *rate)
208 {
209 u32 i = 0;
210
211
212 while (rate[i] != 0) {
213 if ((((rate[i]) & 0x7f) != 2) && (((rate[i]) & 0x7f) != 4) &&
214 (((rate[i]) & 0x7f) != 11) && (((rate[i]) & 0x7f) != 22))
215 return _FALSE;
216
217 i++;
218 }
219
220 return _TRUE;
221
222 }
223
rtw_check_network_type(unsigned char * rate,int ratelen,int channel)224 int rtw_check_network_type(unsigned char *rate, int ratelen, int channel)
225 {
226 if (channel > 14) {
227 if ((rtw_is_cckrates_included(rate)) == _TRUE)
228 return WIRELESS_INVALID;
229 else
230 return WIRELESS_11A;
231 } else { /* could be pure B, pure G, or B/G */
232 if ((rtw_is_cckratesonly_included(rate)) == _TRUE)
233 return WIRELESS_11B;
234 else if ((rtw_is_cckrates_included(rate)) == _TRUE)
235 return WIRELESS_11BG;
236 else
237 return WIRELESS_11G;
238 }
239
240 }
241
rtw_set_fixed_ie(unsigned char * pbuf,unsigned int len,unsigned char * source,unsigned int * frlen)242 u8 *rtw_set_fixed_ie(unsigned char *pbuf, unsigned int len, unsigned char *source,
243 unsigned int *frlen)
244 {
245 _rtw_memcpy((void *)pbuf, (void *)source, len);
246 *frlen = *frlen + len;
247 return pbuf + len;
248 }
249
250 /* rtw_set_ie will update frame length */
rtw_set_ie(u8 * pbuf,sint index,uint len,const u8 * source,uint * frlen)251 u8 *rtw_set_ie
252 (
253 u8 *pbuf,
254 sint index,
255 uint len,
256 const u8 *source,
257 uint *frlen /* frame length */
258 )
259 {
260 *pbuf = (u8)index;
261
262 *(pbuf + 1) = (u8)len;
263
264 if (len > 0)
265 _rtw_memcpy((void *)(pbuf + 2), (void *)source, len);
266
267 if (frlen)
268 *frlen = *frlen + (len + 2);
269
270 return pbuf + len + 2;
271 }
272
rtw_set_ie_tpc_report(u8 * buf,u32 * buf_len,u8 tx_power,u8 link_margin)273 u8 *rtw_set_ie_tpc_report(u8 *buf, u32 *buf_len, u8 tx_power, u8 link_margin)
274 {
275 u8 ie_data[2];
276
277 ie_data[0] = tx_power;
278 ie_data[1] = link_margin;
279 return rtw_set_ie(buf, WLAN_EID_TPC_REPORT, 2, ie_data, buf_len);
280 }
281
rtw_set_ie_ch_switch(u8 * buf,u32 * buf_len,u8 ch_switch_mode,u8 new_ch,u8 ch_switch_cnt)282 inline u8 *rtw_set_ie_ch_switch(u8 *buf, u32 *buf_len, u8 ch_switch_mode,
283 u8 new_ch, u8 ch_switch_cnt)
284 {
285 u8 ie_data[3];
286
287 ie_data[0] = ch_switch_mode;
288 ie_data[1] = new_ch;
289 ie_data[2] = ch_switch_cnt;
290 return rtw_set_ie(buf, WLAN_EID_CHANNEL_SWITCH, 3, ie_data, buf_len);
291 }
292
secondary_ch_offset_to_hal_ch_offset(u8 ch_offset)293 inline u8 secondary_ch_offset_to_hal_ch_offset(u8 ch_offset)
294 {
295 if (ch_offset == SCN)
296 return HAL_PRIME_CHNL_OFFSET_DONT_CARE;
297 else if (ch_offset == SCA)
298 return HAL_PRIME_CHNL_OFFSET_LOWER;
299 else if (ch_offset == SCB)
300 return HAL_PRIME_CHNL_OFFSET_UPPER;
301
302 return HAL_PRIME_CHNL_OFFSET_DONT_CARE;
303 }
304
hal_ch_offset_to_secondary_ch_offset(u8 ch_offset)305 inline u8 hal_ch_offset_to_secondary_ch_offset(u8 ch_offset)
306 {
307 if (ch_offset == HAL_PRIME_CHNL_OFFSET_DONT_CARE)
308 return SCN;
309 else if (ch_offset == HAL_PRIME_CHNL_OFFSET_LOWER)
310 return SCA;
311 else if (ch_offset == HAL_PRIME_CHNL_OFFSET_UPPER)
312 return SCB;
313
314 return SCN;
315 }
316
rtw_set_ie_secondary_ch_offset(u8 * buf,u32 * buf_len,u8 secondary_ch_offset)317 inline u8 *rtw_set_ie_secondary_ch_offset(u8 *buf, u32 *buf_len, u8 secondary_ch_offset)
318 {
319 return rtw_set_ie(buf, WLAN_EID_SECONDARY_CHANNEL_OFFSET, 1, &secondary_ch_offset, buf_len);
320 }
321
rtw_set_ie_mesh_ch_switch_parm(u8 * buf,u32 * buf_len,u8 ttl,u8 flags,u16 reason,u16 precedence)322 inline u8 *rtw_set_ie_mesh_ch_switch_parm(u8 *buf, u32 *buf_len, u8 ttl,
323 u8 flags, u16 reason, u16 precedence)
324 {
325 u8 ie_data[6];
326
327 ie_data[0] = ttl;
328 ie_data[1] = flags;
329 RTW_PUT_LE16((u8 *)&ie_data[2], reason);
330 RTW_PUT_LE16((u8 *)&ie_data[4], precedence);
331
332 return rtw_set_ie(buf, 0x118, 6, ie_data, buf_len);
333 }
334
335 /*----------------------------------------------------------------------------
336 index: the information element id index, limit is the limit for search
337 -----------------------------------------------------------------------------*/
rtw_get_ie(const u8 * pbuf,sint index,sint * len,sint limit)338 u8 *rtw_get_ie(const u8 *pbuf, sint index, sint *len, sint limit)
339 {
340 sint tmp, i;
341 const u8 *p;
342 if (limit < 1) {
343 return NULL;
344 }
345
346 p = pbuf;
347 i = 0;
348 *len = 0;
349 while (1) {
350 if (*p == index) {
351 *len = *(p + 1);
352 return (u8 *)p;
353 } else {
354 tmp = *(p + 1);
355 p += (tmp + 2);
356 i += (tmp + 2);
357 }
358 if (i >= limit)
359 break;
360 }
361 return NULL;
362 }
363
364 /**
365 * rtw_get_ie_ex - Search specific IE from a series of IEs
366 * @in_ie: Address of IEs to search
367 * @in_len: Length limit from in_ie
368 * @eid: Element ID to match
369 * @oui: OUI to match
370 * @oui_len: OUI length
371 * @ie: If not NULL and the specific IE is found, the IE will be copied to the buf starting from the specific IE
372 * @ielen: If not NULL and the specific IE is found, will set to the length of the entire IE
373 *
374 * Returns: The address of the specific IE found, or NULL
375 */
rtw_get_ie_ex(const u8 * in_ie,uint in_len,u8 eid,const u8 * oui,u8 oui_len,u8 * ie,uint * ielen)376 u8 *rtw_get_ie_ex(const u8 *in_ie, uint in_len, u8 eid, const u8 *oui, u8 oui_len, u8 *ie, uint *ielen)
377 {
378 uint cnt;
379 const u8 *target_ie = NULL;
380
381
382 if (ielen)
383 *ielen = 0;
384
385 if (!in_ie || in_len <= 0)
386 return (u8 *)target_ie;
387
388 cnt = 0;
389
390 while (cnt < in_len) {
391 if (eid == in_ie[cnt]
392 && (!oui || _rtw_memcmp(&in_ie[cnt + 2], oui, oui_len) == _TRUE)) {
393 target_ie = &in_ie[cnt];
394
395 if (ie)
396 _rtw_memcpy(ie, &in_ie[cnt], in_ie[cnt + 1] + 2);
397
398 if (ielen)
399 *ielen = in_ie[cnt + 1] + 2;
400
401 break;
402 } else {
403 cnt += in_ie[cnt + 1] + 2; /* goto next */
404 }
405
406 }
407
408 return (u8 *)target_ie;
409 }
410
411 /**
412 * rtw_ies_remove_ie - Find matching IEs and remove
413 * @ies: Address of IEs to search
414 * @ies_len: Pointer of length of ies, will update to new length
415 * @offset: The offset to start scarch
416 * @eid: Element ID to match
417 * @oui: OUI to match
418 * @oui_len: OUI length
419 *
420 * Returns: _SUCCESS: ies is updated, _FAIL: not updated
421 */
rtw_ies_remove_ie(u8 * ies,uint * ies_len,uint offset,u8 eid,u8 * oui,u8 oui_len)422 int rtw_ies_remove_ie(u8 *ies, uint *ies_len, uint offset, u8 eid, u8 *oui, u8 oui_len)
423 {
424 int ret = _FAIL;
425 u8 *target_ie;
426 u32 target_ielen;
427 u8 *start;
428 uint search_len;
429
430 if (!ies || !ies_len || *ies_len <= offset)
431 goto exit;
432
433 start = ies + offset;
434 search_len = *ies_len - offset;
435
436 while (1) {
437 target_ie = rtw_get_ie_ex(start, search_len, eid, oui, oui_len, NULL, &target_ielen);
438 if (target_ie && target_ielen) {
439 u8 *remain_ies = target_ie + target_ielen;
440 uint remain_len = search_len - (remain_ies - start);
441
442 _rtw_memmove(target_ie, remain_ies, remain_len);
443 *ies_len = *ies_len - target_ielen;
444 ret = _SUCCESS;
445
446 start = target_ie;
447 search_len = remain_len;
448 } else
449 break;
450 }
451 exit:
452 return ret;
453 }
454
rtw_set_supported_rate(u8 * SupportedRates,uint mode)455 void rtw_set_supported_rate(u8 *SupportedRates, uint mode)
456 {
457
458 _rtw_memset(SupportedRates, 0, NDIS_802_11_LENGTH_RATES_EX);
459
460 switch (mode) {
461 case WIRELESS_11B:
462 _rtw_memcpy(SupportedRates, WIFI_CCKRATES, IEEE80211_CCK_RATE_LEN);
463 break;
464
465 case WIRELESS_11G:
466 case WIRELESS_11A:
467 case WIRELESS_11_5N:
468 case WIRELESS_11A_5N: /* Todo: no basic rate for ofdm ? */
469 case WIRELESS_11_5AC:
470 _rtw_memcpy(SupportedRates, WIFI_OFDMRATES, IEEE80211_NUM_OFDM_RATESLEN);
471 break;
472
473 case WIRELESS_11BG:
474 case WIRELESS_11G_24N:
475 case WIRELESS_11_24N:
476 case WIRELESS_11BG_24N:
477 _rtw_memcpy(SupportedRates, WIFI_CCKRATES, IEEE80211_CCK_RATE_LEN);
478 _rtw_memcpy(SupportedRates + IEEE80211_CCK_RATE_LEN, WIFI_OFDMRATES, IEEE80211_NUM_OFDM_RATESLEN);
479 break;
480
481 }
482 }
483
rtw_filter_suppport_rateie(WLAN_BSSID_EX * pbss_network,u8 keep)484 void rtw_filter_suppport_rateie(WLAN_BSSID_EX *pbss_network, u8 keep)
485 {
486 u8 i, idx = 0, new_rate[NDIS_802_11_LENGTH_RATES_EX], *p;
487 uint iscck, isofdm, ie_orilen = 0, remain_len;
488 u8 *remain_ies;
489
490 p = rtw_get_ie(pbss_network->IEs + _BEACON_IE_OFFSET_, _SUPPORTEDRATES_IE_, &ie_orilen, (pbss_network->IELength - _BEACON_IE_OFFSET_));
491 if (!p)
492 return;
493
494 _rtw_memset(new_rate, 0, NDIS_802_11_LENGTH_RATES_EX);
495 for (i=0; i < ie_orilen; i++) {
496 iscck = rtw_is_cck_rate(p[i+2]);
497 isofdm= rtw_is_ofdm_rate(p[i+2]);
498 if (((keep == CCK) && iscck)
499 || ((keep == OFDM) && isofdm))
500 new_rate[idx++]= rtw_is_basic_rate_ofdm(p[i+2]) ? p[i+2]|IEEE80211_BASIC_RATE_MASK : p[i+2];
501 }
502 /* update rate ie */
503 p[1] = idx;
504 _rtw_memcpy(p+2, new_rate, idx);
505 /* update remain ie & IELength*/
506 remain_ies = p + 2 + ie_orilen;
507 remain_len = pbss_network->IELength - (remain_ies - pbss_network->IEs);
508 _rtw_memmove(p+2+idx, remain_ies, remain_len);
509 pbss_network->IELength -= (ie_orilen - idx);
510 }
511
512
513 /*
514 Adjust those items by given wireless_mode
515 1. pbss_network->IELength
516 2. pbss_network->IE (SUPPORTRATE & EXT_SUPPORTRATE)
517 3. pbss_network->SupportedRates
518 */
519
rtw_update_rate_bymode(WLAN_BSSID_EX * pbss_network,u32 mode)520 u8 rtw_update_rate_bymode(WLAN_BSSID_EX *pbss_network, u32 mode)
521 {
522 u8 network_type, *p, *ie = pbss_network->IEs;
523 sint ie_len;
524 uint network_ielen = pbss_network->IELength;
525
526 if (mode == WIRELESS_11B) {
527 /*only keep CCK in support_rate IE and remove whole ext_support_rate IE*/
528 rtw_filter_suppport_rateie(pbss_network, CCK);
529 p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _EXT_SUPPORTEDRATES_IE_, &ie_len, pbss_network->IELength - _BEACON_IE_OFFSET_);
530 if (p) {
531 rtw_ies_remove_ie(ie , &network_ielen, _BEACON_IE_OFFSET_, _EXT_SUPPORTEDRATES_IE_, NULL, 0);
532 pbss_network->IELength -= ie_len;
533 }
534 network_type = WIRELESS_11B;
535 } else {
536 if (pbss_network->Configuration.DSConfig > 14) {
537 /* Remove CCK in support_rate IE */
538 rtw_filter_suppport_rateie(pbss_network, OFDM);
539 network_type = WIRELESS_11A;
540 } else {
541 if ((mode & WIRELESS_11B) == 0) {
542 /* Remove CCK in support_rate IE */
543 rtw_filter_suppport_rateie(pbss_network, OFDM);
544 network_type = WIRELESS_11G;
545 } else {
546 network_type = WIRELESS_11BG;
547 }
548 }
549 }
550
551 rtw_set_supported_rate(pbss_network->SupportedRates, network_type);
552
553 return network_type;
554 }
555
rtw_get_rateset_len(u8 * rateset)556 uint rtw_get_rateset_len(u8 *rateset)
557 {
558 uint i = 0;
559 while (1) {
560 if ((rateset[i]) == 0)
561 break;
562
563 if (i > 12)
564 break;
565
566 i++;
567 }
568 return i;
569 }
570
rtw_generate_ie(struct registry_priv * pregistrypriv)571 int rtw_generate_ie(struct registry_priv *pregistrypriv)
572 {
573 u8 wireless_mode;
574 int sz = 0, rateLen;
575 WLAN_BSSID_EX *pdev_network = &pregistrypriv->dev_network;
576 u8 *ie = pdev_network->IEs;
577
578
579 /* timestamp will be inserted by hardware */
580 sz += 8;
581 ie += sz;
582
583 /* beacon interval : 2bytes */
584 *(u16 *)ie = cpu_to_le16((u16)pdev_network->Configuration.BeaconPeriod); /* BCN_INTERVAL; */
585 sz += 2;
586 ie += 2;
587
588 /* capability info */
589 *(u16 *)ie = 0;
590
591 *(u16 *)ie |= cpu_to_le16(cap_IBSS);
592
593 if (pregistrypriv->preamble == PREAMBLE_SHORT)
594 *(u16 *)ie |= cpu_to_le16(cap_ShortPremble);
595
596 if (pdev_network->Privacy)
597 *(u16 *)ie |= cpu_to_le16(cap_Privacy);
598
599 sz += 2;
600 ie += 2;
601
602 /* SSID */
603 ie = rtw_set_ie(ie, _SSID_IE_, pdev_network->Ssid.SsidLength, pdev_network->Ssid.Ssid, &sz);
604
605 /* supported rates */
606 if (pregistrypriv->wireless_mode == WIRELESS_11ABGN) {
607 if (pdev_network->Configuration.DSConfig > 14)
608 wireless_mode = WIRELESS_11A_5N;
609 else
610 wireless_mode = WIRELESS_11BG_24N;
611 } else if (pregistrypriv->wireless_mode == WIRELESS_MODE_MAX) { /* WIRELESS_11ABGN | WIRELESS_11AC */
612 if (pdev_network->Configuration.DSConfig > 14)
613 wireless_mode = WIRELESS_11_5AC;
614 else
615 wireless_mode = WIRELESS_11BG_24N;
616 } else
617 wireless_mode = pregistrypriv->wireless_mode;
618
619 rtw_set_supported_rate(pdev_network->SupportedRates, wireless_mode) ;
620
621 rateLen = rtw_get_rateset_len(pdev_network->SupportedRates);
622
623 if (rateLen > 8) {
624 ie = rtw_set_ie(ie, _SUPPORTEDRATES_IE_, 8, pdev_network->SupportedRates, &sz);
625 /* ie = rtw_set_ie(ie, _EXT_SUPPORTEDRATES_IE_, (rateLen - 8), (pdev_network->SupportedRates + 8), &sz); */
626 } else
627 ie = rtw_set_ie(ie, _SUPPORTEDRATES_IE_, rateLen, pdev_network->SupportedRates, &sz);
628
629 /* DS parameter set */
630 ie = rtw_set_ie(ie, _DSSET_IE_, 1, (u8 *)&(pdev_network->Configuration.DSConfig), &sz);
631
632
633 /* IBSS Parameter Set */
634
635 ie = rtw_set_ie(ie, _IBSS_PARA_IE_, 2, (u8 *)&(pdev_network->Configuration.ATIMWindow), &sz);
636
637 if (rateLen > 8)
638 ie = rtw_set_ie(ie, _EXT_SUPPORTEDRATES_IE_, (rateLen - 8), (pdev_network->SupportedRates + 8), &sz);
639
640 #ifdef CONFIG_80211N_HT
641 /* HT Cap. */
642 if (is_supported_ht(pregistrypriv->wireless_mode)
643 && (pregistrypriv->ht_enable == _TRUE)) {
644 /* todo: */
645 }
646 #endif /* CONFIG_80211N_HT */
647
648 /* pdev_network->IELength = sz; */ /* update IELength */
649
650
651 /* return _SUCCESS; */
652
653 return sz;
654
655 }
656
rtw_get_wpa_ie(unsigned char * pie,int * wpa_ie_len,int limit)657 unsigned char *rtw_get_wpa_ie(unsigned char *pie, int *wpa_ie_len, int limit)
658 {
659 int len;
660 u16 val16;
661 unsigned char wpa_oui_type[] = {0x00, 0x50, 0xf2, 0x01};
662 u8 *pbuf = pie;
663 int limit_new = limit;
664
665 while (1) {
666 pbuf = rtw_get_ie(pbuf, _WPA_IE_ID_, &len, limit_new);
667
668 if (pbuf) {
669
670 /* check if oui matches... */
671 if (_rtw_memcmp((pbuf + 2), wpa_oui_type, sizeof(wpa_oui_type)) == _FALSE)
672
673 goto check_next_ie;
674
675 /* check version... */
676 _rtw_memcpy((u8 *)&val16, (pbuf + 6), sizeof(val16));
677
678 val16 = le16_to_cpu(val16);
679 if (val16 != 0x0001)
680 goto check_next_ie;
681
682 *wpa_ie_len = *(pbuf + 1);
683
684 return pbuf;
685
686 } else {
687
688 *wpa_ie_len = 0;
689 return NULL;
690 }
691
692 check_next_ie:
693
694 limit_new = limit - (pbuf - pie) - 2 - len;
695
696 if (limit_new <= 0)
697 break;
698
699 pbuf += (2 + len);
700
701 }
702
703 *wpa_ie_len = 0;
704
705 return NULL;
706
707 }
708
rtw_get_wpa2_ie(unsigned char * pie,int * rsn_ie_len,int limit)709 unsigned char *rtw_get_wpa2_ie(unsigned char *pie, int *rsn_ie_len, int limit)
710 {
711
712 return rtw_get_ie(pie, _WPA2_IE_ID_, rsn_ie_len, limit);
713
714 }
715
rtw_get_wpa_cipher_suite(u8 * s)716 int rtw_get_wpa_cipher_suite(u8 *s)
717 {
718 if (_rtw_memcmp(s, WPA_CIPHER_SUITE_NONE, WPA_SELECTOR_LEN) == _TRUE)
719 return WPA_CIPHER_NONE;
720 if (_rtw_memcmp(s, WPA_CIPHER_SUITE_WEP40, WPA_SELECTOR_LEN) == _TRUE)
721 return WPA_CIPHER_WEP40;
722 if (_rtw_memcmp(s, WPA_CIPHER_SUITE_TKIP, WPA_SELECTOR_LEN) == _TRUE)
723 return WPA_CIPHER_TKIP;
724 if (_rtw_memcmp(s, WPA_CIPHER_SUITE_CCMP, WPA_SELECTOR_LEN) == _TRUE)
725 return WPA_CIPHER_CCMP;
726 if (_rtw_memcmp(s, WPA_CIPHER_SUITE_WEP104, WPA_SELECTOR_LEN) == _TRUE)
727 return WPA_CIPHER_WEP104;
728
729 return 0;
730 }
731
rtw_get_rsn_cipher_suite(u8 * s)732 int rtw_get_rsn_cipher_suite(u8 *s)
733 {
734 if (_rtw_memcmp(s, RSN_CIPHER_SUITE_NONE, RSN_SELECTOR_LEN) == _TRUE)
735 return WPA_CIPHER_NONE;
736 if (_rtw_memcmp(s, RSN_CIPHER_SUITE_WEP40, RSN_SELECTOR_LEN) == _TRUE)
737 return WPA_CIPHER_WEP40;
738 if (_rtw_memcmp(s, RSN_CIPHER_SUITE_TKIP, RSN_SELECTOR_LEN) == _TRUE)
739 return WPA_CIPHER_TKIP;
740 if (_rtw_memcmp(s, RSN_CIPHER_SUITE_CCMP, RSN_SELECTOR_LEN) == _TRUE)
741 return WPA_CIPHER_CCMP;
742 if (_rtw_memcmp(s, RSN_CIPHER_SUITE_GCMP, RSN_SELECTOR_LEN) == _TRUE)
743 return WPA_CIPHER_GCMP;
744 if (_rtw_memcmp(s, RSN_CIPHER_SUITE_GCMP_256, RSN_SELECTOR_LEN) == _TRUE)
745 return WPA_CIPHER_GCMP_256;
746 if (_rtw_memcmp(s, RSN_CIPHER_SUITE_CCMP_256, RSN_SELECTOR_LEN) == _TRUE)
747 return WPA_CIPHER_CCMP_256;
748 if (_rtw_memcmp(s, RSN_CIPHER_SUITE_WEP104, RSN_SELECTOR_LEN) == _TRUE)
749 return WPA_CIPHER_WEP104;
750 if (_rtw_memcmp(s, RSN_CIPHER_SUITE_AES_128_CMAC, RSN_SELECTOR_LEN) == _TRUE)
751 return WPA_CIPHER_BIP_CMAC_128;
752 if (_rtw_memcmp(s, RSN_CIPHER_SUITE_BIP_GMAC_128, RSN_SELECTOR_LEN) == _TRUE)
753 return WPA_CIPHER_BIP_GMAC_128;
754 if (_rtw_memcmp(s, RSN_CIPHER_SUITE_BIP_GMAC_256, RSN_SELECTOR_LEN) == _TRUE)
755 return WPA_CIPHER_BIP_GMAC_256;
756 if (_rtw_memcmp(s, RSN_CIPHER_SUITE_BIP_CMAC_256, RSN_SELECTOR_LEN) == _TRUE)
757 return WPA_CIPHER_BIP_CMAC_256;
758 return 0;
759 }
760
rtw_get_akm_suite_bitmap(u8 * s)761 u32 rtw_get_akm_suite_bitmap(u8 *s)
762 {
763 if (_rtw_memcmp(s, WLAN_AKM_8021X, RSN_SELECTOR_LEN) == _TRUE)
764 return WLAN_AKM_TYPE_8021X;
765 if (_rtw_memcmp(s, WLAN_AKM_PSK, RSN_SELECTOR_LEN) == _TRUE)
766 return WLAN_AKM_TYPE_PSK;
767 if (_rtw_memcmp(s, WLAN_AKM_FT_8021X, RSN_SELECTOR_LEN) == _TRUE)
768 return WLAN_AKM_TYPE_FT_8021X;
769 if (_rtw_memcmp(s, WLAN_AKM_FT_PSK, RSN_SELECTOR_LEN) == _TRUE)
770 return WLAN_AKM_TYPE_FT_PSK;
771 if (_rtw_memcmp(s, WLAN_AKM_8021X_SHA256, RSN_SELECTOR_LEN) == _TRUE)
772 return WLAN_AKM_TYPE_8021X_SHA256;
773 if (_rtw_memcmp(s, WLAN_AKM_PSK_SHA256, RSN_SELECTOR_LEN) == _TRUE)
774 return WLAN_AKM_TYPE_PSK_SHA256;
775 if (_rtw_memcmp(s, WLAN_AKM_TDLS, RSN_SELECTOR_LEN) == _TRUE)
776 return WLAN_AKM_TYPE_TDLS;
777 if (_rtw_memcmp(s, WLAN_AKM_SAE, RSN_SELECTOR_LEN) == _TRUE)
778 return WLAN_AKM_TYPE_SAE;
779 if (_rtw_memcmp(s, WLAN_AKM_FT_OVER_SAE, RSN_SELECTOR_LEN) == _TRUE)
780 return WLAN_AKM_TYPE_FT_OVER_SAE;
781 if (_rtw_memcmp(s, WLAN_AKM_8021X_SUITE_B, RSN_SELECTOR_LEN) == _TRUE)
782 return WLAN_AKM_TYPE_8021X_SUITE_B;
783 if (_rtw_memcmp(s, WLAN_AKM_8021X_SUITE_B_192, RSN_SELECTOR_LEN) == _TRUE)
784 return WLAN_AKM_TYPE_8021X_SUITE_B_192;
785 if (_rtw_memcmp(s, WLAN_AKM_FILS_SHA256, RSN_SELECTOR_LEN) == _TRUE)
786 return WLAN_AKM_TYPE_FILS_SHA256;
787 if (_rtw_memcmp(s, WLAN_AKM_FILS_SHA384, RSN_SELECTOR_LEN) == _TRUE)
788 return WLAN_AKM_TYPE_FILS_SHA384;
789 if (_rtw_memcmp(s, WLAN_AKM_FT_FILS_SHA256, RSN_SELECTOR_LEN) == _TRUE)
790 return WLAN_AKM_TYPE_FT_FILS_SHA256;
791 if (_rtw_memcmp(s, WLAN_AKM_FT_FILS_SHA384, RSN_SELECTOR_LEN) == _TRUE)
792 return WLAN_AKM_TYPE_FT_FILS_SHA384;
793
794 return 0;
795 }
796
rtw_parse_wpa_ie(u8 * wpa_ie,int wpa_ie_len,int * group_cipher,int * pairwise_cipher,u32 * akm)797 int rtw_parse_wpa_ie(u8 *wpa_ie, int wpa_ie_len, int *group_cipher,
798 int *pairwise_cipher, u32 *akm)
799 {
800 int i, ret = _SUCCESS;
801 int left, count;
802 u8 *pos;
803 u8 SUITE_1X[4] = {0x00, 0x50, 0xf2, 1};
804
805 if (wpa_ie_len <= 0) {
806 /* No WPA IE - fail silently */
807 return _FAIL;
808 }
809
810
811 if ((*wpa_ie != _WPA_IE_ID_) || (*(wpa_ie + 1) != (u8)(wpa_ie_len - 2)) ||
812 (_rtw_memcmp(wpa_ie + 2, RTW_WPA_OUI_TYPE, WPA_SELECTOR_LEN) != _TRUE))
813 return _FAIL;
814
815 pos = wpa_ie;
816
817 pos += 8;
818 left = wpa_ie_len - 8;
819
820
821 /* group_cipher */
822 if (left >= WPA_SELECTOR_LEN) {
823
824 *group_cipher = rtw_get_wpa_cipher_suite(pos);
825
826 pos += WPA_SELECTOR_LEN;
827 left -= WPA_SELECTOR_LEN;
828
829 } else if (left > 0) {
830
831 return _FAIL;
832 }
833
834
835 /* pairwise_cipher */
836 if (left >= 2) {
837 /* count = le16_to_cpu(*(u16*)pos); */
838 count = RTW_GET_LE16(pos);
839 pos += 2;
840 left -= 2;
841
842 if (count == 0 || left < count * WPA_SELECTOR_LEN) {
843 return _FAIL;
844 }
845
846 for (i = 0; i < count; i++) {
847 *pairwise_cipher |= rtw_get_wpa_cipher_suite(pos);
848
849 pos += WPA_SELECTOR_LEN;
850 left -= WPA_SELECTOR_LEN;
851 }
852
853 } else if (left == 1) {
854 return _FAIL;
855 }
856
857 if (akm) {
858 if (left >= 6) {
859 pos += 2;
860 if (_rtw_memcmp(pos, SUITE_1X, 4) == 1) {
861 *akm = WLAN_AKM_TYPE_8021X;
862 }
863 }
864 }
865
866 return ret;
867
868 }
869
rtw_rsne_info_parse(const u8 * ie,uint ie_len,struct rsne_info * info)870 int rtw_rsne_info_parse(const u8 *ie, uint ie_len, struct rsne_info *info)
871 {
872 const u8 *pos = ie;
873 u16 cnt;
874
875 _rtw_memset(info, 0, sizeof(struct rsne_info));
876
877 if (ie + ie_len < pos + 4)
878 goto err;
879
880 if (*ie != WLAN_EID_RSN || *(ie + 1) != ie_len - 2)
881 goto err;
882 pos += 2 + 2;
883
884 /* Group CS */
885 if (ie + ie_len < pos + 4) {
886 if (ie + ie_len != pos)
887 goto err;
888 goto exit;
889 }
890 info->gcs = (u8 *)pos;
891 pos += 4;
892
893 /* Pairwise CS */
894 if (ie + ie_len < pos + 2) {
895 if (ie + ie_len != pos)
896 goto err;
897 goto exit;
898 }
899 cnt = RTW_GET_LE16(pos);
900 pos += 2;
901 if (ie + ie_len < pos + 4 * cnt) {
902 if (ie + ie_len != pos)
903 goto err;
904 goto exit;
905 }
906 info->pcs_cnt = cnt;
907 info->pcs_list = (u8 *)pos;
908 pos += 4 * cnt;
909
910 /* AKM */
911 if (ie + ie_len < pos + 2) {
912 if (ie + ie_len != pos)
913 goto err;
914 goto exit;
915 }
916 cnt = RTW_GET_LE16(pos);
917 pos += 2;
918 if (ie + ie_len < pos + 4 * cnt) {
919 if (ie + ie_len != pos)
920 goto err;
921 goto exit;
922 }
923 info->akm_cnt = cnt;
924 info->akm_list = (u8 *)pos;
925 pos += 4 * cnt;
926
927 /* RSN cap */
928 if (ie + ie_len < pos + 2) {
929 if (ie + ie_len != pos)
930 goto err;
931 goto exit;
932 }
933 info->cap = (u8 *)pos;
934 pos += 2;
935
936 /* PMKID */
937 if (ie + ie_len < pos + 2) {
938 if (ie + ie_len != pos)
939 goto err;
940 goto exit;
941 }
942 cnt = RTW_GET_LE16(pos);
943 pos += 2;
944 if (ie + ie_len < pos + 16 * cnt) {
945 if (ie + ie_len != pos)
946 goto err;
947 goto exit;
948 }
949 info->pmkid_cnt = cnt;
950 info->pmkid_list = (u8 *)pos;
951 pos += 16 * cnt;
952
953 /* Group Mgmt CS */
954 if (ie + ie_len < pos + 4) {
955 if (ie + ie_len != pos)
956 goto err;
957 goto exit;
958 }
959 info->gmcs = (u8 *)pos;
960
961 exit:
962 return _SUCCESS;
963
964 err:
965 info->err = 1;
966 return _FAIL;
967 }
968
rtw_parse_wpa2_ie(u8 * rsn_ie,int rsn_ie_len,int * group_cipher,int * pairwise_cipher,int * gmcs,u32 * akm,u8 * mfp_opt,u8 * spp_opt)969 int rtw_parse_wpa2_ie(u8 *rsn_ie, int rsn_ie_len, int *group_cipher,
970 int *pairwise_cipher, int *gmcs, u32 *akm, u8 *mfp_opt, u8 *spp_opt)
971 {
972 struct rsne_info info;
973 int i, ret = _SUCCESS;
974
975 ret = rtw_rsne_info_parse(rsn_ie, rsn_ie_len, &info);
976 if (ret != _SUCCESS)
977 goto exit;
978
979 if (group_cipher) {
980 if (info.gcs)
981 *group_cipher = rtw_get_rsn_cipher_suite(info.gcs);
982 else
983 *group_cipher = 0;
984 }
985
986 if (pairwise_cipher) {
987 *pairwise_cipher = 0;
988 for (i = 0; i < info.pcs_cnt; i++)
989 *pairwise_cipher |= rtw_get_rsn_cipher_suite(info.pcs_list + 4 * i);
990 }
991
992 if (gmcs) {
993 if (info.gmcs)
994 *gmcs = rtw_get_rsn_cipher_suite(info.gmcs);
995 else
996 *gmcs = WPA_CIPHER_BIP_CMAC_128; /* default value when absent */
997 }
998
999 if (akm) {
1000 *akm = 0;
1001 for (i = 0; i < info.akm_cnt; i++)
1002 *akm |= rtw_get_akm_suite_bitmap(info.akm_list + 4 * i);
1003 }
1004
1005 if (mfp_opt) {
1006 *mfp_opt = MFP_NO;
1007 if (info.cap)
1008 *mfp_opt = GET_RSN_CAP_MFP_OPTION(info.cap);
1009 }
1010
1011 if (spp_opt) {
1012 *spp_opt = 0;
1013 if (info.cap)
1014 *spp_opt = GET_RSN_CAP_SPP_OPT(info.cap);
1015 }
1016
1017 exit:
1018 return ret;
1019 }
1020
1021 /* #ifdef CONFIG_WAPI_SUPPORT */
rtw_get_wapi_ie(u8 * in_ie,uint in_len,u8 * wapi_ie,u16 * wapi_len)1022 int rtw_get_wapi_ie(u8 *in_ie, uint in_len, u8 *wapi_ie, u16 *wapi_len)
1023 {
1024 int len = 0;
1025 u8 authmode;
1026 uint cnt;
1027 u8 wapi_oui1[4] = {0x0, 0x14, 0x72, 0x01};
1028 u8 wapi_oui2[4] = {0x0, 0x14, 0x72, 0x02};
1029
1030
1031 if (wapi_len)
1032 *wapi_len = 0;
1033
1034 if (!in_ie || in_len <= 0)
1035 return len;
1036
1037 cnt = (_TIMESTAMP_ + _BEACON_ITERVAL_ + _CAPABILITY_);
1038
1039 while (cnt < in_len) {
1040 authmode = in_ie[cnt];
1041
1042 /* if(authmode==_WAPI_IE_) */
1043 if (authmode == _WAPI_IE_ && (_rtw_memcmp(&in_ie[cnt + 6], wapi_oui1, 4) == _TRUE ||
1044 _rtw_memcmp(&in_ie[cnt + 6], wapi_oui2, 4) == _TRUE)) {
1045 if (wapi_ie)
1046 _rtw_memcpy(wapi_ie, &in_ie[cnt], in_ie[cnt + 1] + 2);
1047
1048 if (wapi_len)
1049 *wapi_len = in_ie[cnt + 1] + 2;
1050
1051 cnt += in_ie[cnt + 1] + 2; /* get next */
1052 } else {
1053 cnt += in_ie[cnt + 1] + 2; /* get next */
1054 }
1055 }
1056
1057 if (wapi_len)
1058 len = *wapi_len;
1059
1060
1061 return len;
1062
1063 }
1064 /* #endif */
1065
rtw_get_sec_ie(u8 * in_ie,uint in_len,u8 * rsn_ie,u16 * rsn_len,u8 * wpa_ie,u16 * wpa_len)1066 int rtw_get_sec_ie(u8 *in_ie, uint in_len, u8 *rsn_ie, u16 *rsn_len, u8 *wpa_ie, u16 *wpa_len)
1067 {
1068 u8 authmode, sec_idx;
1069 u8 wpa_oui[4] = {0x0, 0x50, 0xf2, 0x01};
1070 uint cnt;
1071
1072
1073 /* Search required WPA or WPA2 IE and copy to sec_ie[ ] */
1074
1075 cnt = (_TIMESTAMP_ + _BEACON_ITERVAL_ + _CAPABILITY_);
1076
1077 sec_idx = 0;
1078
1079 while (cnt < in_len) {
1080 authmode = in_ie[cnt];
1081
1082 if ((authmode == _WPA_IE_ID_) && (_rtw_memcmp(&in_ie[cnt + 2], &wpa_oui[0], 4) == _TRUE)) {
1083
1084 if (wpa_ie)
1085 _rtw_memcpy(wpa_ie, &in_ie[cnt], in_ie[cnt + 1] + 2);
1086
1087 *wpa_len = in_ie[cnt + 1] + 2;
1088 cnt += in_ie[cnt + 1] + 2; /* get next */
1089 } else {
1090 if (authmode == _WPA2_IE_ID_) {
1091
1092 if (rsn_ie)
1093 _rtw_memcpy(rsn_ie, &in_ie[cnt], in_ie[cnt + 1] + 2);
1094
1095 *rsn_len = in_ie[cnt + 1] + 2;
1096 cnt += in_ie[cnt + 1] + 2; /* get next */
1097 } else {
1098 cnt += in_ie[cnt + 1] + 2; /* get next */
1099 }
1100 }
1101
1102 }
1103
1104
1105 return *rsn_len + *wpa_len;
1106
1107 }
1108
rtw_is_wps_ie(u8 * ie_ptr,uint * wps_ielen)1109 u8 rtw_is_wps_ie(u8 *ie_ptr, uint *wps_ielen)
1110 {
1111 u8 match = _FALSE;
1112 u8 eid, wps_oui[4] = {0x0, 0x50, 0xf2, 0x04};
1113
1114 if (ie_ptr == NULL)
1115 return match;
1116
1117 eid = ie_ptr[0];
1118
1119 if ((eid == _WPA_IE_ID_) && (_rtw_memcmp(&ie_ptr[2], wps_oui, 4) == _TRUE)) {
1120 /* RTW_INFO("==> found WPS_IE.....\n"); */
1121 *wps_ielen = ie_ptr[1] + 2;
1122 match = _TRUE;
1123 }
1124 return match;
1125 }
1126
rtw_get_wps_ie_from_scan_queue(u8 * in_ie,uint in_len,u8 * wps_ie,uint * wps_ielen,enum bss_type frame_type)1127 u8 *rtw_get_wps_ie_from_scan_queue(u8 *in_ie, uint in_len, u8 *wps_ie, uint *wps_ielen, enum bss_type frame_type)
1128 {
1129 u8 *wps = NULL;
1130
1131 RTW_INFO("[%s] frame_type = %d\n", __FUNCTION__, frame_type);
1132 switch (frame_type) {
1133 case BSS_TYPE_BCN:
1134 case BSS_TYPE_PROB_RSP: {
1135 /* Beacon or Probe Response */
1136 wps = rtw_get_wps_ie(in_ie + _PROBERSP_IE_OFFSET_, in_len - _PROBERSP_IE_OFFSET_, wps_ie, wps_ielen);
1137 break;
1138 }
1139 case BSS_TYPE_PROB_REQ: {
1140 /* Probe Request */
1141 wps = rtw_get_wps_ie(in_ie + _PROBEREQ_IE_OFFSET_ , in_len - _PROBEREQ_IE_OFFSET_ , wps_ie, wps_ielen);
1142 break;
1143 }
1144 default:
1145 case BSS_TYPE_UNDEF:
1146 break;
1147 }
1148 return wps;
1149 }
1150
1151 /**
1152 * rtw_get_wps_ie - Search WPS IE from a series of IEs
1153 * @in_ie: Address of IEs to search
1154 * @in_len: Length limit from in_ie
1155 * @wps_ie: If not NULL and WPS IE is found, WPS IE will be copied to the buf starting from wps_ie
1156 * @wps_ielen: If not NULL and WPS IE is found, will set to the length of the entire WPS IE
1157 *
1158 * Returns: The address of the WPS IE found, or NULL
1159 */
rtw_get_wps_ie(const u8 * in_ie,uint in_len,u8 * wps_ie,uint * wps_ielen)1160 u8 *rtw_get_wps_ie(const u8 *in_ie, uint in_len, u8 *wps_ie, uint *wps_ielen)
1161 {
1162 uint cnt;
1163 const u8 *wpsie_ptr = NULL;
1164 u8 eid, wps_oui[4] = {0x00, 0x50, 0xf2, 0x04};
1165
1166 if (wps_ielen)
1167 *wps_ielen = 0;
1168
1169 if (!in_ie) {
1170 rtw_warn_on(1);
1171 return (u8 *)wpsie_ptr;
1172 }
1173
1174 if (in_len <= 0)
1175 return (u8 *)wpsie_ptr;
1176
1177 cnt = 0;
1178
1179 while (cnt + 1 + 4 < in_len) {
1180 eid = in_ie[cnt];
1181
1182 if (cnt + 1 + 4 >= MAX_IE_SZ) {
1183 rtw_warn_on(1);
1184 return NULL;
1185 }
1186
1187 if (eid == WLAN_EID_VENDOR_SPECIFIC && _rtw_memcmp(&in_ie[cnt + 2], wps_oui, 4) == _TRUE) {
1188 wpsie_ptr = in_ie + cnt;
1189
1190 if (wps_ie)
1191 _rtw_memcpy(wps_ie, &in_ie[cnt], in_ie[cnt + 1] + 2);
1192
1193 if (wps_ielen)
1194 *wps_ielen = in_ie[cnt + 1] + 2;
1195
1196 break;
1197 } else
1198 cnt += in_ie[cnt + 1] + 2;
1199
1200 }
1201
1202 return (u8 *)wpsie_ptr;
1203 }
1204
1205 /**
1206 * rtw_get_wps_attr - Search a specific WPS attribute from a given WPS IE
1207 * @wps_ie: Address of WPS IE to search
1208 * @wps_ielen: Length limit from wps_ie
1209 * @target_attr_id: The attribute ID of WPS attribute to search
1210 * @buf_attr: If not NULL and the WPS attribute is found, WPS attribute will be copied to the buf starting from buf_attr
1211 * @len_attr: If not NULL and the WPS attribute is found, will set to the length of the entire WPS attribute
1212 *
1213 * Returns: the address of the specific WPS attribute found, or NULL
1214 */
rtw_get_wps_attr(u8 * wps_ie,uint wps_ielen,u16 target_attr_id,u8 * buf_attr,u32 * len_attr)1215 u8 *rtw_get_wps_attr(u8 *wps_ie, uint wps_ielen, u16 target_attr_id , u8 *buf_attr, u32 *len_attr)
1216 {
1217 u8 *attr_ptr = NULL;
1218 u8 *target_attr_ptr = NULL;
1219 u8 wps_oui[4] = {0x00, 0x50, 0xF2, 0x04};
1220
1221 if (len_attr)
1222 *len_attr = 0;
1223
1224 if ((wps_ie[0] != _VENDOR_SPECIFIC_IE_) ||
1225 (_rtw_memcmp(wps_ie + 2, wps_oui , 4) != _TRUE))
1226 return attr_ptr;
1227
1228 /* 6 = 1(Element ID) + 1(Length) + 4(WPS OUI) */
1229 attr_ptr = wps_ie + 6; /* goto first attr */
1230
1231 while (attr_ptr - wps_ie < wps_ielen) {
1232 /* 4 = 2(Attribute ID) + 2(Length) */
1233 u16 attr_id = RTW_GET_BE16(attr_ptr);
1234 u16 attr_data_len = RTW_GET_BE16(attr_ptr + 2);
1235 u16 attr_len = attr_data_len + 4;
1236
1237 /* RTW_INFO("%s attr_ptr:%p, id:%u, length:%u\n", __FUNCTION__, attr_ptr, attr_id, attr_data_len); */
1238 if (attr_id == target_attr_id) {
1239 target_attr_ptr = attr_ptr;
1240
1241 if (buf_attr)
1242 _rtw_memcpy(buf_attr, attr_ptr, attr_len);
1243
1244 if (len_attr)
1245 *len_attr = attr_len;
1246
1247 break;
1248 } else {
1249 attr_ptr += attr_len; /* goto next */
1250 }
1251
1252 }
1253
1254 return target_attr_ptr;
1255 }
1256
1257 /**
1258 * rtw_get_wps_attr_content - Search a specific WPS attribute content from a given WPS IE
1259 * @wps_ie: Address of WPS IE to search
1260 * @wps_ielen: Length limit from wps_ie
1261 * @target_attr_id: The attribute ID of WPS attribute to search
1262 * @buf_content: If not NULL and the WPS attribute is found, WPS attribute content will be copied to the buf starting from buf_content
1263 * If len_content is NULL, only copy one byte.
1264 * @len_content: If not NULL and the WPS attribute is found, will set to the length of the WPS attribute content
1265 *
1266 * Returns: the address of the specific WPS attribute content found, or NULL
1267 */
rtw_get_wps_attr_content(u8 * wps_ie,uint wps_ielen,u16 target_attr_id,u8 * buf_content,uint * len_content)1268 u8 *rtw_get_wps_attr_content(u8 *wps_ie, uint wps_ielen, u16 target_attr_id , u8 *buf_content, uint *len_content)
1269 {
1270 u8 *attr_ptr;
1271 u32 attr_len;
1272
1273 attr_ptr = rtw_get_wps_attr(wps_ie, wps_ielen, target_attr_id, NULL, &attr_len);
1274
1275 if (attr_ptr && attr_len) {
1276 if (len_content) {
1277 if ((buf_content && (*len_content > (attr_len - 4))) || !buf_content)
1278 *len_content = attr_len - 4;
1279 }
1280
1281 if (len_content && buf_content) {
1282 _rtw_memcpy(buf_content, attr_ptr + 4, *len_content);
1283 } else if (buf_content) {
1284 _rtw_memcpy(buf_content, attr_ptr + 4, 1);
1285 }
1286
1287 return attr_ptr + 4;
1288 }
1289
1290 if (len_content)
1291 *len_content = 0;
1292
1293 return NULL;
1294 }
1295
1296 /* OWE */
1297
1298 /**
1299 * rtw_get_OWE_ie - Search OWE IE from a series of IEs
1300 * @in_ie: Address of IEs to search
1301 * @in_len: Length limit from in_ie
1302 * @wps_ie: If not NULL and OWE IE is found, OWE IE will be copied to the buf starting from owe_ie
1303 * @wps_ielen: If not NULL and OWE IE is found, will set to the length of the entire OWE IE
1304 *
1305 * Returns: The address of the OWE IE found, or NULL
1306 */
rtw_get_owe_ie(const u8 * in_ie,uint in_len,u8 * owe_ie,uint * owe_ielen)1307 u8 *rtw_get_owe_ie(const u8 *in_ie, uint in_len, u8 *owe_ie, uint *owe_ielen)
1308 {
1309 uint cnt;
1310 const u8 *oweie_ptr = NULL;
1311 u8 eid;
1312
1313 if (owe_ielen)
1314 *owe_ielen = 0;
1315
1316 if (!in_ie) {
1317 rtw_warn_on(1);
1318 return (u8 *)oweie_ptr;
1319 }
1320
1321 if (in_len <= 0)
1322 return (u8 *)oweie_ptr;
1323
1324 cnt = 0;
1325
1326 while (cnt + 1 + 4 < in_len) {
1327 eid = in_ie[cnt];
1328
1329 if (cnt + 1 + 4 >= MAX_IE_SZ) {
1330 rtw_warn_on(1);
1331 return NULL;
1332 }
1333
1334 if ((eid == WLAN_EID_EXTENSION) && (in_ie[cnt + 2] == WLAN_EID_EXT_OWE_DH_PARAM)) {
1335 oweie_ptr = in_ie + cnt;
1336
1337 if (owe_ie)
1338 _rtw_memcpy(owe_ie, &in_ie[cnt], in_ie[cnt + 1] + 2);
1339
1340 if (owe_ielen)
1341 *owe_ielen = in_ie[cnt + 1] + 2;
1342
1343 break;
1344 } else
1345 cnt += in_ie[cnt + 1] + 2;
1346
1347 }
1348
1349 return (u8 *)oweie_ptr;
1350 }
1351
rtw_ieee802_11_parse_vendor_specific(u8 * pos,uint elen,struct rtw_ieee802_11_elems * elems,int show_errors)1352 static int rtw_ieee802_11_parse_vendor_specific(u8 *pos, uint elen,
1353 struct rtw_ieee802_11_elems *elems,
1354 int show_errors)
1355 {
1356 unsigned int oui;
1357
1358 /* first 3 bytes in vendor specific information element are the IEEE
1359 * OUI of the vendor. The following byte is used a vendor specific
1360 * sub-type. */
1361 if (elen < 4) {
1362 if (show_errors) {
1363 RTW_INFO("short vendor specific "
1364 "information element ignored (len=%lu)\n",
1365 (unsigned long) elen);
1366 }
1367 return -1;
1368 }
1369
1370 oui = RTW_GET_BE24(pos);
1371 switch (oui) {
1372 case OUI_MICROSOFT:
1373 /* Microsoft/Wi-Fi information elements are further typed and
1374 * subtyped */
1375 switch (pos[3]) {
1376 case 1:
1377 /* Microsoft OUI (00:50:F2) with OUI Type 1:
1378 * real WPA information element */
1379 elems->wpa_ie = pos;
1380 elems->wpa_ie_len = elen;
1381 break;
1382 case WME_OUI_TYPE: /* this is a Wi-Fi WME info. element */
1383 if (elen < 5) {
1384 RTW_DBG("short WME "
1385 "information element ignored "
1386 "(len=%lu)\n",
1387 (unsigned long) elen);
1388 return -1;
1389 }
1390 switch (pos[4]) {
1391 case WME_OUI_SUBTYPE_INFORMATION_ELEMENT:
1392 case WME_OUI_SUBTYPE_PARAMETER_ELEMENT:
1393 elems->wme = pos;
1394 elems->wme_len = elen;
1395 break;
1396 case WME_OUI_SUBTYPE_TSPEC_ELEMENT:
1397 elems->wme_tspec = pos;
1398 elems->wme_tspec_len = elen;
1399 break;
1400 default:
1401 RTW_DBG("unknown WME "
1402 "information element ignored "
1403 "(subtype=%d len=%lu)\n",
1404 pos[4], (unsigned long) elen);
1405 return -1;
1406 }
1407 break;
1408 case 4:
1409 /* Wi-Fi Protected Setup (WPS) IE */
1410 elems->wps_ie = pos;
1411 elems->wps_ie_len = elen;
1412 break;
1413 default:
1414 RTW_DBG("Unknown Microsoft "
1415 "information element ignored "
1416 "(type=%d len=%lu)\n",
1417 pos[3], (unsigned long) elen);
1418 return -1;
1419 }
1420 break;
1421
1422 case OUI_BROADCOM:
1423 switch (pos[3]) {
1424 case VENDOR_HT_CAPAB_OUI_TYPE:
1425 elems->vendor_ht_cap = pos;
1426 elems->vendor_ht_cap_len = elen;
1427 break;
1428 default:
1429 RTW_DBG("Unknown Broadcom "
1430 "information element ignored "
1431 "(type=%d len=%lu)\n",
1432 pos[3], (unsigned long) elen);
1433 return -1;
1434 }
1435 break;
1436 #ifdef CONFIG_RTW_TOKEN_BASED_XMIT
1437 case OUI_REALTEK:
1438 if (elen == 8) { // TBTX capable IE length is 8
1439 elems->tbtx_cap = pos;
1440 elems->tbtx_cap_len = elen;
1441 }
1442 break;
1443 #endif
1444 default:
1445 RTW_DBG("unknown vendor specific information "
1446 "element ignored (vendor OUI %02x:%02x:%02x "
1447 "len=%lu)\n",
1448 pos[0], pos[1], pos[2], (unsigned long) elen);
1449 return -1;
1450 }
1451
1452 return 0;
1453
1454 }
1455
1456 /**
1457 * ieee802_11_parse_elems - Parse information elements in management frames
1458 * @start: Pointer to the start of IEs
1459 * @len: Length of IE buffer in octets
1460 * @elems: Data structure for parsed elements
1461 * @show_errors: Whether to show parsing errors in debug log
1462 * Returns: Parsing result
1463 */
rtw_ieee802_11_parse_elems(u8 * start,uint len,struct rtw_ieee802_11_elems * elems,int show_errors)1464 ParseRes rtw_ieee802_11_parse_elems(u8 *start, uint len,
1465 struct rtw_ieee802_11_elems *elems,
1466 int show_errors)
1467 {
1468 uint left = len;
1469 u8 *pos = start;
1470 int unknown = 0;
1471
1472 _rtw_memset(elems, 0, sizeof(*elems));
1473
1474 while (left >= 2) {
1475 u8 id, elen;
1476
1477 id = *pos++;
1478 elen = *pos++;
1479 left -= 2;
1480
1481 if (elen > left) {
1482 if (show_errors) {
1483 RTW_INFO("IEEE 802.11 element "
1484 "parse failed (id=%d elen=%d "
1485 "left=%lu)\n",
1486 id, elen, (unsigned long) left);
1487 }
1488 return ParseFailed;
1489 }
1490
1491 switch (id) {
1492 case WLAN_EID_SSID:
1493 elems->ssid = pos;
1494 elems->ssid_len = elen;
1495 break;
1496 case WLAN_EID_SUPP_RATES:
1497 elems->supp_rates = pos;
1498 elems->supp_rates_len = elen;
1499 break;
1500 case WLAN_EID_FH_PARAMS:
1501 elems->fh_params = pos;
1502 elems->fh_params_len = elen;
1503 break;
1504 case WLAN_EID_DS_PARAMS:
1505 elems->ds_params = pos;
1506 elems->ds_params_len = elen;
1507 break;
1508 case WLAN_EID_CF_PARAMS:
1509 elems->cf_params = pos;
1510 elems->cf_params_len = elen;
1511 break;
1512 case WLAN_EID_TIM:
1513 elems->tim = pos;
1514 elems->tim_len = elen;
1515 break;
1516 case WLAN_EID_IBSS_PARAMS:
1517 elems->ibss_params = pos;
1518 elems->ibss_params_len = elen;
1519 break;
1520 case WLAN_EID_CHALLENGE:
1521 elems->challenge = pos;
1522 elems->challenge_len = elen;
1523 break;
1524 case WLAN_EID_ERP_INFO:
1525 elems->erp_info = pos;
1526 elems->erp_info_len = elen;
1527 break;
1528 case WLAN_EID_EXT_SUPP_RATES:
1529 elems->ext_supp_rates = pos;
1530 elems->ext_supp_rates_len = elen;
1531 break;
1532 case WLAN_EID_VENDOR_SPECIFIC:
1533 if (rtw_ieee802_11_parse_vendor_specific(pos, elen,
1534 elems,
1535 show_errors))
1536 unknown++;
1537 break;
1538 case WLAN_EID_RSN:
1539 elems->rsn_ie = pos;
1540 elems->rsn_ie_len = elen;
1541 break;
1542 case WLAN_EID_PWR_CAPABILITY:
1543 elems->power_cap = pos;
1544 elems->power_cap_len = elen;
1545 break;
1546 case WLAN_EID_SUPPORTED_CHANNELS:
1547 elems->supp_channels = pos;
1548 elems->supp_channels_len = elen;
1549 break;
1550 case WLAN_EID_MOBILITY_DOMAIN:
1551 elems->mdie = pos;
1552 elems->mdie_len = elen;
1553 break;
1554 case WLAN_EID_FAST_BSS_TRANSITION:
1555 elems->ftie = pos;
1556 elems->ftie_len = elen;
1557 break;
1558 case WLAN_EID_TIMEOUT_INTERVAL:
1559 elems->timeout_int = pos;
1560 elems->timeout_int_len = elen;
1561 break;
1562 case WLAN_EID_HT_CAP:
1563 elems->ht_capabilities = pos;
1564 elems->ht_capabilities_len = elen;
1565 break;
1566 case WLAN_EID_HT_OPERATION:
1567 elems->ht_operation = pos;
1568 elems->ht_operation_len = elen;
1569 break;
1570 case WLAN_EID_VHT_CAPABILITY:
1571 elems->vht_capabilities = pos;
1572 elems->vht_capabilities_len = elen;
1573 break;
1574 case WLAN_EID_VHT_OPERATION:
1575 elems->vht_operation = pos;
1576 elems->vht_operation_len = elen;
1577 break;
1578 case WLAN_EID_VHT_OP_MODE_NOTIFY:
1579 elems->vht_op_mode_notify = pos;
1580 elems->vht_op_mode_notify_len = elen;
1581 break;
1582 case _EID_RRM_EN_CAP_IE_:
1583 elems->rm_en_cap = pos;
1584 elems->rm_en_cap_len = elen;
1585 break;
1586 #ifdef CONFIG_RTW_MESH
1587 case WLAN_EID_PREQ:
1588 elems->preq = pos;
1589 elems->preq_len = elen;
1590 break;
1591 case WLAN_EID_PREP:
1592 elems->prep = pos;
1593 elems->prep_len = elen;
1594 break;
1595 case WLAN_EID_PERR:
1596 elems->perr = pos;
1597 elems->perr_len = elen;
1598 break;
1599 case WLAN_EID_RANN:
1600 elems->rann = pos;
1601 elems->rann_len = elen;
1602 break;
1603 #endif
1604 default:
1605 unknown++;
1606 if (!show_errors)
1607 break;
1608 RTW_DBG("IEEE 802.11 element parse "
1609 "ignored unknown element (id=%d elen=%d)\n",
1610 id, elen);
1611 break;
1612 }
1613
1614 left -= elen;
1615 pos += elen;
1616 }
1617
1618 if (left)
1619 return ParseFailed;
1620
1621 return unknown ? ParseUnknown : ParseOK;
1622
1623 }
1624
1625 static u8 key_char2num(u8 ch);
key_char2num(u8 ch)1626 static u8 key_char2num(u8 ch)
1627 {
1628 if ((ch >= '0') && (ch <= '9'))
1629 return ch - '0';
1630 else if ((ch >= 'a') && (ch <= 'f'))
1631 return ch - 'a' + 10;
1632 else if ((ch >= 'A') && (ch <= 'F'))
1633 return ch - 'A' + 10;
1634 else
1635 return 0xff;
1636 }
1637
1638 u8 str_2char2num(u8 hch, u8 lch);
str_2char2num(u8 hch,u8 lch)1639 u8 str_2char2num(u8 hch, u8 lch)
1640 {
1641 return (key_char2num(hch) * 10) + key_char2num(lch);
1642 }
1643
1644 u8 key_2char2num(u8 hch, u8 lch);
key_2char2num(u8 hch,u8 lch)1645 u8 key_2char2num(u8 hch, u8 lch)
1646 {
1647 return (key_char2num(hch) << 4) | key_char2num(lch);
1648 }
1649
1650 void macstr2num(u8 *dst, u8 *src);
macstr2num(u8 * dst,u8 * src)1651 void macstr2num(u8 *dst, u8 *src)
1652 {
1653 int jj, kk;
1654 for (jj = 0, kk = 0; jj < ETH_ALEN; jj++, kk += 3)
1655 dst[jj] = key_2char2num(src[kk], src[kk + 1]);
1656 }
1657
convert_ip_addr(u8 hch,u8 mch,u8 lch)1658 u8 convert_ip_addr(u8 hch, u8 mch, u8 lch)
1659 {
1660 return (key_char2num(hch) * 100) + (key_char2num(mch) * 10) + key_char2num(lch);
1661 }
1662
1663 #ifdef CONFIG_PLATFORM_INTEL_BYT
1664 #define MAC_ADDRESS_LEN 12
1665
rtw_get_mac_addr_intel(unsigned char * buf)1666 int rtw_get_mac_addr_intel(unsigned char *buf)
1667 {
1668 int ret = 0;
1669 int i;
1670 struct file *fp = NULL;
1671 mm_segment_t oldfs;
1672 unsigned char c_mac[MAC_ADDRESS_LEN];
1673 char fname[] = "/config/wifi/mac.txt";
1674 int jj, kk;
1675
1676 RTW_INFO("%s Enter\n", __FUNCTION__);
1677
1678 ret = rtw_retrieve_from_file(fname, c_mac, MAC_ADDRESS_LEN);
1679 if (ret < MAC_ADDRESS_LEN)
1680 return -1;
1681
1682 for (jj = 0, kk = 0; jj < ETH_ALEN; jj++, kk += 2)
1683 buf[jj] = key_2char2num(c_mac[kk], c_mac[kk + 1]);
1684
1685 RTW_INFO("%s: read from file mac address: "MAC_FMT"\n",
1686 __FUNCTION__, MAC_ARG(buf));
1687
1688 return 0;
1689 }
1690 #endif /* CONFIG_PLATFORM_INTEL_BYT */
1691
1692 /*
1693 * Description:
1694 * rtw_check_invalid_mac_address:
1695 * This is only used for checking mac address valid or not.
1696 *
1697 * Input:
1698 * adapter: mac_address pointer.
1699 * check_local_bit: check locally bit or not.
1700 *
1701 * Output:
1702 * _TRUE: The mac address is invalid.
1703 * _FALSE: The mac address is valid.
1704 *
1705 * Auther: Isaac.Li
1706 */
rtw_check_invalid_mac_address(u8 * mac_addr,u8 check_local_bit)1707 u8 rtw_check_invalid_mac_address(u8 *mac_addr, u8 check_local_bit)
1708 {
1709 u8 null_mac_addr[ETH_ALEN] = {0, 0, 0, 0, 0, 0};
1710 u8 multi_mac_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
1711 u8 res = _FALSE;
1712
1713 if (_rtw_memcmp(mac_addr, null_mac_addr, ETH_ALEN)) {
1714 res = _TRUE;
1715 goto func_exit;
1716 }
1717
1718 if (_rtw_memcmp(mac_addr, multi_mac_addr, ETH_ALEN)) {
1719 res = _TRUE;
1720 goto func_exit;
1721 }
1722
1723 if (mac_addr[0] & BIT0) {
1724 res = _TRUE;
1725 goto func_exit;
1726 }
1727
1728 if (check_local_bit == _TRUE) {
1729 if (mac_addr[0] & BIT1) {
1730 res = _TRUE;
1731 goto func_exit;
1732 }
1733 }
1734
1735 func_exit:
1736 return res;
1737 }
1738
1739 extern char *rtw_initmac;
1740 /**
1741 * rtw_macaddr_cfg - Decide the mac address used
1742 * @out: buf to store mac address decided
1743 * @hw_mac_addr: mac address from efuse/epprom
1744 */
rtw_macaddr_cfg(u8 * out,const u8 * hw_mac_addr)1745 void rtw_macaddr_cfg(u8 *out, const u8 *hw_mac_addr)
1746 {
1747 #define DEFAULT_RANDOM_MACADDR 1
1748 u8 mac[ETH_ALEN];
1749
1750 if (out == NULL) {
1751 rtw_warn_on(1);
1752 return;
1753 }
1754
1755 /* Users specify the mac address */
1756 if (rtw_initmac) {
1757 int jj, kk;
1758
1759 for (jj = 0, kk = 0; jj < ETH_ALEN; jj++, kk += 3)
1760 mac[jj] = key_2char2num(rtw_initmac[kk], rtw_initmac[kk + 1]);
1761
1762 goto err_chk;
1763 }
1764
1765 /* platform specified */
1766 #ifdef CONFIG_PLATFORM_INTEL_BYT
1767 if (rtw_get_mac_addr_intel(mac) == 0)
1768 goto err_chk;
1769 #endif
1770
1771 /* Use the mac address stored in the Efuse */
1772 if (hw_mac_addr) {
1773 _rtw_memcpy(mac, hw_mac_addr, ETH_ALEN);
1774 goto err_chk;
1775 }
1776
1777 err_chk:
1778 if (rtw_check_invalid_mac_address(mac, _TRUE) == _TRUE) {
1779 #if DEFAULT_RANDOM_MACADDR
1780 RTW_ERR("invalid mac addr:"MAC_FMT", assign random MAC\n", MAC_ARG(mac));
1781 *((u32 *)(&mac[2])) = rtw_random32();
1782 mac[0] = 0x00;
1783 mac[1] = 0xe0;
1784 mac[2] = 0x4c;
1785 #else
1786 RTW_ERR("invalid mac addr:"MAC_FMT", assign default one\n", MAC_ARG(mac));
1787 mac[0] = 0x00;
1788 mac[1] = 0xe0;
1789 mac[2] = 0x4c;
1790 mac[3] = 0x87;
1791 mac[4] = 0x00;
1792 mac[5] = 0x00;
1793 #endif
1794 }
1795
1796 _rtw_memcpy(out, mac, ETH_ALEN);
1797 RTW_INFO("%s mac addr:"MAC_FMT"\n", __func__, MAC_ARG(out));
1798 }
1799
1800 #ifdef CONFIG_RTW_DEBUG
1801 #ifdef CONFIG_80211N_HT
dump_ht_cap_ie_content(void * sel,const u8 * buf,u32 buf_len)1802 void dump_ht_cap_ie_content(void *sel, const u8 *buf, u32 buf_len)
1803 {
1804 if (buf_len != HT_CAP_IE_LEN) {
1805 RTW_PRINT_SEL(sel, "Invalid HT capability IE len:%d != %d\n", buf_len, HT_CAP_IE_LEN);
1806 return;
1807 }
1808
1809 RTW_PRINT_SEL(sel, "cap_info:%02x%02x:%s\n", *(buf), *(buf + 1)
1810 , GET_HT_CAP_ELE_CHL_WIDTH(buf) ? " 40MHz" : " 20MHz");
1811 RTW_PRINT_SEL(sel, "A-MPDU Parameters:"HT_AMPDU_PARA_FMT"\n"
1812 , HT_AMPDU_PARA_ARG(HT_CAP_ELE_AMPDU_PARA(buf)));
1813 RTW_PRINT_SEL(sel, "Supported MCS Set:"HT_SUP_MCS_SET_FMT"\n"
1814 , HT_SUP_MCS_SET_ARG(HT_CAP_ELE_SUP_MCS_SET(buf)));
1815 }
1816
dump_ht_cap_ie(void * sel,const u8 * ie,u32 ie_len)1817 void dump_ht_cap_ie(void *sel, const u8 *ie, u32 ie_len)
1818 {
1819 const u8 *ht_cap_ie;
1820 sint ht_cap_ielen;
1821
1822 ht_cap_ie = rtw_get_ie(ie, WLAN_EID_HT_CAP, &ht_cap_ielen, ie_len);
1823 if (!ie || ht_cap_ie != ie)
1824 return;
1825
1826 dump_ht_cap_ie_content(sel, ht_cap_ie + 2, ht_cap_ielen);
1827 }
1828
1829 const char *const _ht_sc_offset_str[] = {
1830 "SCN",
1831 "SCA",
1832 "SC-RSVD",
1833 "SCB",
1834 };
1835
dump_ht_op_ie_content(void * sel,const u8 * buf,u32 buf_len)1836 void dump_ht_op_ie_content(void *sel, const u8 *buf, u32 buf_len)
1837 {
1838 if (buf_len != HT_OP_IE_LEN) {
1839 RTW_PRINT_SEL(sel, "Invalid HT operation IE len:%d != %d\n", buf_len, HT_OP_IE_LEN);
1840 return;
1841 }
1842
1843 RTW_PRINT_SEL(sel, "ch:%u%s %s\n"
1844 , GET_HT_OP_ELE_PRI_CHL(buf)
1845 , GET_HT_OP_ELE_STA_CHL_WIDTH(buf) ? "" : " 20MHz only"
1846 , ht_sc_offset_str(GET_HT_OP_ELE_2ND_CHL_OFFSET(buf))
1847 );
1848 }
1849
dump_ht_op_ie(void * sel,const u8 * ie,u32 ie_len)1850 void dump_ht_op_ie(void *sel, const u8 *ie, u32 ie_len)
1851 {
1852 const u8 *ht_op_ie;
1853 sint ht_op_ielen;
1854
1855 ht_op_ie = rtw_get_ie(ie, WLAN_EID_HT_OPERATION, &ht_op_ielen, ie_len);
1856 if (!ie || ht_op_ie != ie)
1857 return;
1858
1859 dump_ht_op_ie_content(sel, ht_op_ie + 2, ht_op_ielen);
1860 }
1861 #endif /* CONFIG_80211N_HT */
1862
dump_wps_ie(void * sel,const u8 * ie,u32 ie_len)1863 void dump_wps_ie(void *sel, const u8 *ie, u32 ie_len)
1864 {
1865 const u8 *pos = ie;
1866 u16 id;
1867 u16 len;
1868
1869 const u8 *wps_ie;
1870 uint wps_ielen;
1871
1872 wps_ie = rtw_get_wps_ie(ie, ie_len, NULL, &wps_ielen);
1873 if (wps_ie != ie || wps_ielen == 0)
1874 return;
1875
1876 pos += 6;
1877 while (pos - ie + 4 <= ie_len) {
1878 id = RTW_GET_BE16(pos);
1879 len = RTW_GET_BE16(pos + 2);
1880
1881 RTW_PRINT_SEL(sel, "%s ID:0x%04x, LEN:%u%s\n", __func__, id, len
1882 , ((pos - ie + 4 + len) <= ie_len) ? "" : "(exceed ie_len)");
1883
1884 pos += (4 + len);
1885 }
1886 }
1887 #endif /* CONFIG_RTW_DEBUG */
dump_ies(void * sel,const u8 * buf,u32 buf_len)1888 void dump_ies(void *sel, const u8 *buf, u32 buf_len)
1889 {
1890 #ifdef CONFIG_RTW_DEBUG
1891 const u8 *pos = buf;
1892 u8 id, len;
1893
1894 while (pos - buf + 1 < buf_len) {
1895 id = *pos;
1896 len = *(pos + 1);
1897
1898 RTW_PRINT_SEL(sel, "%s ID:%u, LEN:%u\n", __FUNCTION__, id, len);
1899 #ifdef CONFIG_80211N_HT
1900 dump_ht_cap_ie(sel, pos, len + 2);
1901 dump_ht_op_ie(sel, pos, len + 2);
1902 #endif
1903 #ifdef CONFIG_80211AC_VHT
1904 dump_vht_cap_ie(sel, pos, len + 2);
1905 dump_vht_op_ie(sel, pos, len + 2);
1906 #endif
1907 dump_wps_ie(sel, pos, len + 2);
1908 #ifdef CONFIG_P2P
1909 dump_p2p_ie(sel, pos, len + 2);
1910 #ifdef CONFIG_WFD
1911 dump_wfd_ie(sel, pos, len + 2);
1912 #endif
1913 #endif
1914 #ifdef CONFIG_RTW_MULTI_AP
1915 dump_multi_ap_ie(sel, pos, len + 2);
1916 #endif
1917
1918 pos += (2 + len);
1919 }
1920 #endif /* CONFIG_RTW_DEBUG */
1921 }
1922
1923 /**
1924 * rtw_ies_get_chbw - get operation ch, bw, offset from IEs of BSS.
1925 * @ies: pointer of the first tlv IE
1926 * @ies_len: length of @ies
1927 * @ch: pointer of ch, used as output
1928 * @bw: pointer of bw, used as output
1929 * @offset: pointer of offset, used as output
1930 * @ht: check HT IEs
1931 * @vht: check VHT IEs, if true imply ht is true
1932 */
rtw_ies_get_chbw(u8 * ies,int ies_len,u8 * ch,u8 * bw,u8 * offset,u8 ht,u8 vht)1933 void rtw_ies_get_chbw(u8 *ies, int ies_len, u8 *ch, u8 *bw, u8 *offset, u8 ht, u8 vht)
1934 {
1935 u8 *p;
1936 int ie_len;
1937
1938 *ch = 0;
1939 *bw = CHANNEL_WIDTH_20;
1940 *offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
1941
1942 p = rtw_get_ie(ies, _DSSET_IE_, &ie_len, ies_len);
1943 if (p && ie_len > 0)
1944 *ch = *(p + 2);
1945
1946 #ifdef CONFIG_80211N_HT
1947 if (ht || vht) {
1948 u8 *ht_cap_ie, *ht_op_ie;
1949 int ht_cap_ielen, ht_op_ielen;
1950
1951 ht_cap_ie = rtw_get_ie(ies, EID_HTCapability, &ht_cap_ielen, ies_len);
1952 if (ht_cap_ie && ht_cap_ielen) {
1953 if (GET_HT_CAP_ELE_CHL_WIDTH(ht_cap_ie + 2))
1954 *bw = CHANNEL_WIDTH_40;
1955 }
1956
1957 ht_op_ie = rtw_get_ie(ies, EID_HTInfo, &ht_op_ielen, ies_len);
1958 if (ht_op_ie && ht_op_ielen) {
1959 if (*ch == 0)
1960 *ch = GET_HT_OP_ELE_PRI_CHL(ht_op_ie + 2);
1961 else if (*ch != 0 && *ch != GET_HT_OP_ELE_PRI_CHL(ht_op_ie + 2)) {
1962 RTW_INFO("%s ch inconsistent, DSSS:%u, HT primary:%u\n"
1963 , __func__, *ch, GET_HT_OP_ELE_PRI_CHL(ht_op_ie + 2));
1964 }
1965
1966 if (!GET_HT_OP_ELE_STA_CHL_WIDTH(ht_op_ie + 2))
1967 *bw = CHANNEL_WIDTH_20;
1968
1969 if (*bw == CHANNEL_WIDTH_40) {
1970 switch (GET_HT_OP_ELE_2ND_CHL_OFFSET(ht_op_ie + 2)) {
1971 case SCA:
1972 *offset = HAL_PRIME_CHNL_OFFSET_LOWER;
1973 break;
1974 case SCB:
1975 *offset = HAL_PRIME_CHNL_OFFSET_UPPER;
1976 break;
1977 }
1978 }
1979 }
1980
1981 #ifdef CONFIG_80211AC_VHT
1982 if (vht) {
1983 u8 *vht_op_ie;
1984 int vht_op_ielen;
1985
1986 vht_op_ie = rtw_get_ie(ies, EID_VHTOperation, &vht_op_ielen, ies_len);
1987 if (vht_op_ie && vht_op_ielen) {
1988 if (GET_VHT_OPERATION_ELE_CHL_WIDTH(vht_op_ie + 2) >= 1)
1989 *bw = CHANNEL_WIDTH_80;
1990 }
1991 }
1992 #endif /* CONFIG_80211AC_VHT */
1993
1994 }
1995 #endif /* CONFIG_80211N_HT */
1996 }
1997
rtw_bss_get_chbw(WLAN_BSSID_EX * bss,u8 * ch,u8 * bw,u8 * offset,u8 ht,u8 vht)1998 void rtw_bss_get_chbw(WLAN_BSSID_EX *bss, u8 *ch, u8 *bw, u8 *offset, u8 ht, u8 vht)
1999 {
2000 rtw_ies_get_chbw(bss->IEs + sizeof(NDIS_802_11_FIXED_IEs)
2001 , bss->IELength - sizeof(NDIS_802_11_FIXED_IEs)
2002 , ch, bw, offset, ht, vht);
2003
2004 if (*ch == 0)
2005 *ch = bss->Configuration.DSConfig;
2006 else if (*ch != bss->Configuration.DSConfig) {
2007 RTW_INFO("inconsistent ch - ies:%u bss->Configuration.DSConfig:%u\n"
2008 , *ch, bss->Configuration.DSConfig);
2009 *ch = bss->Configuration.DSConfig;
2010 rtw_warn_on(1);
2011 }
2012 }
2013
2014 /**
2015 * rtw_is_chbw_grouped - test if the two ch settings can be grouped together
2016 * @ch_a: ch of set a
2017 * @bw_a: bw of set a
2018 * @offset_a: offset of set a
2019 * @ch_b: ch of set b
2020 * @bw_b: bw of set b
2021 * @offset_b: offset of set b
2022 */
rtw_is_chbw_grouped(u8 ch_a,u8 bw_a,u8 offset_a,u8 ch_b,u8 bw_b,u8 offset_b)2023 bool rtw_is_chbw_grouped(u8 ch_a, u8 bw_a, u8 offset_a
2024 , u8 ch_b, u8 bw_b, u8 offset_b)
2025 {
2026 bool is_grouped = _FALSE;
2027
2028 if (ch_a != ch_b) {
2029 /* ch is different */
2030 goto exit;
2031 } else if ((bw_a == CHANNEL_WIDTH_40 || bw_a == CHANNEL_WIDTH_80)
2032 && (bw_b == CHANNEL_WIDTH_40 || bw_b == CHANNEL_WIDTH_80)
2033 ) {
2034 if (offset_a != offset_b)
2035 goto exit;
2036 }
2037
2038 is_grouped = _TRUE;
2039
2040 exit:
2041 return is_grouped;
2042 }
2043
2044 /**
2045 * rtw_sync_chbw - obey g_ch, adjust g_bw, g_offset, bw, offset
2046 * @req_ch: pointer of the request ch, may be modified further
2047 * @req_bw: pointer of the request bw, may be modified further
2048 * @req_offset: pointer of the request offset, may be modified further
2049 * @g_ch: pointer of the ongoing group ch
2050 * @g_bw: pointer of the ongoing group bw, may be modified further
2051 * @g_offset: pointer of the ongoing group offset, may be modified further
2052 */
rtw_sync_chbw(u8 * req_ch,u8 * req_bw,u8 * req_offset,u8 * g_ch,u8 * g_bw,u8 * g_offset)2053 void rtw_sync_chbw(u8 *req_ch, u8 *req_bw, u8 *req_offset
2054 , u8 *g_ch, u8 *g_bw, u8 *g_offset)
2055 {
2056
2057 *req_ch = *g_ch;
2058
2059 if (*req_bw == CHANNEL_WIDTH_80 && *g_ch <= 14) {
2060 /*2.4G ch, downgrade to 40Mhz */
2061 *req_bw = CHANNEL_WIDTH_40;
2062 }
2063
2064 switch (*req_bw) {
2065 case CHANNEL_WIDTH_80:
2066 if (*g_bw == CHANNEL_WIDTH_40 || *g_bw == CHANNEL_WIDTH_80)
2067 *req_offset = *g_offset;
2068 else if (*g_bw == CHANNEL_WIDTH_20)
2069 rtw_get_offset_by_chbw(*req_ch, *req_bw, req_offset);
2070
2071 if (*req_offset == HAL_PRIME_CHNL_OFFSET_DONT_CARE) {
2072 RTW_ERR("%s req 80MHz BW without offset, down to 20MHz\n", __func__);
2073 rtw_warn_on(1);
2074 *req_bw = CHANNEL_WIDTH_20;
2075 }
2076 break;
2077 case CHANNEL_WIDTH_40:
2078 if (*g_bw == CHANNEL_WIDTH_40 || *g_bw == CHANNEL_WIDTH_80)
2079 *req_offset = *g_offset;
2080 else if (*g_bw == CHANNEL_WIDTH_20)
2081 rtw_get_offset_by_chbw(*req_ch, *req_bw, req_offset);
2082
2083 if (*req_offset == HAL_PRIME_CHNL_OFFSET_DONT_CARE) {
2084 RTW_ERR("%s req 40MHz BW without offset, down to 20MHz\n", __func__);
2085 rtw_warn_on(1);
2086 *req_bw = CHANNEL_WIDTH_20;
2087 }
2088 break;
2089 case CHANNEL_WIDTH_20:
2090 *req_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
2091 break;
2092 default:
2093 RTW_ERR("%s req unsupported BW:%u\n", __func__, *req_bw);
2094 rtw_warn_on(1);
2095 }
2096
2097 if (*req_bw > *g_bw) {
2098 *g_bw = *req_bw;
2099 *g_offset = *req_offset;
2100 }
2101 }
2102
2103 #ifdef CONFIG_P2P
2104 /**
2105 * rtw_get_p2p_merged_len - Get merged ie length from muitiple p2p ies.
2106 * @in_ie: Pointer of the first p2p ie
2107 * @in_len: Total len of muiltiple p2p ies
2108 * Returns: Length of merged p2p ie length
2109 */
rtw_get_p2p_merged_ies_len(u8 * in_ie,u32 in_len)2110 u32 rtw_get_p2p_merged_ies_len(u8 *in_ie, u32 in_len)
2111 {
2112 PNDIS_802_11_VARIABLE_IEs pIE;
2113 u8 OUI[4] = { 0x50, 0x6f, 0x9a, 0x09 };
2114 int i = 0;
2115 int len = 0;
2116
2117 while (i < in_len) {
2118 pIE = (PNDIS_802_11_VARIABLE_IEs)(in_ie + i);
2119
2120 if (pIE->ElementID == _VENDOR_SPECIFIC_IE_ && _rtw_memcmp(pIE->data, OUI, 4)) {
2121 len += pIE->Length - 4; /* 4 is P2P OUI length, don't count it in this loop */
2122 }
2123
2124 i += (pIE->Length + 2);
2125 }
2126
2127 return len + 4; /* Append P2P OUI length at last. */
2128 }
2129
2130 /**
2131 * rtw_p2p_merge_ies - Merge muitiple p2p ies into one
2132 * @in_ie: Pointer of the first p2p ie
2133 * @in_len: Total len of muiltiple p2p ies
2134 * @merge_ie: Pointer of merged ie
2135 * Returns: Length of merged p2p ie
2136 */
rtw_p2p_merge_ies(u8 * in_ie,u32 in_len,u8 * merge_ie)2137 int rtw_p2p_merge_ies(u8 *in_ie, u32 in_len, u8 *merge_ie)
2138 {
2139 PNDIS_802_11_VARIABLE_IEs pIE;
2140 u8 len = 0;
2141 u8 OUI[4] = { 0x50, 0x6f, 0x9a, 0x09 };
2142 u8 ELOUI[6] = { 0xDD, 0x00, 0x50, 0x6f, 0x9a, 0x09 }; /* EID;Len;OUI, Len would copy at the end of function */
2143 int i = 0;
2144
2145 if (merge_ie != NULL) {
2146 /* Set first P2P OUI */
2147 _rtw_memcpy(merge_ie, ELOUI, 6);
2148 merge_ie += 6;
2149
2150 while (i < in_len) {
2151 pIE = (PNDIS_802_11_VARIABLE_IEs)(in_ie + i);
2152
2153 /* Take out the rest of P2P OUIs */
2154 if (pIE->ElementID == _VENDOR_SPECIFIC_IE_ && _rtw_memcmp(pIE->data, OUI, 4)) {
2155 _rtw_memcpy(merge_ie, pIE->data + 4, pIE->Length - 4);
2156 len += pIE->Length - 4;
2157 merge_ie += pIE->Length - 4;
2158 }
2159
2160 i += (pIE->Length + 2);
2161 }
2162
2163 return len + 4; /* 4 is for P2P OUI */
2164
2165 }
2166
2167 return 0;
2168 }
2169
dump_p2p_ie(void * sel,const u8 * ie,u32 ie_len)2170 void dump_p2p_ie(void *sel, const u8 *ie, u32 ie_len)
2171 {
2172 const u8 *pos = ie;
2173 u8 id;
2174 u16 len;
2175
2176 const u8 *p2p_ie;
2177 uint p2p_ielen;
2178
2179 p2p_ie = rtw_get_p2p_ie(ie, ie_len, NULL, &p2p_ielen);
2180 if (p2p_ie != ie || p2p_ielen == 0)
2181 return;
2182
2183 pos += 6;
2184 while (pos - ie + 3 <= ie_len) {
2185 id = *pos;
2186 len = RTW_GET_LE16(pos + 1);
2187
2188 RTW_PRINT_SEL(sel, "%s ID:%u, LEN:%u%s\n", __func__, id, len
2189 , ((pos - ie + 3 + len) <= ie_len) ? "" : "(exceed ie_len)");
2190
2191 pos += (3 + len);
2192 }
2193 }
2194
2195 /**
2196 * rtw_get_p2p_ie - Search P2P IE from a series of IEs
2197 * @in_ie: Address of IEs to search
2198 * @in_len: Length limit from in_ie
2199 * @p2p_ie: If not NULL and P2P IE is found, P2P IE will be copied to the buf starting from p2p_ie
2200 * @p2p_ielen: If not NULL and P2P IE is found, will set to the length of the entire P2P IE
2201 *
2202 * Returns: The address of the P2P IE found, or NULL
2203 */
rtw_get_p2p_ie(const u8 * in_ie,int in_len,u8 * p2p_ie,uint * p2p_ielen)2204 u8 *rtw_get_p2p_ie(const u8 *in_ie, int in_len, u8 *p2p_ie, uint *p2p_ielen)
2205 {
2206 uint cnt;
2207 const u8 *p2p_ie_ptr = NULL;
2208 u8 eid, p2p_oui[4] = {0x50, 0x6F, 0x9A, 0x09};
2209
2210 if (p2p_ielen)
2211 *p2p_ielen = 0;
2212
2213 if (!in_ie || in_len < 0) {
2214 rtw_warn_on(1);
2215 return (u8 *)p2p_ie_ptr;
2216 }
2217
2218 if (in_len <= 0)
2219 return (u8 *)p2p_ie_ptr;
2220
2221 cnt = 0;
2222
2223 while (cnt + 1 + 4 < in_len) {
2224 eid = in_ie[cnt];
2225
2226 if (cnt + 1 + 4 >= MAX_IE_SZ) {
2227 rtw_warn_on(1);
2228 return NULL;
2229 }
2230
2231 if (eid == WLAN_EID_VENDOR_SPECIFIC && _rtw_memcmp(&in_ie[cnt + 2], p2p_oui, 4) == _TRUE) {
2232 p2p_ie_ptr = in_ie + cnt;
2233
2234 if (p2p_ie)
2235 _rtw_memcpy(p2p_ie, &in_ie[cnt], in_ie[cnt + 1] + 2);
2236
2237 if (p2p_ielen)
2238 *p2p_ielen = in_ie[cnt + 1] + 2;
2239
2240 break;
2241 } else
2242 cnt += in_ie[cnt + 1] + 2;
2243
2244 }
2245
2246 return (u8 *)p2p_ie_ptr;
2247 }
2248
2249 /**
2250 * rtw_get_p2p_attr - Search a specific P2P attribute from a given P2P IE
2251 * @p2p_ie: Address of P2P IE to search
2252 * @p2p_ielen: Length limit from p2p_ie
2253 * @target_attr_id: The attribute ID of P2P attribute to search
2254 * @buf_attr: If not NULL and the P2P attribute is found, P2P attribute will be copied to the buf starting from buf_attr
2255 * @len_attr: If not NULL and the P2P attribute is found, will set to the length of the entire P2P attribute
2256 *
2257 * Returns: the address of the specific WPS attribute found, or NULL
2258 */
rtw_get_p2p_attr(u8 * p2p_ie,uint p2p_ielen,u8 target_attr_id,u8 * buf_attr,u32 * len_attr)2259 u8 *rtw_get_p2p_attr(u8 *p2p_ie, uint p2p_ielen, u8 target_attr_id , u8 *buf_attr, u32 *len_attr)
2260 {
2261 u8 *attr_ptr = NULL;
2262 u8 *target_attr_ptr = NULL;
2263 u8 p2p_oui[4] = {0x50, 0x6F, 0x9A, 0x09};
2264
2265 if (len_attr)
2266 *len_attr = 0;
2267
2268 if (!p2p_ie
2269 || p2p_ielen <= 6
2270 || (p2p_ie[0] != WLAN_EID_VENDOR_SPECIFIC)
2271 || (_rtw_memcmp(p2p_ie + 2, p2p_oui, 4) != _TRUE))
2272 return attr_ptr;
2273
2274 /* 6 = 1(Element ID) + 1(Length) + 3 (OUI) + 1(OUI Type) */
2275 attr_ptr = p2p_ie + 6; /* goto first attr */
2276
2277 while ((attr_ptr - p2p_ie + 3) <= p2p_ielen) {
2278 /* 3 = 1(Attribute ID) + 2(Length) */
2279 u8 attr_id = *attr_ptr;
2280 u16 attr_data_len = RTW_GET_LE16(attr_ptr + 1);
2281 u16 attr_len = attr_data_len + 3;
2282
2283 if (0)
2284 RTW_INFO("%s attr_ptr:%p, id:%u, length:%u\n", __func__, attr_ptr, attr_id, attr_data_len);
2285
2286 if ((attr_ptr - p2p_ie + attr_len) > p2p_ielen)
2287 break;
2288
2289 if (attr_id == target_attr_id) {
2290 target_attr_ptr = attr_ptr;
2291
2292 if (buf_attr)
2293 _rtw_memcpy(buf_attr, attr_ptr, attr_len);
2294
2295 if (len_attr)
2296 *len_attr = attr_len;
2297
2298 break;
2299 } else
2300 attr_ptr += attr_len;
2301 }
2302
2303 return target_attr_ptr;
2304 }
2305
2306 /**
2307 * rtw_get_p2p_attr_content - Search a specific P2P attribute content from a given P2P IE
2308 * @p2p_ie: Address of P2P IE to search
2309 * @p2p_ielen: Length limit from p2p_ie
2310 * @target_attr_id: The attribute ID of P2P attribute to search
2311 * @buf_content: If not NULL and the P2P attribute is found, P2P attribute content will be copied to the buf starting from buf_content
2312 * If len_content is NULL, only copy one byte.
2313 * @len_content: If not NULL and the P2P attribute is found, will set to the length of the P2P attribute content
2314 *
2315 * Returns: the address of the specific P2P attribute content found, or NULL
2316 */
rtw_get_p2p_attr_content(u8 * p2p_ie,uint p2p_ielen,u8 target_attr_id,u8 * buf_content,uint * len_content)2317 u8 *rtw_get_p2p_attr_content(u8 *p2p_ie, uint p2p_ielen, u8 target_attr_id , u8 *buf_content, uint *len_content)
2318 {
2319 u8 *attr_ptr;
2320 u32 attr_len;
2321
2322 attr_ptr = rtw_get_p2p_attr(p2p_ie, p2p_ielen, target_attr_id, NULL, &attr_len);
2323
2324 if (attr_ptr && attr_len) {
2325 if (len_content) {
2326 if ((buf_content && (*len_content > (attr_len - 3))) || !buf_content)
2327 *len_content = attr_len - 3;
2328 }
2329
2330 if (len_content && buf_content) {
2331 _rtw_memcpy(buf_content, attr_ptr + 3, *len_content);
2332 } else if (buf_content) {
2333 _rtw_memcpy(buf_content, attr_ptr + 3, 1);
2334 }
2335
2336 return attr_ptr + 3;
2337 }
2338
2339 if (len_content)
2340 *len_content = 0;
2341
2342 return NULL;
2343 }
2344
rtw_set_p2p_attr_content(u8 * pbuf,u8 attr_id,u16 attr_len,u8 * pdata_attr)2345 u32 rtw_set_p2p_attr_content(u8 *pbuf, u8 attr_id, u16 attr_len, u8 *pdata_attr)
2346 {
2347 u32 a_len;
2348
2349 *pbuf = attr_id;
2350
2351 /* *(u16*)(pbuf + 1) = cpu_to_le16(attr_len); */
2352 RTW_PUT_LE16(pbuf + 1, attr_len);
2353
2354 if (pdata_attr)
2355 _rtw_memcpy(pbuf + 3, pdata_attr, attr_len);
2356
2357 a_len = attr_len + 3;
2358
2359 return a_len;
2360 }
2361
rtw_del_p2p_ie(u8 * ies,uint ies_len_ori,const char * msg)2362 uint rtw_del_p2p_ie(u8 *ies, uint ies_len_ori, const char *msg)
2363 {
2364 #define DBG_DEL_P2P_IE 0
2365
2366 u8 *target_ie;
2367 u32 target_ie_len;
2368 uint ies_len = ies_len_ori;
2369 int index = 0;
2370
2371 while (1) {
2372 target_ie = rtw_get_p2p_ie(ies, ies_len, NULL, &target_ie_len);
2373 if (target_ie && target_ie_len) {
2374 u8 *next_ie = target_ie + target_ie_len;
2375 uint remain_len = ies_len - (next_ie - ies);
2376
2377 if (DBG_DEL_P2P_IE && msg) {
2378 RTW_INFO("%s %d before\n", __func__, index);
2379 dump_ies(RTW_DBGDUMP, ies, ies_len);
2380
2381 RTW_INFO("ies:%p, ies_len:%u\n", ies, ies_len);
2382 RTW_INFO("target_ie:%p, target_ie_len:%u\n", target_ie, target_ie_len);
2383 RTW_INFO("next_ie:%p, remain_len:%u\n", next_ie, remain_len);
2384 }
2385
2386 _rtw_memmove(target_ie, next_ie, remain_len);
2387 _rtw_memset(target_ie + remain_len, 0, target_ie_len);
2388 ies_len -= target_ie_len;
2389
2390 if (DBG_DEL_P2P_IE && msg) {
2391 RTW_INFO("%s %d after\n", __func__, index);
2392 dump_ies(RTW_DBGDUMP, ies, ies_len);
2393 }
2394
2395 index++;
2396 } else
2397 break;
2398 }
2399
2400 return ies_len;
2401 }
2402
rtw_del_p2p_attr(u8 * ie,uint ielen_ori,u8 attr_id)2403 uint rtw_del_p2p_attr(u8 *ie, uint ielen_ori, u8 attr_id)
2404 {
2405 #define DBG_DEL_P2P_ATTR 0
2406
2407 u8 *target_attr;
2408 u32 target_attr_len;
2409 uint ielen = ielen_ori;
2410 int index = 0;
2411
2412 while (1) {
2413 target_attr = rtw_get_p2p_attr(ie, ielen, attr_id, NULL, &target_attr_len);
2414 if (target_attr && target_attr_len) {
2415 u8 *next_attr = target_attr + target_attr_len;
2416 uint remain_len = ielen - (next_attr - ie);
2417
2418 if (DBG_DEL_P2P_ATTR) {
2419 RTW_INFO("%s %d before\n", __func__, index);
2420 dump_ies(RTW_DBGDUMP, ie, ielen);
2421
2422 RTW_INFO("ie:%p, ielen:%u\n", ie, ielen);
2423 RTW_INFO("target_attr:%p, target_attr_len:%u\n", target_attr, target_attr_len);
2424 RTW_INFO("next_attr:%p, remain_len:%u\n", next_attr, remain_len);
2425 }
2426
2427 _rtw_memmove(target_attr, next_attr, remain_len);
2428 _rtw_memset(target_attr + remain_len, 0, target_attr_len);
2429 *(ie + 1) -= target_attr_len;
2430 ielen -= target_attr_len;
2431
2432 if (DBG_DEL_P2P_ATTR) {
2433 RTW_INFO("%s %d after\n", __func__, index);
2434 dump_ies(RTW_DBGDUMP, ie, ielen);
2435 }
2436
2437 index++;
2438 } else
2439 break;
2440 }
2441
2442 return ielen;
2443 }
2444
rtw_bss_ex_get_p2p_ie(WLAN_BSSID_EX * bss_ex,u8 * p2p_ie,uint * p2p_ielen)2445 inline u8 *rtw_bss_ex_get_p2p_ie(WLAN_BSSID_EX *bss_ex, u8 *p2p_ie, uint *p2p_ielen)
2446 {
2447 return rtw_get_p2p_ie(BSS_EX_TLV_IES(bss_ex), BSS_EX_TLV_IES_LEN(bss_ex), p2p_ie, p2p_ielen);
2448 }
2449
rtw_bss_ex_del_p2p_ie(WLAN_BSSID_EX * bss_ex)2450 void rtw_bss_ex_del_p2p_ie(WLAN_BSSID_EX *bss_ex)
2451 {
2452 #define DBG_BSS_EX_DEL_P2P_IE 0
2453
2454 u8 *ies = BSS_EX_TLV_IES(bss_ex);
2455 uint ies_len_ori = BSS_EX_TLV_IES_LEN(bss_ex);
2456 uint ies_len;
2457
2458 ies_len = rtw_del_p2p_ie(ies, ies_len_ori, DBG_BSS_EX_DEL_P2P_IE ? __func__ : NULL);
2459 bss_ex->IELength -= ies_len_ori - ies_len;
2460 }
2461
rtw_bss_ex_del_p2p_attr(WLAN_BSSID_EX * bss_ex,u8 attr_id)2462 void rtw_bss_ex_del_p2p_attr(WLAN_BSSID_EX *bss_ex, u8 attr_id)
2463 {
2464 #define DBG_BSS_EX_DEL_P2P_ATTR 0
2465
2466 u8 *ies = BSS_EX_TLV_IES(bss_ex);
2467 uint ies_len = BSS_EX_TLV_IES_LEN(bss_ex);
2468
2469 u8 *ie;
2470 uint ie_len, ie_len_ori;
2471
2472 int index = 0;
2473
2474 while (1) {
2475 ie = rtw_get_p2p_ie(ies, ies_len, NULL, &ie_len_ori);
2476 if (ie) {
2477 u8 *next_ie_ori = ie + ie_len_ori;
2478 uint remain_len = bss_ex->IELength - (next_ie_ori - bss_ex->IEs);
2479 u8 has_target_attr = 0;
2480
2481 if (DBG_BSS_EX_DEL_P2P_ATTR) {
2482 if (rtw_get_p2p_attr(ie, ie_len_ori, attr_id, NULL, NULL)) {
2483 RTW_INFO("%s %d before\n", __func__, index);
2484 dump_ies(RTW_DBGDUMP, BSS_EX_TLV_IES(bss_ex), BSS_EX_TLV_IES_LEN(bss_ex));
2485
2486 RTW_INFO("ies:%p, ies_len:%u\n", ies, ies_len);
2487 RTW_INFO("ie:%p, ie_len_ori:%u\n", ie, ie_len_ori);
2488 RTW_INFO("next_ie_ori:%p, remain_len:%u\n", next_ie_ori, remain_len);
2489 has_target_attr = 1;
2490 }
2491 }
2492
2493 ie_len = rtw_del_p2p_attr(ie, ie_len_ori, attr_id);
2494 if (ie_len != ie_len_ori) {
2495 u8 *next_ie = ie + ie_len;
2496
2497 _rtw_memmove(next_ie, next_ie_ori, remain_len);
2498 _rtw_memset(next_ie + remain_len, 0, ie_len_ori - ie_len);
2499 bss_ex->IELength -= ie_len_ori - ie_len;
2500
2501 ies = next_ie;
2502 } else
2503 ies = next_ie_ori;
2504
2505 if (DBG_BSS_EX_DEL_P2P_ATTR) {
2506 if (has_target_attr) {
2507 RTW_INFO("%s %d after\n", __func__, index);
2508 dump_ies(RTW_DBGDUMP, BSS_EX_TLV_IES(bss_ex), BSS_EX_TLV_IES_LEN(bss_ex));
2509 }
2510 }
2511
2512 ies_len = remain_len;
2513
2514 index++;
2515 } else
2516 break;
2517 }
2518 }
2519 #endif /* CONFIG_P2P */
2520
2521 /**
2522 * rtw_get_wfd_ie - Search WFD IE from a series of IEs
2523 * @in_ie: Address of IEs to search
2524 * @in_len: Length limit from in_ie
2525 * @wfd_ie: If not NULL and WFD IE is found, WFD IE will be copied to the buf starting from wfd_ie
2526 * @wfd_ielen: If not NULL and WFD IE is found, will set to the length of the entire WFD IE
2527 *
2528 * Returns: The address of the P2P IE found, or NULL
2529 */
rtw_get_wfd_ie(const u8 * in_ie,int in_len,u8 * wfd_ie,uint * wfd_ielen)2530 u8 *rtw_get_wfd_ie(const u8 *in_ie, int in_len, u8 *wfd_ie, uint *wfd_ielen)
2531 {
2532 uint cnt;
2533 const u8 *wfd_ie_ptr = NULL;
2534 u8 eid, wfd_oui[4] = {0x50, 0x6F, 0x9A, 0x0A};
2535
2536 if (wfd_ielen)
2537 *wfd_ielen = 0;
2538
2539 if (!in_ie || in_len < 0) {
2540 rtw_warn_on(1);
2541 return (u8 *)wfd_ie_ptr;
2542 }
2543
2544 if (in_len <= 0)
2545 return (u8 *)wfd_ie_ptr;
2546
2547 cnt = 0;
2548
2549 while (cnt + 1 + 4 < in_len) {
2550 eid = in_ie[cnt];
2551
2552 if (cnt + 1 + 4 >= MAX_IE_SZ) {
2553 rtw_warn_on(1);
2554 return NULL;
2555 }
2556
2557 if (eid == WLAN_EID_VENDOR_SPECIFIC && _rtw_memcmp(&in_ie[cnt + 2], wfd_oui, 4) == _TRUE) {
2558 wfd_ie_ptr = in_ie + cnt;
2559
2560 if (wfd_ie)
2561 _rtw_memcpy(wfd_ie, &in_ie[cnt], in_ie[cnt + 1] + 2);
2562
2563 if (wfd_ielen)
2564 *wfd_ielen = in_ie[cnt + 1] + 2;
2565
2566 break;
2567 } else
2568 cnt += in_ie[cnt + 1] + 2;
2569
2570 }
2571
2572 return (u8 *)wfd_ie_ptr;
2573 }
2574
rtw_del_wfd_ie(u8 * ies,uint ies_len_ori,const char * msg)2575 uint rtw_del_wfd_ie(u8 *ies, uint ies_len_ori, const char *msg)
2576 {
2577 #define DBG_DEL_WFD_IE 0
2578
2579 u8 *target_ie;
2580 u32 target_ie_len;
2581 uint ies_len = ies_len_ori;
2582 int index = 0;
2583
2584 while (1) {
2585 target_ie = rtw_get_wfd_ie(ies, ies_len, NULL, &target_ie_len);
2586 if (target_ie && target_ie_len) {
2587 u8 *next_ie = target_ie + target_ie_len;
2588 uint remain_len = ies_len - (next_ie - ies);
2589
2590 if (DBG_DEL_WFD_IE && msg) {
2591 RTW_INFO("%s %d before\n", __func__, index);
2592 dump_ies(RTW_DBGDUMP, ies, ies_len);
2593
2594 RTW_INFO("ies:%p, ies_len:%u\n", ies, ies_len);
2595 RTW_INFO("target_ie:%p, target_ie_len:%u\n", target_ie, target_ie_len);
2596 RTW_INFO("next_ie:%p, remain_len:%u\n", next_ie, remain_len);
2597 }
2598
2599 _rtw_memmove(target_ie, next_ie, remain_len);
2600 _rtw_memset(target_ie + remain_len, 0, target_ie_len);
2601 ies_len -= target_ie_len;
2602
2603 if (DBG_DEL_WFD_IE && msg) {
2604 RTW_INFO("%s %d after\n", __func__, index);
2605 dump_ies(RTW_DBGDUMP, ies, ies_len);
2606 }
2607
2608 index++;
2609 } else
2610 break;
2611 }
2612
2613 return ies_len;
2614 }
2615
rtw_bss_ex_del_wfd_ie(WLAN_BSSID_EX * bss_ex)2616 void rtw_bss_ex_del_wfd_ie(WLAN_BSSID_EX *bss_ex)
2617 {
2618 #define DBG_BSS_EX_DEL_WFD_IE 0
2619 u8 *ies = BSS_EX_TLV_IES(bss_ex);
2620 uint ies_len_ori = BSS_EX_TLV_IES_LEN(bss_ex);
2621 uint ies_len;
2622
2623 ies_len = rtw_del_wfd_ie(ies, ies_len_ori, DBG_BSS_EX_DEL_WFD_IE ? __func__ : NULL);
2624 bss_ex->IELength -= ies_len_ori - ies_len;
2625 }
2626
2627 #ifdef CONFIG_WFD
dump_wfd_ie(void * sel,const u8 * ie,u32 ie_len)2628 void dump_wfd_ie(void *sel, const u8 *ie, u32 ie_len)
2629 {
2630 const u8 *pos = ie;
2631 u8 id;
2632 u16 len;
2633
2634 const u8 *wfd_ie;
2635 uint wfd_ielen;
2636
2637 wfd_ie = rtw_get_wfd_ie(ie, ie_len, NULL, &wfd_ielen);
2638 if (wfd_ie != ie || wfd_ielen == 0)
2639 return;
2640
2641 pos += 6;
2642 while (pos - ie + 3 <= ie_len) {
2643 id = *pos;
2644 len = RTW_GET_BE16(pos + 1);
2645
2646 RTW_PRINT_SEL(sel, "%s ID:%u, LEN:%u%s\n", __func__, id, len
2647 , ((pos - ie + 3 + len) <= ie_len) ? "" : "(exceed ie_len)");
2648
2649 pos += (3 + len);
2650 }
2651 }
2652
2653 /**
2654 * rtw_get_wfd_attr - Search a specific WFD attribute from a given WFD IE
2655 * @wfd_ie: Address of WFD IE to search
2656 * @wfd_ielen: Length limit from wfd_ie
2657 * @target_attr_id: The attribute ID of WFD attribute to search
2658 * @buf_attr: If not NULL and the WFD attribute is found, WFD attribute will be copied to the buf starting from buf_attr
2659 * @len_attr: If not NULL and the WFD attribute is found, will set to the length of the entire WFD attribute
2660 *
2661 * Returns: the address of the specific WPS attribute found, or NULL
2662 */
rtw_get_wfd_attr(u8 * wfd_ie,uint wfd_ielen,u8 target_attr_id,u8 * buf_attr,u32 * len_attr)2663 u8 *rtw_get_wfd_attr(u8 *wfd_ie, uint wfd_ielen, u8 target_attr_id, u8 *buf_attr, u32 *len_attr)
2664 {
2665 u8 *attr_ptr = NULL;
2666 u8 *target_attr_ptr = NULL;
2667 u8 wfd_oui[4] = {0x50, 0x6F, 0x9A, 0x0A};
2668
2669 if (len_attr)
2670 *len_attr = 0;
2671
2672 if (!wfd_ie
2673 || wfd_ielen <= 6
2674 || (wfd_ie[0] != WLAN_EID_VENDOR_SPECIFIC)
2675 || (_rtw_memcmp(wfd_ie + 2, wfd_oui, 4) != _TRUE))
2676 return attr_ptr;
2677
2678 /* 6 = 1(Element ID) + 1(Length) + 3 (OUI) + 1(OUI Type) */
2679 attr_ptr = wfd_ie + 6; /* goto first attr */
2680
2681 while ((attr_ptr - wfd_ie + 3) <= wfd_ielen) {
2682 /* 3 = 1(Attribute ID) + 2(Length) */
2683 u8 attr_id = *attr_ptr;
2684 u16 attr_data_len = RTW_GET_BE16(attr_ptr + 1);
2685 u16 attr_len = attr_data_len + 3;
2686
2687 if (0)
2688 RTW_INFO("%s attr_ptr:%p, id:%u, length:%u\n", __func__, attr_ptr, attr_id, attr_data_len);
2689
2690 if ((attr_ptr - wfd_ie + attr_len) > wfd_ielen)
2691 break;
2692
2693 if (attr_id == target_attr_id) {
2694 target_attr_ptr = attr_ptr;
2695
2696 if (buf_attr)
2697 _rtw_memcpy(buf_attr, attr_ptr, attr_len);
2698
2699 if (len_attr)
2700 *len_attr = attr_len;
2701
2702 break;
2703 } else
2704 attr_ptr += attr_len;
2705 }
2706
2707 return target_attr_ptr;
2708 }
2709
2710 /**
2711 * rtw_get_wfd_attr_content - Search a specific WFD attribute content from a given WFD IE
2712 * @wfd_ie: Address of WFD IE to search
2713 * @wfd_ielen: Length limit from wfd_ie
2714 * @target_attr_id: The attribute ID of WFD attribute to search
2715 * @buf_content: If not NULL and the WFD attribute is found, WFD attribute content will be copied to the buf starting from buf_content
2716 * @len_content: If not NULL and the WFD attribute is found, will set to the length of the WFD attribute content
2717 *
2718 * Returns: the address of the specific WFD attribute content found, or NULL
2719 */
rtw_get_wfd_attr_content(u8 * wfd_ie,uint wfd_ielen,u8 target_attr_id,u8 * buf_content,uint * len_content)2720 u8 *rtw_get_wfd_attr_content(u8 *wfd_ie, uint wfd_ielen, u8 target_attr_id, u8 *buf_content, uint *len_content)
2721 {
2722 u8 *attr_ptr;
2723 u32 attr_len;
2724
2725 if (len_content)
2726 *len_content = 0;
2727
2728 attr_ptr = rtw_get_wfd_attr(wfd_ie, wfd_ielen, target_attr_id, NULL, &attr_len);
2729
2730 if (attr_ptr && attr_len) {
2731 if (buf_content)
2732 _rtw_memcpy(buf_content, attr_ptr + 3, attr_len - 3);
2733
2734 if (len_content)
2735 *len_content = attr_len - 3;
2736
2737 return attr_ptr + 3;
2738 }
2739
2740 return NULL;
2741 }
2742
rtw_del_wfd_attr(u8 * ie,uint ielen_ori,u8 attr_id)2743 uint rtw_del_wfd_attr(u8 *ie, uint ielen_ori, u8 attr_id)
2744 {
2745 #define DBG_DEL_WFD_ATTR 0
2746
2747 u8 *target_attr;
2748 u32 target_attr_len;
2749 uint ielen = ielen_ori;
2750 int index = 0;
2751
2752 while (1) {
2753 target_attr = rtw_get_wfd_attr(ie, ielen, attr_id, NULL, &target_attr_len);
2754 if (target_attr && target_attr_len) {
2755 u8 *next_attr = target_attr + target_attr_len;
2756 uint remain_len = ielen - (next_attr - ie);
2757
2758 if (DBG_DEL_WFD_ATTR) {
2759 RTW_INFO("%s %d before\n", __func__, index);
2760 dump_ies(RTW_DBGDUMP, ie, ielen);
2761
2762 RTW_INFO("ie:%p, ielen:%u\n", ie, ielen);
2763 RTW_INFO("target_attr:%p, target_attr_len:%u\n", target_attr, target_attr_len);
2764 RTW_INFO("next_attr:%p, remain_len:%u\n", next_attr, remain_len);
2765 }
2766
2767 _rtw_memmove(target_attr, next_attr, remain_len);
2768 _rtw_memset(target_attr + remain_len, 0, target_attr_len);
2769 *(ie + 1) -= target_attr_len;
2770 ielen -= target_attr_len;
2771
2772 if (DBG_DEL_WFD_ATTR) {
2773 RTW_INFO("%s %d after\n", __func__, index);
2774 dump_ies(RTW_DBGDUMP, ie, ielen);
2775 }
2776
2777 index++;
2778 } else
2779 break;
2780 }
2781
2782 return ielen;
2783 }
2784
rtw_bss_ex_get_wfd_ie(WLAN_BSSID_EX * bss_ex,u8 * wfd_ie,uint * wfd_ielen)2785 inline u8 *rtw_bss_ex_get_wfd_ie(WLAN_BSSID_EX *bss_ex, u8 *wfd_ie, uint *wfd_ielen)
2786 {
2787 return rtw_get_wfd_ie(BSS_EX_TLV_IES(bss_ex), BSS_EX_TLV_IES_LEN(bss_ex), wfd_ie, wfd_ielen);
2788 }
2789
rtw_bss_ex_del_wfd_attr(WLAN_BSSID_EX * bss_ex,u8 attr_id)2790 void rtw_bss_ex_del_wfd_attr(WLAN_BSSID_EX *bss_ex, u8 attr_id)
2791 {
2792 #define DBG_BSS_EX_DEL_WFD_ATTR 0
2793
2794 u8 *ies = BSS_EX_TLV_IES(bss_ex);
2795 uint ies_len = BSS_EX_TLV_IES_LEN(bss_ex);
2796
2797 u8 *ie;
2798 uint ie_len, ie_len_ori;
2799
2800 int index = 0;
2801
2802 while (1) {
2803 ie = rtw_get_wfd_ie(ies, ies_len, NULL, &ie_len_ori);
2804 if (ie) {
2805 u8 *next_ie_ori = ie + ie_len_ori;
2806 uint remain_len = bss_ex->IELength - (next_ie_ori - bss_ex->IEs);
2807 u8 has_target_attr = 0;
2808
2809 if (DBG_BSS_EX_DEL_WFD_ATTR) {
2810 if (rtw_get_wfd_attr(ie, ie_len_ori, attr_id, NULL, NULL)) {
2811 RTW_INFO("%s %d before\n", __func__, index);
2812 dump_ies(RTW_DBGDUMP, BSS_EX_TLV_IES(bss_ex), BSS_EX_TLV_IES_LEN(bss_ex));
2813
2814 RTW_INFO("ies:%p, ies_len:%u\n", ies, ies_len);
2815 RTW_INFO("ie:%p, ie_len_ori:%u\n", ie, ie_len_ori);
2816 RTW_INFO("next_ie_ori:%p, remain_len:%u\n", next_ie_ori, remain_len);
2817 has_target_attr = 1;
2818 }
2819 }
2820
2821 ie_len = rtw_del_wfd_attr(ie, ie_len_ori, attr_id);
2822 if (ie_len != ie_len_ori) {
2823 u8 *next_ie = ie + ie_len;
2824
2825 _rtw_memmove(next_ie, next_ie_ori, remain_len);
2826 _rtw_memset(next_ie + remain_len, 0, ie_len_ori - ie_len);
2827 bss_ex->IELength -= ie_len_ori - ie_len;
2828
2829 ies = next_ie;
2830 } else
2831 ies = next_ie_ori;
2832
2833 if (DBG_BSS_EX_DEL_WFD_ATTR) {
2834 if (has_target_attr) {
2835 RTW_INFO("%s %d after\n", __func__, index);
2836 dump_ies(RTW_DBGDUMP, BSS_EX_TLV_IES(bss_ex), BSS_EX_TLV_IES_LEN(bss_ex));
2837 }
2838 }
2839
2840 ies_len = remain_len;
2841
2842 index++;
2843 } else
2844 break;
2845 }
2846 }
2847 #endif /* CONFIG_WFD */
2848
2849 #ifdef CONFIG_RTW_MULTI_AP
dump_multi_ap_ie(void * sel,const u8 * ie,u32 ie_len)2850 void dump_multi_ap_ie(void *sel, const u8 *ie, u32 ie_len)
2851 {
2852 const u8 *pos = ie;
2853 u8 id;
2854 u8 len;
2855
2856 const u8 *multi_ap_ie;
2857 uint multi_ap_ielen;
2858
2859 multi_ap_ie = rtw_get_ie_ex(ie, ie_len, WLAN_EID_VENDOR_SPECIFIC, MULTI_AP_OUI, 4, NULL, &multi_ap_ielen);
2860 if (multi_ap_ie != ie || multi_ap_ielen == 0)
2861 return;
2862
2863 pos += 6;
2864 while (pos - ie + 2 <= ie_len) {
2865 id = *pos;
2866 len = *(pos + 1);
2867
2868 RTW_PRINT_SEL(sel, "%s ID:%u, LEN:%u%s\n", __func__, id, len
2869 , ((pos - ie + 2 + len) <= ie_len) ? "" : "(exceed ie_len)");
2870 RTW_DUMP_SEL(sel, pos + 2, len);
2871
2872 pos += (2 + len);
2873 }
2874 }
2875
2876 /**
2877 * rtw_get_multi_ap_ext - Search Multi-AP IE from a series of IEs and return extension subelement value
2878 * @ies: Address of IEs to search
2879 * @ies_len: Length limit from in_ie
2880 *
2881 * Returns: The address of the target IE found, or NULL
2882 */
rtw_get_multi_ap_ie_ext(const u8 * ies,int ies_len)2883 u8 rtw_get_multi_ap_ie_ext(const u8 *ies, int ies_len)
2884 {
2885 u8 *ie;
2886 uint ielen;
2887 u8 val = 0;
2888
2889 ie = rtw_get_ie_ex(ies, ies_len, WLAN_EID_VENDOR_SPECIFIC, MULTI_AP_OUI, 4, NULL, &ielen);
2890 if (ielen < 9)
2891 goto exit;
2892
2893 if (ie[6] != MULTI_AP_SUB_ELEM_TYPE)
2894 goto exit;
2895
2896 val = ie[8];
2897
2898 exit:
2899 return val;
2900 }
2901
rtw_set_multi_ap_ie_ext(u8 * pbuf,uint * frlen,u8 val)2902 u8 *rtw_set_multi_ap_ie_ext(u8 *pbuf, uint *frlen, u8 val)
2903 {
2904 u8 cont_len = 7;
2905
2906 *pbuf++ = WLAN_EID_VENDOR_SPECIFIC;
2907 *pbuf++ = cont_len;
2908 _rtw_memcpy(pbuf, MULTI_AP_OUI, 4);
2909 pbuf += 4;
2910 *pbuf++ = MULTI_AP_SUB_ELEM_TYPE;
2911 *pbuf++ = 1; /* len */
2912 *pbuf++ = val;
2913
2914 if (frlen)
2915 *frlen = *frlen + (cont_len + 2);
2916
2917 return pbuf;
2918 }
2919 #endif /* CONFIG_RTW_MULTI_AP */
2920
2921 /* Baron adds to avoid FreeBSD warning */
ieee80211_is_empty_essid(const char * essid,int essid_len)2922 int ieee80211_is_empty_essid(const char *essid, int essid_len)
2923 {
2924 /* Single white space is for Linksys APs */
2925 if (essid_len == 1 && essid[0] == ' ')
2926 return 1;
2927
2928 /* Otherwise, if the entire essid is 0, we assume it is hidden */
2929 while (essid_len) {
2930 essid_len--;
2931 if (essid[essid_len] != '\0')
2932 return 0;
2933 }
2934
2935 return 1;
2936 }
2937
ieee80211_get_hdrlen(u16 fc)2938 int ieee80211_get_hdrlen(u16 fc)
2939 {
2940 int hdrlen = 24;
2941
2942 switch (WLAN_FC_GET_TYPE(fc)) {
2943 case RTW_IEEE80211_FTYPE_DATA:
2944 if (fc & RTW_IEEE80211_STYPE_QOS_DATA)
2945 hdrlen += 2;
2946 if ((fc & RTW_IEEE80211_FCTL_FROMDS) && (fc & RTW_IEEE80211_FCTL_TODS))
2947 hdrlen += 6; /* Addr4 */
2948 break;
2949 case RTW_IEEE80211_FTYPE_CTL:
2950 switch (WLAN_FC_GET_STYPE(fc)) {
2951 case RTW_IEEE80211_STYPE_CTS:
2952 case RTW_IEEE80211_STYPE_ACK:
2953 hdrlen = 10;
2954 break;
2955 default:
2956 hdrlen = 16;
2957 break;
2958 }
2959 break;
2960 }
2961
2962 return hdrlen;
2963 }
2964
rtw_ht_mcsset_to_nss(u8 * supp_mcs_set)2965 u8 rtw_ht_mcsset_to_nss(u8 *supp_mcs_set)
2966 {
2967 u8 nss = 1;
2968
2969 if (supp_mcs_set[3])
2970 nss = 4;
2971 else if (supp_mcs_set[2])
2972 nss = 3;
2973 else if (supp_mcs_set[1])
2974 nss = 2;
2975 else if (supp_mcs_set[0])
2976 nss = 1;
2977 else
2978 RTW_INFO("%s,%d, warning! supp_mcs_set is zero\n", __func__, __LINE__);
2979 /* RTW_INFO("%s HT: %dSS\n", __FUNCTION__, nss); */
2980 return nss;
2981 }
2982
rtw_ht_mcs_set_to_bitmap(u8 * mcs_set,u8 nss)2983 u32 rtw_ht_mcs_set_to_bitmap(u8 *mcs_set, u8 nss)
2984 {
2985 u8 i;
2986 u32 bitmap = 0;
2987
2988 for (i = 0; i < nss; i++)
2989 bitmap |= mcs_set[i] << (i * 8);
2990
2991 RTW_INFO("ht_mcs_set=%02x %02x %02x %02x, nss=%u, bitmap=%08x\n"
2992 , mcs_set[0], mcs_set[1], mcs_set[2], mcs_set[3], nss, bitmap);
2993
2994 return bitmap;
2995 }
2996
2997 /* show MCS rate, unit: 100Kbps */
rtw_ht_mcs_rate(u8 bw_40MHz,u8 short_GI,unsigned char * MCS_rate)2998 u16 rtw_ht_mcs_rate(u8 bw_40MHz, u8 short_GI, unsigned char *MCS_rate)
2999 {
3000 u16 max_rate = 0;
3001
3002 if (MCS_rate[3]) {
3003 if (MCS_rate[3] & BIT(7))
3004 max_rate = (bw_40MHz) ? ((short_GI) ? 6000 : 5400) : ((short_GI) ? 2889 : 2600);
3005 else if (MCS_rate[3] & BIT(6))
3006 max_rate = (bw_40MHz) ? ((short_GI) ? 5400 : 4860) : ((short_GI) ? 2600 : 2340);
3007 else if (MCS_rate[3] & BIT(5))
3008 max_rate = (bw_40MHz) ? ((short_GI) ? 4800 : 4320) : ((short_GI) ? 2311 : 2080);
3009 else if (MCS_rate[3] & BIT(4))
3010 max_rate = (bw_40MHz) ? ((short_GI) ? 3600 : 3240) : ((short_GI) ? 1733 : 1560);
3011 else if (MCS_rate[3] & BIT(3))
3012 max_rate = (bw_40MHz) ? ((short_GI) ? 2400 : 2160) : ((short_GI) ? 1156 : 1040);
3013 else if (MCS_rate[3] & BIT(2))
3014 max_rate = (bw_40MHz) ? ((short_GI) ? 1800 : 1620) : ((short_GI) ? 867 : 780);
3015 else if (MCS_rate[3] & BIT(1))
3016 max_rate = (bw_40MHz) ? ((short_GI) ? 1200 : 1080) : ((short_GI) ? 578 : 520);
3017 else if (MCS_rate[3] & BIT(0))
3018 max_rate = (bw_40MHz) ? ((short_GI) ? 600 : 540) : ((short_GI) ? 289 : 260);
3019 } else if (MCS_rate[2]) {
3020 if (MCS_rate[2] & BIT(7))
3021 max_rate = (bw_40MHz) ? ((short_GI) ? 4500 : 4050) : ((short_GI) ? 2167 : 1950);
3022 else if (MCS_rate[2] & BIT(6))
3023 max_rate = (bw_40MHz) ? ((short_GI) ? 4050 : 3645) : ((short_GI) ? 1950 : 1750);
3024 else if (MCS_rate[2] & BIT(5))
3025 max_rate = (bw_40MHz) ? ((short_GI) ? 3600 : 3240) : ((short_GI) ? 1733 : 1560);
3026 else if (MCS_rate[2] & BIT(4))
3027 max_rate = (bw_40MHz) ? ((short_GI) ? 2700 : 2430) : ((short_GI) ? 1300 : 1170);
3028 else if (MCS_rate[2] & BIT(3))
3029 max_rate = (bw_40MHz) ? ((short_GI) ? 1800 : 1620) : ((short_GI) ? 867 : 780);
3030 else if (MCS_rate[2] & BIT(2))
3031 max_rate = (bw_40MHz) ? ((short_GI) ? 1350 : 1215) : ((short_GI) ? 650 : 585);
3032 else if (MCS_rate[2] & BIT(1))
3033 max_rate = (bw_40MHz) ? ((short_GI) ? 900 : 810) : ((short_GI) ? 433 : 390);
3034 else if (MCS_rate[2] & BIT(0))
3035 max_rate = (bw_40MHz) ? ((short_GI) ? 450 : 405) : ((short_GI) ? 217 : 195);
3036 } else if (MCS_rate[1]) {
3037 if (MCS_rate[1] & BIT(7))
3038 max_rate = (bw_40MHz) ? ((short_GI) ? 3000 : 2700) : ((short_GI) ? 1444 : 1300);
3039 else if (MCS_rate[1] & BIT(6))
3040 max_rate = (bw_40MHz) ? ((short_GI) ? 2700 : 2430) : ((short_GI) ? 1300 : 1170);
3041 else if (MCS_rate[1] & BIT(5))
3042 max_rate = (bw_40MHz) ? ((short_GI) ? 2400 : 2160) : ((short_GI) ? 1156 : 1040);
3043 else if (MCS_rate[1] & BIT(4))
3044 max_rate = (bw_40MHz) ? ((short_GI) ? 1800 : 1620) : ((short_GI) ? 867 : 780);
3045 else if (MCS_rate[1] & BIT(3))
3046 max_rate = (bw_40MHz) ? ((short_GI) ? 1200 : 1080) : ((short_GI) ? 578 : 520);
3047 else if (MCS_rate[1] & BIT(2))
3048 max_rate = (bw_40MHz) ? ((short_GI) ? 900 : 810) : ((short_GI) ? 433 : 390);
3049 else if (MCS_rate[1] & BIT(1))
3050 max_rate = (bw_40MHz) ? ((short_GI) ? 600 : 540) : ((short_GI) ? 289 : 260);
3051 else if (MCS_rate[1] & BIT(0))
3052 max_rate = (bw_40MHz) ? ((short_GI) ? 300 : 270) : ((short_GI) ? 144 : 130);
3053 } else {
3054 if (MCS_rate[0] & BIT(7))
3055 max_rate = (bw_40MHz) ? ((short_GI) ? 1500 : 1350) : ((short_GI) ? 722 : 650);
3056 else if (MCS_rate[0] & BIT(6))
3057 max_rate = (bw_40MHz) ? ((short_GI) ? 1350 : 1215) : ((short_GI) ? 650 : 585);
3058 else if (MCS_rate[0] & BIT(5))
3059 max_rate = (bw_40MHz) ? ((short_GI) ? 1200 : 1080) : ((short_GI) ? 578 : 520);
3060 else if (MCS_rate[0] & BIT(4))
3061 max_rate = (bw_40MHz) ? ((short_GI) ? 900 : 810) : ((short_GI) ? 433 : 390);
3062 else if (MCS_rate[0] & BIT(3))
3063 max_rate = (bw_40MHz) ? ((short_GI) ? 600 : 540) : ((short_GI) ? 289 : 260);
3064 else if (MCS_rate[0] & BIT(2))
3065 max_rate = (bw_40MHz) ? ((short_GI) ? 450 : 405) : ((short_GI) ? 217 : 195);
3066 else if (MCS_rate[0] & BIT(1))
3067 max_rate = (bw_40MHz) ? ((short_GI) ? 300 : 270) : ((short_GI) ? 144 : 130);
3068 else if (MCS_rate[0] & BIT(0))
3069 max_rate = (bw_40MHz) ? ((short_GI) ? 150 : 135) : ((short_GI) ? 72 : 65);
3070 }
3071
3072 return max_rate;
3073 }
3074
rtw_action_frame_parse(const u8 * frame,u32 frame_len,u8 * category,u8 * action)3075 int rtw_action_frame_parse(const u8 *frame, u32 frame_len, u8 *category, u8 *action)
3076 {
3077 const u8 *frame_body = frame + sizeof(struct rtw_ieee80211_hdr_3addr);
3078 u16 fc;
3079 u8 c;
3080 u8 a = ACT_PUBLIC_MAX;
3081
3082 fc = le16_to_cpu(((struct rtw_ieee80211_hdr_3addr *)frame)->frame_ctl);
3083
3084 if ((fc & (RTW_IEEE80211_FCTL_FTYPE | RTW_IEEE80211_FCTL_STYPE))
3085 != (RTW_IEEE80211_FTYPE_MGMT | RTW_IEEE80211_STYPE_ACTION)
3086 )
3087 return _FALSE;
3088
3089 c = frame_body[0];
3090
3091 switch (c) {
3092 case RTW_WLAN_CATEGORY_P2P: /* vendor-specific */
3093 break;
3094 default:
3095 a = frame_body[1];
3096 }
3097
3098 if (category)
3099 *category = c;
3100 if (action)
3101 *action = a;
3102
3103 return _TRUE;
3104 }
3105
3106 static const char *_action_public_str[] = {
3107 "ACT_PUB_BSSCOEXIST",
3108 "ACT_PUB_DSE_ENABLE",
3109 "ACT_PUB_DSE_DEENABLE",
3110 "ACT_PUB_DSE_REG_LOCATION",
3111 "ACT_PUB_EXT_CHL_SWITCH",
3112 "ACT_PUB_DSE_MSR_REQ",
3113 "ACT_PUB_DSE_MSR_RPRT",
3114 "ACT_PUB_MP",
3115 "ACT_PUB_DSE_PWR_CONSTRAINT",
3116 "ACT_PUB_VENDOR",
3117 "ACT_PUB_GAS_INITIAL_REQ",
3118 "ACT_PUB_GAS_INITIAL_RSP",
3119 "ACT_PUB_GAS_COMEBACK_REQ",
3120 "ACT_PUB_GAS_COMEBACK_RSP",
3121 "ACT_PUB_TDLS_DISCOVERY_RSP",
3122 "ACT_PUB_LOCATION_TRACK",
3123 "ACT_PUB_RSVD",
3124 };
3125
action_public_str(u8 action)3126 const char *action_public_str(u8 action)
3127 {
3128 action = (action >= ACT_PUBLIC_MAX) ? ACT_PUBLIC_MAX : action;
3129 return _action_public_str[action];
3130 }
3131
3132 #if 0
3133 /*tmp for sta mode, root cause have to wait supplicant's update.*/
3134 void rtw_set_spp_amsdu_mode(u8 mode, u8 *rsn_ie, int rsn_ie_len)
3135 {
3136 struct rsne_info info;
3137 int i, ret = _SUCCESS;
3138 u8 spp_req_cap = 0;
3139
3140 ret = rtw_rsne_info_parse(rsn_ie, rsn_ie_len, &info);
3141 if (ret != _SUCCESS)
3142 return;
3143
3144 if (mode == RTW_AMSDU_MODE_NON_SPP ) {
3145 spp_req_cap = 0; /* SPP_CAP=0, SPP_REQ=0 */
3146 } else if (mode == RTW_AMSDU_MODE_SPP) {
3147 spp_req_cap = SPP_CAP | SPP_REQ;
3148 } else if (mode == RTW_AMSDU_MODE_ALL_DROP) {
3149 spp_req_cap = SPP_REQ; /* SPP_CAP=0, SPP_REQ=1 */
3150 } else {
3151 RTW_INFO("%s unexpected mode = %d, please check the config\n", __func__, mode);
3152 return;
3153 }
3154
3155 SET_RSN_CAP_SPP(info.cap, spp_req_cap);
3156 RTW_INFO("%s set spp opt = %d\n", __func__, GET_RSN_CAP_SPP_OPT(info.cap));
3157 }
3158 #endif
3159
3160 /* Returns:
3161 _TRUE -- Disable AMSDU
3162 _FALSE -- Enable AMSDU
3163 */
rtw_check_amsdu_disable(u8 mode,u8 spp_opt)3164 u8 rtw_check_amsdu_disable(u8 mode, u8 spp_opt)
3165 {
3166 u8 ret = _FALSE;
3167
3168 /* pp amsdu: peer's required has to be 0, or disable */
3169 if ((mode == RTW_AMSDU_MODE_NON_SPP) && (spp_opt & SPP_REQ))
3170 ret = _TRUE;
3171 /* spp amsdu: peer's cap has to be 1, or disable */
3172 else if ((mode == RTW_AMSDU_MODE_SPP) && (!(spp_opt & SPP_CAP)))
3173 ret = _TRUE;
3174 /* mode = all drop */
3175 else if (mode == RTW_AMSDU_MODE_ALL_DROP)
3176 ret = _TRUE;
3177 else
3178 ret = _FALSE;
3179 return ret;
3180 }
3181
3182