qcacld-3.0: Enhance set link for nlink connection

Existing code for sending set link request to FW will
get VDEV list from MLO manager and sets the VDEV's
MAC address matching userspace request to active and
non-matching as inactive. However for three link
connection this misses the third link which doesn't
have any VDEV so userspace control for this link is
not available.

Enhance the logic to allow userspace to send MAC addr
of the standby link to manage link active/inactive.

Change-Id: Iba5d976349917b1b4f7146381cb0f49218cf4ed8
CRs-Fixed: 3600291

components/cmn_services/policy_mgr/inc/wlan_policy_mgr_api.h

@@ -5094,9 +5094,23 @@ QDF_STATUS
 policy_mgr_clear_ml_links_settings_in_fw(struct wlan_objmgr_psoc *psoc,
 					 uint8_t vdev_id);
 
+/**
+ * policy_mgr_activate_mlo_links_nlink() - Force active ML links based on user
+ * requested link mac address with link bitmap
+ * @psoc: objmgr psoc
+ * @session_id: session id
+ * @num_links: number of links to be forced active
+ * @active_link_addr: link mac address of links to be forced active
+ *
+ * Return: void
+ */
+void policy_mgr_activate_mlo_links_nlink(struct wlan_objmgr_psoc *psoc,
+					 uint8_t session_id, uint8_t num_links,
+					 struct qdf_mac_addr *active_link_addr);
+
 /**
  * policy_mgr_activate_mlo_links() - Force active ML links based on user
- * requested link mac address
+ * requested link mac address with vdev bitmap
  * @psoc: objmgr psoc
  * @session_id: session id
  * @num_links: number of links to be forced active

components/cmn_services/policy_mgr/src/wlan_policy_mgr_get_set_utils.c

@@ -6085,22 +6085,24 @@ policy_mgr_mlo_sta_set_link_by_linkid(struct wlan_objmgr_psoc *psoc,
 	uint32_t vdev_bitmap2[MLO_VDEV_BITMAP_SZ];
 	uint8_t i, idx;
 	uint32_t link_control_flags = 0;
+	uint8_t vdev_per_bitmap = MLO_MAX_VDEV_COUNT_PER_BIMTAP_ELEMENT;
 
 	qdf_mem_zero(vdev_bitmap, sizeof(vdev_bitmap));
 	qdf_mem_zero(vdev_bitmap2, sizeof(vdev_bitmap2));
 
 	for (i = 0; i < num_mlo_vdev; i++) {
-		idx = mlo_vdev_lst[i] / 32;
+		idx = mlo_vdev_lst[i] / vdev_per_bitmap;
 		if (idx >= MLO_VDEV_BITMAP_SZ)
 			return QDF_STATUS_E_INVAL;
-		vdev_bitmap[idx] |= 1 << (mlo_vdev_lst[i] % 32);
+		vdev_bitmap[idx] |= 1 << (mlo_vdev_lst[i] % vdev_per_bitmap);
 	}
 
 	for (i = 0; i < num_mlo_inactive_vdev; i++) {
-		idx = mlo_inactive_vdev_lst[i] / 32;
+		idx = mlo_inactive_vdev_lst[i] / vdev_per_bitmap;
 		if (idx >= MLO_VDEV_BITMAP_SZ)
 			return QDF_STATUS_E_INVAL;
-		vdev_bitmap2[idx] |= 1 << (mlo_inactive_vdev_lst[i] % 32);
+		vdev_bitmap2[idx] |=
+			1 << (mlo_inactive_vdev_lst[i] % vdev_per_bitmap);
 	}
 
 	ml_nlink_convert_vdev_bitmap_to_linkid_bitmap(
@@ -6552,7 +6554,7 @@ policy_mgr_link_switch_notifier_cb(struct wlan_objmgr_vdev *vdev,
 	uint8_t num_del = 0;
 	struct ml_nlink_change_event data;
 
-	if (notify_reason != MLO_LINK_SWITCH_NOTIFY_REASON_PRE_START_POST_SER)
+	if (notify_reason > MLO_LINK_SWITCH_NOTIFY_REASON_PRE_START_POST_SER)
 		return QDF_STATUS_SUCCESS;
 
 	pm_ctx = policy_mgr_get_context(psoc);
@@ -6561,13 +6563,15 @@ policy_mgr_link_switch_notifier_cb(struct wlan_objmgr_vdev *vdev,
 		return QDF_STATUS_E_INVAL;
 	}
 
-	policy_mgr_debug("target link %d freq %d curr link %d vdev %d",
+	policy_mgr_debug("target link %d freq %d curr link %d notify reason %d link switch reason %d vdev %d",
 			 new_ieee_link_id, new_primary_freq,
-			 curr_ieee_link_id, vdev_id);
+			 curr_ieee_link_id, notify_reason, req->reason,
+			 vdev_id);
 	qdf_mem_zero(&data, sizeof(data));
 	data.evt.link_switch.curr_ieee_link_id = curr_ieee_link_id;
 	data.evt.link_switch.new_ieee_link_id = new_ieee_link_id;
 	data.evt.link_switch.new_primary_freq = new_primary_freq;
+	data.evt.link_switch.reason = req->reason;
 	status = ml_nlink_conn_change_notify(psoc, vdev_id,
 					     ml_nlink_link_switch_start_evt,
 					     &data);
@@ -8220,6 +8224,105 @@ policy_mgr_is_restart_sap_required_with_mlo_sta(struct wlan_objmgr_psoc *psoc,
 	return restart_required;
 }
 
+void policy_mgr_activate_mlo_links_nlink(struct wlan_objmgr_psoc *psoc,
+					 uint8_t session_id, uint8_t num_links,
+					 struct qdf_mac_addr active_link_addr[2])
+{
+	uint8_t *link_mac_addr;
+	uint32_t link_ctrl_flags;
+	enum mlo_link_force_reason reason;
+	enum mlo_link_force_mode mode;
+	struct wlan_objmgr_vdev *vdev;
+	struct mlo_link_info *link_info;
+	bool active_link_present = false;
+	uint8_t iter, link, active_link_cnt = 0, inactive_link_cnt = 0;
+	uint32_t active_link_bitmap = 0;
+	uint32_t inactive_link_bitmap = 0;
+
+	vdev = wlan_objmgr_get_vdev_by_id_from_psoc(psoc, session_id,
+						    WLAN_POLICY_MGR_ID);
+	if (!vdev) {
+		policy_mgr_err("vdev_id: %d vdev not found", session_id);
+		return;
+	}
+
+	if (!wlan_cm_is_vdev_connected(vdev)) {
+		policy_mgr_err("vdev is not in connected state");
+		goto done;
+	}
+
+	if (!wlan_vdev_mlme_is_mlo_vdev(vdev)) {
+		policy_mgr_err("vdev is not mlo vdev");
+		goto done;
+	}
+
+	policy_mgr_debug("Num active links: %d", num_links);
+	link_info = &vdev->mlo_dev_ctx->link_ctx->links_info[0];
+	for (iter = 0; iter < WLAN_MAX_ML_BSS_LINKS; iter++) {
+		if (link_info->link_id == WLAN_INVALID_LINK_ID) {
+			link_info++;
+			continue;
+		}
+
+		link_mac_addr = &link_info->link_addr.bytes[0];
+		policy_mgr_debug("link addr: " QDF_MAC_ADDR_FMT,
+				 QDF_MAC_ADDR_REF(link_mac_addr));
+
+		for (link = 0; link < num_links; link++) {
+			policy_mgr_debug("active addr: " QDF_MAC_ADDR_FMT,
+			   QDF_MAC_ADDR_REF(&active_link_addr[link].bytes[0]));
+			if (!qdf_mem_cmp(link_mac_addr,
+					 &active_link_addr[link].bytes[0],
+					 QDF_MAC_ADDR_SIZE)) {
+				active_link_bitmap |= 1 << link_info->link_id;
+				active_link_cnt++;
+				active_link_present = true;
+				policy_mgr_debug("Link address match");
+			}
+		}
+		if (!active_link_present) {
+			inactive_link_bitmap |= 1 << link_info->link_id;
+			inactive_link_cnt++;
+			policy_mgr_err("No link address match");
+		}
+		active_link_present = false;
+		link_info++;
+	}
+
+	policy_mgr_debug("active link cnt: %d, inactive link cnt: %d",
+			 active_link_cnt, inactive_link_cnt);
+
+	if (!active_link_cnt) {
+		goto done;
+	} else if (policy_mgr_is_emlsr_sta_concurrency_present(psoc)) {
+		policy_mgr_debug("Concurrency exists, cannot enter EMLSR mode");
+		goto done;
+	}
+
+	/*
+	 * If there are both active and inactive vdev count, then issue a
+	 * single WMI with force mode MLO_LINK_FORCE_MODE_ACTIVE_INACTIVE,
+	 * else if there is only active vdev count, send single WMI for
+	 * all active vdevs with force mode MLO_LINK_FORCE_MODE_ACTIVE.
+	 */
+	if (inactive_link_cnt) {
+		reason = MLO_LINK_FORCE_REASON_CONNECT;
+		mode = MLO_LINK_FORCE_MODE_ACTIVE_INACTIVE;
+		link_ctrl_flags = link_ctrl_f_overwrite_active_bitmap |
+					link_ctrl_f_overwrite_inactive_bitmap;
+	} else {
+		reason = MLO_LINK_FORCE_REASON_DISCONNECT;
+		mode = MLO_LINK_FORCE_MODE_ACTIVE;
+		link_ctrl_flags = link_ctrl_f_overwrite_active_bitmap;
+	}
+
+	policy_mgr_mlo_sta_set_nlink(psoc, vdev, reason, mode, 0,
+				     active_link_bitmap, inactive_link_bitmap,
+				     link_ctrl_flags);
+done:
+	wlan_objmgr_vdev_release_ref(vdev, WLAN_POLICY_MGR_ID);
+}
+
 void policy_mgr_activate_mlo_links(struct wlan_objmgr_psoc *psoc,
 				   uint8_t session_id, uint8_t num_links,
 				   struct qdf_mac_addr active_link_addr[2])

