1 files changed, 1076 insertions, 0 deletions
diff --git a/drivers/net/wireless/intel/iwlwifi/mld/mlo.c b/drivers/net/wireless/intel/iwlwifi/mld/mlo.c
new file mode 100644
index 000000000000..a870e169e265
--- /dev/null
+++ b/drivers/net/wireless/intel/iwlwifi/mld/mlo.c
@@ -0,0 +1,1076 @@
+// SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause
+/*
+ * Copyright (C) 2024-2025 Intel Corporation
+ */
+#include "mlo.h"
+#include "phy.h"
+
+/* Block reasons helper */
+#define HANDLE_EMLSR_BLOCKED_REASONS(HOW)	\
+	HOW(PREVENTION)			\
+	HOW(WOWLAN)			\
+	HOW(ROC)			\
+	HOW(NON_BSS)			\
+	HOW(TMP_NON_BSS)		\
+	HOW(TPT)
+
+static const char *
+iwl_mld_get_emlsr_blocked_string(enum iwl_mld_emlsr_blocked blocked)
+{
+	/* Using switch without "default" will warn about missing entries  */
+	switch (blocked) {
+#define REASON_CASE(x) case IWL_MLD_EMLSR_BLOCKED_##x: return #x;
+	HANDLE_EMLSR_BLOCKED_REASONS(REASON_CASE)
+#undef REASON_CASE
+	}
+
+	return "ERROR";
+}
+
+static void iwl_mld_print_emlsr_blocked(struct iwl_mld *mld, u32 mask)
+{
+#define NAME_FMT(x) "%s"
+#define NAME_PR(x) (mask & IWL_MLD_EMLSR_BLOCKED_##x) ? "[" #x "]" : "",
+	IWL_DEBUG_INFO(mld,
+		       "EMLSR blocked = " HANDLE_EMLSR_BLOCKED_REASONS(NAME_FMT)
+		       " (0x%x)\n",
+		       HANDLE_EMLSR_BLOCKED_REASONS(NAME_PR)
+		       mask);
+#undef NAME_FMT
+#undef NAME_PR
+}
+
+/* Exit reasons helper */
+#define HANDLE_EMLSR_EXIT_REASONS(HOW)	\
+	HOW(BLOCK)			\
+	HOW(MISSED_BEACON)		\
+	HOW(FAIL_ENTRY)			\
+	HOW(CSA)			\
+	HOW(EQUAL_BAND)			\
+	HOW(LOW_RSSI)			\
+	HOW(LINK_USAGE)			\
+	HOW(BT_COEX)			\
+	HOW(CHAN_LOAD)			\
+	HOW(RFI)			\
+	HOW(FW_REQUEST)
+
+static const char *
+iwl_mld_get_emlsr_exit_string(enum iwl_mld_emlsr_exit exit)
+{
+	/* Using switch without "default" will warn about missing entries  */
+	switch (exit) {
+#define REASON_CASE(x) case IWL_MLD_EMLSR_EXIT_##x: return #x;
+	HANDLE_EMLSR_EXIT_REASONS(REASON_CASE)
+#undef REASON_CASE
+	}
+
+	return "ERROR";
+}
+
+static void iwl_mld_print_emlsr_exit(struct iwl_mld *mld, u32 mask)
+{
+#define NAME_FMT(x) "%s"
+#define NAME_PR(x) (mask & IWL_MLD_EMLSR_EXIT_##x) ? "[" #x "]" : "",
+	IWL_DEBUG_INFO(mld,
+		       "EMLSR exit = " HANDLE_EMLSR_EXIT_REASONS(NAME_FMT)
+		       " (0x%x)\n",
+		       HANDLE_EMLSR_EXIT_REASONS(NAME_PR)
+		       mask);
+#undef NAME_FMT
+#undef NAME_PR
+}
+
+void iwl_mld_emlsr_prevent_done_wk(struct wiphy *wiphy, struct wiphy_work *wk)
+{
+	struct iwl_mld_vif *mld_vif = container_of(wk, struct iwl_mld_vif,
+						   emlsr.prevent_done_wk.work);
+	struct ieee80211_vif *vif =
+		container_of((void *)mld_vif, struct ieee80211_vif, drv_priv);
+
+	if (WARN_ON(!(mld_vif->emlsr.blocked_reasons &
+		      IWL_MLD_EMLSR_BLOCKED_PREVENTION)))
+		return;
+
+	iwl_mld_unblock_emlsr(mld_vif->mld, vif,
+			      IWL_MLD_EMLSR_BLOCKED_PREVENTION);
+}
+
+void iwl_mld_emlsr_tmp_non_bss_done_wk(struct wiphy *wiphy,
+				       struct wiphy_work *wk)
+{
+	struct iwl_mld_vif *mld_vif = container_of(wk, struct iwl_mld_vif,
+						   emlsr.tmp_non_bss_done_wk.work);
+	struct ieee80211_vif *vif =
+		container_of((void *)mld_vif, struct ieee80211_vif, drv_priv);
+
+	if (WARN_ON(!(mld_vif->emlsr.blocked_reasons &
+		      IWL_MLD_EMLSR_BLOCKED_TMP_NON_BSS)))
+		return;
+
+	iwl_mld_unblock_emlsr(mld_vif->mld, vif,
+			      IWL_MLD_EMLSR_BLOCKED_TMP_NON_BSS);
+}
+
+#define IWL_MLD_TRIGGER_LINK_SEL_TIME	(HZ * IWL_MLD_TRIGGER_LINK_SEL_TIME_SEC)
+#define IWL_MLD_SCAN_EXPIRE_TIME	(HZ * IWL_MLD_SCAN_EXPIRE_TIME_SEC)
+
+/* Exit reasons that can cause longer EMLSR prevention */
+#define IWL_MLD_PREVENT_EMLSR_REASONS	(IWL_MLD_EMLSR_EXIT_MISSED_BEACON	| \
+					 IWL_MLD_EMLSR_EXIT_LINK_USAGE		| \
+					 IWL_MLD_EMLSR_EXIT_FW_REQUEST)
+#define IWL_MLD_PREVENT_EMLSR_TIMEOUT	(HZ * 400)
+
+#define IWL_MLD_EMLSR_PREVENT_SHORT	(HZ * 300)
+#define IWL_MLD_EMLSR_PREVENT_LONG	(HZ * 600)
+
+static void iwl_mld_check_emlsr_prevention(struct iwl_mld *mld,
+					   struct iwl_mld_vif *mld_vif,
+					   enum iwl_mld_emlsr_exit reason)
+{
+	unsigned long delay;
+
+	/*
+	 * Reset the counter if more than 400 seconds have passed between one
+	 * exit and the other, or if we exited due to a different reason.
+	 * Will also reset the counter after the long prevention is done.
+	 */
+	if (time_after(jiffies, mld_vif->emlsr.last_exit_ts +
+				IWL_MLD_PREVENT_EMLSR_TIMEOUT) ||
+	    mld_vif->emlsr.last_exit_reason != reason)
+		mld_vif->emlsr.exit_repeat_count = 0;
+
+	mld_vif->emlsr.last_exit_reason = reason;
+	mld_vif->emlsr.last_exit_ts = jiffies;
+	mld_vif->emlsr.exit_repeat_count++;
+
+	/*
+	 * Do not add a prevention when the reason was a block. For a block,
+	 * EMLSR will be enabled again on unblock.
+	 */
+	if (reason == IWL_MLD_EMLSR_EXIT_BLOCK)
+		return;
+
+	/* Set prevention for a minimum of 30 seconds */
+	mld_vif->emlsr.blocked_reasons |= IWL_MLD_EMLSR_BLOCKED_PREVENTION;
+	delay = IWL_MLD_TRIGGER_LINK_SEL_TIME;
+
+	/* Handle repeats for reasons that can cause long prevention */
+	if (mld_vif->emlsr.exit_repeat_count > 1 &&
+	    reason & IWL_MLD_PREVENT_EMLSR_REASONS) {
+		if (mld_vif->emlsr.exit_repeat_count == 2)
+			delay = IWL_MLD_EMLSR_PREVENT_SHORT;
+		else
+			delay = IWL_MLD_EMLSR_PREVENT_LONG;
+
+		/*
+		 * The timeouts are chosen so that this will not happen, i.e.
+		 * IWL_MLD_EMLSR_PREVENT_LONG > IWL_MLD_PREVENT_EMLSR_TIMEOUT
+		 */
+		WARN_ON(mld_vif->emlsr.exit_repeat_count > 3);
+	}
+
+	IWL_DEBUG_INFO(mld,
+		       "Preventing EMLSR for %ld seconds due to %u exits with the reason = %s (0x%x)\n",
+		       delay / HZ, mld_vif->emlsr.exit_repeat_count,
+		       iwl_mld_get_emlsr_exit_string(reason), reason);
+
+	wiphy_delayed_work_queue(mld->wiphy,
+				 &mld_vif->emlsr.prevent_done_wk, delay);
+}
+
+static void iwl_mld_clear_avg_chan_load_iter(struct ieee80211_hw *hw,
+					     struct ieee80211_chanctx_conf *ctx,
+					     void *dat)
+{
+	struct iwl_mld_phy *phy = iwl_mld_phy_from_mac80211(ctx);
+
+	/* It is ok to do it for all chanctx (and not only for the ones that
+	 * belong to the EMLSR vif) since EMLSR is not allowed if there is
+	 * another vif.
+	 */
+	phy->avg_channel_load_not_by_us = 0;
+}
+
+static int _iwl_mld_exit_emlsr(struct iwl_mld *mld, struct ieee80211_vif *vif,
+			       enum iwl_mld_emlsr_exit exit, u8 link_to_keep,
+			       bool sync)
+{
+	struct iwl_mld_vif *mld_vif = iwl_mld_vif_from_mac80211(vif);
+	u16 new_active_links;
+	int ret = 0;
+
+	lockdep_assert_wiphy(mld->wiphy);
+
+	/* On entry failure need to exit anyway, even if entered from debugfs */
+	if (exit != IWL_MLD_EMLSR_EXIT_FAIL_ENTRY && !IWL_MLD_AUTO_EML_ENABLE)
+		return 0;
+
+	/* Ignore exit request if EMLSR is not active */
+	if (!iwl_mld_emlsr_active(vif))
+		return 0;
+
+	if (WARN_ON(!ieee80211_vif_is_mld(vif) || !mld_vif->authorized))
+		return 0;
+
+	if (WARN_ON(!(vif->active_links & BIT(link_to_keep))))
+		link_to_keep = __ffs(vif->active_links);
+
+	new_active_links = BIT(link_to_keep);
+	IWL_DEBUG_INFO(mld,
+		       "Exiting EMLSR. reason = %s (0x%x). Current active links=0x%x, new active links = 0x%x\n",
+		       iwl_mld_get_emlsr_exit_string(exit), exit,
+		       vif->active_links, new_active_links);
+
+	if (sync)
+		ret = ieee80211_set_active_links(vif, new_active_links);
+	else
+		ieee80211_set_active_links_async(vif, new_active_links);
+
+	/* Update latest exit reason and check EMLSR prevention */
+	iwl_mld_check_emlsr_prevention(mld, mld_vif, exit);
+
+	/* channel_load_not_by_us is invalid when in EMLSR.
+	 * Clear it so wrong values won't be used.
+	 */
+	ieee80211_iter_chan_contexts_atomic(mld->hw,
+					    iwl_mld_clear_avg_chan_load_iter,
+					    NULL);
+
+	return ret;
+}
+
+void iwl_mld_exit_emlsr(struct iwl_mld *mld, struct ieee80211_vif *vif,
+			enum iwl_mld_emlsr_exit exit, u8 link_to_keep)
+{
+	_iwl_mld_exit_emlsr(mld, vif, exit, link_to_keep, false);
+}
+
+static int _iwl_mld_emlsr_block(struct iwl_mld *mld, struct ieee80211_vif *vif,
+				enum iwl_mld_emlsr_blocked reason,
+				u8 link_to_keep, bool sync)
+{
+	struct iwl_mld_vif *mld_vif = iwl_mld_vif_from_mac80211(vif);
+
+	lockdep_assert_wiphy(mld->wiphy);
+
+	if (!IWL_MLD_AUTO_EML_ENABLE || !iwl_mld_vif_has_emlsr_cap(vif))
+		return 0;
+
+	if (mld_vif->emlsr.blocked_reasons & reason)
+		return 0;
+
+	mld_vif->emlsr.blocked_reasons |= reason;
+
+	IWL_DEBUG_INFO(mld,
+		       "Blocking EMLSR mode. reason = %s (0x%x)\n",
+		       iwl_mld_get_emlsr_blocked_string(reason), reason);
+	iwl_mld_print_emlsr_blocked(mld, mld_vif->emlsr.blocked_reasons);
+
+	if (reason == IWL_MLD_EMLSR_BLOCKED_TPT)
+		wiphy_delayed_work_cancel(mld_vif->mld->wiphy,
+					  &mld_vif->emlsr.check_tpt_wk);
+
+	return _iwl_mld_exit_emlsr(mld, vif, IWL_MLD_EMLSR_EXIT_BLOCK,
+				   link_to_keep, sync);
+}
+
+void iwl_mld_block_emlsr(struct iwl_mld *mld, struct ieee80211_vif *vif,
+			 enum iwl_mld_emlsr_blocked reason, u8 link_to_keep)
+{
+	_iwl_mld_emlsr_block(mld, vif, reason, link_to_keep, false);
+}
+
+int iwl_mld_block_emlsr_sync(struct iwl_mld *mld, struct ieee80211_vif *vif,
+			     enum iwl_mld_emlsr_blocked reason, u8 link_to_keep)
+{
+	return _iwl_mld_emlsr_block(mld, vif, reason, link_to_keep, true);
+}
+
+static void _iwl_mld_select_links(struct iwl_mld *mld,
+				  struct ieee80211_vif *vif);
+
+void iwl_mld_unblock_emlsr(struct iwl_mld *mld, struct ieee80211_vif *vif,
+			   enum iwl_mld_emlsr_blocked reason)
+{
+	struct iwl_mld_vif *mld_vif = iwl_mld_vif_from_mac80211(vif);
+
+	lockdep_assert_wiphy(mld->wiphy);
+
+	if (!IWL_MLD_AUTO_EML_ENABLE || !iwl_mld_vif_has_emlsr_cap(vif))
+		return;
+
+	if (!(mld_vif->emlsr.blocked_reasons & reason))
+		return;
+
+	mld_vif->emlsr.blocked_reasons &= ~reason;
+
+	IWL_DEBUG_INFO(mld,
+		       "Unblocking EMLSR mode. reason = %s (0x%x)\n",
+		       iwl_mld_get_emlsr_blocked_string(reason), reason);
+	iwl_mld_print_emlsr_blocked(mld, mld_vif->emlsr.blocked_reasons);
+
+	if (reason == IWL_MLD_EMLSR_BLOCKED_TPT)
+		wiphy_delayed_work_queue(mld_vif->mld->wiphy,
+					 &mld_vif->emlsr.check_tpt_wk,
+					 round_jiffies_relative(IWL_MLD_TPT_COUNT_WINDOW));
+
+	if (mld_vif->emlsr.blocked_reasons)
+		return;
+
+	IWL_DEBUG_INFO(mld, "EMLSR is unblocked\n");
+	iwl_mld_int_mlo_scan(mld, vif);
+}
+
+static void
+iwl_mld_vif_iter_emlsr_mode_notif(void *data, u8 *mac,
+				  struct ieee80211_vif *vif)
+{
+	struct iwl_mld_vif *mld_vif = iwl_mld_vif_from_mac80211(vif);
+	struct iwl_esr_mode_notif *notif = (void *)data;
+
+	if (!iwl_mld_vif_has_emlsr_cap(vif))
+		return;
+
+	switch (le32_to_cpu(notif->action)) {
+	case ESR_RECOMMEND_LEAVE:
+		iwl_mld_exit_emlsr(mld_vif->mld, vif,
+				   IWL_MLD_EMLSR_EXIT_FW_REQUEST,
+				   iwl_mld_get_primary_link(vif));
+		break;
+	case ESR_RECOMMEND_ENTER:
+	case ESR_FORCE_LEAVE:
+	default:
+		IWL_WARN(mld_vif->mld, "Unexpected EMLSR notification: %d\n",
+			 le32_to_cpu(notif->action));
+	}
+}
+
+void iwl_mld_handle_emlsr_mode_notif(struct iwl_mld *mld,
+				     struct iwl_rx_packet *pkt)
+{
+	ieee80211_iterate_active_interfaces_mtx(mld->hw,
+						IEEE80211_IFACE_ITER_NORMAL,
+						iwl_mld_vif_iter_emlsr_mode_notif,
+						pkt->data);
+}
+
+static void
+iwl_mld_vif_iter_disconnect_emlsr(void *data, u8 *mac,
+				  struct ieee80211_vif *vif)
+{
+	if (!iwl_mld_vif_has_emlsr_cap(vif))
+		return;
+
+	ieee80211_connection_loss(vif);
+}
+
+void iwl_mld_handle_emlsr_trans_fail_notif(struct iwl_mld *mld,
+					   struct iwl_rx_packet *pkt)
+{
+	const struct iwl_esr_trans_fail_notif *notif = (const void *)pkt->data;
+	u32 fw_link_id = le32_to_cpu(notif->link_id);
+	struct ieee80211_bss_conf *bss_conf =
+		iwl_mld_fw_id_to_link_conf(mld, fw_link_id);
+
+	IWL_DEBUG_INFO(mld, "Failed to %s EMLSR on link %d (FW: %d), reason %d\n",
+		       le32_to_cpu(notif->activation) ? "enter" : "exit",
+		       bss_conf ? bss_conf->link_id : -1,
+		       le32_to_cpu(notif->link_id),
+		       le32_to_cpu(notif->err_code));
+
+	if (IWL_FW_CHECK(mld, !bss_conf,
+			 "FW reported failure to %sactivate EMLSR on a non-existing link: %d\n",
+			 le32_to_cpu(notif->activation) ? "" : "de",
+			 fw_link_id)) {
+		ieee80211_iterate_active_interfaces_mtx(
+			mld->hw, IEEE80211_IFACE_ITER_NORMAL,
+			iwl_mld_vif_iter_disconnect_emlsr, NULL);
+		return;
+	}
+
+	/* Disconnect if we failed to deactivate a link */
+	if (!le32_to_cpu(notif->activation)) {
+		ieee80211_connection_loss(bss_conf->vif);
+		return;
+	}
+
+	/*
+	 * We failed to activate the second link, go back to the link specified
+	 * by the firmware as that is the one that is still valid now.
+	 */
+	iwl_mld_exit_emlsr(mld, bss_conf->vif, IWL_MLD_EMLSR_EXIT_FAIL_ENTRY,
+			   bss_conf->link_id);
+}
+
+/* Active non-station link tracking */
+static void iwl_mld_count_non_bss_links(void *_data, u8 *mac,
+					struct ieee80211_vif *vif)
+{
+	struct iwl_mld_vif *mld_vif = iwl_mld_vif_from_mac80211(vif);
+	int *count = _data;
+
+	if (ieee80211_vif_type_p2p(vif) == NL80211_IFTYPE_STATION)
+		return;
+
+	*count += iwl_mld_count_active_links(mld_vif->mld, vif);
+}
+
+struct iwl_mld_update_emlsr_block_data {
+	bool block;
+	int result;
+};
+
+static void
+iwl_mld_vif_iter_update_emlsr_non_bss_block(void *_data, u8 *mac,
+					    struct ieee80211_vif *vif)
+{
+	struct iwl_mld_update_emlsr_block_data *data = _data;
+	struct iwl_mld_vif *mld_vif = iwl_mld_vif_from_mac80211(vif);
+	int ret;
+
+	if (data->block) {
+		ret = iwl_mld_block_emlsr_sync(mld_vif->mld, vif,
+					       IWL_MLD_EMLSR_BLOCKED_NON_BSS,
+					       iwl_mld_get_primary_link(vif));
+		if (ret)
+			data->result = ret;
+	} else {
+		iwl_mld_unblock_emlsr(mld_vif->mld, vif,
+				      IWL_MLD_EMLSR_BLOCKED_NON_BSS);
+	}
+}
+
+int iwl_mld_emlsr_check_non_bss_block(struct iwl_mld *mld,
+				      int pending_link_changes)
+{
+	/* An active link of a non-station vif blocks EMLSR. Upon activation
+	 * block EMLSR on the bss vif. Upon deactivation, check if this link
+	 * was the last non-station link active, and if so unblock the bss vif
+	 */
+	struct iwl_mld_update_emlsr_block_data block_data = {};
+	int count = pending_link_changes;
+
+	/* No need to count if we are activating a non-BSS link */
+	if (count <= 0)
+		ieee80211_iterate_active_interfaces_mtx(mld->hw,
+							IEEE80211_IFACE_ITER_NORMAL,
+							iwl_mld_count_non_bss_links,
+							&count);
+
+	/*
+	 * We could skip updating it if the block change did not change (and
+	 * pending_link_changes is non-zero).
+	 */
+	block_data.block = !!count;
+
+	ieee80211_iterate_active_interfaces_mtx(mld->hw,
+						IEEE80211_IFACE_ITER_NORMAL,
+						iwl_mld_vif_iter_update_emlsr_non_bss_block,
+						&block_data);
+
+	return block_data.result;
+}
+
+#define EMLSR_SEC_LINK_MIN_PERC 10
+#define EMLSR_MIN_TX 3000
+#define EMLSR_MIN_RX 400
+
+void iwl_mld_emlsr_check_tpt(struct wiphy *wiphy, struct wiphy_work *wk)
+{
+	struct iwl_mld_vif *mld_vif = container_of(wk, struct iwl_mld_vif,
+						   emlsr.check_tpt_wk.work);
+	struct ieee80211_vif *vif =
+		container_of((void *)mld_vif, struct ieee80211_vif, drv_priv);
+	struct iwl_mld *mld = mld_vif->mld;
+	struct iwl_mld_sta *mld_sta;
+	struct iwl_mld_link *sec_link;
+	unsigned long total_tx = 0, total_rx = 0;
+	unsigned long sec_link_tx = 0, sec_link_rx = 0;
+	u8 sec_link_tx_perc, sec_link_rx_perc;
+	s8 sec_link_id;
+
+	if (!iwl_mld_vif_has_emlsr_cap(vif) || !mld_vif->ap_sta)
+		return;
+
+	mld_sta = iwl_mld_sta_from_mac80211(mld_vif->ap_sta);
+
+	/* We only count for the AP sta in a MLO connection */
+	if (!mld_sta->mpdu_counters)
+		return;
+
+	/* This wk should only run when the TPT blocker isn't set.
+	 * When the blocker is set, the decision to remove it, as well as
+	 * clearing the counters is done in DP (to avoid having a wk every
+	 * 5 seconds when idle. When the blocker is unset, we are not idle anyway)
+	 */
+	if (WARN_ON(mld_vif->emlsr.blocked_reasons & IWL_MLD_EMLSR_BLOCKED_TPT))
+		return;
+	/*
+	 * TPT is unblocked, need to check if the TPT criteria is still met.
+	 *
+	 * If EMLSR is active, then we also need to check the secondar link
+	 * requirements.
+	 */
+	if (iwl_mld_emlsr_active(vif)) {
+		sec_link_id = iwl_mld_get_other_link(vif, iwl_mld_get_primary_link(vif));
+		sec_link = iwl_mld_link_dereference_check(mld_vif, sec_link_id);
+		if (WARN_ON_ONCE(!sec_link))
+			return;
+		/* We need the FW ID here */
+		sec_link_id = sec_link->fw_id;
+	} else {
+		sec_link_id = -1;
+	}
+
+	/* Sum up RX and TX MPDUs from the different queues/links */
+	for (int q = 0; q < mld->trans->num_rx_queues; q++) {
+		struct iwl_mld_per_q_mpdu_counter *queue_counter =
+			&mld_sta->mpdu_counters[q];
+
+		spin_lock_bh(&queue_counter->lock);
+
+		/* The link IDs that doesn't exist will contain 0 */
+		for (int link = 0;
+		     link < ARRAY_SIZE(queue_counter->per_link);
+		     link++) {
+			total_tx += queue_counter->per_link[link].tx;
+			total_rx += queue_counter->per_link[link].rx;
+		}
+
+		if (sec_link_id != -1) {
+			sec_link_tx += queue_counter->per_link[sec_link_id].tx;
+			sec_link_rx += queue_counter->per_link[sec_link_id].rx;
+		}
+
+		memset(queue_counter->per_link, 0,
+		       sizeof(queue_counter->per_link));
+
+		spin_unlock_bh(&queue_counter->lock);
+	}
+
+	IWL_DEBUG_INFO(mld, "total Tx MPDUs: %ld. total Rx MPDUs: %ld\n",
+		       total_tx, total_rx);
+
+	/* If we don't have enough MPDUs - exit EMLSR */
+	if (total_tx < IWL_MLD_ENTER_EMLSR_TPT_THRESH &&
+	    total_rx < IWL_MLD_ENTER_EMLSR_TPT_THRESH) {
+		iwl_mld_block_emlsr(mld, vif, IWL_MLD_EMLSR_BLOCKED_TPT,
+				    iwl_mld_get_primary_link(vif));
+		return;
+	}
+
+	/* EMLSR is not active */
+	if (sec_link_id == -1)
+		return;
+
+	IWL_DEBUG_INFO(mld, "Secondary Link %d: Tx MPDUs: %ld. Rx MPDUs: %ld\n",
+		       sec_link_id, sec_link_tx, sec_link_rx);
+
+	/* Calculate the percentage of the secondary link TX/RX */
+	sec_link_tx_perc = total_tx ? sec_link_tx * 100 / total_tx : 0;
+	sec_link_rx_perc = total_rx ? sec_link_rx * 100 / total_rx : 0;
+
+	/*
+	 * The TX/RX percentage is checked only if it exceeds the required
+	 * minimum. In addition, RX is checked only if the TX check failed.
+	 */
+	if ((total_tx > EMLSR_MIN_TX &&
+	     sec_link_tx_perc < EMLSR_SEC_LINK_MIN_PERC) ||
+	    (total_rx > EMLSR_MIN_RX &&
+	     sec_link_rx_perc < EMLSR_SEC_LINK_MIN_PERC)) {
+		iwl_mld_exit_emlsr(mld, vif, IWL_MLD_EMLSR_EXIT_LINK_USAGE,
+				   iwl_mld_get_primary_link(vif));
+		return;
+	}
+
+	/* Check again when the next window ends  */
+	wiphy_delayed_work_queue(mld_vif->mld->wiphy,
+				 &mld_vif->emlsr.check_tpt_wk,
+				 round_jiffies_relative(IWL_MLD_TPT_COUNT_WINDOW));
+}
+
+void iwl_mld_emlsr_unblock_tpt_wk(struct wiphy *wiphy, struct wiphy_work *wk)
+{
+	struct iwl_mld_vif *mld_vif = container_of(wk, struct iwl_mld_vif,
+						   emlsr.unblock_tpt_wk);
+	struct ieee80211_vif *vif =
+		container_of((void *)mld_vif, struct ieee80211_vif, drv_priv);
+
+	iwl_mld_unblock_emlsr(mld_vif->mld, vif, IWL_MLD_EMLSR_BLOCKED_TPT);
+}
+
+/*
+ * Link selection
+ */
+
+s8 iwl_mld_get_emlsr_rssi_thresh(struct iwl_mld *mld,
+				 const struct cfg80211_chan_def *chandef,
+				 bool low)
+{
+	if (WARN_ON(chandef->chan->band != NL80211_BAND_2GHZ &&
+		    chandef->chan->band != NL80211_BAND_5GHZ &&
+		    chandef->chan->band != NL80211_BAND_6GHZ))
+		return S8_MAX;
+
+#define RSSI_THRESHOLD(_low, _bw)			\
+	(_low) ? IWL_MLD_LOW_RSSI_THRESH_##_bw##MHZ	\
+	       : IWL_MLD_HIGH_RSSI_THRESH_##_bw##MHZ
+
+	switch (chandef->width) {
+	case NL80211_CHAN_WIDTH_20_NOHT:
+	case NL80211_CHAN_WIDTH_20:
+	/* 320 MHz has the same thresholds as 20 MHz */
+	case NL80211_CHAN_WIDTH_320:
+		return RSSI_THRESHOLD(low, 20);
+	case NL80211_CHAN_WIDTH_40:
+		return RSSI_THRESHOLD(low, 40);
+	case NL80211_CHAN_WIDTH_80:
+		return RSSI_THRESHOLD(low, 80);
+	case NL80211_CHAN_WIDTH_160:
+		return RSSI_THRESHOLD(low, 160);
+	default:
+		WARN_ON(1);
+		return S8_MAX;
+	}
+#undef RSSI_THRESHOLD
+}
+
+static u32
+iwl_mld_emlsr_disallowed_with_link(struct iwl_mld *mld,
+				   struct ieee80211_vif *vif,
+				   struct iwl_mld_link_sel_data *link,
+				   bool primary)
+{
+	struct wiphy *wiphy = mld->wiphy;
+	struct ieee80211_bss_conf *conf;
+	enum iwl_mld_emlsr_exit ret = 0;
+
+	conf = wiphy_dereference(wiphy, vif->link_conf[link->link_id]);
+	if (WARN_ON_ONCE(!conf))
+		return false;
+
+	if (link->chandef->chan->band == NL80211_BAND_2GHZ && mld->bt_is_active)
+		ret |= IWL_MLD_EMLSR_EXIT_BT_COEX;
+
+	if (link->signal <
+	    iwl_mld_get_emlsr_rssi_thresh(mld, link->chandef, false))
+		ret |= IWL_MLD_EMLSR_EXIT_LOW_RSSI;
+
+	if (conf->csa_active)
+		ret |= IWL_MLD_EMLSR_EXIT_CSA;
+
+	if (ret) {
+		IWL_DEBUG_INFO(mld,
+			       "Link %d is not allowed for EMLSR as %s\n",
+			       link->link_id,
+			       primary ? "primary" : "secondary");
+		iwl_mld_print_emlsr_exit(mld, ret);
+	}
+
+	return ret;
+}
+
+static u8
+iwl_mld_set_link_sel_data(struct iwl_mld *mld,
+			  struct ieee80211_vif *vif,
+			  struct iwl_mld_link_sel_data *data,
+			  unsigned long usable_links,
+			  u8 *best_link_idx)
+{
+	u8 n_data = 0;
+	u16 max_grade = 0;
+	unsigned long link_id;
+
+	/*
+	 * TODO: don't select links that weren't discovered in the last scan
+	 * This requires mac80211 (or cfg80211) changes to forward/track when
+	 * a BSS was last updated. cfg80211 already tracks this information but
+	 * it is not exposed within the kernel.
+	 */
+	for_each_set_bit(link_id, &usable_links, IEEE80211_MLD_MAX_NUM_LINKS) {
+		struct ieee80211_bss_conf *link_conf =
+			link_conf_dereference_protected(vif, link_id);
+
+		if (WARN_ON_ONCE(!link_conf))
+			continue;
+
+		/* Ignore any BSS that was not seen in the last MLO scan */
+		if (ktime_before(link_conf->bss->ts_boottime,
+				 mld->scan.last_mlo_scan_time))
+			continue;
+
+		data[n_data].link_id = link_id;
+		data[n_data].chandef = &link_conf->chanreq.oper;
+		data[n_data].signal = MBM_TO_DBM(link_conf->bss->signal);
+		data[n_data].grade = iwl_mld_get_link_grade(mld, link_conf);
+
+		if (n_data == 0 || data[n_data].grade > max_grade) {
+			max_grade = data[n_data].grade;
+			*best_link_idx = n_data;
+		}
+		n_data++;
+	}
+
+	return n_data;
+}
+
+static u32
+iwl_mld_get_min_chan_load_thresh(struct ieee80211_chanctx_conf *chanctx)
+{
+	const struct iwl_mld_phy *phy = iwl_mld_phy_from_mac80211(chanctx);
+
+	switch (phy->chandef.width) {
+	case NL80211_CHAN_WIDTH_320:
+	case NL80211_CHAN_WIDTH_160:
+		return 5;
+	case NL80211_CHAN_WIDTH_80:
+		return 7;
+	default:
+		break;
+	}
+	return 10;
+}
+
+VISIBLE_IF_IWLWIFI_KUNIT bool
+iwl_mld_channel_load_allows_emlsr(struct iwl_mld *mld,
+				  struct ieee80211_vif *vif,
+				  const struct iwl_mld_link_sel_data *a,
+				  const struct iwl_mld_link_sel_data *b)
+{
+	struct iwl_mld_vif *mld_vif = iwl_mld_vif_from_mac80211(vif);
+	struct iwl_mld_link *link_a =
+		iwl_mld_link_dereference_check(mld_vif, a->link_id);
+	struct ieee80211_chanctx_conf *chanctx_a = NULL;
+	u32 bw_a, bw_b, ratio;
+	u32 primary_load_perc;
+
+	if (!link_a || !link_a->active) {
+		IWL_DEBUG_EHT(mld, "Primary link is not active. Can't enter EMLSR\n");
+		return false;
+	}
+
+	chanctx_a = wiphy_dereference(mld->wiphy, link_a->chan_ctx);
+
+	if (WARN_ON(!chanctx_a))
+		return false;
+
+	primary_load_perc =
+		iwl_mld_phy_from_mac80211(chanctx_a)->avg_channel_load_not_by_us;
+
+	IWL_DEBUG_EHT(mld, "Average channel load not by us: %u\n", primary_load_perc);
+
+	if (primary_load_perc < iwl_mld_get_min_chan_load_thresh(chanctx_a)) {
+		IWL_DEBUG_EHT(mld, "Channel load is below the minimum threshold\n");
+		return false;
+	}
+
+	if (iwl_mld_vif_low_latency(mld_vif)) {
+		IWL_DEBUG_EHT(mld, "Low latency vif, EMLSR is allowed\n");
+		return true;
+	}
+
+	if (a->chandef->width <= b->chandef->width)
+		return true;
+
+	bw_a = nl80211_chan_width_to_mhz(a->chandef->width);
+	bw_b = nl80211_chan_width_to_mhz(b->chandef->width);
+	ratio = bw_a / bw_b;
+
+	switch (ratio) {
+	case 2:
+		return primary_load_perc > 25;
+	case 4:
+		return primary_load_perc > 40;
+	case 8:
+	case 16:
+		return primary_load_perc > 50;
+	}
+
+	return false;
+}
+EXPORT_SYMBOL_IF_IWLWIFI_KUNIT(iwl_mld_channel_load_allows_emlsr);
+
+static bool
+iwl_mld_valid_emlsr_pair(struct ieee80211_vif *vif,
+			 struct iwl_mld_link_sel_data *a,
+			 struct iwl_mld_link_sel_data *b)
+{
+	struct iwl_mld_vif *mld_vif = iwl_mld_vif_from_mac80211(vif);
+	struct iwl_mld *mld = mld_vif->mld;
+	u32 reason_mask = 0;
+
+	/* Per-link considerations */
+	if (iwl_mld_emlsr_disallowed_with_link(mld, vif, a, true) ||
+	    iwl_mld_emlsr_disallowed_with_link(mld, vif, b, false))
+		return false;
+
+	if (a->chandef->chan->band == b->chandef->chan->band)
+		reason_mask |= IWL_MLD_EMLSR_EXIT_EQUAL_BAND;
+	if (!iwl_mld_channel_load_allows_emlsr(mld, vif, a, b))
+		reason_mask |= IWL_MLD_EMLSR_EXIT_CHAN_LOAD;
+
+	if (reason_mask) {
+		IWL_DEBUG_INFO(mld,
+			       "Links %d and %d are not a valid pair for EMLSR\n",
+			       a->link_id, b->link_id);
+		IWL_DEBUG_INFO(mld,
+			       "Links bandwidth are: %d and %d\n",
+			       nl80211_chan_width_to_mhz(a->chandef->width),
+			       nl80211_chan_width_to_mhz(b->chandef->width));
+		iwl_mld_print_emlsr_exit(mld, reason_mask);
+		return false;
+	}
+
+	return true;
+}
+
+/* Calculation is done with fixed-point with a scaling factor of 1/256 */
+#define SCALE_FACTOR 256
+
+/*
+ * Returns the combined grade of two given links.
+ * Returns 0 if EMLSR is not allowed with these 2 links.
+ */
+static
+unsigned int iwl_mld_get_emlsr_grade(struct iwl_mld *mld,
+				     struct ieee80211_vif *vif,
+				     struct iwl_mld_link_sel_data *a,
+				     struct iwl_mld_link_sel_data *b,
+				     u8 *primary_id)
+{
+	struct ieee80211_bss_conf *primary_conf;
+	struct wiphy *wiphy = ieee80211_vif_to_wdev(vif)->wiphy;
+	unsigned int primary_load;
+
+	lockdep_assert_wiphy(wiphy);
+
+	/* a is always primary, b is always secondary */
+	if (b->grade > a->grade)
+		swap(a, b);
+
+	*primary_id = a->link_id;
+
+	if (!iwl_mld_valid_emlsr_pair(vif, a, b))
+		return 0;
+
+	primary_conf = wiphy_dereference(wiphy, vif->link_conf[*primary_id]);
+
+	if (WARN_ON_ONCE(!primary_conf))
+		return 0;
+
+	primary_load = iwl_mld_get_chan_load(mld, primary_conf);
+
+	/* The more the primary link is loaded, the more worthwhile EMLSR becomes */
+	return a->grade + ((b->grade * primary_load) / SCALE_FACTOR);
+}
+
+static void _iwl_mld_select_links(struct iwl_mld *mld,
+				  struct ieee80211_vif *vif)
+{
+	struct iwl_mld_link_sel_data data[IEEE80211_MLD_MAX_NUM_LINKS];
+	struct iwl_mld_link_sel_data *best_link;
+	struct iwl_mld_vif *mld_vif = iwl_mld_vif_from_mac80211(vif);
+	int max_active_links = iwl_mld_max_active_links(mld, vif);
+	u16 new_active, usable_links = ieee80211_vif_usable_links(vif);
+	u8 best_idx, new_primary, n_data;
+	u16 max_grade;
+
+	lockdep_assert_wiphy(mld->wiphy);
+
+	if (!mld_vif->authorized || hweight16(usable_links) <= 1)
+		return;
+
+	if (WARN(ktime_before(mld->scan.last_mlo_scan_time,
+			      ktime_sub_ns(ktime_get_boottime_ns(),
+					   5ULL * NSEC_PER_SEC)),
+		"Last MLO scan was too long ago, can't select links\n"))
+		return;
+
+	/* The logic below is simple and not suited for more than 2 links */
+	WARN_ON_ONCE(max_active_links > 2);
+
+	n_data = iwl_mld_set_link_sel_data(mld, vif, data, usable_links,
+					   &best_idx);
+
+	if (WARN(!n_data, "Couldn't find a valid grade for any link!\n"))
+		return;
+
+	/* Default to selecting the single best link */
+	best_link = &data[best_idx];
+	new_primary = best_link->link_id;
+	new_active = BIT(best_link->link_id);
+	max_grade = best_link->grade;
+
+	/* If EMLSR is not possible, activate the best link */
+	if (max_active_links == 1 || n_data == 1 ||
+	    !iwl_mld_vif_has_emlsr_cap(vif) || !IWL_MLD_AUTO_EML_ENABLE ||
+	    mld_vif->emlsr.blocked_reasons)
+		goto set_active;
+
+	/* Try to find the best link combination */
+	for (u8 a = 0; a < n_data; a++) {
+		for (u8 b = a + 1; b < n_data; b++) {
+			u8 best_in_pair;
+			u16 emlsr_grade =
+				iwl_mld_get_emlsr_grade(mld, vif,
+							&data[a], &data[b],
+							&best_in_pair);
+
+			/*
+			 * Prefer (new) EMLSR combination to prefer EMLSR over
+			 * a single link.
+			 */
+			if (emlsr_grade < max_grade)
+				continue;
+
+			max_grade = emlsr_grade;
+			new_primary = best_in_pair;
+			new_active = BIT(data[a].link_id) |
+				     BIT(data[b].link_id);
+		}
+	}
+
+set_active:
+	IWL_DEBUG_INFO(mld, "Link selection result: 0x%x. Primary = %d\n",
+		       new_active, new_primary);
+
+	mld_vif->emlsr.selected_primary = new_primary;
+	mld_vif->emlsr.selected_links = new_active;
+
+	ieee80211_set_active_links_async(vif, new_active);
+}
+
+static void iwl_mld_vif_iter_select_links(void *_data, u8 *mac,
+					  struct ieee80211_vif *vif)
+{
+	struct iwl_mld_vif *mld_vif = iwl_mld_vif_from_mac80211(vif);
+	struct iwl_mld *mld = mld_vif->mld;
+
+	_iwl_mld_select_links(mld, vif);
+}
+
+void iwl_mld_select_links(struct iwl_mld *mld)
+{
+	ieee80211_iterate_active_interfaces_mtx(mld->hw,
+						IEEE80211_IFACE_ITER_NORMAL,
+						iwl_mld_vif_iter_select_links,
+						NULL);
+}
+
+static void iwl_mld_emlsr_check_bt_iter(void *_data, u8 *mac,
+					struct ieee80211_vif *vif)
+{
+	struct iwl_mld_vif *mld_vif = iwl_mld_vif_from_mac80211(vif);
+	struct iwl_mld *mld = mld_vif->mld;
+	struct ieee80211_bss_conf *link;
+	unsigned int link_id;
+
+	if (!mld->bt_is_active) {
+		iwl_mld_retry_emlsr(mld, vif);
+		return;
+	}
+
+	/* BT is turned ON but we are not in EMLSR, nothing to do */
+	if (!iwl_mld_emlsr_active(vif))
+		return;
+
+	/* In EMLSR and BT is turned ON */
+
+	for_each_vif_active_link(vif, link, link_id) {
+		if (WARN_ON(!link->chanreq.oper.chan))
+			continue;
+
+		if (link->chanreq.oper.chan->band == NL80211_BAND_2GHZ) {
+			iwl_mld_exit_emlsr(mld, vif, IWL_MLD_EMLSR_EXIT_BT_COEX,
+					   iwl_mld_get_primary_link(vif));
+			return;
+		}
+	}
+}
+
+void iwl_mld_emlsr_check_bt(struct iwl_mld *mld)
+{
+	ieee80211_iterate_active_interfaces_mtx(mld->hw,
+						IEEE80211_IFACE_ITER_NORMAL,
+						iwl_mld_emlsr_check_bt_iter,
+						NULL);
+}
+
+struct iwl_mld_chan_load_data {
+	struct iwl_mld_phy *phy;
+	u32 prev_chan_load_not_by_us;
+};
+
+static void iwl_mld_chan_load_update_iter(void *_data, u8 *mac,
+					  struct ieee80211_vif *vif)
+{
+	struct iwl_mld_chan_load_data *data = _data;
+	const struct iwl_mld_phy *phy = data->phy;
+	struct ieee80211_chanctx_conf *chanctx =
+		container_of((const void *)phy, struct ieee80211_chanctx_conf,
+			     drv_priv);
+	struct iwl_mld *mld = iwl_mld_vif_from_mac80211(vif)->mld;
+	struct ieee80211_bss_conf *prim_link;
+	unsigned int prim_link_id;
+
+	prim_link_id = iwl_mld_get_primary_link(vif);
+	prim_link = link_conf_dereference_protected(vif, prim_link_id);
+
+	if (WARN_ON(!prim_link))
+		return;
+
+	if (chanctx != rcu_access_pointer(prim_link->chanctx_conf))
+		return;
+
+	if (iwl_mld_emlsr_active(vif)) {
+		int chan_load = iwl_mld_get_chan_load_by_others(mld, prim_link,
+								true);
+
+		if (chan_load < 0)
+			return;
+
+		/* chan_load is in range [0,255] */
+		if (chan_load < NORMALIZE_PERCENT_TO_255(IWL_MLD_EXIT_EMLSR_CHAN_LOAD))
+			iwl_mld_exit_emlsr(mld, vif,
+					   IWL_MLD_EMLSR_EXIT_CHAN_LOAD,
+					   prim_link_id);
+	} else {
+		u32 old_chan_load = data->prev_chan_load_not_by_us;
+		u32 new_chan_load = phy->avg_channel_load_not_by_us;
+		u32 min_thresh = iwl_mld_get_min_chan_load_thresh(chanctx);
+
+#define THRESHOLD_CROSSED(threshold) \
+	(old_chan_load <= (threshold) && new_chan_load > (threshold))
+
+		if (THRESHOLD_CROSSED(min_thresh) || THRESHOLD_CROSSED(25) ||
+		    THRESHOLD_CROSSED(40) || THRESHOLD_CROSSED(50))
+			iwl_mld_retry_emlsr(mld, vif);
+#undef THRESHOLD_CROSSED
+	}
+}
+
+void iwl_mld_emlsr_check_chan_load(struct ieee80211_hw *hw,
+				   struct iwl_mld_phy *phy,
+				   u32 prev_chan_load_not_by_us)
+{
+	struct iwl_mld_chan_load_data data = {
+		.phy = phy,
+		.prev_chan_load_not_by_us = prev_chan_load_not_by_us,
+	};
+
+	ieee80211_iterate_active_interfaces_mtx(hw,
+						IEEE80211_IFACE_ITER_NORMAL,
+						iwl_mld_chan_load_update_iter,
+						&data);
+}
+
+void iwl_mld_retry_emlsr(struct iwl_mld *mld, struct ieee80211_vif *vif)
+{
+	struct iwl_mld_vif *mld_vif = iwl_mld_vif_from_mac80211(vif);
+
+	if (!iwl_mld_vif_has_emlsr_cap(vif) || iwl_mld_emlsr_active(vif) ||
+	    mld_vif->emlsr.blocked_reasons)
+		return;
+
+	iwl_mld_int_mlo_scan(mld, vif);
+}