qcacld-3.0: Handle link id bitmap from set link state response

Convert link id bitmap to vdev bitmap and update policy mgr
connection table.
Record the current force link command state to mlo context.

Change-Id: I0b28222de10a9493a153ee00189b87e4275a4231
CRs-Fixed: 3515467

Kbuild

@@ -1359,6 +1359,7 @@ UMAC_MLO_MGR_OBJS := $(UMAC_MLO_MGR_CMN_DIR)/src/wlan_mlo_mgr_main.o \
 			  $(UMAC_MLO_MGR_CMN_DIR)/src/wlan_mlo_mgr_msgq.o \
 			  $(UMAC_MLO_MGR_CMN_DIR)/src/wlan_mlo_mgr_primary_umac.o \
 			  $(MLO_MGR_TARGET_IF_DIR)/src/target_if_mlo_mgr.o \
+			  $(UMAC_MLO_MGR_CLD_DIR)/src/wlan_mlo_link_force.o \
 			  $(UMAC_MLO_MGR_CLD_DIR)/src/wlan_mlo_mgr_roam.o \
 			  $(UMAC_MLO_MGR_CLD_DIR)/src/wlan_t2lm_api.o \
 			  $(UMAC_MLO_MGR_CMN_DIR)/src/wlan_mlo_t2lm.o \

components/cmn_services/policy_mgr/inc/wlan_policy_mgr_api.h

@@ -30,6 +30,7 @@
 #include "qdf_types.h"
 #include "qdf_status.h"
 #include "wlan_objmgr_psoc_obj.h"
+#include "wlan_mlo_mgr_public_structs.h"
 #include "wlan_policy_mgr_public_struct.h"
 #include "wlan_cm_roam_public_struct.h"
 #include "wlan_utility.h"
@@ -1156,6 +1157,37 @@ polic_mgr_send_pcl_to_fw(struct wlan_objmgr_psoc *psoc,
 			 enum QDF_OPMODE mode);
 
 #ifdef WLAN_FEATURE_11BE_MLO
+/**
+ * policy_mgr_mlo_sta_set_nlink() - Set link mode for MLO STA
+ * by link id bitmap
+ * @psoc: psoc object
+ * @vdev: vdev object
+ * @reason: reason to set
+ * @mode: mode to set
+ * @link_num: number of link, valid for mode:
+ * MLO_LINK_FORCE_MODE_ACTIVE_NUM, MLO_LINK_FORCE_MODE_INACTIVE_NUM
+ * @link_bitmap: link bitmap, valid for mode:
+ * MLO_LINK_FORCE_MODE_ACTIVE, MLO_LINK_FORCE_MODE_INACTIVE,
+ * MLO_LINK_FORCE_MODE_ACTIVE_NUM, MLO_LINK_FORCE_MODE_INACTIVE_NUM
+ * MLO_LINK_FORCE_MODE_NO_FORCE.
+ * @link_bitmap2: inactive link bitmap, only valid for mode
+ * MLO_LINK_FORCE_MODE_ACTIVE_INACTIVE
+ * @link_control_flags: bitmap of enum link_control_flags.
+ *
+ * Interface to set link mode for MLO STA
+ *
+ * Return: QDF_STATUS
+ */
+QDF_STATUS
+policy_mgr_mlo_sta_set_nlink(struct wlan_objmgr_psoc *psoc,
+			     struct wlan_objmgr_vdev *vdev,
+			     enum mlo_link_force_reason reason,
+			     enum mlo_link_force_mode mode,
+			     uint8_t link_num,
+			     uint16_t link_bitmap,
+			     uint16_t link_bitmap2,
+			     uint32_t link_control_flags);
+
 /**
  * policy_mgr_mlo_sta_set_link() - Set link mode for MLO STA
  * @psoc: psoc object

components/cmn_services/policy_mgr/inc/wlan_policy_mgr_public_struct.h

@@ -117,6 +117,25 @@ enum sap_csa_reason_code {
 	CSA_REASON_SAP_FIX_CH_CONC_WITH_GO
 };
 
+/*
+ * enum link_control_flags: This enum is used for setting
+ * mlo_control_flags by api policy_mgr_mlo_sta_set_nlink.
+ * @link_ctrl_f_overwrite_active_bitmap: indicate overwrite all earlier
+ * force_active bitmaps. Used with MLO_LINK_FORCE_MODE_ACTIVE or
+ * MLO_LINK_FORCE_MODE_ACTIVE_INACTIVE
+ * @link_ctrl_f_overwrite_inactive_bitmap: indicate overwrite all earlier
+ * force_inactive bitmaps. Used with MLO_LINK_FORCE_MODE_INACTIVE or
+ * MLO_LINK_FORCE_MODE_ACTIVE_INACTIVE.
+ * @link_ctrl_f_dynamic_force_link_num: indicate fw to use force link number
+ * instead of force link bitmaps. Used with MLO_LINK_FORCE_MODE_ACTIVE_NUM.
+ * MLO_LINK_FORCE_MODE_INACTIVE_NUM, MLO_LINK_FORCE_MODE_NO_FORCE.
+ */
+enum link_control_flags {
+	link_ctrl_f_overwrite_active_bitmap =   1 << 0,
+	link_ctrl_f_overwrite_inactive_bitmap = 1 << 1,
+	link_ctrl_f_dynamic_force_link_num =    1 << 2,
+};
+
 /**
  * enum hw_mode_ss_config - Possible spatial stream configuration
  * @HW_MODE_SS_0x0: Unused Tx and Rx of MAC

components/cmn_services/policy_mgr/src/wlan_policy_mgr_get_set_utils.c

@@ -45,6 +45,7 @@
 #include "wlan_cm_roam_api.h"
 #include "wlan_mlme_ucfg_api.h"
 #include "wlan_p2p_ucfg_api.h"
+#include "wlan_mlo_link_force.h"
 
 /* invalid channel id. */
 #define INVALID_CHANNEL_ID 0
@@ -4285,13 +4286,321 @@ policy_mgr_trigger_roam_on_link_removal(struct wlan_objmgr_vdev *vdev)
 		policy_mgr_err("roam invoke failed");
 }
 
