1 /* 2 * Copyright (c) 2021, The Linux Foundation. All rights reserved. 3 * Copyright (c) 2021 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 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 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 69 uint8_t *util_parse_multi_link_ctrl(uint8_t *element, 70 qdf_size_t len, 71 qdf_size_t *link_info_len) 72 { 73 qdf_size_t parsed_ie_len = 0; 74 struct wlan_ie_multilink *mlie_fixed; 75 uint16_t mlcontrol; 76 uint16_t presencebm; 77 78 if (!element) { 79 mlo_err("Pointer to element is NULL"); 80 return NULL; 81 } 82 83 if (!len) { 84 mlo_err("Length is zero"); 85 return NULL; 86 } 87 88 if (len < sizeof(struct wlan_ie_multilink)) { 89 mlo_err_rl("Length %zu octets is smaller than required for the fixed portion of Multi-Link element (%zu octets)", 90 len, sizeof(struct wlan_ie_multilink)); 91 return NULL; 92 } 93 94 mlie_fixed = (struct wlan_ie_multilink *)element; 95 mlcontrol = le16toh(mlie_fixed->mlcontrol); 96 presencebm = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_PBM_IDX, 97 WLAN_ML_CTRL_PBM_BITS); 98 99 parsed_ie_len += sizeof(*mlie_fixed); 100 101 /* Check if MLD MAC address is present */ 102 if (presencebm & WLAN_ML_BV_CTRL_PBM_MLDMACADDR_P) 103 parsed_ie_len += QDF_MAC_ADDR_SIZE; 104 105 /* Check if Link ID info is present */ 106 if (presencebm & WLAN_ML_BV_CTRL_PBM_LINKIDINFO_P) 107 parsed_ie_len += WLAN_ML_BV_CINFO_LINKIDINFO_SIZE; 108 109 /* Check if BSS parameter change count is present */ 110 if (presencebm & WLAN_ML_BV_CTRL_PBM_BSSPARAMCHANGECNT_P) 111 parsed_ie_len += WLAN_ML_BV_CINFO_BSSPARAMCHNGCNT_SIZE; 112 113 /* Check if Medium Sync Delay Info is present */ 114 if (presencebm & WLAN_ML_BV_CTRL_PBM_MEDIUMSYNCDELAYINFO_P) 115 parsed_ie_len += WLAN_ML_BV_CINFO_MEDMSYNCDELAYINFO_SIZE; 116 117 /* Check if EML cap is present */ 118 if (presencebm & WLAN_ML_BV_CTRL_PBM_EMLCAP_P) 119 parsed_ie_len += WLAN_ML_BV_CINFO_EMLCAP_SIZE; 120 121 /* Check if MLD cap is present */ 122 if (presencebm & WLAN_ML_BV_CTRL_PBM_MLDCAP_P) 123 parsed_ie_len += WLAN_ML_BV_CINFO_MLDCAP_SIZE; 124 125 if (link_info_len) { 126 *link_info_len = len - parsed_ie_len; 127 mlo_debug("link_info_len:%zu, parsed_ie_len:%zu", 128 *link_info_len, parsed_ie_len); 129 } 130 131 return &element[parsed_ie_len]; 132 } 133 134 static 135 uint8_t *util_parse_perstaprofile(uint8_t *subelement, 136 qdf_size_t len, 137 bool is_staprof_reqd, 138 qdf_size_t *staprof_len, 139 uint8_t *linkid, 140 bool *is_macaddr_valid, 141 struct qdf_mac_addr *macaddr) 142 { 143 qdf_size_t subelement_len = 0; 144 struct wlan_ml_bv_linfo_perstaprof *perstaprof_fixed; 145 uint16_t stacontrol; 146 uint8_t completeprofile; 147 uint8_t nstrlppresent; 148 enum wlan_ml_bv_linfo_perstaprof_stactrl_nstrbmsz nstrbmsz; 149 150 if (!subelement) { 151 mlo_err("Pointer to subelement is NULL"); 152 return NULL; 153 } 154 155 if (!len) { 156 mlo_err("Length is zero"); 157 return NULL; 158 } 159 160 if (subelement[0] != WLAN_ML_BV_LINFO_SUBELEMID_PERSTAPROFILE) { 161 mlo_err_rl("Pointer to subelement does not point to per-STA profile"); 162 return NULL; 163 } 164 165 if (len < sizeof(struct wlan_ml_bv_linfo_perstaprof)) { 166 mlo_err_rl("len %zu octets is smaller than that required for the fixed portion of per-STA profile (%zu octets)", 167 len, sizeof(struct wlan_ml_bv_linfo_perstaprof)); 168 return NULL; 169 } 170 171 perstaprof_fixed = (struct wlan_ml_bv_linfo_perstaprof *)subelement; 172 173 subelement_len = sizeof(*perstaprof_fixed); 174 175 stacontrol = le16toh(perstaprof_fixed->stacontrol); 176 177 if (linkid) { 178 *linkid = QDF_GET_BITS(stacontrol, 179 WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_LINKID_IDX, 180 WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_LINKID_BITS); 181 } 182 183 /* Check if this a complete profile */ 184 completeprofile = QDF_GET_BITS(stacontrol, 185 WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_CMPLTPROF_IDX, 186 WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_CMPLTPROF_BITS); 187 188 /* We increment the measured length of the per-STA profile by checking 189 * for the presence of individual fields. We validate this length 190 * against the total length of the sublement only at the end, except in 191 * cases where we are actually about to access a given field. 192 */ 193 194 if (is_macaddr_valid) 195 *is_macaddr_valid = false; 196 197 /* Check STA MAC address present bit */ 198 if (QDF_GET_BITS(stacontrol, 199 WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_MACADDRP_IDX, 200 WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_MACADDRP_BITS)) { 201 if (macaddr) { 202 /* Explicityly check if the length is sufficient to hold 203 * the STA MAC address, since we are about to attempt to 204 * access the STA MAC address. 205 */ 206 if (len < (subelement_len + QDF_MAC_ADDR_SIZE)) { 207 mlo_err_rl("len %zu octets is smaller than min size of per-STA profile required to accommodate STA MAC address (%zu octets)", 208 len, 209 (subelement_len + QDF_MAC_ADDR_SIZE)); 210 return NULL; 211 } 212 213 qdf_mem_copy(macaddr->bytes, 214 subelement + subelement_len, 215 QDF_MAC_ADDR_SIZE); 216 217 mlo_nofl_debug("Copied MAC address: " QDF_MAC_ADDR_FMT, 218 subelement + subelement_len); 219 220 if (is_macaddr_valid) 221 *is_macaddr_valid = true; 222 } 223 224 subelement_len += QDF_MAC_ADDR_SIZE; 225 } 226 227 /* Check Beacon Interval present bit */ 228 if (QDF_GET_BITS(stacontrol, 229 WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_BCNINTP_IDX, 230 WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_BCNINTP_BITS)) 231 subelement_len += WLAN_BEACONINTERVAL_LEN; 232 233 /* Check DTIM Info present bit */ 234 if (QDF_GET_BITS(stacontrol, 235 WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_DTIMINFOP_IDX, 236 WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_DTIMINFOP_BITS)) 237 subelement_len += 238 sizeof(struct wlan_ml_bv_linfo_perstaprof_stainfo_dtiminfo); 239 240 /* Check NTSR Link pair present bit */ 241 nstrlppresent = 242 QDF_GET_BITS(stacontrol, 243 WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_NSTRLINKPRP_IDX, 244 WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_NSTRLINKPRP_BITS); 245 246 if (completeprofile && nstrlppresent) { 247 /* Check NTSR Bitmap Size bit */ 248 nstrbmsz = 249 QDF_GET_BITS(stacontrol, 250 WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_NSTRBMSZ_IDX, 251 WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_NSTRBMSZ_BITS); 252 253 if (nstrbmsz == WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_NSTRBMSZ_1_OCTET) { 254 subelement_len += 1; 255 } else if (nstrbmsz == WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_NSTRBMSZ_2_OCTETS) { 256 subelement_len += 2; 257 } else { 258 /* Though an invalid value cannot occur if only 1 bit is 259 * used, we check for it in a generic manner in case the 260 * number of bits is increased in the future. 261 */ 262 mlo_err_rl("Invalid NSTR Bitmap size %u", nstrbmsz); 263 return NULL; 264 } 265 } 266 267 /* Note: Some implementation versions of hostapd/wpa_supplicant may 268 * provide a per-STA profile without STA profile. Let the caller 269 * indicate whether a STA profile is required to be found. This may be 270 * revisited as upstreaming progresses. 271 */ 272 if (!is_staprof_reqd) { 273 if (len < subelement_len) { 274 mlo_err_rl("len %zu < subelement_len %zu", 275 len, 276 subelement_len); 277 return NULL; 278 } 279 280 return &subelement[subelement_len - 1]; 281 } 282 283 if (len <= subelement_len) { 284 mlo_err_rl("len %zu <= subelement_len %zu", 285 len, 286 subelement_len); 287 return NULL; 288 } 289 290 if (staprof_len) 291 *staprof_len = len - subelement_len; 292 293 return &subelement[subelement_len]; 294 } 295 296 static 297 uint8_t *util_get_successorfrag(uint8_t *currie, uint8_t *frame, qdf_size_t len) 298 { 299 uint8_t *nextie; 300 301 if (!currie || !frame || !len) 302 return NULL; 303 304 if ((currie + MIN_IE_LEN) > (frame + len)) 305 return NULL; 306 307 /* Check whether there is sufficient space in the frame for the current 308 * IE, plus at least another MIN_IE_LEN bytes for the IE header of a 309 * fragment (if present) that would come just after the current IE. 310 */ 311 if ((currie + MIN_IE_LEN + currie[TAG_LEN_POS] + MIN_IE_LEN) > 312 (frame + len)) 313 return NULL; 314 315 nextie = currie + currie[TAG_LEN_POS] + MIN_IE_LEN; 316 317 if (nextie[ID_POS] != WLAN_ELEMID_FRAGMENT) 318 return NULL; 319 320 return nextie; 321 } 322 323 static 324 QDF_STATUS util_parse_partner_info_from_linkinfo(uint8_t *linkinfo, 325 qdf_size_t linkinfo_len, 326 struct mlo_partner_info *partner_info) 327 { 328 uint8_t linkid; 329 struct qdf_mac_addr macaddr; 330 bool is_macaddr_valid; 331 uint8_t *currpos; 332 qdf_size_t currlen; 333 uint8_t *endofstainfo; 334 335 if (!linkinfo) { 336 mlo_err("linkinfo is NULL"); 337 return QDF_STATUS_E_NULL_VALUE; 338 } 339 340 if (!linkinfo_len) { 341 mlo_err("linkinfo_len is zero"); 342 return QDF_STATUS_E_NULL_VALUE; 343 } 344 345 if (!partner_info) { 346 mlo_err("ML partner info is NULL"); 347 return QDF_STATUS_E_NULL_VALUE; 348 } 349 350 partner_info->num_partner_links = 0; 351 currpos = linkinfo; 352 currlen = linkinfo_len; 353 354 while (currlen) { 355 if (currlen < MIN_IE_LEN) { 356 mlo_err_rl("currlen %zu is smaller than minimum IE length %u", 357 currlen, MIN_IE_LEN); 358 return QDF_STATUS_E_PROTO; 359 } 360 361 if (currlen < (MIN_IE_LEN + currpos[TAG_LEN_POS])) { 362 mlo_err_rl("currlen %zu is smaller than length of current IE %u", 363 currlen, MIN_IE_LEN + currpos[TAG_LEN_POS]); 364 return QDF_STATUS_E_PROTO; 365 } 366 367 if (currpos[ID_POS] == 368 WLAN_ML_BV_LINFO_SUBELEMID_PERSTAPROFILE) { 369 is_macaddr_valid = false; 370 371 /* Per-STA profile fragmentation support may be added 372 * once support for this is introduced in the standard. 373 */ 374 endofstainfo = util_parse_perstaprofile(currpos, 375 currlen, 376 false, 377 NULL, 378 &linkid, 379 &is_macaddr_valid, 380 &macaddr); 381 382 if (!endofstainfo) { 383 mlo_err_rl("Error in parsing per-STA profile"); 384 return QDF_STATUS_E_EMPTY; 385 } 386 387 if (is_macaddr_valid) { 388 if (partner_info->num_partner_links >= 389 QDF_ARRAY_SIZE(partner_info->partner_link_info)) { 390 mlo_err_rl("Insufficient size %zu of array for partner link info", 391 QDF_ARRAY_SIZE(partner_info->partner_link_info)); 392 return QDF_STATUS_E_NOMEM; 393 } 394 395 partner_info->partner_link_info[partner_info->num_partner_links].link_id = 396 linkid; 397 qdf_mem_copy(&partner_info->partner_link_info[partner_info->num_partner_links].link_addr, 398 &macaddr, 399 sizeof(partner_info->partner_link_info[partner_info->num_partner_links].link_addr)); 400 401 partner_info->num_partner_links++; 402 } else { 403 mlo_warn_rl("MAC address not found in STA Info field of per-STA profile with link ID %u", 404 linkid); 405 } 406 } 407 408 currlen -= (MIN_IE_LEN + currpos[TAG_LEN_POS]); 409 currpos += (MIN_IE_LEN + currpos[TAG_LEN_POS]); 410 } 411 412 mlo_debug("Number of ML partner links found=%u", 413 partner_info->num_partner_links); 414 415 return QDF_STATUS_SUCCESS; 416 } 417 418 QDF_STATUS util_gen_link_assoc_rsp(uint8_t *frame, qdf_size_t len, 419 struct qdf_mac_addr link_addr, 420 uint8_t *assoc_link_frame) 421 { 422 uint8_t *tmp = NULL; 423 const uint8_t *tmp_old, *rsn_ie; 424 qdf_size_t sub_len, tmp_rem_len; 425 qdf_size_t link_info_len, sta_prof_len = 0; 426 uint8_t *subelement; 427 uint8_t *pos; 428 uint8_t *sub_copy, *orig_copy; 429 bool is_bssid_valid; 430 struct qdf_mac_addr bssid; 431 struct wlan_frame_hdr *hdr; 432 433 if (!frame || !len) 434 return QDF_STATUS_E_NULL_VALUE; 435 436 pos = assoc_link_frame; 437 hdr = (struct wlan_frame_hdr *)pos; 438 pos = pos + WLAN_MAC_HDR_LEN_3A; 439 440 /* Assoc resp Capability(2) + AID(2) + Status Code(2) */ 441 qdf_mem_copy(pos, frame, WLAN_ASSOC_RSP_IES_OFFSET); 442 pos = pos + WLAN_ASSOC_RSP_IES_OFFSET; 443 444 rsn_ie = wlan_get_ie_ptr_from_eid(WLAN_ELEMID_RSN, frame, len); 445 if (rsn_ie) { 446 qdf_mem_copy(pos, rsn_ie, rsn_ie[1]); 447 pos = pos + rsn_ie[1]; 448 } 449 /* find MLO IE */ 450 subelement = util_find_extn_eid(WLAN_ELEMID_EXTN_ELEM, 451 WLAN_EXTN_ELEMID_MULTI_LINK, 452 frame, 453 len); 454 if (!subelement) 455 return QDF_STATUS_E_FAILURE; 456 457 /* EID(1) + len (1) = 2 */ 458 sub_len = subelement[TAG_LEN_POS] + 2; 459 sub_copy = qdf_mem_malloc(sub_len); 460 if (!sub_copy) 461 return QDF_STATUS_E_NOMEM; 462 orig_copy = sub_copy; 463 qdf_mem_copy(sub_copy, subelement, sub_len); 464 465 /* parse ml ie */ 466 sub_copy = util_parse_multi_link_ctrl(sub_copy, 467 sub_len, 468 &link_info_len); 469 470 if (!sub_copy) 471 return QDF_STATUS_E_NULL_VALUE; 472 473 mlo_debug("dumping hex after parsing multi link ctrl"); 474 QDF_TRACE_HEX_DUMP(QDF_MODULE_ID_MLO, QDF_TRACE_LEVEL_DEBUG, 475 sub_copy, link_info_len); 476 477 is_bssid_valid = false; 478 479 /* Parse per-STA profile */ 480 sub_copy = util_parse_perstaprofile(sub_copy, 481 link_info_len, 482 true, 483 &sta_prof_len, 484 NULL, 485 &is_bssid_valid, 486 &bssid); 487 488 if (!is_bssid_valid) 489 return QDF_STATUS_E_NULL_VALUE; 490 491 if (!sub_copy) { 492 qdf_mem_copy(pos, frame, len); 493 pos += len - WLAN_MAC_HDR_LEN_3A; 494 goto update_header; 495 } 496 497 /* go through IEs in frame and subelement, 498 * merge them into new_ie 499 */ 500 tmp_old = util_find_eid(WLAN_ELEMID_SSID, frame, len); 501 tmp_old = (tmp_old) ? tmp_old + tmp_old[TAG_LEN_POS] + MIN_IE_LEN : frame; 502 503 while (((tmp_old + tmp_old[TAG_LEN_POS] + MIN_IE_LEN) - frame) <= len) { 504 tmp = (uint8_t *)util_find_eid(tmp_old[0], 505 sub_copy, 506 sta_prof_len); 507 if (!tmp) { 508 /* ie in old ie but not in subelement */ 509 if (tmp_old[2] != WLAN_EXTN_ELEMID_MULTI_LINK) { 510 if ((pos + tmp_old[TAG_LEN_POS] + MIN_IE_LEN) <= 511 (assoc_link_frame + len)) { 512 qdf_mem_copy(pos, tmp_old, 513 (tmp_old[TAG_LEN_POS] + 514 MIN_IE_LEN)); 515 pos += tmp_old[TAG_LEN_POS] + MIN_IE_LEN; 516 } 517 } 518 } else { 519 /* ie in transmitting ie also in subelement, 520 * copy from subelement and flag the ie in subelement 521 * as copied (by setting eid field to 0xff). For 522 * vendor ie, compare OUI + type + subType to 523 * determine if they are the same ie. 524 */ 525 tmp_rem_len = sta_prof_len - (tmp - sub_copy); 526 if (tmp_old[0] == WLAN_ELEMID_VENDOR && 527 tmp_rem_len >= MIN_VENDOR_TAG_LEN) { 528 if (!qdf_mem_cmp(tmp_old + PAYLOAD_START_POS, 529 tmp + PAYLOAD_START_POS, 530 OUI_LEN)) { 531 /* same vendor ie, copy from 532 * subelement 533 */ 534 if ((pos + tmp[TAG_LEN_POS] + MIN_IE_LEN) <= 535 (assoc_link_frame + len)) { 536 qdf_mem_copy(pos, tmp, 537 tmp[TAG_LEN_POS] + 538 MIN_IE_LEN); 539 pos += tmp[TAG_LEN_POS] + MIN_IE_LEN; 540 tmp[0] = 0; 541 } 542 } else { 543 if ((pos + tmp_old[TAG_LEN_POS] + 544 MIN_IE_LEN) <= 545 (assoc_link_frame + len)) { 546 qdf_mem_copy(pos, tmp_old, 547 tmp_old[TAG_LEN_POS] + 548 MIN_IE_LEN); 549 pos += tmp_old[TAG_LEN_POS] + 550 MIN_IE_LEN; 551 } 552 } 553 } else if (tmp_old[0] == WLAN_ELEMID_EXTN_ELEM) { 554 if (tmp_old[PAYLOAD_START_POS] == 555 tmp[PAYLOAD_START_POS]) { 556 /* same ie, copy from subelement */ 557 if ((pos + tmp[TAG_LEN_POS] + MIN_IE_LEN) <= 558 (assoc_link_frame + len)) { 559 qdf_mem_copy(pos, tmp, 560 tmp[TAG_LEN_POS] + 561 MIN_IE_LEN); 562 pos += tmp[TAG_LEN_POS] + MIN_IE_LEN; 563 tmp[0] = 0; 564 } 565 } else { 566 if ((pos + tmp_old[TAG_LEN_POS] + MIN_IE_LEN) <= 567 (assoc_link_frame + len)) { 568 qdf_mem_copy(pos, tmp_old, 569 tmp_old[TAG_LEN_POS] + 570 MIN_IE_LEN); 571 pos += tmp_old[TAG_LEN_POS] + 572 MIN_IE_LEN; 573 } 574 } 575 } else { 576 /* copy ie from subelement into new ie */ 577 if ((pos + tmp[TAG_LEN_POS] + MIN_IE_LEN) <= 578 (assoc_link_frame + len)) { 579 qdf_mem_copy(pos, tmp, 580 tmp[TAG_LEN_POS] + MIN_IE_LEN); 581 pos += tmp[TAG_LEN_POS] + MIN_IE_LEN; 582 tmp[0] = 0; 583 } 584 } 585 } 586 587 if (((tmp_old + tmp_old[TAG_LEN_POS] + MIN_IE_LEN) - frame) >= len) 588 break; 589 590 tmp_old += tmp_old[TAG_LEN_POS] + MIN_IE_LEN; 591 } 592 593 update_header: 594 /* Copy the link mac addr */ 595 qdf_mem_copy(hdr->i_addr3, bssid.bytes, 596 QDF_MAC_ADDR_SIZE); 597 qdf_mem_copy(hdr->i_addr2, bssid.bytes, 598 QDF_MAC_ADDR_SIZE); 599 qdf_mem_copy(hdr->i_addr1, &link_addr, 600 QDF_MAC_ADDR_SIZE); 601 hdr->i_fc[0] = FC0_IEEE_MGMT_FRM; 602 hdr->i_fc[1] = FC1_IEEE_MGMT_FRM; 603 /* seq num not used so not populated */ 604 qdf_mem_free(orig_copy); 605 606 return QDF_STATUS_SUCCESS; 607 } 608 609 QDF_STATUS 610 util_find_mlie(uint8_t *buf, qdf_size_t buflen, uint8_t **mlieseq, 611 qdf_size_t *mlieseqlen) 612 { 613 uint8_t *bufboundary; 614 uint8_t *ieseq; 615 qdf_size_t ieseqlen; 616 uint8_t *currie; 617 uint8_t *successorfrag; 618 619 if (!buf || !buflen || !mlieseq || !mlieseqlen) 620 return QDF_STATUS_E_NULL_VALUE; 621 622 *mlieseq = NULL; 623 *mlieseqlen = 0; 624 625 /* Find Multi-Link element. In case a fragment sequence is present, 626 * this element will be the leading fragment. 627 */ 628 ieseq = util_find_extn_eid(WLAN_ELEMID_EXTN_ELEM, 629 WLAN_EXTN_ELEMID_MULTI_LINK, buf, 630 buflen); 631 632 /* Even if the element is not found, we have successfully examined the 633 * buffer. The caller will be provided a NULL value for the starting of 634 * the Multi-Link element. Hence, we return success. 635 */ 636 if (!ieseq) 637 return QDF_STATUS_SUCCESS; 638 639 bufboundary = buf + buflen; 640 641 if ((ieseq + MIN_IE_LEN) > bufboundary) 642 return QDF_STATUS_E_INVAL; 643 644 ieseqlen = MIN_IE_LEN + ieseq[TAG_LEN_POS]; 645 646 if (ieseqlen < sizeof(struct wlan_ie_multilink)) 647 return QDF_STATUS_E_PROTO; 648 649 if ((ieseq + ieseqlen) > bufboundary) 650 return QDF_STATUS_E_INVAL; 651 652 /* In the next sequence of checks, if there is no space in the buffer 653 * for another element after the Multi-Link element/element fragment 654 * sequence, it could indicate an issue since non-MLO EHT elements 655 * would be expected to follow the Multi-Link element/element fragment 656 * sequence. However, this is outside of the purview of this function, 657 * hence we ignore it. 658 */ 659 660 currie = ieseq; 661 successorfrag = util_get_successorfrag(currie, buf, buflen); 662 663 /* Fragmentation definitions as of IEEE802.11be D1.0 and 664 * IEEE802.11REVme D0.2 are applied. Only the case where Multi-Link 665 * element is present in a buffer from the core frame is considered. 666 * Future changes to fragmentation, cases where the Multi-Link element 667 * is present in a subelement, etc. to be reflected here if applicable 668 * as and when the rules evolve. 669 */ 670 while (successorfrag) { 671 /* We should not be seeing a successor fragment if the length 672 * of the current IE is lesser than the max. 673 */ 674 if (currie[TAG_LEN_POS] != WLAN_MAX_IE_LEN) 675 return QDF_STATUS_E_PROTO; 676 677 if (successorfrag[TAG_LEN_POS] == 0) 678 return QDF_STATUS_E_PROTO; 679 680 ieseqlen += (MIN_IE_LEN + successorfrag[TAG_LEN_POS]); 681 682 currie = successorfrag; 683 successorfrag = util_get_successorfrag(currie, buf, buflen); 684 } 685 686 *mlieseq = ieseq; 687 *mlieseqlen = ieseqlen; 688 return QDF_STATUS_SUCCESS; 689 } 690 691 QDF_STATUS 692 util_get_mlie_variant(uint8_t *mlieseq, qdf_size_t mlieseqlen, 693 int *variant) 694 { 695 struct wlan_ie_multilink *mlie_fixed; 696 enum wlan_ml_variant var; 697 uint16_t mlcontrol; 698 699 if (!mlieseq || !mlieseqlen || !variant) 700 return QDF_STATUS_E_NULL_VALUE; 701 702 if (mlieseqlen < sizeof(struct wlan_ie_multilink)) 703 return QDF_STATUS_E_INVAL; 704 705 mlie_fixed = (struct wlan_ie_multilink *)mlieseq; 706 707 if ((mlie_fixed->elem_id != WLAN_ELEMID_EXTN_ELEM) || 708 (mlie_fixed->elem_id_ext != WLAN_EXTN_ELEMID_MULTI_LINK)) 709 return QDF_STATUS_E_INVAL; 710 711 mlcontrol = le16toh(mlie_fixed->mlcontrol); 712 var = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_TYPE_IDX, 713 WLAN_ML_CTRL_TYPE_BITS); 714 715 if (var >= WLAN_ML_VARIANT_INVALIDSTART) 716 return QDF_STATUS_E_PROTO; 717 718 *variant = var; 719 return QDF_STATUS_SUCCESS; 720 } 721 722 QDF_STATUS 723 util_get_bvmlie_mldmacaddr(uint8_t *mlieseq, qdf_size_t mlieseqlen, 724 bool *mldmacaddrfound, 725 struct qdf_mac_addr *mldmacaddr) 726 { 727 struct wlan_ie_multilink *mlie_fixed; 728 enum wlan_ml_variant variant; 729 uint16_t mlcontrol; 730 uint16_t presencebitmap; 731 732 if (!mlieseq || !mlieseqlen || !mldmacaddrfound || !mldmacaddr) 733 return QDF_STATUS_E_NULL_VALUE; 734 735 *mldmacaddrfound = false; 736 qdf_mem_zero(mldmacaddr, sizeof(*mldmacaddr)); 737 738 if (mlieseqlen < sizeof(struct wlan_ie_multilink)) 739 return QDF_STATUS_E_INVAL; 740 741 mlie_fixed = (struct wlan_ie_multilink *)mlieseq; 742 743 if ((mlie_fixed->elem_id != WLAN_ELEMID_EXTN_ELEM) || 744 (mlie_fixed->elem_id_ext != WLAN_EXTN_ELEMID_MULTI_LINK)) 745 return QDF_STATUS_E_INVAL; 746 747 mlcontrol = le16toh(mlie_fixed->mlcontrol); 748 749 variant = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_TYPE_IDX, 750 WLAN_ML_CTRL_TYPE_BITS); 751 752 if (variant != WLAN_ML_VARIANT_BASIC) 753 return QDF_STATUS_E_INVAL; 754 755 presencebitmap = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_PBM_IDX, 756 WLAN_ML_CTRL_PBM_BITS); 757 758 if (presencebitmap & WLAN_ML_BV_CTRL_PBM_MLDMACADDR_P) { 759 /* Common Info starts at mlieseq + sizeof(struct 760 * wlan_ie_multilink). Check if there is sufficient space in 761 * Common Info for the MLD MAC address. 762 */ 763 if ((sizeof(struct wlan_ie_multilink) + QDF_MAC_ADDR_SIZE) > 764 mlieseqlen) 765 return QDF_STATUS_E_PROTO; 766 767 *mldmacaddrfound = true; 768 qdf_mem_copy(mldmacaddr->bytes, 769 mlieseq + sizeof(struct wlan_ie_multilink), 770 QDF_MAC_ADDR_SIZE); 771 } 772 773 return QDF_STATUS_SUCCESS; 774 } 775 776 QDF_STATUS 777 util_get_bvmlie_primary_linkid(uint8_t *mlieseq, qdf_size_t mlieseqlen, 778 bool *linkidfound, uint8_t *linkid) 779 { 780 struct wlan_ie_multilink *mlie_fixed; 781 enum wlan_ml_variant variant; 782 uint16_t mlcontrol; 783 uint16_t presencebitmap; 784 uint8_t *commoninfo; 785 qdf_size_t commoninfolen; 786 uint8_t *linkidinfo; 787 788 if (!mlieseq || !mlieseqlen || !linkidfound || !linkid) 789 return QDF_STATUS_E_NULL_VALUE; 790 791 *linkidfound = false; 792 *linkid = 0; 793 794 if (mlieseqlen < sizeof(struct wlan_ie_multilink)) 795 return QDF_STATUS_E_INVAL; 796 797 mlie_fixed = (struct wlan_ie_multilink *)mlieseq; 798 799 if ((mlie_fixed->elem_id != WLAN_ELEMID_EXTN_ELEM) || 800 (mlie_fixed->elem_id_ext != WLAN_EXTN_ELEMID_MULTI_LINK)) 801 return QDF_STATUS_E_INVAL; 802 803 mlcontrol = le16toh(mlie_fixed->mlcontrol); 804 805 variant = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_TYPE_IDX, 806 WLAN_ML_CTRL_TYPE_BITS); 807 808 if (variant != WLAN_ML_VARIANT_BASIC) 809 return QDF_STATUS_E_INVAL; 810 811 presencebitmap = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_PBM_IDX, 812 WLAN_ML_CTRL_PBM_BITS); 813 814 commoninfo = mlieseq + sizeof(struct wlan_ie_multilink); 815 commoninfolen = 0; 816 817 if (presencebitmap & WLAN_ML_BV_CTRL_PBM_MLDMACADDR_P) { 818 commoninfolen += QDF_MAC_ADDR_SIZE; 819 820 if ((sizeof(struct wlan_ie_multilink) + commoninfolen) > 821 mlieseqlen) 822 return QDF_STATUS_E_PROTO; 823 } 824 825 if (presencebitmap & WLAN_ML_BV_CTRL_PBM_LINKIDINFO_P) { 826 linkidinfo = commoninfo + commoninfolen; 827 commoninfolen += WLAN_ML_BV_CINFO_LINKIDINFO_SIZE; 828 829 if ((sizeof(struct wlan_ie_multilink) + commoninfolen) > 830 mlieseqlen) 831 return QDF_STATUS_E_PROTO; 832 833 *linkidfound = true; 834 *linkid = QDF_GET_BITS(linkidinfo[0], 835 WLAN_ML_BV_CINFO_LINKIDINFO_LINKID_IDX, 836 WLAN_ML_BV_CINFO_LINKIDINFO_LINKID_BITS); 837 } 838 839 return QDF_STATUS_SUCCESS; 840 } 841 842 QDF_STATUS 843 util_get_bvmlie_persta_partner_info(uint8_t *mlie, qdf_size_t mlielen, 844 struct mlo_partner_info *partner_info) 845 { 846 struct wlan_ie_multilink *mlie_fixed; 847 uint16_t mlcontrol; 848 enum wlan_ml_variant variant; 849 uint8_t *linkinfo; 850 qdf_size_t linkinfo_len; 851 struct mlo_partner_info pinfo = {0}; 852 QDF_STATUS ret; 853 854 if (!mlie) { 855 mlo_err("mlie is NULL"); 856 return QDF_STATUS_E_NULL_VALUE; 857 } 858 859 if (!mlielen) { 860 mlo_err("mlielen is zero"); 861 return QDF_STATUS_E_NULL_VALUE; 862 } 863 864 if (!partner_info) { 865 mlo_err("partner_info is NULL"); 866 return QDF_STATUS_E_NULL_VALUE; 867 } 868 869 partner_info->num_partner_links = 0; 870 871 if (mlielen < sizeof(struct wlan_ie_multilink)) { 872 mlo_err_rl("mlielen %zu octets is smaller than required for the fixed portion of Multi-Link element (%zu octets)", 873 mlielen, sizeof(struct wlan_ie_multilink)); 874 return QDF_STATUS_E_INVAL; 875 } 876 877 /* Note: Multi-Link element fragmentation support will be added in a 878 * later change once shared helper utilities for the same are available. 879 * As of now, we temporarily return error if the mlielen is greater than 880 * the max length for an IE. 881 */ 882 if (mlielen > (sizeof(struct ie_header) + WLAN_MAX_IE_LEN)) { 883 mlo_err_rl("Element fragmentation is not yet supported for this API"); 884 return QDF_STATUS_E_NOSUPPORT; 885 } 886 887 mlie_fixed = (struct wlan_ie_multilink *)mlie; 888 889 if ((mlie_fixed->elem_id != WLAN_ELEMID_EXTN_ELEM) || 890 (mlie_fixed->elem_id_ext != WLAN_EXTN_ELEMID_MULTI_LINK)) { 891 mlo_err("The element is not a Multi-Link element"); 892 return QDF_STATUS_E_INVAL; 893 } 894 895 mlcontrol = le16toh(mlie_fixed->mlcontrol); 896 897 variant = QDF_GET_BITS(mlcontrol, WLAN_ML_CTRL_TYPE_IDX, 898 WLAN_ML_CTRL_TYPE_BITS); 899 900 if (variant != WLAN_ML_VARIANT_BASIC) { 901 mlo_err("The variant value %u does not correspond to Basic Variant value %u", 902 variant, WLAN_ML_VARIANT_BASIC); 903 return QDF_STATUS_E_INVAL; 904 } 905 906 linkinfo_len = 0; 907 linkinfo = util_parse_multi_link_ctrl(mlie, mlielen, 908 &linkinfo_len); 909 910 if (!linkinfo) { 911 mlo_err_rl("Error in parsing Multi-Link element control"); 912 return QDF_STATUS_E_INVAL; 913 } 914 915 /* In case Link Info is absent as indicated by the Link Info length 916 * being 0, return success. The number of partner links will remain 0. 917 */ 918 if (!linkinfo_len) { 919 mlo_warn_rl("Link Info is absent"); 920 return QDF_STATUS_SUCCESS; 921 } 922 923 ret = util_parse_partner_info_from_linkinfo(linkinfo, 924 linkinfo_len, 925 &pinfo); 926 927 if (QDF_IS_STATUS_ERROR(ret)) 928 return ret; 929 930 qdf_mem_copy(partner_info, &pinfo, sizeof(*partner_info)); 931 return QDF_STATUS_SUCCESS; 932 } 933 #endif 934