components/umac/mlme/mlo_mgr/inc/wlan_mlo_link_force.h

@@ -22,6 +22,7 @@
 
 #include <wlan_mlo_mgr_cmn.h>
 #include <wlan_mlo_mgr_public_structs.h>
+#include <wlan_mlo_mgr_link_switch.h>
 
 /**
  * enum ml_nlink_change_event_type - Ml link state change trigger event
@@ -62,6 +63,7 @@ struct ml_nlink_change_event {
 			uint8_t curr_ieee_link_id;
 			uint8_t new_ieee_link_id;
 			uint32_t new_primary_freq;
+			enum wlan_mlo_link_switch_reason reason;
 		} link_switch;
 	} evt;
 };
@@ -127,6 +129,8 @@ ml_nlink_conn_change_notify(struct wlan_objmgr_psoc *psoc,
 			    enum ml_nlink_change_event_type evt,
 			    struct ml_nlink_change_event *data);
 
+#define MLO_MAX_VDEV_COUNT_PER_BIMTAP_ELEMENT (sizeof(uint32_t) * 8)
+
 /**
  * ml_nlink_convert_linkid_bitmap_to_vdev_bitmap() - convert link
  * id bitmap to vdev id bitmap

components/umac/mlme/mlo_mgr/src/wlan_mlo_link_force.c

@@ -45,6 +45,7 @@ ml_nlink_convert_linkid_bitmap_to_vdev_bitmap(
 	uint16_t link_id;
 	uint8_t vdev_id;
 	uint32_t associated_link_bitmap = 0;
+	uint8_t vdev_per_bitmap = MLO_MAX_VDEV_COUNT_PER_BIMTAP_ELEMENT;
 
 	*vdev_id_bitmap_sz = 0;
 	*vdev_id_num = 0;
@@ -85,10 +86,10 @@ ml_nlink_convert_linkid_bitmap_to_vdev_bitmap(
 		 */
 		if (!(link_bitmap & (1 << link_id)))
 			continue;
