xref: /linux/drivers/net/wireless/intel/iwlwifi/mvm/link.c (revision 7f71507851fc7764b36a3221839607d3a45c2025)
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 #include "time-event.h"
7 
8 #define HANDLE_ESR_REASONS(HOW)		\
9 	HOW(BLOCKED_PREVENTION)		\
10 	HOW(BLOCKED_WOWLAN)		\
11 	HOW(BLOCKED_TPT)		\
12 	HOW(BLOCKED_FW)			\
13 	HOW(BLOCKED_NON_BSS)		\
14 	HOW(BLOCKED_ROC)		\
15 	HOW(BLOCKED_TMP_NON_BSS)	\
16 	HOW(EXIT_MISSED_BEACON)		\
17 	HOW(EXIT_LOW_RSSI)		\
18 	HOW(EXIT_COEX)			\
19 	HOW(EXIT_BANDWIDTH)		\
20 	HOW(EXIT_CSA)			\
21 	HOW(EXIT_LINK_USAGE)		\
22 	HOW(EXIT_FAIL_ENTRY)
23 
24 static const char *const iwl_mvm_esr_states_names[] = {
25 #define NAME_ENTRY(x) [ilog2(IWL_MVM_ESR_##x)] = #x,
26 	HANDLE_ESR_REASONS(NAME_ENTRY)
27 };
28 
29 const char *iwl_get_esr_state_string(enum iwl_mvm_esr_state state)
30 {
31 	int offs = ilog2(state);
32 
33 	if (offs >= ARRAY_SIZE(iwl_mvm_esr_states_names) ||
34 	    !iwl_mvm_esr_states_names[offs])
35 		return "UNKNOWN";
36 
37 	return iwl_mvm_esr_states_names[offs];
38 }
39 
40 static void iwl_mvm_print_esr_state(struct iwl_mvm *mvm, u32 mask)
41 {
42 #define NAME_FMT(x) "%s"
43 #define NAME_PR(x) (mask & IWL_MVM_ESR_##x) ? "[" #x "]" : "",
44 	IWL_DEBUG_INFO(mvm,
45 		       "EMLSR state = " HANDLE_ESR_REASONS(NAME_FMT)
46 		       " (0x%x)\n",
47 		       HANDLE_ESR_REASONS(NAME_PR)
48 		       mask);
49 #undef NAME_FMT
50 #undef NAME_PR
51 }
52 
53 static u32 iwl_mvm_get_free_fw_link_id(struct iwl_mvm *mvm,
54 				       struct iwl_mvm_vif *mvm_vif)
55 {
56 	u32 i;
57 
58 	lockdep_assert_held(&mvm->mutex);
59 
60 	for (i = 0; i < ARRAY_SIZE(mvm->link_id_to_link_conf); i++)
61 		if (!rcu_access_pointer(mvm->link_id_to_link_conf[i]))
62 			return i;
63 
64 	return IWL_MVM_FW_LINK_ID_INVALID;
65 }
66 
67 static int iwl_mvm_link_cmd_send(struct iwl_mvm *mvm,
68 				 struct iwl_link_config_cmd *cmd,
69 				 enum iwl_ctxt_action action)
70 {
71 	int ret;
72 
73 	cmd->action = cpu_to_le32(action);
74 	ret = iwl_mvm_send_cmd_pdu(mvm,
75 				   WIDE_ID(MAC_CONF_GROUP, LINK_CONFIG_CMD), 0,
76 				   sizeof(*cmd), cmd);
77 	if (ret)
78 		IWL_ERR(mvm, "Failed to send LINK_CONFIG_CMD (action:%d): %d\n",
79 			action, ret);
80 	return ret;
81 }
82 
83 int iwl_mvm_set_link_mapping(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
84 			     struct ieee80211_bss_conf *link_conf)
85 {
86 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
87 	struct iwl_mvm_vif_link_info *link_info =
88 		mvmvif->link[link_conf->link_id];
89 
90 	if (link_info->fw_link_id == IWL_MVM_FW_LINK_ID_INVALID) {
91 		link_info->fw_link_id = iwl_mvm_get_free_fw_link_id(mvm,
92 								    mvmvif);
93 		if (link_info->fw_link_id >=
94 		    ARRAY_SIZE(mvm->link_id_to_link_conf))
95 			return -EINVAL;
96 
97 		rcu_assign_pointer(mvm->link_id_to_link_conf[link_info->fw_link_id],
98 				   link_conf);
99 	}
100 
101 	return 0;
102 }
103 
104 int iwl_mvm_add_link(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
105 		     struct ieee80211_bss_conf *link_conf)
106 {
107 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
108 	unsigned int link_id = link_conf->link_id;
109 	struct iwl_mvm_vif_link_info *link_info = mvmvif->link[link_id];
110 	struct iwl_link_config_cmd cmd = {};
111 	unsigned int cmd_id = WIDE_ID(MAC_CONF_GROUP, LINK_CONFIG_CMD);
112 	u8 cmd_ver = iwl_fw_lookup_cmd_ver(mvm->fw, cmd_id, 1);
113 	int ret;
114 
115 	if (WARN_ON_ONCE(!link_info))
116 		return -EINVAL;
117 
118 	ret = iwl_mvm_set_link_mapping(mvm, vif, link_conf);
119 	if (ret)
120 		return ret;
121 
122 	/* Update SF - Disable if needed. if this fails, SF might still be on
123 	 * while many macs are bound, which is forbidden - so fail the binding.
124 	 */
125 	if (iwl_mvm_sf_update(mvm, vif, false))
126 		return -EINVAL;
127 
128 	cmd.link_id = cpu_to_le32(link_info->fw_link_id);
129 	cmd.mac_id = cpu_to_le32(mvmvif->id);
130 	cmd.spec_link_id = link_conf->link_id;
131 	WARN_ON_ONCE(link_info->phy_ctxt);
132 	cmd.phy_id = cpu_to_le32(FW_CTXT_INVALID);
133 
134 	memcpy(cmd.local_link_addr, link_conf->addr, ETH_ALEN);
135 
136 	if (vif->type == NL80211_IFTYPE_ADHOC && link_conf->bssid)
137 		memcpy(cmd.ibss_bssid_addr, link_conf->bssid, ETH_ALEN);
138 
139 	if (cmd_ver < 2)
140 		cmd.listen_lmac = cpu_to_le32(link_info->listen_lmac);
141 
142 	return iwl_mvm_link_cmd_send(mvm, &cmd, FW_CTXT_ACTION_ADD);
143 }
144 
145 struct iwl_mvm_esr_iter_data {
146 	struct ieee80211_vif *vif;
147 	unsigned int link_id;
148 	bool lift_block;
149 };
150 
151 static void iwl_mvm_esr_vif_iterator(void *_data, u8 *mac,
152 				     struct ieee80211_vif *vif)
153 {
154 	struct iwl_mvm_esr_iter_data *data = _data;
155 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
156 	int link_id;
157 
158 	if (ieee80211_vif_type_p2p(vif) == NL80211_IFTYPE_STATION)
159 		return;
160 
161 	for_each_mvm_vif_valid_link(mvmvif, link_id) {
162 		struct iwl_mvm_vif_link_info *link_info =
163 			mvmvif->link[link_id];
164 		if (vif == data->vif && link_id == data->link_id)
165 			continue;
166 		if (link_info->active)
167 			data->lift_block = false;
168 	}
169 }
170 
171 int iwl_mvm_esr_non_bss_link(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
172 			     unsigned int link_id, bool active)
173 {
174 	/* An active link of a non-station vif blocks EMLSR. Upon activation
175 	 * block EMLSR on the bss vif. Upon deactivation, check if this link
176 	 * was the last non-station link active, and if so unblock the bss vif
177 	 */
178 	struct ieee80211_vif *bss_vif = iwl_mvm_get_bss_vif(mvm);
179 	struct iwl_mvm_esr_iter_data data = {
180 		.vif = vif,
181 		.link_id = link_id,
182 		.lift_block = true,
183 	};
184 
185 	if (IS_ERR_OR_NULL(bss_vif))
186 		return 0;
187 
188 	if (active)
189 		return iwl_mvm_block_esr_sync(mvm, bss_vif,
190 					      IWL_MVM_ESR_BLOCKED_NON_BSS);
191 
192 	ieee80211_iterate_active_interfaces(mvm->hw,
193 					    IEEE80211_IFACE_ITER_NORMAL,
194 					    iwl_mvm_esr_vif_iterator, &data);
195 	if (data.lift_block) {
196 		mutex_lock(&mvm->mutex);
197 		iwl_mvm_unblock_esr(mvm, bss_vif, IWL_MVM_ESR_BLOCKED_NON_BSS);
198 		mutex_unlock(&mvm->mutex);
199 	}
200 
201 	return 0;
202 }
203 
204 int iwl_mvm_link_changed(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
205 			 struct ieee80211_bss_conf *link_conf,
206 			 u32 changes, bool active)
207 {
208 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
209 	unsigned int link_id = link_conf->link_id;
210 	struct iwl_mvm_vif_link_info *link_info = mvmvif->link[link_id];
211 	struct iwl_mvm_phy_ctxt *phyctxt;
212 	struct iwl_link_config_cmd cmd = {};
213 	u32 ht_flag, flags = 0, flags_mask = 0;
214 	int ret;
215 	unsigned int cmd_id = WIDE_ID(MAC_CONF_GROUP, LINK_CONFIG_CMD);
216 	u8 cmd_ver = iwl_fw_lookup_cmd_ver(mvm->fw, cmd_id, 1);
217 
218 	if (WARN_ON_ONCE(!link_info ||
219 			 link_info->fw_link_id == IWL_MVM_FW_LINK_ID_INVALID))
220 		return -EINVAL;
221 
222 	if (changes & LINK_CONTEXT_MODIFY_ACTIVE) {
223 		/* When activating a link, phy context should be valid;
224 		 * when deactivating a link, it also should be valid since
225 		 * the link was active before. So, do nothing in this case.
226 		 * Since a link is added first with FW_CTXT_INVALID, then we
227 		 * can get here in case it's removed before it was activated.
228 		 */
229 		if (!link_info->phy_ctxt)
230 			return 0;
231 
232 		/* Catch early if driver tries to activate or deactivate a link
233 		 * twice.
234 		 */
235 		WARN_ON_ONCE(active == link_info->active);
236 
237 		/* When deactivating a link session protection should
238 		 * be stopped. Also let the firmware know if we can't Tx.
239 		 */
240 		if (!active && vif->type == NL80211_IFTYPE_STATION) {
241 			iwl_mvm_stop_session_protection(mvm, vif);
242 			if (link_info->csa_block_tx) {
243 				cmd.block_tx = 1;
244 				link_info->csa_block_tx = false;
245 			}
246 		}
247 	}
248 
249 	cmd.link_id = cpu_to_le32(link_info->fw_link_id);
250 
251 	/* The phy_id, link address and listen_lmac can be modified only until
252 	 * the link becomes active, otherwise they will be ignored.
253 	 */
254 	phyctxt = link_info->phy_ctxt;
255 	if (phyctxt)
256 		cmd.phy_id = cpu_to_le32(phyctxt->id);
257 	else
258 		cmd.phy_id = cpu_to_le32(FW_CTXT_INVALID);
259 	cmd.mac_id = cpu_to_le32(mvmvif->id);
260 
261 	memcpy(cmd.local_link_addr, link_conf->addr, ETH_ALEN);
262 
263 	cmd.active = cpu_to_le32(active);
264 
265 	if (vif->type == NL80211_IFTYPE_ADHOC && link_conf->bssid)
266 		memcpy(cmd.ibss_bssid_addr, link_conf->bssid, ETH_ALEN);
267 
268 	iwl_mvm_set_fw_basic_rates(mvm, vif, link_info,
269 				   &cmd.cck_rates, &cmd.ofdm_rates);
270 
271 	cmd.cck_short_preamble = cpu_to_le32(link_conf->use_short_preamble);
272 	cmd.short_slot = cpu_to_le32(link_conf->use_short_slot);
273 
274 	/* The fw does not distinguish between ht and fat */
275 	ht_flag = LINK_PROT_FLG_HT_PROT | LINK_PROT_FLG_FAT_PROT;
276 	iwl_mvm_set_fw_protection_flags(mvm, vif, link_conf,
277 					&cmd.protection_flags,
278 					ht_flag, LINK_PROT_FLG_TGG_PROTECT);
279 
280 	iwl_mvm_set_fw_qos_params(mvm, vif, link_conf, cmd.ac,
281 				  &cmd.qos_flags);
282 
283 
284 	cmd.bi = cpu_to_le32(link_conf->beacon_int);
285 	cmd.dtim_interval = cpu_to_le32(link_conf->beacon_int *
286 					link_conf->dtim_period);
287 
288 	if (!link_conf->he_support || iwlwifi_mod_params.disable_11ax ||
289 	    (vif->type == NL80211_IFTYPE_STATION && !vif->cfg.assoc)) {
290 		changes &= ~LINK_CONTEXT_MODIFY_HE_PARAMS;
291 		goto send_cmd;
292 	}
293 
294 	cmd.htc_trig_based_pkt_ext = link_conf->htc_trig_based_pkt_ext;
295 
296 	if (link_conf->uora_exists) {
297 		cmd.rand_alloc_ecwmin =
298 			link_conf->uora_ocw_range & 0x7;
299 		cmd.rand_alloc_ecwmax =
300 			(link_conf->uora_ocw_range >> 3) & 0x7;
301 	}
302 
303 	/* ap_sta may be NULL if we're disconnecting */
304 	if (changes & LINK_CONTEXT_MODIFY_HE_PARAMS && mvmvif->ap_sta) {
305 		struct ieee80211_link_sta *link_sta =
306 			link_sta_dereference_check(mvmvif->ap_sta, link_id);
307 
308 		if (!WARN_ON(!link_sta) && link_sta->he_cap.has_he &&
309 		    link_sta->he_cap.he_cap_elem.mac_cap_info[5] &
310 		    IEEE80211_HE_MAC_CAP5_OM_CTRL_UL_MU_DATA_DIS_RX)
311 			cmd.ul_mu_data_disable = 1;
312 	}
313 
314 	/* TODO  how to set ndp_fdbk_buff_th_exp? */
315 
316 	if (iwl_mvm_set_fw_mu_edca_params(mvm, mvmvif->link[link_id],
317 					  &cmd.trig_based_txf[0])) {
318 		flags |= LINK_FLG_MU_EDCA_CW;
319 		flags_mask |= LINK_FLG_MU_EDCA_CW;
320 	}
321 
322 	if (changes & LINK_CONTEXT_MODIFY_EHT_PARAMS) {
323 		struct ieee80211_chanctx_conf *ctx;
324 		struct cfg80211_chan_def *def = NULL;
325 
326 		rcu_read_lock();
327 		ctx = rcu_dereference(link_conf->chanctx_conf);
328 		if (ctx)
329 			def = iwl_mvm_chanctx_def(mvm, ctx);
330 
331 		if (iwlwifi_mod_params.disable_11be ||
332 		    !link_conf->eht_support || !def ||
333 		    iwl_fw_lookup_cmd_ver(mvm->fw, PHY_CONTEXT_CMD, 1) >= 6)
334 			changes &= ~LINK_CONTEXT_MODIFY_EHT_PARAMS;
335 		else
336 			cmd.puncture_mask = cpu_to_le16(def->punctured);
337 		rcu_read_unlock();
338 	}
339 
340 	cmd.bss_color = link_conf->he_bss_color.color;
341 
342 	if (!link_conf->he_bss_color.enabled) {
343 		flags |= LINK_FLG_BSS_COLOR_DIS;
344 		flags_mask |= LINK_FLG_BSS_COLOR_DIS;
345 	}
346 
347 	cmd.frame_time_rts_th = cpu_to_le16(link_conf->frame_time_rts_th);
348 
349 	/* Block 26-tone RU OFDMA transmissions */
350 	if (link_info->he_ru_2mhz_block) {
351 		flags |= LINK_FLG_RU_2MHZ_BLOCK;
352 		flags_mask |= LINK_FLG_RU_2MHZ_BLOCK;
353 	}
354 
355 	if (link_conf->nontransmitted) {
356 		ether_addr_copy(cmd.ref_bssid_addr,
357 				link_conf->transmitter_bssid);
358 		cmd.bssid_index = link_conf->bssid_index;
359 	}
360 
361 send_cmd:
362 	cmd.modify_mask = cpu_to_le32(changes);
363 	cmd.flags = cpu_to_le32(flags);
364 	if (cmd_ver < 6)
365 		cmd.flags_mask = cpu_to_le32(flags_mask);
366 	cmd.spec_link_id = link_conf->link_id;
367 	if (cmd_ver < 2)
368 		cmd.listen_lmac = cpu_to_le32(link_info->listen_lmac);
369 
370 	ret = iwl_mvm_link_cmd_send(mvm, &cmd, FW_CTXT_ACTION_MODIFY);
371 	if (!ret && (changes & LINK_CONTEXT_MODIFY_ACTIVE))
372 		link_info->active = active;
373 
374 	return ret;
375 }
376 
377 int iwl_mvm_unset_link_mapping(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
378 			       struct ieee80211_bss_conf *link_conf)
379 {
380 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
381 	struct iwl_mvm_vif_link_info *link_info =
382 		mvmvif->link[link_conf->link_id];
383 
384 	/* mac80211 thought we have the link, but it was never configured */
385 	if (WARN_ON(!link_info ||
386 		    link_info->fw_link_id >=
387 		    ARRAY_SIZE(mvm->link_id_to_link_conf)))
388 		return -EINVAL;
389 
390 	RCU_INIT_POINTER(mvm->link_id_to_link_conf[link_info->fw_link_id],
391 			 NULL);
392 	return 0;
393 }
394 
395 int iwl_mvm_remove_link(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
396 			struct ieee80211_bss_conf *link_conf)
397 {
398 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
399 	unsigned int link_id = link_conf->link_id;
400 	struct iwl_mvm_vif_link_info *link_info = mvmvif->link[link_id];
401 	struct iwl_link_config_cmd cmd = {};
402 	int ret;
403 
404 	ret = iwl_mvm_unset_link_mapping(mvm, vif, link_conf);
405 	if (ret)
406 		return 0;
407 
408 	cmd.link_id = cpu_to_le32(link_info->fw_link_id);
409 	link_info->fw_link_id = IWL_MVM_FW_LINK_ID_INVALID;
410 	cmd.spec_link_id = link_conf->link_id;
411 	cmd.phy_id = cpu_to_le32(FW_CTXT_INVALID);
412 
413 	ret = iwl_mvm_link_cmd_send(mvm, &cmd, FW_CTXT_ACTION_REMOVE);
414 
415 	if (!ret)
416 		if (iwl_mvm_sf_update(mvm, vif, true))
417 			IWL_ERR(mvm, "Failed to update SF state\n");
418 
419 	return ret;
420 }
421 
422 /* link should be deactivated before removal, so in most cases we need to
423  * perform these two operations together
424  */
425 int iwl_mvm_disable_link(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
426 			 struct ieee80211_bss_conf *link_conf)
427 {
428 	int ret;
429 
430 	ret = iwl_mvm_link_changed(mvm, vif, link_conf,
431 				   LINK_CONTEXT_MODIFY_ACTIVE, false);
432 	if (ret)
433 		return ret;
434 
435 	ret = iwl_mvm_remove_link(mvm, vif, link_conf);
436 	if (ret)
437 		return ret;
438 
439 	return ret;
440 }
441 
442 struct iwl_mvm_rssi_to_grade {
443 	s8 rssi[2];
444 	u16 grade;
445 };
446 
447 #define RSSI_TO_GRADE_LINE(_lb, _hb_uhb, _grade) \
448 	{ \
449 		.rssi = {_lb, _hb_uhb}, \
450 		.grade = _grade \
451 	}
452 
453 /*
454  * This array must be sorted by increasing RSSI for proper functionality.
455  * The grades are actually estimated throughput, represented as fixed-point
456  * with a scale factor of 1/10.
457  */
458 static const struct iwl_mvm_rssi_to_grade rssi_to_grade_map[] = {
459 	RSSI_TO_GRADE_LINE(-85, -89, 177),
460 	RSSI_TO_GRADE_LINE(-83, -86, 344),
461 	RSSI_TO_GRADE_LINE(-82, -85, 516),
462 	RSSI_TO_GRADE_LINE(-80, -83, 688),
463 	RSSI_TO_GRADE_LINE(-77, -79, 1032),
464 	RSSI_TO_GRADE_LINE(-73, -76, 1376),
465 	RSSI_TO_GRADE_LINE(-70, -74, 1548),
466 	RSSI_TO_GRADE_LINE(-69, -72, 1750),
467 	RSSI_TO_GRADE_LINE(-65, -68, 2064),
468 	RSSI_TO_GRADE_LINE(-61, -66, 2294),
469 	RSSI_TO_GRADE_LINE(-58, -61, 2580),
470 	RSSI_TO_GRADE_LINE(-55, -58, 2868),
471 	RSSI_TO_GRADE_LINE(-46, -55, 3098),
472 	RSSI_TO_GRADE_LINE(-43, -54, 3442)
473 };
474 
475 #define MAX_GRADE (rssi_to_grade_map[ARRAY_SIZE(rssi_to_grade_map) - 1].grade)
476 
477 #define DEFAULT_CHAN_LOAD_LB	30
478 #define DEFAULT_CHAN_LOAD_HB	15
479 #define DEFAULT_CHAN_LOAD_UHB	0
480 
481 /* Factors calculation is done with fixed-point with a scaling factor of 1/256 */
482 #define SCALE_FACTOR 256
483 
484 /* Convert a percentage from [0,100] to [0,255] */
485 #define NORMALIZE_PERCENT_TO_255(percentage) ((percentage) * SCALE_FACTOR / 100)
486 
487 static unsigned int
488 iwl_mvm_get_puncturing_factor(const struct ieee80211_bss_conf *link_conf)
489 {
490 	enum nl80211_chan_width chan_width =
491 		link_conf->chanreq.oper.width;
492 	int mhz = nl80211_chan_width_to_mhz(chan_width);
493 	unsigned int n_subchannels, n_punctured, puncturing_penalty;
494 
495 	if (WARN_ONCE(mhz < 20 || mhz > 320,
496 		      "Invalid channel width : (%d)\n", mhz))
497 		return SCALE_FACTOR;
498 
499 	/* No puncturing, no penalty */
500 	if (mhz < 80)
501 		return SCALE_FACTOR;
502 
503 	/* total number of subchannels */
504 	n_subchannels = mhz / 20;
505 	/* how many of these are punctured */
506 	n_punctured = hweight16(link_conf->chanreq.oper.punctured);
507 
508 	puncturing_penalty = n_punctured * SCALE_FACTOR / n_subchannels;
509 	return SCALE_FACTOR - puncturing_penalty;
510 }
511 
512 static unsigned int
513 iwl_mvm_get_chan_load(struct ieee80211_bss_conf *link_conf)
514 {
515 	struct ieee80211_vif *vif = link_conf->vif;
516 	struct iwl_mvm_vif_link_info *mvm_link =
517 		iwl_mvm_vif_from_mac80211(link_conf->vif)->link[link_conf->link_id];
518 	const struct element *bss_load_elem;
519 	const struct ieee80211_bss_load_elem *bss_load;
520 	enum nl80211_band band = link_conf->chanreq.oper.chan->band;
521 	const struct cfg80211_bss_ies *ies;
522 	unsigned int chan_load;
523 	u32 chan_load_by_us;
524 
525 	rcu_read_lock();
526 	if (ieee80211_vif_link_active(vif, link_conf->link_id))
527 		ies = rcu_dereference(link_conf->bss->beacon_ies);
528 	else
529 		ies = rcu_dereference(link_conf->bss->ies);
530 
531 	if (ies)
532 		bss_load_elem = cfg80211_find_elem(WLAN_EID_QBSS_LOAD,
533 						   ies->data, ies->len);
534 	else
535 		bss_load_elem = NULL;
536 
537 	/* If there isn't BSS Load element, take the defaults */
538 	if (!bss_load_elem ||
539 	    bss_load_elem->datalen != sizeof(*bss_load)) {
540 		rcu_read_unlock();
541 		switch (band) {
542 		case NL80211_BAND_2GHZ:
543 			chan_load = DEFAULT_CHAN_LOAD_LB;
544 			break;
545 		case NL80211_BAND_5GHZ:
546 			chan_load = DEFAULT_CHAN_LOAD_HB;
547 			break;
548 		case NL80211_BAND_6GHZ:
549 			chan_load = DEFAULT_CHAN_LOAD_UHB;
550 			break;
551 		default:
552 			chan_load = 0;
553 			break;
554 		}
555 		/* The defaults are given in percentage */
556 		return NORMALIZE_PERCENT_TO_255(chan_load);
557 	}
558 
559 	bss_load = (const void *)bss_load_elem->data;
560 	/* Channel util is in range 0-255 */
561 	chan_load = bss_load->channel_util;
562 	rcu_read_unlock();
563 
564 	if (!mvm_link || !mvm_link->active)
565 		return chan_load;
566 
567 	if (WARN_ONCE(!mvm_link->phy_ctxt,
568 		      "Active link (%u) without phy ctxt assigned!\n",
569 		      link_conf->link_id))
570 		return chan_load;
571 
572 	/* channel load by us is given in percentage */
573 	chan_load_by_us =
574 		NORMALIZE_PERCENT_TO_255(mvm_link->phy_ctxt->channel_load_by_us);
575 
576 	/* Use only values that firmware sends that can possibly be valid */
577 	if (chan_load_by_us <= chan_load)
578 		chan_load -= chan_load_by_us;
579 
580 	return chan_load;
581 }
582 
583 static unsigned int
584 iwl_mvm_get_chan_load_factor(struct ieee80211_bss_conf *link_conf)
585 {
586 	return SCALE_FACTOR - iwl_mvm_get_chan_load(link_conf);
587 }
588 
589 /* This function calculates the grade of a link. Returns 0 in error case */
590 VISIBLE_IF_IWLWIFI_KUNIT
591 unsigned int iwl_mvm_get_link_grade(struct ieee80211_bss_conf *link_conf)
592 {
593 	enum nl80211_band band;
594 	int i, rssi_idx;
595 	s32 link_rssi;
596 	unsigned int grade = MAX_GRADE;
597 
598 	if (WARN_ON_ONCE(!link_conf))
599 		return 0;
600 
601 	band = link_conf->chanreq.oper.chan->band;
602 	if (WARN_ONCE(band != NL80211_BAND_2GHZ &&
603 		      band != NL80211_BAND_5GHZ &&
604 		      band != NL80211_BAND_6GHZ,
605 		      "Invalid band (%u)\n", band))
606 		return 0;
607 
608 	link_rssi = MBM_TO_DBM(link_conf->bss->signal);
609 	/*
610 	 * For 6 GHz the RSSI of the beacons is lower than
611 	 * the RSSI of the data.
612 	 */
613 	if (band == NL80211_BAND_6GHZ)
614 		link_rssi += 4;
615 
616 	rssi_idx = band == NL80211_BAND_2GHZ ? 0 : 1;
617 
618 	/* No valid RSSI - take the lowest grade */
619 	if (!link_rssi)
620 		link_rssi = rssi_to_grade_map[0].rssi[rssi_idx];
621 
622 	/* Get grade based on RSSI */
623 	for (i = 0; i < ARRAY_SIZE(rssi_to_grade_map); i++) {
624 		const struct iwl_mvm_rssi_to_grade *line =
625 			&rssi_to_grade_map[i];
626 
627 		if (link_rssi > line->rssi[rssi_idx])
628 			continue;
629 		grade = line->grade;
630 		break;
631 	}
632 
633 	/* apply the channel load and puncturing factors */
634 	grade = grade * iwl_mvm_get_chan_load_factor(link_conf) / SCALE_FACTOR;
635 	grade = grade * iwl_mvm_get_puncturing_factor(link_conf) / SCALE_FACTOR;
636 	return grade;
637 }
638 EXPORT_SYMBOL_IF_IWLWIFI_KUNIT(iwl_mvm_get_link_grade);
639 
640 static
641 u8 iwl_mvm_set_link_selection_data(struct ieee80211_vif *vif,
642 				   struct iwl_mvm_link_sel_data *data,
643 				   unsigned long usable_links,
644 				   u8 *best_link_idx)
645 {
646 	u8 n_data = 0;
647 	u16 max_grade = 0;
648 	unsigned long link_id;
649 
650 	/* TODO: don't select links that weren't discovered in the last scan */
651 	for_each_set_bit(link_id, &usable_links, IEEE80211_MLD_MAX_NUM_LINKS) {
652 		struct ieee80211_bss_conf *link_conf =
653 			link_conf_dereference_protected(vif, link_id);
654 
655 		if (WARN_ON_ONCE(!link_conf))
656 			continue;
657 
658 		data[n_data].link_id = link_id;
659 		data[n_data].chandef = &link_conf->chanreq.oper;
660 		data[n_data].signal = link_conf->bss->signal / 100;
661 		data[n_data].grade = iwl_mvm_get_link_grade(link_conf);
662 
663 		if (data[n_data].grade > max_grade) {
664 			max_grade = data[n_data].grade;
665 			*best_link_idx = n_data;
666 		}
667 		n_data++;
668 	}
669 
670 	return n_data;
671 }
672 
673 struct iwl_mvm_bw_to_rssi_threshs {
674 	s8 low;
675 	s8 high;
676 };
677 
678 #define BW_TO_RSSI_THRESHOLDS(_bw)				\
679 	[IWL_PHY_CHANNEL_MODE ## _bw] = {			\
680 		.low = IWL_MVM_LOW_RSSI_THRESH_##_bw##MHZ,	\
681 		.high = IWL_MVM_HIGH_RSSI_THRESH_##_bw##MHZ	\
682 	}
683 
684 s8 iwl_mvm_get_esr_rssi_thresh(struct iwl_mvm *mvm,
685 			       const struct cfg80211_chan_def *chandef,
686 			       bool low)
687 {
688 	const struct iwl_mvm_bw_to_rssi_threshs bw_to_rssi_threshs_map[] = {
689 		BW_TO_RSSI_THRESHOLDS(20),
690 		BW_TO_RSSI_THRESHOLDS(40),
691 		BW_TO_RSSI_THRESHOLDS(80),
692 		BW_TO_RSSI_THRESHOLDS(160)
693 		/* 320 MHz has the same thresholds as 20 MHz */
694 	};
695 	const struct iwl_mvm_bw_to_rssi_threshs *threshs;
696 	u8 chan_width = iwl_mvm_get_channel_width(chandef);
697 
698 	if (WARN_ON(chandef->chan->band != NL80211_BAND_2GHZ &&
699 		    chandef->chan->band != NL80211_BAND_5GHZ &&
700 		    chandef->chan->band != NL80211_BAND_6GHZ))
701 		return S8_MAX;
702 
703 	/* 6 GHz will always use 20 MHz thresholds, regardless of the BW */
704 	if (chan_width == IWL_PHY_CHANNEL_MODE320)
705 		chan_width = IWL_PHY_CHANNEL_MODE20;
706 
707 	threshs = &bw_to_rssi_threshs_map[chan_width];
708 
709 	return low ? threshs->low : threshs->high;
710 }
711 
712 static u32
713 iwl_mvm_esr_disallowed_with_link(struct iwl_mvm *mvm,
714 				 struct ieee80211_vif *vif,
715 				 const struct iwl_mvm_link_sel_data *link,
716 				 bool primary)
717 {
718 	struct wiphy *wiphy = mvm->hw->wiphy;
719 	struct ieee80211_bss_conf *conf;
720 	enum iwl_mvm_esr_state ret = 0;
721 	s8 thresh;
722 
723 	conf = wiphy_dereference(wiphy, vif->link_conf[link->link_id]);
724 	if (WARN_ON_ONCE(!conf))
725 		return false;
726 
727 	/* BT Coex effects eSR mode only if one of the links is on LB */
728 	if (link->chandef->chan->band == NL80211_BAND_2GHZ &&
729 	    (!iwl_mvm_bt_coex_calculate_esr_mode(mvm, vif, link->signal,
730 						 primary)))
731 		ret |= IWL_MVM_ESR_EXIT_COEX;
732 
733 	thresh = iwl_mvm_get_esr_rssi_thresh(mvm, link->chandef,
734 					     false);
735 
736 	if (link->signal < thresh)
737 		ret |= IWL_MVM_ESR_EXIT_LOW_RSSI;
738 
739 	if (conf->csa_active)
740 		ret |= IWL_MVM_ESR_EXIT_CSA;
741 
742 	if (ret) {
743 		IWL_DEBUG_INFO(mvm,
744 			       "Link %d is not allowed for esr\n",
745 			       link->link_id);
746 		iwl_mvm_print_esr_state(mvm, ret);
747 	}
748 	return ret;
749 }
750 
751 VISIBLE_IF_IWLWIFI_KUNIT
752 bool iwl_mvm_mld_valid_link_pair(struct ieee80211_vif *vif,
753 				 const struct iwl_mvm_link_sel_data *a,
754 				 const struct iwl_mvm_link_sel_data *b)
755 {
756 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
757 	struct iwl_mvm *mvm = mvmvif->mvm;
758 	enum iwl_mvm_esr_state ret = 0;
759 
760 	/* Per-link considerations */
761 	if (iwl_mvm_esr_disallowed_with_link(mvm, vif, a, true) ||
762 	    iwl_mvm_esr_disallowed_with_link(mvm, vif, b, false))
763 		return false;
764 
765 	if (a->chandef->width != b->chandef->width ||
766 	    !(a->chandef->chan->band == NL80211_BAND_6GHZ &&
767 	      b->chandef->chan->band == NL80211_BAND_5GHZ))
768 		ret |= IWL_MVM_ESR_EXIT_BANDWIDTH;
769 
770 	if (ret) {
771 		IWL_DEBUG_INFO(mvm,
772 			       "Links %d and %d are not a valid pair for EMLSR\n",
773 			       a->link_id, b->link_id);
774 		iwl_mvm_print_esr_state(mvm, ret);
775 		return false;
776 	}
777 
778 	return true;
779 
780 }
781 EXPORT_SYMBOL_IF_IWLWIFI_KUNIT(iwl_mvm_mld_valid_link_pair);
782 
783 /*
784  * Returns the combined eSR grade of two given links.
785  * Returns 0 if eSR is not allowed with these 2 links.
786  */
787 static
788 unsigned int iwl_mvm_get_esr_grade(struct ieee80211_vif *vif,
789 				   const struct iwl_mvm_link_sel_data *a,
790 				   const struct iwl_mvm_link_sel_data *b,
791 				   u8 *primary_id)
792 {
793 	struct ieee80211_bss_conf *primary_conf;
794 	struct wiphy *wiphy = ieee80211_vif_to_wdev(vif)->wiphy;
795 	unsigned int primary_load;
796 
797 	lockdep_assert_wiphy(wiphy);
798 
799 	/* a is always primary, b is always secondary */
800 	if (b->grade > a->grade)
801 		swap(a, b);
802 
803 	*primary_id = a->link_id;
804 
805 	if (!iwl_mvm_mld_valid_link_pair(vif, a, b))
806 		return 0;
807 
808 	primary_conf = wiphy_dereference(wiphy, vif->link_conf[*primary_id]);
809 
810 	if (WARN_ON_ONCE(!primary_conf))
811 		return 0;
812 
813 	primary_load = iwl_mvm_get_chan_load(primary_conf);
814 
815 	return a->grade +
816 		((b->grade * primary_load) / SCALE_FACTOR);
817 }
818 
819 void iwl_mvm_select_links(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
820 {
821 	struct iwl_mvm_link_sel_data data[IEEE80211_MLD_MAX_NUM_LINKS];
822 	struct iwl_mvm_link_sel_data *best_link;
823 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
824 	u32 max_active_links = iwl_mvm_max_active_links(mvm, vif);
825 	u16 usable_links = ieee80211_vif_usable_links(vif);
826 	u8 best, primary_link, best_in_pair, n_data;
827 	u16 max_esr_grade = 0, new_active_links;
828 
829 	lockdep_assert_wiphy(mvm->hw->wiphy);
830 
831 	if (!mvmvif->authorized || !ieee80211_vif_is_mld(vif))
832 		return;
833 
834 	if (!IWL_MVM_AUTO_EML_ENABLE)
835 		return;
836 
837 	/* The logic below is a simple version that doesn't suit more than 2
838 	 * links
839 	 */
840 	WARN_ON_ONCE(max_active_links > 2);
841 
842 	n_data = iwl_mvm_set_link_selection_data(vif, data, usable_links,
843 						 &best);
844 
845 	if (WARN(!n_data, "Couldn't find a valid grade for any link!\n"))
846 		return;
847 
848 	best_link = &data[best];
849 	primary_link = best_link->link_id;
850 	new_active_links = BIT(best_link->link_id);
851 
852 	/* eSR is not supported/blocked, or only one usable link */
853 	if (max_active_links == 1 || !iwl_mvm_vif_has_esr_cap(mvm, vif) ||
854 	    mvmvif->esr_disable_reason || n_data == 1)
855 		goto set_active;
856 
857 	for (u8 a = 0; a < n_data; a++)
858 		for (u8 b = a + 1; b < n_data; b++) {
859 			u16 esr_grade = iwl_mvm_get_esr_grade(vif, &data[a],
860 							      &data[b],
861 							      &best_in_pair);
862 
863 			if (esr_grade <= max_esr_grade)
864 				continue;
865 
866 			max_esr_grade = esr_grade;
867 			primary_link = best_in_pair;
868 			new_active_links = BIT(data[a].link_id) |
869 					   BIT(data[b].link_id);
870 		}
871 
872 	/* No valid pair was found, go with the best link */
873 	if (hweight16(new_active_links) <= 1)
874 		goto set_active;
875 
876 	/* For equal grade - prefer EMLSR */
877 	if (best_link->grade > max_esr_grade) {
878 		primary_link = best_link->link_id;
879 		new_active_links = BIT(best_link->link_id);
880 	}
881 set_active:
882 	IWL_DEBUG_INFO(mvm, "Link selection result: 0x%x. Primary = %d\n",
883 		       new_active_links, primary_link);
884 	ieee80211_set_active_links_async(vif, new_active_links);
885 	mvmvif->link_selection_res = new_active_links;
886 	mvmvif->link_selection_primary = primary_link;
887 }
888 
889 u8 iwl_mvm_get_primary_link(struct ieee80211_vif *vif)
890 {
891 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
892 
893 	/* relevant data is written with both locks held, so read with either */
894 	lockdep_assert(lockdep_is_held(&mvmvif->mvm->mutex) ||
895 		       lockdep_is_held(&mvmvif->mvm->hw->wiphy->mtx));
896 
897 	if (!ieee80211_vif_is_mld(vif))
898 		return 0;
899 
900 	/* In AP mode, there is no primary link */
901 	if (vif->type == NL80211_IFTYPE_AP)
902 		return __ffs(vif->active_links);
903 
904 	if (mvmvif->esr_active &&
905 	    !WARN_ON(!(BIT(mvmvif->primary_link) & vif->active_links)))
906 		return mvmvif->primary_link;
907 
908 	return __ffs(vif->active_links);
909 }
910 
911 /*
912  * For non-MLO/single link, this will return the deflink/single active link,
913  * respectively
914  */
915 u8 iwl_mvm_get_other_link(struct ieee80211_vif *vif, u8 link_id)
916 {
917 	switch (hweight16(vif->active_links)) {
918 	case 0:
919 		return 0;
920 	default:
921 		WARN_ON(1);
922 		fallthrough;
923 	case 1:
924 		return __ffs(vif->active_links);
925 	case 2:
926 		return __ffs(vif->active_links & ~BIT(link_id));
927 	}
928 }
929 
930 /* Reasons that can cause esr prevention */
931 #define IWL_MVM_ESR_PREVENT_REASONS	IWL_MVM_ESR_EXIT_MISSED_BEACON
932 #define IWL_MVM_PREVENT_ESR_TIMEOUT	(HZ * 400)
933 #define IWL_MVM_ESR_PREVENT_SHORT	(HZ * 300)
934 #define IWL_MVM_ESR_PREVENT_LONG	(HZ * 600)
935 
936 static bool iwl_mvm_check_esr_prevention(struct iwl_mvm *mvm,
937 					 struct iwl_mvm_vif *mvmvif,
938 					 enum iwl_mvm_esr_state reason)
939 {
940 	bool timeout_expired = time_after(jiffies,
941 					  mvmvif->last_esr_exit.ts +
942 					  IWL_MVM_PREVENT_ESR_TIMEOUT);
943 	unsigned long delay;
944 
945 	lockdep_assert_held(&mvm->mutex);
946 
947 	/* Only handle reasons that can cause prevention */
948 	if (!(reason & IWL_MVM_ESR_PREVENT_REASONS))
949 		return false;
950 
951 	/*
952 	 * Reset the counter if more than 400 seconds have passed between one
953 	 * exit and the other, or if we exited due to a different reason.
954 	 * Will also reset the counter after the long prevention is done.
955 	 */
956 	if (timeout_expired || mvmvif->last_esr_exit.reason != reason) {
957 		mvmvif->exit_same_reason_count = 1;
958 		return false;
959 	}
960 
961 	mvmvif->exit_same_reason_count++;
962 	if (WARN_ON(mvmvif->exit_same_reason_count < 2 ||
963 		    mvmvif->exit_same_reason_count > 3))
964 		return false;
965 
966 	mvmvif->esr_disable_reason |= IWL_MVM_ESR_BLOCKED_PREVENTION;
967 
968 	/*
969 	 * For the second exit, use a short prevention, and for the third one,
970 	 * use a long prevention.
971 	 */
972 	delay = mvmvif->exit_same_reason_count == 2 ?
973 		IWL_MVM_ESR_PREVENT_SHORT :
974 		IWL_MVM_ESR_PREVENT_LONG;
975 
976 	IWL_DEBUG_INFO(mvm,
977 		       "Preventing EMLSR for %ld seconds due to %u exits with the reason = %s (0x%x)\n",
978 		       delay / HZ, mvmvif->exit_same_reason_count,
979 		       iwl_get_esr_state_string(reason), reason);
980 
981 	wiphy_delayed_work_queue(mvm->hw->wiphy,
982 				 &mvmvif->prevent_esr_done_wk, delay);
983 	return true;
984 }
985 
986 #define IWL_MVM_TRIGGER_LINK_SEL_TIME (IWL_MVM_TRIGGER_LINK_SEL_TIME_SEC * HZ)
987 
988 /* API to exit eSR mode */
989 void iwl_mvm_exit_esr(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
990 		      enum iwl_mvm_esr_state reason,
991 		      u8 link_to_keep)
992 {
993 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
994 	u16 new_active_links;
995 	bool prevented;
996 
997 	lockdep_assert_held(&mvm->mutex);
998 
999 	if (!IWL_MVM_AUTO_EML_ENABLE)
1000 		return;
1001 
1002 	/* Nothing to do */
1003 	if (!mvmvif->esr_active)
1004 		return;
1005 
1006 	if (WARN_ON(!ieee80211_vif_is_mld(vif) || !mvmvif->authorized))
1007 		return;
1008 
1009 	if (WARN_ON(!(vif->active_links & BIT(link_to_keep))))
1010 		link_to_keep = __ffs(vif->active_links);
1011 
1012 	new_active_links = BIT(link_to_keep);
1013 	IWL_DEBUG_INFO(mvm,
1014 		       "Exiting EMLSR. reason = %s (0x%x). Current active links=0x%x, new active links = 0x%x\n",
1015 		       iwl_get_esr_state_string(reason), reason,
1016 		       vif->active_links, new_active_links);
1017 
1018 	ieee80211_set_active_links_async(vif, new_active_links);
1019 
1020 	/* Prevent EMLSR if needed */
1021 	prevented = iwl_mvm_check_esr_prevention(mvm, mvmvif, reason);
1022 
1023 	/* Remember why and when we exited EMLSR */
1024 	mvmvif->last_esr_exit.ts = jiffies;
1025 	mvmvif->last_esr_exit.reason = reason;
1026 
1027 	/*
1028 	 * If EMLSR is prevented now - don't try to get back to EMLSR.
1029 	 * If we exited due to a blocking event, we will try to get back to
1030 	 * EMLSR when the corresponding unblocking event will happen.
1031 	 */
1032 	if (prevented || reason & IWL_MVM_BLOCK_ESR_REASONS)
1033 		return;
1034 
1035 	/* If EMLSR is not blocked - try enabling it again in 30 seconds */
1036 	wiphy_delayed_work_queue(mvm->hw->wiphy,
1037 				 &mvmvif->mlo_int_scan_wk,
1038 				 round_jiffies_relative(IWL_MVM_TRIGGER_LINK_SEL_TIME));
1039 }
1040 
1041 void iwl_mvm_block_esr(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
1042 		       enum iwl_mvm_esr_state reason,
1043 		       u8 link_to_keep)
1044 {
1045 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
1046 
1047 	lockdep_assert_held(&mvm->mutex);
1048 
1049 	if (!IWL_MVM_AUTO_EML_ENABLE)
1050 		return;
1051 
1052 	/* This should be called only with disable reasons */
1053 	if (WARN_ON(!(reason & IWL_MVM_BLOCK_ESR_REASONS)))
1054 		return;
1055 
1056 	if (mvmvif->esr_disable_reason & reason)
1057 		return;
1058 
1059 	IWL_DEBUG_INFO(mvm,
1060 		       "Blocking EMLSR mode. reason = %s (0x%x)\n",
1061 		       iwl_get_esr_state_string(reason), reason);
1062 
1063 	mvmvif->esr_disable_reason |= reason;
1064 
1065 	iwl_mvm_print_esr_state(mvm, mvmvif->esr_disable_reason);
1066 
1067 	iwl_mvm_exit_esr(mvm, vif, reason, link_to_keep);
1068 }
1069 
1070 int iwl_mvm_block_esr_sync(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
1071 			   enum iwl_mvm_esr_state reason)
1072 {
1073 	int primary_link = iwl_mvm_get_primary_link(vif);
1074 	int ret;
1075 
1076 	if (!IWL_MVM_AUTO_EML_ENABLE || !ieee80211_vif_is_mld(vif))
1077 		return 0;
1078 
1079 	/* This should be called only with blocking reasons */
1080 	if (WARN_ON(!(reason & IWL_MVM_BLOCK_ESR_REASONS)))
1081 		return 0;
1082 
1083 	/* leave ESR immediately, not only async with iwl_mvm_block_esr() */
1084 	ret = ieee80211_set_active_links(vif, BIT(primary_link));
1085 	if (ret)
1086 		return ret;
1087 
1088 	mutex_lock(&mvm->mutex);
1089 	/* only additionally block for consistency and to avoid concurrency */
1090 	iwl_mvm_block_esr(mvm, vif, reason, primary_link);
1091 	mutex_unlock(&mvm->mutex);
1092 
1093 	return 0;
1094 }
1095 
1096 static void iwl_mvm_esr_unblocked(struct iwl_mvm *mvm,
1097 				  struct ieee80211_vif *vif)
1098 {
1099 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
1100 	bool need_new_sel = time_after(jiffies, mvmvif->last_esr_exit.ts +
1101 						IWL_MVM_TRIGGER_LINK_SEL_TIME);
1102 
1103 	lockdep_assert_held(&mvm->mutex);
1104 
1105 	if (!ieee80211_vif_is_mld(vif) || !mvmvif->authorized ||
1106 	    mvmvif->esr_active)
1107 		return;
1108 
1109 	IWL_DEBUG_INFO(mvm, "EMLSR is unblocked\n");
1110 
1111 	/* If we exited due to an EXIT reason, and the exit was in less than
1112 	 * 30 seconds, then a MLO scan was scheduled already.
1113 	 */
1114 	if (!need_new_sel &&
1115 	    !(mvmvif->last_esr_exit.reason & IWL_MVM_BLOCK_ESR_REASONS)) {
1116 		IWL_DEBUG_INFO(mvm, "Wait for MLO scan\n");
1117 		return;
1118 	}
1119 
1120 	/*
1121 	 * If EMLSR was blocked for more than 30 seconds, or the last link
1122 	 * selection decided to not enter EMLSR, trigger a new scan.
1123 	 */
1124 	if (need_new_sel || hweight16(mvmvif->link_selection_res) < 2) {
1125 		IWL_DEBUG_INFO(mvm, "Trigger MLO scan\n");
1126 		wiphy_delayed_work_queue(mvm->hw->wiphy,
1127 					 &mvmvif->mlo_int_scan_wk, 0);
1128 	/*
1129 	 * If EMLSR was blocked for less than 30 seconds, and the last link
1130 	 * selection decided to use EMLSR, activate EMLSR using the previous
1131 	 * link selection result.
1132 	 */
1133 	} else {
1134 		IWL_DEBUG_INFO(mvm,
1135 			       "Use the latest link selection result: 0x%x\n",
1136 			       mvmvif->link_selection_res);
1137 		ieee80211_set_active_links_async(vif,
1138 						 mvmvif->link_selection_res);
1139 	}
1140 }
1141 
1142 void iwl_mvm_unblock_esr(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
1143 			 enum iwl_mvm_esr_state reason)
1144 {
1145 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
1146 
1147 	lockdep_assert_held(&mvm->mutex);
1148 
1149 	if (!IWL_MVM_AUTO_EML_ENABLE)
1150 		return;
1151 
1152 	/* This should be called only with disable reasons */
1153 	if (WARN_ON(!(reason & IWL_MVM_BLOCK_ESR_REASONS)))
1154 		return;
1155 
1156 	/* No Change */
1157 	if (!(mvmvif->esr_disable_reason & reason))
1158 		return;
1159 
1160 	mvmvif->esr_disable_reason &= ~reason;
1161 
1162 	IWL_DEBUG_INFO(mvm,
1163 		       "Unblocking EMLSR mode. reason = %s (0x%x)\n",
1164 		       iwl_get_esr_state_string(reason), reason);
1165 	iwl_mvm_print_esr_state(mvm, mvmvif->esr_disable_reason);
1166 
1167 	if (!mvmvif->esr_disable_reason)
1168 		iwl_mvm_esr_unblocked(mvm, vif);
1169 }
1170 
1171 void iwl_mvm_init_link(struct iwl_mvm_vif_link_info *link)
1172 {
1173 	link->bcast_sta.sta_id = IWL_INVALID_STA;
1174 	link->mcast_sta.sta_id = IWL_INVALID_STA;
1175 	link->ap_sta_id = IWL_INVALID_STA;
1176 
1177 	for (int r = 0; r < NUM_IWL_MVM_SMPS_REQ; r++)
1178 		link->smps_requests[r] =
1179 			IEEE80211_SMPS_AUTOMATIC;
1180 }
1181