qcacld-3.0: Handle MLO STA + Legacy STA concurrencies

Handle MLO STA + Legacy STA concurrencies such that:

1) Don't disable any link if STA form SCC with any of the
   link.

2) Disable the link on the same MAC which can cause MCC.

   a) On non-SBS devices disable the ML link on the same
      band as STA, As the same band means the same MAC
      in non-SBS mode.
   b) On SBS devices, disable the ML link on the same MAC:
     - If STA freq is 2.4Ghz disable the 2.4Ghz link.
       If a 2.4Ghz link is not present, disable any of the
       5/6Ghz links (FW to decide the link depending on
       SBS mode, low share or high share).
     - If STA freq is 5Ghz and can lead to SBS with on of
       the link, disable the non-SBS link (link which will
       remain on the same MAC).
     - If STA freq is 5Ghz and cannot lead to SBS with
       one of the links, disable the link on the same MAC/
       band.

e.g:

 STA freq:    STA ML combo: NON SBS Action: SBS Action:
 ------------------------------------------------------------------
 2Ghz         2Ghz+5/6Ghz   Disable 2Ghz    Disable 2Ghz
 5Ghz         2Ghz+5/6Ghz   Disable 5Ghz    Disable 2.4Ghz
                                            if both 5Ghz lead
                                            to SBS, else
                                            disable 5Ghz

 5Ghz(lower)  5Ghz+6Ghz     Disable 5Ghz    Disable 5Ghz
 5Ghz(higher) 5Ghz+6Ghz     Disable 5Ghz    Disable 6Ghz
 2Ghz         5Ghz+6Ghz     Disable Any     Disable Any

Change-Id: Ie8ca51b5e108c1cfce1a02fc97f4849c617216b7
CRs-Fixed: 3090333

components/cmn_services/interface_mgr/src/wlan_if_mgr_sta.c

@@ -30,6 +30,8 @@
 #include "wlan_tdls_ucfg_api.h"
 #include "wlan_tdls_api.h"
 #include <wlan_cm_api.h>
+#include <wlan_mlo_mgr_public_structs.h>
+#include <wlan_mlo_mgr_cmn.h>
 
 QDF_STATUS if_mgr_connect_start(struct wlan_objmgr_vdev *vdev,
 				struct if_mgr_event_data *event_data)
@@ -90,6 +92,411 @@ QDF_STATUS if_mgr_connect_start(struct wlan_objmgr_vdev *vdev,
 	return QDF_STATUS_SUCCESS;
 }
 
