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 30 #ifdef WLAN_FEATURE_11BE_MLO 31 32 static uint8_t *util_find_eid(uint8_t eid, uint8_t *frame, qdf_size_t len) 33 { 34 if (!frame) 35 return NULL; 36 37 while (len >= MIN_IE_LEN && len >= frame[TAG_LEN_POS] + MIN_IE_LEN) { 38 if (frame[ID_POS] == eid) 39 return frame; 40 41 len -= frame[TAG_LEN_POS] + MIN_IE_LEN; 42 frame += frame[TAG_LEN_POS] + MIN_IE_LEN; 43 } 44 45 return NULL; 46 } 47 48 static 49 uint8_t *util_find_extn_eid(uint8_t eid, uint8_t extn_eid, 50 uint8_t *frame, qdf_size_t len) 51 { 52 if (!frame) 53 return NULL; 54 55 while (len >= MIN_IE_LEN && len >= frame[TAG_LEN_POS] + MIN_IE_LEN) { 56 if ((frame[ID_POS] == eid) && 57 (frame[ELEM_ID_EXTN_POS] == extn_eid)) 58 return frame; 59 60 len -= frame[TAG_LEN_POS] + MIN_IE_LEN; 61 frame += frame[TAG_LEN_POS] + MIN_IE_LEN; 62 } 63 return NULL; 64 } 65 66 static 67 uint8_t *util_parse_multi_link_ctrl_ie(uint8_t *subelement, 68 qdf_size_t len, 69 qdf_size_t *link_info_ie_len) 70 { 71 qdf_size_t sub_ie_len = 0; 72 73 if (!subelement || !len) 74 return NULL; 75 76 /* sta prof len = ML IE len + EID extn(1) + Multi Lnk ctrl(2) */ 77 sub_ie_len += TAG_LEN_POS + MIN_IE_LEN + 2; 78 79 /* check if MLD MAC address present */ 80 if (qdf_test_bit(WLAN_ML_CTRL_PBM_IDX | 81 WLAN_ML_BV_CTRL_PBM_MLDMACADDR_P, 82 &subelement[MULTI_LINK_CTRL_1])) 83 sub_ie_len = sub_ie_len + QDF_MAC_ADDR_SIZE; 84 85 /* check if Link ID info */ 86 if (qdf_test_bit(WLAN_ML_CTRL_PBM_IDX | 87 WLAN_ML_BV_CTRL_PBM_LINKIDINFO_P, 88 &subelement[MULTI_LINK_CTRL_1])) 89 sub_ie_len = sub_ie_len + TAG_LEN_POS; 90 91 /* check if BSS parameter change count */ 92 if (qdf_test_bit(WLAN_ML_CTRL_PBM_IDX | 93 WLAN_ML_BV_CTRL_PBM_BSSPARAMCHANGECNT_P, 94 &subelement[MULTI_LINK_CTRL_1])) 95 sub_ie_len = sub_ie_len + TAG_LEN_POS; 96 97 /* check if Medium Sync Delay Info present */ 98 if (qdf_test_bit(WLAN_ML_CTRL_PBM_IDX | 99 WLAN_ML_BV_CTRL_PBM_MEDIUMSYNCDELAYINFO_P, 100 &subelement[MULTI_LINK_CTRL_1])) 101 sub_ie_len = sub_ie_len + PAYLOAD_START_POS; 102 103 /* check if EML cap present */ 104 if (qdf_test_bit(WLAN_ML_CTRL_PBM_IDX | 105 WLAN_ML_BV_CTRL_PBM_EMLCAP_P, 106 &subelement[MULTI_LINK_CTRL_2])) 107 sub_ie_len = sub_ie_len + SUBELEMENT_START_POS; 108 109 /* check if MLD cap present */ 110 if (qdf_test_bit(WLAN_ML_CTRL_PBM_IDX | 111 WLAN_ML_BV_CTRL_PBM_MLDCAP_P, 112 &subelement[MULTI_LINK_CTRL_2])) 113 sub_ie_len = sub_ie_len + PAYLOAD_START_POS; 114 115 if (link_info_ie_len) { 116 *link_info_ie_len = len - sub_ie_len; 117 mlo_debug("link_info_ie_len:%lu, sub_ie_len:%lu", 118 *link_info_ie_len, sub_ie_len); 119 } 120 121 return &subelement[sub_ie_len]; 122 } 123 124 static 125 uint8_t *util_parse_sta_profile_ie(uint8_t *subelement, 126 qdf_size_t len, 127 qdf_size_t *per_sta_prof_ie_len, 128 struct qdf_mac_addr *bssid) 129 { 130 qdf_size_t tmp_len = 0; 131 uint8_t *tmp = NULL; 132 133 if (!subelement || !len) 134 return NULL; 135 136 if (subelement[0] == 17) 137 tmp = subelement; 138 if (!tmp) 139 return NULL; 140 141 /* tmp_len = 2 (sta ctrl) + 1 (sub EID) + 1 (len) */ 142 tmp_len = PAYLOAD_START_POS + 2; 143 144 /* check DTIM info present bit */ 145 if (qdf_test_bit(WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_DTIMINFOP_IDX, 146 &subelement[STA_CTRL_1])) 147 tmp_len = tmp_len + MIN_IE_LEN; 148 149 /* check Beacon interval present bit */ 150 if (qdf_test_bit(WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_BCNINTP_IDX, 151 &subelement[STA_CTRL_1])) 152 tmp_len = tmp_len + TAG_LEN_POS; 153 154 /* check STA link mac addr info present bit */ 155 if (qdf_test_bit(WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_MACADDRP_IDX, 156 &subelement[STA_CTRL_1])) { 157 tmp_len = tmp_len + QDF_MAC_ADDR_SIZE; 158 qdf_mem_copy(&bssid->bytes, &tmp[4], QDF_MAC_ADDR_SIZE); 159 } 160 161 /* Add Link ID offset,as it will always be present in assoc rsp mlo ie */ 162 tmp_len = tmp_len + TAG_LEN_POS; 163 164 /* check NTSR Link pair present bit */ 165 if (qdf_test_bit(WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_NSTRLINKPRP_IDX % 8, 166 &subelement[STA_CTRL_2])) 167 tmp_len = tmp_len + MIN_IE_LEN; 168 169 /* check NTSR bitmap size present bit */ 170 if (qdf_test_bit(WLAN_ML_BV_LINFO_PERSTAPROF_STACTRL_NSTRBMSZ_IDX % 8, 171 &subelement[STA_CTRL_2])) 172 tmp_len = tmp_len + TAG_LEN_POS; 173 174 if (len <= tmp_len) { 175 mlo_err("len %lu <= tmp_len %lu, return", len, tmp_len); 176 return NULL; 177 } 178 *per_sta_prof_ie_len = len - tmp_len; 179 180 return &tmp[tmp_len]; 181 } 182 183 QDF_STATUS util_gen_link_assoc_rsp(uint8_t *frame, qdf_size_t len, 184 struct qdf_mac_addr link_addr, 185 uint8_t *assoc_link_frame) 186 { 187 uint8_t *tmp = NULL; 188 const uint8_t *tmp_old; 189 qdf_size_t sub_len, tmp_rem_len; 190 qdf_size_t link_info_len, per_sta_prof_len = 0; 191 uint8_t *subelement; 192 uint8_t *pos; 193 uint8_t *sub_copy, *orig_copy; 194 struct qdf_mac_addr bssid; 195 struct wlan_frame_hdr *hdr; 196 197 if (!frame || !len) 198 return QDF_STATUS_E_NULL_VALUE; 199 200 pos = assoc_link_frame; 201 hdr = (struct wlan_frame_hdr *)pos; 202 pos = pos + WLAN_MAC_HDR_LEN_3A; 203 204 /* Assoc resp Capability(2) + AID(2) + Status Code(2) */ 205 qdf_mem_copy(pos, frame, WLAN_ASSOC_RSP_IES_OFFSET); 206 pos = pos + WLAN_ASSOC_RSP_IES_OFFSET; 207 208 /* find MLO IE */ 209 subelement = util_find_extn_eid(WLAN_ELEMID_EXTN_ELEM, 210 WLAN_EXTN_ELEMID_MULTI_LINK, 211 frame, 212 len); 213 if (!subelement) 214 return QDF_STATUS_E_FAILURE; 215 216 /* EID(1) + len (1) = 2 */ 217 sub_len = subelement[TAG_LEN_POS] + 2; 218 sub_copy = qdf_mem_malloc(sub_len); 219 if (!sub_copy) 220 return QDF_STATUS_E_NOMEM; 221 orig_copy = sub_copy; 222 qdf_mem_copy(sub_copy, subelement, sub_len); 223 224 /* parse ml ie */ 225 sub_copy = util_parse_multi_link_ctrl_ie(sub_copy, 226 sub_len, 227 &link_info_len); 228 229 if (!sub_copy) 230 return QDF_STATUS_E_NULL_VALUE; 231 232 /* parse sta profile ie */ 233 sub_copy = util_parse_sta_profile_ie(sub_copy, 234 link_info_len, 235 &per_sta_prof_len, 236 &bssid); 237 238 if (!sub_copy) { 239 qdf_mem_copy(pos, frame, len); 240 pos += len - WLAN_MAC_HDR_LEN_3A; 241 goto update_header; 242 } 243 244 /* go through IEs in frame and subelement, 245 * merge them into new_ie 246 */ 247 tmp_old = util_find_eid(WLAN_ELEMID_SSID, frame, len); 248 tmp_old = (tmp_old) ? tmp_old + tmp_old[TAG_LEN_POS] + MIN_IE_LEN : frame; 249 250 while (((tmp_old + tmp_old[TAG_LEN_POS] + MIN_IE_LEN) - frame) <= len) { 251 tmp = (uint8_t *)util_find_eid(tmp_old[0], 252 sub_copy, 253 per_sta_prof_len); 254 if (!tmp) { 255 /* ie in old ie but not in subelement */ 256 if (tmp_old[2] != WLAN_EXTN_ELEMID_MULTI_LINK) { 257 if ((pos + tmp_old[TAG_LEN_POS] + MIN_IE_LEN) <= 258 (assoc_link_frame + len)) { 259 qdf_mem_copy(pos, tmp_old, 260 (tmp_old[TAG_LEN_POS] + 261 MIN_IE_LEN)); 262 pos += tmp_old[TAG_LEN_POS] + MIN_IE_LEN; 263 } 264 } 265 } else { 266 /* ie in transmitting ie also in subelement, 267 * copy from subelement and flag the ie in subelement 268 * as copied (by setting eid field to 0xff). For 269 * vendor ie, compare OUI + type + subType to 270 * determine if they are the same ie. 271 */ 272 tmp_rem_len = per_sta_prof_len - (tmp - sub_copy); 273 if (tmp_old[0] == WLAN_ELEMID_VENDOR && 274 tmp_rem_len >= MIN_VENDOR_TAG_LEN) { 275 if (!qdf_mem_cmp(tmp_old + PAYLOAD_START_POS, 276 tmp + PAYLOAD_START_POS, 277 OUI_LEN)) { 278 /* same vendor ie, copy from 279 * subelement 280 */ 281 if ((pos + tmp[TAG_LEN_POS] + MIN_IE_LEN) <= 282 (assoc_link_frame + len)) { 283 qdf_mem_copy(pos, tmp, 284 tmp[TAG_LEN_POS] + 285 MIN_IE_LEN); 286 pos += tmp[TAG_LEN_POS] + MIN_IE_LEN; 287 tmp[0] = 0; 288 } 289 } else { 290 if ((pos + tmp_old[TAG_LEN_POS] + 291 MIN_IE_LEN) <= 292 (assoc_link_frame + len)) { 293 qdf_mem_copy(pos, tmp_old, 294 tmp_old[TAG_LEN_POS] + 295 MIN_IE_LEN); 296 pos += tmp_old[TAG_LEN_POS] + 297 MIN_IE_LEN; 298 } 299 } 300 } else if (tmp_old[0] == WLAN_ELEMID_EXTN_ELEM) { 301 if (tmp_old[PAYLOAD_START_POS] == 302 tmp[PAYLOAD_START_POS]) { 303 /* same ie, copy from subelement */ 304 if ((pos + tmp[TAG_LEN_POS] + MIN_IE_LEN) <= 305 (assoc_link_frame + len)) { 306 qdf_mem_copy(pos, tmp, 307 tmp[TAG_LEN_POS] + 308 MIN_IE_LEN); 309 pos += tmp[TAG_LEN_POS] + MIN_IE_LEN; 310 tmp[0] = 0; 311 } 312 } else { 313 if ((pos + tmp_old[TAG_LEN_POS] + MIN_IE_LEN) <= 314 (assoc_link_frame + len)) { 315 qdf_mem_copy(pos, tmp_old, 316 tmp_old[TAG_LEN_POS] + 317 MIN_IE_LEN); 318 pos += tmp_old[TAG_LEN_POS] + 319 MIN_IE_LEN; 320 } 321 } 322 } else { 323 /* copy ie from subelement into new ie */ 324 if ((pos + tmp[TAG_LEN_POS] + MIN_IE_LEN) <= 325 (assoc_link_frame + len)) { 326 qdf_mem_copy(pos, tmp, 327 tmp[TAG_LEN_POS] + MIN_IE_LEN); 328 pos += tmp[TAG_LEN_POS] + MIN_IE_LEN; 329 tmp[0] = 0; 330 } 331 } 332 } 333 334 if (((tmp_old + tmp_old[TAG_LEN_POS] + MIN_IE_LEN) - frame) >= len) 335 break; 336 337 tmp_old += tmp_old[TAG_LEN_POS] + MIN_IE_LEN; 338 } 339 340 update_header: 341 if (bssid.bytes) { 342 /* Copy the link mac addr */ 343 qdf_mem_copy(hdr->i_addr3, bssid.bytes, 344 QDF_MAC_ADDR_SIZE); 345 qdf_mem_copy(hdr->i_addr2, bssid.bytes, 346 QDF_MAC_ADDR_SIZE); 347 qdf_mem_copy(hdr->i_addr1, &link_addr, 348 QDF_MAC_ADDR_SIZE); 349 hdr->i_fc[0] = FC0_IEEE_MGMT_FRM; 350 hdr->i_fc[1] = FC1_IEEE_MGMT_FRM; 351 /* seq num not used so not populated */ 352 } 353 qdf_mem_free(orig_copy); 354 355 return QDF_STATUS_SUCCESS; 356 } 357 #endif 358