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