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 
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 (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  */
92 static 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 
115 static 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  */
136 int 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  */
175 static 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  */
206 int 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  */
245 int 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  */
279 int 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 
294 int 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 
307 static 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  */
338 static 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 
363 int 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  */
396 int 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 
411 int 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 
419 int 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 */
428 static 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 
515 static 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 
530 static 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 
547 static 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 */
580 static 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 
602 err:
603 	iwl_mvm_mld_sta_rm_all_sta_links(mvm, mvm_sta);
604 	return ret;
605 }
606 
607 static 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  */
622 static 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 
670 int 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 
701 	for_each_sta_active_link(vif, sta, link_sta, link_id) {
702 		struct ieee80211_bss_conf *link_conf =
703 			link_conf_dereference_protected(vif, link_id);
704 		struct iwl_mvm_link_sta *mvm_link_sta =
705 			rcu_dereference_protected(mvm_sta->link[link_id],
706 						  lockdep_is_held(&mvm->mutex));
707 
708 		if (WARN_ON(!link_conf || !mvm_link_sta)) {
709 			ret = -EINVAL;
710 			goto err;
711 		}
712 
713 		ret = iwl_mvm_mld_cfg_sta(mvm, sta, vif, link_sta, link_conf,
714 					  mvm_link_sta);
715 		if (ret)
716 			goto err;
717 
718 		link_sta_added_to_fw |= BIT(link_id);
719 
720 		if (vif->type == NL80211_IFTYPE_STATION)
721 			iwl_mvm_mld_set_ap_sta_id(sta, mvm_vif->link[link_id],
722 						  mvm_link_sta);
723 	}
724 
725 	return 0;
726 
727 err:
728 	/* remove all already allocated stations in FW */
729 	for_each_set_bit(link_id, &link_sta_added_to_fw,
730 			 IEEE80211_MLD_MAX_NUM_LINKS) {
731 		struct iwl_mvm_link_sta *mvm_link_sta =
732 			rcu_dereference_protected(mvm_sta->link[link_id],
733 						  lockdep_is_held(&mvm->mutex));
734 
735 		iwl_mvm_mld_rm_sta_from_fw(mvm, mvm_link_sta->sta_id);
736 	}
737 
738 	/* free all sta resources in the driver */
739 	iwl_mvm_mld_sta_rm_all_sta_links(mvm, mvm_sta);
740 	return ret;
741 }
742 
743 int iwl_mvm_mld_update_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
744 			   struct ieee80211_sta *sta)
745 {
746 	struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
747 	struct ieee80211_link_sta *link_sta;
748 	unsigned int link_id;
749 	int ret = -EINVAL;
750 
751 	lockdep_assert_held(&mvm->mutex);
752 
753 	for_each_sta_active_link(vif, sta, link_sta, link_id) {
754 		struct ieee80211_bss_conf *link_conf =
755 			link_conf_dereference_protected(vif, link_id);
756 		struct iwl_mvm_link_sta *mvm_link_sta =
757 			rcu_dereference_protected(mvm_sta->link[link_id],
758 						  lockdep_is_held(&mvm->mutex));
759 
760 		if (WARN_ON(!link_conf || !mvm_link_sta))
761 			return -EINVAL;
762 
763 		ret = iwl_mvm_mld_cfg_sta(mvm, sta, vif, link_sta, link_conf,
764 					  mvm_link_sta);
765 
766 		if (ret) {
767 			IWL_ERR(mvm, "Failed to update sta link %d\n", link_id);
768 			break;
769 		}
770 	}
771 
772 	return ret;
773 }
774 
775 static void iwl_mvm_mld_disable_sta_queues(struct iwl_mvm *mvm,
776 					   struct ieee80211_vif *vif,
777 					   struct ieee80211_sta *sta)
778 {
779 	struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
780 	u32 sta_mask = iwl_mvm_sta_fw_id_mask(mvm, sta, -1);
781 	int i;
782 
783 	lockdep_assert_held(&mvm->mutex);
784 
785 	for (i = 0; i < ARRAY_SIZE(mvm_sta->tid_data); i++) {
786 		if (mvm_sta->tid_data[i].txq_id == IWL_MVM_INVALID_QUEUE)
787 			continue;
788 
789 		iwl_mvm_mld_disable_txq(mvm, sta_mask,
790 					&mvm_sta->tid_data[i].txq_id, i);
791 		mvm_sta->tid_data[i].txq_id = IWL_MVM_INVALID_QUEUE;
792 	}
793 
794 	for (i = 0; i < ARRAY_SIZE(sta->txq); i++) {
795 		struct iwl_mvm_txq *mvmtxq =
796 			iwl_mvm_txq_from_mac80211(sta->txq[i]);
797 
798 		mvmtxq->txq_id = IWL_MVM_INVALID_QUEUE;
799 	}
800 }
801 
802 int iwl_mvm_mld_rm_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
803 		       struct ieee80211_sta *sta)
804 {
805 	struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
806 	struct ieee80211_link_sta *link_sta;
807 	unsigned int link_id;
808 	int ret;
809 
810 	lockdep_assert_held(&mvm->mutex);
811 
812 	/* flush its queues here since we are freeing mvm_sta */
813 	for_each_sta_active_link(vif, sta, link_sta, link_id) {
814 		struct iwl_mvm_link_sta *mvm_link_sta =
815 			rcu_dereference_protected(mvm_sta->link[link_id],
816 						  lockdep_is_held(&mvm->mutex));
817 
818 		if (WARN_ON(!mvm_link_sta))
819 			return -EINVAL;
820 
821 		ret = iwl_mvm_flush_sta_tids(mvm, mvm_link_sta->sta_id,
822 					     0xffff);
823 		if (ret)
824 			return ret;
825 	}
826 
827 	ret = iwl_mvm_wait_sta_queues_empty(mvm, mvm_sta);
828 	if (ret)
829 		return ret;
830 
831 	iwl_mvm_mld_disable_sta_queues(mvm, vif, sta);
832 
833 	for_each_sta_active_link(vif, sta, link_sta, link_id) {
834 		struct iwl_mvm_link_sta *mvm_link_sta =
835 			rcu_dereference_protected(mvm_sta->link[link_id],
836 						  lockdep_is_held(&mvm->mutex));
837 		bool stay_in_fw;
838 
839 		stay_in_fw = iwl_mvm_sta_del(mvm, vif, sta, link_sta, &ret);
840 		if (ret)
841 			break;
842 
843 		if (!stay_in_fw)
844 			ret = iwl_mvm_mld_rm_sta_from_fw(mvm,
845 							 mvm_link_sta->sta_id);
846 
847 		iwl_mvm_mld_free_sta_link(mvm, mvm_sta, mvm_link_sta,
848 					  link_id, stay_in_fw);
849 	}
850 
851 	return ret;
852 }
853 
854 int iwl_mvm_mld_rm_sta_id(struct iwl_mvm *mvm, u8 sta_id)
855 {
856 	int ret;
857 
858 	lockdep_assert_held(&mvm->mutex);
859 
860 	if (WARN_ON(sta_id == IWL_MVM_INVALID_STA))
861 		return 0;
862 
863 	ret = iwl_mvm_mld_rm_sta_from_fw(mvm, sta_id);
864 
865 	RCU_INIT_POINTER(mvm->fw_id_to_mac_id[sta_id], NULL);
866 	RCU_INIT_POINTER(mvm->fw_id_to_link_sta[sta_id], NULL);
867 	return ret;
868 }
869 
870 void iwl_mvm_mld_sta_modify_disable_tx(struct iwl_mvm *mvm,
871 				       struct iwl_mvm_sta *mvmsta,
872 				       bool disable)
873 {
874 	struct iwl_mvm_sta_disable_tx_cmd cmd;
875 	int ret;
876 
877 	cmd.sta_id = cpu_to_le32(mvmsta->deflink.sta_id);
878 	cmd.disable = cpu_to_le32(disable);
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 
889 void 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 
907 void 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 
934 static 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 
975 static 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 
1016 static 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 
1039 int 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 			    mvm_sta->link[link_id])) {
1116 			ret = -EINVAL;
1117 			goto err;
1118 		}
1119 
1120 		ret = iwl_mvm_mld_alloc_sta_link(mvm, vif, sta, link_id);
1121 		if (WARN_ON(ret))
1122 			goto err;
1123 
1124 		link_sta->agg.max_rc_amsdu_len = 1;
1125 		ieee80211_sta_recalc_aggregates(sta);
1126 
1127 		mvm_sta_link =
1128 			rcu_dereference_protected(mvm_sta->link[link_id],
1129 						  lockdep_is_held(&mvm->mutex));
1130 
1131 		if (WARN_ON(!mvm_sta_link)) {
1132 			ret = -EINVAL;
1133 			goto err;
1134 		}
1135 
1136 		if (vif->type == NL80211_IFTYPE_STATION)
1137 			iwl_mvm_mld_set_ap_sta_id(sta, mvm_vif_link,
1138 						  mvm_sta_link);
1139 
1140 		link_sta_allocated |= BIT(link_id);
1141 
1142 		sta_mask_added |= BIT(mvm_sta_link->sta_id);
1143 
1144 		ret = iwl_mvm_mld_cfg_sta(mvm, sta, vif, link_sta, link_conf,
1145 					  mvm_sta_link);
1146 		if (WARN_ON(ret))
1147 			goto err;
1148 
1149 		link_sta_added_to_fw |= BIT(link_id);
1150 
1151 		iwl_mvm_rs_add_sta_link(mvm, mvm_sta_link);
1152 	}
1153 
1154 	if (sta_mask_added) {
1155 		ret = iwl_mvm_mld_update_sta_resources(mvm, vif, sta,
1156 						       current_sta_mask,
1157 						       current_sta_mask |
1158 							sta_mask_added);
1159 		if (WARN_ON(ret))
1160 			goto err;
1161 	}
1162 
1163 	return 0;
1164 
1165 err:
1166 	/* remove all already allocated stations in FW */
1167 	for_each_set_bit(link_id, &link_sta_added_to_fw,
1168 			 IEEE80211_MLD_MAX_NUM_LINKS) {
1169 		mvm_sta_link =
1170 			rcu_dereference_protected(mvm_sta->link[link_id],
1171 						  lockdep_is_held(&mvm->mutex));
1172 
1173 		iwl_mvm_mld_rm_sta_from_fw(mvm, mvm_sta_link->sta_id);
1174 	}
1175 
1176 	/* remove all already allocated station links in driver */
1177 	for_each_set_bit(link_id, &link_sta_allocated,
1178 			 IEEE80211_MLD_MAX_NUM_LINKS) {
1179 		mvm_sta_link =
1180 			rcu_dereference_protected(mvm_sta->link[link_id],
1181 						  lockdep_is_held(&mvm->mutex));
1182 
1183 		iwl_mvm_mld_free_sta_link(mvm, mvm_sta, mvm_sta_link, link_id,
1184 					  false);
1185 	}
1186 
1187 	return ret;
1188 }
1189