1222 lines · c
1// SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause2/*3 * Copyright (C) 2022-2024 Intel Corporation4 */5#include "mvm.h"6#include "time-sync.h"7#include "sta.h"8 9u32 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 48static int iwl_mvm_mld_send_sta_cmd(struct iwl_mvm *mvm,49 struct iwl_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 table61 */62static 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_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 remove92 * the station validate that the station is indeed known to the driver (sanity93 * only).94 */95static int iwl_mvm_mld_rm_sta_from_fw(struct iwl_mvm *mvm, u32 sta_id)96{97 struct iwl_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 118static 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 queues138 */139int 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 else155 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 unknown161 * to firmware so enable queue here - after the station was added162 */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 */178static 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 */209int 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);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 */248int 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);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 be266 * invalid, so make sure we use the queue we want.267 * Note that this is done here as we want to avoid making DQA268 * 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 */282int 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 297int 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 310static 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 it340 */341static 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 366int 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 data397 * structures, and in addition remove it from the local data structure.398 */399int 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 414int 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 422int 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 */431static 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_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 518void 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 533static 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 550static 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 */583static 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 605err:606 iwl_mvm_mld_sta_rm_all_sta_links(mvm, mvm_sta);607 return ret;608}609 610static 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 622static 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 requires635 * 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 670int 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 728err: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 744int 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 776static 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 803int 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 857int 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 873void 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 895void 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 913void 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 940static 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] = &cmd955 };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 else971 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 981static 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 1027static 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 1050int 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 1198err: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