xref: /linux/drivers/net/wireless/intel/iwlwifi/mvm/mld-mac80211.c (revision be239684b18e1cdcafcf8c7face4a2f562c745ad)
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 
7 static int iwl_mvm_mld_mac_add_interface(struct ieee80211_hw *hw,
8 					 struct ieee80211_vif *vif)
9 {
10 	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
11 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
12 	int ret;
13 	int i;
14 
15 	mutex_lock(&mvm->mutex);
16 
17 	mvmvif->mvm = mvm;
18 
19 	/* Not much to do here. The stack will not allow interface
20 	 * types or combinations that we didn't advertise, so we
21 	 * don't really have to check the types.
22 	 */
23 
24 	/* make sure that beacon statistics don't go backwards with FW reset */
25 	if (test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status))
26 		for_each_mvm_vif_valid_link(mvmvif, i)
27 			mvmvif->link[i]->beacon_stats.accu_num_beacons +=
28 				mvmvif->link[i]->beacon_stats.num_beacons;
29 
30 	/* Allocate resources for the MAC context, and add it to the fw  */
31 	ret = iwl_mvm_mac_ctxt_init(mvm, vif);
32 	if (ret)
33 		goto out_unlock;
34 
35 	rcu_assign_pointer(mvm->vif_id_to_mac[mvmvif->id], vif);
36 
37 	mvmvif->features |= hw->netdev_features;
38 
39 	/* reset deflink MLO parameters */
40 	mvmvif->deflink.fw_link_id = IWL_MVM_FW_LINK_ID_INVALID;
41 	mvmvif->deflink.active = 0;
42 	/* the first link always points to the default one */
43 	mvmvif->link[0] = &mvmvif->deflink;
44 
45 	ret = iwl_mvm_mld_mac_ctxt_add(mvm, vif);
46 	if (ret)
47 		goto out_unlock;
48 
49 	/* beacon filtering */
50 	ret = iwl_mvm_disable_beacon_filter(mvm, vif, 0);
51 	if (ret)
52 		goto out_remove_mac;
53 
54 	if (!mvm->bf_allowed_vif &&
55 	    vif->type == NL80211_IFTYPE_STATION && !vif->p2p) {
56 		mvm->bf_allowed_vif = mvmvif;
57 		vif->driver_flags |= IEEE80211_VIF_BEACON_FILTER |
58 				     IEEE80211_VIF_SUPPORTS_CQM_RSSI;
59 	}
60 
61 	ret = iwl_mvm_add_link(mvm, vif, &vif->bss_conf);
62 	if (ret)
63 		goto out_free_bf;
64 
65 	/* Save a pointer to p2p device vif, so it can later be used to
66 	 * update the p2p device MAC when a GO is started/stopped
67 	 */
68 	if (vif->type == NL80211_IFTYPE_P2P_DEVICE)
69 		mvm->p2p_device_vif = vif;
70 
71 	ret = iwl_mvm_power_update_mac(mvm);
72 	if (ret)
73 		goto out_free_bf;
74 
75 	iwl_mvm_tcm_add_vif(mvm, vif);
76 	INIT_DELAYED_WORK(&mvmvif->csa_work,
77 			  iwl_mvm_channel_switch_disconnect_wk);
78 
79 	if (vif->type == NL80211_IFTYPE_MONITOR) {
80 		mvm->monitor_on = true;
81 		ieee80211_hw_set(mvm->hw, RX_INCLUDES_FCS);
82 	}
83 
84 	if (!test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status))
85 		iwl_mvm_vif_dbgfs_add_link(mvm, vif);
86 
87 	if (!test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status) &&
88 	    vif->type == NL80211_IFTYPE_STATION && !vif->p2p &&
89 	    !mvm->csme_vif && mvm->mei_registered) {
90 		iwl_mei_set_nic_info(vif->addr, mvm->nvm_data->hw_addr);
91 		iwl_mei_set_netdev(ieee80211_vif_to_wdev(vif)->netdev);
92 		mvm->csme_vif = vif;
93 	}
94 
95 	goto out_unlock;
96 
97  out_free_bf:
98 	if (mvm->bf_allowed_vif == mvmvif) {
99 		mvm->bf_allowed_vif = NULL;
100 		vif->driver_flags &= ~(IEEE80211_VIF_BEACON_FILTER |
101 				       IEEE80211_VIF_SUPPORTS_CQM_RSSI);
102 	}
103  out_remove_mac:
104 	mvmvif->link[0] = NULL;
105 	iwl_mvm_mld_mac_ctxt_remove(mvm, vif);
106  out_unlock:
107 	mutex_unlock(&mvm->mutex);
108 
109 	return ret;
110 }
111 
112 static void iwl_mvm_mld_mac_remove_interface(struct ieee80211_hw *hw,
113 					     struct ieee80211_vif *vif)
114 {
115 	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
116 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
117 	struct iwl_probe_resp_data *probe_data;
118 
119 	iwl_mvm_prepare_mac_removal(mvm, vif);
120 
121 	if (!(vif->type == NL80211_IFTYPE_AP ||
122 	      vif->type == NL80211_IFTYPE_ADHOC))
123 		iwl_mvm_tcm_rm_vif(mvm, vif);
124 
125 	mutex_lock(&mvm->mutex);
126 
127 	if (vif == mvm->csme_vif) {
128 		iwl_mei_set_netdev(NULL);
129 		mvm->csme_vif = NULL;
130 	}
131 
132 	if (mvm->bf_allowed_vif == mvmvif) {
133 		mvm->bf_allowed_vif = NULL;
134 		vif->driver_flags &= ~(IEEE80211_VIF_BEACON_FILTER |
135 				       IEEE80211_VIF_SUPPORTS_CQM_RSSI);
136 	}
137 
138 	if (vif->bss_conf.ftm_responder)
139 		memset(&mvm->ftm_resp_stats, 0, sizeof(mvm->ftm_resp_stats));
140 
141 	iwl_mvm_vif_dbgfs_rm_link(mvm, vif);
142 
143 	/* For AP/GO interface, the tear down of the resources allocated to the
144 	 * interface is be handled as part of the stop_ap flow.
145 	 */
146 	if (vif->type == NL80211_IFTYPE_AP ||
147 	    vif->type == NL80211_IFTYPE_ADHOC) {
148 #ifdef CONFIG_NL80211_TESTMODE
149 		if (vif == mvm->noa_vif) {
150 			mvm->noa_vif = NULL;
151 			mvm->noa_duration = 0;
152 		}
153 #endif
154 	}
155 
156 	iwl_mvm_power_update_mac(mvm);
157 
158 	/* Before the interface removal, mac80211 would cancel the ROC, and the
159 	 * ROC worker would be scheduled if needed. The worker would be flushed
160 	 * in iwl_mvm_prepare_mac_removal() and thus at this point the link is
161 	 * not active. So need only to remove the link.
162 	 */
163 	if (vif->type == NL80211_IFTYPE_P2P_DEVICE) {
164 		if (mvmvif->deflink.phy_ctxt) {
165 			iwl_mvm_phy_ctxt_unref(mvm, mvmvif->deflink.phy_ctxt);
166 			mvmvif->deflink.phy_ctxt = NULL;
167 		}
168 		mvm->p2p_device_vif = NULL;
169 		iwl_mvm_remove_link(mvm, vif, &vif->bss_conf);
170 	} else {
171 		iwl_mvm_disable_link(mvm, vif, &vif->bss_conf);
172 	}
173 
174 	iwl_mvm_mld_mac_ctxt_remove(mvm, vif);
175 
176 	RCU_INIT_POINTER(mvm->vif_id_to_mac[mvmvif->id], NULL);
177 
178 	probe_data = rcu_dereference_protected(mvmvif->deflink.probe_resp_data,
179 					       lockdep_is_held(&mvm->mutex));
180 	RCU_INIT_POINTER(mvmvif->deflink.probe_resp_data, NULL);
181 	if (probe_data)
182 		kfree_rcu(probe_data, rcu_head);
183 
184 	if (vif->type == NL80211_IFTYPE_MONITOR) {
185 		mvm->monitor_on = false;
186 		__clear_bit(IEEE80211_HW_RX_INCLUDES_FCS, mvm->hw->flags);
187 	}
188 
189 	mutex_unlock(&mvm->mutex);
190 }
191 
192 static unsigned int iwl_mvm_mld_count_active_links(struct ieee80211_vif *vif)
193 {
194 	unsigned int n_active = 0;
195 	int i;
196 
197 	for (i = 0; i < IEEE80211_MLD_MAX_NUM_LINKS; i++) {
198 		struct ieee80211_bss_conf *link_conf;
199 
200 		link_conf = link_conf_dereference_protected(vif, i);
201 		if (link_conf &&
202 		    rcu_access_pointer(link_conf->chanctx_conf))
203 			n_active++;
204 	}
205 
206 	return n_active;
207 }
208 
209 static int iwl_mvm_esr_mode_active(struct iwl_mvm *mvm,
210 				   struct ieee80211_vif *vif)
211 {
212 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
213 	int link_id, ret = 0;
214 
215 	mvmvif->esr_active = true;
216 
217 	/* Indicate to mac80211 that EML is enabled */
218 	vif->driver_flags |= IEEE80211_VIF_EML_ACTIVE;
219 
220 	iwl_mvm_update_smps_on_active_links(mvm, vif, IWL_MVM_SMPS_REQ_FW,
221 					    IEEE80211_SMPS_OFF);
222 
223 	for_each_mvm_vif_valid_link(mvmvif, link_id) {
224 		struct iwl_mvm_vif_link_info *link = mvmvif->link[link_id];
225 
226 		if (!link->phy_ctxt)
227 			continue;
228 
229 		ret = iwl_mvm_phy_send_rlc(mvm, link->phy_ctxt, 2, 2);
230 		if (ret)
231 			break;
232 
233 		link->phy_ctxt->rlc_disabled = true;
234 	}
235 
236 	return ret;
237 }
238 
239 static int
240 __iwl_mvm_mld_assign_vif_chanctx(struct iwl_mvm *mvm,
241 				 struct ieee80211_vif *vif,
242 				 struct ieee80211_bss_conf *link_conf,
243 				 struct ieee80211_chanctx_conf *ctx,
244 				 bool switching_chanctx)
245 {
246 	u16 *phy_ctxt_id = (u16 *)ctx->drv_priv;
247 	struct iwl_mvm_phy_ctxt *phy_ctxt = &mvm->phy_ctxts[*phy_ctxt_id];
248 	unsigned int n_active = iwl_mvm_mld_count_active_links(vif);
249 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
250 	unsigned int link_id = link_conf->link_id;
251 	int ret;
252 
253 	/* if the assigned one was not counted yet, count it now */
254 	if (!rcu_access_pointer(link_conf->chanctx_conf))
255 		n_active++;
256 
257 	if (n_active > iwl_mvm_max_active_links(mvm, vif))
258 		return -EOPNOTSUPP;
259 
260 	if (WARN_ON_ONCE(!mvmvif->link[link_id]))
261 		return -EINVAL;
262 
263 	/* mac parameters such as HE support can change at this stage
264 	 * For sta, need first to configure correct state from drv_sta_state
265 	 * and only after that update mac config.
266 	 */
267 	if (vif->type == NL80211_IFTYPE_AP) {
268 		ret = iwl_mvm_mld_mac_ctxt_changed(mvm, vif, false);
269 		if (ret) {
270 			IWL_ERR(mvm, "failed to update MAC %pM\n", vif->addr);
271 			return -EINVAL;
272 		}
273 	}
274 
275 	mvmvif->link[link_id]->phy_ctxt = phy_ctxt;
276 
277 	if (iwl_mvm_is_esr_supported(mvm->fwrt.trans) && n_active > 1) {
278 		mvmvif->link[link_id]->listen_lmac = true;
279 		ret = iwl_mvm_esr_mode_active(mvm, vif);
280 		if (ret) {
281 			IWL_ERR(mvm, "failed to activate ESR mode (%d)\n", ret);
282 			goto out;
283 		}
284 	}
285 
286 	if (switching_chanctx) {
287 		/* reactivate if we turned this off during channel switch */
288 		if (vif->type == NL80211_IFTYPE_AP)
289 			mvmvif->ap_ibss_active = true;
290 	}
291 
292 	/* send it first with phy context ID */
293 	ret = iwl_mvm_link_changed(mvm, vif, link_conf, 0, false);
294 	if (ret)
295 		goto out;
296 
297 	/* Initialize rate control for the AP station, since we might be
298 	 * doing a link switch here - we cannot initialize it before since
299 	 * this needs the phy context assigned (and in FW?), and we cannot
300 	 * do it later because it needs to be initialized as soon as we're
301 	 * able to TX on the link, i.e. when active.
302 	 *
303 	 * Firmware restart isn't quite correct yet for MLO, but we don't
304 	 * need to do it in that case anyway since it will happen from the
305 	 * normal station state callback.
306 	 */
307 	if (mvmvif->ap_sta &&
308 	    !test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status)) {
309 		struct ieee80211_link_sta *link_sta;
310 
311 		rcu_read_lock();
312 		link_sta = rcu_dereference(mvmvif->ap_sta->link[link_id]);
313 
314 		if (!WARN_ON_ONCE(!link_sta))
315 			iwl_mvm_rs_rate_init(mvm, vif, mvmvif->ap_sta,
316 					     link_conf, link_sta,
317 					     phy_ctxt->channel->band);
318 		rcu_read_unlock();
319 	}
320 
321 	/* then activate */
322 	ret = iwl_mvm_link_changed(mvm, vif, link_conf,
323 				   LINK_CONTEXT_MODIFY_ACTIVE |
324 				   LINK_CONTEXT_MODIFY_RATES_INFO,
325 				   true);
326 	if (ret)
327 		goto out;
328 
329 	/*
330 	 * Power state must be updated before quotas,
331 	 * otherwise fw will complain.
332 	 */
333 	iwl_mvm_power_update_mac(mvm);
334 
335 	if (vif->type == NL80211_IFTYPE_MONITOR) {
336 		ret = iwl_mvm_mld_add_snif_sta(mvm, vif, link_conf);
337 		if (ret)
338 			goto deactivate;
339 	}
340 
341 	return 0;
342 
343 deactivate:
344 	iwl_mvm_link_changed(mvm, vif, link_conf, LINK_CONTEXT_MODIFY_ACTIVE,
345 			     false);
346 out:
347 	mvmvif->link[link_id]->phy_ctxt = NULL;
348 	iwl_mvm_power_update_mac(mvm);
349 	return ret;
350 }
351 
352 static int iwl_mvm_mld_assign_vif_chanctx(struct ieee80211_hw *hw,
353 					  struct ieee80211_vif *vif,
354 					  struct ieee80211_bss_conf *link_conf,
355 					  struct ieee80211_chanctx_conf *ctx)
356 {
357 	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
358 	int ret;
359 
360 	mutex_lock(&mvm->mutex);
361 	ret = __iwl_mvm_mld_assign_vif_chanctx(mvm, vif, link_conf, ctx, false);
362 	mutex_unlock(&mvm->mutex);
363 
364 	return ret;
365 }
366 
367 static int iwl_mvm_esr_mode_inactive(struct iwl_mvm *mvm,
368 				     struct ieee80211_vif *vif)
369 {
370 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
371 	struct ieee80211_bss_conf *link_conf;
372 	int link_id, ret = 0;
373 
374 	mvmvif->esr_active = false;
375 
376 	vif->driver_flags &= ~IEEE80211_VIF_EML_ACTIVE;
377 
378 	iwl_mvm_update_smps_on_active_links(mvm, vif, IWL_MVM_SMPS_REQ_FW,
379 					    IEEE80211_SMPS_AUTOMATIC);
380 
381 	for_each_vif_active_link(vif, link_conf, link_id) {
382 		struct ieee80211_chanctx_conf *chanctx_conf;
383 		struct iwl_mvm_phy_ctxt *phy_ctxt;
384 		u8 static_chains, dynamic_chains;
385 
386 		mvmvif->link[link_id]->listen_lmac = false;
387 
388 		rcu_read_lock();
389 
390 		chanctx_conf = rcu_dereference(link_conf->chanctx_conf);
391 		phy_ctxt = mvmvif->link[link_id]->phy_ctxt;
392 
393 		if (!chanctx_conf || !phy_ctxt) {
394 			rcu_read_unlock();
395 			continue;
396 		}
397 
398 		phy_ctxt->rlc_disabled = false;
399 		static_chains = chanctx_conf->rx_chains_static;
400 		dynamic_chains = chanctx_conf->rx_chains_dynamic;
401 
402 		rcu_read_unlock();
403 
404 		ret = iwl_mvm_phy_send_rlc(mvm, phy_ctxt, static_chains,
405 					   dynamic_chains);
406 		if (ret)
407 			break;
408 	}
409 
410 	return ret;
411 }
412 
413 static void
414 __iwl_mvm_mld_unassign_vif_chanctx(struct iwl_mvm *mvm,
415 				   struct ieee80211_vif *vif,
416 				   struct ieee80211_bss_conf *link_conf,
417 				   struct ieee80211_chanctx_conf *ctx,
418 				   bool switching_chanctx)
419 
420 {
421 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
422 	unsigned int n_active = iwl_mvm_mld_count_active_links(vif);
423 	unsigned int link_id = link_conf->link_id;
424 
425 	/* shouldn't happen, but verify link_id is valid before accessing */
426 	if (WARN_ON_ONCE(!mvmvif->link[link_id]))
427 		return;
428 
429 	if (vif->type == NL80211_IFTYPE_AP && switching_chanctx) {
430 		mvmvif->csa_countdown = false;
431 
432 		/* Set CS bit on all the stations */
433 		iwl_mvm_modify_all_sta_disable_tx(mvm, mvmvif, true);
434 
435 		/* Save blocked iface, the timeout is set on the next beacon */
436 		rcu_assign_pointer(mvm->csa_tx_blocked_vif, vif);
437 
438 		mvmvif->ap_ibss_active = false;
439 	}
440 
441 	iwl_mvm_link_changed(mvm, vif, link_conf,
442 			     LINK_CONTEXT_MODIFY_ACTIVE, false);
443 
444 	if (iwl_mvm_is_esr_supported(mvm->fwrt.trans) && n_active > 1) {
445 		int ret = iwl_mvm_esr_mode_inactive(mvm, vif);
446 
447 		if (ret)
448 			IWL_ERR(mvm, "failed to deactivate ESR mode (%d)\n",
449 				ret);
450 	}
451 
452 	if (vif->type == NL80211_IFTYPE_MONITOR)
453 		iwl_mvm_mld_rm_snif_sta(mvm, vif);
454 
455 	if (switching_chanctx)
456 		return;
457 	mvmvif->link[link_id]->phy_ctxt = NULL;
458 	iwl_mvm_power_update_mac(mvm);
459 }
460 
461 static void iwl_mvm_mld_unassign_vif_chanctx(struct ieee80211_hw *hw,
462 					     struct ieee80211_vif *vif,
463 					     struct ieee80211_bss_conf *link_conf,
464 					     struct ieee80211_chanctx_conf *ctx)
465 {
466 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
467 	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
468 
469 	mutex_lock(&mvm->mutex);
470 	__iwl_mvm_mld_unassign_vif_chanctx(mvm, vif, link_conf, ctx, false);
471 	/* in the non-MLD case, remove/re-add the link to clean up FW state */
472 	if (!ieee80211_vif_is_mld(vif) && !mvmvif->ap_sta &&
473 	    !WARN_ON_ONCE(vif->cfg.assoc)) {
474 		iwl_mvm_remove_link(mvm, vif, link_conf);
475 		iwl_mvm_add_link(mvm, vif, link_conf);
476 	}
477 	mutex_unlock(&mvm->mutex);
478 }
479 
480 static int iwl_mvm_mld_start_ap_ibss(struct ieee80211_hw *hw,
481 				     struct ieee80211_vif *vif,
482 				     struct ieee80211_bss_conf *link_conf)
483 {
484 	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
485 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
486 	int ret;
487 
488 	mutex_lock(&mvm->mutex);
489 	/* Send the beacon template */
490 	ret = iwl_mvm_mac_ctxt_beacon_changed(mvm, vif, link_conf);
491 	if (ret)
492 		goto out_unlock;
493 
494 	/* the link should be already activated when assigning chan context */
495 	ret = iwl_mvm_link_changed(mvm, vif, link_conf,
496 				   LINK_CONTEXT_MODIFY_ALL &
497 				   ~LINK_CONTEXT_MODIFY_ACTIVE,
498 				   true);
499 	if (ret)
500 		goto out_unlock;
501 
502 	ret = iwl_mvm_mld_add_mcast_sta(mvm, vif, link_conf);
503 	if (ret)
504 		goto out_unlock;
505 
506 	/* Send the bcast station. At this stage the TBTT and DTIM time
507 	 * events are added and applied to the scheduler
508 	 */
509 	ret = iwl_mvm_mld_add_bcast_sta(mvm, vif, link_conf);
510 	if (ret)
511 		goto out_rm_mcast;
512 
513 	if (iwl_mvm_start_ap_ibss_common(hw, vif, &ret))
514 		goto out_failed;
515 
516 	/* Need to update the P2P Device MAC (only GO, IBSS is single vif) */
517 	if (vif->p2p && mvm->p2p_device_vif)
518 		iwl_mvm_mld_mac_ctxt_changed(mvm, mvm->p2p_device_vif, false);
519 
520 	iwl_mvm_bt_coex_vif_change(mvm);
521 
522 	/* we don't support TDLS during DCM */
523 	if (iwl_mvm_phy_ctx_count(mvm) > 1)
524 		iwl_mvm_teardown_tdls_peers(mvm);
525 
526 	iwl_mvm_ftm_restart_responder(mvm, vif, link_conf);
527 
528 	goto out_unlock;
529 
530 out_failed:
531 	iwl_mvm_power_update_mac(mvm);
532 	mvmvif->ap_ibss_active = false;
533 	iwl_mvm_mld_rm_bcast_sta(mvm, vif, link_conf);
534 out_rm_mcast:
535 	iwl_mvm_mld_rm_mcast_sta(mvm, vif, link_conf);
536 out_unlock:
537 	mutex_unlock(&mvm->mutex);
538 	return ret;
539 }
540 
541 static int iwl_mvm_mld_start_ap(struct ieee80211_hw *hw,
542 				struct ieee80211_vif *vif,
543 				struct ieee80211_bss_conf *link_conf)
544 {
545 	return iwl_mvm_mld_start_ap_ibss(hw, vif, link_conf);
546 }
547 
548 static int iwl_mvm_mld_start_ibss(struct ieee80211_hw *hw,
549 				  struct ieee80211_vif *vif)
550 {
551 	return iwl_mvm_mld_start_ap_ibss(hw, vif, &vif->bss_conf);
552 }
553 
554 static void iwl_mvm_mld_stop_ap_ibss(struct ieee80211_hw *hw,
555 				     struct ieee80211_vif *vif,
556 				     struct ieee80211_bss_conf *link_conf)
557 {
558 	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
559 
560 	mutex_lock(&mvm->mutex);
561 
562 	iwl_mvm_stop_ap_ibss_common(mvm, vif);
563 
564 	/* Need to update the P2P Device MAC (only GO, IBSS is single vif) */
565 	if (vif->p2p && mvm->p2p_device_vif)
566 		iwl_mvm_mld_mac_ctxt_changed(mvm, mvm->p2p_device_vif, false);
567 
568 	iwl_mvm_ftm_responder_clear(mvm, vif);
569 
570 	iwl_mvm_mld_rm_bcast_sta(mvm, vif, link_conf);
571 	iwl_mvm_mld_rm_mcast_sta(mvm, vif, link_conf);
572 
573 	iwl_mvm_power_update_mac(mvm);
574 	mutex_unlock(&mvm->mutex);
575 }
576 
577 static void iwl_mvm_mld_stop_ap(struct ieee80211_hw *hw,
578 				struct ieee80211_vif *vif,
579 				struct ieee80211_bss_conf *link_conf)
580 {
581 	iwl_mvm_mld_stop_ap_ibss(hw, vif, link_conf);
582 }
583 
584 static void iwl_mvm_mld_stop_ibss(struct ieee80211_hw *hw,
585 				  struct ieee80211_vif *vif)
586 {
587 	iwl_mvm_mld_stop_ap_ibss(hw, vif, &vif->bss_conf);
588 }
589 
590 static int iwl_mvm_mld_mac_sta_state(struct ieee80211_hw *hw,
591 				     struct ieee80211_vif *vif,
592 				     struct ieee80211_sta *sta,
593 				     enum ieee80211_sta_state old_state,
594 				     enum ieee80211_sta_state new_state)
595 {
596 	static const struct iwl_mvm_sta_state_ops callbacks = {
597 		.add_sta = iwl_mvm_mld_add_sta,
598 		.update_sta = iwl_mvm_mld_update_sta,
599 		.rm_sta = iwl_mvm_mld_rm_sta,
600 		.mac_ctxt_changed = iwl_mvm_mld_mac_ctxt_changed,
601 	};
602 
603 	return iwl_mvm_mac_sta_state_common(hw, vif, sta, old_state, new_state,
604 					    &callbacks);
605 }
606 
607 struct iwl_mvm_link_sel_data {
608 	u8 link_id;
609 	enum nl80211_band band;
610 	bool active;
611 };
612 
613 static bool iwl_mvm_mld_valid_link_pair(struct iwl_mvm_link_sel_data *a,
614 					struct iwl_mvm_link_sel_data *b)
615 {
616 	return a->band != b->band;
617 }
618 
619 void iwl_mvm_mld_select_links(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
620 			      bool valid_links_changed)
621 {
622 	struct iwl_mvm_link_sel_data data[IEEE80211_MLD_MAX_NUM_LINKS];
623 	unsigned long usable_links = ieee80211_vif_usable_links(vif);
624 	u32 max_active_links = iwl_mvm_max_active_links(mvm, vif);
625 	u16 new_active_links;
626 	u8 link_id, n_data = 0, i, j;
627 
628 	if (!IWL_MVM_AUTO_EML_ENABLE)
629 		return;
630 
631 	if (!ieee80211_vif_is_mld(vif) || usable_links == 1)
632 		return;
633 
634 	/* The logic below is a simple version that doesn't suit more than 2
635 	 * links
636 	 */
637 	WARN_ON_ONCE(max_active_links > 2);
638 
639 	/* if only a single active link is supported, assume that the one
640 	 * selected by higher layer for connection establishment is the best.
641 	 */
642 	if (max_active_links == 1 && !valid_links_changed)
643 		return;
644 
645 	/* If we are already using the maximal number of active links, don't do
646 	 * any change. This can later be optimized to pick a 'better' link pair.
647 	 */
648 	if (hweight16(vif->active_links) == max_active_links)
649 		return;
650 
651 	rcu_read_lock();
652 
653 	for_each_set_bit(link_id, &usable_links, IEEE80211_MLD_MAX_NUM_LINKS) {
654 		struct ieee80211_bss_conf *link_conf =
655 			rcu_dereference(vif->link_conf[link_id]);
656 
657 		if (WARN_ON_ONCE(!link_conf))
658 			continue;
659 
660 		data[n_data].link_id = link_id;
661 		data[n_data].band = link_conf->chandef.chan->band;
662 		data[n_data].active = vif->active_links & BIT(link_id);
663 		n_data++;
664 	}
665 
666 	rcu_read_unlock();
667 
668 	/* this is expected to be the current active link */
669 	if (n_data == 1)
670 		return;
671 
672 	new_active_links = 0;
673 
674 	/* Assume that after association only a single link is active, thus,
675 	 * select only the 2nd link
676 	 */
677 	if (!valid_links_changed) {
678 		for (i = 0; i < n_data; i++) {
679 			if (data[i].active)
680 				break;
681 		}
682 
683 		if (WARN_ON_ONCE(i == n_data))
684 			return;
685 
686 		for (j = 0; j < n_data; j++) {
687 			if (i == j)
688 				continue;
689 
690 			if (iwl_mvm_mld_valid_link_pair(&data[i], &data[j]))
691 				break;
692 		}
693 
694 		if (j != n_data)
695 			new_active_links = BIT(data[i].link_id) |
696 				BIT(data[j].link_id);
697 	} else {
698 		/* Try to find a valid link pair for EMLSR operation. If a pair
699 		 * is not found continue using the current active link.
700 		 */
701 		for (i = 0; i < n_data; i++) {
702 			for (j = 0; j < n_data; j++) {
703 				if (i == j)
704 					continue;
705 
706 				if (iwl_mvm_mld_valid_link_pair(&data[i],
707 								&data[j]))
708 					break;
709 			}
710 
711 			/* found a valid pair for EMLSR, use it */
712 			if (j != n_data) {
713 				new_active_links = BIT(data[i].link_id) |
714 					BIT(data[j].link_id);
715 				break;
716 			}
717 		}
718 	}
719 
720 	if (!new_active_links)
721 		return;
722 
723 	if (vif->active_links != new_active_links)
724 		ieee80211_set_active_links_async(vif, new_active_links);
725 }
726 
727 static void
728 iwl_mvm_mld_link_info_changed_station(struct iwl_mvm *mvm,
729 				      struct ieee80211_vif *vif,
730 				      struct ieee80211_bss_conf *link_conf,
731 				      u64 changes)
732 {
733 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
734 	bool has_he, has_eht;
735 	u32 link_changes = 0;
736 	int ret;
737 
738 	if (WARN_ON_ONCE(!mvmvif->link[link_conf->link_id]))
739 		return;
740 
741 	has_he = link_conf->he_support && !iwlwifi_mod_params.disable_11ax;
742 	has_eht = link_conf->eht_support && !iwlwifi_mod_params.disable_11be;
743 
744 	/* Update EDCA params */
745 	if (changes & BSS_CHANGED_QOS && vif->cfg.assoc && link_conf->qos)
746 		link_changes |= LINK_CONTEXT_MODIFY_QOS_PARAMS;
747 
748 	if (changes & BSS_CHANGED_ERP_SLOT)
749 		link_changes |= LINK_CONTEXT_MODIFY_RATES_INFO;
750 
751 	if (vif->cfg.assoc && (has_he || has_eht)) {
752 		IWL_DEBUG_MAC80211(mvm, "Associated in HE mode\n");
753 		link_changes |= LINK_CONTEXT_MODIFY_HE_PARAMS;
754 	}
755 
756 	/* Update EHT Puncturing info */
757 	if (changes & BSS_CHANGED_EHT_PUNCTURING && vif->cfg.assoc)
758 		link_changes |= LINK_CONTEXT_MODIFY_EHT_PARAMS;
759 
760 	if (link_changes) {
761 		ret = iwl_mvm_link_changed(mvm, vif, link_conf, link_changes,
762 					   true);
763 		if (ret)
764 			IWL_ERR(mvm, "failed to update link\n");
765 	}
766 
767 	ret = iwl_mvm_mld_mac_ctxt_changed(mvm, vif, false);
768 	if (ret)
769 		IWL_ERR(mvm, "failed to update MAC %pM\n", vif->addr);
770 
771 	if (changes & BSS_CHANGED_MLD_VALID_LINKS)
772 		iwl_mvm_mld_select_links(mvm, vif, true);
773 
774 	memcpy(mvmvif->link[link_conf->link_id]->bssid, link_conf->bssid,
775 	       ETH_ALEN);
776 
777 	iwl_mvm_bss_info_changed_station_common(mvm, vif, link_conf, changes);
778 }
779 
780 static bool iwl_mvm_mld_vif_have_valid_ap_sta(struct iwl_mvm_vif *mvmvif)
781 {
782 	int i;
783 
784 	for_each_mvm_vif_valid_link(mvmvif, i) {
785 		if (mvmvif->link[i]->ap_sta_id != IWL_MVM_INVALID_STA)
786 			return true;
787 	}
788 
789 	return false;
790 }
791 
792 static void iwl_mvm_mld_vif_delete_all_stas(struct iwl_mvm *mvm,
793 					    struct ieee80211_vif *vif)
794 {
795 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
796 	int i, ret;
797 
798 	if (test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status))
799 		return;
800 
801 	for_each_mvm_vif_valid_link(mvmvif, i) {
802 		struct iwl_mvm_vif_link_info *link = mvmvif->link[i];
803 
804 		if (!link)
805 			continue;
806 
807 		iwl_mvm_sec_key_remove_ap(mvm, vif, link, i);
808 		ret = iwl_mvm_mld_rm_sta_id(mvm, link->ap_sta_id);
809 		if (ret)
810 			IWL_ERR(mvm, "failed to remove AP station\n");
811 
812 		link->ap_sta_id = IWL_MVM_INVALID_STA;
813 	}
814 }
815 
816 static void iwl_mvm_mld_vif_cfg_changed_station(struct iwl_mvm *mvm,
817 						struct ieee80211_vif *vif,
818 						u64 changes)
819 {
820 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
821 	struct ieee80211_bss_conf *link_conf;
822 	bool protect = false;
823 	unsigned int i;
824 	int ret;
825 
826 	/* This might get called without active links during the
827 	 * chanctx switch, but we don't care about it anyway.
828 	 */
829 	if (changes == BSS_CHANGED_IDLE)
830 		return;
831 
832 	ret = iwl_mvm_mld_mac_ctxt_changed(mvm, vif, false);
833 	if (ret)
834 		IWL_ERR(mvm, "failed to update MAC %pM\n", vif->addr);
835 
836 	mvmvif->associated = vif->cfg.assoc;
837 
838 	if (changes & BSS_CHANGED_ASSOC) {
839 		if (vif->cfg.assoc) {
840 			/* clear statistics to get clean beacon counter */
841 			iwl_mvm_request_statistics(mvm, true);
842 			iwl_mvm_sf_update(mvm, vif, false);
843 			iwl_mvm_power_vif_assoc(mvm, vif);
844 
845 			for_each_mvm_vif_valid_link(mvmvif, i) {
846 				memset(&mvmvif->link[i]->beacon_stats, 0,
847 				       sizeof(mvmvif->link[i]->beacon_stats));
848 
849 				if (vif->p2p) {
850 					iwl_mvm_update_smps(mvm, vif,
851 							    IWL_MVM_SMPS_REQ_PROT,
852 							    IEEE80211_SMPS_DYNAMIC, i);
853 				}
854 
855 				rcu_read_lock();
856 				link_conf = rcu_dereference(vif->link_conf[i]);
857 				if (link_conf && !link_conf->dtim_period)
858 					protect = true;
859 				rcu_read_unlock();
860 			}
861 
862 			if (!test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status) &&
863 			    protect) {
864 				/* We are in assoc so only one link is active-
865 				 * The association link
866 				 */
867 				unsigned int link_id =
868 					ffs(vif->active_links) - 1;
869 
870 				/* If we're not restarting and still haven't
871 				 * heard a beacon (dtim period unknown) then
872 				 * make sure we still have enough minimum time
873 				 * remaining in the time event, since the auth
874 				 * might actually have taken quite a while
875 				 * (especially for SAE) and so the remaining
876 				 * time could be small without us having heard
877 				 * a beacon yet.
878 				 */
879 				iwl_mvm_protect_assoc(mvm, vif, 0, link_id);
880 			}
881 
882 			iwl_mvm_sf_update(mvm, vif, false);
883 
884 			/* FIXME: need to decide about misbehaving AP handling */
885 			iwl_mvm_power_vif_assoc(mvm, vif);
886 		} else if (iwl_mvm_mld_vif_have_valid_ap_sta(mvmvif)) {
887 			iwl_mvm_mei_host_disassociated(mvm);
888 
889 			/* If update fails - SF might be running in associated
890 			 * mode while disassociated - which is forbidden.
891 			 */
892 			ret = iwl_mvm_sf_update(mvm, vif, false);
893 			WARN_ONCE(ret &&
894 				  !test_bit(IWL_MVM_STATUS_HW_RESTART_REQUESTED,
895 					    &mvm->status),
896 				  "Failed to update SF upon disassociation\n");
897 
898 			/* If we get an assert during the connection (after the
899 			 * station has been added, but before the vif is set
900 			 * to associated), mac80211 will re-add the station and
901 			 * then configure the vif. Since the vif is not
902 			 * associated, we would remove the station here and
903 			 * this would fail the recovery.
904 			 */
905 			iwl_mvm_mld_vif_delete_all_stas(mvm, vif);
906 		}
907 
908 		iwl_mvm_bss_info_changed_station_assoc(mvm, vif, changes);
909 	}
910 
911 	if (changes & BSS_CHANGED_PS) {
912 		ret = iwl_mvm_power_update_mac(mvm);
913 		if (ret)
914 			IWL_ERR(mvm, "failed to update power mode\n");
915 	}
916 }
917 
918 static void
919 iwl_mvm_mld_link_info_changed_ap_ibss(struct iwl_mvm *mvm,
920 				      struct ieee80211_vif *vif,
921 				      struct ieee80211_bss_conf *link_conf,
922 				      u64 changes)
923 {
924 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
925 	u32 link_changes = LINK_CONTEXT_MODIFY_PROTECT_FLAGS |
926 			   LINK_CONTEXT_MODIFY_QOS_PARAMS;
927 
928 	/* Changes will be applied when the AP/IBSS is started */
929 	if (!mvmvif->ap_ibss_active)
930 		return;
931 
932 	if (link_conf->he_support)
933 		link_changes |= LINK_CONTEXT_MODIFY_HE_PARAMS;
934 
935 	if (changes & BSS_CHANGED_ERP_SLOT)
936 		link_changes |= LINK_CONTEXT_MODIFY_RATES_INFO;
937 
938 	if (changes & (BSS_CHANGED_ERP_CTS_PROT | BSS_CHANGED_ERP_SLOT |
939 		       BSS_CHANGED_HT |
940 		       BSS_CHANGED_BANDWIDTH | BSS_CHANGED_QOS |
941 		       BSS_CHANGED_HE_BSS_COLOR) &&
942 		       iwl_mvm_link_changed(mvm, vif, link_conf,
943 					    link_changes, true))
944 		IWL_ERR(mvm, "failed to update MAC %pM\n", vif->addr);
945 
946 	/* Need to send a new beacon template to the FW */
947 	if (changes & BSS_CHANGED_BEACON &&
948 	    iwl_mvm_mac_ctxt_beacon_changed(mvm, vif, link_conf))
949 		IWL_WARN(mvm, "Failed updating beacon data\n");
950 
951 	/* FIXME: need to decide if we need FTM responder per link */
952 	if (changes & BSS_CHANGED_FTM_RESPONDER) {
953 		int ret = iwl_mvm_ftm_start_responder(mvm, vif, link_conf);
954 
955 		if (ret)
956 			IWL_WARN(mvm, "Failed to enable FTM responder (%d)\n",
957 				 ret);
958 	}
959 }
960 
961 static void iwl_mvm_mld_link_info_changed(struct ieee80211_hw *hw,
962 					  struct ieee80211_vif *vif,
963 					  struct ieee80211_bss_conf *link_conf,
964 					  u64 changes)
965 {
966 	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
967 
968 	mutex_lock(&mvm->mutex);
969 
970 	switch (vif->type) {
971 	case NL80211_IFTYPE_STATION:
972 		iwl_mvm_mld_link_info_changed_station(mvm, vif, link_conf,
973 						      changes);
974 		break;
975 	case NL80211_IFTYPE_AP:
976 	case NL80211_IFTYPE_ADHOC:
977 		iwl_mvm_mld_link_info_changed_ap_ibss(mvm, vif, link_conf,
978 						      changes);
979 		break;
980 	case NL80211_IFTYPE_MONITOR:
981 		if (changes & BSS_CHANGED_MU_GROUPS)
982 			iwl_mvm_update_mu_groups(mvm, vif);
983 		break;
984 	default:
985 		/* shouldn't happen */
986 		WARN_ON_ONCE(1);
987 	}
988 
989 	if (changes & BSS_CHANGED_TXPOWER) {
990 		IWL_DEBUG_CALIB(mvm, "Changing TX Power to %d dBm\n",
991 				link_conf->txpower);
992 		iwl_mvm_set_tx_power(mvm, vif, link_conf->txpower);
993 	}
994 
995 	mutex_unlock(&mvm->mutex);
996 }
997 
998 static void iwl_mvm_mld_vif_cfg_changed(struct ieee80211_hw *hw,
999 					struct ieee80211_vif *vif,
1000 					u64 changes)
1001 {
1002 	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
1003 
1004 	mutex_lock(&mvm->mutex);
1005 
1006 	if (changes & BSS_CHANGED_IDLE && !vif->cfg.idle)
1007 		iwl_mvm_scan_stop(mvm, IWL_MVM_SCAN_SCHED, true);
1008 
1009 	if (vif->type == NL80211_IFTYPE_STATION)
1010 		iwl_mvm_mld_vif_cfg_changed_station(mvm, vif, changes);
1011 
1012 	mutex_unlock(&mvm->mutex);
1013 }
1014 
1015 static int
1016 iwl_mvm_mld_switch_vif_chanctx(struct ieee80211_hw *hw,
1017 			       struct ieee80211_vif_chanctx_switch *vifs,
1018 			       int n_vifs,
1019 			       enum ieee80211_chanctx_switch_mode mode)
1020 {
1021 	static const struct iwl_mvm_switch_vif_chanctx_ops ops = {
1022 		.__assign_vif_chanctx = __iwl_mvm_mld_assign_vif_chanctx,
1023 		.__unassign_vif_chanctx = __iwl_mvm_mld_unassign_vif_chanctx,
1024 	};
1025 
1026 	return iwl_mvm_switch_vif_chanctx_common(hw, vifs, n_vifs, mode, &ops);
1027 }
1028 
1029 static void iwl_mvm_mld_config_iface_filter(struct ieee80211_hw *hw,
1030 					    struct ieee80211_vif *vif,
1031 					    unsigned int filter_flags,
1032 					    unsigned int changed_flags)
1033 {
1034 	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
1035 
1036 	/* We support only filter for probe requests */
1037 	if (!(changed_flags & FIF_PROBE_REQ))
1038 		return;
1039 
1040 	/* Supported only for p2p client interfaces */
1041 	if (vif->type != NL80211_IFTYPE_STATION || !vif->cfg.assoc ||
1042 	    !vif->p2p)
1043 		return;
1044 
1045 	mutex_lock(&mvm->mutex);
1046 	iwl_mvm_mld_mac_ctxt_changed(mvm, vif, false);
1047 	mutex_unlock(&mvm->mutex);
1048 }
1049 
1050 static int
1051 iwl_mvm_mld_mac_conf_tx(struct ieee80211_hw *hw,
1052 			struct ieee80211_vif *vif,
1053 			unsigned int link_id, u16 ac,
1054 			const struct ieee80211_tx_queue_params *params)
1055 {
1056 	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
1057 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
1058 	struct iwl_mvm_vif_link_info *mvm_link = mvmvif->link[link_id];
1059 
1060 	if (!mvm_link)
1061 		return -EINVAL;
1062 
1063 	mvm_link->queue_params[ac] = *params;
1064 
1065 	/* No need to update right away, we'll get BSS_CHANGED_QOS
1066 	 * The exception is P2P_DEVICE interface which needs immediate update.
1067 	 */
1068 	if (vif->type == NL80211_IFTYPE_P2P_DEVICE) {
1069 		int ret;
1070 
1071 		mutex_lock(&mvm->mutex);
1072 		ret = iwl_mvm_link_changed(mvm, vif, &vif->bss_conf,
1073 					   LINK_CONTEXT_MODIFY_QOS_PARAMS,
1074 					   true);
1075 		mutex_unlock(&mvm->mutex);
1076 		return ret;
1077 	}
1078 	return 0;
1079 }
1080 
1081 static int iwl_mvm_mld_roc_link(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
1082 {
1083 	int ret;
1084 
1085 	lockdep_assert_held(&mvm->mutex);
1086 
1087 	/* The PHY context ID might have changed so need to set it */
1088 	ret = iwl_mvm_link_changed(mvm, vif, &vif->bss_conf, 0, false);
1089 	if (WARN(ret, "Failed to set PHY context ID\n"))
1090 		return ret;
1091 
1092 	ret = iwl_mvm_link_changed(mvm, vif, &vif->bss_conf,
1093 				   LINK_CONTEXT_MODIFY_ACTIVE |
1094 				   LINK_CONTEXT_MODIFY_RATES_INFO,
1095 				   true);
1096 
1097 	if (WARN(ret, "Failed linking P2P_DEVICE\n"))
1098 		return ret;
1099 
1100 	/* The station and queue allocation must be done only after the linking
1101 	 * is done, as otherwise the FW might incorrectly configure its state.
1102 	 */
1103 	return iwl_mvm_mld_add_bcast_sta(mvm, vif, &vif->bss_conf);
1104 }
1105 
1106 static int iwl_mvm_mld_roc(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
1107 			   struct ieee80211_channel *channel, int duration,
1108 			   enum ieee80211_roc_type type)
1109 {
1110 	static const struct iwl_mvm_roc_ops ops = {
1111 		.add_aux_sta_for_hs20 = iwl_mvm_mld_add_aux_sta,
1112 		.link = iwl_mvm_mld_roc_link,
1113 	};
1114 
1115 	return iwl_mvm_roc_common(hw, vif, channel, duration, type, &ops);
1116 }
1117 
1118 static int
1119 iwl_mvm_mld_change_vif_links(struct ieee80211_hw *hw,
1120 			     struct ieee80211_vif *vif,
1121 			     u16 old_links, u16 new_links,
1122 			     struct ieee80211_bss_conf *old[IEEE80211_MLD_MAX_NUM_LINKS])
1123 {
1124 	struct iwl_mvm_vif_link_info *new_link[IEEE80211_MLD_MAX_NUM_LINKS] = {};
1125 	unsigned int n_active = iwl_mvm_mld_count_active_links(vif);
1126 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
1127 	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
1128 	u16 removed = old_links & ~new_links;
1129 	u16 added = new_links & ~old_links;
1130 	int err, i;
1131 
1132 	if (hweight16(new_links) > 1 &&
1133 	    n_active > iwl_mvm_max_active_links(mvm, vif))
1134 		return -EOPNOTSUPP;
1135 
1136 	for (i = 0; i < IEEE80211_MLD_MAX_NUM_LINKS; i++) {
1137 		int r;
1138 
1139 		if (test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status))
1140 			break;
1141 
1142 		if (!(added & BIT(i)))
1143 			continue;
1144 		new_link[i] = kzalloc(sizeof(*new_link[i]), GFP_KERNEL);
1145 		if (!new_link[i]) {
1146 			err = -ENOMEM;
1147 			goto free;
1148 		}
1149 
1150 		new_link[i]->bcast_sta.sta_id = IWL_MVM_INVALID_STA;
1151 		new_link[i]->mcast_sta.sta_id = IWL_MVM_INVALID_STA;
1152 		new_link[i]->ap_sta_id = IWL_MVM_INVALID_STA;
1153 		new_link[i]->fw_link_id = IWL_MVM_FW_LINK_ID_INVALID;
1154 
1155 		for (r = 0; r < NUM_IWL_MVM_SMPS_REQ; r++)
1156 			new_link[i]->smps_requests[r] =
1157 				IEEE80211_SMPS_AUTOMATIC;
1158 	}
1159 
1160 	mutex_lock(&mvm->mutex);
1161 
1162 	if (old_links == 0) {
1163 		err = iwl_mvm_disable_link(mvm, vif, &vif->bss_conf);
1164 		if (err)
1165 			goto out_err;
1166 		mvmvif->link[0] = NULL;
1167 	}
1168 
1169 	for (i = 0; i < IEEE80211_MLD_MAX_NUM_LINKS; i++) {
1170 		if (removed & BIT(i)) {
1171 			struct ieee80211_bss_conf *link_conf = old[i];
1172 
1173 			err = iwl_mvm_disable_link(mvm, vif, link_conf);
1174 			if (err)
1175 				goto out_err;
1176 			kfree(mvmvif->link[i]);
1177 			mvmvif->link[i] = NULL;
1178 		} else if (added & BIT(i)) {
1179 			struct ieee80211_bss_conf *link_conf;
1180 
1181 			link_conf = link_conf_dereference_protected(vif, i);
1182 			if (WARN_ON(!link_conf))
1183 				continue;
1184 
1185 			if (!test_bit(IWL_MVM_STATUS_IN_HW_RESTART,
1186 				      &mvm->status))
1187 				mvmvif->link[i] = new_link[i];
1188 			new_link[i] = NULL;
1189 			err = iwl_mvm_add_link(mvm, vif, link_conf);
1190 			if (err)
1191 				goto out_err;
1192 		}
1193 	}
1194 
1195 	err = 0;
1196 	if (new_links == 0) {
1197 		mvmvif->link[0] = &mvmvif->deflink;
1198 		err = iwl_mvm_add_link(mvm, vif, &vif->bss_conf);
1199 	}
1200 
1201 out_err:
1202 	/* we really don't have a good way to roll back here ... */
1203 	mutex_unlock(&mvm->mutex);
1204 
1205 free:
1206 	for (i = 0; i < IEEE80211_MLD_MAX_NUM_LINKS; i++)
1207 		kfree(new_link[i]);
1208 	return err;
1209 }
1210 
1211 static int
1212 iwl_mvm_mld_change_sta_links(struct ieee80211_hw *hw,
1213 			     struct ieee80211_vif *vif,
1214 			     struct ieee80211_sta *sta,
1215 			     u16 old_links, u16 new_links)
1216 {
1217 	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
1218 	int ret;
1219 
1220 	mutex_lock(&mvm->mutex);
1221 	ret = iwl_mvm_mld_update_sta_links(mvm, vif, sta, old_links, new_links);
1222 	mutex_unlock(&mvm->mutex);
1223 
1224 	return ret;
1225 }
1226 
1227 const struct ieee80211_ops iwl_mvm_mld_hw_ops = {
1228 	.tx = iwl_mvm_mac_tx,
1229 	.wake_tx_queue = iwl_mvm_mac_wake_tx_queue,
1230 	.ampdu_action = iwl_mvm_mac_ampdu_action,
1231 	.get_antenna = iwl_mvm_op_get_antenna,
1232 	.set_antenna = iwl_mvm_op_set_antenna,
1233 	.start = iwl_mvm_mac_start,
1234 	.reconfig_complete = iwl_mvm_mac_reconfig_complete,
1235 	.stop = iwl_mvm_mac_stop,
1236 	.add_interface = iwl_mvm_mld_mac_add_interface,
1237 	.remove_interface = iwl_mvm_mld_mac_remove_interface,
1238 	.config = iwl_mvm_mac_config,
1239 	.prepare_multicast = iwl_mvm_prepare_multicast,
1240 	.configure_filter = iwl_mvm_configure_filter,
1241 	.config_iface_filter = iwl_mvm_mld_config_iface_filter,
1242 	.link_info_changed = iwl_mvm_mld_link_info_changed,
1243 	.vif_cfg_changed = iwl_mvm_mld_vif_cfg_changed,
1244 	.hw_scan = iwl_mvm_mac_hw_scan,
1245 	.cancel_hw_scan = iwl_mvm_mac_cancel_hw_scan,
1246 	.sta_pre_rcu_remove = iwl_mvm_sta_pre_rcu_remove,
1247 	.sta_state = iwl_mvm_mld_mac_sta_state,
1248 	.sta_notify = iwl_mvm_mac_sta_notify,
1249 	.allow_buffered_frames = iwl_mvm_mac_allow_buffered_frames,
1250 	.release_buffered_frames = iwl_mvm_mac_release_buffered_frames,
1251 	.set_rts_threshold = iwl_mvm_mac_set_rts_threshold,
1252 	.sta_rc_update = iwl_mvm_sta_rc_update,
1253 	.conf_tx = iwl_mvm_mld_mac_conf_tx,
1254 	.mgd_prepare_tx = iwl_mvm_mac_mgd_prepare_tx,
1255 	.mgd_complete_tx = iwl_mvm_mac_mgd_complete_tx,
1256 	.mgd_protect_tdls_discover = iwl_mvm_mac_mgd_protect_tdls_discover,
1257 	.flush = iwl_mvm_mac_flush,
1258 	.flush_sta = iwl_mvm_mac_flush_sta,
1259 	.sched_scan_start = iwl_mvm_mac_sched_scan_start,
1260 	.sched_scan_stop = iwl_mvm_mac_sched_scan_stop,
1261 	.set_key = iwl_mvm_mac_set_key,
1262 	.update_tkip_key = iwl_mvm_mac_update_tkip_key,
1263 	.remain_on_channel = iwl_mvm_mld_roc,
1264 	.cancel_remain_on_channel = iwl_mvm_cancel_roc,
1265 	.add_chanctx = iwl_mvm_add_chanctx,
1266 	.remove_chanctx = iwl_mvm_remove_chanctx,
1267 	.change_chanctx = iwl_mvm_change_chanctx,
1268 	.assign_vif_chanctx = iwl_mvm_mld_assign_vif_chanctx,
1269 	.unassign_vif_chanctx = iwl_mvm_mld_unassign_vif_chanctx,
1270 	.switch_vif_chanctx = iwl_mvm_mld_switch_vif_chanctx,
1271 
1272 	.start_ap = iwl_mvm_mld_start_ap,
1273 	.stop_ap = iwl_mvm_mld_stop_ap,
1274 	.join_ibss = iwl_mvm_mld_start_ibss,
1275 	.leave_ibss = iwl_mvm_mld_stop_ibss,
1276 
1277 	.tx_last_beacon = iwl_mvm_tx_last_beacon,
1278 
1279 	.channel_switch = iwl_mvm_channel_switch,
1280 	.pre_channel_switch = iwl_mvm_pre_channel_switch,
1281 	.post_channel_switch = iwl_mvm_post_channel_switch,
1282 	.abort_channel_switch = iwl_mvm_abort_channel_switch,
1283 	.channel_switch_rx_beacon = iwl_mvm_channel_switch_rx_beacon,
1284 
1285 	.tdls_channel_switch = iwl_mvm_tdls_channel_switch,
1286 	.tdls_cancel_channel_switch = iwl_mvm_tdls_cancel_channel_switch,
1287 	.tdls_recv_channel_switch = iwl_mvm_tdls_recv_channel_switch,
1288 
1289 	.event_callback = iwl_mvm_mac_event_callback,
1290 
1291 	.sync_rx_queues = iwl_mvm_sync_rx_queues,
1292 
1293 	CFG80211_TESTMODE_CMD(iwl_mvm_mac_testmode_cmd)
1294 
1295 #ifdef CONFIG_PM_SLEEP
1296 	/* look at d3.c */
1297 	.suspend = iwl_mvm_suspend,
1298 	.resume = iwl_mvm_resume,
1299 	.set_wakeup = iwl_mvm_set_wakeup,
1300 	.set_rekey_data = iwl_mvm_set_rekey_data,
1301 #if IS_ENABLED(CONFIG_IPV6)
1302 	.ipv6_addr_change = iwl_mvm_ipv6_addr_change,
1303 #endif
1304 	.set_default_unicast_key = iwl_mvm_set_default_unicast_key,
1305 #endif
1306 	.get_survey = iwl_mvm_mac_get_survey,
1307 	.sta_statistics = iwl_mvm_mac_sta_statistics,
1308 	.get_ftm_responder_stats = iwl_mvm_mac_get_ftm_responder_stats,
1309 	.start_pmsr = iwl_mvm_start_pmsr,
1310 	.abort_pmsr = iwl_mvm_abort_pmsr,
1311 
1312 #ifdef CONFIG_IWLWIFI_DEBUGFS
1313 	.vif_add_debugfs = iwl_mvm_vif_add_debugfs,
1314 	.link_add_debugfs = iwl_mvm_link_add_debugfs,
1315 	.link_sta_add_debugfs = iwl_mvm_link_sta_add_debugfs,
1316 #endif
1317 	.set_hw_timestamp = iwl_mvm_set_hw_timestamp,
1318 
1319 	.change_vif_links = iwl_mvm_mld_change_vif_links,
1320 	.change_sta_links = iwl_mvm_mld_change_sta_links,
1321 };
1322