+static void
+policy_mgr_handle_vdev_active_inactive_resp(
+					struct wlan_objmgr_psoc *psoc,
+					struct wlan_objmgr_vdev *vdev,
+					struct mlo_link_set_active_req *req,
+					struct mlo_link_set_active_resp *resp)
+{
+	uint8_t i;
+	uint8_t vdev_id_num = 0;
+	uint8_t vdev_ids[WLAN_MLO_MAX_VDEVS] = {0};
+	uint32_t assoc_bitmap = 0;
+
+	/* convert link id to vdev id and update vdev status based
+	 * on both inactive and active bitmap.
+	 * In link bitmap based WMI event (use_ieee_link_id = true),
+	 * target will always indicate current force inactive and
+	 * active bitmaps to host. For links in inactive_linkid_bitmap,
+	 * they will be moved to policy mgr disable connection table.
+	 * for other links, they will be in active tables.
+	 */
+	ml_nlink_convert_linkid_bitmap_to_vdev_bitmap(
+		psoc, vdev, resp->inactive_linkid_bitmap,
+		&assoc_bitmap,
+		&resp->inactive_sz, resp->inactive,
+		&vdev_id_num, vdev_ids);
+	ml_nlink_convert_linkid_bitmap_to_vdev_bitmap(
+		psoc, vdev,
+		(~resp->inactive_linkid_bitmap) & assoc_bitmap,
+		NULL,
+		&resp->active_sz, resp->active,
+		&vdev_id_num, vdev_ids);
+	for (i = 0; i < resp->inactive_sz; i++)
+		policy_mgr_enable_disable_link_from_vdev_bitmask(
+				psoc, 0, resp->inactive[i], i * 32);
+	for (i = 0; i < resp->active_sz; i++)
+		policy_mgr_enable_disable_link_from_vdev_bitmask(
+				psoc, resp->active[i], 0, i * 32);
+}
+
+static void
+policy_mgr_handle_force_active_resp(struct wlan_objmgr_psoc *psoc,
+				    struct wlan_objmgr_vdev *vdev,
+				    struct mlo_link_set_active_req *req,
+				    struct mlo_link_set_active_resp *resp)
+{
+	uint8_t i;
+	uint32_t assoc_bitmap = 0;
+
+	if (resp->use_ieee_link_id) {
+		/* save link force active bitmap */
+		ml_nlink_set_curr_force_active_state(
+			psoc, vdev, req->param.force_cmd.ieee_link_id_bitmap,
+			req->param.control_flags.overwrite_force_active_bitmap ?
+			LINK_OVERWRITE : LINK_ADD);
+
+		/* update vdev active inactive status */
+		policy_mgr_handle_vdev_active_inactive_resp(psoc, vdev, req,
+							    resp);
+		return;
+	}
+	ml_nlink_convert_vdev_bitmap_to_linkid_bitmap(
+		psoc, vdev, req->param.num_vdev_bitmap,
+		req->param.vdev_bitmap, &resp->active_linkid_bitmap,
+		&assoc_bitmap);
+	ml_nlink_set_curr_force_active_state(
+		psoc, vdev, resp->active_linkid_bitmap, LINK_ADD);
+
+	for (i = 0; i < resp->active_sz; i++)
+		policy_mgr_enable_disable_link_from_vdev_bitmask(
+				psoc, resp->active[i], 0, i * 32);
+}
+
+static void
+policy_mgr_handle_force_inactive_resp(struct wlan_objmgr_psoc *psoc,
+				      struct wlan_objmgr_vdev *vdev,
+				      struct mlo_link_set_active_req *req,
+				      struct mlo_link_set_active_resp *resp)
+{
+	uint8_t i;
+	uint32_t assoc_bitmap = 0;
+
+	if (resp->use_ieee_link_id) {
+		/* save link force inactive bitmap */
+		ml_nlink_set_curr_force_inactive_state(
+			psoc, vdev, req->param.force_cmd.ieee_link_id_bitmap,
+			req->param.control_flags.overwrite_force_inactive_bitmap ?
+			LINK_OVERWRITE : LINK_ADD);
+
+		/* update vdev active inactive status */
+		policy_mgr_handle_vdev_active_inactive_resp(psoc, vdev, req,
+							    resp);
+		return;
+	}
+	ml_nlink_convert_vdev_bitmap_to_linkid_bitmap(
+		psoc, vdev, req->param.num_vdev_bitmap,
+		req->param.vdev_bitmap, &resp->inactive_linkid_bitmap,
+		&assoc_bitmap);
+	ml_nlink_set_curr_force_inactive_state(
+		psoc, vdev, resp->inactive_linkid_bitmap, LINK_ADD);
+
+	for (i = 0; i < resp->inactive_sz; i++)
+		policy_mgr_enable_disable_link_from_vdev_bitmask(
+				psoc, 0, resp->inactive[i], i * 32);
+}
+
+static void
+policy_mgr_handle_force_active_num_resp(struct wlan_objmgr_psoc *psoc,
+					struct wlan_objmgr_vdev *vdev,
+					struct mlo_link_set_active_req *req,
+					struct mlo_link_set_active_resp *resp)
+{
+	uint8_t i;
+	uint32_t assoc_bitmap = 0;
+	uint32_t link_bitmap = 0;
+
+	if (resp->use_ieee_link_id) {
+		/* save force num and force num bitmap */
+		ml_nlink_set_curr_force_active_num_state(
+			psoc, vdev, req->param.force_cmd.link_num,
+			req->param.force_cmd.ieee_link_id_bitmap);
+
+		/* update vdev active inactive status */
+		policy_mgr_handle_vdev_active_inactive_resp(psoc, vdev, req,
+							    resp);
+		return;
+	}
+	ml_nlink_convert_vdev_bitmap_to_linkid_bitmap(
+		psoc, vdev, req->param.num_vdev_bitmap,
+		req->param.vdev_bitmap,
+		&link_bitmap,
+		&assoc_bitmap);
+	ml_nlink_set_curr_force_active_num_state(
+		psoc, vdev, req->param.link_num[0].num_of_link,
+		link_bitmap);
+	/*
+	 * When the host sends a set link command with force link num
+	 * and dynamic flag set, FW may not process it immediately.
+	 * In this case FW buffer the request and sends a response as
+	 * success to the host with VDEV bitmap as zero.
+	 * FW ensures that the number of active links will be equal to
+	 * the link num sent via WMI_MLO_LINK_SET_ACTIVE_CMDID command.
+	 * So the host should also fill the mlo policy_mgr table as per
+	 * request.
+	 */
+	if (req->param.control_flags.dynamic_force_link_num) {
+		policy_mgr_debug("Enable ML vdev(s) as sent in req");
+		for (i = 0; i < req->param.num_vdev_bitmap; i++)
+			policy_mgr_enable_disable_link_from_vdev_bitmask(
+				psoc,
+				req->param.vdev_bitmap[i], 0, i * 32);
+		return;
+	}
+
+	/*
+	 * MLO_LINK_FORCE_MODE_ACTIVE_NUM return which vdev is active
+	 * So XOR of the requested ML vdev and active vdev bit will give
+	 * the vdev bits to disable
+	 */
+	for (i = 0; i < resp->active_sz; i++)
+		policy_mgr_enable_disable_link_from_vdev_bitmask(
+			psoc, resp->active[i],
+			resp->active[i] ^ req->param.vdev_bitmap[i],
+			i * 32);
+}
+
+static void
+policy_mgr_handle_force_inactive_num_resp(
+				struct wlan_objmgr_psoc *psoc,
+				struct wlan_objmgr_vdev *vdev,
+				struct mlo_link_set_active_req *req,
+				struct mlo_link_set_active_resp *resp)
+{
+	uint8_t i;
+	uint32_t assoc_bitmap = 0;
+	uint32_t link_bitmap = 0;
+
+	if (resp->use_ieee_link_id) {
+		/* save force num and force num bitmap */
+		ml_nlink_set_curr_force_inactive_num_state(
+			psoc, vdev, req->param.force_cmd.link_num,
+			req->param.force_cmd.ieee_link_id_bitmap);
+
+		/* update vdev active inactive status */
+		policy_mgr_handle_vdev_active_inactive_resp(psoc, vdev, req,
+							    resp);
+		return;
+	}
+	ml_nlink_convert_vdev_bitmap_to_linkid_bitmap(
+		psoc, vdev, req->param.num_vdev_bitmap,
+		req->param.vdev_bitmap,
+		&link_bitmap,
+		&assoc_bitmap);
+	ml_nlink_set_curr_force_inactive_num_state(
+		psoc, vdev, req->param.link_num[0].num_of_link,
+		link_bitmap);
+
+	/*
+	 * MLO_LINK_FORCE_MODE_INACTIVE_NUM return which vdev is
+	 * inactive So XOR of the requested ML vdev and inactive vdev
+	 * bit will give the vdev bits to be enable.
+	 */
+	for (i = 0; i < resp->inactive_sz; i++)
+		policy_mgr_enable_disable_link_from_vdev_bitmask(
+			psoc,
+			resp->inactive[i] ^ req->param.vdev_bitmap[i],
+			resp->inactive[i], i * 32);
+}
+
+static void
+policy_mgr_handle_force_active_inactive_resp(
+				struct wlan_objmgr_psoc *psoc,
+				struct wlan_objmgr_vdev *vdev,
+				struct mlo_link_set_active_req *req,
+				struct mlo_link_set_active_resp *resp)
+{
+	uint8_t i;
+	uint32_t assoc_bitmap = 0;
+
+	if (resp->use_ieee_link_id) {
+		/* save link active/inactive bitmap */
+		ml_nlink_set_curr_force_active_state(
+			psoc, vdev, req->param.force_cmd.ieee_link_id_bitmap,
+			req->param.control_flags.overwrite_force_active_bitmap ?
+			LINK_OVERWRITE : LINK_ADD);
+		ml_nlink_set_curr_force_inactive_state(
+			psoc, vdev, req->param.force_cmd.ieee_link_id_bitmap2,
+			req->param.control_flags.overwrite_force_inactive_bitmap ?
+			LINK_OVERWRITE : LINK_ADD);
+
+		/* update vdev active inactive status */
+		policy_mgr_handle_vdev_active_inactive_resp(psoc, vdev, req,
+							    resp);
+		return;
+	}
+	ml_nlink_convert_vdev_bitmap_to_linkid_bitmap(
+		psoc, vdev, req->param.num_inactive_vdev_bitmap,
+		req->param.inactive_vdev_bitmap,
+		&resp->inactive_linkid_bitmap,
+		&assoc_bitmap);
+	ml_nlink_set_curr_force_inactive_state(
+		psoc, vdev, resp->inactive_linkid_bitmap, LINK_ADD);
+	ml_nlink_convert_vdev_bitmap_to_linkid_bitmap(
+		psoc, vdev, req->param.num_vdev_bitmap,
+		req->param.vdev_bitmap,
+		&resp->active_linkid_bitmap,
+		&assoc_bitmap);
+	ml_nlink_set_curr_force_active_state(
+		psoc, vdev, resp->active_linkid_bitmap, LINK_ADD);
+
+	for (i = 0; i < resp->inactive_sz; i++)
+		policy_mgr_enable_disable_link_from_vdev_bitmask(
+				psoc, 0, resp->inactive[i], i * 32);
+	for (i = 0; i < resp->active_sz; i++)
+		policy_mgr_enable_disable_link_from_vdev_bitmask(
+				psoc, resp->active[i], 0, i * 32);
+}
+
+static void
+policy_mgr_handle_no_force_resp(struct wlan_objmgr_psoc *psoc,
+				struct wlan_objmgr_vdev *vdev,
+				struct mlo_link_set_active_req *req,
+				struct mlo_link_set_active_resp *resp)
+{
+	uint8_t i;
+	uint32_t assoc_bitmap = 0;
+	uint32_t link_bitmap = 0;
+
+	if (resp->use_ieee_link_id) {
+		/* update link inactive/active bitmap */
+		if (req->param.force_cmd.ieee_link_id_bitmap) {
+			ml_nlink_set_curr_force_inactive_state(
+				psoc, vdev,
+				req->param.force_cmd.ieee_link_id_bitmap,
+				LINK_CLR);
+			ml_nlink_set_curr_force_active_state(
+				psoc, vdev,
+				req->param.force_cmd.ieee_link_id_bitmap,
+				LINK_CLR);
+		} else {
+			/* special handling for no force to clear all */
+			ml_nlink_clr_force_state(psoc, vdev);
+		}
+
+		/* update vdev active inactive status */
+		policy_mgr_handle_vdev_active_inactive_resp(psoc, vdev, req,
+							    resp);
+		return;
+	}
+
+	ml_nlink_convert_vdev_bitmap_to_linkid_bitmap(
+		psoc, vdev, req->param.num_vdev_bitmap,
+		req->param.vdev_bitmap,
+		&link_bitmap, &assoc_bitmap);
+
+	ml_nlink_set_curr_force_inactive_state(
+		psoc, vdev, link_bitmap, LINK_CLR);
+	ml_nlink_set_curr_force_active_state(
+		psoc, vdev, link_bitmap, LINK_CLR);
+	ml_nlink_set_curr_force_active_num_state(
+		psoc, vdev, 0, 0);
+	ml_nlink_set_curr_force_inactive_num_state(
+		psoc, vdev, 0, 0);
+
+	/* Enable all the ML vdev id sent in request */
+	for (i = 0; i < req->param.num_vdev_bitmap; i++)
+		policy_mgr_enable_disable_link_from_vdev_bitmask(
+			psoc, req->param.vdev_bitmap[i], 0, i * 32);
+}
+
 static void
 policy_mgr_handle_link_enable_disable_resp(struct wlan_objmgr_vdev *vdev,
 					  void *arg,
 					  struct mlo_link_set_active_resp *resp)
 {
 	struct mlo_link_set_active_req *req = arg;
-	uint8_t i;
 	struct wlan_objmgr_psoc *psoc;
 	struct policy_mgr_psoc_priv_obj *pm_ctx;
 
@@ -4327,70 +4636,24 @@ policy_mgr_handle_link_enable_disable_resp(struct wlan_objmgr_vdev *vdev,
 			 resp->active[0], resp->inactive[0]);
 	switch (req->param.force_mode) {
 	case MLO_LINK_FORCE_MODE_ACTIVE:
-		for (i = 0; i < resp->active_sz; i++)
-			policy_mgr_enable_disable_link_from_vdev_bitmask(psoc,
-					resp->active[i], 0, i * 32);
+		policy_mgr_handle_force_active_resp(psoc, vdev, req, resp);
 		break;
 	case MLO_LINK_FORCE_MODE_INACTIVE:
-		for (i = 0; i < resp->inactive_sz; i++)
-			policy_mgr_enable_disable_link_from_vdev_bitmask(psoc,
-				0, resp->inactive[i], i * 32);
+		policy_mgr_handle_force_inactive_resp(psoc, vdev, req, resp);
 		break;
 	case MLO_LINK_FORCE_MODE_ACTIVE_NUM:
-		/*
-		 * When the host sends a set link command with force link num
-		 * and dynamic flag set, FW may not process it immediately.
-		 * In this case FW buffer the request and sends a response as
-		 * success to the host with VDEV bitmap as zero.
-		 * FW ensures that the number of active links will be equal to
-		 * the link num sent via WMI_MLO_LINK_SET_ACTIVE_CMDID command.
-		 * So the host should also fill the mlo policy_mgr table as per
-		 * request.
-		 */
-		if (req->param.control_flags.dynamic_force_link_num) {
-			policy_mgr_debug("Enable ML vdev(s) as sent in req");
-			for (i = 0; i < req->param.num_vdev_bitmap; i++)
-			       policy_mgr_enable_disable_link_from_vdev_bitmask(
-					psoc,
-					req->param.vdev_bitmap[i], 0, i * 32);
-			break;
-		}
-
-		/*
-		 * MLO_LINK_FORCE_MODE_ACTIVE_NUM return which vdev is active
-		 * So XOR of the requested ML vdev and active vdev bit will give
-		 * the vdev bits to disable
-		 */
-		for (i = 0; i < resp->active_sz; i++)
-			policy_mgr_enable_disable_link_from_vdev_bitmask(psoc,
-				resp->active[i],
-				resp->active[i] ^ req->param.vdev_bitmap[i],
-				i * 32);
+		policy_mgr_handle_force_active_num_resp(psoc, vdev, req, resp);
 		break;
 	case MLO_LINK_FORCE_MODE_INACTIVE_NUM:
-		/*
-		 * MLO_LINK_FORCE_MODE_INACTIVE_NUM return which vdev is
-		 * inactive So XOR of the requested ML vdev and inactive vdev
-		 * bit will give the vdev bits to be enable.
-		 */
-		for (i = 0; i < resp->inactive_sz; i++)
-			policy_mgr_enable_disable_link_from_vdev_bitmask(psoc,
-				resp->inactive[i] ^ req->param.vdev_bitmap[i],
-				resp->inactive[i], i * 32);
+		policy_mgr_handle_force_inactive_num_resp(psoc, vdev, req,
+							  resp);
 		break;
 	case MLO_LINK_FORCE_MODE_NO_FORCE:
-		/* Enable all the ML vdev id sent in request */
-		for (i = 0; i < req->param.num_vdev_bitmap; i++)
-			policy_mgr_enable_disable_link_from_vdev_bitmask(psoc,
-					req->param.vdev_bitmap[i], 0, i * 32);
+		policy_mgr_handle_no_force_resp(psoc, vdev, req, resp);
 		break;
 	case MLO_LINK_FORCE_MODE_ACTIVE_INACTIVE:
-		for (i = 0; i < resp->active_sz; i++)
-			policy_mgr_enable_disable_link_from_vdev_bitmask(
-					psoc, resp->active[i], 0, i * 32);
-		for (i = 0; i < resp->inactive_sz; i++)
-			policy_mgr_enable_disable_link_from_vdev_bitmask(
-					psoc, 0, resp->inactive[i], i * 32);
+		policy_mgr_handle_force_active_inactive_resp(psoc, vdev, req,
+							     resp);
 		break;
 	default:
 		policy_mgr_err("Invalid request req mode %d",
@@ -4408,7 +4671,6 @@ complete_evnt:
 	if (req && resp && !resp->status)
 		policy_mgr_check_concurrent_intf_and_restart_sap(psoc, false);
 }
-
 #else
 static inline QDF_STATUS
 policy_mgr_delete_from_disabled_links(struct policy_mgr_psoc_priv_obj *pm_ctx,
@@ -5575,6 +5837,91 @@ policy_mgr_mlo_sta_set_link(struct wlan_objmgr_psoc *psoc,
 					       mlo_vdev_lst, 0, NULL);
 }
 
+QDF_STATUS
+policy_mgr_mlo_sta_set_nlink(struct wlan_objmgr_psoc *psoc,
+			     struct wlan_objmgr_vdev *vdev,
+			     enum mlo_link_force_reason reason,
+			     enum mlo_link_force_mode mode,
+			     uint8_t link_num,
+			     uint16_t link_bitmap,
+			     uint16_t link_bitmap2,
+			     uint32_t link_control_flags)
+{
+	struct mlo_link_set_active_req *req;
+	QDF_STATUS status;
+	struct policy_mgr_psoc_priv_obj *pm_ctx;
+
+	pm_ctx = policy_mgr_get_context(psoc);
+	if (!pm_ctx) {
+		policy_mgr_err("Invalid Context");
+		return QDF_STATUS_E_INVAL;
+	}
+
+	req = qdf_mem_malloc(sizeof(*req));
+	if (!req)
+		return QDF_STATUS_E_NOMEM;
+
+	status = wlan_objmgr_vdev_try_get_ref(vdev, WLAN_POLICY_MGR_ID);
+	if (QDF_IS_STATUS_ERROR(status)) {
+		qdf_mem_free(req);
+		return QDF_STATUS_E_FAILURE;
+	}
+
+	policy_mgr_set_link_in_progress(pm_ctx, true);
+
+	policy_mgr_debug("vdev %d: mode %d %s reason %d",
+			 wlan_vdev_get_id(vdev), mode,
+			 force_mode_to_string(mode), reason);
+
+	req->ctx.vdev = vdev;
+	req->param.reason = reason;
+	req->param.force_mode = mode;
+	req->param.use_ieee_link_id = true;
+	req->param.force_cmd.ieee_link_id_bitmap = link_bitmap;
+	req->param.force_cmd.ieee_link_id_bitmap2 = link_bitmap2;
+	req->param.force_cmd.link_num = link_num;
+	if (link_control_flags & link_ctrl_f_overwrite_active_bitmap)
+		req->param.control_flags.overwrite_force_active_bitmap = true;
+	if (link_control_flags & link_ctrl_f_overwrite_inactive_bitmap)
+		req->param.control_flags.overwrite_force_inactive_bitmap =
+									true;
+	if (link_control_flags & link_ctrl_f_dynamic_force_link_num)
+		req->param.control_flags.dynamic_force_link_num = true;
+
+	status =
+	wlan_vdev_get_bss_peer_mld_mac(vdev,
+				       &req->param.force_cmd.ap_mld_mac_addr);
+	if (QDF_IS_STATUS_ERROR(status)) {
+		policy_mgr_err("fail to get ap mld addr for vdev %d",
+			       wlan_vdev_get_id(vdev));
+		goto end;
+	}
+	if (qdf_is_macaddr_zero(&req->param.force_cmd.ap_mld_mac_addr)) {
+		policy_mgr_err("get ap zero mld addr for vdev %d",
+			       wlan_vdev_get_id(vdev));
+		goto end;
+	}
+
+	req->ctx.set_mlo_link_cb = policy_mgr_handle_link_enable_disable_resp;
+	req->ctx.validate_set_mlo_link_cb =
+		policy_mgr_validate_set_mlo_link_cb;
+	req->ctx.cb_arg = req;
+	status = mlo_ser_set_link_req(req);
+end:
+	if (QDF_IS_STATUS_ERROR(status)) {
+		policy_mgr_err("vdev %d: Failed to set link mode %d num_mlo_vdev %d reason %d",
+			       wlan_vdev_get_id(vdev), mode, link_num,
+			       reason);
+		qdf_mem_free(req);
+		policy_mgr_set_link_in_progress(pm_ctx, false);
+	} else {
+		status = QDF_STATUS_E_PENDING;
+	}
+	wlan_objmgr_vdev_release_ref(vdev, WLAN_POLICY_MGR_ID);
+
+	return status;
+}
+
 uint32_t
 policy_mgr_get_conc_ext_flags(struct wlan_objmgr_vdev *vdev, bool force_mlo)
 {

components/umac/mlme/connection_mgr/core/src/wlan_cm_roam_fw_sync.c

@@ -47,6 +47,7 @@
 #include <wlan_mlo_mgr_sta.h>
 #include "wlan_mlo_mgr_roam.h"
 #include "wlan_vdev_mgr_utils_api.h"
+#include "wlan_mlo_link_force.h"
 
 QDF_STATUS cm_fw_roam_sync_req(struct wlan_objmgr_psoc *psoc, uint8_t vdev_id,
 			       void *event, uint32_t event_data_len)
@@ -1061,6 +1062,7 @@ cm_get_and_disable_link_from_roam_ind(struct wlan_objmgr_psoc *psoc,
 				      struct roam_offload_synch_ind *synch_data)
 {
 	uint8_t i;
+	struct wlan_objmgr_vdev *vdev;
 
 	for (i = 0; i < synch_data->num_setup_links; i++) {
 		if (synch_data->ml_link[i].vdev_id == vdev_id &&
@@ -1069,6 +1071,18 @@ cm_get_and_disable_link_from_roam_ind(struct wlan_objmgr_psoc *psoc,
 				  vdev_id, synch_data->ml_link[i].flags);
 			policy_mgr_move_vdev_from_connection_to_disabled_tbl(
 								psoc, vdev_id);
+
+			vdev = wlan_objmgr_get_vdev_by_id_from_psoc(psoc,
+								    vdev_id,
+								    WLAN_MLME_SB_ID);
+			if (!vdev) {
+				mlme_debug("no vdev for id %d", vdev_id);
+				break;
+			}
+			ml_nlink_set_curr_force_inactive_state(
+				psoc, vdev, synch_data->ml_link[i].link_id,
+				LINK_ADD);
+			wlan_objmgr_vdev_release_ref(vdev, WLAN_MLME_SB_ID);
 			break;
 		}
 	}

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

@@ -0,0 +1,204 @@
+/*
+ * Copyright (c) 2023 Qualcomm Innovation Center, Inc. All rights reserved.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+/*
+ * DOC: contains ML STA link force active/inactive public API
+ */
+#ifndef _WLAN_MLO_LINK_FORCE_H_
+#define _WLAN_MLO_LINK_FORCE_H_
+
+#include <wlan_mlo_mgr_cmn.h>
+#include <wlan_mlo_mgr_public_structs.h>
+
+#ifdef WLAN_FEATURE_11BE_MLO
+static inline const char *force_mode_to_string(uint32_t mode)
+{
+	switch (mode) {
+	CASE_RETURN_STRING(MLO_LINK_FORCE_MODE_ACTIVE);
+	CASE_RETURN_STRING(MLO_LINK_FORCE_MODE_INACTIVE);
+	CASE_RETURN_STRING(MLO_LINK_FORCE_MODE_ACTIVE_NUM);
+	CASE_RETURN_STRING(MLO_LINK_FORCE_MODE_INACTIVE_NUM);
+	CASE_RETURN_STRING(MLO_LINK_FORCE_MODE_NO_FORCE);
+	CASE_RETURN_STRING(MLO_LINK_FORCE_MODE_ACTIVE_INACTIVE);
+	default:
+		return "Unknown";
+	}
+};
+
+#define ml_nlink_dump_force_state(_force_state, format, args...) \
+	mlo_debug("inactive 0x%x active 0x%x inact num %d 0x%x act num %d 0x%x "format, \
+			 (_force_state)->force_inactive_bitmap, \
+			 (_force_state)->force_active_bitmap, \
+			 (_force_state)->force_inactive_num, \
+			 (_force_state)->force_inactive_num_bitmap, \
+			 (_force_state)->force_active_num, \
+			 (_force_state)->force_active_num_bitmap, \
+			 ##args);
+
+/**
+ * ml_nlink_convert_linkid_bitmap_to_vdev_bitmap() - convert link
+ * id bitmap to vdev id bitmap
+ * state
+ * @psoc: psoc object
+ * @vdev: vdev object
+ * @link_bitmap: link id bitmap
+ * @associated_bitmap: all associated the link id bitmap
+ * @vdev_id_bitmap_sz: number vdev id bitamp in vdev_id_bitmap
+ * @vdev_id_bitmap: array to return the vdev id bitmaps
+ * @vdev_id_num: total vdev id number in vdev_ids
+ * @vdev_ids: vdev id array
+ *
+ * Return: None
+ */
+void
+ml_nlink_convert_linkid_bitmap_to_vdev_bitmap(
+			struct wlan_objmgr_psoc *psoc,
+			struct wlan_objmgr_vdev *vdev,
+			uint32_t link_bitmap,
+			uint32_t *associated_bitmap,
+			uint32_t *vdev_id_bitmap_sz,
+			uint32_t vdev_id_bitmap[MLO_VDEV_BITMAP_SZ],
+			uint8_t *vdev_id_num,
+			uint8_t vdev_ids[WLAN_MLO_MAX_VDEVS]);
+
+/**
+ * ml_nlink_convert_vdev_bitmap_to_linkid_bitmap() - convert vdev
+ * id bitmap to link id bitmap
+ * state
+ * @psoc: psoc object
+ * @vdev: vdev object
+ * @vdev_id_bitmap_sz: array size of vdev_id_bitmap
+ * @vdev_id_bitmap: vdev bitmap array
+ * @link_bitmap: link id bitamp converted from vdev id bitmap
+ * @associated_bitmap: all associated the link id bitmap
+ *
+ * Return: None
+ */
+void
+ml_nlink_convert_vdev_bitmap_to_linkid_bitmap(
+				struct wlan_objmgr_psoc *psoc,
+				struct wlan_objmgr_vdev *vdev,
+				uint32_t vdev_id_bitmap_sz,
+				uint32_t *vdev_id_bitmap,
+				uint32_t *link_bitmap,
+				uint32_t *associated_bitmap);
+
+/**
+ * enum set_curr_control - control flag to update current force bitmap
+ * @LINK_OVERWRITE: use bitmap to overwrite existing force bitmap
+ * @LINK_CLR: clr the input bitmap from existing force bitmap
+ * @LINK_ADD: append the input bitmap to existing force bitamp
+ *
+ * This control value will be used in ml_nlink_set_* API to indicate
+ * how to update input link_bitmap to current bitmap
+ */
+enum set_curr_control {
+	LINK_OVERWRITE = 0x0,
+	LINK_CLR       = 0x1,
+	LINK_ADD       = 0x2,
+};
+
+/**
+ * ml_nlink_set_curr_force_active_state() - set link force active
+ * state
+ * @psoc: psoc object
+ * @vdev: vdev object
+ * @link_bitmap: force active link id bitmap
+ * @ctrl: control value to indicate how to update link_bitmap to current
+ * bitmap
+ *
+ * Return: None
+ */
+void
+ml_nlink_set_curr_force_active_state(struct wlan_objmgr_psoc *psoc,
+				     struct wlan_objmgr_vdev *vdev,
+				     uint16_t link_bitmap,
+				     enum set_curr_control ctrl);
+
+/**
+ * ml_nlink_set_curr_force_inactive_state() - set link force inactive
+ * state
+ * @psoc: psoc object
+ * @vdev: vdev object
+ * @link_bitmap: force inactive link id bitmap
+ * @ctrl: control value to indicate how to update link_bitmap to current
+ * bitmap
+ *
+ * Return: None
+ */
+void
+ml_nlink_set_curr_force_inactive_state(struct wlan_objmgr_psoc *psoc,
+				       struct wlan_objmgr_vdev *vdev,
+				       uint16_t link_bitmap,
+				       enum set_curr_control ctrl);
+
+/**
+ * ml_nlink_set_curr_force_active_num_state() - set link force active
+ * number state
+ * @psoc: psoc object
+ * @vdev: vdev object
+ * @link_num: force active num
+ * @link_bitmap: force active num link id bitmap
+ *
+ * Return: None
+ */
+void
+ml_nlink_set_curr_force_active_num_state(struct wlan_objmgr_psoc *psoc,
+					 struct wlan_objmgr_vdev *vdev,
+					 uint8_t link_num,
+					 uint16_t link_bitmap);
+
+/**
+ * ml_nlink_set_curr_force_inactive_num_state() - set link force inactive
+ * number state
+ * @psoc: psoc object
+ * @vdev: vdev object
+ * @link_num: force inactive num
+ * @link_bitmap: force inactive num link id bitmap
+ *
+ * Return: None
+ */
+void
+ml_nlink_set_curr_force_inactive_num_state(struct wlan_objmgr_psoc *psoc,
+					   struct wlan_objmgr_vdev *vdev,
+					   uint8_t link_num,
+					   uint16_t link_bitmap);
+
+/**
+ * ml_nlink_get_curr_force_state() - get link force state
+ * @psoc: psoc object
+ * @vdev: vdev object
+ * @force_cmd: current force state
+ *
+ * Return: None
+ */
+void
+ml_nlink_get_curr_force_state(struct wlan_objmgr_psoc *psoc,
+			      struct wlan_objmgr_vdev *vdev,
+			      struct ml_link_force_state *force_cmd);
+
+/**
+ * ml_nlink_clr_force_state() - clear all link force state
+ * @psoc: psoc object
+ * @vdev: vdev object
+ *
+ * Return: None
+ */
+void
+ml_nlink_clr_force_state(struct wlan_objmgr_psoc *psoc,
+			 struct wlan_objmgr_vdev *vdev);
+#endif
+#endif

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

@@ -0,0 +1,329 @@
+/*
+ * Copyright (c) 2023 Qualcomm Innovation Center, Inc. All rights reserved.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+/*
+ * DOC: contains ML STA link force active/inactive related functionality
+ */
+#include "wlan_mlo_link_force.h"
+#include "wlan_mlo_mgr_sta.h"
+
+void
+ml_nlink_convert_linkid_bitmap_to_vdev_bitmap(
+			struct wlan_objmgr_psoc *psoc,
+			struct wlan_objmgr_vdev *vdev,
+			uint32_t link_bitmap,
+			uint32_t *associated_bitmap,
+			uint32_t *vdev_id_bitmap_sz,
+			uint32_t vdev_id_bitmap[MLO_VDEV_BITMAP_SZ],
+			uint8_t *vdev_id_num,
+			uint8_t vdev_ids[WLAN_MLO_MAX_VDEVS])
+{
+	struct wlan_mlo_dev_context *mlo_dev_ctx;
+	struct wlan_mlo_sta *sta_ctx;
+	uint8_t i, j, bitmap_sz = 0, num_vdev = 0;
+	uint16_t link_id;
+	uint8_t vdev_id;
+	uint32_t associated_link_bitmap = 0;
+
+	*vdev_id_bitmap_sz = 0;
+	*vdev_id_num = 0;
+	qdf_mem_zero(vdev_id_bitmap,
+		     sizeof(vdev_id_bitmap[0]) * MLO_VDEV_BITMAP_SZ);
+	qdf_mem_zero(vdev_ids,
+		     sizeof(vdev_ids[0]) * WLAN_MLO_MAX_VDEVS);
+	if (associated_bitmap)
+		*associated_bitmap = 0;
+
+	mlo_dev_ctx = wlan_vdev_get_mlo_dev_ctx(vdev);
+	if (!mlo_dev_ctx || !mlo_dev_ctx->sta_ctx) {
+		mlo_err("mlo_ctx or sta_ctx null");
+		return;
+	}
+
+	mlo_dev_lock_acquire(mlo_dev_ctx);
+	sta_ctx = mlo_dev_ctx->sta_ctx;
+	for (i = 0; i < WLAN_UMAC_MLO_MAX_VDEVS; i++) {
+		/*todo: add standby link */
+		if (!mlo_dev_ctx->wlan_vdev_list[i])
+			continue;
+		vdev_id = wlan_vdev_get_id(mlo_dev_ctx->wlan_vdev_list[i]);
+		if (!qdf_test_bit(i, sta_ctx->wlan_connected_links)) {
+			mlo_debug("vdev %d is not connected", vdev_id);
+			continue;
+		}
+
+		link_id = wlan_vdev_get_link_id(
+					mlo_dev_ctx->wlan_vdev_list[i]);
+		if (link_id >= MAX_MLO_LINK_ID) {
+			mlo_err("invalid link id %d", link_id);
+			continue;
+		}
+		associated_link_bitmap |= 1 << link_id;
+		/* If the link_id is not interested one which is specified
+		 * in "link_bitmap", continue the search.
+		 */
+		if (!(link_bitmap & (1 << link_id)))
+			continue;
+		j = vdev_id / 32;
+		if (j >= MLO_VDEV_BITMAP_SZ)
+			break;
+		vdev_id_bitmap[j] |= 1 << (vdev_id % 32);
+		if (j + 1 > bitmap_sz)
+			bitmap_sz = j + 1;
+
+		if (num_vdev >= WLAN_MLO_MAX_VDEVS)
+			break;
+		vdev_ids[num_vdev++] = vdev_id;
+	}
+	mlo_dev_lock_release(mlo_dev_ctx);
+
+	*vdev_id_bitmap_sz = bitmap_sz;
+	*vdev_id_num = num_vdev;
+	if (associated_bitmap)
+		*associated_bitmap = associated_link_bitmap;
+
+	mlo_debug("vdev %d link bitmap 0x%x vdev_bitmap 0x%x sz %d num %d assoc 0x%x for bitmap 0x%x",
+		  wlan_vdev_get_id(vdev), link_bitmap & associated_link_bitmap,
+		  vdev_id_bitmap[0], *vdev_id_bitmap_sz, num_vdev,
+		  associated_link_bitmap, link_bitmap);
+}
+
+void
+ml_nlink_convert_vdev_bitmap_to_linkid_bitmap(
+				struct wlan_objmgr_psoc *psoc,
+				struct wlan_objmgr_vdev *vdev,
+				uint32_t vdev_id_bitmap_sz,
+				uint32_t *vdev_id_bitmap,
+				uint32_t *link_bitmap,
+				uint32_t *associated_bitmap)
+{
+	struct wlan_mlo_dev_context *mlo_dev_ctx;
+	struct wlan_mlo_sta *sta_ctx;
+	uint8_t i, j;
+	uint16_t link_id;
+	uint8_t vdev_id;
+	uint32_t associated_link_bitmap = 0;
+
+	*link_bitmap = 0;
+	if (associated_bitmap)
+		*associated_bitmap = 0;
+	if (!vdev_id_bitmap_sz) {
+		mlo_debug("vdev_id_bitmap_sz 0");
+		return;
+	}
+
+	mlo_dev_ctx = wlan_vdev_get_mlo_dev_ctx(vdev);
+	if (!mlo_dev_ctx || !mlo_dev_ctx->sta_ctx) {
+		mlo_err("mlo_ctx or sta_ctx null");
+		return;
+	}
+
+	sta_ctx = mlo_dev_ctx->sta_ctx;
+	mlo_dev_lock_acquire(mlo_dev_ctx);
+	for (i = 0; i < WLAN_UMAC_MLO_MAX_VDEVS; i++) {
+		if (!mlo_dev_ctx->wlan_vdev_list[i])
+			continue;
+		vdev_id = wlan_vdev_get_id(mlo_dev_ctx->wlan_vdev_list[i]);
+		if (!qdf_test_bit(i, sta_ctx->wlan_connected_links)) {
+			mlo_debug("vdev %d is not connected", vdev_id);
+			continue;
+		}
+
+		link_id = wlan_vdev_get_link_id(
+					mlo_dev_ctx->wlan_vdev_list[i]);
+		if (link_id >= MAX_MLO_LINK_ID) {
+			mlo_err("invalid link id %d", link_id);
+			continue;
+		}
+		associated_link_bitmap |= 1 << link_id;
+		j = vdev_id / 32;
+		if (j >= vdev_id_bitmap_sz) {
+			mlo_err("invalid vdev id %d", vdev_id);
+			continue;
+		}
+		/* 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))))
+			continue;
+
+		*link_bitmap |= 1 << link_id;
+	}
+	mlo_dev_lock_release(mlo_dev_ctx);
+
+	if (associated_bitmap)
+		*associated_bitmap = associated_link_bitmap;
+	mlo_debug("vdev %d link bitmap 0x%x vdev_bitmap 0x%x sz %d assoc 0x%x",
+		  wlan_vdev_get_id(vdev), *link_bitmap, vdev_id_bitmap[0],
+		  vdev_id_bitmap_sz, associated_link_bitmap);
+}
+
+void
+ml_nlink_get_curr_force_state(struct wlan_objmgr_psoc *psoc,
+			      struct wlan_objmgr_vdev *vdev,
+			      struct ml_link_force_state *force_cmd)
+{
+	struct wlan_mlo_dev_context *mlo_dev_ctx;
+
+	mlo_dev_ctx = wlan_vdev_get_mlo_dev_ctx(vdev);
+	if (!mlo_dev_ctx || !mlo_dev_ctx->sta_ctx) {
+		mlo_err("mlo_ctx or sta_ctx null");
+		return;
+	}
+
+	mlo_dev_lock_acquire(mlo_dev_ctx);
+	qdf_mem_copy(force_cmd,
+		     &mlo_dev_ctx->sta_ctx->link_force_ctx.force_state,
+		     sizeof(*force_cmd));
+	mlo_dev_lock_release(mlo_dev_ctx);
+}
+
+void
+ml_nlink_clr_force_state(struct wlan_objmgr_psoc *psoc,
+			 struct wlan_objmgr_vdev *vdev)
+{
+	struct wlan_mlo_dev_context *mlo_dev_ctx;
+	struct ml_link_force_state *force_state;
+
+	mlo_dev_ctx = wlan_vdev_get_mlo_dev_ctx(vdev);
+	if (!mlo_dev_ctx || !mlo_dev_ctx->sta_ctx) {
+		mlo_err("mlo_ctx or sta_ctx null");
+		return;
+	}
+
+	mlo_dev_lock_acquire(mlo_dev_ctx);
+	force_state = &mlo_dev_ctx->sta_ctx->link_force_ctx.force_state;
+	qdf_mem_zero(force_state, sizeof(*force_state));
+	ml_nlink_dump_force_state(force_state, "");
+	mlo_dev_lock_release(mlo_dev_ctx);
+}
+
+static void
+ml_nlink_update_link_bitmap(uint16_t *curr_link_bitmap,
+			    uint16_t link_bitmap,
+			    enum set_curr_control ctrl)
+{
+	switch (ctrl) {
+	case LINK_OVERWRITE:
+		*curr_link_bitmap = link_bitmap;
+		break;
+	case LINK_CLR:
+		*curr_link_bitmap &= ~link_bitmap;
+		break;
+	case LINK_ADD:
+		*curr_link_bitmap |= link_bitmap;
+		break;
+	default:
+		mlo_err("unknown update ctrl %d", ctrl);
+		return;
+	}
+}
+
+void
+ml_nlink_set_curr_force_active_state(struct wlan_objmgr_psoc *psoc,
+				     struct wlan_objmgr_vdev *vdev,
+				     uint16_t link_bitmap,
+				     enum set_curr_control ctrl)
+{
+	struct wlan_mlo_dev_context *mlo_dev_ctx;
+	struct ml_link_force_state *force_state;
+
+	mlo_dev_ctx = wlan_vdev_get_mlo_dev_ctx(vdev);
+	if (!mlo_dev_ctx || !mlo_dev_ctx->sta_ctx) {
+		mlo_err("mlo_ctx or sta_ctx null");
+		return;
+	}
+
+	mlo_dev_lock_acquire(mlo_dev_ctx);
+	force_state = &mlo_dev_ctx->sta_ctx->link_force_ctx.force_state;
+	ml_nlink_update_link_bitmap(&force_state->force_active_bitmap,
+				    link_bitmap, ctrl);
+	ml_nlink_dump_force_state(force_state, ":ctrl %d bitmap 0x%x",
+				  ctrl, link_bitmap);
+	mlo_dev_lock_release(mlo_dev_ctx);
+}
+
+void
+ml_nlink_set_curr_force_inactive_state(struct wlan_objmgr_psoc *psoc,
+				       struct wlan_objmgr_vdev *vdev,
+				       uint16_t link_bitmap,
+				       enum set_curr_control ctrl)
+{
+	struct wlan_mlo_dev_context *mlo_dev_ctx;
+	struct ml_link_force_state *force_state;
+
+	mlo_dev_ctx = wlan_vdev_get_mlo_dev_ctx(vdev);
+	if (!mlo_dev_ctx || !mlo_dev_ctx->sta_ctx) {
+		mlo_err("mlo_ctx or sta_ctx null");
+		return;
+	}
+
+	mlo_dev_lock_acquire(mlo_dev_ctx);
+	force_state = &mlo_dev_ctx->sta_ctx->link_force_ctx.force_state;
+	ml_nlink_update_link_bitmap(&force_state->force_inactive_bitmap,
+				    link_bitmap, ctrl);
+	ml_nlink_dump_force_state(force_state, ":ctrl %d bitmap 0x%x", ctrl,
+				  link_bitmap);
+	mlo_dev_lock_release(mlo_dev_ctx);
+}
+
+void
+ml_nlink_set_curr_force_active_num_state(struct wlan_objmgr_psoc *psoc,
+					 struct wlan_objmgr_vdev *vdev,
+					 uint8_t link_num,
+					 uint16_t link_bitmap)
+{
+	struct wlan_mlo_dev_context *mlo_dev_ctx;
+	struct ml_link_force_state *force_state;
+
+	mlo_dev_ctx = wlan_vdev_get_mlo_dev_ctx(vdev);
+	if (!mlo_dev_ctx || !mlo_dev_ctx->sta_ctx) {
+		mlo_err("mlo_ctx or sta_ctx null");
+		return;
+	}
+
+	mlo_dev_lock_acquire(mlo_dev_ctx);
+	force_state = &mlo_dev_ctx->sta_ctx->link_force_ctx.force_state;
+	force_state->force_active_num = link_num;
+	force_state->force_active_num_bitmap = link_bitmap;
+	ml_nlink_dump_force_state(force_state, ":num %d bitmap 0x%x",
+				  link_num, link_bitmap);
+	mlo_dev_lock_release(mlo_dev_ctx);
+}
+
+void
+ml_nlink_set_curr_force_inactive_num_state(struct wlan_objmgr_psoc *psoc,
+					   struct wlan_objmgr_vdev *vdev,
+					   uint8_t link_num,
+					   uint16_t link_bitmap)
+{
+	struct wlan_mlo_dev_context *mlo_dev_ctx;
+	struct ml_link_force_state *force_state;
+
+	mlo_dev_ctx = wlan_vdev_get_mlo_dev_ctx(vdev);
+	if (!mlo_dev_ctx || !mlo_dev_ctx->sta_ctx) {
+		mlo_err("mlo_ctx or sta_ctx null");
+		return;
+	}
+
+	mlo_dev_lock_acquire(mlo_dev_ctx);
+	force_state = &mlo_dev_ctx->sta_ctx->link_force_ctx.force_state;
+	force_state->force_inactive_num = link_num;
+	force_state->force_inactive_num_bitmap = link_bitmap;
+	ml_nlink_dump_force_state(force_state, ":num %d bitmap 0x%x",
+				  link_num, link_bitmap);
+	mlo_dev_lock_release(mlo_dev_ctx);
+}

core/wma/src/wma_mgmt.c

@@ -88,6 +88,7 @@
 #endif
 #include "wlan_cm_roam_api.h"
 #include "wlan_cm_api.h"
+#include "wlan_mlo_link_force.h"
 
 /* Max debug string size for WMM in bytes */
 #define WMA_WMM_DEBUG_STRING_SIZE    512
@@ -1439,6 +1440,7 @@ static void wma_set_mlo_capability(tp_wma_handle wma,
 	uint8_t pdev_id;
 	struct wlan_objmgr_peer *peer;
 	struct wlan_objmgr_psoc *psoc = wma->psoc;
+	uint16_t link_id_bitmap;
 
 	pdev_id = wlan_objmgr_pdev_get_pdev_id(wma->pdev);
 	peer = wlan_objmgr_get_peer(psoc, pdev_id, req->peer_mac,
@@ -1454,12 +1456,18 @@ static void wma_set_mlo_capability(tp_wma_handle wma,
 		req->mlo_params.mlo_assoc_link =
 					wlan_peer_mlme_is_assoc_peer(peer);
 		WLAN_ADDR_COPY(req->mlo_params.mld_mac, peer->mldaddr);
-		if (policy_mgr_ml_link_vdev_need_to_be_disabled(psoc, vdev))
+		if (policy_mgr_ml_link_vdev_need_to_be_disabled(psoc, vdev)) {
 			req->mlo_params.mlo_force_link_inactive = 1;
-		wma_debug("assoc_link %d" QDF_MAC_ADDR_FMT ", force inactive %d",
+			link_id_bitmap = 1 << params->link_id;
+			ml_nlink_set_curr_force_inactive_state(
+					psoc, vdev, link_id_bitmap, LINK_ADD);
+		}
+		wma_debug("assoc_link %d" QDF_MAC_ADDR_FMT ", force inactive %d link id %d",
 			  req->mlo_params.mlo_assoc_link,
 			  QDF_MAC_ADDR_REF(peer->mldaddr),
-			  req->mlo_params.mlo_force_link_inactive);
+			  req->mlo_params.mlo_force_link_inactive,
+			  params->link_id);
+
 		req->mlo_params.emlsr_support = params->emlsr_support;
 		req->mlo_params.ieee_link_id = params->link_id;
 		if (req->mlo_params.emlsr_support) {