1 // SPDX-License-Identifier: GPL-2.0-only 2 /* 3 * NXP Wireless LAN device driver: management IE handling- setting and 4 * deleting IE. 5 * 6 * Copyright 2011-2020 NXP 7 */ 8 9 #include "main.h" 10 11 /* This function checks if current IE index is used by any on other interface. 12 * Return: -1: yes, current IE index is used by someone else. 13 * 0: no, current IE index is NOT used by other interface. 14 */ 15 static int 16 mwifiex_ie_index_used_by_other_intf(struct mwifiex_private *priv, u16 idx) 17 { 18 int i; 19 struct mwifiex_adapter *adapter = priv->adapter; 20 struct mwifiex_ie *ie; 21 22 for (i = 0; i < adapter->priv_num; i++) { 23 if (adapter->priv[i] != priv) { 24 ie = &adapter->priv[i]->mgmt_ie[idx]; 25 if (ie->mgmt_subtype_mask && ie->ie_length) 26 return -1; 27 } 28 } 29 30 return 0; 31 } 32 33 /* Get unused IE index. This index will be used for setting new IE */ 34 static int 35 mwifiex_ie_get_autoidx(struct mwifiex_private *priv, u16 subtype_mask, 36 struct mwifiex_ie *ie, u16 *index) 37 { 38 u16 mask, len, i; 39 40 for (i = 0; i < priv->adapter->max_mgmt_ie_index; i++) { 41 mask = le16_to_cpu(priv->mgmt_ie[i].mgmt_subtype_mask); 42 len = le16_to_cpu(ie->ie_length); 43 44 if (mask == MWIFIEX_AUTO_IDX_MASK) 45 continue; 46 47 if (mask == subtype_mask) { 48 if (len > IEEE_MAX_IE_SIZE) 49 continue; 50 51 *index = i; 52 return 0; 53 } 54 55 if (!priv->mgmt_ie[i].ie_length) { 56 if (mwifiex_ie_index_used_by_other_intf(priv, i)) 57 continue; 58 59 *index = i; 60 return 0; 61 } 62 } 63 64 return -1; 65 } 66 67 /* This function prepares IE data buffer for command to be sent to FW */ 68 static int 69 mwifiex_update_autoindex_ies(struct mwifiex_private *priv, 70 struct mwifiex_ie_list *ie_list) 71 { 72 u16 travel_len, index, mask; 73 s16 input_len, tlv_len; 74 struct mwifiex_ie *ie; 75 u8 *tmp; 76 77 input_len = le16_to_cpu(ie_list->len); 78 travel_len = sizeof(struct mwifiex_ie_types_header); 79 80 ie_list->len = 0; 81 82 while (input_len >= sizeof(struct mwifiex_ie_types_header)) { 83 ie = (struct mwifiex_ie *)(((u8 *)ie_list) + travel_len); 84 tlv_len = le16_to_cpu(ie->ie_length); 85 travel_len += tlv_len + MWIFIEX_IE_HDR_SIZE; 86 87 if (input_len < tlv_len + MWIFIEX_IE_HDR_SIZE) 88 return -1; 89 index = le16_to_cpu(ie->ie_index); 90 mask = le16_to_cpu(ie->mgmt_subtype_mask); 91 92 if (index == MWIFIEX_AUTO_IDX_MASK) { 93 /* automatic addition */ 94 if (mwifiex_ie_get_autoidx(priv, mask, ie, &index)) 95 return -1; 96 if (index == MWIFIEX_AUTO_IDX_MASK) 97 return -1; 98 99 tmp = (u8 *)&priv->mgmt_ie[index].ie_buffer; 100 memcpy(tmp, &ie->ie_buffer, le16_to_cpu(ie->ie_length)); 101 priv->mgmt_ie[index].ie_length = ie->ie_length; 102 priv->mgmt_ie[index].ie_index = cpu_to_le16(index); 103 priv->mgmt_ie[index].mgmt_subtype_mask = 104 cpu_to_le16(mask); 105 106 ie->ie_index = cpu_to_le16(index); 107 } else { 108 if (mask != MWIFIEX_DELETE_MASK) 109 return -1; 110 /* 111 * Check if this index is being used on any 112 * other interface. 113 */ 114 if (mwifiex_ie_index_used_by_other_intf(priv, index)) 115 return -1; 116 117 ie->ie_length = 0; 118 memcpy(&priv->mgmt_ie[index], ie, 119 sizeof(struct mwifiex_ie)); 120 } 121 122 le16_unaligned_add_cpu(&ie_list->len, 123 le16_to_cpu( 124 priv->mgmt_ie[index].ie_length) + 125 MWIFIEX_IE_HDR_SIZE); 126 input_len -= tlv_len + MWIFIEX_IE_HDR_SIZE; 127 } 128 129 if (GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_UAP) 130 return mwifiex_send_cmd(priv, HostCmd_CMD_UAP_SYS_CONFIG, 131 HostCmd_ACT_GEN_SET, 132 UAP_CUSTOM_IE_I, ie_list, true); 133 134 return 0; 135 } 136 137 /* Copy individual custom IEs for beacon, probe response and assoc response 138 * and prepare single structure for IE setting. 139 * This function also updates allocated IE indices from driver. 140 */ 141 static int 142 mwifiex_update_uap_custom_ie(struct mwifiex_private *priv, 143 struct mwifiex_ie *beacon_ie, u16 *beacon_idx, 144 struct mwifiex_ie *pr_ie, u16 *probe_idx, 145 struct mwifiex_ie *ar_ie, u16 *assoc_idx) 146 { 147 struct mwifiex_ie_list *ap_custom_ie; 148 u8 *pos; 149 u16 len; 150 int ret; 151 152 ap_custom_ie = kzalloc(sizeof(*ap_custom_ie), GFP_KERNEL); 153 if (!ap_custom_ie) 154 return -ENOMEM; 155 156 ap_custom_ie->type = cpu_to_le16(TLV_TYPE_MGMT_IE); 157 pos = (u8 *)ap_custom_ie->ie_list; 158 159 if (beacon_ie) { 160 len = sizeof(struct mwifiex_ie) - IEEE_MAX_IE_SIZE + 161 le16_to_cpu(beacon_ie->ie_length); 162 memcpy(pos, beacon_ie, len); 163 pos += len; 164 le16_unaligned_add_cpu(&ap_custom_ie->len, len); 165 } 166 if (pr_ie) { 167 len = sizeof(struct mwifiex_ie) - IEEE_MAX_IE_SIZE + 168 le16_to_cpu(pr_ie->ie_length); 169 memcpy(pos, pr_ie, len); 170 pos += len; 171 le16_unaligned_add_cpu(&ap_custom_ie->len, len); 172 } 173 if (ar_ie) { 174 len = sizeof(struct mwifiex_ie) - IEEE_MAX_IE_SIZE + 175 le16_to_cpu(ar_ie->ie_length); 176 memcpy(pos, ar_ie, len); 177 pos += len; 178 le16_unaligned_add_cpu(&ap_custom_ie->len, len); 179 } 180 181 ret = mwifiex_update_autoindex_ies(priv, ap_custom_ie); 182 183 pos = (u8 *)(&ap_custom_ie->ie_list[0].ie_index); 184 if (beacon_ie && *beacon_idx == MWIFIEX_AUTO_IDX_MASK) { 185 /* save beacon ie index after auto-indexing */ 186 *beacon_idx = le16_to_cpu(ap_custom_ie->ie_list[0].ie_index); 187 len = sizeof(*beacon_ie) - IEEE_MAX_IE_SIZE + 188 le16_to_cpu(beacon_ie->ie_length); 189 pos += len; 190 } 191 if (pr_ie && le16_to_cpu(pr_ie->ie_index) == MWIFIEX_AUTO_IDX_MASK) { 192 /* save probe resp ie index after auto-indexing */ 193 *probe_idx = *((u16 *)pos); 194 len = sizeof(*pr_ie) - IEEE_MAX_IE_SIZE + 195 le16_to_cpu(pr_ie->ie_length); 196 pos += len; 197 } 198 if (ar_ie && le16_to_cpu(ar_ie->ie_index) == MWIFIEX_AUTO_IDX_MASK) 199 /* save assoc resp ie index after auto-indexing */ 200 *assoc_idx = *((u16 *)pos); 201 202 kfree(ap_custom_ie); 203 return ret; 204 } 205 206 /* This function checks if the vendor specified IE is present in passed buffer 207 * and copies it to mwifiex_ie structure. 208 * Function takes pointer to struct mwifiex_ie pointer as argument. 209 * If the vendor specified IE is present then memory is allocated for 210 * mwifiex_ie pointer and filled in with IE. Caller should take care of freeing 211 * this memory. 212 */ 213 static int mwifiex_update_vs_ie(const u8 *ies, int ies_len, 214 struct mwifiex_ie **ie_ptr, u16 mask, 215 unsigned int oui, u8 oui_type) 216 { 217 struct ieee_types_header *vs_ie; 218 struct mwifiex_ie *ie = *ie_ptr; 219 const u8 *vendor_ie; 220 221 vendor_ie = cfg80211_find_vendor_ie(oui, oui_type, ies, ies_len); 222 if (vendor_ie) { 223 if (!*ie_ptr) { 224 *ie_ptr = kzalloc(sizeof(struct mwifiex_ie), 225 GFP_KERNEL); 226 if (!*ie_ptr) 227 return -ENOMEM; 228 ie = *ie_ptr; 229 } 230 231 vs_ie = (struct ieee_types_header *)vendor_ie; 232 if (le16_to_cpu(ie->ie_length) + vs_ie->len + 2 > 233 IEEE_MAX_IE_SIZE) 234 return -EINVAL; 235 memcpy(ie->ie_buffer + le16_to_cpu(ie->ie_length), 236 vs_ie, vs_ie->len + 2); 237 le16_unaligned_add_cpu(&ie->ie_length, vs_ie->len + 2); 238 ie->mgmt_subtype_mask = cpu_to_le16(mask); 239 ie->ie_index = cpu_to_le16(MWIFIEX_AUTO_IDX_MASK); 240 } 241 242 *ie_ptr = ie; 243 return 0; 244 } 245 246 /* This function parses beacon IEs, probe response IEs, association response IEs 247 * from cfg80211_ap_settings->beacon and sets these IE to FW. 248 */ 249 static int mwifiex_set_mgmt_beacon_data_ies(struct mwifiex_private *priv, 250 struct cfg80211_beacon_data *data) 251 { 252 struct mwifiex_ie *beacon_ie = NULL, *pr_ie = NULL, *ar_ie = NULL; 253 u16 beacon_idx = MWIFIEX_AUTO_IDX_MASK, pr_idx = MWIFIEX_AUTO_IDX_MASK; 254 u16 ar_idx = MWIFIEX_AUTO_IDX_MASK; 255 int ret = 0; 256 257 if (data->beacon_ies && data->beacon_ies_len) { 258 mwifiex_update_vs_ie(data->beacon_ies, data->beacon_ies_len, 259 &beacon_ie, MGMT_MASK_BEACON, 260 WLAN_OUI_MICROSOFT, 261 WLAN_OUI_TYPE_MICROSOFT_WPS); 262 mwifiex_update_vs_ie(data->beacon_ies, data->beacon_ies_len, 263 &beacon_ie, MGMT_MASK_BEACON, 264 WLAN_OUI_WFA, WLAN_OUI_TYPE_WFA_P2P); 265 } 266 267 if (data->proberesp_ies && data->proberesp_ies_len) { 268 mwifiex_update_vs_ie(data->proberesp_ies, 269 data->proberesp_ies_len, &pr_ie, 270 MGMT_MASK_PROBE_RESP, WLAN_OUI_MICROSOFT, 271 WLAN_OUI_TYPE_MICROSOFT_WPS); 272 mwifiex_update_vs_ie(data->proberesp_ies, 273 data->proberesp_ies_len, &pr_ie, 274 MGMT_MASK_PROBE_RESP, 275 WLAN_OUI_WFA, WLAN_OUI_TYPE_WFA_P2P); 276 } 277 278 if (data->assocresp_ies && data->assocresp_ies_len) { 279 mwifiex_update_vs_ie(data->assocresp_ies, 280 data->assocresp_ies_len, &ar_ie, 281 MGMT_MASK_ASSOC_RESP | 282 MGMT_MASK_REASSOC_RESP, 283 WLAN_OUI_MICROSOFT, 284 WLAN_OUI_TYPE_MICROSOFT_WPS); 285 mwifiex_update_vs_ie(data->assocresp_ies, 286 data->assocresp_ies_len, &ar_ie, 287 MGMT_MASK_ASSOC_RESP | 288 MGMT_MASK_REASSOC_RESP, WLAN_OUI_WFA, 289 WLAN_OUI_TYPE_WFA_P2P); 290 } 291 292 if (beacon_ie || pr_ie || ar_ie) { 293 ret = mwifiex_update_uap_custom_ie(priv, beacon_ie, 294 &beacon_idx, pr_ie, 295 &pr_idx, ar_ie, &ar_idx); 296 if (ret) 297 goto done; 298 } 299 300 priv->beacon_idx = beacon_idx; 301 priv->proberesp_idx = pr_idx; 302 priv->assocresp_idx = ar_idx; 303 304 done: 305 kfree(beacon_ie); 306 kfree(pr_ie); 307 kfree(ar_ie); 308 309 return ret; 310 } 311 312 /* This function parses head and tail IEs, from cfg80211_beacon_data and sets 313 * these IE to FW. 314 */ 315 static int mwifiex_uap_parse_tail_ies(struct mwifiex_private *priv, 316 struct cfg80211_beacon_data *info) 317 { 318 struct mwifiex_ie *gen_ie; 319 struct ieee_types_header *hdr; 320 struct ieee80211_vendor_ie *vendorhdr; 321 u16 gen_idx = MWIFIEX_AUTO_IDX_MASK, ie_len = 0; 322 int left_len, parsed_len = 0; 323 unsigned int token_len; 324 int err = 0; 325 326 if (!info->tail || !info->tail_len) 327 return 0; 328 329 gen_ie = kzalloc(sizeof(*gen_ie), GFP_KERNEL); 330 if (!gen_ie) 331 return -ENOMEM; 332 333 left_len = info->tail_len; 334 335 /* Many IEs are generated in FW by parsing bss configuration. 336 * Let's not add them here; else we may end up duplicating these IEs 337 */ 338 while (left_len > sizeof(struct ieee_types_header)) { 339 hdr = (void *)(info->tail + parsed_len); 340 token_len = hdr->len + sizeof(struct ieee_types_header); 341 if (token_len > left_len) { 342 err = -EINVAL; 343 goto out; 344 } 345 346 switch (hdr->element_id) { 347 case WLAN_EID_SSID: 348 case WLAN_EID_SUPP_RATES: 349 case WLAN_EID_COUNTRY: 350 case WLAN_EID_PWR_CONSTRAINT: 351 case WLAN_EID_ERP_INFO: 352 case WLAN_EID_EXT_SUPP_RATES: 353 case WLAN_EID_HT_CAPABILITY: 354 case WLAN_EID_HT_OPERATION: 355 case WLAN_EID_VHT_CAPABILITY: 356 case WLAN_EID_VHT_OPERATION: 357 break; 358 case WLAN_EID_VENDOR_SPECIFIC: 359 /* Skip only Microsoft WMM IE */ 360 if (cfg80211_find_vendor_ie(WLAN_OUI_MICROSOFT, 361 WLAN_OUI_TYPE_MICROSOFT_WMM, 362 (const u8 *)hdr, 363 token_len)) 364 break; 365 fallthrough; 366 default: 367 if (ie_len + token_len > IEEE_MAX_IE_SIZE) { 368 err = -EINVAL; 369 goto out; 370 } 371 memcpy(gen_ie->ie_buffer + ie_len, hdr, token_len); 372 ie_len += token_len; 373 break; 374 } 375 left_len -= token_len; 376 parsed_len += token_len; 377 } 378 379 /* parse only WPA vendor IE from tail, WMM IE is configured by 380 * bss_config command 381 */ 382 vendorhdr = (void *)cfg80211_find_vendor_ie(WLAN_OUI_MICROSOFT, 383 WLAN_OUI_TYPE_MICROSOFT_WPA, 384 info->tail, info->tail_len); 385 if (vendorhdr) { 386 token_len = vendorhdr->len + sizeof(struct ieee_types_header); 387 if (ie_len + token_len > IEEE_MAX_IE_SIZE) { 388 err = -EINVAL; 389 goto out; 390 } 391 memcpy(gen_ie->ie_buffer + ie_len, vendorhdr, token_len); 392 ie_len += token_len; 393 } 394 395 if (!ie_len) 396 goto out; 397 398 gen_ie->ie_index = cpu_to_le16(gen_idx); 399 gen_ie->mgmt_subtype_mask = cpu_to_le16(MGMT_MASK_BEACON | 400 MGMT_MASK_PROBE_RESP | 401 MGMT_MASK_ASSOC_RESP); 402 gen_ie->ie_length = cpu_to_le16(ie_len); 403 404 if (mwifiex_update_uap_custom_ie(priv, gen_ie, &gen_idx, NULL, NULL, 405 NULL, NULL)) { 406 err = -EINVAL; 407 goto out; 408 } 409 410 priv->gen_idx = gen_idx; 411 412 out: 413 kfree(gen_ie); 414 return err; 415 } 416 417 /* This function parses different IEs-head & tail IEs, beacon IEs, 418 * probe response IEs, association response IEs from cfg80211_ap_settings 419 * function and sets these IE to FW. 420 */ 421 int mwifiex_set_mgmt_ies(struct mwifiex_private *priv, 422 struct cfg80211_beacon_data *info) 423 { 424 int ret; 425 426 ret = mwifiex_uap_parse_tail_ies(priv, info); 427 428 if (ret) 429 return ret; 430 431 return mwifiex_set_mgmt_beacon_data_ies(priv, info); 432 } 433 434 /* This function removes management IE set */ 435 int mwifiex_del_mgmt_ies(struct mwifiex_private *priv) 436 { 437 struct mwifiex_ie *beacon_ie = NULL, *pr_ie = NULL; 438 struct mwifiex_ie *ar_ie = NULL, *gen_ie = NULL; 439 int ret = 0; 440 441 if (priv->gen_idx != MWIFIEX_AUTO_IDX_MASK) { 442 gen_ie = kmalloc(sizeof(*gen_ie), GFP_KERNEL); 443 if (!gen_ie) 444 return -ENOMEM; 445 446 gen_ie->ie_index = cpu_to_le16(priv->gen_idx); 447 gen_ie->mgmt_subtype_mask = cpu_to_le16(MWIFIEX_DELETE_MASK); 448 gen_ie->ie_length = 0; 449 if (mwifiex_update_uap_custom_ie(priv, gen_ie, &priv->gen_idx, 450 NULL, &priv->proberesp_idx, 451 NULL, &priv->assocresp_idx)) { 452 ret = -1; 453 goto done; 454 } 455 456 priv->gen_idx = MWIFIEX_AUTO_IDX_MASK; 457 } 458 459 if (priv->beacon_idx != MWIFIEX_AUTO_IDX_MASK) { 460 beacon_ie = kmalloc(sizeof(struct mwifiex_ie), GFP_KERNEL); 461 if (!beacon_ie) { 462 ret = -ENOMEM; 463 goto done; 464 } 465 beacon_ie->ie_index = cpu_to_le16(priv->beacon_idx); 466 beacon_ie->mgmt_subtype_mask = cpu_to_le16(MWIFIEX_DELETE_MASK); 467 beacon_ie->ie_length = 0; 468 } 469 if (priv->proberesp_idx != MWIFIEX_AUTO_IDX_MASK) { 470 pr_ie = kmalloc(sizeof(struct mwifiex_ie), GFP_KERNEL); 471 if (!pr_ie) { 472 ret = -ENOMEM; 473 goto done; 474 } 475 pr_ie->ie_index = cpu_to_le16(priv->proberesp_idx); 476 pr_ie->mgmt_subtype_mask = cpu_to_le16(MWIFIEX_DELETE_MASK); 477 pr_ie->ie_length = 0; 478 } 479 if (priv->assocresp_idx != MWIFIEX_AUTO_IDX_MASK) { 480 ar_ie = kmalloc(sizeof(struct mwifiex_ie), GFP_KERNEL); 481 if (!ar_ie) { 482 ret = -ENOMEM; 483 goto done; 484 } 485 ar_ie->ie_index = cpu_to_le16(priv->assocresp_idx); 486 ar_ie->mgmt_subtype_mask = cpu_to_le16(MWIFIEX_DELETE_MASK); 487 ar_ie->ie_length = 0; 488 } 489 490 if (beacon_ie || pr_ie || ar_ie) 491 ret = mwifiex_update_uap_custom_ie(priv, 492 beacon_ie, &priv->beacon_idx, 493 pr_ie, &priv->proberesp_idx, 494 ar_ie, &priv->assocresp_idx); 495 496 done: 497 kfree(gen_ie); 498 kfree(beacon_ie); 499 kfree(pr_ie); 500 kfree(ar_ie); 501 502 return ret; 503 } 504