qcacld-3.0: Add link switch notifier for concurrency check

Add link switch notifier callback to check concurrency
allow or disallow for link switch request.
If the target link is force inactive, disallow the link switch
to it.

Change-Id: Iac8b443c85bbf7c4b714b62e8f3d0968a3ad6226
CRs-Fixed: 3579454

components/cmn_services/policy_mgr/src/wlan_policy_mgr_get_set_utils.c

@@ -6513,6 +6513,66 @@ policy_mgr_is_mlo_sap_concurrency_allowed(struct wlan_objmgr_psoc *psoc,
 	return ret;
 }
 
+QDF_STATUS
+policy_mgr_link_switch_notifier_cb(struct wlan_objmgr_vdev *vdev,
+				   struct wlan_mlo_link_switch_req *req)
+{
+	struct wlan_objmgr_psoc *psoc = wlan_vdev_get_psoc(vdev);
+	struct policy_mgr_psoc_priv_obj *pm_ctx;
+	uint8_t vdev_id = req->vdev_id;
+	uint8_t curr_ieee_link_id = req->curr_ieee_link_id;
+	uint8_t new_ieee_link_id = req->new_ieee_link_id;
+	uint32_t new_primary_freq = req->new_primary_freq;
+	QDF_STATUS status;
+	union conc_ext_flag conc_ext_flags;
+	struct policy_mgr_conc_connection_info
+			info[MAX_NUMBER_OF_CONC_CONNECTIONS] = { {0} };
+	uint8_t num_del = 0;
+	struct ml_nlink_change_event data;
+
+	pm_ctx = policy_mgr_get_context(psoc);
+	if (!pm_ctx) {
+		policy_mgr_err("Invalid Context");
+		return QDF_STATUS_E_INVAL;
+	}
+
+	policy_mgr_debug("target link %d freq %d curr link %d vdev %d",
+			 new_ieee_link_id, new_primary_freq,
+			 curr_ieee_link_id, 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;
+	status = ml_nlink_conn_change_notify(psoc, vdev_id,
+					     ml_nlink_link_switch_start_evt,
+					     &data);
+	if (QDF_IS_STATUS_ERROR(status))
+		return status;
+
+	qdf_mutex_acquire(&pm_ctx->qdf_conc_list_lock);
+
+	policy_mgr_store_and_del_conn_info_by_vdev_id(
+		psoc, vdev_id, info, &num_del);
+	conc_ext_flags.value =
+	policy_mgr_get_conc_ext_flags(vdev, true);
+	if (!policy_mgr_is_concurrency_allowed(psoc, PM_STA_MODE,
+					       new_primary_freq,
+					       HW_MODE_20_MHZ,
+					       conc_ext_flags.value,
+					       NULL)) {
+		status = QDF_STATUS_E_INVAL;
+		policy_mgr_debug("target link %d freq %d not allowed by conc rule",
+				 new_ieee_link_id, new_primary_freq);
+	}
+
+	if (num_del > 0)
+		policy_mgr_restore_deleted_conn_info(psoc, info, num_del);
+
+	qdf_mutex_release(&pm_ctx->qdf_conc_list_lock);
+
+	return status;
+}
+
 bool policy_mgr_is_non_ml_sta_present(struct wlan_objmgr_psoc *psoc)
 {
 	uint32_t conn_index = 0;

components/cmn_services/policy_mgr/src/wlan_policy_mgr_i.h

@@ -787,6 +787,20 @@ void policy_mgr_pdev_set_hw_mode_cb(uint32_t status,
 #ifdef WLAN_FEATURE_11BE_MLO
 void
 policy_mgr_dump_disabled_ml_links(struct policy_mgr_psoc_priv_obj *pm_ctx);
+
+/**
+ * policy_mgr_link_switch_notifier_cb() - link switch notifier callback
+ * @vdev: vdev object
+ * @req: link switch request
+ *
+ * This API will be registered to mlo link switch, to be invoked before
+ * do link switch process.
+ *
+ * Return: QDF_STATUS
+ */
+QDF_STATUS
+policy_mgr_link_switch_notifier_cb(struct wlan_objmgr_vdev *vdev,
+				   struct wlan_mlo_link_switch_req *req);
 #else
 static inline void
 policy_mgr_dump_disabled_ml_links(struct policy_mgr_psoc_priv_obj *pm_ctx) {}

components/cmn_services/policy_mgr/src/wlan_policy_mgr_init_deinit.c

@@ -140,6 +140,49 @@ static void policy_mgr_vdev_obj_status_cb(struct wlan_objmgr_vdev *vdev,
 	return;
 }
 
+#ifdef WLAN_FEATURE_11BE_MLO
+static QDF_STATUS policy_mgr_register_link_switch_notifier(void)
+{
+	QDF_STATUS status;
+
+	status = mlo_mgr_register_link_switch_notifier(
+			WLAN_UMAC_COMP_POLICY_MGR,
+			policy_mgr_link_switch_notifier_cb);
+	if (status == QDF_STATUS_E_NOSUPPORT) {
+		status = QDF_STATUS_SUCCESS;
+		policy_mgr_debug("Link switch not supported");
+	} else if (status != QDF_STATUS_SUCCESS) {
+		policy_mgr_err("Failed to register link switch notifier for policy mgr!");
+	}
+
+	return status;
+}
+
+static QDF_STATUS policy_mgr_unregister_link_switch_notifier(void)
+{
+	QDF_STATUS status;
+
+	status = mlo_mgr_unregister_link_switch_notifier(
+			WLAN_UMAC_COMP_POLICY_MGR);
+	if (status == QDF_STATUS_E_NOSUPPORT)
+		status = QDF_STATUS_SUCCESS;
+	else if (status != QDF_STATUS_SUCCESS)
+		policy_mgr_err("Failed to unregister link switch notifier for policy mgr!");
+
+	return status;
+}
+#else
+static QDF_STATUS policy_mgr_register_link_switch_notifier(void)
+{
+	return QDF_STATUS_SUCCESS;
+}
+
+static QDF_STATUS policy_mgr_unregister_link_switch_notifier(void)
+{
+	return QDF_STATUS_SUCCESS;
+}
+#endif
+
 QDF_STATUS policy_mgr_init(void)
 {
 	QDF_STATUS status = QDF_STATUS_SUCCESS;
@@ -215,10 +258,20 @@ QDF_STATUS policy_mgr_init(void)
 		goto err_vdev_status;
 	}
 
+	status = policy_mgr_register_link_switch_notifier();
+	if (status != QDF_STATUS_SUCCESS) {
+		policy_mgr_err("Failed to register link switch cback");
+		goto err_link_switch;
+	}
+
 	policy_mgr_notice("Callbacks registered with obj mgr");
 
 	return QDF_STATUS_SUCCESS;
-
+err_link_switch:
+	wlan_objmgr_unregister_vdev_status_handler(
+				WLAN_UMAC_COMP_POLICY_MGR,
+				policy_mgr_vdev_obj_status_cb,
+				NULL);
 err_vdev_status:
 	wlan_objmgr_unregister_vdev_destroy_handler(WLAN_UMAC_COMP_POLICY_MGR,
 						policy_mgr_vdev_obj_destroy_cb,
@@ -255,6 +308,10 @@ QDF_STATUS policy_mgr_deinit(void)
 {
 	QDF_STATUS status;
 
+	status = policy_mgr_unregister_link_switch_notifier();
+	if (status != QDF_STATUS_SUCCESS)
+		policy_mgr_err("Failed to deregister link switch cback");
+
 	status = wlan_objmgr_unregister_psoc_status_handler(
 				WLAN_UMAC_COMP_POLICY_MGR,
 				policy_mgr_psoc_obj_status_cb,

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

@@ -1697,6 +1697,11 @@ cm_connect_complete_ind(struct wlan_objmgr_vdev *vdev,
 			rsp->freq);
 
 	if (QDF_IS_STATUS_SUCCESS(rsp->connect_status)) {
+		if (rsp->cm_id & CM_ID_LSWITCH_BIT)
+			ml_nlink_conn_change_notify(
+				psoc, vdev_id,
+				ml_nlink_link_switch_pre_completion_evt, NULL);
+
 		if (policy_mgr_ml_link_vdev_need_to_be_disabled(psoc, vdev,
 								false))
 			policy_mgr_move_vdev_from_connection_to_disabled_tbl(

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

@@ -26,7 +26,7 @@
 /**
  * enum ml_nlink_change_event_type - Ml link state change trigger event
  * @ml_nlink_link_switch_start_evt: link switch start
- * @ml_nlink_link_switch_completion_evt: link switch done
+ * @ml_nlink_link_switch_pre_completion_evt: link switch pre-completion
  * @ml_nlink_roam_sync_start_evt: roam sync start
  * @ml_nlink_roam_sync_completion_evt: roam sync completion
  * @ml_nlink_connect_start_evt: STA/CLI connect start
@@ -39,7 +39,7 @@
  */
 enum ml_nlink_change_event_type {
 	ml_nlink_link_switch_start_evt,
-	ml_nlink_link_switch_completion_evt,
+	ml_nlink_link_switch_pre_completion_evt,
 	ml_nlink_roam_sync_start_evt,
 	ml_nlink_roam_sync_completion_evt,
 	ml_nlink_connect_start_evt,
@@ -54,8 +54,16 @@ enum ml_nlink_change_event_type {
 /**
  * struct ml_nlink_change_event - connection change event data struct
  * @evt: event parameters
+ * @link_switch: link switch start parameters
  */
 struct ml_nlink_change_event {
+	union {
+		struct {
+			uint8_t curr_ieee_link_id;
+			uint8_t new_ieee_link_id;
+			uint32_t new_primary_freq;
+		} link_switch;
+	} evt;
 };
 
 #ifdef WLAN_FEATURE_11BE_MLO
@@ -88,7 +96,7 @@ static inline const char *link_evt_to_string(uint32_t evt)
 {
 	switch (evt) {
 	CASE_RETURN_STRING(ml_nlink_link_switch_start_evt);
-	CASE_RETURN_STRING(ml_nlink_link_switch_completion_evt);
+	CASE_RETURN_STRING(ml_nlink_link_switch_pre_completion_evt);
 	CASE_RETURN_STRING(ml_nlink_roam_sync_start_evt);
 	CASE_RETURN_STRING(ml_nlink_roam_sync_completion_evt);
 	CASE_RETURN_STRING(ml_nlink_connect_start_evt);

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

@@ -20,6 +20,7 @@
 #include "wlan_mlo_link_force.h"
 #include "wlan_mlo_mgr_sta.h"
 #include "wlan_policy_mgr_api.h"
+#include "wlan_policy_mgr_i.h"
 #include "wlan_cm_roam_public_struct.h"
 #include "wlan_cm_roam_api.h"
 #include "wlan_mlo_mgr_roam.h"
@@ -515,6 +516,26 @@ ml_nlink_get_standby_link_info(struct wlan_objmgr_psoc *psoc,
 	}
 }
 
+static uint32_t
+ml_nlink_get_standby_link_bitmap(struct wlan_objmgr_psoc *psoc,
+				 struct wlan_objmgr_vdev *vdev)
+{
+	uint8_t ml_num_link = 0;
+	uint32_t standby_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];
+
+	ml_nlink_get_standby_link_info(psoc, vdev, NLINK_DUMP_LINK,
+				       QDF_ARRAY_SIZE(ml_linkid_lst),
+				       ml_link_info, ml_freq_lst, ml_vdev_lst,
+				       ml_linkid_lst, &ml_num_link,
+				       &standby_link_bitmap);
+
+	return standby_link_bitmap;
+}
+
 /**
  * ml_nlink_get_link_info() - Get ML STA link info
  * @psoc: PSOC object information
@@ -1596,15 +1617,16 @@ ml_nlink_allow_conc(struct wlan_objmgr_psoc *psoc,
 		if (bss_chan)
 			freq = bss_chan->ch_freq;
 
-		if (!policy_mgr_allow_concurrency(psoc, PM_STA_MODE,
-						  freq,
-						  HW_MODE_20_MHZ,
-						  conc_ext_flags.value,
-						  vdev_ids[i])) {
+		if (!policy_mgr_is_concurrency_allowed(psoc, PM_STA_MODE,
+						       freq,
+						       HW_MODE_20_MHZ,
+						       conc_ext_flags.value,
+						       NULL)) {
 			wlan_objmgr_vdev_release_ref(ml_vdev,
 						     WLAN_MLO_MGR_ID);
 			break;
 		}
+
 		wlan_objmgr_vdev_release_ref(ml_vdev, WLAN_MLO_MGR_ID);
 	}
 
@@ -1911,6 +1933,65 @@ ml_nlink_state_change_handler(struct wlan_objmgr_psoc *psoc,
 	return status;
 }
 
+static QDF_STATUS
+ml_nlink_swtich_dynamic_inactive_link(struct wlan_objmgr_psoc *psoc,
+				      struct wlan_objmgr_vdev *vdev)
+{
+	uint8_t link_id;
+	uint32_t standby_link_bitmap, dynamic_inactive_bitmap;
+	struct ml_link_force_state curr_force_state = {0};
+	uint8_t link_ids[MAX_MLO_LINK_ID];
+	uint8_t num_ids;
+
+	link_id = wlan_vdev_get_link_id(vdev);
+	if (link_id >= MAX_MLO_LINK_ID) {
+		mlo_err("invalid link id %d", link_id);
+		return QDF_STATUS_E_INVAL;
+	}
+
+	ml_nlink_get_curr_force_state(psoc, vdev, &curr_force_state);
+	standby_link_bitmap = ml_nlink_get_standby_link_bitmap(psoc, vdev);
+	standby_link_bitmap &= curr_force_state.force_inactive_num_bitmap &
+				~(1 << link_id);
+	/* In DBS RD, ML STA 2+5+6(standby link), force inactive num = 1 and
+	 * force inactive bitmap with 5 + 6 links will be sent to FW, host
+	 * will select 6G as dynamic inactive link, 5G vdev will be kept in
+	 * policy mgr active connection table.
+	 * If FW link switch and repurpose 5G vdev to 6G, host will need to
+	 * select 5G standby link as dynamic inactive.
+	 * Then 6G vdev can be moved to policy mgr active connection table.
+	 */
+	if (((1 << link_id) & curr_force_state.curr_dynamic_inactive_bitmap) &&
+	    ((1 << link_id) & curr_force_state.force_inactive_num_bitmap) &&
+	    !(standby_link_bitmap &
+			curr_force_state.curr_dynamic_inactive_bitmap) &&
+	    (standby_link_bitmap &
+			curr_force_state.force_inactive_num_bitmap)) {
+		num_ids = convert_link_bitmap_to_link_ids(
+						standby_link_bitmap,
+						QDF_ARRAY_SIZE(link_ids),
+						link_ids);
+		if (!num_ids) {
+			mlo_err("unexpected 0 link ids for bitmap 0x%x",
+				standby_link_bitmap);
+			return QDF_STATUS_E_INVAL;
+		}
+		/* Remove the link from dynamic inactive bitmap,
+		 * add the standby link to dynamic inactive bitmap.
+		 */
+		dynamic_inactive_bitmap =
+			curr_force_state.curr_dynamic_inactive_bitmap &
+						~(1 << link_id);
+		dynamic_inactive_bitmap |= 1 << link_ids[0];
+		mlo_debug("move out vdev %d link id %d from dynamic inactive, add standby link id %d",
+			  wlan_vdev_get_id(vdev), link_id, link_ids[0]);
+		ml_nlink_set_dynamic_inactive_links(psoc, vdev,
+						    dynamic_inactive_bitmap);
+	}
+
+	return QDF_STATUS_SUCCESS;
+}
+
 QDF_STATUS
 ml_nlink_conn_change_notify(struct wlan_objmgr_psoc *psoc,
 			    uint8_t vdev_id,
@@ -1920,6 +2001,7 @@ 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;
+	struct ml_link_force_state curr_force_state = {0};
 
 	mlo_debug("vdev %d %s(%d)", vdev_id, link_evt_to_string(evt),
 		  evt);
@@ -1933,12 +2015,17 @@ ml_nlink_conn_change_notify(struct wlan_objmgr_psoc *psoc,
 
 	switch (evt) {
 	case ml_nlink_link_switch_start_evt:
-		/* todo: allow concurrenct check */
+		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) {
+			mlo_debug("target link %d is force inactive, don't switch to it",
+				  data->evt.link_switch.new_ieee_link_id);
+			status = QDF_STATUS_E_INVAL;
+		}
 		break;
-	case ml_nlink_link_switch_completion_evt:
-		status = ml_nlink_state_change_handler(
-			psoc, vdev, MLO_LINK_FORCE_REASON_CONNECT,
-			evt, data);
+	case ml_nlink_link_switch_pre_completion_evt:
+		status = ml_nlink_swtich_dynamic_inactive_link(
+				psoc, vdev);
 		break;
 	case ml_nlink_roam_sync_start_evt:
 		ml_nlink_clr_force_state(psoc, vdev);