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