1 // SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause 2 /* 3 * Copyright (C) 2022-2023 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 broadcast 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 (sta->deflink.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 static 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 /* FIXME: consider waiting for mac80211 to add the STA instead of allocating 623 * queues here 624 */ 625 static int iwl_mvm_alloc_sta_after_restart(struct iwl_mvm *mvm, 626 struct ieee80211_vif *vif, 627 struct ieee80211_sta *sta) 628 { 629 struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta); 630 struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif); 631 struct ieee80211_link_sta *link_sta; 632 unsigned int link_id; 633 /* no active link found */ 634 int ret = -EINVAL; 635 int sta_id; 636 637 /* First add an empty station since allocating a queue requires 638 * a valid station. Since we need a link_id to allocate a station, 639 * pick up the first valid one. 640 */ 641 for_each_sta_active_link(vif, sta, link_sta, link_id) { 642 struct iwl_mvm_vif_link_info *mvm_link; 643 struct ieee80211_bss_conf *link_conf = 644 link_conf_dereference_protected(vif, link_id); 645 struct iwl_mvm_link_sta *mvm_link_sta = 646 rcu_dereference_protected(mvm_sta->link[link_id], 647 lockdep_is_held(&mvm->mutex)); 648 649 if (!link_conf) 650 continue; 651 652 mvm_link = mvmvif->link[link_conf->link_id]; 653 654 if (!mvm_link || !mvm_link_sta) 655 continue; 656 657 sta_id = mvm_link_sta->sta_id; 658 ret = iwl_mvm_mld_cfg_sta(mvm, sta, vif, link_sta, 659 link_conf, mvm_link_sta); 660 if (ret) 661 return ret; 662 663 rcu_assign_pointer(mvm->fw_id_to_mac_id[sta_id], sta); 664 rcu_assign_pointer(mvm->fw_id_to_link_sta[sta_id], link_sta); 665 ret = 0; 666 } 667 668 iwl_mvm_realloc_queues_after_restart(mvm, sta); 669 670 return ret; 671 } 672 673 int iwl_mvm_mld_add_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif, 674 struct ieee80211_sta *sta) 675 { 676 struct iwl_mvm_vif *mvm_vif = iwl_mvm_vif_from_mac80211(vif); 677 struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta); 678 unsigned long link_sta_added_to_fw = 0; 679 struct ieee80211_link_sta *link_sta; 680 int ret = 0; 681 unsigned int link_id; 682 683 lockdep_assert_held(&mvm->mutex); 684 685 if (!test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status)) { 686 ret = iwl_mvm_mld_alloc_sta_links(mvm, vif, sta); 687 if (ret) 688 return ret; 689 690 spin_lock_init(&mvm_sta->lock); 691 692 ret = iwl_mvm_sta_init(mvm, vif, sta, IWL_MVM_INVALID_STA, 693 STATION_TYPE_PEER); 694 } else { 695 ret = iwl_mvm_alloc_sta_after_restart(mvm, vif, sta); 696 } 697 698 if (ret) 699 goto err; 700 701 /* at this stage sta link pointers are already allocated */ 702 ret = iwl_mvm_mld_update_sta(mvm, vif, sta); 703 704 for_each_sta_active_link(vif, sta, link_sta, link_id) { 705 struct ieee80211_bss_conf *link_conf = 706 link_conf_dereference_protected(vif, link_id); 707 struct iwl_mvm_link_sta *mvm_link_sta = 708 rcu_dereference_protected(mvm_sta->link[link_id], 709 lockdep_is_held(&mvm->mutex)); 710 711 if (WARN_ON(!link_conf || !mvm_link_sta)) { 712 ret = -EINVAL; 713 goto err; 714 } 715 716 ret = iwl_mvm_mld_cfg_sta(mvm, sta, vif, link_sta, link_conf, 717 mvm_link_sta); 718 if (ret) 719 goto err; 720 721 link_sta_added_to_fw |= BIT(link_id); 722 723 if (vif->type == NL80211_IFTYPE_STATION) 724 iwl_mvm_mld_set_ap_sta_id(sta, mvm_vif->link[link_id], 725 mvm_link_sta); 726 } 727 728 return 0; 729 730 err: 731 /* remove all already allocated stations in FW */ 732 for_each_set_bit(link_id, &link_sta_added_to_fw, 733 IEEE80211_MLD_MAX_NUM_LINKS) { 734 struct iwl_mvm_link_sta *mvm_link_sta = 735 rcu_dereference_protected(mvm_sta->link[link_id], 736 lockdep_is_held(&mvm->mutex)); 737 738 iwl_mvm_mld_rm_sta_from_fw(mvm, mvm_link_sta->sta_id); 739 } 740 741 /* free all sta resources in the driver */ 742 iwl_mvm_mld_sta_rm_all_sta_links(mvm, mvm_sta); 743 return ret; 744 } 745 746 int iwl_mvm_mld_update_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif, 747 struct ieee80211_sta *sta) 748 { 749 struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta); 750 struct ieee80211_link_sta *link_sta; 751 unsigned int link_id; 752 int ret = -EINVAL; 753 754 lockdep_assert_held(&mvm->mutex); 755 756 for_each_sta_active_link(vif, sta, link_sta, link_id) { 757 struct ieee80211_bss_conf *link_conf = 758 link_conf_dereference_protected(vif, link_id); 759 struct iwl_mvm_link_sta *mvm_link_sta = 760 rcu_dereference_protected(mvm_sta->link[link_id], 761 lockdep_is_held(&mvm->mutex)); 762 763 if (WARN_ON(!link_conf || !mvm_link_sta)) 764 return -EINVAL; 765 766 ret = iwl_mvm_mld_cfg_sta(mvm, sta, vif, link_sta, link_conf, 767 mvm_link_sta); 768 769 if (ret) { 770 IWL_ERR(mvm, "Failed to update sta link %d\n", link_id); 771 break; 772 } 773 } 774 775 return ret; 776 } 777 778 static void iwl_mvm_mld_disable_sta_queues(struct iwl_mvm *mvm, 779 struct ieee80211_vif *vif, 780 struct ieee80211_sta *sta) 781 { 782 struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta); 783 u32 sta_mask = iwl_mvm_sta_fw_id_mask(mvm, sta, -1); 784 int i; 785 786 lockdep_assert_held(&mvm->mutex); 787 788 for (i = 0; i < ARRAY_SIZE(mvm_sta->tid_data); i++) { 789 if (mvm_sta->tid_data[i].txq_id == IWL_MVM_INVALID_QUEUE) 790 continue; 791 792 iwl_mvm_mld_disable_txq(mvm, sta_mask, 793 &mvm_sta->tid_data[i].txq_id, i); 794 mvm_sta->tid_data[i].txq_id = IWL_MVM_INVALID_QUEUE; 795 } 796 797 for (i = 0; i < ARRAY_SIZE(sta->txq); i++) { 798 struct iwl_mvm_txq *mvmtxq = 799 iwl_mvm_txq_from_mac80211(sta->txq[i]); 800 801 mvmtxq->txq_id = IWL_MVM_INVALID_QUEUE; 802 } 803 } 804 805 int iwl_mvm_mld_rm_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif, 806 struct ieee80211_sta *sta) 807 { 808 struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta); 809 struct ieee80211_link_sta *link_sta; 810 unsigned int link_id; 811 int ret; 812 813 lockdep_assert_held(&mvm->mutex); 814 815 /* flush its queues here since we are freeing mvm_sta */ 816 for_each_sta_active_link(vif, sta, link_sta, link_id) { 817 struct iwl_mvm_link_sta *mvm_link_sta = 818 rcu_dereference_protected(mvm_sta->link[link_id], 819 lockdep_is_held(&mvm->mutex)); 820 821 if (WARN_ON(!mvm_link_sta)) 822 return -EINVAL; 823 824 ret = iwl_mvm_flush_sta_tids(mvm, mvm_link_sta->sta_id, 825 0xffff); 826 if (ret) 827 return ret; 828 } 829 830 ret = iwl_mvm_wait_sta_queues_empty(mvm, mvm_sta); 831 if (ret) 832 return ret; 833 834 iwl_mvm_mld_disable_sta_queues(mvm, vif, sta); 835 836 for_each_sta_active_link(vif, sta, link_sta, link_id) { 837 struct iwl_mvm_link_sta *mvm_link_sta = 838 rcu_dereference_protected(mvm_sta->link[link_id], 839 lockdep_is_held(&mvm->mutex)); 840 bool stay_in_fw; 841 842 stay_in_fw = iwl_mvm_sta_del(mvm, vif, sta, link_sta, &ret); 843 if (ret) 844 break; 845 846 if (!stay_in_fw) 847 ret = iwl_mvm_mld_rm_sta_from_fw(mvm, 848 mvm_link_sta->sta_id); 849 850 iwl_mvm_mld_free_sta_link(mvm, mvm_sta, mvm_link_sta, 851 link_id, stay_in_fw); 852 } 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 ret = iwl_mvm_send_cmd_pdu(mvm, 884 WIDE_ID(MAC_CONF_GROUP, STA_DISABLE_TX_CMD), 885 CMD_ASYNC, sizeof(cmd), &cmd); 886 if (ret) 887 IWL_ERR(mvm, 888 "Failed to send STA_DISABLE_TX_CMD command (%d)\n", 889 ret); 890 } 891 892 void iwl_mvm_mld_sta_modify_disable_tx_ap(struct iwl_mvm *mvm, 893 struct ieee80211_sta *sta, 894 bool disable) 895 { 896 struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta); 897 898 spin_lock_bh(&mvm_sta->lock); 899 900 if (mvm_sta->disable_tx == disable) { 901 spin_unlock_bh(&mvm_sta->lock); 902 return; 903 } 904 905 iwl_mvm_mld_sta_modify_disable_tx(mvm, mvm_sta, disable); 906 907 spin_unlock_bh(&mvm_sta->lock); 908 } 909 910 void iwl_mvm_mld_modify_all_sta_disable_tx(struct iwl_mvm *mvm, 911 struct iwl_mvm_vif *mvmvif, 912 bool disable) 913 { 914 struct ieee80211_sta *sta; 915 struct iwl_mvm_sta *mvm_sta; 916 int i; 917 918 rcu_read_lock(); 919 920 /* Block/unblock all the stations of the given mvmvif */ 921 for (i = 0; i < mvm->fw->ucode_capa.num_stations; i++) { 922 sta = rcu_dereference(mvm->fw_id_to_mac_id[i]); 923 if (IS_ERR_OR_NULL(sta)) 924 continue; 925 926 mvm_sta = iwl_mvm_sta_from_mac80211(sta); 927 if (mvm_sta->mac_id_n_color != 928 FW_CMD_ID_AND_COLOR(mvmvif->id, mvmvif->color)) 929 continue; 930 931 iwl_mvm_mld_sta_modify_disable_tx(mvm, mvm_sta, disable); 932 } 933 934 rcu_read_unlock(); 935 } 936 937 static int iwl_mvm_mld_update_sta_queues(struct iwl_mvm *mvm, 938 struct ieee80211_sta *sta, 939 u32 old_sta_mask, 940 u32 new_sta_mask) 941 { 942 struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta); 943 struct iwl_scd_queue_cfg_cmd cmd = { 944 .operation = cpu_to_le32(IWL_SCD_QUEUE_MODIFY), 945 .u.modify.old_sta_mask = cpu_to_le32(old_sta_mask), 946 .u.modify.new_sta_mask = cpu_to_le32(new_sta_mask), 947 }; 948 struct iwl_host_cmd hcmd = { 949 .id = WIDE_ID(DATA_PATH_GROUP, SCD_QUEUE_CONFIG_CMD), 950 .len[0] = sizeof(cmd), 951 .data[0] = &cmd 952 }; 953 int tid; 954 int ret; 955 956 lockdep_assert_held(&mvm->mutex); 957 958 for (tid = 0; tid <= IWL_MAX_TID_COUNT; tid++) { 959 struct iwl_mvm_tid_data *tid_data = &mvm_sta->tid_data[tid]; 960 int txq_id = tid_data->txq_id; 961 962 if (txq_id == IWL_MVM_INVALID_QUEUE) 963 continue; 964 965 if (tid == IWL_MAX_TID_COUNT) 966 cmd.u.modify.tid = cpu_to_le32(IWL_MGMT_TID); 967 else 968 cmd.u.modify.tid = cpu_to_le32(tid); 969 970 ret = iwl_mvm_send_cmd(mvm, &hcmd); 971 if (ret) 972 return ret; 973 } 974 975 return 0; 976 } 977 978 static int iwl_mvm_mld_update_sta_baids(struct iwl_mvm *mvm, 979 u32 old_sta_mask, 980 u32 new_sta_mask) 981 { 982 struct iwl_rx_baid_cfg_cmd cmd = { 983 .action = cpu_to_le32(IWL_RX_BAID_ACTION_MODIFY), 984 .modify.old_sta_id_mask = cpu_to_le32(old_sta_mask), 985 .modify.new_sta_id_mask = cpu_to_le32(new_sta_mask), 986 }; 987 u32 cmd_id = WIDE_ID(DATA_PATH_GROUP, RX_BAID_ALLOCATION_CONFIG_CMD); 988 int baid; 989 990 BUILD_BUG_ON(sizeof(struct iwl_rx_baid_cfg_resp) != sizeof(baid)); 991 992 for (baid = 0; baid < ARRAY_SIZE(mvm->baid_map); baid++) { 993 struct iwl_mvm_baid_data *data; 994 int ret; 995 996 data = rcu_dereference_protected(mvm->baid_map[baid], 997 lockdep_is_held(&mvm->mutex)); 998 if (!data) 999 continue; 1000 1001 if (!(data->sta_mask & old_sta_mask)) 1002 continue; 1003 1004 WARN_ONCE(data->sta_mask != old_sta_mask, 1005 "BAID data for %d corrupted - expected 0x%x found 0x%x\n", 1006 baid, old_sta_mask, data->sta_mask); 1007 1008 cmd.modify.tid = cpu_to_le32(data->tid); 1009 1010 ret = iwl_mvm_send_cmd_pdu(mvm, cmd_id, 0, sizeof(cmd), &cmd); 1011 data->sta_mask = new_sta_mask; 1012 if (ret) 1013 return ret; 1014 } 1015 1016 return 0; 1017 } 1018 1019 static int iwl_mvm_mld_update_sta_resources(struct iwl_mvm *mvm, 1020 struct ieee80211_vif *vif, 1021 struct ieee80211_sta *sta, 1022 u32 old_sta_mask, 1023 u32 new_sta_mask) 1024 { 1025 int ret; 1026 1027 ret = iwl_mvm_mld_update_sta_queues(mvm, sta, 1028 old_sta_mask, 1029 new_sta_mask); 1030 if (ret) 1031 return ret; 1032 1033 ret = iwl_mvm_mld_update_sta_keys(mvm, vif, sta, 1034 old_sta_mask, 1035 new_sta_mask); 1036 if (ret) 1037 return ret; 1038 1039 return iwl_mvm_mld_update_sta_baids(mvm, old_sta_mask, new_sta_mask); 1040 } 1041 1042 int iwl_mvm_mld_update_sta_links(struct iwl_mvm *mvm, 1043 struct ieee80211_vif *vif, 1044 struct ieee80211_sta *sta, 1045 u16 old_links, u16 new_links) 1046 { 1047 struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta); 1048 struct iwl_mvm_vif *mvm_vif = iwl_mvm_vif_from_mac80211(vif); 1049 struct iwl_mvm_link_sta *mvm_sta_link; 1050 struct iwl_mvm_vif_link_info *mvm_vif_link; 1051 unsigned long links_to_add = ~old_links & new_links; 1052 unsigned long links_to_rem = old_links & ~new_links; 1053 unsigned long old_links_long = old_links; 1054 u32 current_sta_mask = 0, sta_mask_added = 0, sta_mask_to_rem = 0; 1055 unsigned long link_sta_added_to_fw = 0, link_sta_allocated = 0; 1056 unsigned int link_id; 1057 int ret; 1058 1059 lockdep_assert_held(&mvm->mutex); 1060 1061 for_each_set_bit(link_id, &old_links_long, 1062 IEEE80211_MLD_MAX_NUM_LINKS) { 1063 mvm_sta_link = 1064 rcu_dereference_protected(mvm_sta->link[link_id], 1065 lockdep_is_held(&mvm->mutex)); 1066 1067 if (WARN_ON(!mvm_sta_link)) { 1068 ret = -EINVAL; 1069 goto err; 1070 } 1071 1072 current_sta_mask |= BIT(mvm_sta_link->sta_id); 1073 if (links_to_rem & BIT(link_id)) 1074 sta_mask_to_rem |= BIT(mvm_sta_link->sta_id); 1075 } 1076 1077 if (sta_mask_to_rem) { 1078 ret = iwl_mvm_mld_update_sta_resources(mvm, vif, sta, 1079 current_sta_mask, 1080 current_sta_mask & 1081 ~sta_mask_to_rem); 1082 if (WARN_ON(ret)) 1083 goto err; 1084 1085 current_sta_mask &= ~sta_mask_to_rem; 1086 } 1087 1088 for_each_set_bit(link_id, &links_to_rem, IEEE80211_MLD_MAX_NUM_LINKS) { 1089 mvm_sta_link = 1090 rcu_dereference_protected(mvm_sta->link[link_id], 1091 lockdep_is_held(&mvm->mutex)); 1092 mvm_vif_link = mvm_vif->link[link_id]; 1093 1094 if (WARN_ON(!mvm_sta_link || !mvm_vif_link)) { 1095 ret = -EINVAL; 1096 goto err; 1097 } 1098 1099 ret = iwl_mvm_mld_rm_sta_from_fw(mvm, mvm_sta_link->sta_id); 1100 if (WARN_ON(ret)) 1101 goto err; 1102 1103 if (vif->type == NL80211_IFTYPE_STATION) 1104 mvm_vif_link->ap_sta_id = IWL_MVM_INVALID_STA; 1105 1106 iwl_mvm_mld_free_sta_link(mvm, mvm_sta, mvm_sta_link, link_id, 1107 false); 1108 } 1109 1110 for_each_set_bit(link_id, &links_to_add, IEEE80211_MLD_MAX_NUM_LINKS) { 1111 struct ieee80211_bss_conf *link_conf = 1112 link_conf_dereference_protected(vif, link_id); 1113 struct ieee80211_link_sta *link_sta = 1114 link_sta_dereference_protected(sta, link_id); 1115 mvm_vif_link = mvm_vif->link[link_id]; 1116 1117 if (WARN_ON(!mvm_vif_link || !link_conf || !link_sta || 1118 mvm_sta->link[link_id])) { 1119 ret = -EINVAL; 1120 goto err; 1121 } 1122 1123 ret = iwl_mvm_mld_alloc_sta_link(mvm, vif, sta, link_id); 1124 if (WARN_ON(ret)) 1125 goto err; 1126 1127 link_sta->agg.max_rc_amsdu_len = 1; 1128 ieee80211_sta_recalc_aggregates(sta); 1129 1130 mvm_sta_link = 1131 rcu_dereference_protected(mvm_sta->link[link_id], 1132 lockdep_is_held(&mvm->mutex)); 1133 1134 if (WARN_ON(!mvm_sta_link)) { 1135 ret = -EINVAL; 1136 goto err; 1137 } 1138 1139 if (vif->type == NL80211_IFTYPE_STATION) 1140 iwl_mvm_mld_set_ap_sta_id(sta, mvm_vif_link, 1141 mvm_sta_link); 1142 1143 link_sta_allocated |= BIT(link_id); 1144 1145 sta_mask_added |= BIT(mvm_sta_link->sta_id); 1146 1147 ret = iwl_mvm_mld_cfg_sta(mvm, sta, vif, link_sta, link_conf, 1148 mvm_sta_link); 1149 if (WARN_ON(ret)) 1150 goto err; 1151 1152 link_sta_added_to_fw |= BIT(link_id); 1153 1154 iwl_mvm_rs_add_sta_link(mvm, mvm_sta_link); 1155 } 1156 1157 if (sta_mask_added) { 1158 ret = iwl_mvm_mld_update_sta_resources(mvm, vif, sta, 1159 current_sta_mask, 1160 current_sta_mask | 1161 sta_mask_added); 1162 if (WARN_ON(ret)) 1163 goto err; 1164 } 1165 1166 return 0; 1167 1168 err: 1169 /* remove all already allocated stations in FW */ 1170 for_each_set_bit(link_id, &link_sta_added_to_fw, 1171 IEEE80211_MLD_MAX_NUM_LINKS) { 1172 mvm_sta_link = 1173 rcu_dereference_protected(mvm_sta->link[link_id], 1174 lockdep_is_held(&mvm->mutex)); 1175 1176 iwl_mvm_mld_rm_sta_from_fw(mvm, mvm_sta_link->sta_id); 1177 } 1178 1179 /* remove all already allocated station links in driver */ 1180 for_each_set_bit(link_id, &link_sta_allocated, 1181 IEEE80211_MLD_MAX_NUM_LINKS) { 1182 mvm_sta_link = 1183 rcu_dereference_protected(mvm_sta->link[link_id], 1184 lockdep_is_held(&mvm->mutex)); 1185 1186 iwl_mvm_mld_free_sta_link(mvm, mvm_sta, mvm_sta_link, link_id, 1187 false); 1188 } 1189 1190 return ret; 1191 } 1192