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