-		j = vdev_id / 32;
+		j = vdev_id / vdev_per_bitmap;
 		if (j >= MLO_VDEV_BITMAP_SZ)
 			break;
-		vdev_id_bitmap[j] |= 1 << (vdev_id % 32);
+		vdev_id_bitmap[j] |= 1 << (vdev_id % vdev_per_bitmap);
 		if (j + 1 > bitmap_sz)
 			bitmap_sz = j + 1;
 
@@ -124,6 +125,7 @@ ml_nlink_convert_vdev_bitmap_to_linkid_bitmap(
 	uint16_t link_id;
 	uint8_t vdev_id;
 	uint32_t associated_link_bitmap = 0;
+	uint8_t vdev_per_bitmap = MLO_MAX_VDEV_COUNT_PER_BIMTAP_ELEMENT;
 
 	*link_bitmap = 0;
 	if (associated_bitmap)
@@ -157,7 +159,7 @@ ml_nlink_convert_vdev_bitmap_to_linkid_bitmap(
 			continue;
 		}
 		associated_link_bitmap |= 1 << link_id;
-		j = vdev_id / 32;
+		j = vdev_id / vdev_per_bitmap;
 		if (j >= vdev_id_bitmap_sz) {
 			mlo_err("invalid vdev id %d", vdev_id);
 			continue;
@@ -165,7 +167,7 @@ ml_nlink_convert_vdev_bitmap_to_linkid_bitmap(
 		/* If the vdev_id is not interested one which is specified
 		 * in "vdev_id_bitmap", continue the search.
 		 */
-		if (!(vdev_id_bitmap[j] & (1 << (vdev_id % 32))))
+		if (!(vdev_id_bitmap[j] & (1 << (vdev_id % vdev_per_bitmap))))
 			continue;
 
 		*link_bitmap |= 1 << link_id;
@@ -2046,6 +2048,8 @@ ml_nlink_conn_change_notify(struct wlan_objmgr_psoc *psoc,
 	enum QDF_OPMODE mode;
 	QDF_STATUS status = QDF_STATUS_SUCCESS;
 	struct ml_link_force_state curr_force_state = {0};
+	bool is_set_link_in_progress = policy_mgr_is_set_link_in_progress(psoc);
+	bool is_host_force;
 
 	mlo_debug("vdev %d %s(%d)", vdev_id, link_evt_to_string(evt),
 		  evt);
@@ -2059,6 +2063,34 @@ ml_nlink_conn_change_notify(struct wlan_objmgr_psoc *psoc,
 
 	switch (evt) {
 	case ml_nlink_link_switch_start_evt:
+		if (data->evt.link_switch.reason ==
+		    MLO_LINK_SWITCH_REASON_HOST_FORCE) {
+			is_host_force = true;
+		} else {
+			is_host_force = false;
+		}
+
+		mlo_debug("set_link_in_prog %d reason %d",
+			  is_set_link_in_progress,
+			  data->evt.link_switch.reason);
+
+		if (is_set_link_in_progress) {
+			/* If set active is in progress then only accept host
+			 * force link switch requests from FW
+			 */
+			if (is_host_force)
+				status = QDF_STATUS_SUCCESS;
+			else
+				status = QDF_STATUS_E_INVAL;
+			break;
+		} else if (is_host_force) {
+			/* If set active is not in progress but FW sent host
+			 * force then reject the link switch
+			 */
+			status = QDF_STATUS_E_INVAL;
+			break;
+		}
+
 		ml_nlink_get_curr_force_state(psoc, vdev, &curr_force_state);
 		if ((1 << data->evt.link_switch.new_ieee_link_id) &
 		    curr_force_state.force_inactive_bitmap) {

core/sme/src/common/sme_api.c

@@ -84,6 +84,7 @@
 #include "wlan_wifi_pos_interface.h"
 #include "wlan_cp_stats_mc_ucfg_api.h"
 #include "wlan_psoc_mlme_ucfg_api.h"
+#include <wlan_mlo_link_force.h>
 
 static QDF_STATUS init_sme_cmd_list(struct mac_context *mac);
 
@@ -15453,8 +15454,14 @@ void sme_activate_mlo_links(mac_handle_t mac_handle, uint8_t session_id,
 		return;
 	}
 
-	policy_mgr_activate_mlo_links(mac_ctx->psoc, session_id, num_links,
-				      active_link_addr);
+	if (ml_is_nlink_service_supported(mac_ctx->psoc)) {
+		policy_mgr_activate_mlo_links_nlink(mac_ctx->psoc, session_id,
+						    num_links,
+						    active_link_addr);
+	} else {
+		policy_mgr_activate_mlo_links(mac_ctx->psoc, session_id,
+					      num_links, active_link_addr);
+	}
 }
 
 int sme_update_eht_caps(mac_handle_t mac_handle, uint8_t session_id,