1 // SPDX-License-Identifier: GPL-2.0-only
2 /*
3 * nxpwifi: management element handling - set/delete elements.
4 *
5 * Copyright 2011-2024 NXP
6 */
7
8 #include "main.h"
9 #include "cmdevt.h"
10
11 /* Return true if the IE index is used by another interface. */
12 static bool
nxpwifi_ie_index_used_by_other_intf(struct nxpwifi_private * priv,u16 idx)13 nxpwifi_ie_index_used_by_other_intf(struct nxpwifi_private *priv, u16 idx)
14 {
15 int i;
16 struct nxpwifi_adapter *adapter = priv->adapter;
17 struct nxpwifi_ie *ie;
18
19 for (i = 0; i < adapter->priv_num; i++) {
20 if (adapter->priv[i] != priv) {
21 ie = &adapter->priv[i]->mgmt_ie[idx];
22 if (ie->mgmt_subtype_mask && ie->ie_length)
23 return true;
24 }
25 }
26
27 return false;
28 }
29
30 /* Pick an unused IE index for a new element. */
31 static int
nxpwifi_ie_get_autoidx(struct nxpwifi_private * priv,u16 subtype_mask,struct nxpwifi_ie * ie,u16 * index)32 nxpwifi_ie_get_autoidx(struct nxpwifi_private *priv, u16 subtype_mask,
33 struct nxpwifi_ie *ie, u16 *index)
34 {
35 u16 mask, len, i;
36
37 for (i = 0; i < priv->adapter->max_mgmt_ie_index; i++) {
38 mask = le16_to_cpu(priv->mgmt_ie[i].mgmt_subtype_mask);
39 len = le16_to_cpu(ie->ie_length);
40
41 if (mask == NXPWIFI_AUTO_IDX_MASK)
42 continue;
43
44 if (mask == subtype_mask) {
45 if (len > IEEE_MAX_IE_SIZE)
46 continue;
47
48 *index = i;
49 return 0;
50 }
51
52 if (!priv->mgmt_ie[i].ie_length) {
53 if (nxpwifi_ie_index_used_by_other_intf(priv, i))
54 continue;
55
56 *index = i;
57 return 0;
58 }
59 }
60
61 return -ENOENT;
62 }
63
64 /* Build IE list and resolve AUTO index before sending to FW. */
65 static int
nxpwifi_update_autoindex_ies(struct nxpwifi_private * priv,struct nxpwifi_ie_list * ie_list)66 nxpwifi_update_autoindex_ies(struct nxpwifi_private *priv,
67 struct nxpwifi_ie_list *ie_list)
68 {
69 u16 travel_len, index, mask;
70 s16 input_len, tlv_len;
71 struct nxpwifi_ie *ie;
72 u8 *tmp;
73
74 input_len = le16_to_cpu(ie_list->len);
75 travel_len = sizeof(struct nxpwifi_ie_types_header);
76
77 ie_list->len = 0;
78
79 while (input_len >= sizeof(struct nxpwifi_ie_types_header)) {
80 ie = (struct nxpwifi_ie *)(((u8 *)ie_list) + travel_len);
81 tlv_len = le16_to_cpu(ie->ie_length);
82 travel_len += tlv_len + NXPWIFI_IE_HDR_SIZE;
83
84 if (input_len < tlv_len + NXPWIFI_IE_HDR_SIZE)
85 return -EINVAL;
86 index = le16_to_cpu(ie->ie_index);
87 mask = le16_to_cpu(ie->mgmt_subtype_mask);
88
89 if (index == NXPWIFI_AUTO_IDX_MASK) {
90 /* automatic addition */
91 if (nxpwifi_ie_get_autoidx(priv, mask, ie, &index))
92 return -ENOENT;
93 if (index == NXPWIFI_AUTO_IDX_MASK)
94 return -EINVAL;
95
96 tmp = (u8 *)&priv->mgmt_ie[index].ie_buffer;
97 memcpy(tmp, &ie->ie_buffer, le16_to_cpu(ie->ie_length));
98 priv->mgmt_ie[index].ie_length = ie->ie_length;
99 priv->mgmt_ie[index].ie_index = cpu_to_le16(index);
100 priv->mgmt_ie[index].mgmt_subtype_mask =
101 cpu_to_le16(mask);
102
103 ie->ie_index = cpu_to_le16(index);
104 } else {
105 if (mask != NXPWIFI_DELETE_MASK)
106 return -EINVAL;
107 /*
108 * Check if this index is being used on any
109 * other interface.
110 */
111 if (nxpwifi_ie_index_used_by_other_intf(priv, index))
112 return -EPERM;
113
114 ie->ie_length = 0;
115 memcpy(&priv->mgmt_ie[index], ie,
116 sizeof(struct nxpwifi_ie));
117 }
118
119 le16_unaligned_add_cpu
120 (&ie_list->len,
121 le16_to_cpu(priv->mgmt_ie[index].ie_length) +
122 NXPWIFI_IE_HDR_SIZE);
123 input_len -= tlv_len + NXPWIFI_IE_HDR_SIZE;
124 }
125
126 if (GET_BSS_ROLE(priv) == NXPWIFI_BSS_ROLE_UAP)
127 return nxpwifi_send_cmd(priv, HOST_CMD_UAP_SYS_CONFIG,
128 HOST_ACT_GEN_SET,
129 UAP_CUSTOM_IE_I, ie_list, true);
130
131 return 0;
132 }
133
134 /* Pack beacon/probe/assoc IEs into one list and update auto-assigned indices. */
135 static int
nxpwifi_update_uap_custom_ie(struct nxpwifi_private * priv,struct nxpwifi_ie * beacon_ie,u16 * beacon_idx,struct nxpwifi_ie * pr_ie,u16 * probe_idx,struct nxpwifi_ie * ar_ie,u16 * assoc_idx)136 nxpwifi_update_uap_custom_ie(struct nxpwifi_private *priv,
137 struct nxpwifi_ie *beacon_ie, u16 *beacon_idx,
138 struct nxpwifi_ie *pr_ie, u16 *probe_idx,
139 struct nxpwifi_ie *ar_ie, u16 *assoc_idx)
140 {
141 struct nxpwifi_ie_list *ap_custom_ie;
142 u8 *pos;
143 u16 len;
144 int ret;
145
146 ap_custom_ie = kzalloc_obj(*ap_custom_ie, GFP_KERNEL);
147 if (!ap_custom_ie)
148 return -ENOMEM;
149
150 ap_custom_ie->type = cpu_to_le16(TLV_TYPE_MGMT_IE);
151 pos = (u8 *)ap_custom_ie->ie_list;
152
153 if (beacon_ie) {
154 len = sizeof(struct nxpwifi_ie) - IEEE_MAX_IE_SIZE +
155 le16_to_cpu(beacon_ie->ie_length);
156 memcpy(pos, beacon_ie, len);
157 pos += len;
158 le16_unaligned_add_cpu(&ap_custom_ie->len, len);
159 }
160 if (pr_ie) {
161 len = sizeof(struct nxpwifi_ie) - IEEE_MAX_IE_SIZE +
162 le16_to_cpu(pr_ie->ie_length);
163 memcpy(pos, pr_ie, len);
164 pos += len;
165 le16_unaligned_add_cpu(&ap_custom_ie->len, len);
166 }
167 if (ar_ie) {
168 len = sizeof(struct nxpwifi_ie) - IEEE_MAX_IE_SIZE +
169 le16_to_cpu(ar_ie->ie_length);
170 memcpy(pos, ar_ie, len);
171 pos += len;
172 le16_unaligned_add_cpu(&ap_custom_ie->len, len);
173 }
174
175 ret = nxpwifi_update_autoindex_ies(priv, ap_custom_ie);
176
177 pos = (u8 *)(&ap_custom_ie->ie_list[0].ie_index);
178 if (beacon_ie && *beacon_idx == NXPWIFI_AUTO_IDX_MASK) {
179 /* save beacon element index after auto-indexing */
180 *beacon_idx = le16_to_cpu(ap_custom_ie->ie_list[0].ie_index);
181 len = sizeof(*beacon_ie) - IEEE_MAX_IE_SIZE +
182 le16_to_cpu(beacon_ie->ie_length);
183 pos += len;
184 }
185 if (pr_ie && le16_to_cpu(pr_ie->ie_index) == NXPWIFI_AUTO_IDX_MASK) {
186 /* save probe resp element index after auto-indexing */
187 *probe_idx = *((u16 *)pos);
188 len = sizeof(*pr_ie) - IEEE_MAX_IE_SIZE +
189 le16_to_cpu(pr_ie->ie_length);
190 pos += len;
191 }
192 if (ar_ie && le16_to_cpu(ar_ie->ie_index) == NXPWIFI_AUTO_IDX_MASK)
193 /* save assoc resp element index after auto-indexing */
194 *assoc_idx = *((u16 *)pos);
195
196 kfree(ap_custom_ie);
197 return ret;
198 }
199
200 /* Append vendor IE (if present) into nxpwifi_ie, allocating as needed. */
nxpwifi_update_vs_ie(const u8 * ies,int ies_len,struct nxpwifi_ie ** ie_ptr,u16 mask,unsigned int oui,u8 oui_type)201 static int nxpwifi_update_vs_ie(const u8 *ies, int ies_len,
202 struct nxpwifi_ie **ie_ptr, u16 mask,
203 unsigned int oui, u8 oui_type)
204 {
205 struct element *vs_ie;
206 struct nxpwifi_ie *ie = *ie_ptr;
207 const u8 *vendor_ie;
208
209 vendor_ie = cfg80211_find_vendor_ie(oui, oui_type, ies, ies_len);
210 if (vendor_ie) {
211 if (!*ie_ptr) {
212 *ie_ptr = kzalloc_obj(struct nxpwifi_ie, GFP_KERNEL);
213 if (!*ie_ptr)
214 return -ENOMEM;
215 ie = *ie_ptr;
216 }
217
218 vs_ie = (struct element *)vendor_ie;
219 if (le16_to_cpu(ie->ie_length) + vs_ie->datalen + 2 >
220 IEEE_MAX_IE_SIZE)
221 return -EINVAL;
222 memcpy(ie->ie_buffer + le16_to_cpu(ie->ie_length),
223 vs_ie, vs_ie->datalen + 2);
224 le16_unaligned_add_cpu(&ie->ie_length, vs_ie->datalen + 2);
225 ie->mgmt_subtype_mask = cpu_to_le16(mask);
226 ie->ie_index = cpu_to_le16(NXPWIFI_AUTO_IDX_MASK);
227 }
228
229 *ie_ptr = ie;
230 return 0;
231 }
232
233 /* Parse beacon/probe/assoc IEs from cfg80211 and push them to FW. */
nxpwifi_set_mgmt_beacon_data_ies(struct nxpwifi_private * priv,struct cfg80211_beacon_data * data)234 static int nxpwifi_set_mgmt_beacon_data_ies(struct nxpwifi_private *priv,
235 struct cfg80211_beacon_data *data)
236 {
237 struct nxpwifi_ie *beacon_ie = NULL, *pr_ie = NULL, *ar_ie = NULL;
238 u16 beacon_idx = NXPWIFI_AUTO_IDX_MASK, pr_idx = NXPWIFI_AUTO_IDX_MASK;
239 u16 ar_idx = NXPWIFI_AUTO_IDX_MASK;
240 int ret = 0;
241
242 if (data->beacon_ies && data->beacon_ies_len) {
243 nxpwifi_update_vs_ie(data->beacon_ies, data->beacon_ies_len,
244 &beacon_ie, MGMT_MASK_BEACON,
245 WLAN_OUI_MICROSOFT,
246 WLAN_OUI_TYPE_MICROSOFT_WPS);
247 nxpwifi_update_vs_ie(data->beacon_ies, data->beacon_ies_len,
248 &beacon_ie, MGMT_MASK_BEACON,
249 WLAN_OUI_WFA, WLAN_OUI_TYPE_WFA_P2P);
250 }
251
252 if (data->proberesp_ies && data->proberesp_ies_len) {
253 nxpwifi_update_vs_ie(data->proberesp_ies,
254 data->proberesp_ies_len, &pr_ie,
255 MGMT_MASK_PROBE_RESP, WLAN_OUI_MICROSOFT,
256 WLAN_OUI_TYPE_MICROSOFT_WPS);
257 nxpwifi_update_vs_ie(data->proberesp_ies,
258 data->proberesp_ies_len, &pr_ie,
259 MGMT_MASK_PROBE_RESP,
260 WLAN_OUI_WFA, WLAN_OUI_TYPE_WFA_P2P);
261 }
262
263 if (data->assocresp_ies && data->assocresp_ies_len) {
264 nxpwifi_update_vs_ie(data->assocresp_ies,
265 data->assocresp_ies_len, &ar_ie,
266 MGMT_MASK_ASSOC_RESP |
267 MGMT_MASK_REASSOC_RESP,
268 WLAN_OUI_MICROSOFT,
269 WLAN_OUI_TYPE_MICROSOFT_WPS);
270 nxpwifi_update_vs_ie(data->assocresp_ies,
271 data->assocresp_ies_len, &ar_ie,
272 MGMT_MASK_ASSOC_RESP |
273 MGMT_MASK_REASSOC_RESP, WLAN_OUI_WFA,
274 WLAN_OUI_TYPE_WFA_P2P);
275 }
276
277 if (beacon_ie || pr_ie || ar_ie) {
278 ret = nxpwifi_update_uap_custom_ie(priv, beacon_ie,
279 &beacon_idx, pr_ie,
280 &pr_idx, ar_ie, &ar_idx);
281 if (ret)
282 goto done;
283 }
284
285 priv->beacon_idx = beacon_idx;
286 priv->proberesp_idx = pr_idx;
287 priv->assocresp_idx = ar_idx;
288
289 done:
290 kfree(beacon_ie);
291 kfree(pr_ie);
292 kfree(ar_ie);
293
294 return ret;
295 }
296
297 /* Parse head/tail IEs from cfg80211_beacon_data and send them to FW. */
nxpwifi_uap_parse_tail_ies(struct nxpwifi_private * priv,struct cfg80211_beacon_data * info)298 static int nxpwifi_uap_parse_tail_ies(struct nxpwifi_private *priv,
299 struct cfg80211_beacon_data *info)
300 {
301 struct nxpwifi_ie *gen_ie;
302 struct element *hdr;
303 struct ieee80211_vendor_ie *vendorhdr;
304 u16 gen_idx = NXPWIFI_AUTO_IDX_MASK, ie_len = 0;
305 int left_len, parsed_len = 0;
306 unsigned int token_len;
307 int ret = 0;
308
309 if (!info->tail || !info->tail_len)
310 return 0;
311
312 gen_ie = kzalloc_obj(*gen_ie, GFP_KERNEL);
313 if (!gen_ie)
314 return -ENOMEM;
315
316 left_len = info->tail_len;
317
318 /* Skip IEs generated by FW from bss configuration to avoid duplicates. */
319 while (left_len > sizeof(struct element)) {
320 hdr = (void *)(info->tail + parsed_len);
321 token_len = hdr->datalen + sizeof(struct element);
322 if (token_len > left_len) {
323 ret = -EINVAL;
324 goto done;
325 }
326
327 switch (hdr->id) {
328 case WLAN_EID_SSID:
329 case WLAN_EID_SUPP_RATES:
330 case WLAN_EID_COUNTRY:
331 case WLAN_EID_PWR_CONSTRAINT:
332 case WLAN_EID_ERP_INFO:
333 case WLAN_EID_EXT_SUPP_RATES:
334 case WLAN_EID_HT_CAPABILITY:
335 case WLAN_EID_HT_OPERATION:
336 case WLAN_EID_VHT_CAPABILITY:
337 break;
338 case WLAN_EID_VENDOR_SPECIFIC:
339 /* Skip only Microsoft WMM element */
340 if (cfg80211_find_vendor_ie(WLAN_OUI_MICROSOFT,
341 WLAN_OUI_TYPE_MICROSOFT_WMM,
342 (const u8 *)hdr,
343 token_len))
344 break;
345 fallthrough;
346 default:
347 if (ie_len + token_len > IEEE_MAX_IE_SIZE) {
348 ret = -EINVAL;
349 goto done;
350 }
351 memcpy(gen_ie->ie_buffer + ie_len, hdr, token_len);
352 ie_len += token_len;
353 break;
354 }
355 left_len -= token_len;
356 parsed_len += token_len;
357 }
358
359 /*
360 * parse only WPA vendor element from tail, WMM element is configured by
361 * bss_config command
362 */
363 vendorhdr = (void *)cfg80211_find_vendor_ie(WLAN_OUI_MICROSOFT,
364 WLAN_OUI_TYPE_MICROSOFT_WPA,
365 info->tail, info->tail_len);
366 if (vendorhdr) {
367 token_len = vendorhdr->len + sizeof(struct element);
368 if (ie_len + token_len > IEEE_MAX_IE_SIZE) {
369 ret = -EINVAL;
370 goto done;
371 }
372 memcpy(gen_ie->ie_buffer + ie_len, vendorhdr, token_len);
373 ie_len += token_len;
374 }
375
376 if (!ie_len)
377 goto done;
378
379 gen_ie->ie_index = cpu_to_le16(gen_idx);
380 gen_ie->mgmt_subtype_mask = cpu_to_le16(MGMT_MASK_BEACON |
381 MGMT_MASK_PROBE_RESP |
382 MGMT_MASK_ASSOC_RESP);
383 gen_ie->ie_length = cpu_to_le16(ie_len);
384
385 ret = nxpwifi_update_uap_custom_ie(priv, gen_ie, &gen_idx, NULL,
386 NULL, NULL, NULL);
387
388 if (ret)
389 goto done;
390
391 priv->gen_idx = gen_idx;
392
393 done:
394 kfree(gen_ie);
395 return ret;
396 }
397
398 /* Parse head/tail/beacon/probe/assoc IEs and program the FW. */
nxpwifi_set_mgmt_ies(struct nxpwifi_private * priv,struct cfg80211_beacon_data * info)399 int nxpwifi_set_mgmt_ies(struct nxpwifi_private *priv,
400 struct cfg80211_beacon_data *info)
401 {
402 int ret;
403
404 ret = nxpwifi_uap_parse_tail_ies(priv, info);
405
406 if (ret)
407 return ret;
408
409 return nxpwifi_set_mgmt_beacon_data_ies(priv, info);
410 }
411
412 /* Remove previously set management IEs. */
nxpwifi_del_mgmt_ies(struct nxpwifi_private * priv)413 int nxpwifi_del_mgmt_ies(struct nxpwifi_private *priv)
414 {
415 struct nxpwifi_ie *beacon_ie = NULL, *pr_ie = NULL;
416 struct nxpwifi_ie *ar_ie = NULL, *gen_ie = NULL;
417 int ret = 0;
418
419 if (priv->gen_idx != NXPWIFI_AUTO_IDX_MASK) {
420 gen_ie = kmalloc_obj(*gen_ie, GFP_KERNEL);
421 if (!gen_ie)
422 return -ENOMEM;
423
424 gen_ie->ie_index = cpu_to_le16(priv->gen_idx);
425 gen_ie->mgmt_subtype_mask = cpu_to_le16(NXPWIFI_DELETE_MASK);
426 gen_ie->ie_length = 0;
427 ret = nxpwifi_update_uap_custom_ie(priv, gen_ie, &priv->gen_idx,
428 NULL, &priv->proberesp_idx,
429 NULL, &priv->assocresp_idx);
430 if (ret)
431 goto done;
432
433 priv->gen_idx = NXPWIFI_AUTO_IDX_MASK;
434 }
435
436 if (priv->beacon_idx != NXPWIFI_AUTO_IDX_MASK) {
437 beacon_ie = kmalloc_obj(*beacon_ie, GFP_KERNEL);
438 if (!beacon_ie) {
439 ret = -ENOMEM;
440 goto done;
441 }
442 beacon_ie->ie_index = cpu_to_le16(priv->beacon_idx);
443 beacon_ie->mgmt_subtype_mask = cpu_to_le16(NXPWIFI_DELETE_MASK);
444 beacon_ie->ie_length = 0;
445 }
446 if (priv->proberesp_idx != NXPWIFI_AUTO_IDX_MASK) {
447 pr_ie = kmalloc_obj(*pr_ie, GFP_KERNEL);
448 if (!pr_ie) {
449 ret = -ENOMEM;
450 goto done;
451 }
452 pr_ie->ie_index = cpu_to_le16(priv->proberesp_idx);
453 pr_ie->mgmt_subtype_mask = cpu_to_le16(NXPWIFI_DELETE_MASK);
454 pr_ie->ie_length = 0;
455 }
456 if (priv->assocresp_idx != NXPWIFI_AUTO_IDX_MASK) {
457 ar_ie = kmalloc_obj(*ar_ie, GFP_KERNEL);
458 if (!ar_ie) {
459 ret = -ENOMEM;
460 goto done;
461 }
462 ar_ie->ie_index = cpu_to_le16(priv->assocresp_idx);
463 ar_ie->mgmt_subtype_mask = cpu_to_le16(NXPWIFI_DELETE_MASK);
464 ar_ie->ie_length = 0;
465 }
466
467 if (beacon_ie || pr_ie || ar_ie)
468 ret = nxpwifi_update_uap_custom_ie(priv,
469 beacon_ie, &priv->beacon_idx,
470 pr_ie, &priv->proberesp_idx,
471 ar_ie, &priv->assocresp_idx);
472
473 done:
474 kfree(gen_ie);
475 kfree(beacon_ie);
476 kfree(pr_ie);
477 kfree(ar_ie);
478
479 return ret;
480 }
481