1 // SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause 2 /* 3 * Copyright (C) 2022-2024 Intel Corporation 4 */ 5 #include "mvm.h" 6 #include "time-sync.h" 7 #include "sta.h" 8 9 u32 iwl_mvm_sta_fw_id_mask(struct iwl_mvm *mvm, struct ieee80211_sta *sta, 10 int filter_link_id) 11 { 12 struct ieee80211_link_sta *link_sta; 13 struct iwl_mvm_sta *mvmsta; 14 struct ieee80211_vif *vif; 15 unsigned int link_id; 16 u32 result = 0; 17 18 if (!sta) 19 return 0; 20 21 mvmsta = iwl_mvm_sta_from_mac80211(sta); 22 vif = mvmsta->vif; 23 24 /* it's easy when the STA is not an MLD */ 25 if (!sta->valid_links) 26 return BIT(mvmsta->deflink.sta_id); 27 28 /* but if it is an MLD, get the mask of all the FW STAs it has ... */ 29 for_each_sta_active_link(vif, sta, link_sta, link_id) { 30 struct iwl_mvm_link_sta *mvm_link_sta; 31 32 /* unless we have a specific link in mind */ 33 if (filter_link_id >= 0 && link_id != filter_link_id) 34 continue; 35 36 mvm_link_sta = 37 rcu_dereference_check(mvmsta->link[link_id], 38 lockdep_is_held(&mvm->mutex)); 39 if (!mvm_link_sta) 40 continue; 41 42 result |= BIT(mvm_link_sta->sta_id); 43 } 44 45 return result; 46 } 47 48 static int iwl_mvm_mld_send_sta_cmd(struct iwl_mvm *mvm, 49 struct iwl_mvm_sta_cfg_cmd *cmd) 50 { 51 int ret = iwl_mvm_send_cmd_pdu(mvm, 52 WIDE_ID(MAC_CONF_GROUP, STA_CONFIG_CMD), 53 0, sizeof(*cmd), cmd); 54 if (ret) 55 IWL_ERR(mvm, "STA_CONFIG_CMD send failed, ret=0x%x\n", ret); 56 return ret; 57 } 58 59 /* 60 * Add an internal station to the FW table 61 */ 62 static int iwl_mvm_mld_add_int_sta_to_fw(struct iwl_mvm *mvm, 63 struct iwl_mvm_int_sta *sta, 64 const u8 *addr, int link_id) 65 { 66 struct iwl_mvm_sta_cfg_cmd cmd; 67 68 lockdep_assert_held(&mvm->mutex); 69 70 memset(&cmd, 0, sizeof(cmd)); 71 cmd.sta_id = cpu_to_le32((u8)sta->sta_id); 72 73 cmd.link_id = cpu_to_le32(link_id); 74 75 cmd.station_type = cpu_to_le32(sta->type); 76 77 if (fw_has_capa(&mvm->fw->ucode_capa, 78 IWL_UCODE_TLV_CAPA_STA_EXP_MFP_SUPPORT) && 79 sta->type == STATION_TYPE_BCAST_MGMT) 80 cmd.mfp = cpu_to_le32(1); 81 82 if (addr) { 83 memcpy(cmd.peer_mld_address, addr, ETH_ALEN); 84 memcpy(cmd.peer_link_address, addr, ETH_ALEN); 85 } 86 87 return iwl_mvm_mld_send_sta_cmd(mvm, &cmd); 88 } 89 90 /* 91 * Remove a station from the FW table. Before sending the command to remove 92 * the station validate that the station is indeed known to the driver (sanity 93 * only). 94 */ 95 static int iwl_mvm_mld_rm_sta_from_fw(struct iwl_mvm *mvm, u32 sta_id) 96 { 97 struct iwl_mvm_remove_sta_cmd rm_sta_cmd = { 98 .sta_id = cpu_to_le32(sta_id), 99 }; 100 int ret; 101 102 /* Note: internal stations are marked as error values */ 103 if (!rcu_access_pointer(mvm->fw_id_to_mac_id[sta_id])) { 104 IWL_ERR(mvm, "Invalid station id %d\n", sta_id); 105 return -EINVAL; 106 } 107 108 ret = iwl_mvm_send_cmd_pdu(mvm, WIDE_ID(MAC_CONF_GROUP, STA_REMOVE_CMD), 109 0, sizeof(rm_sta_cmd), &rm_sta_cmd); 110 if (ret) { 111 IWL_ERR(mvm, "Failed to remove station. Id=%d\n", sta_id); 112 return ret; 113 } 114 115 return 0; 116 } 117 118 static int iwl_mvm_add_aux_sta_to_fw(struct iwl_mvm *mvm, 119 struct iwl_mvm_int_sta *sta, 120 u32 lmac_id) 121 { 122 int ret; 123 124 struct iwl_mvm_aux_sta_cmd cmd = { 125 .sta_id = cpu_to_le32(sta->sta_id), 126 .lmac_id = cpu_to_le32(lmac_id), 127 }; 128 129 ret = iwl_mvm_send_cmd_pdu(mvm, WIDE_ID(MAC_CONF_GROUP, AUX_STA_CMD), 130 0, sizeof(cmd), &cmd); 131 if (ret) 132 IWL_ERR(mvm, "Failed to send AUX_STA_CMD\n"); 133 return ret; 134 } 135 136 /* 137 * Adds an internal sta to the FW table with its queues 138 */ 139 int iwl_mvm_mld_add_int_sta_with_queue(struct iwl_mvm *mvm, 140 struct iwl_mvm_int_sta *sta, 141 const u8 *addr, int link_id, 142 u16 *queue, u8 tid, 143 unsigned int *_wdg_timeout) 144 { 145 int ret, txq; 146 unsigned int wdg_timeout = _wdg_timeout ? *_wdg_timeout : 147 mvm->trans->trans_cfg->base_params->wd_timeout; 148 149 if (WARN_ON_ONCE(sta->sta_id == IWL_MVM_INVALID_STA)) 150 return -ENOSPC; 151 152 if (sta->type == STATION_TYPE_AUX) 153 ret = iwl_mvm_add_aux_sta_to_fw(mvm, sta, link_id); 154 else 155 ret = iwl_mvm_mld_add_int_sta_to_fw(mvm, sta, addr, link_id); 156 if (ret) 157 return ret; 158 159 /* 160 * For 22000 firmware and on we cannot add queue to a station unknown 161 * to firmware so enable queue here - after the station was added 162 */ 163 txq = iwl_mvm_tvqm_enable_txq(mvm, NULL, sta->sta_id, tid, 164 wdg_timeout); 165 if (txq < 0) { 166 iwl_mvm_mld_rm_sta_from_fw(mvm, sta->sta_id); 167 return txq; 168 } 169 *queue = txq; 170 171 return 0; 172 } 173 174 /* 175 * Adds a new int sta: allocate it in the driver, add it to the FW table, 176 * and add its queues. 177 */ 178 static int iwl_mvm_mld_add_int_sta(struct iwl_mvm *mvm, 179 struct iwl_mvm_int_sta *int_sta, u16 *queue, 180 enum nl80211_iftype iftype, 181 enum iwl_fw_sta_type sta_type, 182 int link_id, const u8 *addr, u8 tid, 183 unsigned int *wdg_timeout) 184 { 185 int ret; 186 187 lockdep_assert_held(&mvm->mutex); 188 189 /* qmask argument is not used in the new tx api, send a don't care */ 190 ret = iwl_mvm_allocate_int_sta(mvm, int_sta, 0, iftype, 191 sta_type); 192 if (ret) 193 return ret; 194 195 ret = iwl_mvm_mld_add_int_sta_with_queue(mvm, int_sta, addr, link_id, 196 queue, tid, wdg_timeout); 197 if (ret) { 198 iwl_mvm_dealloc_int_sta(mvm, int_sta); 199 return ret; 200 } 201 202 return 0; 203 } 204 205 /* Allocate a new station entry for the broadcast station to the given vif, 206 * and send it to the FW. 207 * Note that each P2P mac should have its own broadcast station. 208 */ 209 int iwl_mvm_mld_add_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif, 210 struct ieee80211_bss_conf *link_conf) 211 { 212 struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif); 213 struct iwl_mvm_vif_link_info *mvm_link = 214 mvmvif->link[link_conf->link_id]; 215 struct iwl_mvm_int_sta *bsta = &mvm_link->bcast_sta; 216 static const u8 _baddr[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; 217 const u8 *baddr = _baddr; 218 unsigned int wdg_timeout = 219 iwl_mvm_get_wd_timeout(mvm, vif, false, false); 220 u16 *queue; 221 222 lockdep_assert_held(&mvm->mutex); 223 224 if (vif->type == NL80211_IFTYPE_ADHOC) 225 baddr = link_conf->bssid; 226 227 if (vif->type == NL80211_IFTYPE_AP || 228 vif->type == NL80211_IFTYPE_ADHOC) { 229 queue = &mvm_link->mgmt_queue; 230 } else if (vif->type == NL80211_IFTYPE_P2P_DEVICE) { 231 queue = &mvm->p2p_dev_queue; 232 } else { 233 WARN(1, "Missing required TXQ for adding bcast STA\n"); 234 return -EINVAL; 235 } 236 237 return iwl_mvm_mld_add_int_sta(mvm, bsta, queue, 238 ieee80211_vif_type_p2p(vif), 239 STATION_TYPE_BCAST_MGMT, 240 mvm_link->fw_link_id, baddr, 241 IWL_MAX_TID_COUNT, &wdg_timeout); 242 } 243 244 /* Allocate a new station entry for the multicast station to the given vif, 245 * and send it to the FW. 246 * Note that each AP/GO mac should have its own multicast station. 247 */ 248 int iwl_mvm_mld_add_mcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif, 249 struct ieee80211_bss_conf *link_conf) 250 { 251 struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif); 252 struct iwl_mvm_vif_link_info *mvm_link = 253 mvmvif->link[link_conf->link_id]; 254 struct iwl_mvm_int_sta *msta = &mvm_link->mcast_sta; 255 static const u8 _maddr[] = {0x03, 0x00, 0x00, 0x00, 0x00, 0x00}; 256 const u8 *maddr = _maddr; 257 unsigned int timeout = iwl_mvm_get_wd_timeout(mvm, vif, false, false); 258 259 lockdep_assert_held(&mvm->mutex); 260 261 if (WARN_ON(vif->type != NL80211_IFTYPE_AP && 262 vif->type != NL80211_IFTYPE_ADHOC)) 263 return -EOPNOTSUPP; 264 265 /* In IBSS, ieee80211_check_queues() sets the cab_queue to be 266 * invalid, so make sure we use the queue we want. 267 * Note that this is done here as we want to avoid making DQA 268 * changes in mac80211 layer. 269 */ 270 if (vif->type == NL80211_IFTYPE_ADHOC) 271 mvm_link->cab_queue = IWL_MVM_DQA_GCAST_QUEUE; 272 273 return iwl_mvm_mld_add_int_sta(mvm, msta, &mvm_link->cab_queue, 274 vif->type, STATION_TYPE_MCAST, 275 mvm_link->fw_link_id, maddr, 0, 276 &timeout); 277 } 278 279 /* Allocate a new station entry for the sniffer station to the given vif, 280 * and send it to the FW. 281 */ 282 int iwl_mvm_mld_add_snif_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif, 283 struct ieee80211_bss_conf *link_conf) 284 { 285 struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif); 286 struct iwl_mvm_vif_link_info *mvm_link = 287 mvmvif->link[link_conf->link_id]; 288 289 lockdep_assert_held(&mvm->mutex); 290 291 return iwl_mvm_mld_add_int_sta(mvm, &mvm->snif_sta, &mvm->snif_queue, 292 vif->type, STATION_TYPE_BCAST_MGMT, 293 mvm_link->fw_link_id, NULL, 294 IWL_MAX_TID_COUNT, NULL); 295 } 296 297 int iwl_mvm_mld_add_aux_sta(struct iwl_mvm *mvm, u32 lmac_id) 298 { 299 lockdep_assert_held(&mvm->mutex); 300 301 /* In CDB NICs we need to specify which lmac to use for aux activity; 302 * use the link_id argument place to send lmac_id to the function. 303 */ 304 return iwl_mvm_mld_add_int_sta(mvm, &mvm->aux_sta, &mvm->aux_queue, 305 NL80211_IFTYPE_UNSPECIFIED, 306 STATION_TYPE_AUX, lmac_id, NULL, 307 IWL_MAX_TID_COUNT, NULL); 308 } 309 310 static int iwl_mvm_mld_disable_txq(struct iwl_mvm *mvm, u32 sta_mask, 311 u16 *queueptr, u8 tid) 312 { 313 int queue = *queueptr; 314 int ret = 0; 315 316 if (tid == IWL_MAX_TID_COUNT) 317 tid = IWL_MGMT_TID; 318 319 if (mvm->sta_remove_requires_queue_remove) { 320 u32 cmd_id = WIDE_ID(DATA_PATH_GROUP, 321 SCD_QUEUE_CONFIG_CMD); 322 struct iwl_scd_queue_cfg_cmd remove_cmd = { 323 .operation = cpu_to_le32(IWL_SCD_QUEUE_REMOVE), 324 .u.remove.tid = cpu_to_le32(tid), 325 .u.remove.sta_mask = cpu_to_le32(sta_mask), 326 }; 327 328 ret = iwl_mvm_send_cmd_pdu(mvm, cmd_id, 0, 329 sizeof(remove_cmd), 330 &remove_cmd); 331 } 332 333 iwl_trans_txq_free(mvm->trans, queue); 334 *queueptr = IWL_MVM_INVALID_QUEUE; 335 336 return ret; 337 } 338 339 /* Removes a sta from the FW table, disable its queues, and dealloc it 340 */ 341 static int iwl_mvm_mld_rm_int_sta(struct iwl_mvm *mvm, 342 struct iwl_mvm_int_sta *int_sta, 343 bool flush, u8 tid, u16 *queuptr) 344 { 345 int ret; 346 347 lockdep_assert_held(&mvm->mutex); 348 349 if (WARN_ON_ONCE(int_sta->sta_id == IWL_MVM_INVALID_STA)) 350 return -EINVAL; 351 352 if (flush) 353 iwl_mvm_flush_sta(mvm, int_sta->sta_id, int_sta->tfd_queue_msk); 354 355 iwl_mvm_mld_disable_txq(mvm, BIT(int_sta->sta_id), queuptr, tid); 356 357 ret = iwl_mvm_mld_rm_sta_from_fw(mvm, int_sta->sta_id); 358 if (ret) 359 IWL_WARN(mvm, "Failed sending remove station\n"); 360 361 iwl_mvm_dealloc_int_sta(mvm, int_sta); 362 363 return ret; 364 } 365 366 int iwl_mvm_mld_rm_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif, 367 struct ieee80211_bss_conf *link_conf) 368 { 369 struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif); 370 struct iwl_mvm_vif_link_info *link = mvmvif->link[link_conf->link_id]; 371 u16 *queueptr; 372 373 lockdep_assert_held(&mvm->mutex); 374 375 if (WARN_ON(!link)) 376 return -EIO; 377 378 switch (vif->type) { 379 case NL80211_IFTYPE_AP: 380 case NL80211_IFTYPE_ADHOC: 381 queueptr = &link->mgmt_queue; 382 break; 383 case NL80211_IFTYPE_P2P_DEVICE: 384 queueptr = &mvm->p2p_dev_queue; 385 break; 386 default: 387 WARN(1, "Can't free bcast queue on vif type %d\n", 388 vif->type); 389 return -EINVAL; 390 } 391 392 return iwl_mvm_mld_rm_int_sta(mvm, &link->bcast_sta, 393 true, IWL_MAX_TID_COUNT, queueptr); 394 } 395 396 /* Send the FW a request to remove the station from it's internal data 397 * structures, and in addition remove it from the local data structure. 398 */ 399 int iwl_mvm_mld_rm_mcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif, 400 struct ieee80211_bss_conf *link_conf) 401 { 402 struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif); 403 struct iwl_mvm_vif_link_info *link = mvmvif->link[link_conf->link_id]; 404 405 lockdep_assert_held(&mvm->mutex); 406 407 if (WARN_ON(!link)) 408 return -EIO; 409 410 return iwl_mvm_mld_rm_int_sta(mvm, &link->mcast_sta, true, 0, 411 &link->cab_queue); 412 } 413 414 int iwl_mvm_mld_rm_snif_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif) 415 { 416 lockdep_assert_held(&mvm->mutex); 417 418 return iwl_mvm_mld_rm_int_sta(mvm, &mvm->snif_sta, false, 419 IWL_MAX_TID_COUNT, &mvm->snif_queue); 420 } 421 422 int iwl_mvm_mld_rm_aux_sta(struct iwl_mvm *mvm) 423 { 424 lockdep_assert_held(&mvm->mutex); 425 426 return iwl_mvm_mld_rm_int_sta(mvm, &mvm->aux_sta, false, 427 IWL_MAX_TID_COUNT, &mvm->aux_queue); 428 } 429 430 /* send a cfg sta command to add/update a sta in firmware */ 431 static int iwl_mvm_mld_cfg_sta(struct iwl_mvm *mvm, struct ieee80211_sta *sta, 432 struct ieee80211_vif *vif, 433 struct ieee80211_link_sta *link_sta, 434 struct ieee80211_bss_conf *link_conf, 435 struct iwl_mvm_link_sta *mvm_link_sta) 436 { 437 struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta); 438 struct iwl_mvm_vif *mvm_vif = iwl_mvm_vif_from_mac80211(vif); 439 struct iwl_mvm_vif_link_info *link_info = 440 mvm_vif->link[link_conf->link_id]; 441 struct iwl_mvm_sta_cfg_cmd cmd = { 442 .sta_id = cpu_to_le32(mvm_link_sta->sta_id), 443 .station_type = cpu_to_le32(mvm_sta->sta_type), 444 }; 445 u32 agg_size = 0, mpdu_dens = 0; 446 447 /* when adding sta, link should exist in FW */ 448 if (WARN_ON(link_info->fw_link_id == IWL_MVM_FW_LINK_ID_INVALID)) 449 return -EINVAL; 450 451 cmd.link_id = cpu_to_le32(link_info->fw_link_id); 452 453 memcpy(&cmd.peer_mld_address, sta->addr, ETH_ALEN); 454 memcpy(&cmd.peer_link_address, link_sta->addr, ETH_ALEN); 455 456 if (mvm_sta->sta_state >= IEEE80211_STA_ASSOC) 457 cmd.assoc_id = cpu_to_le32(sta->aid); 458 459 if (fw_has_capa(&mvm->fw->ucode_capa, 460 IWL_UCODE_TLV_CAPA_STA_EXP_MFP_SUPPORT) && 461 (sta->mfp || mvm_sta->sta_state < IEEE80211_STA_AUTHORIZED)) 462 cmd.mfp = cpu_to_le32(1); 463 464 switch (link_sta->rx_nss) { 465 case 1: 466 cmd.mimo = cpu_to_le32(0); 467 break; 468 case 2 ... 8: 469 cmd.mimo = cpu_to_le32(1); 470 break; 471 } 472 473 switch (link_sta->smps_mode) { 474 case IEEE80211_SMPS_AUTOMATIC: 475 case IEEE80211_SMPS_NUM_MODES: 476 WARN_ON(1); 477 break; 478 case IEEE80211_SMPS_STATIC: 479 /* override NSS */ 480 cmd.mimo = cpu_to_le32(0); 481 break; 482 case IEEE80211_SMPS_DYNAMIC: 483 cmd.mimo_protection = cpu_to_le32(1); 484 break; 485 case IEEE80211_SMPS_OFF: 486 /* nothing */ 487 break; 488 } 489 490 mpdu_dens = iwl_mvm_get_sta_ampdu_dens(link_sta, link_conf, &agg_size); 491 cmd.tx_ampdu_spacing = cpu_to_le32(mpdu_dens); 492 cmd.tx_ampdu_max_size = cpu_to_le32(agg_size); 493 494 if (sta->wme) { 495 cmd.sp_length = 496 cpu_to_le32(sta->max_sp ? sta->max_sp * 2 : 128); 497 cmd.uapsd_acs = cpu_to_le32(iwl_mvm_get_sta_uapsd_acs(sta)); 498 } 499 500 if (link_sta->he_cap.has_he) { 501 cmd.trig_rnd_alloc = 502 cpu_to_le32(link_conf->uora_exists ? 1 : 0); 503 504 /* PPE Thresholds */ 505 iwl_mvm_set_sta_pkt_ext(mvm, link_sta, &cmd.pkt_ext); 506 507 /* HTC flags */ 508 cmd.htc_flags = iwl_mvm_get_sta_htc_flags(sta, link_sta); 509 510 if (link_sta->he_cap.he_cap_elem.mac_cap_info[2] & 511 IEEE80211_HE_MAC_CAP2_ACK_EN) 512 cmd.ack_enabled = cpu_to_le32(1); 513 } 514 515 return iwl_mvm_mld_send_sta_cmd(mvm, &cmd); 516 } 517 518 void iwl_mvm_mld_free_sta_link(struct iwl_mvm *mvm, 519 struct iwl_mvm_sta *mvm_sta, 520 struct iwl_mvm_link_sta *mvm_sta_link, 521 unsigned int link_id, 522 bool is_in_fw) 523 { 524 RCU_INIT_POINTER(mvm->fw_id_to_mac_id[mvm_sta_link->sta_id], 525 is_in_fw ? ERR_PTR(-EINVAL) : NULL); 526 RCU_INIT_POINTER(mvm->fw_id_to_link_sta[mvm_sta_link->sta_id], NULL); 527 RCU_INIT_POINTER(mvm_sta->link[link_id], NULL); 528 529 if (mvm_sta_link != &mvm_sta->deflink) 530 kfree_rcu(mvm_sta_link, rcu_head); 531 } 532 533 static void iwl_mvm_mld_sta_rm_all_sta_links(struct iwl_mvm *mvm, 534 struct iwl_mvm_sta *mvm_sta) 535 { 536 unsigned int link_id; 537 538 for (link_id = 0; link_id < ARRAY_SIZE(mvm_sta->link); link_id++) { 539 struct iwl_mvm_link_sta *link = 540 rcu_dereference_protected(mvm_sta->link[link_id], 541 lockdep_is_held(&mvm->mutex)); 542 543 if (!link) 544 continue; 545 546 iwl_mvm_mld_free_sta_link(mvm, mvm_sta, link, link_id, false); 547 } 548 } 549 550 static int iwl_mvm_mld_alloc_sta_link(struct iwl_mvm *mvm, 551 struct ieee80211_vif *vif, 552 struct ieee80211_sta *sta, 553 unsigned int link_id) 554 { 555 struct ieee80211_link_sta *link_sta = 556 link_sta_dereference_protected(sta, link_id); 557 struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta); 558 struct iwl_mvm_link_sta *link; 559 u32 sta_id = iwl_mvm_find_free_sta_id(mvm, 560 ieee80211_vif_type_p2p(vif)); 561 562 if (sta_id == IWL_MVM_INVALID_STA) 563 return -ENOSPC; 564 565 if (rcu_access_pointer(sta->link[link_id]) == &sta->deflink) { 566 link = &mvm_sta->deflink; 567 } else { 568 link = kzalloc(sizeof(*link), GFP_KERNEL); 569 if (!link) 570 return -ENOMEM; 571 } 572 573 link->sta_id = sta_id; 574 rcu_assign_pointer(mvm_sta->link[link_id], link); 575 rcu_assign_pointer(mvm->fw_id_to_mac_id[link->sta_id], sta); 576 rcu_assign_pointer(mvm->fw_id_to_link_sta[link->sta_id], 577 link_sta); 578 579 return 0; 580 } 581 582 /* allocate all the links of a sta, called when the station is first added */ 583 static int iwl_mvm_mld_alloc_sta_links(struct iwl_mvm *mvm, 584 struct ieee80211_vif *vif, 585 struct ieee80211_sta *sta) 586 { 587 struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta); 588 struct ieee80211_link_sta *link_sta; 589 unsigned int link_id; 590 int ret; 591 592 lockdep_assert_held(&mvm->mutex); 593 594 for_each_sta_active_link(vif, sta, link_sta, link_id) { 595 if (WARN_ON(mvm_sta->link[link_id])) 596 continue; 597 598 ret = iwl_mvm_mld_alloc_sta_link(mvm, vif, sta, link_id); 599 if (ret) 600 goto err; 601 } 602 603 return 0; 604 605 err: 606 iwl_mvm_mld_sta_rm_all_sta_links(mvm, mvm_sta); 607 return ret; 608 } 609 610 static void iwl_mvm_mld_set_ap_sta_id(struct ieee80211_sta *sta, 611 struct iwl_mvm_vif_link_info *vif_link, 612 struct iwl_mvm_link_sta *sta_link) 613 { 614 if (!sta->tdls) { 615 WARN_ON(vif_link->ap_sta_id != IWL_MVM_INVALID_STA); 616 vif_link->ap_sta_id = sta_link->sta_id; 617 } else { 618 WARN_ON(vif_link->ap_sta_id == IWL_MVM_INVALID_STA); 619 } 620 } 621 622 static int iwl_mvm_alloc_sta_after_restart(struct iwl_mvm *mvm, 623 struct ieee80211_vif *vif, 624 struct ieee80211_sta *sta) 625 { 626 struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta); 627 struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif); 628 struct ieee80211_link_sta *link_sta; 629 unsigned int link_id; 630 /* no active link found */ 631 int ret = -EINVAL; 632 int sta_id; 633 634 /* First add an empty station since allocating a queue requires 635 * a valid station. Since we need a link_id to allocate a station, 636 * pick up the first valid one. 637 */ 638 for_each_sta_active_link(vif, sta, link_sta, link_id) { 639 struct iwl_mvm_vif_link_info *mvm_link; 640 struct ieee80211_bss_conf *link_conf = 641 link_conf_dereference_protected(vif, link_id); 642 struct iwl_mvm_link_sta *mvm_link_sta = 643 rcu_dereference_protected(mvm_sta->link[link_id], 644 lockdep_is_held(&mvm->mutex)); 645 646 if (!link_conf) 647 continue; 648 649 mvm_link = mvmvif->link[link_conf->link_id]; 650 651 if (!mvm_link || !mvm_link_sta) 652 continue; 653 654 sta_id = mvm_link_sta->sta_id; 655 ret = iwl_mvm_mld_cfg_sta(mvm, sta, vif, link_sta, 656 link_conf, mvm_link_sta); 657 if (ret) 658 return ret; 659 660 rcu_assign_pointer(mvm->fw_id_to_mac_id[sta_id], sta); 661 rcu_assign_pointer(mvm->fw_id_to_link_sta[sta_id], link_sta); 662 ret = 0; 663 } 664 665 iwl_mvm_realloc_queues_after_restart(mvm, sta); 666 667 return ret; 668 } 669 670 int iwl_mvm_mld_add_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif, 671 struct ieee80211_sta *sta) 672 { 673 struct iwl_mvm_vif *mvm_vif = iwl_mvm_vif_from_mac80211(vif); 674 struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta); 675 unsigned long link_sta_added_to_fw = 0; 676 struct ieee80211_link_sta *link_sta; 677 int ret = 0; 678 unsigned int link_id; 679 680 lockdep_assert_held(&mvm->mutex); 681 682 if (!test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status)) { 683 ret = iwl_mvm_mld_alloc_sta_links(mvm, vif, sta); 684 if (ret) 685 return ret; 686 687 spin_lock_init(&mvm_sta->lock); 688 689 ret = iwl_mvm_sta_init(mvm, vif, sta, IWL_MVM_INVALID_STA, 690 STATION_TYPE_PEER); 691 } else { 692 ret = iwl_mvm_alloc_sta_after_restart(mvm, vif, sta); 693 } 694 695 if (ret) 696 goto err; 697 698 /* at this stage sta link pointers are already allocated */ 699 ret = iwl_mvm_mld_update_sta(mvm, vif, sta); 700 if (ret) 701 goto err; 702 703 for_each_sta_active_link(vif, sta, link_sta, link_id) { 704 struct ieee80211_bss_conf *link_conf = 705 link_conf_dereference_protected(vif, link_id); 706 struct iwl_mvm_link_sta *mvm_link_sta = 707 rcu_dereference_protected(mvm_sta->link[link_id], 708 lockdep_is_held(&mvm->mutex)); 709 710 if (WARN_ON(!link_conf || !mvm_link_sta)) { 711 ret = -EINVAL; 712 goto err; 713 } 714 715 ret = iwl_mvm_mld_cfg_sta(mvm, sta, vif, link_sta, link_conf, 716 mvm_link_sta); 717 if (ret) 718 goto err; 719 720 link_sta_added_to_fw |= BIT(link_id); 721 722 if (vif->type == NL80211_IFTYPE_STATION) 723 iwl_mvm_mld_set_ap_sta_id(sta, mvm_vif->link[link_id], 724 mvm_link_sta); 725 } 726 return 0; 727 728 err: 729 /* remove all already allocated stations in FW */ 730 for_each_set_bit(link_id, &link_sta_added_to_fw, 731 IEEE80211_MLD_MAX_NUM_LINKS) { 732 struct iwl_mvm_link_sta *mvm_link_sta = 733 rcu_dereference_protected(mvm_sta->link[link_id], 734 lockdep_is_held(&mvm->mutex)); 735 736 iwl_mvm_mld_rm_sta_from_fw(mvm, mvm_link_sta->sta_id); 737 } 738 739 /* free all sta resources in the driver */ 740 iwl_mvm_mld_sta_rm_all_sta_links(mvm, mvm_sta); 741 return ret; 742 } 743 744 int iwl_mvm_mld_update_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif, 745 struct ieee80211_sta *sta) 746 { 747 struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta); 748 struct ieee80211_link_sta *link_sta; 749 unsigned int link_id; 750 int ret = -EINVAL; 751 752 lockdep_assert_held(&mvm->mutex); 753 754 for_each_sta_active_link(vif, sta, link_sta, link_id) { 755 struct ieee80211_bss_conf *link_conf = 756 link_conf_dereference_protected(vif, link_id); 757 struct iwl_mvm_link_sta *mvm_link_sta = 758 rcu_dereference_protected(mvm_sta->link[link_id], 759 lockdep_is_held(&mvm->mutex)); 760 761 if (WARN_ON(!link_conf || !mvm_link_sta)) 762 return -EINVAL; 763 764 ret = iwl_mvm_mld_cfg_sta(mvm, sta, vif, link_sta, link_conf, 765 mvm_link_sta); 766 767 if (ret) { 768 IWL_ERR(mvm, "Failed to update sta link %d\n", link_id); 769 break; 770 } 771 } 772 773 return ret; 774 } 775 776 static void iwl_mvm_mld_disable_sta_queues(struct iwl_mvm *mvm, 777 struct ieee80211_vif *vif, 778 struct ieee80211_sta *sta) 779 { 780 struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta); 781 u32 sta_mask = iwl_mvm_sta_fw_id_mask(mvm, sta, -1); 782 int i; 783 784 lockdep_assert_held(&mvm->mutex); 785 786 for (i = 0; i < ARRAY_SIZE(mvm_sta->tid_data); i++) { 787 if (mvm_sta->tid_data[i].txq_id == IWL_MVM_INVALID_QUEUE) 788 continue; 789 790 iwl_mvm_mld_disable_txq(mvm, sta_mask, 791 &mvm_sta->tid_data[i].txq_id, i); 792 mvm_sta->tid_data[i].txq_id = IWL_MVM_INVALID_QUEUE; 793 } 794 795 for (i = 0; i < ARRAY_SIZE(sta->txq); i++) { 796 struct iwl_mvm_txq *mvmtxq = 797 iwl_mvm_txq_from_mac80211(sta->txq[i]); 798 799 mvmtxq->txq_id = IWL_MVM_INVALID_QUEUE; 800 } 801 } 802 803 int iwl_mvm_mld_rm_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif, 804 struct ieee80211_sta *sta) 805 { 806 struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta); 807 struct ieee80211_link_sta *link_sta; 808 unsigned int link_id; 809 int ret; 810 811 lockdep_assert_held(&mvm->mutex); 812 813 /* flush its queues here since we are freeing mvm_sta */ 814 for_each_sta_active_link(vif, sta, link_sta, link_id) { 815 struct iwl_mvm_link_sta *mvm_link_sta = 816 rcu_dereference_protected(mvm_sta->link[link_id], 817 lockdep_is_held(&mvm->mutex)); 818 819 if (WARN_ON(!mvm_link_sta)) 820 return -EINVAL; 821 822 ret = iwl_mvm_flush_sta_tids(mvm, mvm_link_sta->sta_id, 823 0xffff); 824 if (ret) 825 return ret; 826 } 827 828 ret = iwl_mvm_wait_sta_queues_empty(mvm, mvm_sta); 829 if (ret) 830 return ret; 831 832 iwl_mvm_mld_disable_sta_queues(mvm, vif, sta); 833 834 for_each_sta_active_link(vif, sta, link_sta, link_id) { 835 struct iwl_mvm_link_sta *mvm_link_sta = 836 rcu_dereference_protected(mvm_sta->link[link_id], 837 lockdep_is_held(&mvm->mutex)); 838 bool stay_in_fw; 839 840 stay_in_fw = iwl_mvm_sta_del(mvm, vif, sta, link_sta, &ret); 841 if (ret) 842 break; 843 844 if (!stay_in_fw) 845 ret = iwl_mvm_mld_rm_sta_from_fw(mvm, 846 mvm_link_sta->sta_id); 847 848 iwl_mvm_mld_free_sta_link(mvm, mvm_sta, mvm_link_sta, 849 link_id, stay_in_fw); 850 } 851 kfree(mvm_sta->mpdu_counters); 852 mvm_sta->mpdu_counters = NULL; 853 854 return ret; 855 } 856 857 int iwl_mvm_mld_rm_sta_id(struct iwl_mvm *mvm, u8 sta_id) 858 { 859 int ret; 860 861 lockdep_assert_held(&mvm->mutex); 862 863 if (WARN_ON(sta_id == IWL_MVM_INVALID_STA)) 864 return 0; 865 866 ret = iwl_mvm_mld_rm_sta_from_fw(mvm, sta_id); 867 868 RCU_INIT_POINTER(mvm->fw_id_to_mac_id[sta_id], NULL); 869 RCU_INIT_POINTER(mvm->fw_id_to_link_sta[sta_id], NULL); 870 return ret; 871 } 872 873 void iwl_mvm_mld_sta_modify_disable_tx(struct iwl_mvm *mvm, 874 struct iwl_mvm_sta *mvmsta, 875 bool disable) 876 { 877 struct iwl_mvm_sta_disable_tx_cmd cmd; 878 int ret; 879 880 cmd.sta_id = cpu_to_le32(mvmsta->deflink.sta_id); 881 cmd.disable = cpu_to_le32(disable); 882 883 if (WARN_ON(iwl_mvm_has_no_host_disable_tx(mvm))) 884 return; 885 886 ret = iwl_mvm_send_cmd_pdu(mvm, 887 WIDE_ID(MAC_CONF_GROUP, STA_DISABLE_TX_CMD), 888 CMD_ASYNC, sizeof(cmd), &cmd); 889 if (ret) 890 IWL_ERR(mvm, 891 "Failed to send STA_DISABLE_TX_CMD command (%d)\n", 892 ret); 893 } 894 895 void iwl_mvm_mld_sta_modify_disable_tx_ap(struct iwl_mvm *mvm, 896 struct ieee80211_sta *sta, 897 bool disable) 898 { 899 struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta); 900 901 spin_lock_bh(&mvm_sta->lock); 902 903 if (mvm_sta->disable_tx == disable) { 904 spin_unlock_bh(&mvm_sta->lock); 905 return; 906 } 907 908 iwl_mvm_mld_sta_modify_disable_tx(mvm, mvm_sta, disable); 909 910 spin_unlock_bh(&mvm_sta->lock); 911 } 912 913 void iwl_mvm_mld_modify_all_sta_disable_tx(struct iwl_mvm *mvm, 914 struct iwl_mvm_vif *mvmvif, 915 bool disable) 916 { 917 struct ieee80211_sta *sta; 918 struct iwl_mvm_sta *mvm_sta; 919 int i; 920 921 rcu_read_lock(); 922 923 /* Block/unblock all the stations of the given mvmvif */ 924 for (i = 0; i < mvm->fw->ucode_capa.num_stations; i++) { 925 sta = rcu_dereference(mvm->fw_id_to_mac_id[i]); 926 if (IS_ERR_OR_NULL(sta)) 927 continue; 928 929 mvm_sta = iwl_mvm_sta_from_mac80211(sta); 930 if (mvm_sta->mac_id_n_color != 931 FW_CMD_ID_AND_COLOR(mvmvif->id, mvmvif->color)) 932 continue; 933 934 iwl_mvm_mld_sta_modify_disable_tx(mvm, mvm_sta, disable); 935 } 936 937 rcu_read_unlock(); 938 } 939 940 static int iwl_mvm_mld_update_sta_queues(struct iwl_mvm *mvm, 941 struct ieee80211_sta *sta, 942 u32 old_sta_mask, 943 u32 new_sta_mask) 944 { 945 struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta); 946 struct iwl_scd_queue_cfg_cmd cmd = { 947 .operation = cpu_to_le32(IWL_SCD_QUEUE_MODIFY), 948 .u.modify.old_sta_mask = cpu_to_le32(old_sta_mask), 949 .u.modify.new_sta_mask = cpu_to_le32(new_sta_mask), 950 }; 951 struct iwl_host_cmd hcmd = { 952 .id = WIDE_ID(DATA_PATH_GROUP, SCD_QUEUE_CONFIG_CMD), 953 .len[0] = sizeof(cmd), 954 .data[0] = &cmd 955 }; 956 int tid; 957 int ret; 958 959 lockdep_assert_held(&mvm->mutex); 960 961 for (tid = 0; tid <= IWL_MAX_TID_COUNT; tid++) { 962 struct iwl_mvm_tid_data *tid_data = &mvm_sta->tid_data[tid]; 963 int txq_id = tid_data->txq_id; 964 965 if (txq_id == IWL_MVM_INVALID_QUEUE) 966 continue; 967 968 if (tid == IWL_MAX_TID_COUNT) 969 cmd.u.modify.tid = cpu_to_le32(IWL_MGMT_TID); 970 else 971 cmd.u.modify.tid = cpu_to_le32(tid); 972 973 ret = iwl_mvm_send_cmd(mvm, &hcmd); 974 if (ret) 975 return ret; 976 } 977 978 return 0; 979 } 980 981 static int iwl_mvm_mld_update_sta_baids(struct iwl_mvm *mvm, 982 u32 old_sta_mask, 983 u32 new_sta_mask) 984 { 985 struct iwl_rx_baid_cfg_cmd cmd = { 986 .action = cpu_to_le32(IWL_RX_BAID_ACTION_MODIFY), 987 .modify.old_sta_id_mask = cpu_to_le32(old_sta_mask), 988 .modify.new_sta_id_mask = cpu_to_le32(new_sta_mask), 989 }; 990 u32 cmd_id = WIDE_ID(DATA_PATH_GROUP, RX_BAID_ALLOCATION_CONFIG_CMD); 991 int baid; 992 993 /* mac80211 will remove sessions later, but we ignore all that */ 994 if (test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status)) 995 return 0; 996 997 BUILD_BUG_ON(sizeof(struct iwl_rx_baid_cfg_resp) != sizeof(baid)); 998 999 for (baid = 0; baid < ARRAY_SIZE(mvm->baid_map); baid++) { 1000 struct iwl_mvm_baid_data *data; 1001 int ret; 1002 1003 data = rcu_dereference_protected(mvm->baid_map[baid], 1004 lockdep_is_held(&mvm->mutex)); 1005 if (!data) 1006 continue; 1007 1008 if (!(data->sta_mask & old_sta_mask)) 1009 continue; 1010 1011 WARN_ONCE(data->sta_mask != old_sta_mask, 1012 "BAID data for %d corrupted - expected 0x%x found 0x%x\n", 1013 baid, old_sta_mask, data->sta_mask); 1014 1015 cmd.modify.tid = cpu_to_le32(data->tid); 1016 1017 ret = iwl_mvm_send_cmd_pdu(mvm, cmd_id, CMD_SEND_IN_RFKILL, 1018 sizeof(cmd), &cmd); 1019 data->sta_mask = new_sta_mask; 1020 if (ret) 1021 return ret; 1022 } 1023 1024 return 0; 1025 } 1026 1027 static int iwl_mvm_mld_update_sta_resources(struct iwl_mvm *mvm, 1028 struct ieee80211_vif *vif, 1029 struct ieee80211_sta *sta, 1030 u32 old_sta_mask, 1031 u32 new_sta_mask) 1032 { 1033 int ret; 1034 1035 ret = iwl_mvm_mld_update_sta_queues(mvm, sta, 1036 old_sta_mask, 1037 new_sta_mask); 1038 if (ret) 1039 return ret; 1040 1041 ret = iwl_mvm_mld_update_sta_keys(mvm, vif, sta, 1042 old_sta_mask, 1043 new_sta_mask); 1044 if (ret) 1045 return ret; 1046 1047 return iwl_mvm_mld_update_sta_baids(mvm, old_sta_mask, new_sta_mask); 1048 } 1049 1050 int iwl_mvm_mld_update_sta_links(struct iwl_mvm *mvm, 1051 struct ieee80211_vif *vif, 1052 struct ieee80211_sta *sta, 1053 u16 old_links, u16 new_links) 1054 { 1055 struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta); 1056 struct iwl_mvm_vif *mvm_vif = iwl_mvm_vif_from_mac80211(vif); 1057 struct iwl_mvm_link_sta *mvm_sta_link; 1058 struct iwl_mvm_vif_link_info *mvm_vif_link; 1059 unsigned long links_to_add = ~old_links & new_links; 1060 unsigned long links_to_rem = old_links & ~new_links; 1061 unsigned long old_links_long = old_links; 1062 u32 current_sta_mask = 0, sta_mask_added = 0, sta_mask_to_rem = 0; 1063 unsigned long link_sta_added_to_fw = 0, link_sta_allocated = 0; 1064 unsigned int link_id; 1065 int ret; 1066 1067 lockdep_assert_held(&mvm->mutex); 1068 1069 for_each_set_bit(link_id, &old_links_long, 1070 IEEE80211_MLD_MAX_NUM_LINKS) { 1071 mvm_sta_link = 1072 rcu_dereference_protected(mvm_sta->link[link_id], 1073 lockdep_is_held(&mvm->mutex)); 1074 1075 if (WARN_ON(!mvm_sta_link)) { 1076 ret = -EINVAL; 1077 goto err; 1078 } 1079 1080 current_sta_mask |= BIT(mvm_sta_link->sta_id); 1081 if (links_to_rem & BIT(link_id)) 1082 sta_mask_to_rem |= BIT(mvm_sta_link->sta_id); 1083 } 1084 1085 if (sta_mask_to_rem) { 1086 ret = iwl_mvm_mld_update_sta_resources(mvm, vif, sta, 1087 current_sta_mask, 1088 current_sta_mask & 1089 ~sta_mask_to_rem); 1090 if (WARN_ON(ret)) 1091 goto err; 1092 1093 current_sta_mask &= ~sta_mask_to_rem; 1094 } 1095 1096 for_each_set_bit(link_id, &links_to_rem, IEEE80211_MLD_MAX_NUM_LINKS) { 1097 mvm_sta_link = 1098 rcu_dereference_protected(mvm_sta->link[link_id], 1099 lockdep_is_held(&mvm->mutex)); 1100 mvm_vif_link = mvm_vif->link[link_id]; 1101 1102 if (WARN_ON(!mvm_sta_link || !mvm_vif_link)) { 1103 ret = -EINVAL; 1104 goto err; 1105 } 1106 1107 ret = iwl_mvm_mld_rm_sta_from_fw(mvm, mvm_sta_link->sta_id); 1108 if (WARN_ON(ret)) 1109 goto err; 1110 1111 if (vif->type == NL80211_IFTYPE_STATION) 1112 mvm_vif_link->ap_sta_id = IWL_MVM_INVALID_STA; 1113 1114 iwl_mvm_mld_free_sta_link(mvm, mvm_sta, mvm_sta_link, link_id, 1115 false); 1116 } 1117 1118 for_each_set_bit(link_id, &links_to_add, IEEE80211_MLD_MAX_NUM_LINKS) { 1119 struct ieee80211_bss_conf *link_conf = 1120 link_conf_dereference_protected(vif, link_id); 1121 struct ieee80211_link_sta *link_sta = 1122 link_sta_dereference_protected(sta, link_id); 1123 mvm_vif_link = mvm_vif->link[link_id]; 1124 1125 if (WARN_ON(!mvm_vif_link || !link_conf || !link_sta)) { 1126 ret = -EINVAL; 1127 goto err; 1128 } 1129 1130 if (test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status)) { 1131 struct iwl_mvm_link_sta *mvm_link_sta = 1132 rcu_dereference_protected(mvm_sta->link[link_id], 1133 lockdep_is_held(&mvm->mutex)); 1134 u32 sta_id; 1135 1136 if (WARN_ON(!mvm_link_sta)) { 1137 ret = -EINVAL; 1138 goto err; 1139 } 1140 1141 sta_id = mvm_link_sta->sta_id; 1142 1143 rcu_assign_pointer(mvm->fw_id_to_mac_id[sta_id], sta); 1144 rcu_assign_pointer(mvm->fw_id_to_link_sta[sta_id], 1145 link_sta); 1146 } else { 1147 if (WARN_ON(mvm_sta->link[link_id])) { 1148 ret = -EINVAL; 1149 goto err; 1150 } 1151 ret = iwl_mvm_mld_alloc_sta_link(mvm, vif, sta, 1152 link_id); 1153 if (WARN_ON(ret)) 1154 goto err; 1155 } 1156 1157 link_sta->agg.max_rc_amsdu_len = 1; 1158 ieee80211_sta_recalc_aggregates(sta); 1159 1160 mvm_sta_link = 1161 rcu_dereference_protected(mvm_sta->link[link_id], 1162 lockdep_is_held(&mvm->mutex)); 1163 1164 if (WARN_ON(!mvm_sta_link)) { 1165 ret = -EINVAL; 1166 goto err; 1167 } 1168 1169 if (vif->type == NL80211_IFTYPE_STATION) 1170 iwl_mvm_mld_set_ap_sta_id(sta, mvm_vif_link, 1171 mvm_sta_link); 1172 1173 link_sta_allocated |= BIT(link_id); 1174 1175 sta_mask_added |= BIT(mvm_sta_link->sta_id); 1176 1177 ret = iwl_mvm_mld_cfg_sta(mvm, sta, vif, link_sta, link_conf, 1178 mvm_sta_link); 1179 if (WARN_ON(ret)) 1180 goto err; 1181 1182 link_sta_added_to_fw |= BIT(link_id); 1183 1184 iwl_mvm_rs_add_sta_link(mvm, mvm_sta_link); 1185 } 1186 1187 if (sta_mask_added) { 1188 ret = iwl_mvm_mld_update_sta_resources(mvm, vif, sta, 1189 current_sta_mask, 1190 current_sta_mask | 1191 sta_mask_added); 1192 if (WARN_ON(ret)) 1193 goto err; 1194 } 1195 1196 return 0; 1197 1198 err: 1199 /* remove all already allocated stations in FW */ 1200 for_each_set_bit(link_id, &link_sta_added_to_fw, 1201 IEEE80211_MLD_MAX_NUM_LINKS) { 1202 mvm_sta_link = 1203 rcu_dereference_protected(mvm_sta->link[link_id], 1204 lockdep_is_held(&mvm->mutex)); 1205 1206 iwl_mvm_mld_rm_sta_from_fw(mvm, mvm_sta_link->sta_id); 1207 } 1208 1209 /* remove all already allocated station links in driver */ 1210 for_each_set_bit(link_id, &link_sta_allocated, 1211 IEEE80211_MLD_MAX_NUM_LINKS) { 1212 mvm_sta_link = 1213 rcu_dereference_protected(mvm_sta->link[link_id], 1214 lockdep_is_held(&mvm->mutex)); 1215 1216 iwl_mvm_mld_free_sta_link(mvm, mvm_sta, mvm_sta_link, link_id, 1217 false); 1218 } 1219 1220 return ret; 1221 } 1222