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