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