1 // SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause
3 * Copyright (C) 2022-2024 Intel Corporation
9 u32 iwl_mvm_sta_fw_id_mask(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
12 struct ieee80211_link_sta *link_sta;
13 struct iwl_mvm_sta *mvmsta;
14 struct ieee80211_vif *vif;
21 mvmsta = iwl_mvm_sta_from_mac80211(sta);
24 /* it's easy when the STA is not an MLD */
25 if (!sta->valid_links)
26 return BIT(mvmsta->deflink.sta_id);
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;
32 /* unless we have a specific link in mind */
33 if (filter_link_id >= 0 && link_id != filter_link_id)
37 rcu_dereference_check(mvmsta->link[link_id],
38 lockdep_is_held(&mvm->mutex));
42 result |= BIT(mvm_link_sta->sta_id);
48 static int iwl_mvm_mld_send_sta_cmd(struct iwl_mvm *mvm,
49 struct iwl_sta_cfg_cmd *cmd)
51 int ret = iwl_mvm_send_cmd_pdu(mvm,
52 WIDE_ID(MAC_CONF_GROUP, STA_CONFIG_CMD),
53 0, sizeof(*cmd), cmd);
55 IWL_ERR(mvm, "STA_CONFIG_CMD send failed, ret=0x%x\n", ret);
60 * Add an internal station to the FW table
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)
66 struct iwl_sta_cfg_cmd cmd;
68 lockdep_assert_held(&mvm->mutex);
70 memset(&cmd, 0, sizeof(cmd));
71 cmd.sta_id = cpu_to_le32((u8)sta->sta_id);
73 cmd.link_id = cpu_to_le32(link_id);
75 cmd.station_type = cpu_to_le32(sta->type);
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);
83 memcpy(cmd.peer_mld_address, addr, ETH_ALEN);
84 memcpy(cmd.peer_link_address, addr, ETH_ALEN);
87 return iwl_mvm_mld_send_sta_cmd(mvm, &cmd);
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
95 static int iwl_mvm_mld_rm_sta_from_fw(struct iwl_mvm *mvm, u32 sta_id)
97 struct iwl_remove_sta_cmd rm_sta_cmd = {
98 .sta_id = cpu_to_le32(sta_id),
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);
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);
111 IWL_ERR(mvm, "Failed to remove station. Id=%d\n", sta_id);
118 static int iwl_mvm_add_aux_sta_to_fw(struct iwl_mvm *mvm,
119 struct iwl_mvm_int_sta *sta,
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),
129 ret = iwl_mvm_send_cmd_pdu(mvm, WIDE_ID(MAC_CONF_GROUP, AUX_STA_CMD),
130 0, sizeof(cmd), &cmd);
132 IWL_ERR(mvm, "Failed to send AUX_STA_CMD\n");
137 * Adds an internal sta to the FW table with its queues
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,
143 unsigned int *_wdg_timeout)
146 unsigned int wdg_timeout = _wdg_timeout ? *_wdg_timeout :
147 mvm->trans->trans_cfg->base_params->wd_timeout;
149 if (WARN_ON_ONCE(sta->sta_id == IWL_INVALID_STA))
152 if (sta->type == STATION_TYPE_AUX)
153 ret = iwl_mvm_add_aux_sta_to_fw(mvm, sta, link_id);
155 ret = iwl_mvm_mld_add_int_sta_to_fw(mvm, sta, addr, link_id);
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
163 txq = iwl_mvm_tvqm_enable_txq(mvm, NULL, sta->sta_id, tid,
166 iwl_mvm_mld_rm_sta_from_fw(mvm, sta->sta_id);
175 * Adds a new int sta: allocate it in the driver, add it to the FW table,
176 * and add its queues.
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)
187 lockdep_assert_held(&mvm->mutex);
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,
195 ret = iwl_mvm_mld_add_int_sta_with_queue(mvm, int_sta, addr, link_id,
196 queue, tid, wdg_timeout);
198 iwl_mvm_dealloc_int_sta(mvm, int_sta);
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.
209 int iwl_mvm_mld_add_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
210 struct ieee80211_bss_conf *link_conf)
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);
222 lockdep_assert_held(&mvm->mutex);
224 if (vif->type == NL80211_IFTYPE_ADHOC)
225 baddr = link_conf->bssid;
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;
233 WARN(1, "Missing required TXQ for adding bcast STA\n");
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);
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.
248 int iwl_mvm_mld_add_mcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
249 struct ieee80211_bss_conf *link_conf)
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);
259 lockdep_assert_held(&mvm->mutex);
261 if (WARN_ON(vif->type != NL80211_IFTYPE_AP &&
262 vif->type != NL80211_IFTYPE_ADHOC))
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.
270 if (vif->type == NL80211_IFTYPE_ADHOC)
271 mvm_link->cab_queue = IWL_MVM_DQA_GCAST_QUEUE;
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,
279 /* Allocate a new station entry for the sniffer station to the given vif,
280 * and send it to the FW.
282 int iwl_mvm_mld_add_snif_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
283 struct ieee80211_bss_conf *link_conf)
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];
289 lockdep_assert_held(&mvm->mutex);
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);
297 int iwl_mvm_mld_add_aux_sta(struct iwl_mvm *mvm, u32 lmac_id)
299 lockdep_assert_held(&mvm->mutex);
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.
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);
310 static int iwl_mvm_mld_disable_txq(struct iwl_mvm *mvm, u32 sta_mask,
311 u16 *queueptr, u8 tid)
313 int queue = *queueptr;
316 if (tid == IWL_MAX_TID_COUNT)
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),
328 ret = iwl_mvm_send_cmd_pdu(mvm, cmd_id, 0,
333 iwl_trans_txq_free(mvm->trans, queue);
334 *queueptr = IWL_MVM_INVALID_QUEUE;
339 /* Removes a sta from the FW table, disable its queues, and dealloc it
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)
347 lockdep_assert_held(&mvm->mutex);
349 if (WARN_ON_ONCE(int_sta->sta_id == IWL_INVALID_STA))
353 iwl_mvm_flush_sta(mvm, int_sta->sta_id, int_sta->tfd_queue_msk);
355 iwl_mvm_mld_disable_txq(mvm, BIT(int_sta->sta_id), queuptr, tid);
357 ret = iwl_mvm_mld_rm_sta_from_fw(mvm, int_sta->sta_id);
359 IWL_WARN(mvm, "Failed sending remove station\n");
361 iwl_mvm_dealloc_int_sta(mvm, int_sta);
366 int iwl_mvm_mld_rm_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
367 struct ieee80211_bss_conf *link_conf)
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];
373 lockdep_assert_held(&mvm->mutex);
379 case NL80211_IFTYPE_AP:
380 case NL80211_IFTYPE_ADHOC:
381 queueptr = &link->mgmt_queue;
383 case NL80211_IFTYPE_P2P_DEVICE:
384 queueptr = &mvm->p2p_dev_queue;
387 WARN(1, "Can't free bcast queue on vif type %d\n",
392 return iwl_mvm_mld_rm_int_sta(mvm, &link->bcast_sta,
393 true, IWL_MAX_TID_COUNT, queueptr);
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.
399 int iwl_mvm_mld_rm_mcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
400 struct ieee80211_bss_conf *link_conf)
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];
405 lockdep_assert_held(&mvm->mutex);
410 return iwl_mvm_mld_rm_int_sta(mvm, &link->mcast_sta, true, 0,
414 int iwl_mvm_mld_rm_snif_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
416 lockdep_assert_held(&mvm->mutex);
418 return iwl_mvm_mld_rm_int_sta(mvm, &mvm->snif_sta, false,
419 IWL_MAX_TID_COUNT, &mvm->snif_queue);
422 int iwl_mvm_mld_rm_aux_sta(struct iwl_mvm *mvm)
424 lockdep_assert_held(&mvm->mutex);
426 return iwl_mvm_mld_rm_int_sta(mvm, &mvm->aux_sta, false,
427 IWL_MAX_TID_COUNT, &mvm->aux_queue);
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)
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),
445 u32 agg_size = 0, mpdu_dens = 0;
447 /* when adding sta, link should exist in FW */
448 if (WARN_ON(link_info->fw_link_id == IWL_MVM_FW_LINK_ID_INVALID))
451 cmd.link_id = cpu_to_le32(link_info->fw_link_id);
453 memcpy(&cmd.peer_mld_address, sta->addr, ETH_ALEN);
454 memcpy(&cmd.peer_link_address, link_sta->addr, ETH_ALEN);
456 if (mvm_sta->sta_state >= IEEE80211_STA_ASSOC)
457 cmd.assoc_id = cpu_to_le32(sta->aid);
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);
464 switch (link_sta->rx_nss) {
466 cmd.mimo = cpu_to_le32(0);
469 cmd.mimo = cpu_to_le32(1);
473 switch (link_sta->smps_mode) {
474 case IEEE80211_SMPS_AUTOMATIC:
475 case IEEE80211_SMPS_NUM_MODES:
478 case IEEE80211_SMPS_STATIC:
480 cmd.mimo = cpu_to_le32(0);
482 case IEEE80211_SMPS_DYNAMIC:
483 cmd.mimo_protection = cpu_to_le32(1);
485 case IEEE80211_SMPS_OFF:
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);
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));
500 if (link_sta->he_cap.has_he) {
502 cpu_to_le32(link_conf->uora_exists ? 1 : 0);
505 iwl_mvm_set_sta_pkt_ext(mvm, link_sta, &cmd.pkt_ext);
508 cmd.htc_flags = iwl_mvm_get_sta_htc_flags(sta, link_sta);
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);
515 return iwl_mvm_mld_send_sta_cmd(mvm, &cmd);
518 void iwl_mvm_mld_free_sta_link(struct iwl_mvm *mvm,
519 struct iwl_mvm_sta *mvm_sta,
520 struct iwl_mvm_link_sta *mvm_sta_link,
521 unsigned int link_id)
523 lockdep_assert_wiphy(mvm->hw->wiphy);
524 lockdep_assert_held(&mvm->mutex);
526 RCU_INIT_POINTER(mvm->fw_id_to_mac_id[mvm_sta_link->sta_id], NULL);
527 RCU_INIT_POINTER(mvm->fw_id_to_link_sta[mvm_sta_link->sta_id], NULL);
528 RCU_INIT_POINTER(mvm_sta->link[link_id], NULL);
530 if (mvm_sta_link != &mvm_sta->deflink)
531 kfree_rcu(mvm_sta_link, rcu_head);
534 static void iwl_mvm_mld_sta_rm_all_sta_links(struct iwl_mvm *mvm,
535 struct iwl_mvm_sta *mvm_sta)
537 unsigned int link_id;
539 for (link_id = 0; link_id < ARRAY_SIZE(mvm_sta->link); link_id++) {
540 struct iwl_mvm_link_sta *link =
541 rcu_dereference_protected(mvm_sta->link[link_id],
542 lockdep_is_held(&mvm->mutex));
547 iwl_mvm_mld_free_sta_link(mvm, mvm_sta, link, link_id);
551 static int iwl_mvm_mld_alloc_sta_link(struct iwl_mvm *mvm,
552 struct ieee80211_vif *vif,
553 struct ieee80211_sta *sta,
554 unsigned int link_id)
556 struct ieee80211_link_sta *link_sta =
557 link_sta_dereference_protected(sta, link_id);
558 struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
559 struct iwl_mvm_link_sta *link;
560 u32 sta_id = iwl_mvm_find_free_sta_id(mvm,
561 ieee80211_vif_type_p2p(vif));
563 lockdep_assert_wiphy(mvm->hw->wiphy);
564 lockdep_assert_held(&mvm->mutex);
566 if (sta_id == IWL_INVALID_STA)
569 if (rcu_access_pointer(sta->link[link_id]) == &sta->deflink) {
570 link = &mvm_sta->deflink;
572 link = kzalloc(sizeof(*link), GFP_KERNEL);
577 link->sta_id = sta_id;
578 rcu_assign_pointer(mvm_sta->link[link_id], link);
579 rcu_assign_pointer(mvm->fw_id_to_mac_id[link->sta_id], sta);
580 rcu_assign_pointer(mvm->fw_id_to_link_sta[link->sta_id],
586 /* allocate all the links of a sta, called when the station is first added */
587 static int iwl_mvm_mld_alloc_sta_links(struct iwl_mvm *mvm,
588 struct ieee80211_vif *vif,
589 struct ieee80211_sta *sta)
591 struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
592 struct ieee80211_link_sta *link_sta;
593 unsigned int link_id;
596 lockdep_assert_held(&mvm->mutex);
598 for_each_sta_active_link(vif, sta, link_sta, link_id) {
599 if (WARN_ON(mvm_sta->link[link_id]))
602 ret = iwl_mvm_mld_alloc_sta_link(mvm, vif, sta, link_id);
610 iwl_mvm_mld_sta_rm_all_sta_links(mvm, mvm_sta);
614 static void iwl_mvm_mld_set_ap_sta_id(struct ieee80211_sta *sta,
615 struct iwl_mvm_vif_link_info *vif_link,
616 struct iwl_mvm_link_sta *sta_link)
619 WARN_ON(vif_link->ap_sta_id != IWL_INVALID_STA);
620 vif_link->ap_sta_id = sta_link->sta_id;
622 WARN_ON(vif_link->ap_sta_id == IWL_INVALID_STA);
626 static int iwl_mvm_alloc_sta_after_restart(struct iwl_mvm *mvm,
627 struct ieee80211_vif *vif,
628 struct ieee80211_sta *sta)
630 struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
631 struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
632 struct ieee80211_link_sta *link_sta;
633 unsigned int link_id;
634 /* no active link found */
638 lockdep_assert_wiphy(mvm->hw->wiphy);
639 lockdep_assert_held(&mvm->mutex);
641 /* First add an empty station since allocating a queue requires
642 * a valid station. Since we need a link_id to allocate a station,
643 * pick up the first valid one.
645 for_each_sta_active_link(vif, sta, link_sta, link_id) {
646 struct iwl_mvm_vif_link_info *mvm_link;
647 struct ieee80211_bss_conf *link_conf =
648 link_conf_dereference_protected(vif, link_id);
649 struct iwl_mvm_link_sta *mvm_link_sta =
650 rcu_dereference_protected(mvm_sta->link[link_id],
651 lockdep_is_held(&mvm->mutex));
656 mvm_link = mvmvif->link[link_conf->link_id];
658 if (!mvm_link || !mvm_link_sta)
661 sta_id = mvm_link_sta->sta_id;
662 ret = iwl_mvm_mld_cfg_sta(mvm, sta, vif, link_sta,
663 link_conf, mvm_link_sta);
667 rcu_assign_pointer(mvm->fw_id_to_mac_id[sta_id], sta);
668 rcu_assign_pointer(mvm->fw_id_to_link_sta[sta_id], link_sta);
672 iwl_mvm_realloc_queues_after_restart(mvm, sta);
677 int iwl_mvm_mld_add_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
678 struct ieee80211_sta *sta)
680 struct iwl_mvm_vif *mvm_vif = iwl_mvm_vif_from_mac80211(vif);
681 struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
682 unsigned long link_sta_added_to_fw = 0;
683 struct ieee80211_link_sta *link_sta;
685 unsigned int link_id;
687 lockdep_assert_held(&mvm->mutex);
689 if (!test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status)) {
690 ret = iwl_mvm_mld_alloc_sta_links(mvm, vif, sta);
694 spin_lock_init(&mvm_sta->lock);
696 ret = iwl_mvm_sta_init(mvm, vif, sta, IWL_INVALID_STA,
699 ret = iwl_mvm_alloc_sta_after_restart(mvm, vif, sta);
705 /* at this stage sta link pointers are already allocated */
706 ret = iwl_mvm_mld_update_sta(mvm, vif, sta);
710 for_each_sta_active_link(vif, sta, link_sta, link_id) {
711 struct ieee80211_bss_conf *link_conf =
712 link_conf_dereference_protected(vif, link_id);
713 struct iwl_mvm_link_sta *mvm_link_sta =
714 rcu_dereference_protected(mvm_sta->link[link_id],
715 lockdep_is_held(&mvm->mutex));
717 if (WARN_ON(!link_conf || !mvm_link_sta)) {
722 ret = iwl_mvm_mld_cfg_sta(mvm, sta, vif, link_sta, link_conf,
727 link_sta_added_to_fw |= BIT(link_id);
729 if (vif->type == NL80211_IFTYPE_STATION)
730 iwl_mvm_mld_set_ap_sta_id(sta, mvm_vif->link[link_id],
736 /* remove all already allocated stations in FW */
737 for_each_set_bit(link_id, &link_sta_added_to_fw,
738 IEEE80211_MLD_MAX_NUM_LINKS) {
739 struct iwl_mvm_link_sta *mvm_link_sta =
740 rcu_dereference_protected(mvm_sta->link[link_id],
741 lockdep_is_held(&mvm->mutex));
743 iwl_mvm_mld_rm_sta_from_fw(mvm, mvm_link_sta->sta_id);
746 /* free all sta resources in the driver */
747 iwl_mvm_mld_sta_rm_all_sta_links(mvm, mvm_sta);
751 int iwl_mvm_mld_update_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
752 struct ieee80211_sta *sta)
754 struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
755 struct ieee80211_link_sta *link_sta;
756 unsigned int link_id;
759 lockdep_assert_held(&mvm->mutex);
761 for_each_sta_active_link(vif, sta, link_sta, link_id) {
762 struct ieee80211_bss_conf *link_conf =
763 link_conf_dereference_protected(vif, link_id);
764 struct iwl_mvm_link_sta *mvm_link_sta =
765 rcu_dereference_protected(mvm_sta->link[link_id],
766 lockdep_is_held(&mvm->mutex));
768 if (WARN_ON(!link_conf || !mvm_link_sta))
771 ret = iwl_mvm_mld_cfg_sta(mvm, sta, vif, link_sta, link_conf,
775 IWL_ERR(mvm, "Failed to update sta link %d\n", link_id);
783 static void iwl_mvm_mld_disable_sta_queues(struct iwl_mvm *mvm,
784 struct ieee80211_vif *vif,
785 struct ieee80211_sta *sta)
787 struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
788 u32 sta_mask = iwl_mvm_sta_fw_id_mask(mvm, sta, -1);
791 lockdep_assert_held(&mvm->mutex);
793 for (i = 0; i < ARRAY_SIZE(mvm_sta->tid_data); i++) {
794 if (mvm_sta->tid_data[i].txq_id == IWL_MVM_INVALID_QUEUE)
797 iwl_mvm_mld_disable_txq(mvm, sta_mask,
798 &mvm_sta->tid_data[i].txq_id, i);
799 mvm_sta->tid_data[i].txq_id = IWL_MVM_INVALID_QUEUE;
802 for (i = 0; i < ARRAY_SIZE(sta->txq); i++) {
803 struct iwl_mvm_txq *mvmtxq =
804 iwl_mvm_txq_from_mac80211(sta->txq[i]);
806 mvmtxq->txq_id = IWL_MVM_INVALID_QUEUE;
810 int iwl_mvm_mld_rm_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
811 struct ieee80211_sta *sta)
813 struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
814 struct ieee80211_link_sta *link_sta;
815 unsigned int link_id;
818 lockdep_assert_held(&mvm->mutex);
820 /* flush its queues here since we are freeing mvm_sta */
821 for_each_sta_active_link(vif, sta, link_sta, link_id) {
822 struct iwl_mvm_link_sta *mvm_link_sta =
823 rcu_dereference_protected(mvm_sta->link[link_id],
824 lockdep_is_held(&mvm->mutex));
826 if (WARN_ON(!mvm_link_sta))
829 ret = iwl_mvm_flush_sta_tids(mvm, mvm_link_sta->sta_id,
835 ret = iwl_mvm_wait_sta_queues_empty(mvm, mvm_sta);
839 iwl_mvm_mld_disable_sta_queues(mvm, vif, sta);
841 for_each_sta_active_link(vif, sta, link_sta, link_id) {
842 struct iwl_mvm_link_sta *mvm_link_sta =
843 rcu_dereference_protected(mvm_sta->link[link_id],
844 lockdep_is_held(&mvm->mutex));
845 iwl_mvm_sta_del(mvm, vif, sta, link_sta);
847 ret = iwl_mvm_mld_rm_sta_from_fw(mvm, mvm_link_sta->sta_id);
849 iwl_mvm_mld_free_sta_link(mvm, mvm_sta, mvm_link_sta, link_id);
851 kfree(mvm_sta->mpdu_counters);
852 mvm_sta->mpdu_counters = NULL;
857 int iwl_mvm_mld_rm_sta_id(struct iwl_mvm *mvm, u8 sta_id)
861 lockdep_assert_wiphy(mvm->hw->wiphy);
862 lockdep_assert_held(&mvm->mutex);
864 if (WARN_ON(sta_id == IWL_INVALID_STA))
867 ret = iwl_mvm_mld_rm_sta_from_fw(mvm, sta_id);
869 RCU_INIT_POINTER(mvm->fw_id_to_mac_id[sta_id], NULL);
870 RCU_INIT_POINTER(mvm->fw_id_to_link_sta[sta_id], NULL);
874 void iwl_mvm_mld_sta_modify_disable_tx(struct iwl_mvm *mvm,
875 struct iwl_mvm_sta *mvmsta,
878 struct iwl_mvm_sta_disable_tx_cmd cmd;
881 cmd.sta_id = cpu_to_le32(mvmsta->deflink.sta_id);
882 cmd.disable = cpu_to_le32(disable);
884 if (WARN_ON(iwl_mvm_has_no_host_disable_tx(mvm)))
887 ret = iwl_mvm_send_cmd_pdu(mvm,
888 WIDE_ID(MAC_CONF_GROUP, STA_DISABLE_TX_CMD),
889 CMD_ASYNC, sizeof(cmd), &cmd);
892 "Failed to send STA_DISABLE_TX_CMD command (%d)\n",
896 void iwl_mvm_mld_sta_modify_disable_tx_ap(struct iwl_mvm *mvm,
897 struct ieee80211_sta *sta,
900 struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
902 spin_lock_bh(&mvm_sta->lock);
904 if (mvm_sta->disable_tx == disable) {
905 spin_unlock_bh(&mvm_sta->lock);
909 iwl_mvm_mld_sta_modify_disable_tx(mvm, mvm_sta, disable);
911 spin_unlock_bh(&mvm_sta->lock);
914 void iwl_mvm_mld_modify_all_sta_disable_tx(struct iwl_mvm *mvm,
915 struct iwl_mvm_vif *mvmvif,
918 struct ieee80211_sta *sta;
919 struct iwl_mvm_sta *mvm_sta;
924 /* Block/unblock all the stations of the given mvmvif */
925 for (i = 0; i < mvm->fw->ucode_capa.num_stations; i++) {
926 sta = rcu_dereference(mvm->fw_id_to_mac_id[i]);
927 if (IS_ERR_OR_NULL(sta))
930 mvm_sta = iwl_mvm_sta_from_mac80211(sta);
931 if (mvm_sta->mac_id_n_color !=
932 FW_CMD_ID_AND_COLOR(mvmvif->id, mvmvif->color))
935 iwl_mvm_mld_sta_modify_disable_tx(mvm, mvm_sta, disable);
941 static int iwl_mvm_mld_update_sta_queues(struct iwl_mvm *mvm,
942 struct ieee80211_sta *sta,
946 struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
947 struct iwl_scd_queue_cfg_cmd cmd = {
948 .operation = cpu_to_le32(IWL_SCD_QUEUE_MODIFY),
949 .u.modify.old_sta_mask = cpu_to_le32(old_sta_mask),
950 .u.modify.new_sta_mask = cpu_to_le32(new_sta_mask),
952 struct iwl_host_cmd hcmd = {
953 .id = WIDE_ID(DATA_PATH_GROUP, SCD_QUEUE_CONFIG_CMD),
954 .len[0] = sizeof(cmd),
960 lockdep_assert_held(&mvm->mutex);
962 for (tid = 0; tid <= IWL_MAX_TID_COUNT; tid++) {
963 struct iwl_mvm_tid_data *tid_data = &mvm_sta->tid_data[tid];
964 int txq_id = tid_data->txq_id;
966 if (txq_id == IWL_MVM_INVALID_QUEUE)
969 if (tid == IWL_MAX_TID_COUNT)
970 cmd.u.modify.tid = cpu_to_le32(IWL_MGMT_TID);
972 cmd.u.modify.tid = cpu_to_le32(tid);
974 ret = iwl_mvm_send_cmd(mvm, &hcmd);
982 static int iwl_mvm_mld_update_sta_baids(struct iwl_mvm *mvm,
986 struct iwl_rx_baid_cfg_cmd cmd = {
987 .action = cpu_to_le32(IWL_RX_BAID_ACTION_MODIFY),
988 .modify.old_sta_id_mask = cpu_to_le32(old_sta_mask),
989 .modify.new_sta_id_mask = cpu_to_le32(new_sta_mask),
991 u32 cmd_id = WIDE_ID(DATA_PATH_GROUP, RX_BAID_ALLOCATION_CONFIG_CMD);
994 /* mac80211 will remove sessions later, but we ignore all that */
995 if (test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status))
998 BUILD_BUG_ON(sizeof(struct iwl_rx_baid_cfg_resp) != sizeof(baid));
1000 for (baid = 0; baid < ARRAY_SIZE(mvm->baid_map); baid++) {
1001 struct iwl_mvm_baid_data *data;
1004 data = rcu_dereference_protected(mvm->baid_map[baid],
1005 lockdep_is_held(&mvm->mutex));
1009 if (!(data->sta_mask & old_sta_mask))
1012 WARN_ONCE(data->sta_mask != old_sta_mask,
1013 "BAID data for %d corrupted - expected 0x%x found 0x%x\n",
1014 baid, old_sta_mask, data->sta_mask);
1016 cmd.modify.tid = cpu_to_le32(data->tid);
1018 ret = iwl_mvm_send_cmd_pdu(mvm, cmd_id, CMD_SEND_IN_RFKILL,
1020 data->sta_mask = new_sta_mask;
1028 static int iwl_mvm_mld_update_sta_resources(struct iwl_mvm *mvm,
1029 struct ieee80211_vif *vif,
1030 struct ieee80211_sta *sta,
1036 ret = iwl_mvm_mld_update_sta_queues(mvm, sta,
1042 ret = iwl_mvm_mld_update_sta_keys(mvm, vif, sta,
1048 return iwl_mvm_mld_update_sta_baids(mvm, old_sta_mask, new_sta_mask);
1051 int iwl_mvm_mld_update_sta_links(struct iwl_mvm *mvm,
1052 struct ieee80211_vif *vif,
1053 struct ieee80211_sta *sta,
1054 u16 old_links, u16 new_links)
1056 struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
1057 struct iwl_mvm_vif *mvm_vif = iwl_mvm_vif_from_mac80211(vif);
1058 struct iwl_mvm_link_sta *mvm_sta_link;
1059 struct iwl_mvm_vif_link_info *mvm_vif_link;
1060 unsigned long links_to_add = ~old_links & new_links;
1061 unsigned long links_to_rem = old_links & ~new_links;
1062 unsigned long old_links_long = old_links;
1063 u32 current_sta_mask = 0, sta_mask_added = 0, sta_mask_to_rem = 0;
1064 unsigned long link_sta_added_to_fw = 0, link_sta_allocated = 0;
1065 unsigned int link_id;
1068 lockdep_assert_wiphy(mvm->hw->wiphy);
1069 lockdep_assert_held(&mvm->mutex);
1071 for_each_set_bit(link_id, &old_links_long,
1072 IEEE80211_MLD_MAX_NUM_LINKS) {
1074 rcu_dereference_protected(mvm_sta->link[link_id],
1075 lockdep_is_held(&mvm->mutex));
1077 if (WARN_ON(!mvm_sta_link)) {
1082 current_sta_mask |= BIT(mvm_sta_link->sta_id);
1083 if (links_to_rem & BIT(link_id))
1084 sta_mask_to_rem |= BIT(mvm_sta_link->sta_id);
1087 if (sta_mask_to_rem) {
1088 ret = iwl_mvm_mld_update_sta_resources(mvm, vif, sta,
1095 current_sta_mask &= ~sta_mask_to_rem;
1098 for_each_set_bit(link_id, &links_to_rem, IEEE80211_MLD_MAX_NUM_LINKS) {
1100 rcu_dereference_protected(mvm_sta->link[link_id],
1101 lockdep_is_held(&mvm->mutex));
1102 mvm_vif_link = mvm_vif->link[link_id];
1104 if (WARN_ON(!mvm_sta_link || !mvm_vif_link)) {
1109 ret = iwl_mvm_mld_rm_sta_from_fw(mvm, mvm_sta_link->sta_id);
1113 if (vif->type == NL80211_IFTYPE_STATION)
1114 mvm_vif_link->ap_sta_id = IWL_INVALID_STA;
1116 iwl_mvm_mld_free_sta_link(mvm, mvm_sta, mvm_sta_link, link_id);
1119 for_each_set_bit(link_id, &links_to_add, IEEE80211_MLD_MAX_NUM_LINKS) {
1120 struct ieee80211_bss_conf *link_conf =
1121 link_conf_dereference_protected(vif, link_id);
1122 struct ieee80211_link_sta *link_sta =
1123 link_sta_dereference_protected(sta, link_id);
1124 mvm_vif_link = mvm_vif->link[link_id];
1126 if (WARN_ON(!mvm_vif_link || !link_conf || !link_sta)) {
1131 if (test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status)) {
1132 struct iwl_mvm_link_sta *mvm_link_sta =
1133 rcu_dereference_protected(mvm_sta->link[link_id],
1134 lockdep_is_held(&mvm->mutex));
1137 if (WARN_ON(!mvm_link_sta)) {
1142 sta_id = mvm_link_sta->sta_id;
1144 rcu_assign_pointer(mvm->fw_id_to_mac_id[sta_id], sta);
1145 rcu_assign_pointer(mvm->fw_id_to_link_sta[sta_id],
1148 if (WARN_ON(mvm_sta->link[link_id])) {
1152 ret = iwl_mvm_mld_alloc_sta_link(mvm, vif, sta,
1158 link_sta->agg.max_rc_amsdu_len = 1;
1159 ieee80211_sta_recalc_aggregates(sta);
1162 rcu_dereference_protected(mvm_sta->link[link_id],
1163 lockdep_is_held(&mvm->mutex));
1165 if (WARN_ON(!mvm_sta_link)) {
1170 if (vif->type == NL80211_IFTYPE_STATION)
1171 iwl_mvm_mld_set_ap_sta_id(sta, mvm_vif_link,
1174 link_sta_allocated |= BIT(link_id);
1176 sta_mask_added |= BIT(mvm_sta_link->sta_id);
1178 ret = iwl_mvm_mld_cfg_sta(mvm, sta, vif, link_sta, link_conf,
1183 link_sta_added_to_fw |= BIT(link_id);
1185 iwl_mvm_rs_add_sta_link(mvm, mvm_sta_link);
1187 iwl_mvm_rs_rate_init(mvm, vif, sta, link_conf, link_sta,
1188 link_conf->chanreq.oper.chan->band);
1191 if (sta_mask_added) {
1192 ret = iwl_mvm_mld_update_sta_resources(mvm, vif, sta,
1203 /* remove all already allocated stations in FW */
1204 for_each_set_bit(link_id, &link_sta_added_to_fw,
1205 IEEE80211_MLD_MAX_NUM_LINKS) {
1207 rcu_dereference_protected(mvm_sta->link[link_id],
1208 lockdep_is_held(&mvm->mutex));
1210 iwl_mvm_mld_rm_sta_from_fw(mvm, mvm_sta_link->sta_id);
1213 /* remove all already allocated station links in driver */
1214 for_each_set_bit(link_id, &link_sta_allocated,
1215 IEEE80211_MLD_MAX_NUM_LINKS) {
1217 rcu_dereference_protected(mvm_sta->link[link_id],
1218 lockdep_is_held(&mvm->mutex));
1220 iwl_mvm_mld_free_sta_link(mvm, mvm_sta, mvm_sta_link, link_id);