wifi: iwlwifi: mvm: Add basic link selection logic

Add simple logic that would allow using EMLSR in case
there are multiple valid links:

- In case the connection establishment has just been
  completed try to find a valid link pair for EMLSR
  functionality where one of the links in the pair is
  the current active link.
- In case the valid links changed after connection
  was already established, try to find a valid link pair
  for EMLSR functionality, in case the EMSLR is not active
  yet.

If a valid link pair is found call mac80211 to asynchronously
set the new link pair, otherwise continue using the current active
links.

type=feature
ticket=jira:WIFI-279734

Signed-off-by: Ilan Peer <ilan.peer@intel.com>
Change-Id: I6600bd16551d75e2bf520d8d0add525568a9f85fx-iwlwifi-stack-dev: bd8d1b8974e5d49ce0ee73d93ea659606d9b756b

6 files changed, 136 insertions, 1 deletions

diff --git a/drivers/net/wireless/intel/iwlwifi/iwl-dbg-cfg.h b/drivers/net/wireless/intel/iwlwifi/iwl-dbg-cfg.h
index 72adab2c47..9c3c305460 100644
--- a/drivers/net/wireless/intel/iwlwifi/iwl-dbg-cfg.h
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-dbg-cfg.h
@@ -293,6 +293,7 @@ struct iwl_dbg_cfg {
 	IWL_DBG_CFG_NODEF(bool, amsdu_in_ampdu_disabled)
 	IWL_DBG_CFG_NODEF(bool, disable_eml)
 	IWL_DBG_CFG_NODEF(u32, step_analog_params)
+	IWL_DBG_CFG(bool, MVM_AUTO_EML_ENABLE)
 #ifdef CPTCFG_IWLWIFI_DEBUG
 	IWL_MOD_PARAM(u32, debug_level)
 #endif /* CPTCFG_IWLWIFI_DEBUG */
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/constants.h b/drivers/net/wireless/intel/iwlwifi/mvm/constants.h
index a32cf122e2..3634d80b9a 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/constants.h
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/constants.h
@@ -123,6 +123,7 @@
 #define IWL_MVM_MEI_REPORT_RFKILL               false
 #define IWL_MVM_MIN_BEACON_INTERVAL_TU          16
 #define IWL_MVM_ADAPTIVE_DWELL_NUM_APS_OVERRIDE 0
+#define IWL_MVM_AUTO_EML_ENABLE			true
 #else /* CPTCFG_IWLWIFI_SUPPORT_DEBUG_OVERRIDES */
 #define IWL_MVM_DEFAULT_PS_TX_DATA_TIMEOUT	(mvm->trans->dbg_cfg.MVM_DEFAULT_PS_TX_DATA_TIMEOUT)
 #define IWL_MVM_DEFAULT_PS_RX_DATA_TIMEOUT	(mvm->trans->dbg_cfg.MVM_DEFAULT_PS_RX_DATA_TIMEOUT)
@@ -240,6 +241,7 @@
 #define IWL_MVM_MEI_REPORT_RFKILL               (mvm->trans->dbg_cfg.MVM_MEI_REPORT_RFKILL)
 #define IWL_MVM_MIN_BEACON_INTERVAL_TU          (mvm->trans->dbg_cfg.MVM_MIN_BEACON_INTERVAL_TU)
 #define IWL_MVM_ADAPTIVE_DWELL_NUM_APS_OVERRIDE (mvm->trans->dbg_cfg.MVM_ADAPTIVE_DWELL_NUM_APS_OVERRIDE)
+#define IWL_MVM_AUTO_EML_ENABLE                 (mvm->trans->dbg_cfg.MVM_AUTO_EML_ENABLE)
 
 #endif /* CPTCFG_IWLWIFI_SUPPORT_DEBUG_OVERRIDES */
 
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/mac80211.c b/drivers/net/wireless/intel/iwlwifi/mvm/mac80211.c
index 9a99400def..cbf0c9b2aa 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/mac80211.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/mac80211.c
@@ -4028,6 +4028,13 @@ iwl_mvm_sta_state_assoc_to_authorized(struct iwl_mvm *mvm,
 
 		callbacks->mac_ctxt_changed(mvm, vif, false);
 		iwl_mvm_mei_host_associated(mvm, vif, mvm_sta);
+
+		/* when client is authorized (AP station marked as such),
+		 * try to enable more links
+		 */
+		if (vif->type == NL80211_IFTYPE_STATION &&
+		    !test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status))
+			iwl_mvm_mld_select_links(mvm, vif, false);
 	}
 
 	mvm_sta->authorized = true;
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/mld-mac80211.c b/drivers/net/wireless/intel/iwlwifi/mvm/mld-mac80211.c
index 8b9903f780..12a198a874 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/mld-mac80211.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/mld-mac80211.c
@@ -650,6 +650,126 @@ static int iwl_mvm_mld_mac_sta_state(struct ieee80211_hw *hw,
 					    &callbacks);
 }
 
