1 /* 2 * Copyright (c) 2021, The Linux Foundation. All rights reserved. 3 * 4 * Permission to use, copy, modify, and/or distribute this software for any 5 * purpose with or without fee is hereby granted, provided that the above 6 * copyright notice and this permission notice appear in all copies. 7 * 8 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES 9 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF 10 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR 11 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES 12 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN 13 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 14 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. 15 */ 16 17 /* 18 * DOC: contains MLO manager util api's 19 */ 20 #include <wlan_cmn.h> 21 #include <wlan_mlo_mgr_sta.h> 22 #include <wlan_cm_public_struct.h> 23 #include <wlan_mlo_mgr_main.h> 24 #include <wlan_cm_api.h> 25 #include "wlan_scan_api.h" 26 #include "qdf_types.h" 27 #include "utils_mlo.h" 28 #include "wlan_mlo_mgr_cmn.h" 29 #include "wlan_utility.h" 30 31 #ifdef WLAN_FEATURE_11BE_MLO 32 33 static uint8_t *util_find_eid(uint8_t eid, uint8_t *frame, qdf_size_t len) 34 { 35 if (!frame) 36 return NULL; 37 38 while (len >= MIN_IE_LEN && len >= frame[TAG_LEN_POS] + MIN_IE_LEN) { 39 if (frame[ID_POS] == eid) 40 return frame; 41 42 len -= frame[TAG_LEN_POS] + MIN_IE_LEN; 43 frame += frame[TAG_LEN_POS] + MIN_IE_LEN; 44 } 45 46 return NULL; 47 } 48 49 static 50 uint8_t *util_find_extn_eid(uint8_t eid, uint8_t extn_eid, 51 uint8_t *frame, qdf_size_t len) 52 { 53 if (!frame) 54 return NULL; 55 56 while (len >= MIN_IE_LEN && len >= frame[TAG_LEN_POS] + MIN_IE_LEN) { 57 if ((frame[ID_POS] == eid) && 58 (frame[ELEM_ID_EXTN_POS] == extn_eid)) 59 return frame; 60 61 len -= frame[TAG_LEN_POS] + MIN_IE_LEN; 62 frame += frame[TAG_LEN_POS] + MIN_IE_LEN; 63 } 64 return NULL; 65 } 66 67 static 68 uint8_t *util_parse_multi_link_ctrl_ie(uint8_t *subelement, 69 qdf_size_t len, 70 qdf_size_t *link_info_ie_len) 71 { 72 qdf_size_t sub_ie_len = 0; 73 74 if (!subelement || !len) 75 return NULL; 76 77 /* sta prof len = ML IE len + EID extn(1) + Multi Lnk ctrl(2) */ 78 sub_ie_len += TAG_LEN_POS + MIN_IE_LEN + 2; 79 80 /* check if MLD MAC address present */ 81 if (qdf_test_bit(WLAN_ML_CTRL_PBM_IDX | 82 WLAN_ML_BV_CTRL_PBM_MLDMACADDR_P, 83 (unsigned long *)&subelement[MULTI_LINK_CTRL_1])) 84 sub_ie_len = sub_ie_len + QDF_MAC_ADDR_SIZE; 85 86 /* check if Link ID info */ 87 if (qdf_test_bit(WLAN_ML_CTRL_PBM_IDX | 88 WLAN_ML_BV_CTRL_PBM_LINKIDINFO_P, 89 (unsigned long *)&subelement[MULTI_LINK_CTRL_1])) 90 sub_ie_len = sub_ie_len + TAG_LEN_POS; 91 92 /* check if BSS parameter change count */ 93 if (qdf_test_bit(WLAN_ML_CTRL_PBM_IDX | 94 WLAN_ML_BV_CTRL_PBM_BSSPARAMCHANGECNT_P, 95 (unsigned long *)&subelement[MULTI_LINK_CTRL_1])) 96 sub_ie_len = sub_ie_len + TAG_LEN_POS; 97 98 /* check if Medium Sync Delay Info present */ 99 if (qdf_test_bit(WLAN_ML_CTRL_PBM_IDX | 100 WLAN_ML_BV_CTRL_PBM_MEDIUMSYNCDELAYINFO_P, 101 (unsigned long *)&subelement[MULTI_LINK_CTRL_1])) 102 sub_ie_len = sub_ie_len + PAYLOAD_START_POS; 103 104 /* check if EML cap present */ 105 if (qdf_test_bit(WLAN_ML_CTRL_PBM_IDX | 106 WLAN_ML_BV_CTRL_PBM_EMLCAP_P, 107 (unsigned long *)&subelement[MULTI_LINK_CTRL_2])) 108 sub_ie_len = sub_ie_len + SUBELEMENT_START_POS; 109 110 /* check if MLD cap present */ 111 if (qdf_test_bit(WLAN_ML_CTRL_PBM_IDX | 112 WLAN_ML_BV_CTRL_PBM_MLDCAP_P, 113 (unsigned long *)&subelement[MULTI_LINK_CTRL_2])) 114 sub_ie_len = sub_ie_len + PAYLOAD_START_POS; 115 116 if (link_info_ie_len) { 117 *link_info_ie_len = len - sub_ie_len; 118 mlo_debug("link_info_ie_len:%zu, sub_ie_len:%zu", 119 *link_info_ie_len, sub_ie_len); 120 } 121 122 return &subelement[sub_ie_len]; 123 } 124 125 static 126 uint8_t *util_parse_sta_profile_ie(uint8_t *subelement, 127 qdf_size_t len, 128 qdf_size_t *per_sta_prof_ie_len, 129 struct qdf_mac_addr *bssid) 130 { 131 qdf_size_t tmp_len = 0; 132 uint8_t *tmp = NULL; 133 134 if (!subelement || !len) 135 return NULL; 136 137 if (subelement[0] == 0) 138 tmp = subelement; 139 if (!tmp) 140 return NULL; 141 142 /* tmp_len = 2 (sta ctrl) + 1 (sub EID) + 1 (len) */ 143 tmp_len = PAYLOAD_START_POS + 2; 144 145 /* check DTIM info present bit */ 146 if (qdf_test_bit(WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_DTIMINFOP_IDX, 147 (unsigned long *)&subelement[STA_CTRL_1])) 148 tmp_len = tmp_len + MIN_IE_LEN; 149 150 /* check Beacon interval present bit */ 151 if (qdf_test_bit(WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_BCNINTP_IDX, 152 (unsigned long *)&subelement[STA_CTRL_1])) 153 tmp_len = tmp_len + TAG_LEN_POS; 154 155 /* check STA link mac addr info present bit */ 156 if (qdf_test_bit(WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_MACADDRP_IDX, 157 (unsigned long *)&subelement[STA_CTRL_1])) { 158 tmp_len = tmp_len + QDF_MAC_ADDR_SIZE; 159 qdf_mem_copy(&bssid->bytes, &tmp[4], QDF_MAC_ADDR_SIZE); 160 } 161 162 /* Add Link ID offset,as it will always be present in assoc rsp mlo ie */ 163 tmp_len = tmp_len + TAG_LEN_POS; 164 165 /* check NTSR Link pair present bit */ 166 if (qdf_test_bit(WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_NSTRLINKPRP_IDX % 8, 167 (unsigned long *)&subelement[STA_CTRL_2])) 168 tmp_len = tmp_len + MIN_IE_LEN; 169 170 /* check NTSR bitmap size present bit */ 171 if (qdf_test_bit(WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_NSTRBMSZ_IDX % 8, 172 (unsigned long *)&subelement[STA_CTRL_2])) 173 tmp_len = tmp_len + TAG_LEN_POS; 174 175 if (len <= tmp_len) { 176 mlo_err("len %zu <= tmp_len %zu, return", len, tmp_len); 177 return NULL; 178 } 179 *per_sta_prof_ie_len = len - tmp_len; 180 181 return &tmp[tmp_len]; 182 } 183 184 QDF_STATUS util_gen_link_assoc_rsp(uint8_t *frame, qdf_size_t len, 185 struct qdf_mac_addr link_addr, 186 uint8_t *assoc_link_frame) 187 { 188 uint8_t *tmp = NULL; 189 const uint8_t *tmp_old, *rsn_ie; 190 qdf_size_t sub_len, tmp_rem_len; 191 qdf_size_t link_info_len, per_sta_prof_len = 0; 192 uint8_t *subelement; 193 uint8_t *pos; 194 uint8_t *sub_copy, *orig_copy; 195 struct qdf_mac_addr bssid; 196 struct wlan_frame_hdr *hdr; 197 198 if (!frame || !len) 199 return QDF_STATUS_E_NULL_VALUE; 200 201 pos = assoc_link_frame; 202 hdr = (struct wlan_frame_hdr *)pos; 203 pos = pos + WLAN_MAC_HDR_LEN_3A; 204 205 /* Assoc resp Capability(2) + AID(2) + Status Code(2) */ 206 qdf_mem_copy(pos, frame, WLAN_ASSOC_RSP_IES_OFFSET); 207 pos = pos + WLAN_ASSOC_RSP_IES_OFFSET; 208 209 rsn_ie = wlan_get_ie_ptr_from_eid(WLAN_ELEMID_RSN, frame, len); 210 if (rsn_ie) { 211 qdf_mem_copy(pos, rsn_ie, rsn_ie[1]); 212 pos = pos + rsn_ie[1]; 213 } 214 /* find MLO IE */ 215 subelement = util_find_extn_eid(WLAN_ELEMID_EXTN_ELEM, 216 WLAN_EXTN_ELEMID_MULTI_LINK, 217 frame, 218 len); 219 if (!subelement) 220 return QDF_STATUS_E_FAILURE; 221 222 /* EID(1) + len (1) = 2 */ 223 sub_len = subelement[TAG_LEN_POS] + 2; 224 sub_copy = qdf_mem_malloc(sub_len); 225 if (!sub_copy) 226 return QDF_STATUS_E_NOMEM; 227 orig_copy = sub_copy; 228 qdf_mem_copy(sub_copy, subelement, sub_len); 229 230 /* parse ml ie */ 231 sub_copy = util_parse_multi_link_ctrl_ie(sub_copy, 232 sub_len, 233 &link_info_len); 234 235 if (!sub_copy) 236 return QDF_STATUS_E_NULL_VALUE; 237 238 /* parse sta profile ie */ 239 sub_copy = util_parse_sta_profile_ie(sub_copy, 240 link_info_len, 241 &per_sta_prof_len, 242 &bssid); 243 244 if (!sub_copy) { 245 qdf_mem_copy(pos, frame, len); 246 pos += len - WLAN_MAC_HDR_LEN_3A; 247 goto update_header; 248 } 249 250 /* go through IEs in frame and subelement, 251 * merge them into new_ie 252 */ 253 tmp_old = util_find_eid(WLAN_ELEMID_SSID, frame, len); 254 tmp_old = (tmp_old) ? tmp_old + tmp_old[TAG_LEN_POS] + MIN_IE_LEN : frame; 255 256 while (((tmp_old + tmp_old[TAG_LEN_POS] + MIN_IE_LEN) - frame) <= len) { 257 tmp = (uint8_t *)util_find_eid(tmp_old[0], 258 sub_copy, 259 per_sta_prof_len); 260 if (!tmp) { 261 /* ie in old ie but not in subelement */ 262 if (tmp_old[2] != WLAN_EXTN_ELEMID_MULTI_LINK) { 263 if ((pos + tmp_old[TAG_LEN_POS] + MIN_IE_LEN) <= 264 (assoc_link_frame + len)) { 265 qdf_mem_copy(pos, tmp_old, 266 (tmp_old[TAG_LEN_POS] + 267 MIN_IE_LEN)); 268 pos += tmp_old[TAG_LEN_POS] + MIN_IE_LEN; 269 } 270 } 271 } else { 272 /* ie in transmitting ie also in subelement, 273 * copy from subelement and flag the ie in subelement 274 * as copied (by setting eid field to 0xff). For 275 * vendor ie, compare OUI + type + subType to 276 * determine if they are the same ie. 277 */ 278 tmp_rem_len = per_sta_prof_len - (tmp - sub_copy); 279 if (tmp_old[0] == WLAN_ELEMID_VENDOR && 280 tmp_rem_len >= MIN_VENDOR_TAG_LEN) { 281 if (!qdf_mem_cmp(tmp_old + PAYLOAD_START_POS, 282 tmp + PAYLOAD_START_POS, 283 OUI_LEN)) { 284 /* same vendor ie, copy from 285 * subelement 286 */ 287 if ((pos + tmp[TAG_LEN_POS] + MIN_IE_LEN) <= 288 (assoc_link_frame + len)) { 289 qdf_mem_copy(pos, tmp, 290 tmp[TAG_LEN_POS] + 291 MIN_IE_LEN); 292 pos += tmp[TAG_LEN_POS] + MIN_IE_LEN; 293 tmp[0] = 0; 294 } 295 } else { 296 if ((pos + tmp_old[TAG_LEN_POS] + 297 MIN_IE_LEN) <= 298 (assoc_link_frame + len)) { 299 qdf_mem_copy(pos, tmp_old, 300 tmp_old[TAG_LEN_POS] + 301 MIN_IE_LEN); 302 pos += tmp_old[TAG_LEN_POS] + 303 MIN_IE_LEN; 304 } 305 } 306 } else if (tmp_old[0] == WLAN_ELEMID_EXTN_ELEM) { 307 if (tmp_old[PAYLOAD_START_POS] == 308 tmp[PAYLOAD_START_POS]) { 309 /* same ie, copy from subelement */ 310 if ((pos + tmp[TAG_LEN_POS] + MIN_IE_LEN) <= 311 (assoc_link_frame + len)) { 312 qdf_mem_copy(pos, tmp, 313 tmp[TAG_LEN_POS] + 314 MIN_IE_LEN); 315 pos += tmp[TAG_LEN_POS] + MIN_IE_LEN; 316 tmp[0] = 0; 317 } 318 } else { 319 if ((pos + tmp_old[TAG_LEN_POS] + MIN_IE_LEN) <= 320 (assoc_link_frame + len)) { 321 qdf_mem_copy(pos, tmp_old, 322 tmp_old[TAG_LEN_POS] + 323 MIN_IE_LEN); 324 pos += tmp_old[TAG_LEN_POS] + 325 MIN_IE_LEN; 326 } 327 } 328 } else { 329 /* copy ie from subelement into new ie */ 330 if ((pos + tmp[TAG_LEN_POS] + MIN_IE_LEN) <= 331 (assoc_link_frame + len)) { 332 qdf_mem_copy(pos, tmp, 333 tmp[TAG_LEN_POS] + MIN_IE_LEN); 334 pos += tmp[TAG_LEN_POS] + MIN_IE_LEN; 335 tmp[0] = 0; 336 } 337 } 338 } 339 340 if (((tmp_old + tmp_old[TAG_LEN_POS] + MIN_IE_LEN) - frame) >= len) 341 break; 342 343 tmp_old += tmp_old[TAG_LEN_POS] + MIN_IE_LEN; 344 } 345 346 update_header: 347 /* Copy the link mac addr */ 348 qdf_mem_copy(hdr->i_addr3, bssid.bytes, 349 QDF_MAC_ADDR_SIZE); 350 qdf_mem_copy(hdr->i_addr2, bssid.bytes, 351 QDF_MAC_ADDR_SIZE); 352 qdf_mem_copy(hdr->i_addr1, &link_addr, 353 QDF_MAC_ADDR_SIZE); 354 hdr->i_fc[0] = FC0_IEEE_MGMT_FRM; 355 hdr->i_fc[1] = FC1_IEEE_MGMT_FRM; 356 /* seq num not used so not populated */ 357 qdf_mem_free(orig_copy); 358 359 return QDF_STATUS_SUCCESS; 360 } 361 #endif 362