xref: /wlan-driver/qca-wifi-host-cmn/umac/mlo_mgr/src/utils_mlo.c (revision 5113495b16420b49004c444715d2daae2066e7dc)
1 /*
2  * Copyright (c) 2021, The Linux Foundation. All rights reserved.
3  * Copyright (c) 2021-2024 Qualcomm Innovation Center, Inc. All rights reserved.
4  *
5  * Permission to use, copy, modify, and/or distribute this software for any
6  * purpose with or without fee is hereby granted, provided that the above
7  * copyright notice and this permission notice appear in all copies.
8  *
9  * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
10  * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
11  * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
12  * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
13  * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
14  * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
15  * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
16  */
17 
18 /*
19  * DOC: contains MLO manager util api's
20  */
21 #include <wlan_cmn.h>
22 #include <wlan_mlo_mgr_sta.h>
23 #include <wlan_cm_public_struct.h>
24 #include <wlan_mlo_mgr_main.h>
25 #include <wlan_cm_api.h>
26 #include "wlan_scan_api.h"
27 #include "qdf_types.h"
28 #include "utils_mlo.h"
29 #include "wlan_mlo_mgr_cmn.h"
30 #include "wlan_utility.h"
31 
32 #ifdef WLAN_FEATURE_11BE_MLO
33 
util_find_eid(uint8_t eid,uint8_t * frame,qdf_size_t len)34 static uint8_t *util_find_eid(uint8_t eid, uint8_t *frame, qdf_size_t len)
35 {
36 	if (!frame)
37 		return NULL;
38 
39 	while (len >= MIN_IE_LEN && len >= frame[TAG_LEN_POS] + MIN_IE_LEN) {
40 		if (frame[ID_POS] == eid)
41 			return frame;
42 
43 		len -= frame[TAG_LEN_POS] + MIN_IE_LEN;
44 		frame += frame[TAG_LEN_POS] + MIN_IE_LEN;
45 	}
46 
47 	return NULL;
48 }
49 
50 static
util_find_extn_eid(uint8_t eid,uint8_t extn_eid,uint8_t * frame,qdf_size_t len)51 uint8_t *util_find_extn_eid(uint8_t eid, uint8_t extn_eid,
52 			    uint8_t *frame, qdf_size_t len)
53 {
54 	if (!frame)
55 		return NULL;
56 
57 	while (len > MIN_IE_LEN && len >= frame[TAG_LEN_POS] + MIN_IE_LEN) {
58 		if ((frame[ID_POS] == eid) &&
59 		    (frame[ELEM_ID_EXTN_POS] == extn_eid))
60 			return frame;
61 
62 		len -= frame[TAG_LEN_POS] + MIN_IE_LEN;
63 		frame += frame[TAG_LEN_POS] + MIN_IE_LEN;
64 	}
65 	return NULL;
66 }
67 
68 static QDF_STATUS
util_parse_multi_link_ctrl(uint8_t * mlieseqpayload,qdf_size_t mlieseqpayloadlen,uint8_t ** link_info,qdf_size_t * link_info_len)69 util_parse_multi_link_ctrl(uint8_t *mlieseqpayload,
70 			   qdf_size_t mlieseqpayloadlen,
71 			   uint8_t **link_info,
72 			   qdf_size_t *link_info_len)
73 {
74 	qdf_size_t parsed_payload_len;
75 	uint16_t mlcontrol;
76 	uint16_t presence_bm;
77 	uint16_t cinfo_len = 0;
78 	uint16_t exp_cinfo_len = 0;
79 
80 	/* This helper returns the location(s) and length(s) of (sub)field(s)
81 	 * inferable after parsing the Multi Link element Control field. These
82 	 * location(s) and length(s) is/are in reference to the payload section
83 	 * of the Multi Link element (after defragmentation, if applicable).
84 	 * Here, the payload is the point after the element ID extension of the
85 	 * Multi Link element, and includes the payloads of all subsequent
86 	 * fragments (if any) but not the headers of those fragments.
87 	 *
88 	 * Currently, the helper returns the location and length of the Link
89 	 * Info field in the Multi Link element sequence. Other (sub)field(s)
90 	 * can be added later as required.
91 	 */
92 
93 	if (!mlieseqpayload) {
94 		mlo_err("ML seq payload pointer is NULL");
95 		return QDF_STATUS_E_NULL_VALUE;
96 	}
97 
98 	if (!mlieseqpayloadlen) {
99 		mlo_err("ML seq payload len is 0");
100 		return QDF_STATUS_E_INVAL;
101 	}
102 
103 	if (mlieseqpayloadlen < WLAN_ML_CTRL_SIZE) {
104 		mlo_err_rl("ML seq payload len %zu < ML Control size %u",
105 			   mlieseqpayloadlen, WLAN_ML_CTRL_SIZE);
106 		return QDF_STATUS_E_PROTO;
107 	}
108 
109 	parsed_payload_len = 0;
110 
111 	qdf_mem_copy(&mlcontrol, mlieseqpayload, WLAN_ML_CTRL_SIZE);
112 	mlcontrol = qdf_le16_to_cpu(mlcontrol);
113 	parsed_payload_len += WLAN_ML_CTRL_SIZE;
114 
115 	presence_bm = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_PBM_IDX,
116 				   WLAN_ML_CTRL_PBM_BITS);
117 
118 	if (mlieseqpayloadlen <
119 			(parsed_payload_len + WLAN_ML_BV_CINFO_LENGTH_SIZE)) {
120 		mlo_err_rl("ML seq payload len %zu insufficient for common info length size %u after parsed payload len %zu.",
121 			   mlieseqpayloadlen,
122 			   WLAN_ML_BV_CINFO_LENGTH_SIZE,
123 			   parsed_payload_len);
124 		return QDF_STATUS_E_PROTO;
125 	}
126 
127 	cinfo_len = *(mlieseqpayload + parsed_payload_len);
128 
129 	if (cinfo_len >
130 			(mlieseqpayloadlen - parsed_payload_len)) {
131 		mlo_err_rl("ML seq common info len %u larger than ML seq payload len %zu after parsed payload len %zu.",
132 			   cinfo_len,
133 			   mlieseqpayloadlen,
134 			   parsed_payload_len);
135 		return QDF_STATUS_E_PROTO;
136 	}
137 
138 	parsed_payload_len += WLAN_ML_BV_CINFO_LENGTH_SIZE;
139 
140 	if (mlieseqpayloadlen <
141 			(parsed_payload_len + QDF_MAC_ADDR_SIZE)) {
142 		mlo_err_rl("ML seq payload len %zu insufficient for MAC address size %u after parsed payload len %zu.",
143 			   mlieseqpayloadlen,
144 			   QDF_MAC_ADDR_SIZE,
145 			   parsed_payload_len);
146 		return QDF_STATUS_E_PROTO;
147 	}
148 
149 	parsed_payload_len += QDF_MAC_ADDR_SIZE;
150 
151 	/* Check if Link ID info is present */
152 	if (presence_bm & WLAN_ML_BV_CTRL_PBM_LINKIDINFO_P) {
153 		if (mlieseqpayloadlen <
154 				(parsed_payload_len +
155 				 WLAN_ML_BV_CINFO_LINKIDINFO_SIZE)) {
156 			mlo_err_rl("ML seq payload len %zu insufficient for Link ID info size %u after parsed payload len %zu.",
157 				   mlieseqpayloadlen,
158 				   WLAN_ML_BV_CINFO_LINKIDINFO_SIZE,
159 				   parsed_payload_len);
160 			return QDF_STATUS_E_PROTO;
161 		}
162 
163 		parsed_payload_len += WLAN_ML_BV_CINFO_LINKIDINFO_SIZE;
164 	}
165 
166 	/* Check if BSS parameter change count is present */
167 	if (presence_bm & WLAN_ML_BV_CTRL_PBM_BSSPARAMCHANGECNT_P) {
168 		if (mlieseqpayloadlen <
169 				(parsed_payload_len +
170 				 WLAN_ML_BSSPARAMCHNGCNT_SIZE)) {
171 			mlo_err_rl("ML seq payload len %zu insufficient for BSS parameter change count size %u after parsed payload len %zu.",
172 				   mlieseqpayloadlen,
173 				   WLAN_ML_BSSPARAMCHNGCNT_SIZE,
174 				   parsed_payload_len);
175 			return QDF_STATUS_E_PROTO;
176 		}
177 
178 		parsed_payload_len += WLAN_ML_BSSPARAMCHNGCNT_SIZE;
179 	}
180 
181 	/* Check if Medium Sync Delay Info is present */
182 	if (presence_bm & WLAN_ML_BV_CTRL_PBM_MEDIUMSYNCDELAYINFO_P) {
183 		if (mlieseqpayloadlen <
184 				(parsed_payload_len +
185 				 WLAN_ML_BV_CINFO_MEDMSYNCDELAYINFO_SIZE)) {
186 			mlo_err_rl("ML seq payload len %zu insufficient for Medium Sync Delay Info size %u after parsed payload len %zu.",
187 				   mlieseqpayloadlen,
188 				   WLAN_ML_BV_CINFO_MEDMSYNCDELAYINFO_SIZE,
189 				   parsed_payload_len);
190 			return QDF_STATUS_E_PROTO;
191 		}
192 
193 		parsed_payload_len += WLAN_ML_BV_CINFO_MEDMSYNCDELAYINFO_SIZE;
194 	}
195 
196 	/* Check if EML cap is present */
197 	if (presence_bm & WLAN_ML_BV_CTRL_PBM_EMLCAP_P) {
198 		if (mlieseqpayloadlen <
199 				(parsed_payload_len +
200 				 WLAN_ML_BV_CINFO_EMLCAP_SIZE)) {
201 			mlo_err_rl("ML seq payload len %zu insufficient for EML cap size %u after parsed payload len %zu.",
202 				   mlieseqpayloadlen,
203 				   WLAN_ML_BV_CINFO_EMLCAP_SIZE,
204 				   parsed_payload_len);
205 			return QDF_STATUS_E_PROTO;
206 		}
207 
208 		parsed_payload_len += WLAN_ML_BV_CINFO_EMLCAP_SIZE;
209 	}
210 
211 	/* Check if MLD cap is present */
212 	if (presence_bm & WLAN_ML_BV_CTRL_PBM_MLDCAPANDOP_P) {
213 		if (mlieseqpayloadlen <
214 				(parsed_payload_len +
215 				 WLAN_ML_BV_CINFO_MLDCAPANDOP_SIZE)) {
216 			mlo_err_rl("ML seq payload len %zu insufficient for MLD cap size %u after parsed payload len %zu.",
217 				   mlieseqpayloadlen,
218 				   WLAN_ML_BV_CINFO_MLDCAPANDOP_SIZE,
219 				   parsed_payload_len);
220 			return QDF_STATUS_E_PROTO;
221 		}
222 
223 		parsed_payload_len += WLAN_ML_BV_CINFO_MLDCAPANDOP_SIZE;
224 	}
225 
226 	/* Check if MLD ID is present */
227 	if (presence_bm & WLAN_ML_BV_CTRL_PBM_MLDID_P) {
228 		if (mlieseqpayloadlen <
229 				(parsed_payload_len +
230 				 WLAN_ML_BV_CINFO_MLDID_SIZE)) {
231 			mlo_err_rl("ML seq payload len %zu insufficient for MLD ID size %u after parsed payload len %zu.",
232 				   mlieseqpayloadlen,
233 				   WLAN_ML_BV_CINFO_MLDID_SIZE,
234 				   parsed_payload_len);
235 			return QDF_STATUS_E_PROTO;
236 		}
237 
238 		parsed_payload_len += WLAN_ML_BV_CINFO_MLDID_SIZE;
239 	}
240 
241 	/* Check if Ext MLD CAP OP is present */
242 	if (presence_bm & WLAN_ML_BV_CTRL_PBM_EXT_MLDCAPANDOP_P) {
243 		if (mlieseqpayloadlen <
244 				(parsed_payload_len +
245 				 WLAN_ML_BV_CINFO_EXT_MLDCAPANDOP_SIZE)) {
246 			mlo_err_rl("ML seq payload len %zu insufficient for Ext MLD CAP OP size %u after parsed payload len %zu.",
247 				   mlieseqpayloadlen,
248 				   WLAN_ML_BV_CINFO_EXT_MLDCAPANDOP_SIZE,
249 				   parsed_payload_len);
250 			return QDF_STATUS_E_PROTO;
251 		}
252 
253 		parsed_payload_len += WLAN_ML_BV_CINFO_EXT_MLDCAPANDOP_SIZE;
254 	}
255 
256 	exp_cinfo_len = parsed_payload_len - WLAN_ML_CTRL_SIZE;
257 	if (cinfo_len >= exp_cinfo_len) {
258 		/* If common info length received is greater,
259 		 *  skip through the additional bytes
260 		 */
261 		parsed_payload_len += (cinfo_len - exp_cinfo_len);
262 		mlo_debug("ML seq common info len %u, parsed payload length %zu, expected common info len %u",
263 			  cinfo_len, parsed_payload_len, exp_cinfo_len);
264 	} else {
265 		/* If cinfo_len < exp_cinfo_len return error */
266 		mlo_err_rl("ML seq common info len %u doesn't match with expected common info len %u",
267 			   cinfo_len, exp_cinfo_len);
268 		return QDF_STATUS_E_PROTO;
269 	}
270 
271 	if (link_info_len) {
272 		*link_info_len = mlieseqpayloadlen - parsed_payload_len;
273 		mlo_debug("link_info_len:%zu, parsed_payload_len:%zu",
274 			  *link_info_len, parsed_payload_len);
275 	}
276 
277 	if (mlieseqpayloadlen == parsed_payload_len) {
278 		if (link_info)
279 			*link_info = NULL;
280 		return QDF_STATUS_SUCCESS;
281 	}
282 
283 	if (link_info)
284 		*link_info = mlieseqpayload + parsed_payload_len;
285 
286 	return QDF_STATUS_SUCCESS;
287 }
288 
289 static QDF_STATUS
util_parse_prv_multi_link_ctrl(uint8_t * mlieseqpayload,qdf_size_t mlieseqpayloadlen,uint8_t ** link_info,qdf_size_t * link_info_len)290 util_parse_prv_multi_link_ctrl(uint8_t *mlieseqpayload,
291 			       qdf_size_t mlieseqpayloadlen,
292 			       uint8_t **link_info,
293 			       qdf_size_t *link_info_len)
294 {
295 	qdf_size_t parsed_payload_len;
296 	uint16_t mlcontrol;
297 	uint16_t presence_bm;
298 	uint16_t cinfo_len = 0;
299 	uint16_t exp_cinfo_len = 0;
300 
301 	/* This helper returns the location(s) and length(s) of (sub)field(s)
302 	 * inferable after parsing the Multi Link element Control field. These
303 	 * location(s) and length(s) is/are in reference to the payload section
304 	 * of the Multi Link element (after defragmentation, if applicable).
305 	 * Here, the payload is the point after the element ID extension of the
306 	 * Multi Link element, and includes the payloads of all subsequent
307 	 * fragments (if any) but not the headers of those fragments.
308 	 *
309 	 * Currently, the helper returns the location and length of the Link
310 	 * Info field in the Multi Link element sequence. Other (sub)field(s)
311 	 * can be added later as required.
312 	 */
313 
314 	if (!mlieseqpayload) {
315 		mlo_err("ML seq payload pointer is NULL");
316 		return QDF_STATUS_E_NULL_VALUE;
317 	}
318 
319 	if (!mlieseqpayloadlen) {
320 		mlo_err("ML seq payload len is 0");
321 		return QDF_STATUS_E_INVAL;
322 	}
323 
324 	if (mlieseqpayloadlen < WLAN_ML_CTRL_SIZE) {
325 		mlo_err_rl("ML seq payload len %zu < ML Control size %u",
326 			   mlieseqpayloadlen, WLAN_ML_CTRL_SIZE);
327 		return QDF_STATUS_E_PROTO;
328 	}
329 
330 	parsed_payload_len = 0;
331 
332 	qdf_mem_copy(&mlcontrol, mlieseqpayload, WLAN_ML_CTRL_SIZE);
333 	mlcontrol = qdf_le16_to_cpu(mlcontrol);
334 	parsed_payload_len += WLAN_ML_CTRL_SIZE;
335 
336 	presence_bm = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_PBM_IDX,
337 				   WLAN_ML_CTRL_PBM_BITS);
338 
339 	if (mlieseqpayloadlen <
340 			(parsed_payload_len + WLAN_ML_PRV_CINFO_LENGTH_SIZE)) {
341 		mlo_err_rl("ML seq payload len %zu insufficient for common info length size %u after parsed payload len %zu.",
342 			   mlieseqpayloadlen,
343 			   WLAN_ML_PRV_CINFO_LENGTH_SIZE,
344 			   parsed_payload_len);
345 		return QDF_STATUS_E_PROTO;
346 	}
347 
348 	cinfo_len = *(mlieseqpayload + parsed_payload_len);
349 	parsed_payload_len += WLAN_ML_PRV_CINFO_LENGTH_SIZE;
350 
351 	/* Check if MLD ID is present */
352 	if (presence_bm & WLAN_ML_PRV_CTRL_PBM_MLDID_P) {
353 		if (mlieseqpayloadlen <
354 				(parsed_payload_len +
355 				 WLAN_ML_PRV_CINFO_MLDID_SIZE)) {
356 			mlo_err_rl("ML seq payload len %zu insufficient for MLD ID size %u after parsed payload len %zu.",
357 				   mlieseqpayloadlen,
358 				   WLAN_ML_PRV_CINFO_MLDID_SIZE,
359 				   parsed_payload_len);
360 			return QDF_STATUS_E_PROTO;
361 		}
362 
363 		parsed_payload_len += WLAN_ML_PRV_CINFO_MLDID_SIZE;
364 	}
365 
366 	exp_cinfo_len = parsed_payload_len - WLAN_ML_CTRL_SIZE;
367 	if (cinfo_len != exp_cinfo_len) {
368 		mlo_err_rl("ML seq common info len %u doesn't match with expected common info len %u",
369 			   cinfo_len, exp_cinfo_len);
370 		return QDF_STATUS_E_PROTO;
371 	}
372 
373 	if (link_info_len) {
374 		*link_info_len = mlieseqpayloadlen - parsed_payload_len;
375 		mlo_debug("link_info_len:%zu, parsed_payload_len:%zu",
376 			  *link_info_len, parsed_payload_len);
377 	}
378 
379 	if (mlieseqpayloadlen == parsed_payload_len) {
380 		mlo_debug("No Link Info field present");
381 		if (link_info)
382 			*link_info = NULL;
383 		return QDF_STATUS_SUCCESS;
384 	}
385 
386 	if (link_info)
387 		*link_info = mlieseqpayload + parsed_payload_len;
388 
389 	return QDF_STATUS_SUCCESS;
390 }
391 
392 static QDF_STATUS
util_parse_bvmlie_perstaprofile_stactrl(uint8_t * subelempayload,qdf_size_t subelempayloadlen,uint8_t * linkid,uint16_t * beaconinterval,bool * is_beaconinterval_valid,uint64_t * tsfoffset,bool * is_tsfoffset_valid,bool * is_complete_profile,bool * is_macaddr_valid,struct qdf_mac_addr * macaddr,bool is_staprof_reqd,uint8_t ** staprof,qdf_size_t * staprof_len,struct mlo_nstr_info * nstr_info,bool * is_nstrlp_present)393 util_parse_bvmlie_perstaprofile_stactrl(uint8_t *subelempayload,
394 					qdf_size_t subelempayloadlen,
395 					uint8_t *linkid,
396 					uint16_t *beaconinterval,
397 					bool *is_beaconinterval_valid,
398 					uint64_t *tsfoffset,
399 					bool *is_tsfoffset_valid,
400 					bool *is_complete_profile,
401 					bool *is_macaddr_valid,
402 					struct qdf_mac_addr *macaddr,
403 					bool is_staprof_reqd,
404 					uint8_t **staprof,
405 					qdf_size_t *staprof_len,
406 					struct mlo_nstr_info *nstr_info,
407 					bool *is_nstrlp_present)
408 {
409 	qdf_size_t parsed_payload_len = 0;
410 	uint16_t stacontrol;
411 	uint8_t completeprofile;
412 	uint8_t nstrlppresent;
413 	enum wlan_ml_bv_linfo_perstaprof_stactrl_nstrbmsz nstrbmsz;
414 	qdf_size_t nstrlpoffset = 0;
415 	uint8_t link_id;
416 
417 	/* This helper returns the location(s) and where required, the length(s)
418 	 * of (sub)field(s) inferable after parsing the STA Control field in the
419 	 * per-STA profile subelement. These location(s) and length(s) is/are in
420 	 * reference to the payload section of the per-STA profile subelement
421 	 * (after defragmentation, if applicable).  Here, the payload is the
422 	 * point after the subelement length in the subelement, and includes the
423 	 * payloads of all subsequent fragments (if any) but not the headers of
424 	 * those fragments.
425 	 *
426 	 * Currently, the helper returns the link ID, MAC address, and STA
427 	 * profile. More (sub)fields can be added when required.
428 	 */
429 
430 	if (!subelempayload) {
431 		mlo_err("Pointer to subelement payload is NULL");
432 		return QDF_STATUS_E_NULL_VALUE;
433 	}
434 
435 	if (!subelempayloadlen) {
436 		mlo_err("Length of subelement payload is zero");
437 		return QDF_STATUS_E_INVAL;
438 	}
439 
440 	if (subelempayloadlen < WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_SIZE) {
441 		mlo_err_rl("Subelement payload length %zu octets is smaller than STA control field of per-STA profile subelement %u octets",
442 			   subelempayloadlen,
443 			   WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_SIZE);
444 		return QDF_STATUS_E_PROTO;
445 	}
446 
447 	parsed_payload_len = 0;
448 
449 	qdf_mem_copy(&stacontrol,
450 		     subelempayload,
451 		     WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_SIZE);
452 	stacontrol = le16toh(stacontrol);
453 	parsed_payload_len += WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_SIZE;
454 
455 	link_id = QDF_GET_BITS(stacontrol,
456 			       WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_LINKID_IDX,
457 			       WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_LINKID_BITS);
458 	if (linkid)
459 		*linkid = link_id;
460 
461 	/* Check if this a complete profile */
462 	completeprofile = QDF_GET_BITS(stacontrol,
463 				       WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_CMPLTPROF_IDX,
464 				       WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_CMPLTPROF_BITS);
465 
466 	if (completeprofile && is_complete_profile)
467 		*is_complete_profile = true;
468 
469 	/* Check STA Info Length */
470 	if (subelempayloadlen <
471 		parsed_payload_len + WLAN_ML_BV_LINFO_PERSTAPROF_STAINFO_LENGTH_SIZE) {
472 		mlo_err_rl("Length of subelement payload %zu octets not sufficient to contain STA Info Length of size %u octets after parsed payload length of %zu octets.",
473 			   subelempayloadlen,
474 			   WLAN_ML_BV_LINFO_PERSTAPROF_STAINFO_LENGTH_SIZE,
475 			   parsed_payload_len);
476 		return QDF_STATUS_E_PROTO;
477 	}
478 
479 	parsed_payload_len += WLAN_ML_BV_LINFO_PERSTAPROF_STAINFO_LENGTH_SIZE;
480 
481 	if (is_macaddr_valid)
482 		*is_macaddr_valid = false;
483 
484 	/* Check STA MAC address present bit */
485 	if (QDF_GET_BITS(stacontrol,
486 			 WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_MACADDRP_IDX,
487 			 WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_MACADDRP_BITS)) {
488 		if (subelempayloadlen <
489 				(parsed_payload_len + QDF_MAC_ADDR_SIZE)) {
490 			mlo_err_rl("Length of subelement payload %zu octets not sufficient to contain MAC address of size %u octets after parsed payload length of %zu octets.",
491 				   subelempayloadlen,
492 				   QDF_MAC_ADDR_SIZE,
493 				   parsed_payload_len);
494 			return QDF_STATUS_E_PROTO;
495 		}
496 
497 		if (macaddr) {
498 			qdf_mem_copy(macaddr->bytes,
499 				     subelempayload + parsed_payload_len,
500 				     QDF_MAC_ADDR_SIZE);
501 
502 			mlo_nofl_debug("Copied MAC address: " QDF_MAC_ADDR_FMT,
503 				       QDF_MAC_ADDR_REF(macaddr->bytes));
504 
505 			if (is_macaddr_valid)
506 				*is_macaddr_valid = true;
507 		}
508 
509 		parsed_payload_len += QDF_MAC_ADDR_SIZE;
510 	}
511 
512 	/* Check Beacon Interval present bit */
513 	if (QDF_GET_BITS(stacontrol,
514 			 WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_BCNINTP_IDX,
515 			 WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_BCNINTP_BITS)) {
516 		if (subelempayloadlen <
517 				(parsed_payload_len +
518 				 WLAN_BEACONINTERVAL_LEN)) {
519 			mlo_err_rl("Length of subelement payload %zu octets not sufficient to contain Beacon Interval of size %u octets after parsed payload length of %zu octets.",
520 				   subelempayloadlen,
521 				   WLAN_BEACONINTERVAL_LEN,
522 				   parsed_payload_len);
523 			return QDF_STATUS_E_PROTO;
524 		}
525 
526 		if (beaconinterval) {
527 			qdf_mem_copy(beaconinterval,
528 				     subelempayload + parsed_payload_len,
529 				     WLAN_BEACONINTERVAL_LEN);
530 			*beaconinterval = qdf_le16_to_cpu(*beaconinterval);
531 
532 			if (is_beaconinterval_valid)
533 				*is_beaconinterval_valid = true;
534 		}
535 		parsed_payload_len += WLAN_BEACONINTERVAL_LEN;
536 	}
537 
538 	/* Check TSF Offset present bit */
539 	if (QDF_GET_BITS(stacontrol,
540 			 WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_TSFOFFSETP_IDX,
541 			 WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_TSFOFFSETP_BITS)) {
542 		if (!completeprofile) {
543 			mlo_err_rl("TSF offset is expected only for complete profiles");
544 			return QDF_STATUS_E_PROTO;
545 		}
546 
547 		if (subelempayloadlen <
548 				(parsed_payload_len +
549 				 WLAN_ML_TSF_OFFSET_SIZE)) {
550 			mlo_err_rl("Length of subelement payload %zu octets not sufficient to contain TSF Offset of size %u octets after parsed payload length of %zu octets.",
551 				   subelempayloadlen,
552 				   WLAN_ML_TSF_OFFSET_SIZE,
553 				   parsed_payload_len);
554 			return QDF_STATUS_E_PROTO;
555 		}
556 
557 		if (tsfoffset) {
558 			qdf_mem_copy(tsfoffset,
559 				     subelempayload + parsed_payload_len,
560 				     WLAN_TIMESTAMP_LEN);
561 			*tsfoffset = qdf_le64_to_cpu(*tsfoffset);
562 
563 			if (is_tsfoffset_valid)
564 				*is_tsfoffset_valid = true;
565 		}
566 
567 		parsed_payload_len += WLAN_ML_TSF_OFFSET_SIZE;
568 	}
569 
570 	/* Check DTIM Info present bit */
571 	if (QDF_GET_BITS(stacontrol,
572 			 WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_DTIMINFOP_IDX,
573 			 WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_DTIMINFOP_BITS)) {
574 		if (subelempayloadlen <
575 				(parsed_payload_len +
576 				 sizeof(struct wlan_ml_bv_linfo_perstaprof_stainfo_dtiminfo))) {
577 			mlo_err_rl("Length of subelement payload %zu octets not sufficient to contain DTIM Info of size %zu octets after parsed payload length of %zu octets.",
578 				   subelempayloadlen,
579 				   sizeof(struct wlan_ml_bv_linfo_perstaprof_stainfo_dtiminfo),
580 				   parsed_payload_len);
581 			return QDF_STATUS_E_PROTO;
582 		}
583 
584 		parsed_payload_len +=
585 			sizeof(struct wlan_ml_bv_linfo_perstaprof_stainfo_dtiminfo);
586 	}
587 
588 	/* Check NTSR Link pair present bit */
589 	nstrlppresent =
590 		QDF_GET_BITS(stacontrol,
591 			     WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_NSTRLINKPRP_IDX,
592 			     WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_NSTRLINKPRP_BITS);
593 
594 	if (completeprofile && nstrlppresent) {
595 		nstrlpoffset = parsed_payload_len;
596 		/* Check NTSR Bitmap Size bit */
597 		nstrbmsz =
598 			QDF_GET_BITS(stacontrol,
599 				     WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_NSTRBMSZ_IDX,
600 				     WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_NSTRBMSZ_BITS);
601 
602 		if (nstrbmsz == WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_NSTRBMSZ_1_OCTET) {
603 			if (subelempayloadlen <
604 					(parsed_payload_len + 1)) {
605 				mlo_err_rl("Length of subelement payload %zu octets not sufficient to contain NTSR Bitmap of size 1 octet after parsed payload length of %zu octets.",
606 					   subelempayloadlen,
607 					   parsed_payload_len);
608 				return QDF_STATUS_E_PROTO;
609 			}
610 
611 			parsed_payload_len += 1;
612 		} else if (nstrbmsz == WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_NSTRBMSZ_2_OCTETS) {
613 			if (subelempayloadlen <
614 					(parsed_payload_len + 2)) {
615 				mlo_err_rl("Length of subelement payload %zu octets not sufficient to contain NTSR Bitmap  of size 2 octets after parsed payload length of %zu octets.",
616 					   subelempayloadlen,
617 					   parsed_payload_len);
618 				return QDF_STATUS_E_PROTO;
619 			}
620 
621 			parsed_payload_len += 2;
622 		} else {
623 			/* Though an invalid value cannot occur if only 1 bit is
624 			 * used, we check for it in a generic manner in case the
625 			 * number of bits is increased in the future.
626 			 */
627 			mlo_err_rl("Invalid NSTR Bitmap size %u", nstrbmsz);
628 			return QDF_STATUS_E_PROTO;
629 		}
630 		if (nstr_info) {
631 			nstr_info->nstr_lp_present = nstrlppresent;
632 			nstr_info->nstr_bmp_size = nstrbmsz;
633 			*is_nstrlp_present = true;
634 			nstr_info->link_id = link_id;
635 
636 			if (nstrbmsz == WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_NSTRBMSZ_1_OCTET) {
637 				nstr_info->nstr_lp_bitmap =
638 					*(uint8_t *)(subelempayload + nstrlpoffset);
639 			} else if (nstrbmsz == WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_NSTRBMSZ_2_OCTETS) {
640 				nstr_info->nstr_lp_bitmap =
641 					qdf_le16_to_cpu(*(uint16_t *)(subelempayload + nstrlpoffset));
642 			}
643 		}
644 	}
645 
646 	/* Check BSS Parameters Change Count Present bit */
647 	if (QDF_GET_BITS(stacontrol,
648 			 WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_BSSPARAMCHNGCNTP_IDX,
649 			 WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_BSSPARAMCHNGCNTP_BITS)) {
650 		if (subelempayloadlen <
651 				(parsed_payload_len +
652 				 WLAN_ML_BSSPARAMCHNGCNT_SIZE)) {
653 			mlo_err_rl("Length of subelement payload %zu octets not sufficient to contain BSS Parameters Change Count of size %u octets after parsed payload length of %zu octets.",
654 				   subelempayloadlen,
655 				   WLAN_ML_BSSPARAMCHNGCNT_SIZE,
656 				   parsed_payload_len);
657 			return QDF_STATUS_E_PROTO;
658 		}
659 
660 		parsed_payload_len += WLAN_ML_BSSPARAMCHNGCNT_SIZE;
661 	}
662 
663 	/* Note: Some implementation versions of hostapd/wpa_supplicant may
664 	 * provide a per-STA profile without STA profile. Let the caller
665 	 * indicate whether a STA profile is required to be found. This may be
666 	 * revisited as upstreaming progresses.
667 	 */
668 	if (!is_staprof_reqd)
669 		return QDF_STATUS_SUCCESS;
670 
671 	if (subelempayloadlen == parsed_payload_len) {
672 		mlo_err_rl("Subelement payload length %zu == parsed payload length %zu. Unable to get STA profile.",
673 			   subelempayloadlen,
674 			   parsed_payload_len);
675 		return QDF_STATUS_E_PROTO;
676 	}
677 
678 	if (staprof_len)
679 		*staprof_len = subelempayloadlen - parsed_payload_len;
680 
681 	if (staprof)
682 		*staprof = subelempayload + parsed_payload_len;
683 
684 	return QDF_STATUS_SUCCESS;
685 }
686 
687 static QDF_STATUS
util_parse_prvmlie_perstaprofile_stactrl(uint8_t * subelempayload,qdf_size_t subelempayloadlen,uint8_t * linkid,bool is_staprof_reqd,uint8_t ** staprof,qdf_size_t * staprof_len)688 util_parse_prvmlie_perstaprofile_stactrl(uint8_t *subelempayload,
689 					 qdf_size_t subelempayloadlen,
690 					 uint8_t *linkid,
691 					 bool is_staprof_reqd,
692 					 uint8_t **staprof,
693 					 qdf_size_t *staprof_len)
694 {
695 	qdf_size_t parsed_payload_len = 0;
696 	uint16_t stacontrol;
697 	uint8_t completeprofile;
698 
699 	/* This helper returns the location(s) and where required, the length(s)
700 	 * of (sub)field(s) inferable after parsing the STA Control field in the
701 	 * per-STA profile subelement. These location(s) and length(s) is/are in
702 	 * reference to the payload section of the per-STA profile subelement
703 	 * (after defragmentation, if applicable).  Here, the payload is the
704 	 * point after the subelement length in the subelement, and includes the
705 	 * payloads of all subsequent fragments (if any) but not the headers of
706 	 * those fragments.
707 	 *
708 	 * Currently, the helper returns the link ID, MAC address, and STA
709 	 * profile. More (sub)fields can be added when required.
710 	 */
711 
712 	if (!subelempayload) {
713 		mlo_err("Pointer to subelement payload is NULL");
714 		return QDF_STATUS_E_NULL_VALUE;
715 	}
716 
717 	if (!subelempayloadlen) {
718 		mlo_err("Length of subelement payload is zero");
719 		return QDF_STATUS_E_INVAL;
720 	}
721 
722 	if (subelempayloadlen < WLAN_ML_PRV_LINFO_PERSTAPROF_STACTRL_SIZE) {
723 		mlo_err_rl("Subelement payload length %zu octets is smaller than STA control field of per-STA profile subelement %u octets",
724 			   subelempayloadlen,
725 			   WLAN_ML_PRV_LINFO_PERSTAPROF_STACTRL_SIZE);
726 		return QDF_STATUS_E_PROTO;
727 	}
728 
729 	parsed_payload_len = 0;
730 
731 	qdf_mem_copy(&stacontrol,
732 		     subelempayload,
733 		     WLAN_ML_PRV_LINFO_PERSTAPROF_STACTRL_SIZE);
734 	stacontrol = qdf_le16_to_cpu(stacontrol);
735 	parsed_payload_len += WLAN_ML_PRV_LINFO_PERSTAPROF_STACTRL_SIZE;
736 
737 	if (linkid) {
738 		*linkid = QDF_GET_BITS(stacontrol,
739 				       WLAN_ML_PRV_LINFO_PERSTAPROF_STACTRL_LINKID_IDX,
740 				       WLAN_ML_PRV_LINFO_PERSTAPROF_STACTRL_LINKID_BITS);
741 	}
742 
743 	/* Check if this a complete profile */
744 	completeprofile = QDF_GET_BITS(stacontrol,
745 				       WLAN_ML_PRV_LINFO_PERSTAPROF_STACTRL_CMPLTPROF_IDX,
746 				       WLAN_ML_PRV_LINFO_PERSTAPROF_STACTRL_CMPLTPROF_BITS);
747 
748 	/* Note: Some implementation versions of hostapd/wpa_supplicant may
749 	 * provide a per-STA profile without STA profile. Let the caller
750 	 * indicate whether a STA profile is required to be found. This may be
751 	 * revisited as upstreaming progresses.
752 	 */
753 	if (!is_staprof_reqd)
754 		return QDF_STATUS_SUCCESS;
755 
756 	if (subelempayloadlen == parsed_payload_len) {
757 		mlo_err_rl("Subelement payload length %zu == parsed payload length %zu. Unable to get STA profile.",
758 			   subelempayloadlen,
759 			   parsed_payload_len);
760 		return QDF_STATUS_E_PROTO;
761 	}
762 
763 	if (staprof_len)
764 		*staprof_len = subelempayloadlen - parsed_payload_len;
765 
766 	if (staprof)
767 		*staprof = subelempayload + parsed_payload_len;
768 
769 	return QDF_STATUS_SUCCESS;
770 }
771 
772 static
util_get_successorfrag(uint8_t * currie,uint8_t * frame,qdf_size_t len)773 uint8_t *util_get_successorfrag(uint8_t *currie, uint8_t *frame, qdf_size_t len)
774 {
775 	uint8_t *nextie;
776 
777 	if (!currie || !frame || !len)
778 		return NULL;
779 
780 	if ((currie + MIN_IE_LEN) > (frame + len))
781 		return NULL;
782 
783 	/* Check whether there is sufficient space in the frame for the current
784 	 * IE, plus at least another MIN_IE_LEN bytes for the IE header of a
785 	 * fragment (if present) that would come just after the current IE.
786 	 */
787 	if ((currie + MIN_IE_LEN + currie[TAG_LEN_POS] + MIN_IE_LEN) >
788 			(frame + len))
789 		return NULL;
790 
791 	nextie = currie + currie[TAG_LEN_POS] + MIN_IE_LEN;
792 
793 	/* Check whether there is sufficient space in the frame for the next IE
794 	 */
795 	if ((nextie + MIN_IE_LEN + nextie[TAG_LEN_POS]) > (frame + len))
796 		return NULL;
797 
798 	if (nextie[ID_POS] != WLAN_ELEMID_FRAGMENT)
799 		return NULL;
800 
801 	return nextie;
802 }
803 
804 static
util_parse_partner_info_from_linkinfo(uint8_t * linkinfo,qdf_size_t linkinfo_len,struct mlo_partner_info * partner_info)805 QDF_STATUS util_parse_partner_info_from_linkinfo(uint8_t *linkinfo,
806 						 qdf_size_t linkinfo_len,
807 						 struct mlo_partner_info *partner_info)
808 {
809 	uint8_t linkid;
810 	struct qdf_mac_addr macaddr;
811 	struct mlo_nstr_info nstr_info = {0};
812 	bool is_macaddr_valid;
813 	uint8_t *linkinfo_currpos;
814 	qdf_size_t linkinfo_remlen;
815 	bool is_subelemfragseq;
816 	uint8_t subelemid;
817 	qdf_size_t subelemseqtotallen;
818 	qdf_size_t subelemseqpayloadlen;
819 	qdf_size_t defragpayload_len;
820 	QDF_STATUS ret;
821 	bool is_nstrlp_present = false;
822 
823 	/* This helper function parses partner info from the per-STA profiles
824 	 * present (if any) in the Link Info field in the payload of a Multi
825 	 * Link element (after defragmentation if required). The caller should
826 	 * pass a copy of the payload so that inline defragmentation of
827 	 * subelements can be carried out if required. The subelement
828 	 * defragmentation (if applicable) in this Control Path helper is
829 	 * required for maintainability, accuracy and eliminating current and
830 	 * future per-field-access multi-level fragment boundary checks and
831 	 * adjustments, given the complex format of Multi Link elements. It is
832 	 * also most likely to be required mainly at the client side.
833 	 */
834 
835 	if (!linkinfo) {
836 		mlo_err("linkinfo is NULL");
837 		return QDF_STATUS_E_NULL_VALUE;
838 	}
839 
840 	if (!linkinfo_len) {
841 		mlo_err("linkinfo_len is zero");
842 		return QDF_STATUS_E_NULL_VALUE;
843 	}
844 
845 	if (!partner_info) {
846 		mlo_err("ML partner info is NULL");
847 		return QDF_STATUS_E_NULL_VALUE;
848 	}
849 
850 	partner_info->num_partner_links = 0;
851 	linkinfo_currpos = linkinfo;
852 	linkinfo_remlen = linkinfo_len;
853 
854 	while (linkinfo_remlen) {
855 		if (linkinfo_remlen <  sizeof(struct subelem_header)) {
856 			mlo_err_rl("Remaining length in link info %zu octets is smaller than subelement header length %zu octets",
857 				   linkinfo_remlen,
858 				   sizeof(struct subelem_header));
859 			return QDF_STATUS_E_PROTO;
860 		}
861 
862 		subelemid = linkinfo_currpos[ID_POS];
863 		is_subelemfragseq = false;
864 		subelemseqtotallen = 0;
865 		subelemseqpayloadlen = 0;
866 
867 		ret = wlan_get_subelem_fragseq_info(WLAN_ML_LINFO_SUBELEMID_FRAGMENT,
868 						    linkinfo_currpos,
869 						    linkinfo_remlen,
870 						    &is_subelemfragseq,
871 						    &subelemseqtotallen,
872 						    &subelemseqpayloadlen);
873 		if (QDF_IS_STATUS_ERROR(ret))
874 			return ret;
875 
876 		if (is_subelemfragseq) {
877 			if (!subelemseqpayloadlen) {
878 				mlo_err_rl("Subelement fragment sequence payload is reported as 0, investigate");
879 				return QDF_STATUS_E_FAILURE;
880 			}
881 
882 			mlo_debug("Subelement fragment sequence found with payload len %zu",
883 				  subelemseqpayloadlen);
884 
885 			ret = wlan_defrag_subelem_fragseq(true,
886 							  WLAN_ML_LINFO_SUBELEMID_FRAGMENT,
887 							  linkinfo_currpos,
888 							  linkinfo_remlen,
889 							  NULL,
890 							  0,
891 							  &defragpayload_len);
892 			if (QDF_IS_STATUS_ERROR(ret))
893 				return ret;
894 
895 			if (defragpayload_len != subelemseqpayloadlen) {
896 				mlo_err_rl("Length of defragmented payload %zu octets is not equal to length of subelement fragment sequence payload %zu octets",
897 					   defragpayload_len,
898 					   subelemseqpayloadlen);
899 				return QDF_STATUS_E_FAILURE;
900 			}
901 
902 			/* Adjust linkinfo_remlen to reflect removal of all
903 			 * subelement headers except the header of the lead
904 			 * subelement.
905 			 */
906 			linkinfo_remlen -= (subelemseqtotallen -
907 					    subelemseqpayloadlen -
908 					    sizeof(struct subelem_header));
909 		} else {
910 			if (linkinfo_remlen <
911 				(sizeof(struct subelem_header) +
912 				 linkinfo_currpos[TAG_LEN_POS])) {
913 				mlo_err_rl("Remaining length in link info %zu octets is smaller than total size of current subelement %zu octets",
914 					   linkinfo_remlen,
915 					   sizeof(struct subelem_header) +
916 					   linkinfo_currpos[TAG_LEN_POS]);
917 				return QDF_STATUS_E_PROTO;
918 			}
919 
920 			subelemseqpayloadlen = linkinfo_currpos[TAG_LEN_POS];
921 		}
922 
923 		if (subelemid == WLAN_ML_LINFO_SUBELEMID_PERSTAPROFILE) {
924 			is_macaddr_valid = false;
925 
926 			ret = util_parse_bvmlie_perstaprofile_stactrl(linkinfo_currpos +
927 								      sizeof(struct subelem_header),
928 								      subelemseqpayloadlen,
929 								      &linkid,
930 								      NULL,
931 								      NULL,
932 								      NULL,
933 								      NULL,
934 								      NULL,
935 								      &is_macaddr_valid,
936 								      &macaddr,
937 								      false,
938 								      NULL,
939 								      NULL,
940 								      &nstr_info,
941 								      &is_nstrlp_present);
942 			if (QDF_IS_STATUS_ERROR(ret)) {
943 				return ret;
944 			}
945 
946 			if (is_nstrlp_present) {
947 				if (partner_info->num_nstr_info_links >=
948 					QDF_ARRAY_SIZE(partner_info->nstr_info)) {
949 					mlo_err_rl("Insufficient size %zu of array for nstr link info",
950 						   QDF_ARRAY_SIZE(partner_info->nstr_info));
951 					return QDF_STATUS_E_NOMEM;
952 				}
953 				qdf_mem_copy(&partner_info->nstr_info[partner_info->num_nstr_info_links],
954 					     &nstr_info, sizeof(nstr_info));
955 				partner_info->num_nstr_info_links++;
956 				is_nstrlp_present = false;
957 			}
958 
959 			if (is_macaddr_valid) {
960 				if (partner_info->num_partner_links >=
961 					QDF_ARRAY_SIZE(partner_info->partner_link_info)) {
962 					mlo_err_rl("Insufficient size %zu of array for partner link info",
963 						   QDF_ARRAY_SIZE(partner_info->partner_link_info));
964 					return QDF_STATUS_E_NOMEM;
965 				}
966 
967 				partner_info->partner_link_info[partner_info->num_partner_links].link_id =
968 					linkid;
969 				qdf_mem_copy(&partner_info->partner_link_info[partner_info->num_partner_links].link_addr,
970 					     &macaddr,
971 					     sizeof(partner_info->partner_link_info[partner_info->num_partner_links].link_addr));
972 
973 				partner_info->num_partner_links++;
974 			} else {
975 				mlo_warn_rl("MAC address not found in STA Info field of per-STA profile with link ID %u",
976 					    linkid);
977 			}
978 		}
979 
980 		linkinfo_remlen -= (sizeof(struct subelem_header) +
981 				    subelemseqpayloadlen);
982 		linkinfo_currpos += (sizeof(struct subelem_header) +
983 				     subelemseqpayloadlen);
984 	}
985 
986 	mlo_debug("Number of ML partner links found=%u",
987 		  partner_info->num_partner_links);
988 
989 	return QDF_STATUS_SUCCESS;
990 }
991 
992 static QDF_STATUS
util_parse_probereq_info_from_linkinfo(uint8_t * linkinfo,qdf_size_t linkinfo_len,struct mlo_probereq_info * probereq_info)993 util_parse_probereq_info_from_linkinfo(uint8_t *linkinfo,
994 				       qdf_size_t linkinfo_len,
995 				       struct mlo_probereq_info *probereq_info)
996 {
997 	uint8_t linkid;
998 	uint8_t *linkinfo_currpos;
999 	qdf_size_t linkinfo_remlen;
1000 	bool is_subelemfragseq;
1001 	uint8_t subelemid;
1002 	qdf_size_t subelemseqtotallen;
1003 	qdf_size_t subelemseqpayloadlen;
1004 	qdf_size_t defragpayload_len;
1005 	QDF_STATUS ret;
1006 
1007 	/* This helper function parses probe request info from the per-STA prof
1008 	 * present (if any) in the Link Info field in the payload of a Multi
1009 	 * Link element (after defragmentation if required). The caller should
1010 	 * pass a copy of the payload so that inline defragmentation of
1011 	 * subelements can be carried out if required. The subelement
1012 	 * defragmentation (if applicable) in this Control Path helper is
1013 	 * required for maintainability, accuracy and eliminating current and
1014 	 * future per-field-access multi-level fragment boundary checks and
1015 	 * adjustments, given the complex format of Multi Link elements. It is
1016 	 * also most likely to be required mainly at the client side.
1017 	 */
1018 
1019 	if (!linkinfo) {
1020 		mlo_err("linkinfo is NULL");
1021 		return QDF_STATUS_E_NULL_VALUE;
1022 	}
1023 
1024 	if (!linkinfo_len) {
1025 		mlo_err("linkinfo_len is zero");
1026 		return QDF_STATUS_E_NULL_VALUE;
1027 	}
1028 
1029 	if (!probereq_info) {
1030 		mlo_err("ML probe req info is NULL");
1031 		return QDF_STATUS_E_NULL_VALUE;
1032 	}
1033 
1034 	probereq_info->num_links = 0;
1035 	linkinfo_currpos = linkinfo;
1036 	linkinfo_remlen = linkinfo_len;
1037 
1038 	while (linkinfo_remlen) {
1039 		if (linkinfo_remlen <  sizeof(struct subelem_header)) {
1040 			mlo_err_rl("Remaining length in link info %zu octets is smaller than subelement header length %zu octets",
1041 				   linkinfo_remlen,
1042 				   sizeof(struct subelem_header));
1043 			return QDF_STATUS_E_PROTO;
1044 		}
1045 
1046 		subelemid = linkinfo_currpos[ID_POS];
1047 		is_subelemfragseq = false;
1048 		subelemseqtotallen = 0;
1049 		subelemseqpayloadlen = 0;
1050 
1051 		ret = wlan_get_subelem_fragseq_info(WLAN_ML_LINFO_SUBELEMID_FRAGMENT,
1052 						    linkinfo_currpos,
1053 						    linkinfo_remlen,
1054 						    &is_subelemfragseq,
1055 						    &subelemseqtotallen,
1056 						    &subelemseqpayloadlen);
1057 		if (QDF_IS_STATUS_ERROR(ret))
1058 			return ret;
1059 
1060 		if (is_subelemfragseq) {
1061 			if (!subelemseqpayloadlen) {
1062 				mlo_err_rl("Subelement fragment sequence payload is reported as 0, investigate");
1063 				return QDF_STATUS_E_FAILURE;
1064 			}
1065 
1066 			mlo_debug("Subelement fragment sequence found with payload len %zu",
1067 				  subelemseqpayloadlen);
1068 
1069 			ret = wlan_defrag_subelem_fragseq(true,
1070 							  WLAN_ML_LINFO_SUBELEMID_FRAGMENT,
1071 							  linkinfo_currpos,
1072 							  linkinfo_remlen,
1073 							  NULL,
1074 							  0,
1075 							  &defragpayload_len);
1076 			if (QDF_IS_STATUS_ERROR(ret))
1077 				return ret;
1078 
1079 			if (defragpayload_len != subelemseqpayloadlen) {
1080 				mlo_err_rl("Length of defragmented payload %zu octets is not equal to length of subelement fragment sequence payload %zu octets",
1081 					   defragpayload_len,
1082 					   subelemseqpayloadlen);
1083 				return QDF_STATUS_E_FAILURE;
1084 			}
1085 
1086 			/* Adjust linkinfo_remlen to reflect removal of all
1087 			 * subelement headers except the header of the lead
1088 			 * subelement.
1089 			 */
1090 			linkinfo_remlen -= (subelemseqtotallen -
1091 					    subelemseqpayloadlen -
1092 					    sizeof(struct subelem_header));
1093 		} else {
1094 			if (linkinfo_remlen <
1095 				(sizeof(struct subelem_header) +
1096 				 linkinfo_currpos[TAG_LEN_POS])) {
1097 				mlo_err_rl("Remaining length in link info %zu octets is smaller than total size of current subelement %zu octets",
1098 					   linkinfo_remlen,
1099 					   sizeof(struct subelem_header) +
1100 					   linkinfo_currpos[TAG_LEN_POS]);
1101 				return QDF_STATUS_E_PROTO;
1102 			}
1103 
1104 			subelemseqpayloadlen = linkinfo_currpos[TAG_LEN_POS];
1105 		}
1106 
1107 		if (subelemid == WLAN_ML_LINFO_SUBELEMID_PERSTAPROFILE) {
1108 			ret = util_parse_prvmlie_perstaprofile_stactrl(linkinfo_currpos +
1109 								      sizeof(struct subelem_header),
1110 								      subelemseqpayloadlen,
1111 								      &linkid,
1112 								      false,
1113 								      NULL,
1114 								      NULL);
1115 			if (QDF_IS_STATUS_ERROR(ret))
1116 				return ret;
1117 
1118 			if (probereq_info->num_links >=
1119 				QDF_ARRAY_SIZE(probereq_info->link_id)) {
1120 				mlo_err_rl("Insufficient size %zu of array for probe req link id",
1121 					   QDF_ARRAY_SIZE(probereq_info->link_id));
1122 				return QDF_STATUS_E_NOMEM;
1123 			}
1124 
1125 			probereq_info->link_id[probereq_info->num_links] = linkid;
1126 
1127 			probereq_info->num_links++;
1128 			mlo_debug("LINK ID requested is = %u", linkid);
1129 		}
1130 
1131 		linkinfo_remlen -= (sizeof(struct subelem_header) +
1132 				    subelemseqpayloadlen);
1133 		linkinfo_currpos += (sizeof(struct subelem_header) +
1134 				     subelemseqpayloadlen);
1135 	}
1136 
1137 	mlo_debug("Number of ML probe request links found=%u",
1138 		  probereq_info->num_links);
1139 
1140 	return QDF_STATUS_SUCCESS;
1141 }
1142 
1143 static
util_get_noninheritlists(uint8_t * buff,qdf_size_t buff_len,uint8_t ** ninherit_elemlist,qdf_size_t * ninherit_elemlist_len,uint8_t ** ninherit_elemextlist,qdf_size_t * ninherit_elemextlist_len)1144 QDF_STATUS util_get_noninheritlists(uint8_t *buff, qdf_size_t buff_len,
1145 				    uint8_t **ninherit_elemlist,
1146 				    qdf_size_t *ninherit_elemlist_len,
1147 				    uint8_t **ninherit_elemextlist,
1148 				    qdf_size_t *ninherit_elemextlist_len)
1149 {
1150 	uint8_t *ninherit_ie;
1151 	qdf_size_t unparsed_len;
1152 
1153 	/* Note: This functionality provided by this helper may be combined with
1154 	 * other, older non-inheritance parsing helper functionality and exposed
1155 	 * as a common API as part of future efforts once the older
1156 	 * functionality can be made generic.
1157 	 */
1158 
1159 	if (!buff) {
1160 		mlo_err("Pointer to buffer for IEs is NULL");
1161 		return QDF_STATUS_E_NULL_VALUE;
1162 	}
1163 
1164 	if (!buff_len) {
1165 		mlo_err("IE buffer length is zero");
1166 		return QDF_STATUS_E_INVAL;
1167 	}
1168 
1169 	if (!ninherit_elemlist) {
1170 		mlo_err("Pointer to Non-Inheritance element ID list array is NULL");
1171 		return QDF_STATUS_E_NULL_VALUE;
1172 	}
1173 
1174 	if (!ninherit_elemlist_len) {
1175 		mlo_err("Pointer to Non-Inheritance element ID list array length is NULL");
1176 		return QDF_STATUS_E_NULL_VALUE;
1177 	}
1178 
1179 	if (!ninherit_elemextlist) {
1180 		mlo_err("Pointer to Non-Inheritance element ID extension list array is NULL");
1181 		return QDF_STATUS_E_NULL_VALUE;
1182 	}
1183 
1184 	if (!ninherit_elemextlist_len) {
1185 		mlo_err("Pointer to Non-Inheritance element ID extension list array length is NULL");
1186 		return QDF_STATUS_E_NULL_VALUE;
1187 	}
1188 
1189 	ninherit_ie = NULL;
1190 	*ninherit_elemlist_len = 0;
1191 	*ninherit_elemlist = NULL;
1192 	*ninherit_elemextlist_len = 0;
1193 	*ninherit_elemextlist = NULL;
1194 
1195 	ninherit_ie =
1196 		(uint8_t *)util_find_extn_eid(WLAN_ELEMID_EXTN_ELEM,
1197 					      WLAN_EXTN_ELEMID_NONINHERITANCE,
1198 					      buff,
1199 					      buff_len);
1200 
1201 	if (ninherit_ie) {
1202 		if ((ninherit_ie + TAG_LEN_POS) > (buff + buff_len - 1)) {
1203 			mlo_err_rl("Position of length field of Non-Inheritance element would exceed IE buffer boundary");
1204 			return QDF_STATUS_E_PROTO;
1205 		}
1206 
1207 		if ((ninherit_ie + ninherit_ie[TAG_LEN_POS] + MIN_IE_LEN) >
1208 				(buff + buff_len)) {
1209 			mlo_err_rl("Non-Inheritance element with total length %u would exceed IE buffer boundary",
1210 				   ninherit_ie[TAG_LEN_POS] + MIN_IE_LEN);
1211 			return QDF_STATUS_E_PROTO;
1212 		}
1213 
1214 		if ((ninherit_ie[TAG_LEN_POS] + MIN_IE_LEN) <
1215 				MIN_NONINHERITANCEELEM_LEN) {
1216 			mlo_err_rl("Non-Inheritance element size %u is smaller than the minimum required %u",
1217 				   ninherit_ie[TAG_LEN_POS] + MIN_IE_LEN,
1218 				   MIN_NONINHERITANCEELEM_LEN);
1219 			return QDF_STATUS_E_PROTO;
1220 		}
1221 
1222 		/* Track the number of unparsed octets, excluding the IE header.
1223 		 */
1224 		unparsed_len = ninherit_ie[TAG_LEN_POS];
1225 
1226 		/* Mark the element ID extension as parsed */
1227 		unparsed_len--;
1228 
1229 		*ninherit_elemlist_len = ninherit_ie[ELEM_ID_LIST_LEN_POS];
1230 		unparsed_len--;
1231 
1232 		/* While checking if the Non-Inheritance element ID list length
1233 		 * exceeds the remaining unparsed IE space, we factor in one
1234 		 * octet for the element extension ID list length and subtract
1235 		 * this from the unparsed IE space.
1236 		 */
1237 		if (*ninherit_elemlist_len > (unparsed_len - 1)) {
1238 			mlo_err_rl("Non-Inheritance element ID list length %zu exceeds remaining unparsed IE space, minus an octet for element extension ID list length %zu",
1239 				   *ninherit_elemlist_len, unparsed_len - 1);
1240 
1241 			return QDF_STATUS_E_PROTO;
1242 		}
1243 
1244 		if (*ninherit_elemlist_len != 0) {
1245 			*ninherit_elemlist = ninherit_ie + ELEM_ID_LIST_POS;
1246 			unparsed_len -= *ninherit_elemlist_len;
1247 		}
1248 
1249 		*ninherit_elemextlist_len =
1250 			ninherit_ie[ELEM_ID_LIST_LEN_POS + *ninherit_elemlist_len + 1];
1251 		unparsed_len--;
1252 
1253 		if (*ninherit_elemextlist_len > unparsed_len) {
1254 			mlo_err_rl("Non-Inheritance element ID extension list length %zu exceeds remaining unparsed IE space %zu",
1255 				   *ninherit_elemextlist_len, unparsed_len);
1256 
1257 			return QDF_STATUS_E_PROTO;
1258 		}
1259 
1260 		if (*ninherit_elemextlist_len != 0) {
1261 			*ninherit_elemextlist = ninherit_ie +
1262 				ELEM_ID_LIST_LEN_POS + (*ninherit_elemlist_len)
1263 				+ 2;
1264 			unparsed_len -= *ninherit_elemextlist_len;
1265 		}
1266 
1267 		if (unparsed_len > 0) {
1268 			mlo_err_rl("Unparsed length is %zu, expected 0",
1269 				   unparsed_len);
1270 			return QDF_STATUS_E_PROTO;
1271 		}
1272 	}
1273 
1274 	/* If Non-Inheritance element is not found, we still return success,
1275 	 * with the list lengths kept at zero.
1276 	 */
1277 	mlo_debug("Non-Inheritance element ID list array length=%zu",
1278 		  *ninherit_elemlist_len);
1279 	mlo_debug("Non-Inheritance element ID extension list array length=%zu",
1280 		  *ninherit_elemextlist_len);
1281 
1282 	return QDF_STATUS_SUCCESS;
1283 }
1284 
1285 static
util_eval_ie_in_noninheritlist(uint8_t * ie,qdf_size_t total_ie_len,uint8_t * ninherit_elemlist,qdf_size_t ninherit_elemlist_len,uint8_t * ninherit_elemextlist,qdf_size_t ninherit_elemextlist_len,bool * is_in_noninheritlist)1286 QDF_STATUS util_eval_ie_in_noninheritlist(uint8_t *ie, qdf_size_t total_ie_len,
1287 					  uint8_t *ninherit_elemlist,
1288 					  qdf_size_t ninherit_elemlist_len,
1289 					  uint8_t *ninherit_elemextlist,
1290 					  qdf_size_t ninherit_elemextlist_len,
1291 					  bool *is_in_noninheritlist)
1292 {
1293 	int i;
1294 
1295 	/* Evaluate whether the given IE is in the given Non-Inheritance element
1296 	 * ID list or Non-Inheritance element ID extension list, and update the
1297 	 * result into is_in_noninheritlist. If any list is empty, then the IE
1298 	 * is considered to not be present in that list. Both lists can be
1299 	 * empty.
1300 	 *
1301 	 * If QDF_STATUS_SUCCESS is returned, it means that the evaluation is
1302 	 * successful, and that is_in_noninheritlist contains a valid value
1303 	 * (which could be true or false). If a QDF_STATUS error value is
1304 	 * returned, the value in is_in_noninheritlist is invalid and the caller
1305 	 * should ignore it.
1306 	 */
1307 
1308 	/* Note: The functionality provided by this helper may be combined with
1309 	 * other, older non-inheritance parsing helper functionality and exposed
1310 	 * as a common API as part of future efforts once the older
1311 	 * functionality can be made generic.
1312 	 */
1313 
1314 	/* Except for is_in_noninheritlist and ie, other pointer arguments are
1315 	 * permitted to be NULL if they are inapplicable. If they are
1316 	 * applicable, they will be checked to ensure they are not NULL.
1317 	 */
1318 
1319 	if (!is_in_noninheritlist) {
1320 		mlo_err("NULL pointer to flag that indicates if element is in a Non-Inheritance list");
1321 		return QDF_STATUS_E_NULL_VALUE;
1322 	}
1323 
1324 	/* If ninherit_elemlist_len and ninherit_elemextlist_len are both zero
1325 	 * as checked soon in this function, we won't be accessing the IE.
1326 	 * However, we still check right-away if the pointer to the IE is
1327 	 * non-NULL and whether the total IE length is sane enough to access the
1328 	 * element ID and if applicable, the element ID extension, since it
1329 	 * doesn't make sense to set the flag in is_in_noninheritlist for a NULL
1330 	 * IE pointer or an IE whose total length is not sane enough to
1331 	 * distinguish the identity of the IE.
1332 	 */
1333 	if (!ie) {
1334 		mlo_err("NULL pointer to IE");
1335 		return QDF_STATUS_E_NULL_VALUE;
1336 	}
1337 
1338 	if (total_ie_len < (ID_POS + 1)) {
1339 		mlo_err("Total IE length %zu is smaller than minimum required to access element ID %u",
1340 			total_ie_len, ID_POS + 1);
1341 		return QDF_STATUS_E_INVAL;
1342 	}
1343 
1344 	if ((ie[ID_POS] == WLAN_ELEMID_EXTN_ELEM) &&
1345 	    (total_ie_len < (IDEXT_POS + 1))) {
1346 		mlo_err("Total IE length %zu is smaller than minimum required to access element ID extension %u",
1347 			total_ie_len, IDEXT_POS + 1);
1348 		return QDF_STATUS_E_INVAL;
1349 	}
1350 
1351 	*is_in_noninheritlist = false;
1352 
1353 	/* If both the Non-Inheritance element list and Non-Inheritance element
1354 	 * ID extension list are empty, then return success since we can
1355 	 * conclude immediately that the given element does not occur in any
1356 	 * Non-Inheritance list. The is_in_noninheritlist remains set to false
1357 	 * as required.
1358 	 */
1359 	if (!ninherit_elemlist_len && !ninherit_elemextlist_len)
1360 		return QDF_STATUS_SUCCESS;
1361 
1362 	if (ie[ID_POS] != WLAN_ELEMID_EXTN_ELEM) {
1363 		if (!ninherit_elemlist_len)
1364 			return QDF_STATUS_SUCCESS;
1365 
1366 		if (!ninherit_elemlist) {
1367 			mlo_err("NULL pointer to Non-Inheritance element ID list though length of element ID list is %zu",
1368 				ninherit_elemlist_len);
1369 			return QDF_STATUS_E_NULL_VALUE;
1370 		}
1371 
1372 		for (i = 0; i < ninherit_elemlist_len; i++) {
1373 			if (ie[ID_POS] == ninherit_elemlist[i]) {
1374 				*is_in_noninheritlist = true;
1375 				return QDF_STATUS_SUCCESS;
1376 			}
1377 		}
1378 	} else {
1379 		if (!ninherit_elemextlist_len)
1380 			return QDF_STATUS_SUCCESS;
1381 
1382 		if (!ninherit_elemextlist) {
1383 			mlo_err("NULL pointer to Non-Inheritance element ID extension list though length of element ID extension list is %zu",
1384 				ninherit_elemextlist_len);
1385 			return QDF_STATUS_E_NULL_VALUE;
1386 		}
1387 
1388 		for (i = 0; i < ninherit_elemextlist_len; i++) {
1389 			if (ie[IDEXT_POS] == ninherit_elemextlist[i]) {
1390 				*is_in_noninheritlist = true;
1391 				return QDF_STATUS_SUCCESS;
1392 			}
1393 		}
1394 	}
1395 
1396 	return QDF_STATUS_SUCCESS;
1397 }
1398 
1399 #if defined (SAP_MULTI_LINK_EMULATION)
1400 static inline
util_validate_reportingsta_ie(const uint8_t * reportingsta_ie,const uint8_t * frame_iesection,const qdf_size_t frame_iesection_len)1401 QDF_STATUS util_validate_reportingsta_ie(const uint8_t *reportingsta_ie,
1402 					 const uint8_t *frame_iesection,
1403 					 const qdf_size_t frame_iesection_len)
1404 {
1405 	return QDF_STATUS_SUCCESS;
1406 }
1407 #else
1408 static inline
util_validate_reportingsta_ie(const uint8_t * reportingsta_ie,const uint8_t * frame_iesection,const qdf_size_t frame_iesection_len)1409 QDF_STATUS util_validate_reportingsta_ie(const uint8_t *reportingsta_ie,
1410 					 const uint8_t *frame_iesection,
1411 					 const qdf_size_t frame_iesection_len)
1412 {
1413 	qdf_size_t reportingsta_ie_size;
1414 
1415 	if (!reportingsta_ie) {
1416 		mlo_err("Pointer to reporting STA IE is NULL");
1417 		return QDF_STATUS_E_NULL_VALUE;
1418 	}
1419 
1420 	if (!frame_iesection) {
1421 		mlo_err("Pointer to start of IE section in reporting frame is NULL");
1422 		return QDF_STATUS_E_NULL_VALUE;
1423 	}
1424 
1425 	if (!frame_iesection_len) {
1426 		mlo_err("Length of IE section in reporting frame is zero");
1427 		return QDF_STATUS_E_INVAL;
1428 	}
1429 
1430 	if ((reportingsta_ie + ID_POS) > (frame_iesection +
1431 			frame_iesection_len - 1)) {
1432 		mlo_err_rl("Position of element ID field of element for reporting STA would exceed frame IE section boundary");
1433 		return QDF_STATUS_E_PROTO;
1434 	}
1435 
1436 	if ((reportingsta_ie + TAG_LEN_POS) > (frame_iesection +
1437 			frame_iesection_len - 1)) {
1438 		mlo_err_rl("Position of length field of element with element ID %u for reporting STA would exceed frame IE section boundary",
1439 			   reportingsta_ie[ID_POS]);
1440 		return QDF_STATUS_E_PROTO;
1441 	}
1442 
1443 	if ((reportingsta_ie[ID_POS] == WLAN_ELEMID_EXTN_ELEM) &&
1444 	    ((reportingsta_ie + IDEXT_POS) > (frame_iesection +
1445 				frame_iesection_len - 1))) {
1446 		mlo_err_rl("Position of element ID extension field of element would exceed frame IE section boundary");
1447 		return QDF_STATUS_E_PROTO;
1448 	}
1449 
1450 	reportingsta_ie_size = reportingsta_ie[TAG_LEN_POS] + MIN_IE_LEN;
1451 
1452 	if ((reportingsta_ie[ID_POS] == WLAN_ELEMID_EXTN_ELEM) &&
1453 	    (reportingsta_ie_size < (IDEXT_POS + 1))) {
1454 		mlo_err_rl("Total length %zu of element for reporting STA is smaller than minimum required to access element ID extension %u",
1455 			   reportingsta_ie_size, IDEXT_POS + 1);
1456 		return QDF_STATUS_E_PROTO;
1457 	}
1458 
1459 	if ((reportingsta_ie[ID_POS] == WLAN_ELEMID_VENDOR) &&
1460 	    (reportingsta_ie_size < (PAYLOAD_START_POS + OUI_LEN))) {
1461 		mlo_err_rl("Total length %zu of element for reporting STA is smaller than minimum required to access vendor EID %u",
1462 			   reportingsta_ie_size, PAYLOAD_START_POS + OUI_LEN);
1463 		return QDF_STATUS_E_PROTO;
1464 	}
1465 
1466 	if ((reportingsta_ie + reportingsta_ie_size) >
1467 			(frame_iesection + frame_iesection_len)) {
1468 		if (reportingsta_ie[ID_POS] == WLAN_ELEMID_EXTN_ELEM) {
1469 			mlo_err_rl("Total size %zu octets of element with element ID %u element ID extension %u for reporting STA would exceed frame IE section boundary",
1470 				   reportingsta_ie_size,
1471 				   reportingsta_ie[ID_POS],
1472 				   reportingsta_ie[IDEXT_POS]);
1473 		} else {
1474 			mlo_err_rl("Total size %zu octets of element with element ID %u for reporting STA would exceed frame IE section boundary",
1475 				   reportingsta_ie_size,
1476 				   reportingsta_ie[ID_POS]);
1477 		}
1478 
1479 		return QDF_STATUS_E_PROTO;
1480 	}
1481 
1482 	return QDF_STATUS_SUCCESS;
1483 }
1484 #endif
1485 
1486 static inline
util_validate_sta_prof_ie(const uint8_t * sta_prof_ie,const uint8_t * sta_prof_iesection,const qdf_size_t sta_prof_iesection_len)1487 QDF_STATUS util_validate_sta_prof_ie(const uint8_t *sta_prof_ie,
1488 				     const uint8_t *sta_prof_iesection,
1489 				     const qdf_size_t sta_prof_iesection_len)
1490 {
1491 	qdf_size_t sta_prof_ie_size;
1492 
1493 	if (!sta_prof_ie) {
1494 		mlo_err("Pointer to STA profile IE is NULL");
1495 		return QDF_STATUS_E_NULL_VALUE;
1496 	}
1497 
1498 	if (!sta_prof_iesection) {
1499 		mlo_err("Pointer to start of IE section in STA profile is NULL");
1500 		return QDF_STATUS_E_NULL_VALUE;
1501 	}
1502 
1503 	if (!sta_prof_iesection_len) {
1504 		mlo_err("Length of IE section in STA profile is zero");
1505 		return QDF_STATUS_E_INVAL;
1506 	}
1507 
1508 	if ((sta_prof_ie + ID_POS) > (sta_prof_iesection +
1509 			sta_prof_iesection_len - 1)) {
1510 		mlo_err_rl("Position of element ID field of STA profile element would exceed STA profile IE section boundary");
1511 		return QDF_STATUS_E_PROTO;
1512 	}
1513 
1514 	if ((sta_prof_ie + TAG_LEN_POS) > (sta_prof_iesection +
1515 			sta_prof_iesection_len - 1)) {
1516 		mlo_err_rl("Position of length field of element with element ID %u in STA profile would exceed STA profile IE section boundary",
1517 			   sta_prof_ie[ID_POS]);
1518 		return QDF_STATUS_E_PROTO;
1519 	}
1520 
1521 	if ((sta_prof_ie[ID_POS] == WLAN_ELEMID_EXTN_ELEM) &&
1522 	    ((sta_prof_ie + IDEXT_POS) > (sta_prof_iesection +
1523 				sta_prof_iesection_len - 1))) {
1524 		mlo_err_rl("Position of element ID extension field of element would exceed STA profile IE section boundary");
1525 		return QDF_STATUS_E_PROTO;
1526 	}
1527 
1528 	sta_prof_ie_size = sta_prof_ie[TAG_LEN_POS] + MIN_IE_LEN;
1529 
1530 	if ((sta_prof_ie[ID_POS] == WLAN_ELEMID_EXTN_ELEM) &&
1531 	    (sta_prof_ie_size < (IDEXT_POS + 1))) {
1532 		mlo_err_rl("Total length %zu of STA profile element is smaller than minimum required to access element ID extension %u",
1533 			   sta_prof_ie_size, IDEXT_POS + 1);
1534 		return QDF_STATUS_E_PROTO;
1535 	}
1536 
1537 	if ((sta_prof_ie + sta_prof_ie_size) >
1538 			(sta_prof_iesection + sta_prof_iesection_len)) {
1539 		if (sta_prof_ie[ID_POS] == WLAN_ELEMID_EXTN_ELEM) {
1540 			mlo_err_rl("Total size %zu octets of element with element ID %u element ID extension %u in STA profile would exceed STA profile IE section boundary",
1541 				   sta_prof_ie_size,
1542 				   sta_prof_ie[ID_POS],
1543 				   sta_prof_ie[IDEXT_POS]);
1544 		} else {
1545 			mlo_err_rl("Total size %zu octets of element with element ID %u in STA profile would exceed STA profile IE section boundary",
1546 				   sta_prof_ie_size,
1547 				   sta_prof_ie[ID_POS]);
1548 		}
1549 
1550 		return QDF_STATUS_E_PROTO;
1551 	}
1552 
1553 	return QDF_STATUS_SUCCESS;
1554 }
1555 
1556 #ifdef CONN_MGR_ADV_FEATURE
1557 /**
1558  * util_add_mlie_for_prb_rsp_gen - Add the basic variant Multi-Link element
1559  * when generating link specific probe response.
1560  * @reportingsta_ie: Pointer to the reportingsta ie
1561  * @reportingsta_ie_len: Length for reporting sta ie
1562  * @plink_frame_currpos: Pointer to Link frame current pos
1563  * @plink_frame_currlen: Current length of link frame.
1564  * @link_frame_maxsize: Maximum size of the frame to be generated
1565  * @linkid: Link Id value
1566  *
1567  * Add the basic variant Multi-Link element when
1568  * generating link specific probe response.
1569  *
1570  * Return: QDF_STATUS_SUCCESS in the case of success, QDF_STATUS value giving
1571  * the reason for error in the case of failure
1572  */
1573 static QDF_STATUS
util_add_mlie_for_prb_rsp_gen(const uint8_t * reportingsta_ie,qdf_size_t reportingsta_ie_len,uint8_t ** plink_frame_currpos,qdf_size_t * plink_frame_currlen,qdf_size_t link_frame_maxsize,uint8_t linkid)1574 util_add_mlie_for_prb_rsp_gen(const uint8_t *reportingsta_ie,
1575 			      qdf_size_t reportingsta_ie_len,
1576 			      uint8_t **plink_frame_currpos,
1577 			      qdf_size_t *plink_frame_currlen,
1578 			      qdf_size_t link_frame_maxsize,
1579 			      uint8_t linkid)
1580 {
1581 	uint8_t mlie_len = 0;
1582 	uint8_t common_info_len = 0;
1583 	struct wlan_ie_multilink ml_ie_ff;
1584 	uint16_t mlcontrol;
1585 	uint16_t presencebm;
1586 	uint8_t *mlie_frame = NULL;
1587 	uint8_t link_id_offset = sizeof(struct wlan_ie_multilink) +
1588 				QDF_MAC_ADDR_SIZE +
1589 				WLAN_ML_BV_CINFO_LENGTH_SIZE;
1590 	uint8_t *link_frame_currpos = *plink_frame_currpos;
1591 	qdf_size_t link_frame_currlen = *plink_frame_currlen;
1592 	QDF_STATUS status = QDF_STATUS_SUCCESS;
1593 
1594 	status = util_get_mlie_common_info_len((uint8_t *)reportingsta_ie,
1595 					       reportingsta_ie_len,
1596 					       &common_info_len);
1597 	if (QDF_IS_STATUS_ERROR(status) ||
1598 	    common_info_len > reportingsta_ie_len ||
1599 	    (reportingsta_ie_len - common_info_len <
1600 	     sizeof(struct wlan_ie_multilink))) {
1601 		mlo_err("Failed to parse common info, mlie len %d common info len %d",
1602 			reportingsta_ie_len, common_info_len);
1603 		return status;
1604 	}
1605 
1606 	/* common info len + bvmlie fixed fields */
1607 	mlie_len = common_info_len + sizeof(struct wlan_ie_multilink);
1608 
1609 	mlo_debug_rl("mlie_len %d, common_info_len %d, link_id_offset %d",
1610 		     mlie_len,
1611 		     common_info_len,
1612 		     link_id_offset);
1613 
1614 	/*
1615 	 * Validate the buffer available before copying ML IE.
1616 	 * Incase if mlie_len is modified at later place, move this validation
1617 	 * there to make sure no buffer overflow happens.
1618 	 */
1619 	if ((link_frame_maxsize - link_frame_currlen) < mlie_len) {
1620 		mlo_err("Insufficient space in link specific frame for ML IE. Required: %u octets, available: %zu octets",
1621 			mlie_len, (link_frame_maxsize - link_frame_currlen));
1622 		return QDF_STATUS_E_NOMEM;
1623 	}
1624 
1625 	mlie_frame = qdf_mem_malloc(mlie_len);
1626 	if (!mlie_frame)
1627 		return QDF_STATUS_E_NOMEM;
1628 
1629 	/* Copy ml ie fixed fields */
1630 	qdf_mem_copy(&ml_ie_ff,
1631 		     reportingsta_ie,
1632 		     sizeof(struct wlan_ie_multilink));
1633 
1634 	ml_ie_ff.elem_len = mlie_len - sizeof(struct ie_header);
1635 
1636 	mlcontrol = qdf_le16_to_cpu(ml_ie_ff.mlcontrol);
1637 	presencebm = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_PBM_IDX,
1638 				  WLAN_ML_CTRL_PBM_BITS);
1639 	qdf_set_bit(WLAN_ML_BV_CTRL_PBM_LINKIDINFO_P,
1640 		    (unsigned long *)&presencebm);
1641 
1642 	QDF_SET_BITS(ml_ie_ff.mlcontrol,
1643 		     WLAN_ML_CTRL_PBM_IDX,
1644 		     WLAN_ML_CTRL_PBM_BITS,
1645 		     presencebm);
1646 
1647 	qdf_mem_copy(mlie_frame,
1648 		     &ml_ie_ff,
1649 		     sizeof(struct wlan_ie_multilink));
1650 
1651 	qdf_mem_copy(mlie_frame + sizeof(struct wlan_ie_multilink),
1652 		     reportingsta_ie + sizeof(struct wlan_ie_multilink),
1653 		     mlie_len - sizeof(struct wlan_ie_multilink));
1654 
1655 	if (linkid == 0xFF || mlie_len <= link_id_offset) {
1656 		qdf_mem_free(mlie_frame);
1657 		mlo_err("Failed to process link id, link_id %d", linkid);
1658 		return QDF_STATUS_E_INVAL;
1659 	}
1660 	mlie_frame[link_id_offset] = (mlie_frame[link_id_offset] & ~0x0f) |
1661 				   (linkid & 0x0f);
1662 	qdf_mem_copy(link_frame_currpos,
1663 		     mlie_frame,
1664 		     mlie_len);
1665 
1666 	mlo_debug("Add mlie for link id %d", linkid);
1667 	QDF_TRACE_HEX_DUMP(QDF_MODULE_ID_PE, QDF_TRACE_LEVEL_DEBUG,
1668 			   mlie_frame, mlie_len);
1669 
1670 	link_frame_currpos += mlie_len;
1671 	link_frame_currlen += mlie_len;
1672 	*plink_frame_currpos = link_frame_currpos;
1673 	*plink_frame_currlen = link_frame_currlen;
1674 	qdf_mem_free(mlie_frame);
1675 
1676 	return QDF_STATUS_SUCCESS;
1677 }
1678 #else
1679 static QDF_STATUS
util_add_mlie_for_prb_rsp_gen(const uint8_t * reportingsta_ie,qdf_size_t reportingsta_ie_len,uint8_t ** plink_frame_currpos,qdf_size_t * plink_frame_currlen,qdf_size_t link_frame_maxsize,uint8_t linkid)1680 util_add_mlie_for_prb_rsp_gen(const uint8_t *reportingsta_ie,
1681 			      qdf_size_t reportingsta_ie_len,
1682 			      uint8_t **plink_frame_currpos,
1683 			      qdf_size_t *plink_frame_currlen,
1684 			      qdf_size_t link_frame_maxsize,
1685 			      uint8_t linkid)
1686 {
1687 	return QDF_STATUS_SUCCESS;
1688 }
1689 #endif
1690 
1691 /**
1692  * util_find_bvmlie_persta_prof_for_linkid() - get per sta profile per link id
1693  * @req_link_id: link id
1694  * @linkinfo: the pointer of link info
1695  * @linkinfo_len: the length of link info
1696  * @persta_prof_frame: the pointer to store the address of sta profile
1697  * @persta_prof_len: the sta profile length
1698  *
1699  * This helper function parses partner info from the per-STA profiles
1700  * present (if any) in the Link Info field in the payload of a Multi
1701  * Link element (after defragmentation if required). The caller should
1702  * pass a copy of the payload so that inline defragmentation of
1703  * subelements can be carried out if required. The subelement
1704  * defragmentation (if applicable) in this Control Path helper is
1705  * required for maintainability, accuracy and eliminating current and
1706  * future per-field-access multi-level fragment boundary checks and
1707  * adjustments, given the complex format of Multi Link elements. It is
1708  * also most likely to be required mainly at the client side.
1709  *
1710  * Return: QDF_STATUS
1711  */
1712 static QDF_STATUS
util_find_bvmlie_persta_prof_for_linkid(uint8_t req_link_id,uint8_t * linkinfo,qdf_size_t linkinfo_len,uint8_t ** persta_prof_frame,qdf_size_t * persta_prof_len)1713 util_find_bvmlie_persta_prof_for_linkid(uint8_t req_link_id,
1714 					uint8_t *linkinfo,
1715 					qdf_size_t linkinfo_len,
1716 					uint8_t **persta_prof_frame,
1717 					qdf_size_t *persta_prof_len)
1718 {
1719 	uint8_t linkid;
1720 	struct qdf_mac_addr macaddr;
1721 	bool is_macaddr_valid;
1722 	uint8_t *linkinfo_currpos;
1723 	qdf_size_t linkinfo_remlen;
1724 	bool is_subelemfragseq;
1725 	uint8_t subelemid;
1726 	qdf_size_t subelemseqtotallen;
1727 	qdf_size_t subelemseqpayloadlen;
1728 	qdf_size_t defragpayload_len;
1729 	QDF_STATUS ret;
1730 
1731 	if (!linkinfo) {
1732 		mlo_err("linkinfo is NULL");
1733 		return QDF_STATUS_E_NULL_VALUE;
1734 	}
1735 
1736 	if (!linkinfo_len) {
1737 		mlo_err("linkinfo_len is zero");
1738 		return QDF_STATUS_E_NULL_VALUE;
1739 	}
1740 
1741 	if (!persta_prof_frame) {
1742 		mlo_err("Pointer to per-STA prof frame is NULL");
1743 		return QDF_STATUS_E_NULL_VALUE;
1744 	}
1745 
1746 	if (!persta_prof_len) {
1747 		mlo_err("Length to per-STA prof frame is 0");
1748 		return QDF_STATUS_E_NULL_VALUE;
1749 	}
1750 
1751 	linkinfo_currpos = linkinfo;
1752 	linkinfo_remlen = linkinfo_len;
1753 
1754 	while (linkinfo_remlen) {
1755 		if (linkinfo_remlen <  sizeof(struct subelem_header)) {
1756 			mlo_err_rl("Remaining length in link info %zu octets is smaller than subelement header length %zu octets",
1757 				   linkinfo_remlen,
1758 				   sizeof(struct subelem_header));
1759 			return QDF_STATUS_E_PROTO;
1760 		}
1761 
1762 		subelemid = linkinfo_currpos[ID_POS];
1763 		is_subelemfragseq = false;
1764 		subelemseqtotallen = 0;
1765 		subelemseqpayloadlen = 0;
1766 
1767 		ret = wlan_get_subelem_fragseq_info(WLAN_ML_LINFO_SUBELEMID_FRAGMENT,
1768 						    linkinfo_currpos,
1769 						    linkinfo_remlen,
1770 						    &is_subelemfragseq,
1771 						    &subelemseqtotallen,
1772 						    &subelemseqpayloadlen);
1773 		if (QDF_IS_STATUS_ERROR(ret))
1774 			return ret;
1775 
1776 		if (is_subelemfragseq) {
1777 			if (!subelemseqpayloadlen) {
1778 				mlo_err_rl("Subelement fragment sequence payload is reported as 0, investigate");
1779 				return QDF_STATUS_E_FAILURE;
1780 			}
1781 
1782 			mlo_debug("Subelement fragment sequence found with payload len %zu",
1783 				  subelemseqpayloadlen);
1784 
1785 			ret = wlan_defrag_subelem_fragseq(true,
1786 							  WLAN_ML_LINFO_SUBELEMID_FRAGMENT,
1787 							  linkinfo_currpos,
1788 							  linkinfo_remlen,
1789 							  NULL,
1790 							  0,
1791 							  &defragpayload_len);
1792 			if (QDF_IS_STATUS_ERROR(ret))
1793 				return ret;
1794 
1795 			if (defragpayload_len != subelemseqpayloadlen) {
1796 				mlo_err_rl("Length of defragmented payload %zu octets is not equal to length of subelement fragment sequence payload %zu octets",
1797 					   defragpayload_len,
1798 					   subelemseqpayloadlen);
1799 				return QDF_STATUS_E_FAILURE;
1800 			}
1801 
1802 			/* Adjust linkinfo_remlen to reflect removal of all
1803 			 * subelement headers except the header of the lead
1804 			 * subelement.
1805 			 */
1806 			linkinfo_remlen -= (subelemseqtotallen -
1807 					    subelemseqpayloadlen -
1808 					    sizeof(struct subelem_header));
1809 		} else {
1810 			if (linkinfo_remlen <
1811 				(sizeof(struct subelem_header) +
1812 				 linkinfo_currpos[TAG_LEN_POS])) {
1813 				mlo_err_rl("Remaining length in link info %zu octets is smaller than total size of current subelement %zu octets",
1814 					   linkinfo_remlen,
1815 					   sizeof(struct subelem_header) +
1816 					   linkinfo_currpos[TAG_LEN_POS]);
1817 				return QDF_STATUS_E_PROTO;
1818 			}
1819 
1820 			subelemseqpayloadlen = linkinfo_currpos[TAG_LEN_POS];
1821 		}
1822 
1823 		if (subelemid == WLAN_ML_LINFO_SUBELEMID_PERSTAPROFILE) {
1824 			is_macaddr_valid = false;
1825 
1826 			ret = util_parse_bvmlie_perstaprofile_stactrl(linkinfo_currpos +
1827 								      sizeof(struct subelem_header),
1828 								      subelemseqpayloadlen,
1829 								      &linkid,
1830 								      NULL,
1831 								      NULL,
1832 								      NULL,
1833 								      NULL,
1834 								      NULL,
1835 								      &is_macaddr_valid,
1836 								      &macaddr,
1837 								      false,
1838 								      NULL,
1839 								      NULL,
1840 								      NULL,
1841 								      NULL);
1842 			if (QDF_IS_STATUS_ERROR(ret))
1843 				return ret;
1844 
1845 			if (req_link_id == linkid) {
1846 				mlo_debug("Found requested per-STA prof for linkid %u, len %zu",
1847 					  linkid, subelemseqpayloadlen);
1848 				*persta_prof_frame = linkinfo_currpos;
1849 				*persta_prof_len = subelemseqpayloadlen;
1850 				return QDF_STATUS_SUCCESS;
1851 			}
1852 		}
1853 
1854 		linkinfo_remlen -= (sizeof(struct subelem_header) +
1855 				    subelemseqpayloadlen);
1856 		linkinfo_currpos += (sizeof(struct subelem_header) +
1857 				     subelemseqpayloadlen);
1858 	}
1859 
1860 	return QDF_STATUS_E_PROTO;
1861 }
1862 
1863 static
util_gen_link_reqrsp_cmn(uint8_t * frame,qdf_size_t frame_len,uint8_t subtype,uint8_t req_link_id,struct qdf_mac_addr link_addr,uint8_t * link_frame,qdf_size_t link_frame_maxsize,qdf_size_t * link_frame_len)1864 QDF_STATUS util_gen_link_reqrsp_cmn(uint8_t *frame, qdf_size_t frame_len,
1865 				    uint8_t subtype,
1866 				    uint8_t req_link_id,
1867 				    struct qdf_mac_addr link_addr,
1868 				    uint8_t *link_frame,
1869 				    qdf_size_t link_frame_maxsize,
1870 				    qdf_size_t *link_frame_len)
1871 {
1872 	/* Please see documentation for util_gen_link_assoc_req() and
1873 	 * util_gen_link_assoc_resp() for information on the inputs to and
1874 	 * output from this helper, since those APIs are essentially wrappers
1875 	 * over this helper.
1876 	 */
1877 
1878 	/* Pointer to Multi-Link element/Multi-Link element fragment sequence */
1879 	uint8_t *mlieseq;
1880 	/* Total length of Multi-Link element sequence (including fragments if
1881 	 * any)
1882 	 */
1883 	qdf_size_t mlieseqlen;
1884 	/* Variant (i.e. type) of the Multi-Link element */
1885 	enum wlan_ml_variant variant;
1886 
1887 	/* Length of the payload of the Multi-Link element (inclusive of
1888 	 * fragment payloads if any) without IE headers and element ID extension
1889 	 */
1890 	qdf_size_t mlieseqpayloadlen;
1891 	/* Pointer to copy of the payload of the Multi-Link element (inclusive
1892 	 * of fragment payloads if any) without IE headers and element ID
1893 	 * extension
1894 	 */
1895 	uint8_t *mlieseqpayload_copy;
1896 
1897 	/* Pointer to start of Link Info within the copy of the payload of the
1898 	 * Multi-Link element
1899 	 */
1900 	uint8_t *link_info;
1901 	/* Length of the Link Info */
1902 	qdf_size_t link_info_len;
1903 
1904 	/* Pointer to the IE section that occurs after the fixed fields in the
1905 	 * original frame for the reporting STA.
1906 	 */
1907 	uint8_t *frame_iesection;
1908 	/* Offset to the start of the IE section in the original frame for the
1909 	 * reporting STA.
1910 	 */
1911 	qdf_size_t frame_iesection_offset;
1912 	/* Total length of the IE section in the original frame for the
1913 	 * reporting STA.
1914 	 */
1915 	qdf_size_t frame_iesection_len;
1916 
1917 	/* Pointer to the IEEE802.11 frame header in the link specific frame
1918 	 * being generated for the reported STA.
1919 	 */
1920 	struct wlan_frame_hdr *link_frame_hdr;
1921 	/* Current position in the link specific frame being generated for the
1922 	 * reported STA.
1923 	 */
1924 	uint8_t *link_frame_currpos;
1925 	/* Current length of the link specific frame being generated for the
1926 	 * reported STA.
1927 	 */
1928 	qdf_size_t link_frame_currlen;
1929 
1930 	/* Pointer to IE for reporting STA */
1931 	const uint8_t *reportingsta_ie;
1932 	/* Total size of IE for reporting STA, inclusive of the element header
1933 	 */
1934 	qdf_size_t reportingsta_ie_size;
1935 
1936 	/* Pointer to current position in STA profile */
1937 	uint8_t *sta_prof_currpos;
1938 	/* Remaining length of STA profile */
1939 	qdf_size_t sta_prof_remlen;
1940 	/* Pointer to start of IE section in STA profile that occurs after fixed
1941 	 * fields.
1942 	 */
1943 	uint8_t *sta_prof_iesection;
1944 	/* Total length of IE section in STA profile */
1945 	qdf_size_t sta_prof_iesection_len;
1946 	/* Pointer to current position being processed in IE section in STA
1947 	 * profile.
1948 	 */
1949 	uint8_t *sta_prof_iesection_currpos;
1950 	/* Remaining length of IE section in STA profile */
1951 	qdf_size_t sta_prof_iesection_remlen;
1952 
1953 	/* Pointer to IE in STA profile, that occurs within IE section */
1954 	uint8_t *sta_prof_ie;
1955 	/* Total size of IE in STA profile, inclusive of the element header */
1956 	qdf_size_t sta_prof_ie_size;
1957 
1958 	/* Pointer to element ID list in Non-Inheritance IE */
1959 	uint8_t *ninherit_elemlist;
1960 	/* Length of element ID list in Non-Inheritance IE */
1961 	qdf_size_t ninherit_elemlist_len;
1962 	/* Pointer to element ID extension list in Non-Inheritance IE */
1963 	uint8_t *ninherit_elemextlist;
1964 	/* Length of element ID extension list in Non-Inheritance IE */
1965 	qdf_size_t ninherit_elemextlist_len;
1966 	/* Whether a given IE is in a non-inheritance list */
1967 	bool is_in_noninheritlist;
1968 
1969 	/* Whether MAC address of reported STA is valid */
1970 	bool is_reportedmacaddr_valid;
1971 	/* MAC address of reported STA */
1972 	struct qdf_mac_addr reportedmacaddr;
1973 
1974 	/* Pointer to per-STA profile */
1975 	uint8_t *persta_prof;
1976 	/* Length of the containing buffer which starts with the per-STA profile
1977 	 */
1978 	qdf_size_t persta_prof_bufflen;
1979 
1980 	/* Other variables for temporary purposes */
1981 
1982 	/* Variable into which API for determining fragment information will
1983 	 * indicate whether the element is the start of a fragment sequence or
1984 	 * not.
1985 	 */
1986 	bool is_elemfragseq;
1987 	/*  De-fragmented payload length returned by API for element
1988 	 *  defragmentation.
1989 	 */
1990 	qdf_size_t defragpayload_len;
1991 	/* Pointer to Beacon interval in STA info field */
1992 	uint16_t beaconinterval;
1993 	/* Whether Beacon interval value valid */
1994 	bool is_beaconinterval_valid;
1995 	/* TSF timer of the reporting AP */
1996 	uint64_t tsf;
1997 	/* TSF offset of the reproted AP */
1998 	uint64_t tsfoffset;
1999 	/* TSF offset value valid */
2000 	bool is_tsfoffset_valid;
2001 	/* If Complete Profile or not*/
2002 	bool is_completeprofile;
2003 	qdf_size_t tmplen;
2004 	QDF_STATUS ret;
2005 	uint8_t linkid = 0xFF;
2006 
2007 	if (!frame) {
2008 		mlo_err("Pointer to original frame is NULL");
2009 		return QDF_STATUS_E_NULL_VALUE;
2010 	}
2011 
2012 	if (!frame_len) {
2013 		mlo_err("Length of original frame is zero");
2014 		return QDF_STATUS_E_INVAL;
2015 	}
2016 
2017 	if ((subtype != WLAN_FC0_STYPE_ASSOC_REQ) &&
2018 	    (subtype != WLAN_FC0_STYPE_REASSOC_REQ) &&
2019 	    (subtype != WLAN_FC0_STYPE_ASSOC_RESP) &&
2020 	    (subtype != WLAN_FC0_STYPE_REASSOC_RESP) &&
2021 	    (subtype != WLAN_FC0_STYPE_PROBE_RESP)) {
2022 		mlo_err("802.11 frame subtype %u is invalid", subtype);
2023 		return QDF_STATUS_E_INVAL;
2024 	}
2025 
2026 	if (!link_frame) {
2027 		mlo_err("Pointer to secondary link specific frame is NULL");
2028 		return QDF_STATUS_E_NULL_VALUE;
2029 	}
2030 
2031 	if (!link_frame_maxsize) {
2032 		mlo_err("Maximum size of secondary link specific frame is zero");
2033 		return QDF_STATUS_E_INVAL;
2034 	}
2035 
2036 	if (!link_frame_len) {
2037 		mlo_err("Pointer to populated length of secondary link specific frame is NULL");
2038 		return QDF_STATUS_E_NULL_VALUE;
2039 	}
2040 
2041 	frame_iesection_offset = 0;
2042 
2043 	if (subtype == WLAN_FC0_STYPE_ASSOC_REQ) {
2044 		frame_iesection_offset = WLAN_ASSOC_REQ_IES_OFFSET;
2045 	} else if (subtype == WLAN_FC0_STYPE_REASSOC_REQ) {
2046 		frame_iesection_offset = WLAN_REASSOC_REQ_IES_OFFSET;
2047 	} else if (subtype == WLAN_FC0_STYPE_PROBE_RESP) {
2048 		frame_iesection_offset = WLAN_PROBE_RESP_IES_OFFSET;
2049 		if (frame_len < WLAN_TIMESTAMP_LEN) {
2050 			mlo_err("Frame length %zu is smaller than required timestamp length",
2051 				frame_len);
2052 			return QDF_STATUS_E_INVAL;
2053 		}
2054 		qdf_mem_copy(&tsf, frame, WLAN_TIMESTAMP_LEN);
2055 		tsf = qdf_le64_to_cpu(tsf);
2056 	} else {
2057 		/* This is a (re)association response */
2058 		frame_iesection_offset = WLAN_ASSOC_RSP_IES_OFFSET;
2059 	}
2060 
2061 	if (frame_len < frame_iesection_offset) {
2062 		/* The caller is supposed to have confirmed that this is a valid
2063 		 * frame containing a Multi-Link element. Hence we treat this as
2064 		 * a case of invalid argument being passed to us.
2065 		 */
2066 		mlo_err("Frame length %zu is smaller than the IE section offset %zu for subtype %u",
2067 			frame_len, frame_iesection_offset, subtype);
2068 		return QDF_STATUS_E_INVAL;
2069 	}
2070 
2071 	frame_iesection_len = frame_len - frame_iesection_offset;
2072 
2073 	if (frame_iesection_len == 0) {
2074 		/* The caller is supposed to have confirmed that this is a valid
2075 		 * frame containing a Multi-Link element. Hence we treat this as
2076 		 * a case of invalid argument being passed to us.
2077 		 */
2078 		mlo_err("No space left in frame for IE section");
2079 		return QDF_STATUS_E_INVAL;
2080 	}
2081 
2082 	frame_iesection = frame + frame_iesection_offset;
2083 
2084 	mlieseq = NULL;
2085 	mlieseqlen = 0;
2086 
2087 	ret = util_find_mlie(frame_iesection, frame_iesection_len, &mlieseq,
2088 			     &mlieseqlen);
2089 	if (QDF_IS_STATUS_ERROR(ret))
2090 		return ret;
2091 
2092 	if (!mlieseq) {
2093 		/* The caller is supposed to have confirmed that a Multi-Link
2094 		 * element is present in the frame. Hence we treat this as a
2095 		 * case of invalid argument being passed to us.
2096 		 */
2097 		mlo_err("Invalid original frame since no Multi-Link element found");
2098 		return QDF_STATUS_E_INVAL;
2099 	}
2100 
2101 	/* Sanity check the Multi-Link element sequence length */
2102 	if (!mlieseqlen) {
2103 		mlo_err("Length of Multi-Link element sequence is zero. Investigate.");
2104 		return QDF_STATUS_E_FAILURE;
2105 	}
2106 
2107 	if (mlieseqlen < sizeof(struct wlan_ie_multilink)) {
2108 		mlo_err_rl("Multi-Link element sequence length %zu octets is smaller than required for the fixed portion of Multi-Link element (%zu octets)",
2109 			   mlieseqlen, sizeof(struct wlan_ie_multilink));
2110 		return QDF_STATUS_E_PROTO;
2111 	}
2112 
2113 	ret = util_get_mlie_variant(mlieseq, mlieseqlen, (int *)&variant);
2114 	if (QDF_IS_STATUS_ERROR(ret))
2115 		return ret;
2116 
2117 	if (variant != WLAN_ML_VARIANT_BASIC) {
2118 		mlo_err_rl("Unexpected variant %u of Multi-Link element.",
2119 			   variant);
2120 		return QDF_STATUS_E_PROTO;
2121 	}
2122 
2123 	mlieseqpayloadlen = 0;
2124 	tmplen = 0;
2125 	is_elemfragseq = false;
2126 
2127 	ret = wlan_get_elem_fragseq_info(mlieseq,
2128 					 mlieseqlen,
2129 					 &is_elemfragseq,
2130 					 &tmplen,
2131 					 &mlieseqpayloadlen);
2132 	if (QDF_IS_STATUS_ERROR(ret))
2133 		return ret;
2134 
2135 	if (is_elemfragseq) {
2136 		if (tmplen != mlieseqlen) {
2137 			mlo_err_rl("Mismatch in values of element fragment sequence total length. Val per frag info determination: %zu octets, val per Multi-Link element search: %zu octets",
2138 				   tmplen, mlieseqlen);
2139 			return QDF_STATUS_E_FAILURE;
2140 		}
2141 
2142 		if (!mlieseqpayloadlen) {
2143 			mlo_err_rl("Multi-Link element fragment sequence payload is reported as 0, investigate");
2144 			return QDF_STATUS_E_FAILURE;
2145 		}
2146 
2147 		mlo_debug("ML IE fragment sequence found with payload len %zu",
2148 			  mlieseqpayloadlen);
2149 	} else {
2150 		if (mlieseqlen > (sizeof(struct ie_header) + WLAN_MAX_IE_LEN)) {
2151 			mlo_err_rl("Expected presence of valid fragment sequence since Multi-Link element sequence length %zu octets is larger than frag threshold of %zu octets, however no valid fragment sequence found",
2152 				   mlieseqlen,
2153 				   sizeof(struct ie_header) + WLAN_MAX_IE_LEN);
2154 			return QDF_STATUS_E_FAILURE;
2155 		}
2156 
2157 		mlieseqpayloadlen = mlieseqlen - (sizeof(struct ie_header) + 1);
2158 	}
2159 
2160 	mlieseqpayload_copy = qdf_mem_malloc(mlieseqpayloadlen);
2161 
2162 	if (!mlieseqpayload_copy) {
2163 		mlo_err_rl("Could not allocate memory for Multi-Link element payload copy");
2164 		ret = QDF_STATUS_E_NOMEM;
2165 		goto mem_free;
2166 	}
2167 
2168 	if (is_elemfragseq) {
2169 		ret = wlan_defrag_elem_fragseq(false,
2170 					       mlieseq,
2171 					       mlieseqlen,
2172 					       mlieseqpayload_copy,
2173 					       mlieseqpayloadlen,
2174 					       &defragpayload_len);
2175 		if (QDF_IS_STATUS_ERROR(ret))
2176 			goto mem_free;
2177 
2178 		if (defragpayload_len != mlieseqpayloadlen) {
2179 			mlo_err_rl("Length of de-fragmented payload %zu octets is not equal to length of Multi-Link element fragment sequence payload %zu octets",
2180 				   defragpayload_len, mlieseqpayloadlen);
2181 			ret = QDF_STATUS_E_FAILURE;
2182 			goto mem_free;
2183 		}
2184 	} else {
2185 		qdf_mem_copy(mlieseqpayload_copy,
2186 			     mlieseq + sizeof(struct ie_header) + 1,
2187 			     mlieseqpayloadlen);
2188 	}
2189 
2190 	link_info = NULL;
2191 	link_info_len = 0;
2192 
2193 	ret = util_parse_multi_link_ctrl(mlieseqpayload_copy,
2194 					 mlieseqpayloadlen,
2195 					 &link_info,
2196 					 &link_info_len);
2197 	if (QDF_IS_STATUS_ERROR(ret))
2198 		goto mem_free;
2199 
2200 	/* As per the standard, the sender must include Link Info for
2201 	 * association request/response. Throw an error if we are unable to
2202 	 * obtain this.
2203 	 */
2204 	if (!link_info) {
2205 		mlo_err_rl("Unable to successfully obtain Link Info");
2206 		ret = QDF_STATUS_E_PROTO;
2207 		goto mem_free;
2208 	}
2209 
2210 	/* Note: We may have a future change to skip subelements which are not
2211 	 * Per-STA Profile, handle more than two links in MLO, handle cases
2212 	 * where we unexpectedly find more Per-STA Profiles than expected, etc.
2213 	 */
2214 
2215 	persta_prof = NULL;
2216 	persta_prof_bufflen = 0;
2217 
2218 	ret = util_find_bvmlie_persta_prof_for_linkid(req_link_id,
2219 						      link_info,
2220 						      link_info_len,
2221 						      &persta_prof,
2222 						      &persta_prof_bufflen);
2223 
2224 	if (QDF_IS_STATUS_ERROR(ret)) {
2225 		mlo_err_rl("Per STA profile not found for link id %d",
2226 			   req_link_id);
2227 		goto mem_free;
2228 	}
2229 
2230 	sta_prof_remlen = 0;
2231 	sta_prof_currpos = NULL;
2232 	is_reportedmacaddr_valid = false;
2233 	is_beaconinterval_valid = false;
2234 	is_completeprofile = false;
2235 	is_tsfoffset_valid = false;
2236 
2237 	/* Parse per-STA profile */
2238 	ret = util_parse_bvmlie_perstaprofile_stactrl(persta_prof +
2239 						      sizeof(struct subelem_header),
2240 						      persta_prof_bufflen,
2241 						      &linkid,
2242 						      &beaconinterval,
2243 						      &is_beaconinterval_valid,
2244 						      &tsfoffset,
2245 						      &is_tsfoffset_valid,
2246 						      &is_completeprofile,
2247 						      &is_reportedmacaddr_valid,
2248 						      &reportedmacaddr,
2249 						      true,
2250 						      &sta_prof_currpos,
2251 						      &sta_prof_remlen,
2252 						      NULL,
2253 						      NULL);
2254 	if (QDF_IS_STATUS_ERROR(ret))
2255 		goto mem_free;
2256 
2257 	if (subtype == WLAN_FC0_STYPE_PROBE_RESP && !is_completeprofile) {
2258 		mlo_err("Complete profile information is not present in per-STA profile of probe response frame");
2259 		ret = QDF_STATUS_E_NOSUPPORT;
2260 		goto mem_free;
2261 	}
2262 
2263 	/* We double check for a NULL STA Profile, though the helper function
2264 	 * above would have taken care of this. We need to get a non-NULL STA
2265 	 * profile, because we need to get at least the expected fixed fields,
2266 	 * even if there is an (improbable) total inheritance.
2267 	 */
2268 	if (!sta_prof_currpos) {
2269 		mlo_err_rl("STA profile is NULL");
2270 		ret = QDF_STATUS_E_PROTO;
2271 		goto mem_free;
2272 	}
2273 
2274 	/* As per the standard, the sender sets the MAC address in the per-STA
2275 	 * profile in association request/response. Without this, we cannot
2276 	 * generate the link specific frame.
2277 	 */
2278 	if (!is_reportedmacaddr_valid) {
2279 		mlo_err_rl("Unable to get MAC address from per-STA profile");
2280 		ret = QDF_STATUS_E_PROTO;
2281 		goto mem_free;
2282 	}
2283 
2284 	link_frame_currpos = link_frame;
2285 	*link_frame_len = 0;
2286 	link_frame_currlen = 0;
2287 
2288 	if (link_frame_maxsize < WLAN_MAC_HDR_LEN_3A) {
2289 		mlo_err("Insufficient space in link specific frame for 802.11 header. Required: %u octets, available: %zu octets",
2290 			WLAN_MAC_HDR_LEN_3A, link_frame_maxsize);
2291 
2292 		ret = QDF_STATUS_E_NOMEM;
2293 		goto mem_free;
2294 	}
2295 
2296 	link_frame_currpos += WLAN_MAC_HDR_LEN_3A;
2297 	link_frame_currlen += WLAN_MAC_HDR_LEN_3A;
2298 
2299 	if ((subtype == WLAN_FC0_STYPE_ASSOC_REQ) ||
2300 	    (subtype == WLAN_FC0_STYPE_REASSOC_REQ)) {
2301 		mlo_debug("Populating fixed fields for (re)assoc req in link specific frame");
2302 
2303 		if (sta_prof_remlen < WLAN_CAPABILITYINFO_LEN) {
2304 			mlo_err_rl("Remaining length of STA profile %zu octets is less than length of Capability Info %u",
2305 				   sta_prof_remlen,
2306 				   WLAN_CAPABILITYINFO_LEN);
2307 
2308 			ret = QDF_STATUS_E_PROTO;
2309 			goto mem_free;
2310 		}
2311 
2312 		/* Capability information is specific to the link. Copy this
2313 		 * from the STA profile.
2314 		 */
2315 
2316 		if ((link_frame_maxsize - link_frame_currlen) <
2317 				WLAN_CAPABILITYINFO_LEN) {
2318 			mlo_err("Insufficient space in link specific frame for Capability Info field. Required: %u octets, available: %zu octets",
2319 				WLAN_CAPABILITYINFO_LEN,
2320 				(link_frame_maxsize - link_frame_currlen));
2321 
2322 			ret = QDF_STATUS_E_NOMEM;
2323 			goto mem_free;
2324 		}
2325 
2326 		qdf_mem_copy(link_frame_currpos, sta_prof_currpos,
2327 			     WLAN_CAPABILITYINFO_LEN);
2328 		link_frame_currpos += WLAN_CAPABILITYINFO_LEN;
2329 		link_frame_currlen += WLAN_CAPABILITYINFO_LEN;
2330 
2331 		sta_prof_currpos += WLAN_CAPABILITYINFO_LEN;
2332 		sta_prof_remlen -= WLAN_CAPABILITYINFO_LEN;
2333 
2334 		/* Listen Interval is common between all links. Copy this from
2335 		 * the reporting section of the frame.
2336 		 */
2337 
2338 		if ((link_frame_maxsize - link_frame_currlen) <
2339 				WLAN_LISTENINTERVAL_LEN) {
2340 			mlo_err("Insufficient space in link specific frame for Listen Interval field. Required: %u octets, available: %zu octets",
2341 				WLAN_LISTENINTERVAL_LEN,
2342 				(link_frame_maxsize - link_frame_currlen));
2343 
2344 			ret = QDF_STATUS_E_NOMEM;
2345 			goto mem_free;
2346 		}
2347 
2348 		qdf_mem_copy(link_frame_currpos,
2349 			     frame + WLAN_CAPABILITYINFO_LEN,
2350 			     WLAN_LISTENINTERVAL_LEN);
2351 		link_frame_currpos += WLAN_LISTENINTERVAL_LEN;
2352 		link_frame_currlen += WLAN_LISTENINTERVAL_LEN;
2353 
2354 		if (subtype == WLAN_FC0_STYPE_REASSOC_REQ) {
2355 			/* Current AP address is common between all links. Copy
2356 			 * this from the reporting section of the frame.
2357 			 */
2358 			if ((link_frame_maxsize - link_frame_currlen) <
2359 				QDF_MAC_ADDR_SIZE) {
2360 				mlo_err("Insufficient space in link specific frame for current AP address. Required: %u octets, available: %zu octets",
2361 					QDF_MAC_ADDR_SIZE,
2362 					(link_frame_maxsize -
2363 						link_frame_currlen));
2364 
2365 				ret = QDF_STATUS_E_NOMEM;
2366 				goto mem_free;
2367 			}
2368 
2369 			qdf_mem_copy(link_frame_currpos,
2370 				     frame + WLAN_CAPABILITYINFO_LEN +
2371 						WLAN_LISTENINTERVAL_LEN,
2372 				     QDF_MAC_ADDR_SIZE);
2373 			link_frame_currpos += QDF_MAC_ADDR_SIZE;
2374 			link_frame_currlen += QDF_MAC_ADDR_SIZE;
2375 			mlo_debug("Reassoc req: Added Current AP address field (%u octets) to link specific frame",
2376 				  QDF_MAC_ADDR_SIZE);
2377 		}
2378 	} else if (subtype == WLAN_FC0_STYPE_ASSOC_RESP ||
2379 		   subtype == WLAN_FC0_STYPE_REASSOC_RESP) {
2380 		/* This is a (re)association response */
2381 		mlo_debug("Populating fixed fields for (re)assoc resp in link specific frame");
2382 
2383 		if (sta_prof_remlen <
2384 			(WLAN_CAPABILITYINFO_LEN + WLAN_STATUSCODE_LEN)) {
2385 			mlo_err_rl("Remaining length of STA profile %zu octets is less than length of Capability Info + length of Status Code %u",
2386 				   sta_prof_remlen,
2387 				   WLAN_CAPABILITYINFO_LEN +
2388 					WLAN_STATUSCODE_LEN);
2389 
2390 			ret = QDF_STATUS_E_PROTO;
2391 			goto mem_free;
2392 		}
2393 
2394 		/* Capability information and Status Code are specific to the
2395 		 * link. Copy these from the STA profile.
2396 		 */
2397 
2398 		if ((link_frame_maxsize - link_frame_currlen) <
2399 			(WLAN_CAPABILITYINFO_LEN + WLAN_STATUSCODE_LEN)) {
2400 			mlo_err("Insufficient space in link specific frame for Capability Info and Status Code fields. Required: %u octets, available: %zu octets",
2401 				WLAN_CAPABILITYINFO_LEN + WLAN_STATUSCODE_LEN,
2402 				(link_frame_maxsize - link_frame_currlen));
2403 
2404 			ret = QDF_STATUS_E_NOMEM;
2405 			goto mem_free;
2406 
2407 		}
2408 
2409 		qdf_mem_copy(link_frame_currpos, sta_prof_currpos,
2410 			     (WLAN_CAPABILITYINFO_LEN + WLAN_STATUSCODE_LEN));
2411 		link_frame_currpos += (WLAN_CAPABILITYINFO_LEN +
2412 						WLAN_STATUSCODE_LEN);
2413 		link_frame_currlen += (WLAN_CAPABILITYINFO_LEN +
2414 				WLAN_STATUSCODE_LEN);
2415 		mlo_debug("Added Capability Info and Status Code fields (%u octets) to link specific frame",
2416 			  WLAN_CAPABILITYINFO_LEN + WLAN_STATUSCODE_LEN);
2417 
2418 		sta_prof_currpos += (WLAN_CAPABILITYINFO_LEN +
2419 				WLAN_STATUSCODE_LEN);
2420 		sta_prof_remlen -= (WLAN_CAPABILITYINFO_LEN +
2421 				WLAN_STATUSCODE_LEN);
2422 
2423 		/* AID is common between all links. Copy this from the original
2424 		 * frame.
2425 		 */
2426 
2427 		if ((link_frame_maxsize - link_frame_currlen) < WLAN_AID_LEN) {
2428 			mlo_err("Insufficient space in link specific frame for AID field. Required: %u octets, available: %zu octets",
2429 				WLAN_AID_LEN,
2430 				(link_frame_maxsize - link_frame_currlen));
2431 
2432 			ret = QDF_STATUS_E_NOMEM;
2433 			goto mem_free;
2434 		}
2435 
2436 		qdf_mem_copy(link_frame_currpos,
2437 			     frame + WLAN_CAPABILITYINFO_LEN +
2438 					WLAN_STATUSCODE_LEN,
2439 			     WLAN_AID_LEN);
2440 		link_frame_currpos += WLAN_AID_LEN;
2441 		link_frame_currlen += WLAN_AID_LEN;
2442 		mlo_debug("Added AID field (%u octets) to link specific frame",
2443 			  WLAN_AID_LEN);
2444 	} else if (subtype == WLAN_FC0_STYPE_PROBE_RESP) {
2445 		/* This is a probe response */
2446 		mlo_debug("Populating fixed fields for probe response in link specific frame");
2447 
2448 		if ((link_frame_maxsize - link_frame_currlen) <
2449 				WLAN_TIMESTAMP_LEN) {
2450 			mlo_err("Insufficient space in link specific frame for Timestamp Info field. Required: %u octets, available: %zu octets",
2451 				WLAN_TIMESTAMP_LEN,
2452 				(link_frame_maxsize - link_frame_currlen));
2453 
2454 			ret = QDF_STATUS_E_NOMEM;
2455 			goto mem_free;
2456 		}
2457 
2458 		/* Per spec 11be_D2.1.1, the TSF Offset subfield of the STA Info
2459 		 * field indicates the offset (Toffset)between the TSF timer of
2460 		 * the reported AP (TA) and the TSF timer of the reporting
2461 		 * AP (TB) and is encoded as a 2s complement signed integer
2462 		 * with units of 2 µs. Toffset is calculated as
2463 		 * Toffset= Floor((TA – TB)/2).
2464 		 */
2465 		if (is_tsfoffset_valid)
2466 			tsf += tsfoffset * 2;
2467 
2468 		qdf_mem_copy(link_frame_currpos, &tsf, WLAN_TIMESTAMP_LEN);
2469 		link_frame_currpos += WLAN_TIMESTAMP_LEN;
2470 		link_frame_currlen += WLAN_TIMESTAMP_LEN;
2471 
2472 		if (!is_beaconinterval_valid) {
2473 			mlo_err_rl("Beacon interval information not present in STA info field of per-STA profile");
2474 			ret = QDF_STATUS_E_PROTO;
2475 			goto mem_free;
2476 		}
2477 
2478 		/* Beacon Interval information copy this from
2479 		 * the STA info field.
2480 		 */
2481 		if ((link_frame_maxsize - link_frame_currlen) <
2482 				WLAN_BEACONINTERVAL_LEN) {
2483 			mlo_err("Insufficient space in link specific frame for Beacon Interval Info field. Required: %u octets, available: %zu octets",
2484 				WLAN_BEACONINTERVAL_LEN,
2485 				(link_frame_maxsize - link_frame_currlen));
2486 
2487 			ret = QDF_STATUS_E_NOMEM;
2488 			goto mem_free;
2489 		}
2490 
2491 		qdf_mem_copy(link_frame_currpos, &beaconinterval,
2492 			     WLAN_BEACONINTERVAL_LEN);
2493 		link_frame_currpos += WLAN_BEACONINTERVAL_LEN;
2494 		link_frame_currlen += WLAN_BEACONINTERVAL_LEN;
2495 
2496 		if (sta_prof_remlen < WLAN_CAPABILITYINFO_LEN) {
2497 			mlo_err_rl("Remaining length of STA profile %zu octets is less than length of Capability Info %u",
2498 				   sta_prof_remlen,
2499 				   WLAN_CAPABILITYINFO_LEN);
2500 
2501 			ret = QDF_STATUS_E_PROTO;
2502 			goto mem_free;
2503 		}
2504 
2505 		/* Capability information is specific to the link. Copy this
2506 		 * from the STA profile.
2507 		 */
2508 
2509 		if ((link_frame_maxsize - link_frame_currlen) <
2510 				WLAN_CAPABILITYINFO_LEN) {
2511 			mlo_err("Insufficient space in link specific frame for Capability Info field. Required: %u octets, available: %zu octets",
2512 				WLAN_CAPABILITYINFO_LEN,
2513 				(link_frame_maxsize - link_frame_currlen));
2514 
2515 			ret = QDF_STATUS_E_NOMEM;
2516 			goto mem_free;
2517 		}
2518 
2519 		qdf_mem_copy(link_frame_currpos, sta_prof_currpos,
2520 			     WLAN_CAPABILITYINFO_LEN);
2521 		link_frame_currpos += WLAN_CAPABILITYINFO_LEN;
2522 		link_frame_currlen += WLAN_CAPABILITYINFO_LEN;
2523 
2524 		sta_prof_currpos += WLAN_CAPABILITYINFO_LEN;
2525 		sta_prof_remlen -= WLAN_CAPABILITYINFO_LEN;
2526 	}
2527 
2528 	sta_prof_iesection = sta_prof_currpos;
2529 	sta_prof_iesection_len = sta_prof_remlen;
2530 
2531 	/* Populate non-inheritance lists if applicable */
2532 	ninherit_elemlist_len = 0;
2533 	ninherit_elemlist = NULL;
2534 	ninherit_elemextlist_len = 0;
2535 	ninherit_elemextlist = NULL;
2536 
2537 	ret = util_get_noninheritlists(sta_prof_iesection,
2538 				       sta_prof_iesection_len,
2539 				       &ninherit_elemlist,
2540 				       &ninherit_elemlist_len,
2541 				       &ninherit_elemextlist,
2542 				       &ninherit_elemextlist_len);
2543 	if (QDF_IS_STATUS_ERROR(ret))
2544 		goto mem_free;
2545 
2546 	/* Go through IEs of the reporting STA, and those in STA profile, merge
2547 	 * them into link_frame (except for elements in the Non-Inheritance
2548 	 * list).
2549 	 *
2550 	 * Note: Currently, only 2-link MLO is supported here. We may have a
2551 	 * future change to expand to more links.
2552 	 */
2553 	reportingsta_ie = util_find_eid(WLAN_ELEMID_SSID, frame_iesection,
2554 					frame_iesection_len);
2555 
2556 	if ((subtype == WLAN_FC0_STYPE_ASSOC_REQ) ||
2557 	    (subtype == WLAN_FC0_STYPE_REASSOC_REQ) ||
2558 	    (subtype == WLAN_FC0_STYPE_PROBE_RESP)) {
2559 		/* Sanity check that the SSID element is present for the
2560 		 * reporting STA. There is no stipulation in the standard for
2561 		 * the STA profile in this regard, so we do not check the STA
2562 		 * profile for the SSID element.
2563 		 */
2564 		if (!reportingsta_ie) {
2565 			mlo_err_rl("SSID element not found in reporting STA of the frame.");
2566 			ret = QDF_STATUS_E_PROTO;
2567 			goto mem_free;
2568 		}
2569 	} else {
2570 		/* This is a (re)association response. Sanity check that the
2571 		 * SSID element is present neither for the reporting STA nor in
2572 		 * the STA profile.
2573 		 */
2574 		if (reportingsta_ie) {
2575 			mlo_err_rl("SSID element found for reporting STA for (re)association response. It should not be present.");
2576 			ret = QDF_STATUS_E_PROTO;
2577 			goto mem_free;
2578 		}
2579 
2580 		sta_prof_ie = util_find_eid(WLAN_ELEMID_SSID,
2581 					    sta_prof_iesection,
2582 					    sta_prof_iesection_len);
2583 
2584 		if (sta_prof_ie) {
2585 			mlo_err_rl("SSID element found in STA profile for (re)association response. It should not be present.");
2586 			ret = QDF_STATUS_E_PROTO;
2587 			goto mem_free;
2588 		}
2589 	}
2590 
2591 	reportingsta_ie = reportingsta_ie ? reportingsta_ie : frame_iesection;
2592 
2593 	ret = util_validate_reportingsta_ie(reportingsta_ie, frame_iesection,
2594 					    frame_iesection_len);
2595 	if (QDF_IS_STATUS_ERROR(ret))
2596 		goto mem_free;
2597 
2598 	reportingsta_ie_size = reportingsta_ie[TAG_LEN_POS] + MIN_IE_LEN;
2599 
2600 	while (((reportingsta_ie + reportingsta_ie_size) - frame_iesection)
2601 			<= frame_iesection_len) {
2602 		/* Skip Multi-Link element */
2603 		if ((reportingsta_ie[ID_POS] == WLAN_ELEMID_EXTN_ELEM) &&
2604 		    (reportingsta_ie[IDEXT_POS] ==
2605 				WLAN_EXTN_ELEMID_MULTI_LINK)) {
2606 			if (((reportingsta_ie + reportingsta_ie_size) -
2607 					frame_iesection) == frame_iesection_len)
2608 				break;
2609 
2610 			/* Add BV ML IE for link specific probe response */
2611 			if (subtype == WLAN_FC0_STYPE_PROBE_RESP) {
2612 				ret = util_add_mlie_for_prb_rsp_gen(
2613 					reportingsta_ie,
2614 					reportingsta_ie[TAG_LEN_POS],
2615 					&link_frame_currpos,
2616 					&link_frame_currlen,
2617 					link_frame_maxsize,
2618 					linkid);
2619 				if (QDF_IS_STATUS_ERROR(ret))
2620 					goto mem_free;
2621 			}
2622 			reportingsta_ie += reportingsta_ie_size;
2623 
2624 			ret = util_validate_reportingsta_ie(reportingsta_ie,
2625 							    frame_iesection,
2626 							    frame_iesection_len);
2627 			if (QDF_IS_STATUS_ERROR(ret))
2628 				goto mem_free;
2629 
2630 			reportingsta_ie_size = reportingsta_ie[TAG_LEN_POS] +
2631 				MIN_IE_LEN;
2632 
2633 			continue;
2634 		}
2635 
2636 		sta_prof_ie = NULL;
2637 		sta_prof_ie_size = 0;
2638 
2639 		if (sta_prof_iesection_len) {
2640 			if (reportingsta_ie[ID_POS] == WLAN_ELEMID_EXTN_ELEM) {
2641 				sta_prof_ie = (uint8_t *)util_find_extn_eid(reportingsta_ie[ID_POS],
2642 									    reportingsta_ie[IDEXT_POS],
2643 									    sta_prof_iesection,
2644 									    sta_prof_iesection_len);
2645 			} else {
2646 				sta_prof_ie = (uint8_t *)util_find_eid(reportingsta_ie[ID_POS],
2647 								       sta_prof_iesection,
2648 								       sta_prof_iesection_len);
2649 			}
2650 		}
2651 
2652 		if (!sta_prof_ie) {
2653 			/* IE is present for reporting STA, but not in STA
2654 			 * profile.
2655 			 */
2656 
2657 			is_in_noninheritlist = false;
2658 
2659 			ret = util_eval_ie_in_noninheritlist((uint8_t *)reportingsta_ie,
2660 							     reportingsta_ie_size,
2661 							     ninherit_elemlist,
2662 							     ninherit_elemlist_len,
2663 							     ninherit_elemextlist,
2664 							     ninherit_elemextlist_len,
2665 							     &is_in_noninheritlist);
2666 
2667 			if (QDF_IS_STATUS_ERROR(ret))
2668 				goto mem_free;
2669 
2670 			if (!is_in_noninheritlist) {
2671 				if ((link_frame_currpos +
2672 						reportingsta_ie_size) <=
2673 					(link_frame + link_frame_maxsize)) {
2674 					qdf_mem_copy(link_frame_currpos,
2675 						     reportingsta_ie,
2676 						     reportingsta_ie_size);
2677 
2678 					link_frame_currpos +=
2679 						reportingsta_ie_size;
2680 					link_frame_currlen +=
2681 						reportingsta_ie_size;
2682 
2683 					if (reportingsta_ie[ID_POS] == WLAN_ELEMID_EXTN_ELEM) {
2684 						mlo_etrace_debug("IE with element ID : %u extension element ID : %u (%zu octets) present for reporting STA but not in STA profile. Copied IE from reporting frame to link specific frame",
2685 							      reportingsta_ie[ID_POS],
2686 							      reportingsta_ie[IDEXT_POS],
2687 							      reportingsta_ie_size);
2688 					} else {
2689 						mlo_etrace_debug("IE with element ID : %u (%zu octets) present for reporting STA but not in STA profile. Copied IE from reporting frame to link specific frame",
2690 							      reportingsta_ie[ID_POS],
2691 							      reportingsta_ie_size);
2692 					}
2693 				} else {
2694 					if (reportingsta_ie[ID_POS] == WLAN_ELEMID_EXTN_ELEM) {
2695 						mlo_etrace_err_rl("Insufficient space in link specific frame for IE with element ID : %u extension element ID : %u. Required: %zu octets, available: %zu octets",
2696 							       reportingsta_ie[ID_POS],
2697 							       reportingsta_ie[IDEXT_POS],
2698 							       reportingsta_ie_size,
2699 							       link_frame_maxsize -
2700 							       link_frame_currlen);
2701 					} else {
2702 						mlo_etrace_err_rl("Insufficient space in link specific frame for IE with element ID : %u. Required: %zu octets, available: %zu octets",
2703 							       reportingsta_ie[ID_POS],
2704 							       reportingsta_ie_size,
2705 							       link_frame_maxsize -
2706 							       link_frame_currlen);
2707 					}
2708 
2709 					ret = QDF_STATUS_E_NOMEM;
2710 					goto mem_free;
2711 				}
2712 			} else {
2713 				if (reportingsta_ie[ID_POS] == WLAN_ELEMID_EXTN_ELEM) {
2714 					mlo_etrace_debug("IE with element ID : %u extension element ID : %u (%zu octets) present for reporting STA but not in STA profile. However it is in Non-Inheritance list, hence ignoring.",
2715 						      reportingsta_ie[ID_POS],
2716 						      reportingsta_ie[IDEXT_POS],
2717 						      reportingsta_ie_size);
2718 				} else {
2719 					mlo_etrace_debug("IE with element ID : %u (%zu octets) present for reporting STA but not in STA profile. However it is in Non-Inheritance list, hence ignoring.",
2720 						      reportingsta_ie[ID_POS],
2721 						      reportingsta_ie_size);
2722 				}
2723 			}
2724 		} else {
2725 			/* IE is present for reporting STA and also in STA
2726 			 * profile, copy from STA profile and flag the IE in STA
2727 			 * profile as copied (by setting EID field to 0). The
2728 			 * SSID element (with EID 0) is processed first to
2729 			 * enable this. For vendor IE, compare OUI + type +
2730 			 * subType to determine if they are the same IE.
2731 			 */
2732 			/* Note: This may be revisited in a future change, to
2733 			 * adhere to provisions in the standard for multiple
2734 			 * occurrences of a given element ID/extension element
2735 			 * ID.
2736 			 */
2737 
2738 			ret = util_validate_sta_prof_ie(sta_prof_ie,
2739 							sta_prof_iesection,
2740 							sta_prof_iesection_len);
2741 			if (QDF_IS_STATUS_ERROR(ret))
2742 				goto mem_free;
2743 
2744 			sta_prof_ie_size = sta_prof_ie[TAG_LEN_POS] +
2745 				MIN_IE_LEN;
2746 
2747 			sta_prof_iesection_remlen =
2748 				sta_prof_iesection_len -
2749 					(sta_prof_ie - sta_prof_iesection);
2750 
2751 			if ((reportingsta_ie[ID_POS] == WLAN_ELEMID_VENDOR) &&
2752 			    (sta_prof_iesection_remlen >= MIN_VENDOR_TAG_LEN)) {
2753 				/* If Vendor IE also presents in STA profile,
2754 				 * then ignore the Vendor IE which is for
2755 				 * reporting STA. It only needs to copy Vendor
2756 				 * IE from STA profile to link specific frame.
2757 				 * The copy happens when going through the
2758 				 * remaining IEs.
2759 				 */
2760 				;
2761 			} else {
2762 				/* Copy IE from STA profile into link specific
2763 				 * frame.
2764 				 */
2765 				if ((link_frame_currpos + sta_prof_ie_size) <=
2766 					(link_frame + link_frame_maxsize)) {
2767 					qdf_mem_copy(link_frame_currpos,
2768 						     sta_prof_ie,
2769 						     sta_prof_ie_size);
2770 
2771 					link_frame_currpos += sta_prof_ie_size;
2772 					link_frame_currlen +=
2773 						sta_prof_ie_size;
2774 
2775 					if (reportingsta_ie[ID_POS] ==
2776 							WLAN_ELEMID_EXTN_ELEM) {
2777 						mlo_etrace_debug("IE with element ID : %u extension element ID : %u (%zu octets) for reporting STA also present in STA profile. Copied IE from STA profile to link specific frame",
2778 							      sta_prof_ie[ID_POS],
2779 							      sta_prof_ie[IDEXT_POS],
2780 							      sta_prof_ie_size);
2781 					} else {
2782 						mlo_etrace_debug("IE with element ID : %u (%zu octets) for reporting STA also present in STA profile. Copied IE from STA profile to link specific frame",
2783 								 sta_prof_ie[ID_POS],
2784 								 sta_prof_ie_size);
2785 					}
2786 
2787 					sta_prof_ie[0] = 0;
2788 				} else {
2789 					if (sta_prof_ie[ID_POS] ==
2790 							WLAN_ELEMID_EXTN_ELEM) {
2791 						mlo_etrace_err_rl("Insufficient space in link specific frame for IE with element ID : %u extension element ID : %u. Required: %zu octets, available: %zu octets",
2792 								  sta_prof_ie[ID_POS],
2793 								  sta_prof_ie[IDEXT_POS],
2794 								  sta_prof_ie_size,
2795 								  link_frame_maxsize -
2796 								  link_frame_currlen);
2797 					} else {
2798 						mlo_etrace_err_rl("Insufficient space in link specific frame for IE with element ID : %u. Required: %zu octets, available: %zu octets",
2799 								  sta_prof_ie[ID_POS],
2800 								  sta_prof_ie_size,
2801 								  link_frame_maxsize -
2802 								  link_frame_currlen);
2803 					}
2804 
2805 					ret = QDF_STATUS_E_NOMEM;
2806 					goto mem_free;
2807 				}
2808 			}
2809 		}
2810 
2811 		if (((reportingsta_ie + reportingsta_ie_size) -
2812 					frame_iesection) == frame_iesection_len)
2813 			break;
2814 
2815 		reportingsta_ie += reportingsta_ie_size;
2816 
2817 		ret = util_validate_reportingsta_ie(reportingsta_ie,
2818 						    frame_iesection,
2819 						    frame_iesection_len);
2820 		if (QDF_IS_STATUS_ERROR(ret))
2821 			goto mem_free;
2822 
2823 		reportingsta_ie_size = reportingsta_ie[TAG_LEN_POS] +
2824 			MIN_IE_LEN;
2825 	}
2826 
2827 	/* Go through the remaining unprocessed IEs in STA profile and copy them
2828 	 * to the link specific frame. The processed ones are marked with 0 in
2829 	 * the first octet. The first octet corresponds to the element ID. In
2830 	 * the case of (re)association request, the element with actual ID
2831 	 * WLAN_ELEMID_SSID(0) has already been copied to the link specific
2832 	 * frame. In the case of (re)association response, it has been verified
2833 	 * that the element with actual ID WLAN_ELEMID_SSID(0) is present
2834 	 * neither for the reporting STA nor in the STA profile.
2835 	 */
2836 	sta_prof_iesection_currpos = sta_prof_iesection;
2837 	sta_prof_iesection_remlen = sta_prof_iesection_len;
2838 
2839 	while (sta_prof_iesection_remlen > 0) {
2840 		sta_prof_ie = sta_prof_iesection_currpos;
2841 		ret = util_validate_sta_prof_ie(sta_prof_ie,
2842 						sta_prof_iesection_currpos,
2843 						sta_prof_iesection_remlen);
2844 		if (QDF_IS_STATUS_ERROR(ret))
2845 			goto mem_free;
2846 
2847 		sta_prof_ie_size = sta_prof_ie[TAG_LEN_POS] + MIN_IE_LEN;
2848 
2849 		if (!sta_prof_ie[0]) {
2850 			/* Skip this, since it has already been processed */
2851 			sta_prof_iesection_currpos += sta_prof_ie_size;
2852 			sta_prof_iesection_remlen -= sta_prof_ie_size;
2853 			continue;
2854 		}
2855 
2856 		/* Copy IE from STA profile into link specific frame. */
2857 		if ((link_frame_currpos + sta_prof_ie_size) <=
2858 			(link_frame + link_frame_maxsize)) {
2859 			qdf_mem_copy(link_frame_currpos,
2860 				     sta_prof_ie,
2861 				     sta_prof_ie_size);
2862 
2863 			link_frame_currpos += sta_prof_ie_size;
2864 			link_frame_currlen +=
2865 				sta_prof_ie_size;
2866 
2867 			if (reportingsta_ie[ID_POS] ==
2868 					WLAN_ELEMID_EXTN_ELEM) {
2869 				mlo_etrace_debug("IE with element ID : %u extension element ID : %u (%zu octets) is present only in STA profile. Copied IE from STA profile to link specific frame",
2870 						 sta_prof_ie[ID_POS],
2871 						 sta_prof_ie[IDEXT_POS],
2872 						 sta_prof_ie_size);
2873 			} else {
2874 				mlo_etrace_debug("IE with element ID : %u (%zu octets) is present only in STA profile. Copied IE from STA profile to link specific frame",
2875 						 sta_prof_ie[ID_POS],
2876 						 sta_prof_ie_size);
2877 			}
2878 
2879 			sta_prof_ie[0] = 0;
2880 		} else {
2881 			if (sta_prof_ie[ID_POS] == WLAN_ELEMID_EXTN_ELEM) {
2882 				mlo_etrace_err_rl("Insufficient space in link specific frame for IE with element ID : %u extension element ID : %u. Required: %zu octets, available: %zu octets",
2883 						  sta_prof_ie[ID_POS],
2884 						  sta_prof_ie[IDEXT_POS],
2885 						  sta_prof_ie_size,
2886 						  link_frame_maxsize -
2887 						  link_frame_currlen);
2888 			} else {
2889 				mlo_etrace_err_rl("Insufficient space in link specific frame for IE with element ID : %u. Required: %zu octets, available: %zu octets",
2890 						  sta_prof_ie[ID_POS],
2891 						  sta_prof_ie_size,
2892 						  link_frame_maxsize -
2893 						  link_frame_currlen);
2894 			}
2895 
2896 			ret = QDF_STATUS_E_NOMEM;
2897 			goto mem_free;
2898 		}
2899 
2900 		sta_prof_iesection_currpos += sta_prof_ie_size;
2901 		sta_prof_iesection_remlen -= sta_prof_ie_size;
2902 	}
2903 
2904 	/* Copy the link MAC addr */
2905 	link_frame_hdr = (struct wlan_frame_hdr *)link_frame;
2906 
2907 	if ((subtype == WLAN_FC0_STYPE_ASSOC_REQ) ||
2908 	    (subtype == WLAN_FC0_STYPE_REASSOC_REQ)) {
2909 		qdf_mem_copy(link_frame_hdr->i_addr3, &link_addr,
2910 			     QDF_MAC_ADDR_SIZE);
2911 		qdf_mem_copy(link_frame_hdr->i_addr2, reportedmacaddr.bytes,
2912 			     QDF_MAC_ADDR_SIZE);
2913 		qdf_mem_copy(link_frame_hdr->i_addr1, &link_addr,
2914 			     QDF_MAC_ADDR_SIZE);
2915 
2916 		link_frame_hdr->i_fc[0] = MLO_LINKSPECIFIC_ASSOC_REQ_FC0;
2917 		link_frame_hdr->i_fc[1] = MLO_LINKSPECIFIC_ASSOC_REQ_FC1;
2918 	} else if (subtype == WLAN_FC0_STYPE_PROBE_RESP) {
2919 		qdf_mem_copy(link_frame_hdr->i_addr3, reportedmacaddr.bytes,
2920 			     QDF_MAC_ADDR_SIZE);
2921 		qdf_mem_copy(link_frame_hdr->i_addr2, reportedmacaddr.bytes,
2922 			     QDF_MAC_ADDR_SIZE);
2923 		qdf_mem_copy(link_frame_hdr->i_addr1, &link_addr,
2924 			     QDF_MAC_ADDR_SIZE);
2925 
2926 		link_frame_hdr->i_fc[0] = MLO_LINKSPECIFIC_PROBE_RESP_FC0;
2927 		link_frame_hdr->i_fc[1] = MLO_LINKSPECIFIC_PROBE_RESP_FC1;
2928 	} else {
2929 		/* This is a (re)association response */
2930 
2931 		qdf_mem_copy(link_frame_hdr->i_addr3, reportedmacaddr.bytes,
2932 			     QDF_MAC_ADDR_SIZE);
2933 		qdf_mem_copy(link_frame_hdr->i_addr2, reportedmacaddr.bytes,
2934 			     QDF_MAC_ADDR_SIZE);
2935 		qdf_mem_copy(link_frame_hdr->i_addr1, &link_addr,
2936 			     QDF_MAC_ADDR_SIZE);
2937 
2938 		link_frame_hdr->i_fc[0] = MLO_LINKSPECIFIC_ASSOC_RESP_FC0;
2939 		link_frame_hdr->i_fc[1] = MLO_LINKSPECIFIC_ASSOC_RESP_FC1;
2940 	}
2941 
2942 	mlo_debug("subtype:%u addr3:" QDF_MAC_ADDR_FMT " addr2:"
2943 		  QDF_MAC_ADDR_FMT " addr1:" QDF_MAC_ADDR_FMT,
2944 		  subtype,
2945 		  QDF_MAC_ADDR_REF(link_frame_hdr->i_addr3),
2946 		  QDF_MAC_ADDR_REF(link_frame_hdr->i_addr2),
2947 		  QDF_MAC_ADDR_REF(link_frame_hdr->i_addr1));
2948 
2949 	/* Seq num not used so not populated */
2950 
2951 	*link_frame_len = link_frame_currlen;
2952 	ret = QDF_STATUS_SUCCESS;
2953 
2954 mem_free:
2955 	qdf_mem_free(mlieseqpayload_copy);
2956 	return ret;
2957 }
2958 
2959 QDF_STATUS
util_gen_link_assoc_req(uint8_t * frame,qdf_size_t frame_len,bool isreassoc,uint8_t link_id,struct qdf_mac_addr link_addr,uint8_t * link_frame,qdf_size_t link_frame_maxsize,qdf_size_t * link_frame_len)2960 util_gen_link_assoc_req(uint8_t *frame, qdf_size_t frame_len, bool isreassoc,
2961 			uint8_t link_id,
2962 			struct qdf_mac_addr link_addr,
2963 			uint8_t *link_frame,
2964 			qdf_size_t link_frame_maxsize,
2965 			qdf_size_t *link_frame_len)
2966 {
2967 	return util_gen_link_reqrsp_cmn(frame, frame_len,
2968 			(isreassoc ? WLAN_FC0_STYPE_REASSOC_REQ :
2969 				WLAN_FC0_STYPE_ASSOC_REQ),
2970 			link_id, link_addr, link_frame,
2971 			link_frame_maxsize, link_frame_len);
2972 }
2973 
2974 QDF_STATUS
util_gen_link_assoc_rsp(uint8_t * frame,qdf_size_t frame_len,bool isreassoc,uint8_t link_id,struct qdf_mac_addr link_addr,uint8_t * link_frame,qdf_size_t link_frame_maxsize,qdf_size_t * link_frame_len)2975 util_gen_link_assoc_rsp(uint8_t *frame, qdf_size_t frame_len, bool isreassoc,
2976 			uint8_t link_id,
2977 			struct qdf_mac_addr link_addr,
2978 			uint8_t *link_frame,
2979 			qdf_size_t link_frame_maxsize,
2980 			qdf_size_t *link_frame_len)
2981 {
2982 	return util_gen_link_reqrsp_cmn(frame, frame_len,
2983 			(isreassoc ?  WLAN_FC0_STYPE_REASSOC_RESP :
2984 				WLAN_FC0_STYPE_ASSOC_RESP),
2985 			link_id, link_addr, link_frame,
2986 			link_frame_maxsize, link_frame_len);
2987 }
2988 
2989 QDF_STATUS
util_gen_link_probe_rsp(uint8_t * frame,qdf_size_t frame_len,uint8_t link_id,struct qdf_mac_addr link_addr,uint8_t * link_frame,qdf_size_t link_frame_maxsize,qdf_size_t * link_frame_len)2990 util_gen_link_probe_rsp(uint8_t *frame, qdf_size_t frame_len,
2991 			uint8_t link_id,
2992 			struct qdf_mac_addr link_addr,
2993 			uint8_t *link_frame,
2994 			qdf_size_t link_frame_maxsize,
2995 			qdf_size_t *link_frame_len)
2996 {
2997 	return util_gen_link_reqrsp_cmn(frame, frame_len,
2998 			 WLAN_FC0_STYPE_PROBE_RESP, link_id,
2999 			link_addr, link_frame, link_frame_maxsize,
3000 			link_frame_len);
3001 }
3002 
3003 QDF_STATUS
util_find_mlie(uint8_t * buf,qdf_size_t buflen,uint8_t ** mlieseq,qdf_size_t * mlieseqlen)3004 util_find_mlie(uint8_t *buf, qdf_size_t buflen, uint8_t **mlieseq,
3005 	       qdf_size_t *mlieseqlen)
3006 {
3007 	uint8_t *bufboundary;
3008 	uint8_t *ieseq;
3009 	qdf_size_t ieseqlen;
3010 	uint8_t *currie;
3011 	uint8_t *successorfrag;
3012 
3013 	if (!buf || !buflen || !mlieseq || !mlieseqlen)
3014 		return QDF_STATUS_E_NULL_VALUE;
3015 
3016 	*mlieseq = NULL;
3017 	*mlieseqlen = 0;
3018 
3019 	/* Find Multi-Link element. In case a fragment sequence is present,
3020 	 * this element will be the leading fragment.
3021 	 */
3022 	ieseq = util_find_extn_eid(WLAN_ELEMID_EXTN_ELEM,
3023 				   WLAN_EXTN_ELEMID_MULTI_LINK, buf,
3024 				   buflen);
3025 
3026 	/* Even if the element is not found, we have successfully examined the
3027 	 * buffer. The caller will be provided a NULL value for the starting of
3028 	 * the Multi-Link element. Hence, we return success.
3029 	 */
3030 	if (!ieseq)
3031 		return QDF_STATUS_SUCCESS;
3032 
3033 	bufboundary = buf + buflen;
3034 
3035 	if ((ieseq + MIN_IE_LEN) > bufboundary)
3036 		return QDF_STATUS_E_INVAL;
3037 
3038 	ieseqlen = MIN_IE_LEN + ieseq[TAG_LEN_POS];
3039 
3040 	if (ieseqlen < sizeof(struct wlan_ie_multilink))
3041 		return QDF_STATUS_E_PROTO;
3042 
3043 	if ((ieseq + ieseqlen) > bufboundary)
3044 		return QDF_STATUS_E_INVAL;
3045 
3046 	/* In the next sequence of checks, if there is no space in the buffer
3047 	 * for another element after the Multi-Link element/element fragment
3048 	 * sequence, it could indicate an issue since non-MLO EHT elements
3049 	 * would be expected to follow the Multi-Link element/element fragment
3050 	 * sequence. However, this is outside of the purview of this function,
3051 	 * hence we ignore it.
3052 	 */
3053 
3054 	currie = ieseq;
3055 	successorfrag = util_get_successorfrag(currie, buf, buflen);
3056 
3057 	/* Fragmentation definitions as of IEEE802.11be D1.0 and
3058 	 * IEEE802.11REVme D0.2 are applied. Only the case where Multi-Link
3059 	 * element is present in a buffer from the core frame is considered.
3060 	 * Future changes to fragmentation, cases where the Multi-Link element
3061 	 * is present in a subelement, etc. to be reflected here if applicable
3062 	 * as and when the rules evolve.
3063 	 */
3064 	while (successorfrag) {
3065 		/* We should not be seeing a successor fragment if the length
3066 		 * of the current IE is lesser than the max.
3067 		 */
3068 		if (currie[TAG_LEN_POS] != WLAN_MAX_IE_LEN)
3069 			return QDF_STATUS_E_PROTO;
3070 
3071 		if (successorfrag[TAG_LEN_POS] == 0)
3072 			return QDF_STATUS_E_PROTO;
3073 
3074 		ieseqlen +=  (MIN_IE_LEN + successorfrag[TAG_LEN_POS]);
3075 
3076 		currie = successorfrag;
3077 		successorfrag = util_get_successorfrag(currie, buf, buflen);
3078 	}
3079 
3080 	*mlieseq = ieseq;
3081 	*mlieseqlen = ieseqlen;
3082 	return QDF_STATUS_SUCCESS;
3083 }
3084 
3085 static inline QDF_STATUS
util_validate_bv_mlie_min_seq_len(qdf_size_t mlieseqlen)3086 util_validate_bv_mlie_min_seq_len(qdf_size_t mlieseqlen)
3087 {
3088 	qdf_size_t parsed_len = sizeof(struct wlan_ie_multilink);
3089 
3090 	if (mlieseqlen < parsed_len + WLAN_ML_BV_CINFO_LENGTH_SIZE) {
3091 		mlo_err_rl("ML seq payload of len %zu doesn't accommodate the mandatory BV ML IE Common info len field",
3092 			   mlieseqlen);
3093 		return QDF_STATUS_E_PROTO;
3094 	}
3095 	parsed_len += WLAN_ML_BV_CINFO_LENGTH_SIZE;
3096 
3097 	if (mlieseqlen < parsed_len + QDF_MAC_ADDR_SIZE) {
3098 		mlo_err_rl("ML seq payload of len %zu doesn't accommodate the mandatory MLD addr",
3099 			   mlieseqlen);
3100 		return QDF_STATUS_E_PROTO;
3101 	}
3102 
3103 	return QDF_STATUS_SUCCESS;
3104 }
3105 
3106 QDF_STATUS
util_find_mlie_by_variant(uint8_t * buf,qdf_size_t buflen,uint8_t ** mlieseq,qdf_size_t * mlieseqlen,int variant)3107 util_find_mlie_by_variant(uint8_t *buf, qdf_size_t buflen, uint8_t **mlieseq,
3108 			  qdf_size_t *mlieseqlen, int variant)
3109 {
3110 	uint8_t *ieseq;
3111 	qdf_size_t ieseqlen;
3112 	QDF_STATUS status;
3113 	int ml_variant;
3114 	qdf_size_t buf_parsed_len;
3115 
3116 	if (!buf || !buflen || !mlieseq || !mlieseqlen)
3117 		return QDF_STATUS_E_NULL_VALUE;
3118 
3119 	if (variant >= WLAN_ML_VARIANT_INVALIDSTART)
3120 		return QDF_STATUS_E_PROTO;
3121 
3122 	ieseq = NULL;
3123 	ieseqlen = 0;
3124 	*mlieseq = NULL;
3125 	*mlieseqlen = 0;
3126 	buf_parsed_len = 0;
3127 
3128 	while (buflen > buf_parsed_len) {
3129 		status = util_find_mlie(buf + buf_parsed_len,
3130 					buflen - buf_parsed_len,
3131 					&ieseq, &ieseqlen);
3132 
3133 		if (QDF_IS_STATUS_ERROR(status))
3134 			return status;
3135 
3136 		/* Even if the element is not found, we have successfully
3137 		 * examined the buffer. The caller will be provided a NULL value
3138 		 * for the starting of the Multi-Link element. Hence, we return
3139 		 * success.
3140 		 */
3141 		if (!ieseq)
3142 			return QDF_STATUS_SUCCESS;
3143 
3144 		status = util_get_mlie_variant(ieseq, ieseqlen,
3145 					       &ml_variant);
3146 		if (QDF_IS_STATUS_ERROR(status)) {
3147 			mlo_err("Unable to get Multi-link element variant");
3148 			return status;
3149 		}
3150 
3151 		if (ml_variant == variant) {
3152 			*mlieseq = ieseq;
3153 			*mlieseqlen = ieseqlen;
3154 			return QDF_STATUS_SUCCESS;
3155 		}
3156 
3157 		buf_parsed_len = ieseq + ieseqlen - buf;
3158 	}
3159 
3160 	return QDF_STATUS_E_INVAL;
3161 }
3162 
3163 QDF_STATUS
util_get_mlie_common_info_len(uint8_t * mlieseq,qdf_size_t mlieseqlen,uint8_t * commoninfo_len)3164 util_get_mlie_common_info_len(uint8_t *mlieseq, qdf_size_t mlieseqlen,
3165 			      uint8_t *commoninfo_len)
3166 {
3167 	struct wlan_ie_multilink *mlie_fixed;
3168 	enum wlan_ml_variant variant;
3169 	uint16_t mlcontrol;
3170 
3171 	if (!mlieseq || !mlieseqlen || !commoninfo_len)
3172 		return QDF_STATUS_E_NULL_VALUE;
3173 
3174 	if (mlieseqlen < sizeof(struct wlan_ie_multilink))
3175 		return QDF_STATUS_E_INVAL;
3176 
3177 	mlie_fixed = (struct wlan_ie_multilink *)mlieseq;
3178 
3179 	if (mlie_fixed->elem_id != WLAN_ELEMID_EXTN_ELEM ||
3180 	    mlie_fixed->elem_id_ext != WLAN_EXTN_ELEMID_MULTI_LINK)
3181 		return QDF_STATUS_E_INVAL;
3182 
3183 	mlcontrol = qdf_le16_to_cpu(mlie_fixed->mlcontrol);
3184 
3185 	variant = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_TYPE_IDX,
3186 			       WLAN_ML_CTRL_TYPE_BITS);
3187 
3188 	if (variant != WLAN_ML_VARIANT_BASIC)
3189 		return QDF_STATUS_E_INVAL;
3190 
3191 	/* Common Info starts at mlieseq + sizeof(struct wlan_ie_multilink).
3192 	 * Check if there is sufficient space in the buffer for the Common Info
3193 	 * Length and MLD MAC address.
3194 	 */
3195 	if ((sizeof(struct wlan_ie_multilink) + WLAN_ML_BV_CINFO_LENGTH_SIZE +
3196 	    QDF_MAC_ADDR_SIZE) > mlieseqlen)
3197 		return QDF_STATUS_E_PROTO;
3198 
3199 	*commoninfo_len = *(mlieseq + sizeof(struct wlan_ie_multilink));
3200 
3201 	return QDF_STATUS_SUCCESS;
3202 }
3203 
3204 QDF_STATUS
util_get_bvmlie_bssparamchangecnt(uint8_t * mlieseq,qdf_size_t mlieseqlen,bool * bssparamchangecntfound,uint8_t * bssparamchangecnt)3205 util_get_bvmlie_bssparamchangecnt(uint8_t *mlieseq, qdf_size_t mlieseqlen,
3206 				  bool *bssparamchangecntfound,
3207 				  uint8_t *bssparamchangecnt)
3208 {
3209 	struct wlan_ie_multilink *mlie_fixed;
3210 	enum wlan_ml_variant variant;
3211 	uint16_t mlcontrol;
3212 	uint16_t presencebitmap;
3213 	uint8_t *commoninfo;
3214 	uint8_t commoninfolen;
3215 	qdf_size_t mldcap_offset;
3216 
3217 	if (!mlieseq || !mlieseqlen || !bssparamchangecntfound ||
3218 	    !bssparamchangecnt)
3219 		return QDF_STATUS_E_NULL_VALUE;
3220 
3221 	*bssparamchangecntfound = false;
3222 	*bssparamchangecnt = 0;
3223 
3224 	if (mlieseqlen < sizeof(struct wlan_ie_multilink))
3225 		return QDF_STATUS_E_INVAL;
3226 
3227 	mlie_fixed = (struct wlan_ie_multilink *)mlieseq;
3228 
3229 	if (mlie_fixed->elem_id != WLAN_ELEMID_EXTN_ELEM ||
3230 	    mlie_fixed->elem_id_ext != WLAN_EXTN_ELEMID_MULTI_LINK)
3231 		return QDF_STATUS_E_INVAL;
3232 
3233 	mlcontrol = qdf_le16_to_cpu(mlie_fixed->mlcontrol);
3234 
3235 	variant = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_TYPE_IDX,
3236 			       WLAN_ML_CTRL_TYPE_BITS);
3237 
3238 	if (variant != WLAN_ML_VARIANT_BASIC)
3239 		return QDF_STATUS_E_NOSUPPORT;
3240 
3241 	presencebitmap = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_PBM_IDX,
3242 				      WLAN_ML_CTRL_PBM_BITS);
3243 
3244 	if (QDF_IS_STATUS_ERROR(util_validate_bv_mlie_min_seq_len(mlieseqlen)))
3245 		return QDF_STATUS_E_INVAL;
3246 
3247 	commoninfo = mlieseq + sizeof(struct wlan_ie_multilink);
3248 	commoninfolen = *(mlieseq + sizeof(struct wlan_ie_multilink));
3249 
3250 	mldcap_offset = WLAN_ML_BV_CINFO_LENGTH_SIZE;
3251 
3252 	mldcap_offset += QDF_MAC_ADDR_SIZE;
3253 
3254 	if (presencebitmap & WLAN_ML_BV_CTRL_PBM_LINKIDINFO_P) {
3255 		mldcap_offset += WLAN_ML_BV_CINFO_LINKIDINFO_SIZE;
3256 
3257 		if ((sizeof(struct wlan_ie_multilink) + mldcap_offset) >
3258 				mlieseqlen)
3259 			return QDF_STATUS_E_PROTO;
3260 	}
3261 
3262 	if (presencebitmap & WLAN_ML_BV_CTRL_PBM_BSSPARAMCHANGECNT_P) {
3263 		if (commoninfolen < (mldcap_offset +
3264 				     WLAN_ML_BSSPARAMCHNGCNT_SIZE))
3265 			return QDF_STATUS_E_PROTO;
3266 
3267 		if ((sizeof(struct wlan_ie_multilink) + mldcap_offset +
3268 				WLAN_ML_BSSPARAMCHNGCNT_SIZE) >
3269 				mlieseqlen)
3270 			return QDF_STATUS_E_PROTO;
3271 		*bssparamchangecntfound = true;
3272 		*bssparamchangecnt = *(commoninfo + mldcap_offset);
3273 	}
3274 
3275 	return QDF_STATUS_SUCCESS;
3276 }
3277 
3278 QDF_STATUS
util_get_mlie_variant(uint8_t * mlieseq,qdf_size_t mlieseqlen,int * variant)3279 util_get_mlie_variant(uint8_t *mlieseq, qdf_size_t mlieseqlen,
3280 		      int *variant)
3281 {
3282 	struct wlan_ie_multilink *mlie_fixed;
3283 	enum wlan_ml_variant var;
3284 	uint16_t mlcontrol;
3285 
3286 	if (!mlieseq || !mlieseqlen || !variant)
3287 		return QDF_STATUS_E_NULL_VALUE;
3288 
3289 	if (mlieseqlen < sizeof(struct wlan_ie_multilink))
3290 		return QDF_STATUS_E_INVAL;
3291 
3292 	mlie_fixed = (struct wlan_ie_multilink *)mlieseq;
3293 
3294 	if ((mlie_fixed->elem_id != WLAN_ELEMID_EXTN_ELEM) ||
3295 	    (mlie_fixed->elem_id_ext != WLAN_EXTN_ELEMID_MULTI_LINK))
3296 		return QDF_STATUS_E_INVAL;
3297 
3298 	mlcontrol = le16toh(mlie_fixed->mlcontrol);
3299 	var = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_TYPE_IDX,
3300 			   WLAN_ML_CTRL_TYPE_BITS);
3301 
3302 	if (var >= WLAN_ML_VARIANT_INVALIDSTART)
3303 		return QDF_STATUS_E_PROTO;
3304 
3305 	*variant = var;
3306 	return QDF_STATUS_SUCCESS;
3307 }
3308 
3309 QDF_STATUS
util_get_bvmlie_eml_cap(uint8_t * mlieseq,qdf_size_t mlieseqlen,bool * eml_cap_found,uint16_t * eml_cap)3310 util_get_bvmlie_eml_cap(uint8_t *mlieseq, qdf_size_t mlieseqlen,
3311 			bool *eml_cap_found,
3312 			uint16_t *eml_cap)
3313 {
3314 	struct wlan_ie_multilink *mlie_fixed;
3315 	enum wlan_ml_variant variant;
3316 	uint16_t mlcontrol;
3317 	uint8_t eml_cap_offset;
3318 	uint8_t commoninfo_len;
3319 	uint16_t presencebitmap;
3320 
3321 	if (!mlieseq || !mlieseqlen || !eml_cap_found || !eml_cap)
3322 		return QDF_STATUS_E_NULL_VALUE;
3323 
3324 	*eml_cap = 0;
3325 	*eml_cap_found = false;
3326 
3327 	if (mlieseqlen < sizeof(struct wlan_ie_multilink))
3328 		return QDF_STATUS_E_INVAL;
3329 
3330 	mlie_fixed = (struct wlan_ie_multilink *)mlieseq;
3331 
3332 	if ((mlie_fixed->elem_id != WLAN_ELEMID_EXTN_ELEM) ||
3333 	    (mlie_fixed->elem_id_ext != WLAN_EXTN_ELEMID_MULTI_LINK))
3334 		return QDF_STATUS_E_INVAL;
3335 
3336 	mlcontrol = qdf_le16_to_cpu(mlie_fixed->mlcontrol);
3337 
3338 	variant = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_TYPE_IDX,
3339 			       WLAN_ML_CTRL_TYPE_BITS);
3340 
3341 	if (variant != WLAN_ML_VARIANT_BASIC)
3342 		return QDF_STATUS_E_INVAL;
3343 
3344 	presencebitmap = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_PBM_IDX,
3345 				      WLAN_ML_CTRL_PBM_BITS);
3346 
3347 	/* eml_cap_offset stores the offset of EML Capabilities within
3348 	 * Common Info
3349 	 */
3350 	eml_cap_offset = WLAN_ML_BV_CINFO_LENGTH_SIZE + QDF_MAC_ADDR_SIZE;
3351 	if (presencebitmap & WLAN_ML_BV_CTRL_PBM_LINKIDINFO_P)
3352 		eml_cap_offset += WLAN_ML_BV_CINFO_LINKIDINFO_SIZE;
3353 	if (presencebitmap & WLAN_ML_BV_CTRL_PBM_BSSPARAMCHANGECNT_P)
3354 		eml_cap_offset += WLAN_ML_BSSPARAMCHNGCNT_SIZE;
3355 	if (presencebitmap & WLAN_ML_BV_CTRL_PBM_MEDIUMSYNCDELAYINFO_P)
3356 		eml_cap_offset += WLAN_ML_BV_CINFO_MEDMSYNCDELAYINFO_SIZE;
3357 
3358 	if (presencebitmap & WLAN_ML_BV_CTRL_PBM_EMLCAP_P) {
3359 		/* Common Info starts at
3360 		 * mlieseq + sizeof(struct wlan_ie_multilink).
3361 		 * Check if there is sufficient space in the buffer for
3362 		 * the Common Info Length.
3363 		 */
3364 		if (mlieseqlen < (sizeof(struct wlan_ie_multilink) +
3365 				  WLAN_ML_BV_CINFO_LENGTH_SIZE))
3366 			return QDF_STATUS_E_PROTO;
3367 
3368 		/* Check if the value indicated in the Common Info Length
3369 		 * subfield is sufficient to access the EML capabilities.
3370 		 */
3371 		commoninfo_len = *(mlieseq + sizeof(struct wlan_ie_multilink));
3372 		if (commoninfo_len < (eml_cap_offset +
3373 				      WLAN_ML_BV_CINFO_EMLCAP_SIZE))
3374 			return QDF_STATUS_E_PROTO;
3375 
3376 		/* Common Info starts at mlieseq + sizeof(struct
3377 		 * wlan_ie_multilink). Check if there is sufficient space in
3378 		 * Common Info for the EML capability.
3379 		 */
3380 		if (mlieseqlen < (sizeof(struct wlan_ie_multilink) +
3381 				  eml_cap_offset +
3382 				  WLAN_ML_BV_CINFO_EMLCAP_SIZE))
3383 			return QDF_STATUS_E_PROTO;
3384 
3385 		*eml_cap_found = true;
3386 		*eml_cap = qdf_le16_to_cpu(*(uint16_t *)(mlieseq +
3387 							 sizeof(struct wlan_ie_multilink) +
3388 							 eml_cap_offset));
3389 	}
3390 	return QDF_STATUS_SUCCESS;
3391 }
3392 
3393 QDF_STATUS
util_get_bvmlie_msd_cap(uint8_t * mlieseq,qdf_size_t mlieseqlen,bool * msd_cap_found,uint16_t * msd_cap)3394 util_get_bvmlie_msd_cap(uint8_t *mlieseq, qdf_size_t mlieseqlen,
3395 			bool *msd_cap_found,
3396 			uint16_t *msd_cap)
3397 {
3398 	struct wlan_ie_multilink *mlie_fixed;
3399 	enum wlan_ml_variant variant;
3400 	uint16_t mlcontrol;
3401 	uint8_t msd_cap_offset;
3402 	uint8_t commoninfo_len;
3403 	uint16_t presencebitmap;
3404 
3405 	if (!mlieseq || !mlieseqlen || !msd_cap_found || !msd_cap)
3406 		return QDF_STATUS_E_NULL_VALUE;
3407 
3408 	*msd_cap = 0;
3409 	*msd_cap_found = false;
3410 
3411 	if (mlieseqlen < sizeof(struct wlan_ie_multilink))
3412 		return QDF_STATUS_E_INVAL;
3413 
3414 	mlie_fixed = (struct wlan_ie_multilink *)mlieseq;
3415 
3416 	if ((mlie_fixed->elem_id != WLAN_ELEMID_EXTN_ELEM) ||
3417 	    (mlie_fixed->elem_id_ext != WLAN_EXTN_ELEMID_MULTI_LINK))
3418 		return QDF_STATUS_E_INVAL;
3419 
3420 	mlcontrol = qdf_le16_to_cpu(mlie_fixed->mlcontrol);
3421 
3422 	variant = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_TYPE_IDX,
3423 			       WLAN_ML_CTRL_TYPE_BITS);
3424 
3425 	if (variant != WLAN_ML_VARIANT_BASIC)
3426 		return QDF_STATUS_E_INVAL;
3427 
3428 	presencebitmap = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_PBM_IDX,
3429 				      WLAN_ML_CTRL_PBM_BITS);
3430 
3431 	/* msd_cap_offset stores the offset of MSD capabilities within
3432 	 * Common Info
3433 	 */
3434 	msd_cap_offset = WLAN_ML_BV_CINFO_LENGTH_SIZE + QDF_MAC_ADDR_SIZE;
3435 	if (presencebitmap & WLAN_ML_BV_CTRL_PBM_LINKIDINFO_P)
3436 		msd_cap_offset += WLAN_ML_BV_CINFO_LINKIDINFO_SIZE;
3437 	if (presencebitmap & WLAN_ML_BV_CTRL_PBM_BSSPARAMCHANGECNT_P)
3438 		msd_cap_offset += WLAN_ML_BSSPARAMCHNGCNT_SIZE;
3439 	if (presencebitmap & WLAN_ML_BV_CTRL_PBM_MEDIUMSYNCDELAYINFO_P) {
3440 		/* Common Info starts at
3441 		 * mlieseq + sizeof(struct wlan_ie_multilink).
3442 		 * Check if there is sufficient space in the buffer for
3443 		 * the Common Info Length.
3444 		 */
3445 		if (mlieseqlen < (sizeof(struct wlan_ie_multilink) +
3446 				  WLAN_ML_BV_CINFO_LENGTH_SIZE))
3447 			return QDF_STATUS_E_PROTO;
3448 
3449 		/* Check if the value indicated in the Common Info Length
3450 		 * subfield is sufficient to access the MSD capabilities.
3451 		 */
3452 		commoninfo_len = *(mlieseq + sizeof(struct wlan_ie_multilink));
3453 		if (commoninfo_len < (msd_cap_offset +
3454 				      WLAN_ML_BV_CINFO_MEDMSYNCDELAYINFO_SIZE))
3455 			return QDF_STATUS_E_PROTO;
3456 
3457 		/* Common Info starts at mlieseq + sizeof(struct
3458 		 * wlan_ie_multilink). Check if there is sufficient space in
3459 		 * Common Info for the MSD capability.
3460 		 */
3461 		if (mlieseqlen < (sizeof(struct wlan_ie_multilink) +
3462 				  msd_cap_offset +
3463 				  WLAN_ML_BV_CINFO_MEDMSYNCDELAYINFO_SIZE))
3464 			return QDF_STATUS_E_PROTO;
3465 
3466 		*msd_cap_found = true;
3467 		*msd_cap = qdf_le16_to_cpu(*(uint16_t *)(mlieseq +
3468 							 sizeof(struct wlan_ie_multilink) +
3469 							 msd_cap_offset));
3470 	} else {
3471 		mlo_debug("MSD caps not found in assoc rsp");
3472 	}
3473 
3474 	return QDF_STATUS_SUCCESS;
3475 }
3476 
3477 QDF_STATUS
util_get_bvmlie_mldmacaddr(uint8_t * mlieseq,qdf_size_t mlieseqlen,struct qdf_mac_addr * mldmacaddr)3478 util_get_bvmlie_mldmacaddr(uint8_t *mlieseq, qdf_size_t mlieseqlen,
3479 			   struct qdf_mac_addr *mldmacaddr)
3480 {
3481 	struct wlan_ie_multilink *mlie_fixed;
3482 	enum wlan_ml_variant variant;
3483 	uint16_t mlcontrol;
3484 	uint8_t commoninfo_len;
3485 
3486 	if (!mlieseq || !mlieseqlen || !mldmacaddr)
3487 		return QDF_STATUS_E_NULL_VALUE;
3488 
3489 	qdf_mem_zero(mldmacaddr, sizeof(*mldmacaddr));
3490 
3491 	if (mlieseqlen < sizeof(struct wlan_ie_multilink))
3492 		return QDF_STATUS_E_INVAL;
3493 
3494 	mlie_fixed = (struct wlan_ie_multilink *)mlieseq;
3495 
3496 	if ((mlie_fixed->elem_id != WLAN_ELEMID_EXTN_ELEM) ||
3497 	    (mlie_fixed->elem_id_ext != WLAN_EXTN_ELEMID_MULTI_LINK))
3498 		return QDF_STATUS_E_INVAL;
3499 
3500 	mlcontrol = qdf_le16_to_cpu(mlie_fixed->mlcontrol);
3501 
3502 	variant = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_TYPE_IDX,
3503 			       WLAN_ML_CTRL_TYPE_BITS);
3504 
3505 	if (variant != WLAN_ML_VARIANT_BASIC)
3506 		return QDF_STATUS_E_INVAL;
3507 
3508 	/* Common Info starts at mlieseq + sizeof(struct wlan_ie_multilink).
3509 	 * Check if there is sufficient space in the buffer for the Common Info
3510 	 * Length and MLD MAC address.
3511 	 */
3512 	if ((sizeof(struct wlan_ie_multilink) + WLAN_ML_BV_CINFO_LENGTH_SIZE +
3513 	    QDF_MAC_ADDR_SIZE) > mlieseqlen)
3514 		return QDF_STATUS_E_PROTO;
3515 
3516 	/* Check if the value indicated in the Common Info Length subfield is
3517 	 * sufficient to access the MLD MAC address.
3518 	 */
3519 	commoninfo_len = *(mlieseq + sizeof(struct wlan_ie_multilink));
3520 	if (commoninfo_len < (WLAN_ML_BV_CINFO_LENGTH_SIZE + QDF_MAC_ADDR_SIZE))
3521 		return QDF_STATUS_E_PROTO;
3522 
3523 	qdf_mem_copy(mldmacaddr->bytes,
3524 		     mlieseq + sizeof(struct wlan_ie_multilink) +
3525 		     WLAN_ML_BV_CINFO_LENGTH_SIZE,
3526 		     QDF_MAC_ADDR_SIZE);
3527 
3528 	return QDF_STATUS_SUCCESS;
3529 }
3530 
3531 QDF_STATUS
util_get_bvmlie_primary_linkid(uint8_t * mlieseq,qdf_size_t mlieseqlen,bool * linkidfound,uint8_t * linkid)3532 util_get_bvmlie_primary_linkid(uint8_t *mlieseq, qdf_size_t mlieseqlen,
3533 			       bool *linkidfound, uint8_t *linkid)
3534 {
3535 	struct wlan_ie_multilink *mlie_fixed;
3536 	enum wlan_ml_variant variant;
3537 	uint16_t mlcontrol;
3538 	uint16_t presencebitmap;
3539 	uint8_t *commoninfo;
3540 	qdf_size_t commoninfolen;
3541 	uint8_t *linkidinfo;
3542 
3543 	if (!mlieseq || !mlieseqlen || !linkidfound || !linkid)
3544 		return QDF_STATUS_E_NULL_VALUE;
3545 
3546 	*linkidfound = false;
3547 	*linkid = 0;
3548 
3549 	if (mlieseqlen < sizeof(struct wlan_ie_multilink))
3550 		return QDF_STATUS_E_INVAL;
3551 
3552 	mlie_fixed = (struct wlan_ie_multilink *)mlieseq;
3553 
3554 	if ((mlie_fixed->elem_id != WLAN_ELEMID_EXTN_ELEM) ||
3555 	    (mlie_fixed->elem_id_ext != WLAN_EXTN_ELEMID_MULTI_LINK))
3556 		return QDF_STATUS_E_INVAL;
3557 
3558 	mlcontrol = le16toh(mlie_fixed->mlcontrol);
3559 
3560 	variant = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_TYPE_IDX,
3561 			       WLAN_ML_CTRL_TYPE_BITS);
3562 
3563 	if (variant != WLAN_ML_VARIANT_BASIC)
3564 		return QDF_STATUS_E_INVAL;
3565 
3566 	presencebitmap = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_PBM_IDX,
3567 				      WLAN_ML_CTRL_PBM_BITS);
3568 
3569 	commoninfo = mlieseq + sizeof(struct wlan_ie_multilink);
3570 	commoninfolen = 0;
3571 	commoninfolen += WLAN_ML_BV_CINFO_LENGTH_SIZE;
3572 	if ((sizeof(struct wlan_ie_multilink) + commoninfolen) >
3573 			mlieseqlen)
3574 		return QDF_STATUS_E_PROTO;
3575 
3576 	commoninfolen += QDF_MAC_ADDR_SIZE;
3577 	if ((sizeof(struct wlan_ie_multilink) + commoninfolen) >
3578 			mlieseqlen)
3579 		return QDF_STATUS_E_PROTO;
3580 
3581 	if (presencebitmap & WLAN_ML_BV_CTRL_PBM_LINKIDINFO_P) {
3582 		linkidinfo = commoninfo + commoninfolen;
3583 		commoninfolen += WLAN_ML_BV_CINFO_LINKIDINFO_SIZE;
3584 
3585 		if ((sizeof(struct wlan_ie_multilink) + commoninfolen) >
3586 				mlieseqlen)
3587 			return QDF_STATUS_E_PROTO;
3588 
3589 		*linkidfound = true;
3590 		*linkid = QDF_GET_BITS(linkidinfo[0],
3591 				       WLAN_ML_BV_CINFO_LINKIDINFO_LINKID_IDX,
3592 				       WLAN_ML_BV_CINFO_LINKIDINFO_LINKID_BITS);
3593 	}
3594 
3595 	return QDF_STATUS_SUCCESS;
3596 }
3597 
3598 QDF_STATUS
util_get_bvmlie_mldcap(uint8_t * mlieseq,qdf_size_t mlieseqlen,bool * mldcapfound,uint16_t * mldcap)3599 util_get_bvmlie_mldcap(uint8_t *mlieseq, qdf_size_t mlieseqlen,
3600 		       bool *mldcapfound, uint16_t *mldcap)
3601 {
3602 	struct wlan_ie_multilink *mlie_fixed;
3603 	enum wlan_ml_variant variant;
3604 	uint16_t mlcontrol;
3605 	uint16_t presencebitmap;
3606 	uint8_t *commoninfo;
3607 	uint8_t commoninfo_len;
3608 	qdf_size_t mldcap_offset;
3609 
3610 	if (!mlieseq || !mlieseqlen || !mldcapfound || !mldcap)
3611 		return QDF_STATUS_E_NULL_VALUE;
3612 
3613 	*mldcapfound = false;
3614 	*mldcap = 0;
3615 
3616 	if (mlieseqlen < sizeof(struct wlan_ie_multilink))
3617 		return QDF_STATUS_E_INVAL;
3618 
3619 	mlie_fixed = (struct wlan_ie_multilink *)mlieseq;
3620 
3621 	if (mlie_fixed->elem_id != WLAN_ELEMID_EXTN_ELEM ||
3622 	    mlie_fixed->elem_id_ext != WLAN_EXTN_ELEMID_MULTI_LINK)
3623 		return QDF_STATUS_E_INVAL;
3624 
3625 	mlcontrol = qdf_le16_to_cpu(mlie_fixed->mlcontrol);
3626 
3627 	variant = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_TYPE_IDX,
3628 			       WLAN_ML_CTRL_TYPE_BITS);
3629 
3630 	if (variant != WLAN_ML_VARIANT_BASIC)
3631 		return QDF_STATUS_E_NOSUPPORT;
3632 
3633 	presencebitmap = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_PBM_IDX,
3634 				      WLAN_ML_CTRL_PBM_BITS);
3635 
3636 	if (QDF_IS_STATUS_ERROR(util_validate_bv_mlie_min_seq_len(mlieseqlen)))
3637 		return QDF_STATUS_E_INVAL;
3638 
3639 	commoninfo = mlieseq + sizeof(struct wlan_ie_multilink);
3640 	commoninfo_len = *(mlieseq + sizeof(struct wlan_ie_multilink));
3641 	/* mldcap_offset stores the offset of MLD Capabilities within
3642 	 * Common Info
3643 	 */
3644 	mldcap_offset = WLAN_ML_BV_CINFO_LENGTH_SIZE;
3645 	mldcap_offset += QDF_MAC_ADDR_SIZE;
3646 
3647 	if (presencebitmap & WLAN_ML_BV_CTRL_PBM_LINKIDINFO_P) {
3648 		mldcap_offset += WLAN_ML_BV_CINFO_LINKIDINFO_SIZE;
3649 
3650 		if ((sizeof(struct wlan_ie_multilink) + mldcap_offset) >
3651 				mlieseqlen)
3652 			return QDF_STATUS_E_PROTO;
3653 	}
3654 
3655 	if (presencebitmap & WLAN_ML_BV_CTRL_PBM_BSSPARAMCHANGECNT_P) {
3656 		mldcap_offset += WLAN_ML_BSSPARAMCHNGCNT_SIZE;
3657 
3658 		if ((sizeof(struct wlan_ie_multilink) + mldcap_offset) >
3659 				mlieseqlen)
3660 			return QDF_STATUS_E_PROTO;
3661 	}
3662 
3663 	if (presencebitmap & WLAN_ML_BV_CTRL_PBM_MEDIUMSYNCDELAYINFO_P) {
3664 		mldcap_offset += WLAN_ML_BV_CINFO_MEDMSYNCDELAYINFO_SIZE;
3665 
3666 		if ((sizeof(struct wlan_ie_multilink) + mldcap_offset) >
3667 				mlieseqlen)
3668 			return QDF_STATUS_E_PROTO;
3669 	}
3670 
3671 	if (presencebitmap & WLAN_ML_BV_CTRL_PBM_EMLCAP_P) {
3672 		mldcap_offset += WLAN_ML_BV_CINFO_EMLCAP_SIZE;
3673 
3674 		if ((sizeof(struct wlan_ie_multilink) + mldcap_offset) >
3675 				mlieseqlen)
3676 			return QDF_STATUS_E_PROTO;
3677 	}
3678 
3679 	if (presencebitmap & WLAN_ML_BV_CTRL_PBM_MLDCAPANDOP_P) {
3680 		/* Check if the value indicated in the Common Info Length
3681 		 * subfield is sufficient to access the MLD capabilities.
3682 		 */
3683 		if (commoninfo_len < (mldcap_offset +
3684 				      WLAN_ML_BV_CINFO_MLDCAPANDOP_SIZE))
3685 			return QDF_STATUS_E_PROTO;
3686 
3687 		if ((sizeof(struct wlan_ie_multilink) + mldcap_offset +
3688 					WLAN_ML_BV_CINFO_MLDCAPANDOP_SIZE) >
3689 				mlieseqlen)
3690 			return QDF_STATUS_E_PROTO;
3691 
3692 		*mldcap = qdf_le16_to_cpu(*((uint16_t *)(commoninfo + mldcap_offset)));
3693 		*mldcapfound = true;
3694 	}
3695 
3696 	return QDF_STATUS_SUCCESS;
3697 }
3698 
3699 QDF_STATUS
util_get_bvmlie_ext_mld_cap_op_info(uint8_t * mlie_seq,qdf_size_t mlie_seqlen,bool * ext_mld_cap_found,uint16_t * ext_mld_cap)3700 util_get_bvmlie_ext_mld_cap_op_info(uint8_t *mlie_seq,
3701 				    qdf_size_t mlie_seqlen,
3702 				    bool *ext_mld_cap_found,
3703 				    uint16_t *ext_mld_cap)
3704 {
3705 	struct wlan_ie_multilink *mlie_fixed;
3706 	uint16_t mlcontrol;
3707 	enum wlan_ml_variant variant;
3708 	uint16_t presence_bitmap;
3709 	uint8_t *commoninfo;
3710 	uint8_t commoninfo_len;
3711 	qdf_size_t extmldcap_offset;
3712 
3713 	if (!mlie_seq || !mlie_seqlen || !ext_mld_cap_found || !ext_mld_cap)
3714 		return QDF_STATUS_E_NULL_VALUE;
3715 
3716 	*ext_mld_cap_found = false;
3717 	*ext_mld_cap = 0;
3718 
3719 	if (mlie_seqlen < sizeof(struct wlan_ie_multilink))
3720 		return QDF_STATUS_E_INVAL;
3721 
3722 	mlie_fixed = (struct wlan_ie_multilink *)mlie_seq;
3723 
3724 	if (mlie_fixed->elem_id != WLAN_ELEMID_EXTN_ELEM ||
3725 	    mlie_fixed->elem_id_ext != WLAN_EXTN_ELEMID_MULTI_LINK)
3726 		return QDF_STATUS_E_INVAL;
3727 
3728 	mlcontrol = qdf_le16_to_cpu(mlie_fixed->mlcontrol);
3729 
3730 	variant = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_TYPE_IDX,
3731 			       WLAN_ML_CTRL_TYPE_BITS);
3732 
3733 	if (variant != WLAN_ML_VARIANT_BASIC)
3734 		return QDF_STATUS_E_NOSUPPORT;
3735 
3736 	presence_bitmap = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_PBM_IDX,
3737 				       WLAN_ML_CTRL_PBM_BITS);
3738 
3739 	commoninfo = mlie_seq + sizeof(struct wlan_ie_multilink);
3740 	commoninfo_len = *(mlie_seq + sizeof(struct wlan_ie_multilink));
3741 	/* extmldcap_offset stores the offset of Ext MLD Capabilities and
3742 	 * operations within the Common Info
3743 	 */
3744 
3745 	extmldcap_offset = WLAN_ML_BV_CINFO_LENGTH_SIZE;
3746 	extmldcap_offset += QDF_MAC_ADDR_SIZE;
3747 
3748 	if (presence_bitmap & WLAN_ML_BV_CTRL_PBM_LINKIDINFO_P) {
3749 		extmldcap_offset += WLAN_ML_BV_CINFO_LINKIDINFO_SIZE;
3750 
3751 		if ((sizeof(struct wlan_ie_multilink) + extmldcap_offset) >
3752 				mlie_seqlen)
3753 			return QDF_STATUS_E_PROTO;
3754 	}
3755 
3756 	if (presence_bitmap & WLAN_ML_BV_CTRL_PBM_BSSPARAMCHANGECNT_P) {
3757 		extmldcap_offset += WLAN_ML_BSSPARAMCHNGCNT_SIZE;
3758 
3759 		if ((sizeof(struct wlan_ie_multilink) + extmldcap_offset) >
3760 				mlie_seqlen)
3761 			return QDF_STATUS_E_PROTO;
3762 	}
3763 
3764 	if (presence_bitmap & WLAN_ML_BV_CTRL_PBM_MEDIUMSYNCDELAYINFO_P) {
3765 		extmldcap_offset += WLAN_ML_BV_CINFO_MEDMSYNCDELAYINFO_SIZE;
3766 
3767 		if ((sizeof(struct wlan_ie_multilink) + extmldcap_offset) >
3768 				mlie_seqlen)
3769 			return QDF_STATUS_E_PROTO;
3770 	}
3771 
3772 	if (presence_bitmap & WLAN_ML_BV_CTRL_PBM_EMLCAP_P) {
3773 		extmldcap_offset += WLAN_ML_BV_CINFO_EMLCAP_SIZE;
3774 
3775 		if ((sizeof(struct wlan_ie_multilink) + extmldcap_offset) >
3776 				mlie_seqlen)
3777 			return QDF_STATUS_E_PROTO;
3778 	}
3779 
3780 	if (presence_bitmap & WLAN_ML_BV_CTRL_PBM_MLDCAPANDOP_P) {
3781 		extmldcap_offset += WLAN_ML_BV_CINFO_MLDCAPANDOP_SIZE;
3782 
3783 		if ((sizeof(struct wlan_ie_multilink) + extmldcap_offset) >
3784 				mlie_seqlen)
3785 			return QDF_STATUS_E_PROTO;
3786 	}
3787 
3788 	if (presence_bitmap & WLAN_ML_BV_CTRL_PBM_MLDID_P) {
3789 		extmldcap_offset += WLAN_ML_BV_CINFO_MLDID_SIZE;
3790 
3791 		if ((sizeof(struct wlan_ie_multilink) + extmldcap_offset) >
3792 				mlie_seqlen)
3793 			return QDF_STATUS_E_PROTO;
3794 	}
3795 
3796 	if (presence_bitmap & WLAN_ML_BV_CTRL_PBM_EXT_MLDCAPANDOP_P) {
3797 		/* Check if the value indicated in the Common Info Length
3798 		 * subfield is sufficient to access the Ext MLD capabilities.
3799 		 */
3800 		if (commoninfo_len < (extmldcap_offset +
3801 					WLAN_ML_BV_CINFO_EXT_MLDCAPANDOP_SIZE))
3802 			return QDF_STATUS_E_PROTO;
3803 
3804 		if ((sizeof(struct wlan_ie_multilink) + extmldcap_offset +
3805 					WLAN_ML_BV_CINFO_EXT_MLDCAPANDOP_SIZE) >
3806 				mlie_seqlen)
3807 			return QDF_STATUS_E_PROTO;
3808 
3809 		*ext_mld_cap = qdf_le16_to_cpu(*((uint16_t *)(commoninfo +
3810 						extmldcap_offset)));
3811 
3812 		*ext_mld_cap_found = true;
3813 	}
3814 
3815 	return QDF_STATUS_SUCCESS;
3816 }
3817 
3818 QDF_STATUS
util_get_bvmlie_persta_partner_info(uint8_t * mlieseq,qdf_size_t mlieseqlen,struct mlo_partner_info * partner_info)3819 util_get_bvmlie_persta_partner_info(uint8_t *mlieseq,
3820 				    qdf_size_t mlieseqlen,
3821 				    struct mlo_partner_info *partner_info)
3822 {
3823 	struct wlan_ie_multilink *mlie_fixed;
3824 	uint16_t mlcontrol;
3825 	enum wlan_ml_variant variant;
3826 	uint8_t *linkinfo;
3827 	qdf_size_t linkinfo_len;
3828 	struct mlo_partner_info pinfo = {0};
3829 	qdf_size_t mlieseqpayloadlen;
3830 	uint8_t *mlieseqpayload_copy;
3831 	bool is_elemfragseq;
3832 	qdf_size_t defragpayload_len;
3833 
3834 	qdf_size_t tmplen;
3835 	QDF_STATUS ret;
3836 
3837 	if (!mlieseq) {
3838 		mlo_err("Pointer to Multi-Link element sequence is NULL");
3839 		return QDF_STATUS_E_NULL_VALUE;
3840 	}
3841 
3842 	if (!mlieseqlen) {
3843 		mlo_err("Length of Multi-Link element sequence is zero");
3844 		return QDF_STATUS_E_INVAL;
3845 	}
3846 
3847 	if (!partner_info) {
3848 		mlo_err("partner_info is NULL");
3849 		return QDF_STATUS_E_NULL_VALUE;
3850 	}
3851 
3852 	partner_info->num_partner_links = 0;
3853 
3854 	if (mlieseqlen < sizeof(struct wlan_ie_multilink)) {
3855 		mlo_err_rl("Multi-Link element sequence length %zu octets is smaller than required for the fixed portion of Multi-Link element (%zu octets)",
3856 			   mlieseqlen, sizeof(struct wlan_ie_multilink));
3857 		return QDF_STATUS_E_INVAL;
3858 	}
3859 
3860 	mlie_fixed = (struct wlan_ie_multilink *)mlieseq;
3861 
3862 	if ((mlie_fixed->elem_id != WLAN_ELEMID_EXTN_ELEM) ||
3863 	    (mlie_fixed->elem_id_ext != WLAN_EXTN_ELEMID_MULTI_LINK)) {
3864 		mlo_err("The element is not a Multi-Link element");
3865 		return QDF_STATUS_E_INVAL;
3866 	}
3867 
3868 	mlcontrol = le16toh(mlie_fixed->mlcontrol);
3869 
3870 	variant = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_TYPE_IDX,
3871 			       WLAN_ML_CTRL_TYPE_BITS);
3872 
3873 	if (variant != WLAN_ML_VARIANT_BASIC) {
3874 		mlo_err("The variant value %u does not correspond to Basic Variant value %u",
3875 			variant, WLAN_ML_VARIANT_BASIC);
3876 		return QDF_STATUS_E_INVAL;
3877 	}
3878 
3879 	mlieseqpayloadlen = 0;
3880 	tmplen = 0;
3881 	is_elemfragseq = false;
3882 
3883 	ret = wlan_get_elem_fragseq_info(mlieseq,
3884 					 mlieseqlen,
3885 					 &is_elemfragseq,
3886 					 &tmplen,
3887 					 &mlieseqpayloadlen);
3888 	if (QDF_IS_STATUS_ERROR(ret))
3889 		return ret;
3890 
3891 	if (is_elemfragseq) {
3892 		if (tmplen != mlieseqlen) {
3893 			mlo_err_rl("Mismatch in values of element fragment sequence total length. Val per frag info determination: %zu octets, val passed as arg: %zu octets",
3894 				   tmplen, mlieseqlen);
3895 			return QDF_STATUS_E_INVAL;
3896 		}
3897 
3898 		if (!mlieseqpayloadlen) {
3899 			mlo_err_rl("Multi-Link element fragment sequence payload is reported as 0, investigate");
3900 			return QDF_STATUS_E_FAILURE;
3901 		}
3902 
3903 		mlo_debug("Multi-Link element fragment sequence found with payload len %zu",
3904 			  mlieseqpayloadlen);
3905 	} else {
3906 		if (mlieseqlen > (sizeof(struct ie_header) + WLAN_MAX_IE_LEN)) {
3907 			mlo_err_rl("Expected presence of valid fragment sequence since Multi-Link element sequence length %zu octets is larger than frag threshold of %zu octets, however no valid fragment sequence found",
3908 				   mlieseqlen,
3909 				   sizeof(struct ie_header) + WLAN_MAX_IE_LEN);
3910 			return QDF_STATUS_E_FAILURE;
3911 		}
3912 
3913 		mlieseqpayloadlen = mlieseqlen - (sizeof(struct ie_header) + 1);
3914 	}
3915 
3916 	mlieseqpayload_copy = qdf_mem_malloc(mlieseqpayloadlen);
3917 
3918 	if (!mlieseqpayload_copy) {
3919 		mlo_err_rl("Could not allocate memory for Multi-Link element payload copy");
3920 		return QDF_STATUS_E_NOMEM;
3921 	}
3922 
3923 	if (is_elemfragseq) {
3924 		ret = wlan_defrag_elem_fragseq(false,
3925 					       mlieseq,
3926 					       mlieseqlen,
3927 					       mlieseqpayload_copy,
3928 					       mlieseqpayloadlen,
3929 					       &defragpayload_len);
3930 		if (QDF_IS_STATUS_ERROR(ret)) {
3931 			qdf_mem_free(mlieseqpayload_copy);
3932 			return ret;
3933 		}
3934 
3935 		if (defragpayload_len != mlieseqpayloadlen) {
3936 			mlo_err_rl("Length of de-fragmented payload %zu octets is not equal to length of Multi-Link element fragment sequence payload %zu octets",
3937 				   defragpayload_len, mlieseqpayloadlen);
3938 			qdf_mem_free(mlieseqpayload_copy);
3939 			return QDF_STATUS_E_FAILURE;
3940 		}
3941 	} else {
3942 		qdf_mem_copy(mlieseqpayload_copy,
3943 			     mlieseq + sizeof(struct ie_header) + 1,
3944 			     mlieseqpayloadlen);
3945 	}
3946 
3947 	linkinfo = NULL;
3948 	linkinfo_len = 0;
3949 
3950 	ret = util_parse_multi_link_ctrl(mlieseqpayload_copy,
3951 					 mlieseqpayloadlen,
3952 					 &linkinfo,
3953 					 &linkinfo_len);
3954 	if (QDF_IS_STATUS_ERROR(ret)) {
3955 		qdf_mem_free(mlieseqpayload_copy);
3956 		return ret;
3957 	}
3958 
3959 	/*
3960 	 * If Probe Request variant Multi-Link element in the Multi-Link probe
3961 	 * request does not include any per-STA profile, then all APs affiliated
3962 	 * with the same AP MLD as the AP identified in the Addr 1 or Addr 3
3963 	 * field or AP MLD ID of the Multi-Link probe request are requested
3964 	 * APs return success here
3965 	 */
3966 	if (!linkinfo) {
3967 		qdf_mem_free(mlieseqpayload_copy);
3968 		return QDF_STATUS_SUCCESS;
3969 	}
3970 
3971 	ret = util_parse_partner_info_from_linkinfo(linkinfo,
3972 						    linkinfo_len,
3973 						    &pinfo);
3974 
3975 	if (QDF_IS_STATUS_ERROR(ret)) {
3976 		qdf_mem_free(mlieseqpayload_copy);
3977 		return ret;
3978 	}
3979 
3980 	qdf_mem_copy(partner_info, &pinfo, sizeof(*partner_info));
3981 
3982 	qdf_mem_free(mlieseqpayload_copy);
3983 
3984 	return QDF_STATUS_SUCCESS;
3985 }
3986 
3987 QDF_STATUS
util_get_prvmlie_persta_link_id(uint8_t * mlieseq,qdf_size_t mlieseqlen,struct mlo_probereq_info * probereq_info)3988 util_get_prvmlie_persta_link_id(uint8_t *mlieseq,
3989 				qdf_size_t mlieseqlen,
3990 				struct mlo_probereq_info *probereq_info)
3991 {
3992 	struct wlan_ie_multilink *mlie_fixed;
3993 	uint16_t mlcontrol;
3994 	enum wlan_ml_variant variant;
3995 	uint8_t *linkinfo;
3996 	qdf_size_t linkinfo_len;
3997 	qdf_size_t mlieseqpayloadlen;
3998 	uint8_t *mlieseqpayload_copy;
3999 	bool is_elemfragseq;
4000 	qdf_size_t defragpayload_len;
4001 
4002 	qdf_size_t tmplen;
4003 	QDF_STATUS ret;
4004 
4005 	if (!mlieseq) {
4006 		mlo_err("Pointer to Multi-Link element sequence is NULL");
4007 		return QDF_STATUS_E_NULL_VALUE;
4008 	}
4009 
4010 	if (!mlieseqlen) {
4011 		mlo_err("Length of Multi-Link element sequence is zero");
4012 		return QDF_STATUS_E_INVAL;
4013 	}
4014 
4015 	if (!probereq_info) {
4016 		mlo_err("probe request_info is NULL");
4017 		return QDF_STATUS_E_NULL_VALUE;
4018 	}
4019 
4020 	probereq_info->num_links = 0;
4021 
4022 	if (mlieseqlen < sizeof(struct wlan_ie_multilink)) {
4023 		mlo_err_rl("Multi-Link element sequence length %zu octets is smaller than required for the fixed portion of Multi-Link element (%zu octets)",
4024 			   mlieseqlen, sizeof(struct wlan_ie_multilink));
4025 		return QDF_STATUS_E_INVAL;
4026 	}
4027 
4028 	mlie_fixed = (struct wlan_ie_multilink *)mlieseq;
4029 
4030 	if ((mlie_fixed->elem_id != WLAN_ELEMID_EXTN_ELEM) ||
4031 	    (mlie_fixed->elem_id_ext != WLAN_EXTN_ELEMID_MULTI_LINK)) {
4032 		mlo_err("The element is not a Multi-Link element");
4033 		return QDF_STATUS_E_INVAL;
4034 	}
4035 
4036 	mlcontrol = qdf_le16_to_cpu(mlie_fixed->mlcontrol);
4037 
4038 	variant = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_TYPE_IDX,
4039 			       WLAN_ML_CTRL_TYPE_BITS);
4040 
4041 	if (variant != WLAN_ML_VARIANT_PROBEREQ) {
4042 		mlo_err("The variant value %u does not correspond to Probe Request Variant value %u",
4043 			variant, WLAN_ML_VARIANT_PROBEREQ);
4044 		return QDF_STATUS_E_INVAL;
4045 	}
4046 
4047 	mlieseqpayloadlen = 0;
4048 	tmplen = 0;
4049 	is_elemfragseq = false;
4050 
4051 	ret = wlan_get_elem_fragseq_info(mlieseq,
4052 					 mlieseqlen,
4053 					 &is_elemfragseq,
4054 					 &tmplen,
4055 					 &mlieseqpayloadlen);
4056 	if (QDF_IS_STATUS_ERROR(ret))
4057 		return ret;
4058 
4059 	if (is_elemfragseq) {
4060 		if (tmplen != mlieseqlen) {
4061 			mlo_err_rl("Mismatch in values of element fragment sequence total length. Val per frag info determination: %zu octets, val passed as arg: %zu octets",
4062 				   tmplen, mlieseqlen);
4063 			return QDF_STATUS_E_INVAL;
4064 		}
4065 
4066 		if (!mlieseqpayloadlen) {
4067 			mlo_err_rl("Multi-Link element fragment sequence payload is reported as 0, investigate");
4068 			return QDF_STATUS_E_FAILURE;
4069 		}
4070 
4071 		mlo_debug("Multi-Link element fragment sequence found with payload len %zu",
4072 			  mlieseqpayloadlen);
4073 	} else {
4074 		if (mlieseqlen > (sizeof(struct ie_header) + WLAN_MAX_IE_LEN)) {
4075 			mlo_err_rl("Expected presence of valid fragment sequence since Multi-Link element sequence length %zu octets is larger than frag threshold of %zu octets, however no valid fragment sequence found",
4076 				   mlieseqlen,
4077 				   sizeof(struct ie_header) + WLAN_MAX_IE_LEN);
4078 			return QDF_STATUS_E_FAILURE;
4079 		}
4080 
4081 		mlieseqpayloadlen = mlieseqlen - (sizeof(struct ie_header) + 1);
4082 	}
4083 
4084 	mlieseqpayload_copy = qdf_mem_malloc(mlieseqpayloadlen);
4085 
4086 	if (!mlieseqpayload_copy) {
4087 		mlo_err_rl("Could not allocate memory for Multi-Link element payload copy");
4088 		return QDF_STATUS_E_NOMEM;
4089 	}
4090 
4091 	if (is_elemfragseq) {
4092 		ret = wlan_defrag_elem_fragseq(false,
4093 					       mlieseq,
4094 					       mlieseqlen,
4095 					       mlieseqpayload_copy,
4096 					       mlieseqpayloadlen,
4097 					       &defragpayload_len);
4098 		if (QDF_IS_STATUS_ERROR(ret)) {
4099 			qdf_mem_free(mlieseqpayload_copy);
4100 			return ret;
4101 		}
4102 
4103 		if (defragpayload_len != mlieseqpayloadlen) {
4104 			mlo_err_rl("Length of de-fragmented payload %zu octets is not equal to length of Multi-Link element fragment sequence payload %zu octets",
4105 				   defragpayload_len, mlieseqpayloadlen);
4106 			qdf_mem_free(mlieseqpayload_copy);
4107 			return QDF_STATUS_E_FAILURE;
4108 		}
4109 	} else {
4110 		qdf_mem_copy(mlieseqpayload_copy,
4111 			     mlieseq + sizeof(struct ie_header) + 1,
4112 			     mlieseqpayloadlen);
4113 	}
4114 
4115 	linkinfo = NULL;
4116 	linkinfo_len = 0;
4117 	ret = util_parse_prv_multi_link_ctrl(mlieseqpayload_copy,
4118 					     mlieseqpayloadlen,
4119 					     &linkinfo,
4120 					     &linkinfo_len);
4121 	if (QDF_IS_STATUS_ERROR(ret)) {
4122 		qdf_mem_free(mlieseqpayload_copy);
4123 		return ret;
4124 	}
4125 
4126 	/* In case Link Info is absent, the number of links will remain
4127 	 * zero.
4128 	 */
4129 	if (!linkinfo) {
4130 		mlo_debug("No link info present");
4131 		qdf_mem_free(mlieseqpayload_copy);
4132 		return QDF_STATUS_SUCCESS;
4133 	}
4134 
4135 	ret = util_parse_probereq_info_from_linkinfo(linkinfo,
4136 						     linkinfo_len,
4137 						     probereq_info);
4138 
4139 	if (QDF_IS_STATUS_ERROR(ret)) {
4140 		qdf_mem_free(mlieseqpayload_copy);
4141 		return ret;
4142 	}
4143 
4144 	qdf_mem_free(mlieseqpayload_copy);
4145 
4146 	return QDF_STATUS_SUCCESS;
4147 }
4148 
4149 QDF_STATUS
util_get_prvmlie_mldid(uint8_t * mlieseq,qdf_size_t mlieseqlen,bool * mldidfound,uint8_t * mldid)4150 util_get_prvmlie_mldid(uint8_t *mlieseq, qdf_size_t mlieseqlen,
4151 		       bool *mldidfound, uint8_t *mldid)
4152 {
4153 	struct wlan_ie_multilink *mlie_fixed;
4154 	enum wlan_ml_variant variant;
4155 	uint16_t mlcontrol;
4156 	uint16_t presencebitmap;
4157 	uint8_t *commoninfo;
4158 	qdf_size_t commoninfolen;
4159 
4160 	if (!mlieseq || !mlieseqlen || !mldidfound || !mldid)
4161 		return QDF_STATUS_E_NULL_VALUE;
4162 
4163 	*mldidfound = false;
4164 	*mldid = 0;
4165 
4166 	if (mlieseqlen < sizeof(struct wlan_ie_multilink))
4167 		return QDF_STATUS_E_INVAL;
4168 
4169 	mlie_fixed = (struct wlan_ie_multilink *)mlieseq;
4170 
4171 	if (mlie_fixed->elem_id != WLAN_ELEMID_EXTN_ELEM ||
4172 	    mlie_fixed->elem_id_ext != WLAN_EXTN_ELEMID_MULTI_LINK)
4173 		return QDF_STATUS_E_INVAL;
4174 
4175 	mlcontrol = qdf_le16_to_cpu(mlie_fixed->mlcontrol);
4176 
4177 	variant = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_TYPE_IDX,
4178 			       WLAN_ML_CTRL_TYPE_BITS);
4179 
4180 	if (variant != WLAN_ML_VARIANT_PROBEREQ)
4181 		return QDF_STATUS_E_NOSUPPORT;
4182 
4183 	presencebitmap = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_PBM_IDX,
4184 				      WLAN_ML_CTRL_PBM_BITS);
4185 
4186 	commoninfo = mlieseq + sizeof(struct wlan_ie_multilink);
4187 	commoninfolen = WLAN_ML_PRV_CINFO_LENGTH_SIZE;
4188 
4189 	if (presencebitmap & WLAN_ML_PRV_CTRL_PBM_MLDID_P) {
4190 		if ((sizeof(struct wlan_ie_multilink) + commoninfolen +
4191 		     WLAN_ML_PRV_CINFO_MLDID_SIZE) >
4192 		    mlieseqlen)
4193 			return QDF_STATUS_E_PROTO;
4194 
4195 		*mldid = *((uint8_t *)(commoninfo + commoninfolen));
4196 		commoninfolen += WLAN_ML_PRV_CINFO_MLDID_SIZE;
4197 
4198 		*mldidfound = true;
4199 	}
4200 
4201 	return QDF_STATUS_SUCCESS;
4202 }
4203 
util_get_rvmlie_mldmacaddr(uint8_t * mlieseq,qdf_size_t mlieseqlen,struct qdf_mac_addr * mldmacaddr,bool * is_mldmacaddr_found)4204 QDF_STATUS util_get_rvmlie_mldmacaddr(uint8_t *mlieseq, qdf_size_t mlieseqlen,
4205 				      struct qdf_mac_addr *mldmacaddr,
4206 				      bool *is_mldmacaddr_found)
4207 {
4208 	struct wlan_ie_multilink *mlie_fixed;
4209 	enum wlan_ml_variant variant;
4210 	uint16_t mlcontrol;
4211 	uint16_t presencebitmap;
4212 	qdf_size_t rv_cinfo_len;
4213 
4214 	if (!mlieseq || !mlieseqlen || !mldmacaddr || !is_mldmacaddr_found)
4215 		return QDF_STATUS_E_NULL_VALUE;
4216 
4217 	*is_mldmacaddr_found = false;
4218 	qdf_mem_zero(mldmacaddr, sizeof(*mldmacaddr));
4219 
4220 	if (mlieseqlen < sizeof(struct wlan_ie_multilink))
4221 		return QDF_STATUS_E_INVAL;
4222 
4223 	mlie_fixed = (struct wlan_ie_multilink *)mlieseq;
4224 
4225 	if (mlie_fixed->elem_id != WLAN_ELEMID_EXTN_ELEM ||
4226 	    mlie_fixed->elem_id_ext != WLAN_EXTN_ELEMID_MULTI_LINK)
4227 		return QDF_STATUS_E_INVAL;
4228 
4229 	mlcontrol = qdf_le16_to_cpu(mlie_fixed->mlcontrol);
4230 
4231 	variant = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_TYPE_IDX,
4232 			       WLAN_ML_CTRL_TYPE_BITS);
4233 
4234 	if (variant != WLAN_ML_VARIANT_RECONFIG)
4235 		return QDF_STATUS_E_INVAL;
4236 
4237 	/* ML Reconfig Common Info Length field present */
4238 	if ((sizeof(struct wlan_ie_multilink) + WLAN_ML_RV_CINFO_LENGTH_SIZE) >
4239 	    mlieseqlen)
4240 		return QDF_STATUS_E_PROTO;
4241 
4242 	rv_cinfo_len = *(mlieseq + sizeof(struct wlan_ie_multilink));
4243 
4244 	presencebitmap = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_PBM_IDX,
4245 				      WLAN_ML_CTRL_PBM_BITS);
4246 
4247 	/* Check if MLD mac address is present */
4248 	if (presencebitmap & WLAN_ML_RV_CTRL_PBM_MLDMACADDR_P) {
4249 		/* Check if the value indicated in the Common Info Length
4250 		 * subfield is sufficient to access the MLD MAC address.
4251 		 */
4252 		if (rv_cinfo_len < (WLAN_ML_RV_CINFO_LENGTH_SIZE +
4253 				    QDF_MAC_ADDR_SIZE))
4254 			return QDF_STATUS_E_PROTO;
4255 
4256 		if ((sizeof(struct wlan_ie_multilink) +
4257 		     WLAN_ML_RV_CINFO_LENGTH_SIZE + QDF_MAC_ADDR_SIZE) >
4258 			mlieseqlen)
4259 			return QDF_STATUS_E_PROTO;
4260 
4261 		qdf_mem_copy(mldmacaddr->bytes,
4262 			     mlieseq + sizeof(struct wlan_ie_multilink) +
4263 			     WLAN_ML_RV_CINFO_LENGTH_SIZE,
4264 			     QDF_MAC_ADDR_SIZE);
4265 		*is_mldmacaddr_found = true;
4266 	}
4267 
4268 	return QDF_STATUS_SUCCESS;
4269 }
4270 
4271 static QDF_STATUS
util_parse_rv_multi_link_ctrl(uint8_t * mlieseqpayload,qdf_size_t mlieseqpayloadlen,uint8_t ** link_info,qdf_size_t * link_info_len)4272 util_parse_rv_multi_link_ctrl(uint8_t *mlieseqpayload,
4273 			      qdf_size_t mlieseqpayloadlen,
4274 			      uint8_t **link_info,
4275 			      qdf_size_t *link_info_len)
4276 {
4277 	qdf_size_t parsed_payload_len, rv_cinfo_len;
4278 	uint16_t mlcontrol;
4279 	uint16_t presence_bm;
4280 
4281 	/* This helper returns the location(s) and length(s) of (sub)field(s)
4282 	 * inferable after parsing the Multi Link element Control field. These
4283 	 * location(s) and length(s) is/are in reference to the payload section
4284 	 * of the Multi Link element (after defragmentation, if applicable).
4285 	 * Here, the payload is the point after the element ID extension of the
4286 	 * Multi Link element, and includes the payloads of all subsequent
4287 	 * fragments (if any) but not the headers of those fragments.
4288 	 *
4289 	 * Currently, the helper returns the location and length of the Link
4290 	 * Info field in the Multi Link element sequence. Other (sub)field(s)
4291 	 * can be added later as required.
4292 	 */
4293 	if (!mlieseqpayload) {
4294 		mlo_err("ML seq payload pointer is NULL");
4295 		return QDF_STATUS_E_NULL_VALUE;
4296 	}
4297 
4298 	if (!mlieseqpayloadlen) {
4299 		mlo_err("ML seq payload len is 0");
4300 		return QDF_STATUS_E_INVAL;
4301 	}
4302 
4303 	if (mlieseqpayloadlen < WLAN_ML_CTRL_SIZE) {
4304 		mlo_err_rl("ML seq payload len %zu < ML Control size %u",
4305 			   mlieseqpayloadlen, WLAN_ML_CTRL_SIZE);
4306 		return QDF_STATUS_E_PROTO;
4307 	}
4308 
4309 	parsed_payload_len = 0;
4310 
4311 	qdf_mem_copy(&mlcontrol, mlieseqpayload, WLAN_ML_CTRL_SIZE);
4312 	mlcontrol = qdf_le16_to_cpu(mlcontrol);
4313 	parsed_payload_len += WLAN_ML_CTRL_SIZE;
4314 
4315 	if (mlieseqpayloadlen <
4316 			(parsed_payload_len + WLAN_ML_RV_CINFO_LENGTH_SIZE)) {
4317 		mlo_err_rl("ML rv seq payload len %zu insufficient for common info length size %u after parsed payload len %zu.",
4318 			   mlieseqpayloadlen,
4319 			   WLAN_ML_RV_CINFO_LENGTH_SIZE,
4320 			   parsed_payload_len);
4321 		return QDF_STATUS_E_PROTO;
4322 	}
4323 
4324 	rv_cinfo_len = *(mlieseqpayload + parsed_payload_len);
4325 	parsed_payload_len += WLAN_ML_RV_CINFO_LENGTH_SIZE;
4326 
4327 	presence_bm = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_PBM_IDX,
4328 				   WLAN_ML_CTRL_PBM_BITS);
4329 
4330 	/* Check if MLD MAC address is present */
4331 	if (presence_bm & WLAN_ML_RV_CTRL_PBM_MLDMACADDR_P) {
4332 		/* Check if the value indicated in the Common Info Length
4333 		 * subfield is sufficient to access the MLD MAC address.
4334 		 * Note: In D3.0, MLD MAC address will not be present
4335 		 * in ML Reconfig IE. But for code completeness, we
4336 		 * should have below code to sanity check.
4337 		 */
4338 		if (rv_cinfo_len < (WLAN_ML_RV_CINFO_LENGTH_SIZE +
4339 				    QDF_MAC_ADDR_SIZE)) {
4340 			mlo_err_rl("ML rv Common Info Length %zu insufficient to access MLD MAC addr size %u.",
4341 				   rv_cinfo_len,
4342 				   QDF_MAC_ADDR_SIZE);
4343 			return QDF_STATUS_E_PROTO;
4344 		}
4345 
4346 		if (mlieseqpayloadlen <
4347 				(parsed_payload_len +
4348 				 QDF_MAC_ADDR_SIZE)) {
4349 			mlo_err_rl("ML seq payload len %zu insufficient for MLD MAC size %u after parsed payload len %zu.",
4350 				   mlieseqpayloadlen,
4351 				   QDF_MAC_ADDR_SIZE,
4352 				   parsed_payload_len);
4353 			return QDF_STATUS_E_PROTO;
4354 		}
4355 
4356 		parsed_payload_len += QDF_MAC_ADDR_SIZE;
4357 	}
4358 
4359 	/* At present, we only handle MAC address field in common info field.
4360 	 * To be compatible with future spec updating, if new items are added
4361 	 * to common info, below log will highlight the spec change.
4362 	 */
4363 	if (rv_cinfo_len != (parsed_payload_len - WLAN_ML_CTRL_SIZE))
4364 		mlo_debug_rl("ML rv seq common info len %zu doesn't match with expected common info len %zu",
4365 			     rv_cinfo_len,
4366 			     parsed_payload_len - WLAN_ML_CTRL_SIZE);
4367 
4368 	if (mlieseqpayloadlen < (WLAN_ML_CTRL_SIZE + rv_cinfo_len)) {
4369 		mlo_err_rl("ML seq payload len %zu insufficient for rv link info after parsed mutli-link control %u and indicated Common Info length %zu",
4370 			   mlieseqpayloadlen,
4371 			   WLAN_ML_CTRL_SIZE,
4372 			   rv_cinfo_len);
4373 		return QDF_STATUS_E_PROTO;
4374 	}
4375 	/* Update parsed_payload_len to reflect the actual bytes in common info
4376 	 * field to be compatible with future spec updating.
4377 	 */
4378 	parsed_payload_len = WLAN_ML_CTRL_SIZE + rv_cinfo_len;
4379 
4380 	if (link_info_len) {
4381 		*link_info_len = mlieseqpayloadlen - parsed_payload_len;
4382 		mlo_debug("link_info_len:%zu, parsed_payload_len:%zu, rv_cinfo_len %zu ",
4383 			  *link_info_len, parsed_payload_len, rv_cinfo_len);
4384 	}
4385 
4386 	if (mlieseqpayloadlen == parsed_payload_len) {
4387 		mlo_debug("No Link Info field present");
4388 		if (link_info)
4389 			*link_info = NULL;
4390 
4391 		return QDF_STATUS_SUCCESS;
4392 	}
4393 
4394 	if (link_info)
4395 		*link_info = mlieseqpayload + parsed_payload_len;
4396 
4397 	return QDF_STATUS_SUCCESS;
4398 }
4399 
4400 static QDF_STATUS
util_parse_rvmlie_perstaprofile_stactrl(uint8_t * subelempayload,qdf_size_t subelempayloadlen,uint8_t * linkid,bool * is_macaddr_valid,struct qdf_mac_addr * macaddr,bool * is_ap_removal_timer_valid,uint16_t * ap_removal_timer)4401 util_parse_rvmlie_perstaprofile_stactrl(uint8_t *subelempayload,
4402 					qdf_size_t subelempayloadlen,
4403 					uint8_t *linkid,
4404 					bool *is_macaddr_valid,
4405 					struct qdf_mac_addr *macaddr,
4406 					bool *is_ap_removal_timer_valid,
4407 					uint16_t *ap_removal_timer)
4408 {
4409 	qdf_size_t parsed_payload_len = 0, sta_info_len;
4410 	qdf_size_t parsed_sta_info_len;
4411 	uint16_t stacontrol;
4412 	uint8_t completeprofile;
4413 
4414 	/* This helper returns the location(s) and where required, the length(s)
4415 	 * of (sub)field(s) inferable after parsing the STA Control field in the
4416 	 * per-STA profile subelement. These location(s) and length(s) is/are in
4417 	 * reference to the payload section of the per-STA profile subelement
4418 	 * (after defragmentation, if applicable).  Here, the payload is the
4419 	 * point after the subelement length in the subelement, and includes the
4420 	 * payloads of all subsequent fragments (if any) but not the headers of
4421 	 * those fragments.
4422 	 *
4423 	 * Currently, the helper returns the link ID, MAC address, AP removal
4424 	 * timer and STA profile. More (sub)fields can be added when required.
4425 	 */
4426 	if (!subelempayload) {
4427 		mlo_err("Pointer to subelement payload is NULL");
4428 		return QDF_STATUS_E_NULL_VALUE;
4429 	}
4430 
4431 	if (!subelempayloadlen) {
4432 		mlo_err("Length of subelement payload is zero");
4433 		return QDF_STATUS_E_INVAL;
4434 	}
4435 
4436 	if (subelempayloadlen < WLAN_ML_RV_LINFO_PERSTAPROF_STACTRL_SIZE) {
4437 		mlo_err_rl("Subelement payload length %zu octets is smaller than STA control field of per-STA profile subelement %u octets",
4438 			   subelempayloadlen,
4439 			   WLAN_ML_RV_LINFO_PERSTAPROF_STACTRL_SIZE);
4440 		return QDF_STATUS_E_PROTO;
4441 	}
4442 
4443 	parsed_payload_len = 0;
4444 	qdf_mem_copy(&stacontrol,
4445 		     subelempayload,
4446 		     WLAN_ML_RV_LINFO_PERSTAPROF_STACTRL_SIZE);
4447 
4448 	stacontrol = qdf_le16_to_cpu(stacontrol);
4449 	parsed_payload_len += WLAN_ML_RV_LINFO_PERSTAPROF_STACTRL_SIZE;
4450 
4451 	if (linkid)
4452 		*linkid = QDF_GET_BITS(stacontrol,
4453 				WLAN_ML_RV_LINFO_PERSTAPROF_STACTRL_LINKID_IDX,
4454 				WLAN_ML_RV_LINFO_PERSTAPROF_STACTRL_LINKID_BITS);
4455 
4456 	/* Check if this a complete profile */
4457 	completeprofile = QDF_GET_BITS(stacontrol,
4458 				WLAN_ML_RV_LINFO_PERSTAPROF_STACTRL_CMPLTPROF_IDX,
4459 				WLAN_ML_RV_LINFO_PERSTAPROF_STACTRL_CMPLTPROF_BITS);
4460 
4461 	if (is_macaddr_valid)
4462 		*is_macaddr_valid = false;
4463 	if (subelempayloadlen < parsed_payload_len +
4464 		WLAN_ML_RV_LINFO_PERSTAPROF_STAINFO_LENGTH_SIZE) {
4465 		mlo_err_rl("Length of subelement payload %zu octets not sufficient for sta info length of size %u octets after parsed payload length of %zu octets.",
4466 			   subelempayloadlen, WLAN_ML_RV_LINFO_PERSTAPROF_STAINFO_LENGTH_SIZE,
4467 			   parsed_payload_len);
4468 		return QDF_STATUS_E_PROTO;
4469 	}
4470 
4471 	sta_info_len = *(subelempayload + parsed_payload_len);
4472 	parsed_payload_len += WLAN_ML_RV_LINFO_PERSTAPROF_STAINFO_LENGTH_SIZE;
4473 	parsed_sta_info_len = WLAN_ML_RV_LINFO_PERSTAPROF_STAINFO_LENGTH_SIZE;
4474 
4475 	/* Check STA MAC address present bit */
4476 	if (QDF_GET_BITS(stacontrol,
4477 			 WLAN_ML_RV_LINFO_PERSTAPROF_STACTRL_STAMACADDRP_IDX,
4478 			 WLAN_ML_RV_LINFO_PERSTAPROF_STACTRL_STAMACADDRP_BITS)) {
4479 		if (sta_info_len < (parsed_sta_info_len + QDF_MAC_ADDR_SIZE)) {
4480 			mlo_err_rl("Length of sta info len %zu octets not sufficient to contain MAC address of size %u octets after parsed sta info length of %zu octets.",
4481 				   sta_info_len, QDF_MAC_ADDR_SIZE,
4482 				   parsed_sta_info_len);
4483 			return QDF_STATUS_E_PROTO;
4484 		}
4485 
4486 		if (subelempayloadlen <
4487 				(parsed_payload_len + QDF_MAC_ADDR_SIZE)) {
4488 			mlo_err_rl("Length of subelement payload %zu octets not sufficient to contain MAC address of size %u octets after parsed payload length of %zu octets.",
4489 				   subelempayloadlen, QDF_MAC_ADDR_SIZE,
4490 				   parsed_payload_len);
4491 			return QDF_STATUS_E_PROTO;
4492 		}
4493 
4494 		if (macaddr) {
4495 			qdf_mem_copy(macaddr->bytes,
4496 				     subelempayload + parsed_payload_len,
4497 				     QDF_MAC_ADDR_SIZE);
4498 			mlo_nofl_debug("Copied MAC address: " QDF_MAC_ADDR_FMT,
4499 				       QDF_MAC_ADDR_REF(macaddr->bytes));
4500 
4501 			if (is_macaddr_valid)
4502 				*is_macaddr_valid = true;
4503 		}
4504 
4505 		parsed_payload_len += QDF_MAC_ADDR_SIZE;
4506 		parsed_sta_info_len += QDF_MAC_ADDR_SIZE;
4507 	}
4508 
4509 	/* Check AP Removal timer present bit */
4510 	if (QDF_GET_BITS(stacontrol,
4511 			 WLAN_ML_RV_LINFO_PERSTAPROF_STACTRL_APREMOVALTIMERP_IDX,
4512 			 WLAN_ML_RV_LINFO_PERSTAPROF_STACTRL_APREMOVALTIMERP_BITS)) {
4513 		if (sta_info_len <
4514 				(parsed_sta_info_len +
4515 				 WLAN_ML_RV_LINFO_PERSTAPROF_STAINFO_APREMOVALTIMER_SIZE)) {
4516 			mlo_err_rl("Length of sta info len %zu octets not sufficient to contain AP removal timer of size %u octets after parsed sta info length of %zu octets.",
4517 				   sta_info_len, WLAN_ML_RV_LINFO_PERSTAPROF_STAINFO_APREMOVALTIMER_SIZE,
4518 				   parsed_sta_info_len);
4519 			return QDF_STATUS_E_PROTO;
4520 		}
4521 
4522 		if (subelempayloadlen <
4523 				(parsed_payload_len +
4524 				 WLAN_ML_RV_LINFO_PERSTAPROF_STAINFO_APREMOVALTIMER_SIZE)) {
4525 			mlo_err_rl("Length of subelement payload %zu octets not sufficient to contain AP removal timer of size %u octets after parsed payload length of %zu octets.",
4526 				   subelempayloadlen,
4527 				   WLAN_ML_RV_LINFO_PERSTAPROF_STAINFO_APREMOVALTIMER_SIZE,
4528 				   parsed_payload_len);
4529 			return QDF_STATUS_E_PROTO;
4530 		}
4531 
4532 		if (ap_removal_timer) {
4533 			qdf_mem_copy(ap_removal_timer,
4534 				     subelempayload + parsed_payload_len,
4535 				     WLAN_ML_RV_LINFO_PERSTAPROF_STAINFO_APREMOVALTIMER_SIZE);
4536 
4537 			if (is_ap_removal_timer_valid)
4538 				*is_ap_removal_timer_valid = true;
4539 		}
4540 
4541 		parsed_payload_len +=
4542 			WLAN_ML_RV_LINFO_PERSTAPROF_STAINFO_APREMOVALTIMER_SIZE;
4543 		parsed_sta_info_len += WLAN_ML_RV_LINFO_PERSTAPROF_STAINFO_APREMOVALTIMER_SIZE;
4544 	}
4545 	/* At present, we only handle link MAC address field and ap removal
4546 	 * timer tbtt field parsing. To be compatible with future spec
4547 	 * updating, if new items are added to sta info, below log will
4548 	 * highlight the spec change.
4549 	 */
4550 	if (sta_info_len != (parsed_payload_len -
4551 			     WLAN_ML_RV_LINFO_PERSTAPROF_STACTRL_SIZE))
4552 		mlo_debug_rl("Length of sta info len %zu octets not match parsed payload length of %zu octets.",
4553 			     sta_info_len,
4554 			     parsed_payload_len -
4555 			     WLAN_ML_RV_LINFO_PERSTAPROF_STACTRL_SIZE);
4556 
4557 	return QDF_STATUS_SUCCESS;
4558 }
4559 
4560 static QDF_STATUS
util_parse_rv_info_from_linkinfo(uint8_t * linkinfo,qdf_size_t linkinfo_len,struct ml_rv_info * reconfig_info)4561 util_parse_rv_info_from_linkinfo(uint8_t *linkinfo,
4562 				 qdf_size_t linkinfo_len,
4563 				 struct ml_rv_info *reconfig_info)
4564 {
4565 	uint8_t linkid;
4566 	uint8_t *linkinfo_currpos;
4567 	qdf_size_t linkinfo_remlen;
4568 	bool is_subelemfragseq;
4569 	uint8_t subelemid;
4570 	qdf_size_t subelemseqtotallen;
4571 	qdf_size_t subelemseqpayloadlen;
4572 	qdf_size_t defragpayload_len;
4573 	QDF_STATUS ret;
4574 	struct qdf_mac_addr mac_addr;
4575 	bool is_macaddr_valid;
4576 	bool is_ap_removal_timer_valid;
4577 	uint16_t ap_removal_timer;
4578 
4579 	/* This helper function parses probe request info from the per-STA prof
4580 	 * present (if any) in the Link Info field in the payload of a Multi
4581 	 * Link element (after defragmentation if required). The caller should
4582 	 * pass a copy of the payload so that inline defragmentation of
4583 	 * subelements can be carried out if required. The subelement
4584 	 * defragmentation (if applicable) in this Control Path helper is
4585 	 * required for maintainability, accuracy and eliminating current and
4586 	 * future per-field-access multi-level fragment boundary checks and
4587 	 * adjustments, given the complex format of Multi Link elements. It is
4588 	 * also most likely to be required mainly at the client side.
4589 	 * Fragmentation is currently unlikely to be required for subelements
4590 	 * in Reconfiguration variant Multi-Link elements, but it should be
4591 	 * handled in order to be future ready.
4592 	 */
4593 	if (!linkinfo) {
4594 		mlo_err("linkinfo is NULL");
4595 		return QDF_STATUS_E_NULL_VALUE;
4596 	}
4597 
4598 	if (!linkinfo_len) {
4599 		mlo_err("linkinfo_len is zero");
4600 		return QDF_STATUS_E_NULL_VALUE;
4601 	}
4602 
4603 	if (!reconfig_info) {
4604 		mlo_err("ML reconfig info is NULL");
4605 		return QDF_STATUS_E_NULL_VALUE;
4606 	}
4607 
4608 	reconfig_info->num_links = 0;
4609 	linkinfo_currpos = linkinfo;
4610 	linkinfo_remlen = linkinfo_len;
4611 
4612 	while (linkinfo_remlen) {
4613 		if (linkinfo_remlen <  sizeof(struct subelem_header)) {
4614 			mlo_err_rl("Remaining length in link info %zu octets is smaller than subelement header length %zu octets",
4615 				   linkinfo_remlen,
4616 				   sizeof(struct subelem_header));
4617 			return QDF_STATUS_E_PROTO;
4618 		}
4619 
4620 		subelemid = linkinfo_currpos[ID_POS];
4621 		is_subelemfragseq = false;
4622 		subelemseqtotallen = 0;
4623 		subelemseqpayloadlen = 0;
4624 
4625 		ret = wlan_get_subelem_fragseq_info(WLAN_ML_LINFO_SUBELEMID_FRAGMENT,
4626 						    linkinfo_currpos,
4627 						    linkinfo_remlen,
4628 						    &is_subelemfragseq,
4629 						    &subelemseqtotallen,
4630 						    &subelemseqpayloadlen);
4631 		if (QDF_IS_STATUS_ERROR(ret))
4632 			return ret;
4633 
4634 		if (qdf_unlikely(is_subelemfragseq)) {
4635 			if (!subelemseqpayloadlen) {
4636 				mlo_err_rl("Subelement fragment sequence payload is reported as 0, investigate");
4637 				return QDF_STATUS_E_FAILURE;
4638 			}
4639 
4640 			mlo_debug("Subelement fragment sequence found with payload len %zu",
4641 				  subelemseqpayloadlen);
4642 
4643 			ret = wlan_defrag_subelem_fragseq(true,
4644 							  WLAN_ML_LINFO_SUBELEMID_FRAGMENT,
4645 							  linkinfo_currpos,
4646 							  linkinfo_remlen,
4647 							  NULL,
4648 							  0,
4649 							  &defragpayload_len);
4650 
4651 			if (QDF_IS_STATUS_ERROR(ret))
4652 				return ret;
4653 
4654 			if (defragpayload_len != subelemseqpayloadlen) {
4655 				mlo_err_rl("Length of defragmented payload %zu octets is not equal to length of subelement fragment sequence payload %zu octets",
4656 					   defragpayload_len,
4657 					   subelemseqpayloadlen);
4658 				return QDF_STATUS_E_FAILURE;
4659 			}
4660 
4661 			/* Adjust linkinfo_remlen to reflect removal of all
4662 			 * subelement headers except the header of the lead
4663 			 * subelement.
4664 			 */
4665 			linkinfo_remlen -= (subelemseqtotallen -
4666 					    subelemseqpayloadlen -
4667 					    sizeof(struct subelem_header));
4668 		} else {
4669 			if (linkinfo_remlen <
4670 				(sizeof(struct subelem_header) +
4671 				linkinfo_currpos[TAG_LEN_POS])) {
4672 				mlo_err_rl("Remaining length in link info %zu octets is smaller than total size of current subelement %zu octets",
4673 					   linkinfo_remlen,
4674 					   sizeof(struct subelem_header) +
4675 					   linkinfo_currpos[TAG_LEN_POS]);
4676 				return QDF_STATUS_E_PROTO;
4677 			}
4678 
4679 			subelemseqpayloadlen = linkinfo_currpos[TAG_LEN_POS];
4680 		}
4681 
4682 		if (subelemid == WLAN_ML_LINFO_SUBELEMID_PERSTAPROFILE) {
4683 			struct ml_rv_partner_link_info *link_info;
4684 			is_macaddr_valid = false;
4685 			is_ap_removal_timer_valid = false;
4686 			ret = util_parse_rvmlie_perstaprofile_stactrl(linkinfo_currpos +
4687 								      sizeof(struct subelem_header),
4688 								      subelemseqpayloadlen,
4689 								      &linkid,
4690 								      &is_macaddr_valid,
4691 								      &mac_addr,
4692 								      &is_ap_removal_timer_valid,
4693 								      &ap_removal_timer);
4694 			if (QDF_IS_STATUS_ERROR(ret))
4695 				return ret;
4696 			if (reconfig_info->num_links >=
4697 						WLAN_UMAC_MLO_MAX_VDEVS) {
4698 				mlo_err("num_link %d invalid",
4699 					reconfig_info->num_links);
4700 				return QDF_STATUS_E_INVAL;
4701 			}
4702 			link_info =
4703 			&reconfig_info->link_info[reconfig_info->num_links];
4704 			link_info->link_id = linkid;
4705 			link_info->is_ap_removal_timer_p = is_ap_removal_timer_valid;
4706 			if (is_macaddr_valid)
4707 				qdf_copy_macaddr(&link_info->link_mac_addr,
4708 						 &mac_addr);
4709 
4710 			if (is_ap_removal_timer_valid)
4711 				link_info->ap_removal_timer = ap_removal_timer;
4712 			else
4713 				mlo_warn_rl("AP removal timer not found in STA Info field of per-STA profile with link ID %u",
4714 					    linkid);
4715 
4716 			mlo_debug("Per-STA Profile Link ID: %u AP removal timer present: %d AP removal timer: %u",
4717 				  link_info->link_id,
4718 				  link_info->is_ap_removal_timer_p,
4719 				  link_info->ap_removal_timer);
4720 
4721 			reconfig_info->num_links++;
4722 		}
4723 
4724 		linkinfo_remlen -= (sizeof(struct subelem_header) +
4725 				    subelemseqpayloadlen);
4726 		linkinfo_currpos += (sizeof(struct subelem_header) +
4727 				     subelemseqpayloadlen);
4728 	}
4729 
4730 	mlo_debug("Number of ML probe request links found=%u",
4731 		  reconfig_info->num_links);
4732 
4733 	return QDF_STATUS_SUCCESS;
4734 }
4735 
util_get_rvmlie_persta_link_info(uint8_t * mlieseq,qdf_size_t mlieseqlen,struct ml_rv_info * reconfig_info)4736 QDF_STATUS util_get_rvmlie_persta_link_info(uint8_t *mlieseq,
4737 					    qdf_size_t mlieseqlen,
4738 					    struct ml_rv_info *reconfig_info)
4739 {
4740 	struct wlan_ie_multilink *mlie_fixed;
4741 	uint16_t mlcontrol;
4742 	enum wlan_ml_variant variant;
4743 	uint8_t *linkinfo;
4744 	qdf_size_t linkinfo_len;
4745 	struct ml_rv_info rinfo = {0};
4746 	qdf_size_t mlieseqpayloadlen;
4747 	uint8_t *mlieseqpayload_copy;
4748 	bool is_elemfragseq;
4749 	qdf_size_t defragpayload_len;
4750 	qdf_size_t tmplen;
4751 	QDF_STATUS ret;
4752 
4753 	if (!mlieseq) {
4754 		mlo_err("Pointer to Multi-Link element sequence is NULL");
4755 		return QDF_STATUS_E_NULL_VALUE;
4756 	}
4757 
4758 	if (!mlieseqlen) {
4759 		mlo_err("Length of Multi-Link element sequence is zero");
4760 		return QDF_STATUS_E_INVAL;
4761 	}
4762 
4763 	if (!reconfig_info) {
4764 		mlo_err("reconfig_info is NULL");
4765 		return QDF_STATUS_E_NULL_VALUE;
4766 	}
4767 
4768 	reconfig_info->num_links = 0;
4769 
4770 	if (mlieseqlen < sizeof(struct wlan_ie_multilink)) {
4771 		mlo_err_rl("Multi-Link element sequence length %zu octets is smaller than required for the fixed portion of Multi-Link element (%zu octets)",
4772 			   mlieseqlen, sizeof(struct wlan_ie_multilink));
4773 		return QDF_STATUS_E_INVAL;
4774 	}
4775 
4776 	mlie_fixed = (struct wlan_ie_multilink *)mlieseq;
4777 	if (mlie_fixed->elem_id != WLAN_ELEMID_EXTN_ELEM ||
4778 	    mlie_fixed->elem_id_ext != WLAN_EXTN_ELEMID_MULTI_LINK) {
4779 		mlo_err("The element is not a Multi-Link element");
4780 		return QDF_STATUS_E_INVAL;
4781 	}
4782 
4783 	mlcontrol = qdf_le16_to_cpu(mlie_fixed->mlcontrol);
4784 
4785 	variant = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_TYPE_IDX,
4786 			       WLAN_ML_CTRL_TYPE_BITS);
4787 
4788 	if (variant != WLAN_ML_VARIANT_RECONFIG) {
4789 		mlo_err("The variant value %u does not correspond to Reconfig Variant value %u",
4790 			variant, WLAN_ML_VARIANT_RECONFIG);
4791 		return QDF_STATUS_E_INVAL;
4792 	}
4793 
4794 	mlieseqpayloadlen = 0;
4795 	tmplen = 0;
4796 	is_elemfragseq = false;
4797 
4798 	ret = wlan_get_elem_fragseq_info(mlieseq,
4799 					 mlieseqlen,
4800 					 &is_elemfragseq,
4801 					 &tmplen,
4802 					 &mlieseqpayloadlen);
4803 
4804 	if (QDF_IS_STATUS_ERROR(ret))
4805 		return ret;
4806 
4807 	if (qdf_unlikely(is_elemfragseq)) {
4808 		if (tmplen != mlieseqlen) {
4809 			mlo_err_rl("Mismatch in values of element fragment sequence total length. Val per frag info determination: %zu octets, val passed as arg: %zu octets",
4810 				   tmplen, mlieseqlen);
4811 			return QDF_STATUS_E_INVAL;
4812 		}
4813 
4814 		if (!mlieseqpayloadlen) {
4815 			mlo_err_rl("Multi-Link element fragment sequence payload is reported as 0, investigate");
4816 			return QDF_STATUS_E_FAILURE;
4817 		}
4818 
4819 		mlo_debug("Multi-Link element fragment sequence found with payload len %zu",
4820 			  mlieseqpayloadlen);
4821 	} else {
4822 		if (mlieseqlen > (sizeof(struct ie_header) + WLAN_MAX_IE_LEN)) {
4823 			mlo_err_rl("Expected presence of valid fragment sequence since Multi-Link element sequence length %zu octets is larger than frag threshold of %zu octets, however no valid fragment sequence found",
4824 				   mlieseqlen,
4825 				   sizeof(struct ie_header) + WLAN_MAX_IE_LEN);
4826 			return QDF_STATUS_E_FAILURE;
4827 		}
4828 
4829 		mlieseqpayloadlen = mlieseqlen - (sizeof(struct ie_header) + 1);
4830 	}
4831 
4832 	mlieseqpayload_copy = qdf_mem_malloc(mlieseqpayloadlen);
4833 
4834 	if (!mlieseqpayload_copy) {
4835 		mlo_err_rl("Could not allocate memory for Multi-Link element payload copy");
4836 		return QDF_STATUS_E_NOMEM;
4837 	}
4838 
4839 	if (qdf_unlikely(is_elemfragseq)) {
4840 		ret = wlan_defrag_elem_fragseq(false,
4841 					       mlieseq,
4842 					       mlieseqlen,
4843 					       mlieseqpayload_copy,
4844 					       mlieseqpayloadlen,
4845 					       &defragpayload_len);
4846 		if (QDF_IS_STATUS_ERROR(ret)) {
4847 			qdf_mem_free(mlieseqpayload_copy);
4848 			return ret;
4849 		}
4850 
4851 		if (defragpayload_len != mlieseqpayloadlen) {
4852 			mlo_err_rl("Length of de-fragmented payload %zu octets is not equal to length of Multi-Link element fragment sequence payload %zu octets",
4853 				   defragpayload_len, mlieseqpayloadlen);
4854 			qdf_mem_free(mlieseqpayload_copy);
4855 			return QDF_STATUS_E_FAILURE;
4856 		}
4857 	} else {
4858 		qdf_mem_copy(mlieseqpayload_copy,
4859 			     mlieseq + sizeof(struct ie_header) + 1,
4860 			     mlieseqpayloadlen);
4861 	}
4862 
4863 	linkinfo = NULL;
4864 	linkinfo_len = 0;
4865 
4866 	ret = util_parse_rv_multi_link_ctrl(mlieseqpayload_copy,
4867 					    mlieseqpayloadlen,
4868 					    &linkinfo,
4869 					    &linkinfo_len);
4870 	if (QDF_IS_STATUS_ERROR(ret)) {
4871 		qdf_mem_free(mlieseqpayload_copy);
4872 		return ret;
4873 	}
4874 
4875 	/* In case Link Info is absent, the number of links will remain
4876 	 * zero.
4877 	 */
4878 	if (!linkinfo) {
4879 		qdf_mem_free(mlieseqpayload_copy);
4880 		return QDF_STATUS_SUCCESS;
4881 	}
4882 
4883 	ret = util_parse_rv_info_from_linkinfo(linkinfo, linkinfo_len, &rinfo);
4884 	if (QDF_IS_STATUS_ERROR(ret)) {
4885 		qdf_mem_free(mlieseqpayload_copy);
4886 		return ret;
4887 	}
4888 
4889 	qdf_mem_copy(reconfig_info, &rinfo, sizeof(*reconfig_info));
4890 	qdf_mem_free(mlieseqpayload_copy);
4891 
4892 	return QDF_STATUS_SUCCESS;
4893 }
4894 
4895 static QDF_STATUS
util_parse_pa_multi_link_ctrl(uint8_t * mlieseqpayload,qdf_size_t mlieseqpayloadlen,uint8_t ** link_info,qdf_size_t * link_info_len)4896 util_parse_pa_multi_link_ctrl(uint8_t *mlieseqpayload,
4897 			      qdf_size_t mlieseqpayloadlen,
4898 			      uint8_t **link_info,
4899 			      qdf_size_t *link_info_len)
4900 {
4901 	qdf_size_t parsed_payload_len;
4902 
4903 	/* This helper returns the location(s) and length(s) of (sub)field(s)
4904 	 * inferable after parsing the Multi Link element Control field. These
4905 	 * location(s) and length(s) is/are in reference to the payload section
4906 	 * of the Multi Link element (after defragmentation, if applicable).
4907 	 * Here, the payload is the point after the element ID extension of the
4908 	 * Multi Link element, and includes the payloads of all subsequent
4909 	 * fragments (if any) but not the headers of those fragments.
4910 	 *
4911 	 * Currently, the helper returns the location and length of the Link
4912 	 * Info field in the Multi Link element sequence. Other (sub)field(s)
4913 	 * can be added later as required.
4914 	 */
4915 	if (!mlieseqpayload) {
4916 		mlo_err("ML seq payload pointer is NULL");
4917 		return QDF_STATUS_E_NULL_VALUE;
4918 	}
4919 
4920 	if (!mlieseqpayloadlen) {
4921 		mlo_err("ML seq payload len is 0");
4922 		return QDF_STATUS_E_INVAL;
4923 	}
4924 
4925 	if (mlieseqpayloadlen < WLAN_ML_CTRL_SIZE) {
4926 		mlo_err_rl("ML seq payload len %zu < ML Control size %u",
4927 			   mlieseqpayloadlen, WLAN_ML_CTRL_SIZE);
4928 		return QDF_STATUS_E_PROTO;
4929 	}
4930 
4931 	parsed_payload_len = 0;
4932 
4933 	parsed_payload_len += WLAN_ML_CTRL_SIZE;
4934 
4935 	if (mlieseqpayloadlen <
4936 		(parsed_payload_len +
4937 		WLAN_ML_PAV_CINFO_LENGTH_MAX)) {
4938 		mlo_err_rl("ML seq payload len %zu insufficient for MLD cmn size %u after parsed payload len %zu.",
4939 			   mlieseqpayloadlen,
4940 			   WLAN_ML_PAV_CINFO_LENGTH_MAX,
4941 			   parsed_payload_len);
4942 		return QDF_STATUS_E_PROTO;
4943 	}
4944 
4945 	parsed_payload_len += QDF_MAC_ADDR_SIZE + WLAN_ML_PAV_CINFO_LENGTH_SIZE;
4946 
4947 	if (link_info_len) {
4948 		*link_info_len = mlieseqpayloadlen - parsed_payload_len;
4949 		mlo_debug("link_info_len:%zu, parsed_payload_len:%zu",
4950 			  *link_info_len, parsed_payload_len);
4951 	}
4952 
4953 	if (mlieseqpayloadlen == parsed_payload_len) {
4954 		mlo_debug("No Link Info field present");
4955 		if (link_info)
4956 			*link_info = NULL;
4957 
4958 		return QDF_STATUS_SUCCESS;
4959 	}
4960 
4961 	if (link_info)
4962 		*link_info = mlieseqpayload + parsed_payload_len;
4963 
4964 	return QDF_STATUS_SUCCESS;
4965 }
4966 
4967 static QDF_STATUS
util_parse_pamlie_perstaprofile_stactrl(uint8_t * subelempayload,qdf_size_t subelempayloadlen,struct ml_pa_partner_link_info * pa_link_info)4968 util_parse_pamlie_perstaprofile_stactrl(uint8_t *subelempayload,
4969 					qdf_size_t subelempayloadlen,
4970 					struct ml_pa_partner_link_info *pa_link_info)
4971 {
4972 	qdf_size_t parsed_payload_len = 0;
4973 	uint16_t stacontrol;
4974 	struct ie_header *ie;
4975 	struct extn_ie_header *extn_ie;
4976 
4977 	/* This helper returns the location(s) and where required, the length(s)
4978 	 * of (sub)field(s) inferable after parsing the STA Control field in the
4979 	 * per-STA profile subelement. These location(s) and length(s) is/are in
4980 	 * reference to the payload section of the per-STA profile subelement
4981 	 * (after defragmentation, if applicable).  Here, the payload is the
4982 	 * point after the subelement length in the subelement, and includes the
4983 	 * payloads of all subsequent fragments (if any) but not the headers of
4984 	 * those fragments.
4985 	 *
4986 	 * Currently, the helper returns the priority access link information
4987 	 * for all parner links.
4988 	 */
4989 	if (!subelempayload) {
4990 		mlo_err("Pointer to subelement payload is NULL");
4991 		return QDF_STATUS_E_NULL_VALUE;
4992 	}
4993 
4994 	if (!subelempayloadlen) {
4995 		mlo_err("Length of subelement payload is zero");
4996 		return QDF_STATUS_E_INVAL;
4997 	}
4998 
4999 	if (subelempayloadlen < WLAN_ML_PAV_LINFO_PERSTAPROF_STACTRL_SIZE) {
5000 		mlo_err_rl("Subelement payload length %zu octets is smaller than STA control field of per-STA profile subelement %u octets",
5001 			   subelempayloadlen,
5002 			   WLAN_ML_PAV_LINFO_PERSTAPROF_STACTRL_SIZE);
5003 		return QDF_STATUS_E_PROTO;
5004 	}
5005 
5006 	parsed_payload_len = 0;
5007 	qdf_mem_copy(&stacontrol,
5008 		     subelempayload,
5009 		     WLAN_ML_PAV_LINFO_PERSTAPROF_STACTRL_SIZE);
5010 
5011 	stacontrol = qdf_le16_to_cpu(stacontrol);
5012 	parsed_payload_len += WLAN_ML_PAV_LINFO_PERSTAPROF_STACTRL_SIZE;
5013 
5014 	subelempayload += WLAN_ML_PAV_LINFO_PERSTAPROF_STACTRL_SIZE;
5015 
5016 	pa_link_info->link_id =
5017 		QDF_GET_BITS(stacontrol,
5018 			     WLAN_ML_PAV_LINFO_PERSTAPROF_STACTRL_LINKID_IDX,
5019 			     WLAN_ML_PAV_LINFO_PERSTAPROF_STACTRL_LINKID_BITS);
5020 
5021 	pa_link_info->edca_ie_present = false;
5022 	pa_link_info->ven_wme_ie_present = false;
5023 	pa_link_info->muedca_ie_present = false;
5024 
5025 	do {
5026 		if (subelempayloadlen <
5027 			(parsed_payload_len +
5028 			sizeof(struct ie_header))) {
5029 			mlo_err_rl("Length of subelement payload %zu octets not sufficient to contain min ie length %zu after parsed payload length of %zu octets",
5030 				   subelempayloadlen,
5031 				   sizeof(struct ie_header),
5032 				   parsed_payload_len);
5033 			return QDF_STATUS_E_PROTO;
5034 		}
5035 
5036 		ie = (struct ie_header *)subelempayload;
5037 
5038 		if (subelempayloadlen <
5039 			(parsed_payload_len +
5040 			(sizeof(struct ie_header) + ie->ie_len))) {
5041 			mlo_err_rl("Length of subelement payload %zu octets not sufficient to contain ie length %zu after parsed payload length of %zu octets",
5042 				   subelempayloadlen,
5043 				   sizeof(struct ie_header) + ie->ie_len,
5044 				   parsed_payload_len);
5045 			return QDF_STATUS_E_PROTO;
5046 		}
5047 
5048 		switch (ie->ie_id) {
5049 		case WLAN_ELEMID_EDCAPARMS:
5050 			if (pa_link_info->ven_wme_ie_present) {
5051 				/* WME parameters already present
5052 				 * use that one instead of EDCA */
5053 				break;
5054 			}
5055 			if (ie->ie_len == (sizeof(struct edca_ie) -
5056 			    sizeof(struct ie_header))) {
5057 				pa_link_info->edca_ie_present = true;
5058 				qdf_mem_copy(&pa_link_info->edca,
5059 					     subelempayload,
5060 					     sizeof(struct edca_ie));
5061 			} else {
5062 				epcs_debug("Invalid edca length %d in PAV IE",
5063 					   ie->ie_len);
5064 			}
5065 			break;
5066 		case WLAN_ELEMID_VENDOR:
5067 			if (is_wme_param((uint8_t *)ie) &&
5068 			    (ie->ie_len == WLAN_VENDOR_WME_IE_LEN)) {
5069 				pa_link_info->ven_wme_ie_present = true;
5070 				qdf_mem_copy(&pa_link_info->ven_wme_ie_bytes,
5071 					     subelempayload,
5072 					     sizeof(WLAN_VENDOR_WME_IE_LEN +
5073 						    sizeof(struct ie_header)));
5074 				pa_link_info->edca_ie_present = false;
5075 			} else {
5076 				epcs_debug("Unrelated Venfor IE reecived ie_id %d ie_len %d",
5077 					   ie->ie_id,
5078 					   ie->ie_len);
5079 			}
5080 			break;
5081 		case WLAN_ELEMID_EXTN_ELEM:
5082 			extn_ie = (struct extn_ie_header *)ie;
5083 			switch (extn_ie->ie_extn_id) {
5084 			case WLAN_EXTN_ELEMID_MUEDCA:
5085 				if (extn_ie->ie_len == WLAN_MAX_MUEDCA_IE_LEN) {
5086 					pa_link_info->muedca_ie_present = true;
5087 					qdf_mem_copy(&pa_link_info->muedca,
5088 						     subelempayload,
5089 						     sizeof(struct muedca_ie));
5090 				} else {
5091 					epcs_debug("Invalid muedca length %d in PAV IE",
5092 						   ie->ie_len);
5093 				}
5094 				break;
5095 			default:
5096 				epcs_debug("Unrelated Extn IE reecived ie_id %d ie_len %d extid %d IN PAV IE",
5097 					   ie->ie_id,
5098 					   ie->ie_len,
5099 					   extn_ie->ie_extn_id);
5100 				break;
5101 			}
5102 			break;
5103 		default:
5104 			epcs_debug("Unrelated IE reecived ie_id %d ie_len %d in PAV IE",
5105 				   ie->ie_id,
5106 				   ie->ie_len);
5107 			break;
5108 		}
5109 		subelempayload += ie->ie_len + sizeof(struct ie_header);
5110 		parsed_payload_len += ie->ie_len + sizeof(struct ie_header);
5111 	} while (parsed_payload_len < subelempayloadlen);
5112 
5113 	if (parsed_payload_len != subelempayloadlen)
5114 		epcs_debug("Error in processing per sta profile of PA ML IE %zu %zu",
5115 			   parsed_payload_len,
5116 			   subelempayloadlen);
5117 
5118 	epcs_debug("Link id %d presence of edca %d muedca %d wme %d",
5119 		   pa_link_info->link_id,
5120 		   pa_link_info->edca_ie_present,
5121 		   pa_link_info->muedca_ie_present,
5122 		   pa_link_info->ven_wme_ie_present);
5123 
5124 	return QDF_STATUS_SUCCESS;
5125 }
5126 
5127 static QDF_STATUS
util_parse_pa_info_from_linkinfo(uint8_t * linkinfo,qdf_size_t linkinfo_len,struct ml_pa_info * pa_info)5128 util_parse_pa_info_from_linkinfo(uint8_t *linkinfo,
5129 				 qdf_size_t linkinfo_len,
5130 				 struct ml_pa_info *pa_info)
5131 {
5132 	uint8_t *linkinfo_currpos;
5133 	qdf_size_t linkinfo_remlen;
5134 	bool is_subelemfragseq;
5135 	uint8_t subelemid;
5136 	qdf_size_t subelemseqtotallen;
5137 	qdf_size_t subelemseqpayloadlen;
5138 	qdf_size_t defragpayload_len;
5139 	QDF_STATUS ret;
5140 
5141 	/* This helper function parses priority access info from the per-STA
5142 	 * prof present (if any) in the Link Info field in the payload of a
5143 	 * Multi Link element (after defragmentation if required). The caller
5144 	 * should pass a copy of the payload so that inline defragmentation of
5145 	 * subelements can be carried out if required. The subelement
5146 	 * defragmentation (if applicable) in this Control Path helper is
5147 	 * required for maintainability, accuracy and eliminating current and
5148 	 * future per-field-access multi-level fragment boundary checks and
5149 	 * adjustments, given the complex format of Multi Link elements. It is
5150 	 * also most likely to be required mainly at the client side.
5151 	 * Fragmentation is currently unlikely to be required for subelements
5152 	 * in Reconfiguration variant Multi-Link elements, but it should be
5153 	 * handled in order to be future ready.
5154 	 */
5155 	if (!linkinfo) {
5156 		mlo_err("linkinfo is NULL");
5157 		return QDF_STATUS_E_NULL_VALUE;
5158 	}
5159 
5160 	if (!linkinfo_len) {
5161 		mlo_err("linkinfo_len is zero");
5162 		return QDF_STATUS_E_NULL_VALUE;
5163 	}
5164 
5165 	if (!pa_info) {
5166 		mlo_err("ML pa info is NULL");
5167 		return QDF_STATUS_E_NULL_VALUE;
5168 	}
5169 
5170 	pa_info->num_links = 0;
5171 	linkinfo_currpos = linkinfo;
5172 	linkinfo_remlen = linkinfo_len;
5173 
5174 	while (linkinfo_remlen) {
5175 		if (linkinfo_remlen <  sizeof(struct subelem_header)) {
5176 			mlo_err_rl("Remaining length in link info %zu octets is smaller than subelement header length %zu octets",
5177 				   linkinfo_remlen,
5178 				   sizeof(struct subelem_header));
5179 			return QDF_STATUS_E_PROTO;
5180 		}
5181 
5182 		subelemid = linkinfo_currpos[ID_POS];
5183 		is_subelemfragseq = false;
5184 		subelemseqtotallen = 0;
5185 		subelemseqpayloadlen = 0;
5186 
5187 		ret = wlan_get_subelem_fragseq_info(WLAN_ML_LINFO_SUBELEMID_FRAGMENT,
5188 						    linkinfo_currpos,
5189 						    linkinfo_remlen,
5190 						    &is_subelemfragseq,
5191 						    &subelemseqtotallen,
5192 						    &subelemseqpayloadlen);
5193 		if (QDF_IS_STATUS_ERROR(ret))
5194 			return ret;
5195 
5196 		if (qdf_unlikely(is_subelemfragseq)) {
5197 			if (!subelemseqpayloadlen) {
5198 				mlo_err_rl("Subelement fragment sequence payload is reported as 0, investigate");
5199 				return QDF_STATUS_E_FAILURE;
5200 			}
5201 
5202 			mlo_debug("Subelement fragment sequence found with payload len %zu",
5203 				  subelemseqpayloadlen);
5204 
5205 			ret = wlan_defrag_subelem_fragseq(true,
5206 							  WLAN_ML_LINFO_SUBELEMID_FRAGMENT,
5207 							  linkinfo_currpos,
5208 							  linkinfo_remlen,
5209 							  NULL,
5210 							  0,
5211 							  &defragpayload_len);
5212 
5213 			if (QDF_IS_STATUS_ERROR(ret))
5214 				return ret;
5215 
5216 			if (defragpayload_len != subelemseqpayloadlen) {
5217 				mlo_err_rl("Length of defragmented payload %zu octets is not equal to length of subelement fragment sequence payload %zu octets",
5218 					   defragpayload_len,
5219 					   subelemseqpayloadlen);
5220 				return QDF_STATUS_E_FAILURE;
5221 			}
5222 
5223 			/* Adjust linkinfo_remlen to reflect removal of all
5224 			 * subelement headers except the header of the lead
5225 			 * subelement.
5226 			 */
5227 			linkinfo_remlen -= (subelemseqtotallen -
5228 					    subelemseqpayloadlen -
5229 					    sizeof(struct subelem_header));
5230 		} else {
5231 			if (linkinfo_remlen <
5232 				(sizeof(struct subelem_header) +
5233 				linkinfo_currpos[TAG_LEN_POS])) {
5234 				mlo_err_rl("Remaining length in link info %zu octets is smaller than total size of current subelement %zu octets",
5235 					   linkinfo_remlen,
5236 					   sizeof(struct subelem_header) +
5237 					   linkinfo_currpos[TAG_LEN_POS]);
5238 				return QDF_STATUS_E_PROTO;
5239 			}
5240 
5241 			subelemseqpayloadlen = linkinfo_currpos[TAG_LEN_POS];
5242 		}
5243 
5244 		if (subelemid == WLAN_ML_LINFO_SUBELEMID_PERSTAPROFILE) {
5245 			struct ml_pa_partner_link_info *link_info =
5246 					&pa_info->link_info[pa_info->num_links];
5247 			ret = util_parse_pamlie_perstaprofile_stactrl(linkinfo_currpos +
5248 								      sizeof(struct subelem_header),
5249 								      subelemseqpayloadlen,
5250 								      link_info);
5251 			if (QDF_IS_STATUS_ERROR(ret))
5252 				return ret;
5253 		}
5254 
5255 		pa_info->num_links++;
5256 
5257 		linkinfo_remlen -= (sizeof(struct subelem_header) +
5258 				    subelemseqpayloadlen);
5259 		linkinfo_currpos += (sizeof(struct subelem_header) +
5260 				     subelemseqpayloadlen);
5261 	}
5262 
5263 	mlo_debug("Number of ML probe request links found=%u",
5264 		  pa_info->num_links);
5265 
5266 	return QDF_STATUS_SUCCESS;
5267 }
5268 
5269 QDF_STATUS
util_get_pav_mlie_link_info(uint8_t * mlieseq,qdf_size_t mlieseqlen,struct ml_pa_info * pa_info)5270 util_get_pav_mlie_link_info(uint8_t *mlieseq,
5271 			    qdf_size_t mlieseqlen,
5272 			    struct ml_pa_info *pa_info)
5273 {
5274 	struct wlan_ie_multilink *mlie_fixed;
5275 	uint16_t mlcontrol;
5276 	enum wlan_ml_variant variant;
5277 	uint8_t *linkinfo;
5278 	qdf_size_t linkinfo_len;
5279 	struct ml_pa_info painfo = {0};
5280 	qdf_size_t mlieseqpayloadlen;
5281 	uint8_t *mlieseqpayload_copy;
5282 	bool is_elemfragseq;
5283 	qdf_size_t defragpayload_len;
5284 	qdf_size_t tmplen;
5285 	QDF_STATUS ret;
5286 
5287 	if (!mlieseq) {
5288 		mlo_err("Pointer to Multi-Link element sequence is NULL");
5289 		return QDF_STATUS_E_NULL_VALUE;
5290 	}
5291 
5292 	if (!mlieseqlen) {
5293 		mlo_err("Length of Multi-Link element sequence is zero");
5294 		return QDF_STATUS_E_INVAL;
5295 	}
5296 
5297 	if (!pa_info) {
5298 		mlo_err("pa_info is NULL");
5299 		return QDF_STATUS_E_NULL_VALUE;
5300 	}
5301 
5302 	pa_info->num_links = 0;
5303 
5304 	if (mlieseqlen < sizeof(struct wlan_ie_multilink)) {
5305 		mlo_err_rl("Multi-Link element sequence length %zu octets is smaller than required for the fixed portion of Multi-Link element (%zu octets)",
5306 			   mlieseqlen, sizeof(struct wlan_ie_multilink));
5307 		return QDF_STATUS_E_INVAL;
5308 	}
5309 
5310 	mlie_fixed = (struct wlan_ie_multilink *)mlieseq;
5311 	if (mlie_fixed->elem_id != WLAN_ELEMID_EXTN_ELEM ||
5312 	    mlie_fixed->elem_id_ext != WLAN_EXTN_ELEMID_MULTI_LINK) {
5313 		mlo_err("The element is not a Multi-Link element");
5314 		return QDF_STATUS_E_INVAL;
5315 	}
5316 
5317 	mlcontrol = qdf_le16_to_cpu(mlie_fixed->mlcontrol);
5318 
5319 	variant = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_TYPE_IDX,
5320 			       WLAN_ML_CTRL_TYPE_BITS);
5321 
5322 	if (variant != WLAN_ML_VARIANT_PRIORITYACCESS) {
5323 		mlo_err("The variant value %u does not correspond to priority access Variant value %u",
5324 			variant, WLAN_ML_VARIANT_PRIORITYACCESS);
5325 		return QDF_STATUS_E_INVAL;
5326 	}
5327 
5328 	mlieseqpayloadlen = 0;
5329 	tmplen = 0;
5330 	is_elemfragseq = false;
5331 
5332 	ret = wlan_get_elem_fragseq_info(mlieseq,
5333 					 mlieseqlen,
5334 					 &is_elemfragseq,
5335 					 &tmplen,
5336 					 &mlieseqpayloadlen);
5337 
5338 	if (QDF_IS_STATUS_ERROR(ret))
5339 		return ret;
5340 
5341 	if (qdf_unlikely(is_elemfragseq)) {
5342 		if (tmplen != mlieseqlen) {
5343 			mlo_err_rl("Mismatch in values of element fragment sequence total length. Val per frag info determination: %zu octets, val passed as arg: %zu octets",
5344 				   tmplen, mlieseqlen);
5345 			return QDF_STATUS_E_INVAL;
5346 		}
5347 
5348 		if (!mlieseqpayloadlen) {
5349 			mlo_err_rl("Multi-Link element fragment sequence payload is reported as 0, investigate");
5350 			return QDF_STATUS_E_FAILURE;
5351 		}
5352 
5353 		mlo_debug("Multi-Link element fragment sequence found with payload len %zu",
5354 			  mlieseqpayloadlen);
5355 	} else {
5356 		if (mlieseqlen > (sizeof(struct ie_header) + WLAN_MAX_IE_LEN)) {
5357 			mlo_err_rl("Expected presence of valid fragment sequence since Multi-Link element sequence length %zu octets is larger than frag threshold of %zu octets, however no valid fragment sequence found",
5358 				   mlieseqlen,
5359 				   sizeof(struct ie_header) + WLAN_MAX_IE_LEN);
5360 			return QDF_STATUS_E_FAILURE;
5361 		}
5362 
5363 		mlieseqpayloadlen = mlieseqlen - (sizeof(struct ie_header) + 1);
5364 	}
5365 
5366 	mlieseqpayload_copy = qdf_mem_malloc(mlieseqpayloadlen);
5367 
5368 	if (!mlieseqpayload_copy) {
5369 		mlo_err_rl("Could not allocate memory for Multi-Link element payload copy");
5370 		return QDF_STATUS_E_NOMEM;
5371 	}
5372 
5373 	if (qdf_unlikely(is_elemfragseq)) {
5374 		ret = wlan_defrag_elem_fragseq(false,
5375 					       mlieseq,
5376 					       mlieseqlen,
5377 					       mlieseqpayload_copy,
5378 					       mlieseqpayloadlen,
5379 					       &defragpayload_len);
5380 		if (QDF_IS_STATUS_ERROR(ret)) {
5381 			qdf_mem_free(mlieseqpayload_copy);
5382 			return ret;
5383 		}
5384 
5385 		if (defragpayload_len != mlieseqpayloadlen) {
5386 			mlo_err_rl("Length of de-fragmented payload %zu octets is not equal to length of Multi-Link element fragment sequence payload %zu octets",
5387 				   defragpayload_len, mlieseqpayloadlen);
5388 			qdf_mem_free(mlieseqpayload_copy);
5389 			return QDF_STATUS_E_FAILURE;
5390 		}
5391 	} else {
5392 		qdf_mem_copy(mlieseqpayload_copy,
5393 			     mlieseq + sizeof(struct ie_header) + 1,
5394 			     mlieseqpayloadlen);
5395 	}
5396 
5397 	linkinfo = NULL;
5398 	linkinfo_len = 0;
5399 
5400 	ret = util_parse_pa_multi_link_ctrl(mlieseqpayload_copy,
5401 					    mlieseqpayloadlen,
5402 					    &linkinfo,
5403 					    &linkinfo_len);
5404 	if (QDF_IS_STATUS_ERROR(ret)) {
5405 		qdf_mem_free(mlieseqpayload_copy);
5406 		return ret;
5407 	}
5408 
5409 	/* In case Link Info is absent, the number of links will remain
5410 	 * zero.
5411 	 */
5412 	if (!linkinfo) {
5413 		qdf_mem_free(mlieseqpayload_copy);
5414 		return QDF_STATUS_SUCCESS;
5415 	}
5416 
5417 	mlo_debug("Dumping hex of link info after parsing Multi-Link element control");
5418 	QDF_TRACE_HEX_DUMP(QDF_MODULE_ID_MLO, QDF_TRACE_LEVEL_ERROR,
5419 			   linkinfo, linkinfo_len);
5420 
5421 	ret = util_parse_pa_info_from_linkinfo(linkinfo, linkinfo_len, &painfo);
5422 	if (QDF_IS_STATUS_ERROR(ret)) {
5423 		qdf_mem_free(mlieseqpayload_copy);
5424 		return ret;
5425 	}
5426 
5427 	qdf_mem_copy(pa_info, &painfo, sizeof(painfo));
5428 	qdf_mem_free(mlieseqpayload_copy);
5429 
5430 	return QDF_STATUS_SUCCESS;
5431 }
5432 
5433 #endif
5434 
5435 #ifdef WLAN_FEATURE_11BE
5436 
util_add_bw_ind(struct wlan_ie_bw_ind * bw_ind,uint8_t ccfs0,uint8_t ccfs1,enum phy_ch_width ch_width,uint16_t puncture_bitmap,int * bw_ind_len)5437 QDF_STATUS util_add_bw_ind(struct wlan_ie_bw_ind *bw_ind, uint8_t ccfs0,
5438 			   uint8_t ccfs1, enum phy_ch_width ch_width,
5439 			   uint16_t puncture_bitmap, int *bw_ind_len)
5440 {
5441 	uint8_t bw_ind_width;
5442 
5443 	if (!bw_ind) {
5444 		mlo_err("Pointer to bandwidth indiaction element is NULL");
5445 		return QDF_STATUS_E_NULL_VALUE;
5446 	}
5447 
5448 	if (!bw_ind_len) {
5449 		mlo_err("Length of bandwidth indaication element is Zero");
5450 		return QDF_STATUS_E_INVAL;
5451 	}
5452 
5453 	switch (ch_width) {
5454 	case CH_WIDTH_20MHZ:
5455 		bw_ind_width = IEEE80211_11BEOP_CHWIDTH_20;
5456 		break;
5457 	case CH_WIDTH_40MHZ:
5458 		bw_ind_width = IEEE80211_11BEOP_CHWIDTH_40;
5459 		break;
5460 	case CH_WIDTH_80MHZ:
5461 		bw_ind_width = IEEE80211_11BEOP_CHWIDTH_80;
5462 		break;
5463 	case CH_WIDTH_160MHZ:
5464 		bw_ind_width = IEEE80211_11BEOP_CHWIDTH_160;
5465 		break;
5466 	case CH_WIDTH_320MHZ:
5467 		bw_ind_width = IEEE80211_11BEOP_CHWIDTH_320;
5468 		break;
5469 	default:
5470 		bw_ind_width = IEEE80211_11BEOP_CHWIDTH_20;
5471 	}
5472 
5473 	bw_ind->elem_id = WLAN_ELEMID_EXTN_ELEM;
5474 	*bw_ind_len = WLAN_BW_IND_IE_MAX_LEN;
5475 	bw_ind->elem_len = WLAN_BW_IND_IE_MAX_LEN - WLAN_IE_HDR_LEN;
5476 	bw_ind->elem_id_extn = WLAN_EXTN_ELEMID_BW_IND;
5477 	bw_ind->ccfs0 = ccfs0;
5478 	bw_ind->ccfs1 = ccfs1;
5479 	QDF_SET_BITS(bw_ind->control, BW_IND_CHAN_WIDTH_IDX,
5480 		     BW_IND_CHAN_WIDTH_BITS, bw_ind_width);
5481 
5482 	if (puncture_bitmap) {
5483 		bw_ind->disabled_sub_chan_bitmap[0] =
5484 			QDF_GET_BITS(puncture_bitmap, 0, 8);
5485 		bw_ind->disabled_sub_chan_bitmap[1] =
5486 			QDF_GET_BITS(puncture_bitmap, 8, 8);
5487 		QDF_SET_BITS(bw_ind->bw_ind_param,
5488 			     BW_IND_PARAM_DISABLED_SC_BITMAP_PRESENT_IDX,
5489 			     BW_IND_PARAM_DISABLED_SC_BITMAP_PRESENT_BITS, 1);
5490 	} else {
5491 		QDF_SET_BITS(bw_ind->bw_ind_param,
5492 			     BW_IND_PARAM_DISABLED_SC_BITMAP_PRESENT_IDX,
5493 			     BW_IND_PARAM_DISABLED_SC_BITMAP_PRESENT_BITS, 0);
5494 		bw_ind->elem_len -=
5495 			QDF_ARRAY_SIZE(bw_ind->disabled_sub_chan_bitmap);
5496 		*bw_ind_len -=
5497 			QDF_ARRAY_SIZE(bw_ind->disabled_sub_chan_bitmap);
5498 	}
5499 
5500 	return QDF_STATUS_SUCCESS;
5501 }
5502 
util_parse_bw_ind(struct wlan_ie_bw_ind * bw_ind,uint8_t * ccfs0,uint8_t * ccfs1,enum phy_ch_width * ch_width,uint16_t * puncture_bitmap)5503 QDF_STATUS util_parse_bw_ind(struct wlan_ie_bw_ind *bw_ind, uint8_t *ccfs0,
5504 			     uint8_t *ccfs1, enum phy_ch_width *ch_width,
5505 			     uint16_t *puncture_bitmap)
5506 {
5507 	uint8_t bw_ind_width;
5508 
5509 	if (!bw_ind) {
5510 		mlo_err("Pointer to bandwidth indiaction element is NULL");
5511 		return QDF_STATUS_E_NULL_VALUE;
5512 	}
5513 
5514 	*ccfs0 = bw_ind->ccfs0;
5515 	*ccfs1 = bw_ind->ccfs1;
5516 	bw_ind_width = QDF_GET_BITS(bw_ind->control, BW_IND_CHAN_WIDTH_IDX,
5517 				    BW_IND_CHAN_WIDTH_BITS);
5518 
5519 	switch (bw_ind_width) {
5520 	case IEEE80211_11BEOP_CHWIDTH_20:
5521 		*ch_width = CH_WIDTH_20MHZ;
5522 		break;
5523 	case IEEE80211_11BEOP_CHWIDTH_40:
5524 		*ch_width = CH_WIDTH_40MHZ;
5525 		break;
5526 	case IEEE80211_11BEOP_CHWIDTH_80:
5527 		*ch_width = CH_WIDTH_80MHZ;
5528 		break;
5529 	case IEEE80211_11BEOP_CHWIDTH_160:
5530 		*ch_width = CH_WIDTH_160MHZ;
5531 		break;
5532 	case IEEE80211_11BEOP_CHWIDTH_320:
5533 		*ch_width = CH_WIDTH_320MHZ;
5534 		break;
5535 	default:
5536 		*ch_width = CH_WIDTH_20MHZ;
5537 	}
5538 
5539 	if (QDF_GET_BITS(bw_ind->bw_ind_param,
5540 			 BW_IND_PARAM_DISABLED_SC_BITMAP_PRESENT_IDX,
5541 			 BW_IND_PARAM_DISABLED_SC_BITMAP_PRESENT_BITS)) {
5542 		QDF_SET_BITS(*puncture_bitmap, 0, 8,
5543 			     bw_ind->disabled_sub_chan_bitmap[0]);
5544 		QDF_SET_BITS(*puncture_bitmap, 8, 8,
5545 			     bw_ind->disabled_sub_chan_bitmap[1]);
5546 	} else {
5547 		*puncture_bitmap = 0;
5548 	}
5549 
5550 	return QDF_STATUS_SUCCESS;
5551 }
5552 #endif
5553