qcacld-3.0: Handle dynamic force inactive num

With force link inactive cmd rsp, FW will always send a current
active/inactive link bitmap. Host will select one link from current
inactive bitmap, and update the policy mgr table. But FW can
still switch the active/inactive between the links in force num bitmap.
When SAP coming up on same MAC and MCC with the dynamic inactive link,
host will send force inactive the link to FW. Then FW will not try
to active the link even in dynamic mode.

Change-Id: Ib4e438ca4c8e6945b3d7db42cd92d0393fcc1fc9
CRs-Fixed: 3564737

components/cmn_services/policy_mgr/inc/wlan_policy_mgr_api.h

@@ -1255,12 +1255,19 @@ void policy_mgr_move_vdev_from_connection_to_disabled_tbl(
  * disabled during connection.
  * @psoc: psoc
  * @vdev: vdev
+ * @peer_assoc: check peer assoc command
+ *
+ * Check the vdev need to be moved to disabled policy mgr table.
+ * If peer_assoc = false, the API will check the forced inactive link bitmap
+ * as well. Vdev will be disabled if vdev's link id is forced inactive(includes
+ * dynamic inactive)
  *
  * Return: true if STA link is need to be disabled else false.
  */
 bool
 policy_mgr_ml_link_vdev_need_to_be_disabled(struct wlan_objmgr_psoc *psoc,
-					    struct wlan_objmgr_vdev *vdev);
+					    struct wlan_objmgr_vdev *vdev,
+					    bool peer_assoc);
 
 /**
  * policy_mgr_is_set_link_in_progress() - Check set link in progress or not
@@ -1328,7 +1335,8 @@ policy_mgr_move_vdev_from_disabled_to_connection_tbl(
 
 static inline bool
 policy_mgr_ml_link_vdev_need_to_be_disabled(struct wlan_objmgr_psoc *psoc,
-					    struct wlan_objmgr_vdev *vdev)
+					    struct wlan_objmgr_vdev *vdev,
+					    bool peer_assoc)
 {
 	return false;
 }

components/cmn_services/policy_mgr/src/wlan_policy_mgr_get_set_utils.c

@@ -4064,9 +4064,37 @@ void policy_mgr_move_vdev_from_connection_to_disabled_tbl(
 	policy_mgr_dump_current_concurrency(psoc);
 }
 
+static bool
+policy_mgr_vdev_disabled_by_link_force(struct wlan_objmgr_psoc *psoc,
+				       struct wlan_objmgr_vdev *vdev,
+				       bool peer_assoc)
+{
+	uint16_t dynamic_inactive = 0, forced_inactive = 0;
+	uint16_t link_id;
+
+	if (ml_is_nlink_service_supported(psoc) &&
+	    !peer_assoc) {
+		ml_nlink_get_dynamic_inactive_links(psoc, vdev,
+						    &dynamic_inactive,
+						    &forced_inactive);
+		link_id = wlan_vdev_get_link_id(vdev);
+		if ((forced_inactive | dynamic_inactive) &
+		    (1 << link_id)) {
+			policy_mgr_debug("vdev %d linkid %d is forced inactived 0x%0x dyn 0x%x",
+					 wlan_vdev_get_id(vdev),
+					 link_id, forced_inactive,
+					 dynamic_inactive);
+			return true;
+		}
+	}
+
+	return false;
+}
+
 bool
 policy_mgr_ml_link_vdev_need_to_be_disabled(struct wlan_objmgr_psoc *psoc,
-					    struct wlan_objmgr_vdev *vdev)
+					    struct wlan_objmgr_vdev *vdev,
+					    bool peer_assoc)
 {
 	union conc_ext_flag conc_ext_flags;
 
@@ -4078,6 +4106,11 @@ policy_mgr_ml_link_vdev_need_to_be_disabled(struct wlan_objmgr_psoc *psoc,
 	    !wlan_vdev_mlme_is_mlo_link_vdev(vdev))
 		return false;
 
+	/* Check vdev is disabled by link force command */
+	if (policy_mgr_vdev_disabled_by_link_force(psoc, vdev,
+						   peer_assoc))
+		return true;
+
 	conc_ext_flags.value = policy_mgr_get_conc_ext_flags(vdev, false);
 	/*
 	 * For non-assoc link vdev set link as disabled if concurrency is
@@ -4288,6 +4321,73 @@ policy_mgr_trigger_roam_on_link_removal(struct wlan_objmgr_vdev *vdev)
 		policy_mgr_err("roam invoke failed");
 }
 
+static void
+policy_mgr_update_dynamic_inactive_bitmap(
+			struct wlan_objmgr_psoc *psoc,
+			struct wlan_objmgr_vdev *vdev,
+			struct mlo_link_set_active_req *req,
+			struct mlo_link_set_active_resp *resp)
+{
+	uint32_t candidate_inactive_links;
+	uint32_t dyn_inactive_links = 0;
+	uint8_t dyn_num = 0, num = 0, i;
+	uint8_t link_ids[MAX_MLO_LINK_ID * 2];
+
+	if (req->param.force_mode != MLO_LINK_FORCE_MODE_INACTIVE_NUM ||
+	    !req->param.control_flags.dynamic_force_link_num)
+		return;
+
+	/* force inactive num "clear" case, return 0 - no
+	 * dynamic inactive links.
+	 */
+	if (!req->param.force_cmd.link_num) {
+		dyn_inactive_links = 0;
+		dyn_num = 0;
+		goto update;
+	}
+	/* 1. If force inactive overlap with force num bitmap,
+	 * select the inactive link from overlapped links firstly.
+	 * 2. If selected inactive link num <
+	 * req->param.force_cmd.link_num, then select the inactive
+	 * links from current inactive links reported from FW.
+	 */
+	candidate_inactive_links =
+		req->param.force_cmd.ieee_link_id_bitmap &
+		resp->inactive_linkid_bitmap;
+
+	num = ml_nlink_convert_link_bitmap_to_ids(candidate_inactive_links,
+						  QDF_ARRAY_SIZE(link_ids),
+						  link_ids);
+	if (num < req->param.force_cmd.link_num &&
+	    num < QDF_ARRAY_SIZE(link_ids)) {
+		candidate_inactive_links =
+			req->param.force_cmd.ieee_link_id_bitmap &
+			resp->curr_inactive_linkid_bitmap &
+			~candidate_inactive_links;
+		num += ml_nlink_convert_link_bitmap_to_ids(
+				candidate_inactive_links,
+				QDF_ARRAY_SIZE(link_ids) - num,
+				&link_ids[num]);
+	}
+	for (i = 0; i < num; i++) {
+		if (dyn_num >= req->param.force_cmd.link_num)
+			break;
+		dyn_inactive_links |= 1 << link_ids[i];
+		dyn_num++;
+	}
+
+update:
+	policy_mgr_debug("inactive link num %d bitmap 0x%x force inactive 0x%x dyn links 0x%x num %d",
+			 req->param.force_cmd.link_num,
+			 req->param.force_cmd.ieee_link_id_bitmap,
+			 resp->inactive_linkid_bitmap,
+			 dyn_inactive_links, dyn_num);
+	if (dyn_num < req->param.force_cmd.link_num)
+		policy_mgr_debug("unexpected selected dynamic inactive link num %d",
+				 dyn_num);
+	ml_nlink_set_dynamic_inactive_links(psoc, vdev, dyn_inactive_links);
+}
+
 static void
 policy_mgr_handle_vdev_active_inactive_resp(
 					struct wlan_objmgr_psoc *psoc,
@@ -4299,6 +4399,8 @@ policy_mgr_handle_vdev_active_inactive_resp(
 	uint8_t vdev_id_num = 0;
 	uint8_t vdev_ids[WLAN_MLO_MAX_VDEVS] = {0};
 	uint32_t assoc_bitmap = 0;
+	uint16_t dynamic_inactive_bitmap = 0;
+	uint16_t forced_inactive_bitmap = 0;
 
 	/* convert link id to vdev id and update vdev status based
 	 * on both inactive and active bitmap.
@@ -4308,11 +4410,16 @@ policy_mgr_handle_vdev_active_inactive_resp(
 	 * they will be moved to policy mgr disable connection table.
 	 * for other links, they will be in active tables.
 	 */
+	ml_nlink_get_dynamic_inactive_links(psoc, vdev,
+					    &dynamic_inactive_bitmap,
+					    &forced_inactive_bitmap);
+	resp->inactive_linkid_bitmap |= dynamic_inactive_bitmap;
 	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,
@@ -4469,6 +4576,8 @@ policy_mgr_handle_force_inactive_num_resp(
 		ml_nlink_set_curr_force_inactive_num_state(
 			psoc, vdev, req->param.force_cmd.link_num,
 			req->param.force_cmd.ieee_link_id_bitmap);
+		policy_mgr_update_dynamic_inactive_bitmap(psoc, vdev, req,
+							  resp);
 
 		/* update vdev active inactive status */
 		policy_mgr_handle_vdev_active_inactive_resp(psoc, vdev, req,

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

@@ -1693,7 +1693,8 @@ cm_connect_complete_ind(struct wlan_objmgr_vdev *vdev,
 			rsp->freq);
 
 	if (QDF_IS_STATUS_SUCCESS(rsp->connect_status)) {
-		if (policy_mgr_ml_link_vdev_need_to_be_disabled(psoc, vdev))
+		if (policy_mgr_ml_link_vdev_need_to_be_disabled(psoc, vdev,
+								false))
 			policy_mgr_move_vdev_from_connection_to_disabled_tbl(
 								psoc, vdev_id);
 		else

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

@@ -53,6 +53,7 @@ enum ml_nlink_change_event_type {
 
 /**
  * struct ml_nlink_change_event - connection change event data struct
+ * @evt: event parameters
  */
 struct ml_nlink_change_event {
 };
@@ -73,13 +74,14 @@ static inline const char *force_mode_to_string(uint32_t mode)
 };
 
 #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, \
+	mlo_debug("inactive 0x%x active 0x%x inact num %d 0x%x act num %d 0x%x dyn 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, \
+			 (_force_state)->curr_dynamic_inactive_bitmap, \
 			 ##args);
 
 static inline const char *link_evt_to_string(uint32_t evt)
@@ -165,6 +167,20 @@ ml_nlink_convert_vdev_bitmap_to_linkid_bitmap(
 				uint32_t *link_bitmap,
 				uint32_t *associated_bitmap);
 
+/**
+ * ml_nlink_convert_link_bitmap_to_ids() - convert link bitmap
+ * to link ids
+ * @link_bitmap: link bitmap
+ * @link_id_sz: array size of link_ids
+ * @link_ids: link id array
+ *
+ * Return: number of link ids
+ */
+uint32_t
+ml_nlink_convert_link_bitmap_to_ids(uint32_t link_bitmap,
+				    uint8_t link_id_sz,
+				    uint8_t *link_ids);
+
 /**
  * enum set_curr_control - control flag to update current force bitmap
  * @LINK_OVERWRITE: use bitmap to overwrite existing force bitmap
@@ -246,6 +262,36 @@ ml_nlink_set_curr_force_inactive_num_state(struct wlan_objmgr_psoc *psoc,
 					   uint8_t link_num,
 					   uint16_t link_bitmap);
 
+/**
+ * ml_nlink_set_dynamic_inactive_links() - set link dynamic inactive
+ * link bitmap
+ * @psoc: psoc object
+ * @vdev: vdev object
+ * @dynamic_link_bitmap: dynamic inactive bitmap
+ *
+ * Return: None
+ */
+void
+ml_nlink_set_dynamic_inactive_links(struct wlan_objmgr_psoc *psoc,
+				    struct wlan_objmgr_vdev *vdev,
+				    uint16_t dynamic_link_bitmap);
+
+/**
+ * ml_nlink_get_dynamic_inactive_links() - get link dynamic inactive
+ * link bitmap
+ * @psoc: psoc object
+ * @vdev: vdev object
+ * @dynamic_link_bitmap: dynamic inactive bitmap
+ * @force_link_bitmap: forced inactive bitmap
+ *
+ * Return: None
+ */
+void
+ml_nlink_get_dynamic_inactive_links(struct wlan_objmgr_psoc *psoc,
+				    struct wlan_objmgr_vdev *vdev,
+				    uint16_t *dynamic_link_bitmap,
+				    uint16_t *force_link_bitmap);
+
 /**
  * ml_nlink_get_curr_force_state() - get link force state
  * @psoc: psoc object
@@ -269,6 +315,14 @@ ml_nlink_get_curr_force_state(struct wlan_objmgr_psoc *psoc,
 void
 ml_nlink_clr_force_state(struct wlan_objmgr_psoc *psoc,
 			 struct wlan_objmgr_vdev *vdev);
+
+/**
+ * ml_is_nlink_service_supported() - support nlink or not
+ * @psoc: psoc object
+ *
+ * Return: true if supported
+ */
+bool ml_is_nlink_service_supported(struct wlan_objmgr_psoc *psoc);
 #else
 static inline QDF_STATUS
 ml_nlink_conn_change_notify(struct wlan_objmgr_psoc *psoc,
@@ -278,5 +332,11 @@ ml_nlink_conn_change_notify(struct wlan_objmgr_psoc *psoc,
 {
 	return QDF_STATUS_SUCCESS;
 }
+
+static inline bool
+ml_is_nlink_service_supported(struct wlan_objmgr_psoc *psoc)
+{
+	return false;
+}
 #endif
 #endif

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

@@ -24,6 +24,7 @@
 #include "wlan_cm_roam_api.h"
 #include "wlan_mlo_mgr_roam.h"
 #include "wlan_mlme_main.h"
+#include "wlan_mlo_mgr_link_switch.h"
 
 void
 ml_nlink_convert_linkid_bitmap_to_vdev_bitmap(
@@ -333,6 +334,50 @@ ml_nlink_set_curr_force_inactive_num_state(struct wlan_objmgr_psoc *psoc,
 	mlo_dev_lock_release(mlo_dev_ctx);
 }
 
+void
+ml_nlink_set_dynamic_inactive_links(struct wlan_objmgr_psoc *psoc,
+				    struct wlan_objmgr_vdev *vdev,
+				    uint16_t dynamic_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->curr_dynamic_inactive_bitmap = dynamic_link_bitmap;
+	ml_nlink_dump_force_state(force_state, ":dynamic bitmap 0x%x",
+				  dynamic_link_bitmap);
+	mlo_dev_lock_release(mlo_dev_ctx);
+}
+
+void
+ml_nlink_get_dynamic_inactive_links(struct wlan_objmgr_psoc *psoc,
+				    struct wlan_objmgr_vdev *vdev,
+				    uint16_t *dynamic_link_bitmap,
+				    uint16_t *force_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;
+	*dynamic_link_bitmap = force_state->curr_dynamic_inactive_bitmap;
+	*force_link_bitmap = force_state->force_inactive_bitmap;
+	mlo_dev_lock_release(mlo_dev_ctx);
+}
+
 /**
  * ml_nlink_get_affect_ml_sta() - Get ML STA whose link can be
  * force inactive
@@ -376,7 +421,7 @@ ml_nlink_get_affect_ml_sta(struct wlan_objmgr_psoc *psoc)
 	return vdev;
 }
 
-static bool ml_is_nlink_service_supported(void)
+bool ml_is_nlink_service_supported(struct wlan_objmgr_psoc *psoc)
 {
 	/*todo: check WMI_SERVICE_N_LINK_MLO_SUPPORT service bit */
 	return false;
@@ -389,6 +434,79 @@ static bool ml_is_nlink_service_supported(void)
 /* Exclude QUITE link */
 #define NLINK_EXCLUDE_QUIET_LINK	0x04
 
+static void
+ml_nlink_get_standby_link_info(struct wlan_objmgr_psoc *psoc,
+			       struct wlan_objmgr_vdev *vdev,
+			       uint8_t flag,
+			       uint8_t ml_num_link_sz,
+			       struct ml_link_info *ml_link_info,
+			       qdf_freq_t *ml_freq_lst,
+			       uint8_t *ml_vdev_lst,
+			       uint8_t *ml_linkid_lst,
+			       uint8_t *ml_num_link,
+			       uint32_t *ml_link_bitmap)
+{
+	struct mlo_link_info *link_info;
+	uint8_t link_info_iter;
+
+	link_info = mlo_mgr_get_ap_link(vdev);
+	if (!link_info)
+		return;
+
+	for (link_info_iter = 0; link_info_iter < WLAN_MAX_ML_BSS_LINKS;
+	     link_info_iter++) {
+		if (qdf_is_macaddr_zero(&link_info->ap_link_addr))
+			break;
+
+		if (link_info->vdev_id == WLAN_INVALID_VDEV_ID) {
+			if (*ml_num_link >= ml_num_link_sz) {
+				mlo_debug("link lst overflow");
+				break;
+			}
+			if (!link_info->link_chan_info->ch_freq) {
+				mlo_debug("link freq 0!");
+				break;
+			}
+			if (*ml_link_bitmap & (1 << link_info->link_id)) {
+				mlo_debug("unexpected standby linkid %d",
+					  link_info->link_id);
+				break;
+			}
+			if (link_info->link_id >= MAX_MLO_LINK_ID) {
+				mlo_debug("invalid standby link id %d",
+					  link_info->link_id);
+				break;
+			}
+
+			if ((flag & NLINK_EXCLUDE_REMOVED_LINK) &&
+			    link_info->link_status_flags) {
+				mlo_debug("standby link %d is removed",
+					  link_info->link_id);
+				continue;
+			}
+			if ((flag & NLINK_INCLUDE_REMOVED_LINK_ONLY) &&
+			    !link_info->link_status_flags) {
+				continue;
+			}
+
+			ml_freq_lst[*ml_num_link] =
+				link_info->link_chan_info->ch_freq;
+			ml_vdev_lst[*ml_num_link] = WLAN_INVALID_VDEV_ID;
+			ml_linkid_lst[*ml_num_link] = link_info->link_id;
+			*ml_link_bitmap |= 1 << link_info->link_id;
+
+			mlo_debug("vdev %d link %d freq %d bitmap 0x%x flag 0x%x",
+				  ml_vdev_lst[*ml_num_link],
+				  ml_linkid_lst[*ml_num_link],
+				  ml_freq_lst[*ml_num_link],
+				  *ml_link_bitmap, flag);
+			(*ml_num_link)++;
+		}
+
+		link_info++;
+	}
+}
+
 /**
  * ml_nlink_get_link_info() - Get ML STA link info
  * @psoc: PSOC object information
@@ -422,6 +540,7 @@ static void ml_nlink_get_link_info(struct wlan_objmgr_psoc *psoc,
 	uint32_t link_bitmap = 0;
 	uint16_t link_id;
 	uint8_t vdev_id;
+	bool connected = false;
 
 	*ml_num_link = 0;
 	*ml_link_bitmap = 0;
@@ -441,7 +560,6 @@ static void ml_nlink_get_link_info(struct wlan_objmgr_psoc *psoc,
 
 	link_bitmap = 0;
 	for (i = 0; i < WLAN_UMAC_MLO_MAX_VDEVS; i++) {
-		/*todo: add standby link */
 		if (!mlo_dev_ctx->wlan_vdev_list[i])
 			continue;
 		if (!qdf_test_bit(i, sta_ctx->wlan_connected_links))
@@ -454,6 +572,7 @@ static void ml_nlink_get_link_info(struct wlan_objmgr_psoc *psoc,
 					mlo_dev_ctx->wlan_vdev_list[i]));
 			continue;
 		}
+		connected = true;
 
 		vdev_id = wlan_vdev_get_id(mlo_dev_ctx->wlan_vdev_list[i]);
 		link_id = wlan_vdev_get_link_id(
@@ -496,6 +615,17 @@ static void ml_nlink_get_link_info(struct wlan_objmgr_psoc *psoc,
 			  ml_freq_lst[num_link], link_bitmap, flag);
 		num_link++;
 	}
+	/* Add standby link only if mlo sta is connected */
+	if (connected)
+		ml_nlink_get_standby_link_info(psoc, vdev, flag,
+					       ml_num_link_sz,
+					       ml_link_info,
+					       ml_freq_lst,
+					       ml_vdev_lst,
+					       ml_linkid_lst,
+					       &num_link,
+					       &link_bitmap);
+
 	mlo_dev_lock_release(mlo_dev_ctx);
 	*ml_num_link = num_link;
 	*ml_link_bitmap = link_bitmap;
@@ -543,6 +673,15 @@ convert_link_bitmap_to_link_ids(uint32_t link_bitmap,
 	return i;
 }
 
+uint32_t
+ml_nlink_convert_link_bitmap_to_ids(uint32_t link_bitmap,
+				    uint8_t link_id_sz,
+				    uint8_t *link_ids)
+{
+	return convert_link_bitmap_to_link_ids(link_bitmap, link_id_sz,
+					       link_ids);
+}
+
 /**
  * ml_nlink_handle_mcc_links() - Check force inactive needed
  * if ML STA links are in MCC channels
@@ -984,7 +1123,7 @@ ml_nlink_handle_legacy_intf_3_ports(struct wlan_objmgr_psoc *psoc,
 		return;
 
 	for (i = 0; i < ml_num_link; i++) {
-		if (ml_vdev_lst[i] == 0xff) {
+		if (ml_vdev_lst[i] == WLAN_INVALID_VDEV_ID) {
 			/*standby link will be handled later. */
 			continue;
 		}
@@ -1007,7 +1146,7 @@ ml_nlink_handle_legacy_intf_3_ports(struct wlan_objmgr_psoc *psoc,
 	}
 	/* handle standby link */
 	for (i = 0; i < ml_num_link; i++) {
-		if (ml_vdev_lst[i] == 0xff) {
+		if (ml_vdev_lst[i] == WLAN_INVALID_VDEV_ID) {
 			/*standby link will be forced inactive if mcc with
 			 * legacy sta
 			 */
@@ -1136,6 +1275,98 @@ ml_nlink_handle_legacy_intf(struct wlan_objmgr_psoc *psoc,
 	ml_nlink_dump_force_state(force_cmd, "");
 }
 
+/**
+ * ml_nlink_handle_dynamic_inactive() - Handle dynamic force inactive num
+ * with legacy SAP
+ * @psoc: PSOC object information
+ * @vdev: vdev object
+ * @curr: current force command state
+ * @new: new force command
+ *
+ * If ML STA 2 or 3 links are present and force inactive num = 1 with dynamic
+ * flag enabled for some reason, FW will report the current inactive links,
+ * host will select one and save to curr_dynamic_inactive_bitmap.
+ * If SAP starting on channel which is same mac as links in
+ * the curr_dynamic_inactive_bitmap, host will force inactive the links in
+ * curr_dynamic_inactive_bitmap to avoid FW link switch between the dynamic
+ * inactive links.
+ *
+ * Return: void
+ */
+static void
+ml_nlink_handle_dynamic_inactive(struct wlan_objmgr_psoc *psoc,
+				 struct wlan_objmgr_vdev *vdev,
+				 struct ml_link_force_state *curr,
+				 struct ml_link_force_state *new)
+{
+	uint8_t vdev_lst[MAX_NUMBER_OF_CONC_CONNECTIONS];
+	qdf_freq_t freq_lst[MAX_NUMBER_OF_CONC_CONNECTIONS];
+	enum policy_mgr_con_mode mode_lst[MAX_NUMBER_OF_CONC_CONNECTIONS];
+	uint8_t num;
+	uint8_t ml_num_link = 0;
+	uint32_t ml_link_bitmap;
+	uint32_t force_inactive_link_bitmap = 0;
+	uint8_t ml_vdev_lst[MAX_NUMBER_OF_CONC_CONNECTIONS];
+	qdf_freq_t ml_freq_lst[MAX_NUMBER_OF_CONC_CONNECTIONS];
+	uint8_t ml_linkid_lst[MAX_NUMBER_OF_CONC_CONNECTIONS];
+	struct ml_link_info ml_link_info[MAX_NUMBER_OF_CONC_CONNECTIONS];
+	uint32_t i, j;
+
+	/* If force inactive num wasn't sent to fw, no need to handle
+	 * dynamic inactive links.
+	 */
+	if (!curr->force_inactive_num ||
+	    !curr->force_inactive_num_bitmap ||
+	    !curr->curr_dynamic_inactive_bitmap)
+		return;
+	if (curr->force_inactive_num != new->force_inactive_num ||
+	    curr->force_inactive_num_bitmap !=
+				new->force_inactive_num_bitmap)
+		return;
+	/* If links have been forced inactive by bitmap, no need to force
+	 * again.
+	 */
+	if ((new->force_inactive_bitmap &
+	     curr->curr_dynamic_inactive_bitmap) ==
+	    curr->curr_dynamic_inactive_bitmap)
+		return;
+
+	num = policy_mgr_get_legacy_conn_info(
+					psoc, vdev_lst,
+					freq_lst, mode_lst,
+					QDF_ARRAY_SIZE(vdev_lst));
+	if (!num)
+		return;
+	ml_nlink_get_link_info(psoc, vdev, NLINK_EXCLUDE_REMOVED_LINK,
+			       QDF_ARRAY_SIZE(ml_linkid_lst),
+			       ml_link_info, ml_freq_lst, ml_vdev_lst,
+			       ml_linkid_lst, &ml_num_link,
+			       &ml_link_bitmap);
+	if (ml_num_link < 2)
+		return;
+	for (i = 0; i < ml_num_link; i++) {
+		if (!((1 << ml_linkid_lst[i]) &
+		      curr->curr_dynamic_inactive_bitmap))
+			continue;
+		for (j = 0; j < num; j++) {
+			if (mode_lst[j] != PM_SAP_MODE)
+				continue;
+			if (policy_mgr_2_freq_always_on_same_mac(
+				psoc, freq_lst[j], ml_freq_lst[i])) {
+				force_inactive_link_bitmap |=
+					1 << ml_linkid_lst[i];
+				mlo_debug("force dynamic inactive link id %d freq %d for sap freq %d",
+					  ml_linkid_lst[i], ml_freq_lst[i],
+					  freq_lst[j]);
+			}
+		}
+	}
+	if (force_inactive_link_bitmap) {
+		new->force_inactive_bitmap |= force_inactive_link_bitmap;
+		ml_nlink_dump_force_state(new, "");
+	}
+}
+
 /**
  * ml_nlink_sta_inactivity_allowed_with_quiet() - Check force inactive allowed
  * for links in bitmap
@@ -1355,7 +1586,7 @@ ml_nlink_update_force_inactive_num(struct wlan_objmgr_psoc *psoc,
 					new->force_inactive_num,
 					new->force_inactive_num_bitmap,
 					0,
-					0);
+					link_ctrl_f_dynamic_force_link_num);
 	}
 
 	return status;
@@ -1386,6 +1617,8 @@ ml_nlink_update_force_active_num(struct wlan_objmgr_psoc *psoc,
  * with concurrency internal function
  * @psoc: PSOC object information
  * @reason: reason code of trigger force mode change.
+ * @evt: event type
+ * @data: event data
  *
  * This API handle link force for connected ML STA.
  * At present we only support one ML STA. so ml_nlink_get_affect_ml_sta
@@ -1398,11 +1631,13 @@ ml_nlink_update_force_active_num(struct wlan_objmgr_psoc *psoc,
  * record after get successful respone from target.
  *
  * Return: QDF_STATUS_SUCCESS if no new command updated to target.
- *         QDF_STATUS_E_PENDING if new command is sent to target.
- *         otherwise QDF_STATUS error code
+ *	   QDF_STATUS_E_PENDING if new command is sent to target.
+ *	   otherwise QDF_STATUS error code
  */
 static QDF_STATUS ml_nlink_state_change(struct wlan_objmgr_psoc *psoc,
-					enum mlo_link_force_reason reason)
+					enum mlo_link_force_reason reason,
+					enum ml_nlink_change_event_type evt,
+					struct ml_nlink_change_event *data)
 {
 	struct ml_link_force_state force_state = {0};
 	struct ml_link_force_state legacy_intf_force_state = {0};
@@ -1446,6 +1681,9 @@ static QDF_STATUS ml_nlink_state_change(struct wlan_objmgr_psoc *psoc,
 		legacy_intf_force_state.force_inactive_num_bitmap;
 	}
 
+	ml_nlink_handle_dynamic_inactive(psoc, vdev, &curr_force_state,
+					 &force_state);
+
 	status = ml_nlink_update_no_force_for_all(psoc, vdev,
 						  &curr_force_state,
 						  &force_state,
@@ -1501,16 +1739,21 @@ end:
  * @psoc: PSOC object information
  * @vdev: ml sta vdev object
  * @reason: reason code of trigger force mode change.
+ * @evt: event type
+ * @data: event data
  *
- * Return: void
+ * Return: QDF_STATUS_SUCCESS if successfully
  */
-static void
+static QDF_STATUS
 ml_nlink_state_change_handler(struct wlan_objmgr_psoc *psoc,
 			      struct wlan_objmgr_vdev *vdev,
-			      enum mlo_link_force_reason reason)
+			      enum mlo_link_force_reason reason,
+			      enum ml_nlink_change_event_type evt,
+			      struct ml_nlink_change_event *data)
 {
 	enum QDF_OPMODE mode = wlan_vdev_mlme_get_opmode(vdev);
 	uint8_t vdev_id = wlan_vdev_get_id(vdev);
+	QDF_STATUS status = QDF_STATUS_SUCCESS;
 
 	/* If WMI_SERVICE_N_LINK_MLO_SUPPORT = 381 is enabled,
 	 * indicate FW support N MLO link & vdev re-purpose between links,
@@ -1519,14 +1762,16 @@ ml_nlink_state_change_handler(struct wlan_objmgr_psoc *psoc,
 	 * Otherwise, use legacy policy mgr API to inactive/active based
 	 * on vdev id bitmap.
 	 */
-	if (ml_is_nlink_service_supported())
-		ml_nlink_state_change(psoc, reason);
+	if (ml_is_nlink_service_supported(psoc))
+		status = ml_nlink_state_change(psoc, reason, evt, data);
 	else if (reason == MLO_LINK_FORCE_REASON_CONNECT)
 		policy_mgr_handle_ml_sta_links_on_vdev_up_csa(psoc, mode,
 							      vdev_id);
 	else
 		policy_mgr_handle_ml_sta_links_on_vdev_down(psoc, mode,
 							    vdev_id);
+
+	return status;
 }
 
 QDF_STATUS
@@ -1537,8 +1782,10 @@ ml_nlink_conn_change_notify(struct wlan_objmgr_psoc *psoc,
 {
 	struct wlan_objmgr_vdev *vdev;
 	enum QDF_OPMODE mode;
+	QDF_STATUS status = QDF_STATUS_SUCCESS;
 
-	mlo_debug("vdev %d %s", vdev_id, link_evt_to_string(evt));
+	mlo_debug("vdev %d %s(%d)", vdev_id, link_evt_to_string(evt),
+		  evt);
 	vdev = wlan_objmgr_get_vdev_by_id_from_psoc(psoc, vdev_id,
 						    WLAN_MLO_MGR_ID);
 	if (!vdev) {
@@ -1552,37 +1799,43 @@ ml_nlink_conn_change_notify(struct wlan_objmgr_psoc *psoc,
 		/* todo: allow concurrenct check */
 		break;
 	case ml_nlink_link_switch_completion_evt:
-		ml_nlink_state_change_handler(
-			psoc, vdev, MLO_LINK_FORCE_REASON_CONNECT);
+		status = ml_nlink_state_change_handler(
+			psoc, vdev, MLO_LINK_FORCE_REASON_CONNECT,
+			evt, data);
 		break;
 	case ml_nlink_roam_sync_start_evt:
 		ml_nlink_clr_force_state(psoc, vdev);
 		break;
 	case ml_nlink_roam_sync_completion_evt:
-		ml_nlink_state_change_handler(
-			psoc, vdev, MLO_LINK_FORCE_REASON_CONNECT);
+		status = ml_nlink_state_change_handler(
+			psoc, vdev, MLO_LINK_FORCE_REASON_CONNECT,
+			evt, data);
 		break;
 	case ml_nlink_connect_start_evt:
 		ml_nlink_clr_force_state(psoc, vdev);
 		break;
 	case ml_nlink_connect_completion_evt:
-		ml_nlink_state_change_handler(
-			psoc, vdev, MLO_LINK_FORCE_REASON_CONNECT);
+		status = ml_nlink_state_change_handler(
+			psoc, vdev, MLO_LINK_FORCE_REASON_CONNECT,
+			evt, data);
 		break;
 	case ml_nlink_disconnect_start_evt:
 		ml_nlink_clr_force_state(psoc, vdev);
 		break;
 	case ml_nlink_disconnect_completion_evt:
-		ml_nlink_state_change_handler(
-			psoc, vdev, MLO_LINK_FORCE_REASON_DISCONNECT);
+		status = ml_nlink_state_change_handler(
+			psoc, vdev, MLO_LINK_FORCE_REASON_DISCONNECT,
+			evt, data);
 		break;
 	case ml_nlink_ap_started_evt:
-		ml_nlink_state_change_handler(
-			psoc, vdev, MLO_LINK_FORCE_REASON_CONNECT);
+		status = ml_nlink_state_change_handler(
+			psoc, vdev, MLO_LINK_FORCE_REASON_CONNECT,
+			evt, data);
 		break;
 	case ml_nlink_ap_stopped_evt:
-		ml_nlink_state_change_handler(
-			psoc, vdev, MLO_LINK_FORCE_REASON_DISCONNECT);
+		status = ml_nlink_state_change_handler(
+			psoc, vdev, MLO_LINK_FORCE_REASON_DISCONNECT,
+			evt, data);
 		break;
 	case ml_nlink_connection_updated_evt:
 		if (mode == QDF_STA_MODE &&
@@ -1591,8 +1844,9 @@ ml_nlink_conn_change_notify(struct wlan_objmgr_psoc *psoc,
 			mlo_debug("vdev id %d in roam sync", vdev_id);
 			break;
 		}
-		ml_nlink_state_change_handler(
-			psoc, vdev, MLO_LINK_FORCE_REASON_CONNECT);
+		status = ml_nlink_state_change_handler(
+			psoc, vdev, MLO_LINK_FORCE_REASON_CONNECT,
+			evt, data);
 		break;
 	default:
 		break;
@@ -1601,5 +1855,5 @@ ml_nlink_conn_change_notify(struct wlan_objmgr_psoc *psoc,
 	if (vdev)
 		wlan_objmgr_vdev_release_ref(vdev, WLAN_MLO_MGR_ID);
 
-	return QDF_STATUS_SUCCESS;
+	return status;
 }

core/wma/src/wma_mgmt.c

@@ -1347,7 +1347,8 @@ 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,
+								true) ||
 		    policy_mgr_is_emlsr_sta_concurrency_present(psoc)) {
 			req->mlo_params.mlo_force_link_inactive = 1;
 			link_id_bitmap = 1 << params->link_id;