qcacmn: Clear mld soc setup if teardown fails

Add support to clear mld soc setup and link down
such that in case of teardown fails counters are updated
properly.

Change-Id: I5dc8c581069ef1d5bd0614568b435c0a9448fdcc
CRs-Fixed: 3127308

umac/mlo_mgr/inc/wlan_mlo_mgr_public_structs.h

@@ -73,12 +73,14 @@ struct vdev_mlme_obj;
  * @MLO_LINK_SETUP_DONE - MLO link SETUP exchange started
  * @MLO_LINK_READY - MLO link SETUP done and READY sent
  * @MLO_LINK_TEARDOWN - MLO teardown done.
+ * @MLO_LINK_UNINITIALIZED - MLO link in blank state
  */
 enum MLO_LINK_STATE {
 	MLO_LINK_SETUP_INIT,
 	MLO_LINK_SETUP_DONE,
 	MLO_LINK_READY,
-	MLO_LINK_TEARDOWN
+	MLO_LINK_TEARDOWN,
+	MLO_LINK_UNINITIALIZED,
 };
 
 /**
@@ -95,7 +97,7 @@ enum MLO_LINK_STATE {
  * @qdf_event_t: event for tearodwn completion
  */
 #define MAX_MLO_LINKS 6
-#define MAX_MLO_CHIPS 3
+#define MAX_MLO_CHIPS 5
 struct mlo_setup_info {
 	uint8_t ml_grp_id;
 	uint8_t tot_socs;
@@ -109,6 +111,16 @@ struct mlo_setup_info {
 	qdf_event_t event;
 };
 
+/**
+ * struct mlo_state_params: MLO state params for pdev iteration
+ * @link_state_fail: Flag to check when pdev not in expected state
+ * @check_state: State on against which pdev is to be expected
+ */
+struct mlo_state_params {
+	bool link_state_fail;
+	enum MLO_LINK_STATE check_state;
+};
+
 #define MAX_MLO_GROUP 1
 #endif
 

umac/mlo_mgr/inc/wlan_mlo_mgr_setup.h

@@ -68,6 +68,16 @@ void mlo_link_setup_complete(struct wlan_objmgr_pdev *pdev);
  */
 void mlo_link_teardown_complete(struct wlan_objmgr_pdev *pdev);
 
+/**
+ * mlo_setup_update_soc_down() - API to check and clear all links and bring
+ *                               back to initial state for the particular soc
+ *
+ * @pdev: Pointer to pdev object
+ *
+ * Return: None.
+ */
+void mlo_setup_update_soc_down(struct wlan_objmgr_psoc *psoc);
+
 /**
  * mlo_link_teardown_link() - API to trigger teardown
  * @psoc: Pointer to psoc object

umac/mlo_mgr/src/wlan_mlo_mgr_setup.c

@@ -36,6 +36,28 @@ void mlo_setup_update_total_socs(uint8_t tot_socs)
 
 qdf_export_symbol(mlo_setup_update_total_socs);
 
+static QDF_STATUS mlo_find_pdev_idx(struct wlan_objmgr_pdev *pdev,
+				    uint8_t *link_idx)
+{
+	struct mlo_mgr_context *mlo_ctx = wlan_objmgr_get_mlo_ctx();
+	uint8_t idx;
+
+	if (!mlo_ctx)
+		return QDF_STATUS_E_FAILURE;
+
+	if (!link_idx)
+		return QDF_STATUS_E_FAILURE;
+
+	for (idx = 0; idx < mlo_ctx->setup_info.tot_links; idx++) {
+		if (mlo_ctx->setup_info.pdev_list[idx] == pdev) {
+			*link_idx = idx;
+			return QDF_STATUS_SUCCESS;
+		}
+	}
+
+	return QDF_STATUS_E_FAILURE;
+}
+
 void mlo_setup_update_num_links(struct wlan_objmgr_psoc *psoc,
 				uint8_t num_links)
 {
@@ -52,26 +74,33 @@ qdf_export_symbol(mlo_setup_update_num_links);
 void mlo_setup_update_soc_ready(struct wlan_objmgr_psoc *psoc)
 {
 	struct mlo_mgr_context *mlo_ctx = wlan_objmgr_get_mlo_ctx();
-	uint8_t chip_idx;
+	uint8_t chip_idx, tot_socs = 0;
 
 	if (!mlo_ctx || !mlo_ctx->setup_info.tot_socs)
 		return;
 
-	chip_idx = mlo_ctx->setup_info.num_soc;
-	qdf_assert_always(chip_idx < MAX_MLO_CHIPS);
+	tot_socs = mlo_ctx->setup_info.tot_socs;
+	chip_idx = wlan_psoc_get_id(psoc);
+
+	if (!(chip_idx < MAX_MLO_CHIPS)) {
+		qdf_err("Invalid chip index, SoC setup failed");
+		return;
+	}
+
 	mlo_ctx->setup_info.soc_list[chip_idx] = psoc;
 	mlo_ctx->setup_info.num_soc++;
+	qdf_debug("soc updated to mld list, id %d num soc %d",
+		  chip_idx, mlo_ctx->setup_info.num_soc);
 
 	if (mlo_ctx->setup_info.num_soc != mlo_ctx->setup_info.tot_socs)
 		return;
 
-	for (chip_idx = 0; chip_idx < mlo_ctx->setup_info.num_soc;
-			chip_idx++) {
+	for (chip_idx = 0; chip_idx < MAX_MLO_CHIPS; chip_idx++) {
 		struct wlan_objmgr_psoc *tmp_soc =
 			mlo_ctx->setup_info.soc_list[chip_idx];
-
-		cdp_soc_mlo_soc_setup(wlan_psoc_get_dp_handle(tmp_soc),
-				      mlo_ctx->dp_handle);
+		if (tmp_soc)
+			cdp_soc_mlo_soc_setup(wlan_psoc_get_dp_handle(tmp_soc),
+					      mlo_ctx->dp_handle);
 	}
 
 	cdp_mlo_setup_complete(wlan_psoc_get_dp_handle(psoc),
@@ -88,12 +117,27 @@ void mlo_setup_link_ready(struct wlan_objmgr_pdev *pdev)
 	if (!mlo_ctx || !mlo_ctx->setup_info.tot_links)
 		return;
 
-	link_idx = mlo_ctx->setup_info.num_links;
-	/* TODO: Get reference to PDEV */
-	qdf_assert_always(link_idx < MAX_MLO_LINKS);
+	if (mlo_find_pdev_idx(pdev, &link_idx) == QDF_STATUS_SUCCESS) {
+		qdf_debug("pdev already part of list link idx %d", link_idx);
+		return;
+	}
+
+	for (link_idx = 0; link_idx < mlo_ctx->setup_info.tot_links; link_idx++)
+		if (!mlo_ctx->setup_info.pdev_list[link_idx])
+			break;
+
+	if (link_idx >= mlo_ctx->setup_info.tot_links) {
+		qdf_err("Exceeding max total mld links");
+		return;
+	}
+
 	mlo_ctx->setup_info.pdev_list[link_idx] = pdev;
 	mlo_ctx->setup_info.state[link_idx] = MLO_LINK_SETUP_INIT;
 	mlo_ctx->setup_info.num_links++;
+	qdf_debug("pdev updated to mld link %d num_links %d",
+		  link_idx, mlo_ctx->setup_info.num_links);
+
+	qdf_assert_always(link_idx < MAX_MLO_LINKS);
 
 	if (mlo_ctx->setup_info.num_links == mlo_ctx->setup_info.tot_links &&
 	    mlo_ctx->setup_info.num_soc == mlo_ctx->setup_info.tot_socs) {
@@ -102,7 +146,7 @@ void mlo_setup_link_ready(struct wlan_objmgr_pdev *pdev)
 
 		psoc = wlan_pdev_get_psoc(pdev);
 		tx_ops = wlan_psoc_get_lmac_if_txops(psoc);
-		/* Trigger MLO setup */
+		qdf_debug("Trigger MLO Setup request");
 		if (tx_ops && tx_ops->mops.target_if_mlo_setup_req) {
 			tx_ops->mops.target_if_mlo_setup_req(
 					mlo_ctx->setup_info.pdev_list,
@@ -152,28 +196,60 @@ void mlo_link_setup_complete(struct wlan_objmgr_pdev *pdev)
 
 qdf_export_symbol(mlo_link_setup_complete);
 
-static QDF_STATUS mlo_find_pdev_idx(struct wlan_objmgr_pdev *pdev,
-				    uint8_t *link_idx)
+static void mlo_setup_link_down(struct wlan_objmgr_psoc *psoc,
+				void *obj, void *args)
 {
+	struct wlan_objmgr_pdev *pdev;
+	uint8_t link_idx;
 	struct mlo_mgr_context *mlo_ctx = wlan_objmgr_get_mlo_ctx();
-	uint8_t idx;
+	pdev = (struct wlan_objmgr_pdev *)obj;
+
+	if (mlo_find_pdev_idx(pdev, &link_idx) != QDF_STATUS_SUCCESS) {
+		qdf_info("Failed to find pdev");
+		return;
+	}
+
+	mlo_ctx->setup_info.pdev_list[link_idx] = NULL;
+	mlo_ctx->setup_info.state[link_idx] = MLO_LINK_UNINITIALIZED;
+	mlo_ctx->setup_info.num_links--;
+	qdf_debug("link down link_idx %d num_links %d",
+		  link_idx, mlo_ctx->setup_info.num_links);
+}
+
+void mlo_setup_update_soc_down(struct wlan_objmgr_psoc *psoc)
+{
+	struct mlo_mgr_context *mlo_ctx = wlan_objmgr_get_mlo_ctx();
+	uint8_t chip_idx;
 
 	if (!mlo_ctx)
-		return QDF_STATUS_E_FAILURE;
+		return;
 
-	if (!link_idx)
-		return QDF_STATUS_E_FAILURE;
+	if (!mlo_ctx->setup_info.num_links) {
+		qdf_debug("Links are already down");
+		return;
+	}
 
-	for (idx = 0; idx < mlo_ctx->setup_info.tot_links; idx++) {
-		if (mlo_ctx->setup_info.pdev_list[idx] == pdev) {
-			*link_idx = idx;
-			return QDF_STATUS_SUCCESS;
-		}
+	wlan_objmgr_iterate_obj_list(psoc, WLAN_PDEV_OP,
+				     mlo_setup_link_down, NULL,
+				     0, WLAN_MLME_NB_ID);
+
+	chip_idx = wlan_psoc_get_id(psoc);
+
+	if (!(chip_idx < MAX_MLO_CHIPS)) {
+		qdf_err("Invalid chip index, SoC setup down failed");
+		return;
 	}
 
-	return QDF_STATUS_E_FAILURE;
+	mlo_ctx->setup_info.soc_list[chip_idx] = NULL;
+	mlo_ctx->setup_info.num_soc--;
+
+	qdf_debug("Soc down, num soc %d num links %d",
+		  mlo_ctx->setup_info.num_soc,
+		  mlo_ctx->setup_info.num_links);
 }
 
+qdf_export_symbol(mlo_setup_update_soc_down);
+
 void mlo_link_teardown_complete(struct wlan_objmgr_pdev *pdev)
 {
 	struct mlo_mgr_context *mlo_ctx = wlan_objmgr_get_mlo_ctx();
@@ -192,10 +268,9 @@ void mlo_link_teardown_complete(struct wlan_objmgr_pdev *pdev)
 		return;
 	}
 
-	qdf_debug("link idx = %d", link_idx);
+	qdf_debug("Teardown link idx = %d", link_idx);
 	mlo_ctx->setup_info.pdev_list[link_idx] = NULL;
 	mlo_ctx->setup_info.state[link_idx] = MLO_LINK_TEARDOWN;
-	mlo_ctx->setup_info.num_links--;
 
 	if (!mlo_ctx->setup_info.num_links) {
 		qdf_info("Teardown complete");
@@ -205,6 +280,61 @@ void mlo_link_teardown_complete(struct wlan_objmgr_pdev *pdev)
 
 qdf_export_symbol(mlo_link_teardown_complete);
 
+static void mlo_check_state(struct wlan_objmgr_psoc *psoc,
+			    void *obj, void *args)
+{
+	struct wlan_objmgr_pdev *pdev;
+	uint8_t link_idx;
+	struct mlo_mgr_context *mlo_ctx = wlan_objmgr_get_mlo_ctx();
+	struct mlo_state_params *params = (struct mlo_state_params *)args;
+
+	pdev = (struct wlan_objmgr_pdev *)obj;
+
+	if (!mlo_ctx)
+		return;
+
+	if (mlo_find_pdev_idx(pdev, &link_idx) != QDF_STATUS_SUCCESS) {
+		qdf_info("Failed to find pdev");
+		return;
+	}
+
+	if (mlo_ctx->setup_info.state[link_idx] != params->check_state)
+		params->link_state_fail = 1;
+}
+
+static void mlo_force_teardown(void)
+{
+	struct mlo_mgr_context *mlo_ctx = wlan_objmgr_get_mlo_ctx();
+	uint8_t link_idx = 0;
+
+	if (!mlo_ctx)
+		return;
+
+	for (link_idx = 0; link_idx < mlo_ctx->setup_info.tot_links; link_idx++)
+		mlo_ctx->setup_info.state[link_idx] = MLO_LINK_TEARDOWN;
+}
+
+static
+QDF_STATUS mlo_check_all_pdev_state(struct wlan_objmgr_psoc *psoc,
+				    enum MLO_LINK_STATE state)
+{
+	QDF_STATUS status = QDF_STATUS_E_INVAL;
+	struct mlo_state_params params = {0};
+
+	params.check_state = state;
+
+	wlan_objmgr_iterate_obj_list(psoc, WLAN_PDEV_OP,
+				     mlo_check_state, &params,
+				     0, WLAN_MLME_NB_ID);
+
+	if (params.link_state_fail)
+		status = QDF_STATUS_E_INVAL;
+	else
+		status = QDF_STATUS_SUCCESS;
+
+	return status;
+}
+
 #define MLO_MGR_TEARDOWN_TIMEOUT 3000
 QDF_STATUS mlo_link_teardown_link(struct wlan_objmgr_psoc *psoc,
 				  uint32_t reason)
@@ -213,7 +343,7 @@ QDF_STATUS mlo_link_teardown_link(struct wlan_objmgr_psoc *psoc,
 	QDF_STATUS status;
 	struct mlo_mgr_context *mlo_ctx = wlan_objmgr_get_mlo_ctx();
 
-	qdf_debug(" %d %d ",
+	qdf_debug("Teardown req with num_soc %d num_link %d",
 		  mlo_ctx->setup_info.num_soc,
 		  mlo_ctx->setup_info.num_links);
 
@@ -223,10 +353,8 @@ QDF_STATUS mlo_link_teardown_link(struct wlan_objmgr_psoc *psoc,
 	if (!mlo_ctx->setup_info.num_soc)
 		return QDF_STATUS_SUCCESS;
 
-	if (!mlo_ctx->setup_info.num_links) {
-		mlo_ctx->setup_info.num_soc--;
+	if (!mlo_check_all_pdev_state(psoc, MLO_LINK_TEARDOWN))
 		return QDF_STATUS_SUCCESS;
-	}
 
 	if (qdf_event_create(&mlo_ctx->setup_info.event) != QDF_STATUS_SUCCESS)
 		return QDF_STATUS_E_FAULT;
@@ -244,13 +372,11 @@ QDF_STATUS mlo_link_teardown_link(struct wlan_objmgr_psoc *psoc,
 					       MLO_MGR_TEARDOWN_TIMEOUT);
 	if (status != QDF_STATUS_SUCCESS) {
 		qdf_info("Teardown timeout");
-		mlo_ctx->setup_info.num_links = 0;
+		mlo_force_teardown();
 	}
 
 	qdf_event_destroy(&mlo_ctx->setup_info.event);
 
-	mlo_ctx->setup_info.num_soc--;
-
 	return status;
 }