From 2274490945695c1da37bc5b708212a40073d7891 Mon Sep 17 00:00:00 2001
From: Sandeep Tripathy <sandeep.tripathy@broadcom.com>
Date: Mon, 17 Aug 2020 20:22:13 +0530
Subject: [PATCH] psci: utility api to invoke stop for other cores

The API can be used to invoke a 'stop_func' callback for all
other cores from any initiating core. Optionally it can also
wait for other cores to power down. There may be various use
of such API by platform. Ex: Platform may use this to power
down all other cores from a crashed core.

Signed-off-by: Sandeep Tripathy <sandeep.tripathy@broadcom.com>
Change-Id: I4f9dc8a38d419f299c021535d5f1bcc6883106f9
---
 include/lib/psci/psci_lib.h |  2 ++
 lib/psci/psci_common.c      | 47 +++++++++++++++++++++++++++++++++++++
 2 files changed, 49 insertions(+)

diff --git a/include/lib/psci/psci_lib.h b/include/lib/psci/psci_lib.h
index 76c1a8dcf..1ac45adf7 100644
--- a/include/lib/psci/psci_lib.h
+++ b/include/lib/psci/psci_lib.h
@@ -89,6 +89,8 @@ void psci_warmboot_entrypoint(void);
 void psci_register_spd_pm_hook(const spd_pm_ops_t *pm);
 void psci_prepare_next_non_secure_ctx(
 			  entry_point_info_t *next_image_info);
+int psci_stop_other_cores(unsigned int wait_ms,
+			  void (*stop_func)(u_register_t mpidr));
 #endif /* __ASSEMBLER__ */
 
 #endif /* PSCI_LIB_H */
diff --git a/lib/psci/psci_common.c b/lib/psci/psci_common.c
index cced2761f..6d813774d 100644
--- a/lib/psci/psci_common.c
+++ b/lib/psci/psci_common.c
@@ -12,6 +12,7 @@
 #include <common/bl_common.h>
 #include <common/debug.h>
 #include <context.h>
+#include <drivers/delay_timer.h>
 #include <lib/el3_runtime/context_mgmt.h>
 #include <lib/utils.h>
 #include <plat/common/platform.h>
@@ -973,3 +974,49 @@ void psci_do_pwrdown_sequence(unsigned int power_level)
 	psci_do_pwrdown_cache_maintenance(power_level);
 #endif
 }
+
+/*******************************************************************************
+ * This function invokes the callback 'stop_func()' with the 'mpidr' of each
+ * online PE. Caller can pass suitable method to stop a remote core.
+ *
+ * 'wait_ms' is the timeout value in milliseconds for the other cores to
+ * transition to power down state. Passing '0' makes it non-blocking.
+ *
+ * The function returns 'PSCI_E_DENIED' if some cores failed to stop within the
+ * given timeout.
+ ******************************************************************************/
+int psci_stop_other_cores(unsigned int wait_ms,
+				   void (*stop_func)(u_register_t mpidr))
+{
+	unsigned int idx, this_cpu_idx;
+
+	this_cpu_idx = plat_my_core_pos();
+
+	/* Invoke stop_func for each core */
+	for (idx = 0U; idx < psci_plat_core_count; idx++) {
+		/* skip current CPU */
+		if (idx == this_cpu_idx) {
+			continue;
+		}
+
+		/* Check if the CPU is ON */
+		if (psci_get_aff_info_state_by_idx(idx) == AFF_STATE_ON) {
+			(*stop_func)(psci_cpu_pd_nodes[idx].mpidr);
+		}
+	}
+
+	/* Need to wait for other cores to shutdown */
+	if (wait_ms != 0U) {
+		while ((wait_ms-- != 0U) && (psci_is_last_on_cpu() != 0U)) {
+			mdelay(1U);
+		}
+
+		if (psci_is_last_on_cpu() != 0U) {
+			WARN("Failed to stop all cores!\n");
+			psci_print_power_domain_map();
+			return PSCI_E_DENIED;
+		}
+	}
+
+	return PSCI_E_SUCCESS;
+}