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