+#ifdef WLAN_FEATURE_11BE_MLO
+/*
+ * if_mgr_get_mlo_and_legacy_sta_count() - get mlo and legacy sta count
+ * also fills the freq and legacy/mlo list
+ * @psoc: Objmgr psoc
+ * @num_mlo: num mlo as output
+ * @mlo_idx: mlo vdev index as output
+ * @num_legacy: num legacy as output
+ * @legacy_idx: legacy vdev index as output
+ * @freq_list: freq list of each sta vdev
+ * @vdev_id_list: vdev id list
+ *
+ * Return: void
+ */
+static void if_mgr_get_mlo_and_legacy_sta_count(struct wlan_objmgr_psoc *psoc,
+						uint8_t *num_mlo,
+						uint8_t *mlo_idx,
+						uint8_t *num_legacy,
+						uint8_t *legacy_idx,
+						qdf_freq_t *freq_list,
+						uint8_t *vdev_id_list)
+{
+	uint32_t sta_num = 0;
+	uint8_t i;
+	struct wlan_objmgr_vdev *temp_vdev;
+
+	*num_mlo = 0;
+	*num_legacy = 0;
+
+	sta_num = policy_mgr_get_mode_specific_conn_info(psoc, freq_list,
+							 vdev_id_list,
+							 PM_STA_MODE);
+	if (!sta_num)
+		return;
+
+	for (i = 0; i < sta_num; i++) {
+		temp_vdev =
+			wlan_objmgr_get_vdev_by_id_from_psoc(psoc,
+							     vdev_id_list[i],
+							     WLAN_IF_MGR_ID);
+		if (!temp_vdev) {
+			ifmgr_err("invalid vdev for id %d", vdev_id_list[i]);
+			return;
+		}
+
+		if (wlan_vdev_mlme_is_mlo_vdev(temp_vdev))
+			mlo_idx[(*num_mlo)++] = i;
+		else
+			legacy_idx[(*num_legacy)++] = i;
+
+		wlan_objmgr_vdev_release_ref(temp_vdev, WLAN_IF_MGR_ID);
+	}
+}
+
+static void
+if_mgr_fill_active_link_vdev_bitmap(struct mlo_link_set_active_req *req,
+				    uint8_t *mlo_vdev_lst,
+				    uint32_t num_mlo_vdev)
+{
+	uint32_t entry_idx, entry_offset, vdev_idx;
+	uint8_t vdev_id;
+
+	for (vdev_idx = 0; vdev_idx < num_mlo_vdev; vdev_idx++) {
+		vdev_id = mlo_vdev_lst[vdev_idx];
+		entry_idx = vdev_id / 32;
+		entry_offset = vdev_id % 32;
+		if (entry_idx >= MLO_LINK_NUM_SZ) {
+			ifmgr_err("Invalid entry_idx %d num_mlo_vdev %d vdev %d",
+				  entry_idx, num_mlo_vdev, vdev_id);
+			continue;
+		}
+		req->param.vdev_bitmap[entry_idx] |= (1 << entry_offset);
+		/* update entry number if entry index changed */
+		if (req->param.num_vdev_bitmap < entry_idx + 1)
+			req->param.num_vdev_bitmap = entry_idx + 1;
+	}
+
+	ifmgr_debug("num_vdev_bitmap %d vdev_bitmap[0] = 0x%x, vdev_bitmap[1] = 0x%x",
+		    req->param.num_vdev_bitmap, req->param.vdev_bitmap[0],
+		    req->param.vdev_bitmap[1]);
+}
+
+static void
+if_mgr_sta_mlo_concurency_set_link(struct wlan_objmgr_vdev *vdev,
+				   enum mlo_link_force_reason reason,
+				   enum mlo_link_force_mode mode,
+				   uint8_t num_mlo_vdev, uint8_t *mlo_vdev_lst)
+{
+	struct mlo_link_set_active_req *req;
+	QDF_STATUS status;
+
+	req = qdf_mem_malloc(sizeof(*req));
+	if (!req)
+		return;
+
+	ifmgr_debug("vdev %d: mode %d num_mlo_vdev %d reason %d",
+		    wlan_vdev_get_id(vdev), mode, num_mlo_vdev, reason);
+
+	req->ctx.vdev = vdev;
+	req->param.reason = reason;
+	req->param.force_mode = mode;
+
+	/* set MLO vdev bit mask for all case */
+	if_mgr_fill_active_link_vdev_bitmap(req, mlo_vdev_lst, num_mlo_vdev);
+
+	/* fill num of links for MLO_LINK_FORCE_MODE_ACTIVE_NUM */
+	if (mode == MLO_LINK_FORCE_MODE_ACTIVE_NUM) {
+		req->param.force_mode = MLO_LINK_FORCE_MODE_ACTIVE_NUM;
+		req->param.num_link_entry = 1;
+		req->param.link_num[0].num_of_link = num_mlo_vdev - 1;
+	}
+
+	status = mlo_ser_set_link_req(req);
+	if (QDF_IS_STATUS_ERROR(status))
+		ifmgr_err("vdev %d: Failed to set link mode %d num_mlo_vdev %d reason %d",
+			  wlan_vdev_get_id(vdev), mode, num_mlo_vdev, reason);
+
+	qdf_mem_free(req);
+}
+
+static uint8_t
+if_mgr_mlo_get_affected_links_for_sbs(struct wlan_objmgr_psoc *psoc,
+				      uint8_t num_mlo, qdf_freq_t *freq_list,
+				      uint8_t *vdev_id_list,
+				      uint8_t *mlo_vdev_lst,
+				      uint8_t *mlo_idx, qdf_freq_t freq)
+{
+	uint8_t i = 0;
+
+	/*
+	 * STA freq:      STA ML combo:  SBS Action
+	 * ---------------------------------------------------
+	 * 2Ghz           2Ghz+5/6Ghz    Disable 2Ghz(Same MAC)
+	 * 5Ghz           2Ghz+5/6Ghz    Disable 2.4Ghz  if 5Ghz lead to SBS
+	 *                               (SBS, same MAC), else disable 5/6Ghz
+	 *                               (NON SBS, same MAC)
+	 * 5Ghz(lower)    5Ghz+6Ghz      Disable 5Ghz (NON SBS, same MAC)
+	 * 5Ghz(higher)   5Ghz+6Ghz      Disable 6Ghz (NON SBS, Same MAC)
+	 * 2Ghz           5Ghz+6Ghz      Disable Any
+	 */
+
+	/* If legacy STA is 2.4Ghz disable 2.4Ghz if present OR disable any */
+	if (wlan_reg_is_24ghz_ch_freq(freq)) {
+		while (i < num_mlo) {
+			if (wlan_reg_is_24ghz_ch_freq(freq_list[mlo_idx[i]])) {
+				/* Affected MLO link on 2.4Ghz */
+				mlo_vdev_lst[0] = vdev_id_list[mlo_idx[i]];
+				return 1;
+			}
+			/* Fill non effected vdev in list */
+			mlo_vdev_lst[i] = vdev_id_list[mlo_idx[i]];
+			i++;
+		}
+		/* No link affected return mlo_num to disable any */
+		return i;
+	}
+
+	/* This mean legasy STA is 5Ghz */
+
+	/* check if any link lead to SBS, so that we can disable the other*/
+	i = 0;
+	while (i < num_mlo &&
+	       !policy_mgr_are_sbs_chan(psoc, freq, freq_list[mlo_idx[i]]))
+		i++;
+
+	/*
+	 * if i < num_mlo then i is the SBS link, in this case disable the other
+	 * non SBS link, this mean MLO is 5+6 or 2+5/6.
+	 */
+	if (i < num_mlo) {
+		i = 0;
+		while (i < num_mlo) {
+			if (!policy_mgr_are_sbs_chan(psoc, freq,
+						     freq_list[mlo_idx[i]])) {
+				/* Affected non SBS MLO link */
+				mlo_vdev_lst[0] = vdev_id_list[mlo_idx[i]];
+				return 1;
+			}
+			/* Fill non effected vdev in list */
+			mlo_vdev_lst[i] = vdev_id_list[mlo_idx[i]];
+			i++;
+		}
+		/* All link lead to SBS, disable any, This should not happen */
+		return i;
+	}
+
+	/*
+	 * None of the link can lead to SBS, i.e. its 2+ 5/6 MLO in this case
+	 * disabel 5Ghz link.
+	 */
+	i = 0;
+	while (i < num_mlo) {
+		if (!wlan_reg_is_24ghz_ch_freq(freq_list[mlo_idx[i]])) {
+			/* Affected 5/6Ghz MLO link */
+			mlo_vdev_lst[0] = vdev_id_list[mlo_idx[i]];
+			return 1;
+		}
+		/* Fill non effected vdev in list */
+		mlo_vdev_lst[i] = vdev_id_list[mlo_idx[i]];
+		i++;
+	}
+
+	/* No link affected, This should not happen */
+	return i;
+}
+
+/*
+ * if_mgr_mlo_get_concurrent_num_links() - get MLO links which are affected
+ * if no affected then return mlo num. Also fills the mlo_vdev_id to send.
+ * @num_mlo: number of mlo vdev
+ * @freq_list: freq list of all vdev
+ * @vdev_id_list: vdev id list
+ * @mlo_vdev_lst: mlo vdev list
+ * @mlo_idx: mlo index
+ * @freq: legacy sta freq
+ *
+ * Return: number of the affected links, else total link and mlo_vdev_id list.
+ */
+static uint8_t
+if_mgr_mlo_get_concurrent_num_links(struct wlan_objmgr_vdev *vdev,
+				    uint8_t num_mlo, qdf_freq_t *freq_list,
+				    uint8_t *vdev_id_list,
+				    uint8_t *mlo_vdev_lst,
+				    uint8_t *mlo_idx, qdf_freq_t freq)
+{
+	uint8_t i = 0;
+	struct wlan_objmgr_psoc *psoc = wlan_vdev_get_psoc(vdev);
+
+	if (!psoc)
+		return 0;
+
+	while (i < num_mlo && (freq_list[mlo_idx[i]] != freq))
+		i++;
+
+	if (i < num_mlo) {
+		/* if one link is SCC then no need to disable any link */
+		ifmgr_debug("vdev %d: link vdev %d lead to SCC, STA freq %d ML freq %d, no need to disable link",
+			    wlan_vdev_get_id(vdev), vdev_id_list[mlo_idx[i]],
+			    freq, freq_list[mlo_idx[i]]);
+		return 0;
+	}
+
+	if (policy_mgr_is_hw_sbs_capable(psoc))
+		return if_mgr_mlo_get_affected_links_for_sbs(psoc, num_mlo,
+							     freq_list,
+							     vdev_id_list,
+							     mlo_vdev_lst,
+							     mlo_idx, freq);
+
+	/*
+	 * STA freq:      STA ML combo:  NON SBS Action:
+	 * -------------------------------------------------
+	 * 2Ghz           2Ghz+5/6Ghz    Disable 2Ghz (Same MAC)
+	 * 5Ghz           2Ghz+5/6Ghz    Disable 5Ghz (Same MAC)
+	 * 5Ghz           5Ghz+6Ghz      Disable Any of 5/6Ghz (Same MAC)
+	 * 2Ghz           5Ghz+6Ghz      Disable Any
+	 */
+	/*
+	 * Check if any of the link is on same MAC/band(for non SBS) as legacy
+	 * STA's freq. Same MAC/band mean both are either 5Ghz/6Ghz/2.4Ghz
+	 * OR both are non 2.4Ghz (ie one is 5Ghz and other is 6Ghz)
+	 */
+	i = 0;
+	while (i < num_mlo &&
+	       !(wlan_reg_is_same_band_freqs(freq_list[mlo_idx[i]], freq) ||
+		 (!wlan_reg_is_24ghz_ch_freq(freq_list[mlo_idx[i]]) &&
+		  !wlan_reg_is_24ghz_ch_freq(freq)))) {
+		/* Fill non effected vdev in list */
+		mlo_vdev_lst[i] = vdev_id_list[mlo_idx[i]];
+		i++;
+	}
+
+	if (i < num_mlo) {
+		/* affected MLO link on the same MAC/band with legacy STA */
+		mlo_vdev_lst[0] = vdev_id_list[mlo_idx[i]];
+		return 1;
+	}
+
+	/* No link affected return mlo_num to disable any */
+	return i;
+}
+
+static void
+if_mgr_sta_mlo_concurency_on_connect(struct wlan_objmgr_vdev *vdev,
+				     uint8_t num_mlo, uint8_t *mlo_idx,
+				     uint8_t num_legacy, uint8_t *legacy_idx,
+				     qdf_freq_t *freq_list,
+				     uint8_t *vdev_id_list)
+{
+	qdf_freq_t freq = 0;
+	struct wlan_channel *bss_chan;
+	uint8_t vdev_id = wlan_vdev_get_id(vdev);
+	uint8_t mlo_vdev_lst[MAX_NUMBER_OF_CONC_CONNECTIONS] = {0};
+	uint8_t affected_links = 0;
+	enum mlo_link_force_mode mode = MLO_LINK_FORCE_MODE_ACTIVE_NUM;
+
+	/* Legacy STA doesn't exist, no need to change to link.*/
+	if (!num_legacy)
+		return;
+
+	if (wlan_vdev_mlme_is_mlo_vdev(vdev)) {
+		freq = freq_list[legacy_idx[0]];
+	} else {
+		bss_chan = wlan_vdev_mlme_get_bss_chan(vdev);
+		if (bss_chan)
+			freq = bss_chan->ch_freq;
+	}
+	ifmgr_debug("vdev %d: Freq %d (legacy vdev id %d), is MLO %d", vdev_id,
+		    freq, legacy_idx[0], wlan_vdev_mlme_is_mlo_vdev(vdev));
+	if (!freq)
+		return;
+
+	affected_links =
+		if_mgr_mlo_get_concurrent_num_links(vdev, num_mlo, freq_list,
+						    vdev_id_list, mlo_vdev_lst,
+						    mlo_idx, freq);
+
+	if (!affected_links) {
+		ifmgr_debug("vdev %d: no affected link found", vdev_id);
+		return;
+	}
+
+	/*
+	 * If affected link is less than mlo_num, ie not all link are affected,
+	 * send MLO_LINK_FORCE_MODE_INACTIVE.
+	 */
+	if (affected_links < num_mlo)
+		mode = MLO_LINK_FORCE_MODE_INACTIVE;
+
+	if_mgr_sta_mlo_concurency_set_link(vdev, MLO_LINK_FORCE_REASON_CONNECT,
+					   mode, affected_links, mlo_vdev_lst);
+}
+
+static void
+if_mgr_sta_mlo_concurency_on_disconnect(struct wlan_objmgr_vdev *vdev,
+					uint8_t num_mlo, uint8_t *mlo_idx,
+					uint8_t num_legacy,
+					uint8_t *vdev_id_list)
+{
+	uint8_t i = 0;
+	uint8_t mlo_vdev_list[MAX_NUMBER_OF_CONC_CONNECTIONS] = {0};
+
+	/*
+	 * If Legacy STA exist, no need to change to link.
+	 * Only change when legasy sta is disconnected and
+	 * only ML STA is present.
+	 */
+	if (num_legacy)
+		return;
+
+	/*
+	 * On legacy STA disconnect if  MLO has >= 2 links, need to send
+	 * MLO_LINK_FORCE_MODE_NO_FORCE for all MLO Vdevs for letting FW enable
+	 * all the links.
+	 */
+	while (i < num_mlo) {
+		mlo_vdev_list[i] = vdev_id_list[mlo_idx[i]];
+		i++;
+	}
+
+	if_mgr_sta_mlo_concurency_set_link(vdev,
+					   MLO_LINK_FORCE_REASON_DISCONNECT,
+					   MLO_LINK_FORCE_MODE_NO_FORCE,
+					   num_mlo, mlo_vdev_list);
+}
+
+static void
+if_mgr_handle_sta_mlo_link_concurrency(struct wlan_objmgr_psoc *psoc,
+				       struct wlan_objmgr_vdev *vdev,
+				       bool is_connect)
+{
+	uint8_t num_mlo = 0, num_legacy = 0;
+	uint8_t mlo_idx[MAX_NUMBER_OF_CONC_CONNECTIONS] = {0};
+	uint8_t legacy_idx[MAX_NUMBER_OF_CONC_CONNECTIONS] = {0};
+	qdf_freq_t freq_list[MAX_NUMBER_OF_CONC_CONNECTIONS] = {0};
+	uint8_t vdev_id_list[MAX_NUMBER_OF_CONC_CONNECTIONS] = {0};
+
+	if_mgr_get_mlo_and_legacy_sta_count(psoc, &num_mlo, mlo_idx,
+					    &num_legacy, legacy_idx, freq_list,
+					    vdev_id_list);
+
+	ifmgr_debug("vdev %d: num_mlo %d num_legacy %d is_connect %d",
+		    wlan_vdev_get_id(vdev), num_mlo, num_legacy, is_connect);
+	/* ML STA is not up */
+	if (num_mlo < 2)
+		return;
+
+	if (is_connect)
+		if_mgr_sta_mlo_concurency_on_connect(vdev, num_mlo, mlo_idx,
+						     num_legacy, legacy_idx,
+						     freq_list,
+						     vdev_id_list);
+	else
+		if_mgr_sta_mlo_concurency_on_disconnect(vdev, num_mlo, mlo_idx,
+							num_legacy,
+							vdev_id_list);
+}
+#else
+static inline void
+if_mgr_handle_sta_mlo_link_concurrency(struct wlan_objmgr_psoc *psoc,
+				       struct wlan_objmgr_vdev *vdev,
+				       bool is_connect)
+{}
+#endif
+
 QDF_STATUS if_mgr_connect_complete(struct wlan_objmgr_vdev *vdev,
 				   struct if_mgr_event_data *event_data)
 {
@@ -128,6 +535,9 @@ QDF_STATUS if_mgr_connect_complete(struct wlan_objmgr_vdev *vdev,
 			 */
 			if_mgr_enable_roaming(pdev, vdev, RSO_CONNECT_START);
 		}
+		if (wlan_vdev_mlme_get_opmode(vdev) == QDF_STA_MODE)
+			if_mgr_handle_sta_mlo_link_concurrency(psoc, vdev,
+							       true);
 	} else {
 		/* notify connect failure on final failure */
 		ucfg_tdls_notify_connect_failure(psoc);
@@ -197,5 +607,8 @@ QDF_STATUS if_mgr_disconnect_complete(struct wlan_objmgr_vdev *vdev,
 		return status;
 	}
 
+	if (wlan_vdev_mlme_get_opmode(vdev) == QDF_STA_MODE)
+		if_mgr_handle_sta_mlo_link_concurrency(psoc, vdev, false);
+
 	return QDF_STATUS_SUCCESS;
 }