+struct iwl_mvm_link_sel_data {
+	u8 link_id;
+	enum nl80211_band band;
+	bool active;
+};
+
+static bool iwl_mvm_mld_valid_link_pair(struct iwl_mvm_link_sel_data *a,
+					struct iwl_mvm_link_sel_data *b)
+{
+	return a->band != b->band;
+}
+
+void iwl_mvm_mld_select_links(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
+			      bool valid_links_changed)
+{
+	struct iwl_mvm_link_sel_data data[IEEE80211_MLD_MAX_NUM_LINKS];
+	unsigned long usable_links = ieee80211_vif_usable_links(vif);
+	u32 max_active_links = iwl_mvm_max_active_links(mvm, vif);
+	u16 new_active_links;
+	u8 link_id, n_data = 0, i, j;
+
+	if (!IWL_MVM_AUTO_EML_ENABLE)
+		return;
+
+	if (!ieee80211_vif_is_mld(vif) || usable_links == 1)
+		return;
+
+	/* The logic below is a simple version that doesn't suit more than 2
+	 * links
+	 */
+	WARN_ON_ONCE(max_active_links > 2);
+
+	/* if only a single active link is supported, assume that the one
+	 * selected by higher layer for connection establishment is the best.
+	 */
+	if (max_active_links == 1 && !valid_links_changed)
+		return;
+
+	/* If we are already using the maximal number of active links, don't do
+	 * any change. This can later be optimized to pick a 'better' link pair.
+	 */
+	if (hweight16(vif->active_links) == max_active_links)
+		return;
+
+	rcu_read_lock();
+
+	for_each_set_bit(link_id, &usable_links, IEEE80211_MLD_MAX_NUM_LINKS) {
+		struct ieee80211_bss_conf *link_conf =
+			rcu_dereference(vif->link_conf[link_id]);
+
+		if (WARN_ON_ONCE(!link_conf))
+			continue;
+
+		data[n_data].link_id = link_id;
+		data[n_data].band = link_conf->chandef.chan->band;
+		data[n_data].active = vif->active_links & BIT(link_id);
+		n_data++;
+	}
+
+	rcu_read_unlock();
+
+	/* this is expected to be the current active link */
+	if (n_data == 1)
+		return;
+
+	new_active_links = 0;
+
+	/* Assume that after association only a single link is active, thus,
+	 * select only the 2nd link
+	 */
+	if (!valid_links_changed) {
+		for (i = 0; i < n_data; i++) {
+			if (data[i].active)
+				break;
+		}
+
+		if (WARN_ON_ONCE(i == n_data))
+			return;
+
+		for (j = 0; j < n_data; j++) {
+			if (i == j)
+				continue;
+
+			if (iwl_mvm_mld_valid_link_pair(&data[i], &data[j]))
+				break;
+		}
+
+		if (j != n_data)
+			new_active_links = BIT(data[i].link_id) |
+				BIT(data[j].link_id);
+	} else {
+		/* Try to find a valid link pair for EMLSR operation. If a pair
+		 * is not found continue using the current active link.
+		 */
+		for (i = 0; i < n_data; i++) {
+			for (j = 0; j < n_data; j++) {
+				if (i == j)
+					continue;
+
+				if (iwl_mvm_mld_valid_link_pair(&data[i],
+								&data[j]))
+					break;
+			}
+
+			/* found a valid pair for EMLSR, use it */
+			if (j != n_data) {
+				new_active_links = BIT(data[i].link_id) |
+					BIT(data[j].link_id);
+				break;
+			}
+		}
+	}
+
+	if (WARN_ON(!new_active_links))
+		return;
+
+	if (vif->active_links != new_active_links)
+		ieee80211_set_active_links_async(vif, new_active_links);
+}
+
 static void iwl_mvm_mld_link_info_changed_station(struct iwl_mvm *mvm,
 						  struct ieee80211_vif *vif,
 						  struct ieee80211_bss_conf *link_conf,
@@ -693,6 +813,9 @@ static void iwl_mvm_mld_link_info_changed_station(struct iwl_mvm *mvm,
 	if (ret)
 		IWL_ERR(mvm, "failed to update MAC %pM\n", vif->addr);
 
+	if (changes & BSS_CHANGED_MLD_VALID_LINKS)
+		iwl_mvm_mld_select_links(mvm, vif, true);
+
 	memcpy(mvmvif->link[link_conf->link_id]->bssid, link_conf->bssid,
 	       ETH_ALEN);
 
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/mvm.h b/drivers/net/wireless/intel/iwlwifi/mvm/mvm.h
index 835e4ceb0e..becd5056f7 100644
--- a/drivers/net/wireless/intel/iwlwifi/mvm/mvm.h
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/mvm.h
@@ -2835,4 +2835,6 @@ u8 iwl_mvm_eval_dsm_rfi_ddr(struct iwl_mvm *mvm);
 u8 iwl_mvm_eval_dsm_rfi_dlvr(struct iwl_mvm *mvm);
 bool iwl_mvm_enable_fils(struct iwl_mvm *mvm,
 			 struct ieee80211_chanctx_conf *ctx);
+void iwl_mvm_mld_select_links(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
+			      bool valid_links_changed);
 #endif /* __IWL_MVM_H__ */
diff --git a/versions b/versions
index 014382e960..354f2c1745 100644
--- a/versions
+++ b/versions
@@ -2,4 +2,4 @@ BACKPORTS_VERSION="(see git)"
 BACKPORTED_KERNEL_VERSION="(see git)"
 BACKPORTED_KERNEL_NAME="iwlwifi"
 BACKPORTS_BUILD_TSTAMP=__DATE__ \" \" __TIME__
-BACKPORTS_GIT_TRACKED="iwlwifi-stack-public:master:11331:13cb9e9e"
+BACKPORTS_GIT_TRACKED="iwlwifi-stack-public:master:11332:bd8d1b89"