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