stm32mp1: psci: switch secondary boot cpu offline

Secondary boot core can be switched OFF from a functional view by
being reset. To get the core back on line, it shall be woken through
catching secure SGI0 interrupt then proceeding to same sequence as
when getting the core online from a cold reset.

Signed-off-by: Etienne Carriere <etienne.carriere@st.com>
Acked-by: Jerome Forissier <jerome.forissier@linaro.org>

2 files changed, 46 insertions, 4 deletions

diff --git a/core/arch/arm/plat-stm32mp1/platform_config.h b/core/arch/arm/plat-stm32mp1/platform_config.h
index 432ea4f0..c759a4b2 100644
--- a/core/arch/arm/plat-stm32mp1/platform_config.h
+++ b/core/arch/arm/plat-stm32mp1/platform_config.h
@@ -27,6 +27,14 @@
 #define GICC_OFFSET			0x1000
 #define GICD_OFFSET			0x0000
 
+#define GIC_NON_SEC_SGI_0		0
+#define GIC_SEC_SGI_0			8
+#define GIC_SEC_SGI_1			9
+
+#define TARGET_CPU0_GIC_MASK		BIT(0)
+#define TARGET_CPU1_GIC_MASK		BIT(1)
+#define TARGET_CPUS_GIC_MASK		GENMASK_32(CFG_TEE_CORE_NB_CORE - 1, 0)
+
 /* TAMP resources */
 #define TAMP_BKP_REGISTER_OFF		0x100
 
diff --git a/core/arch/arm/plat-stm32mp1/pm/psci.c b/core/arch/arm/plat-stm32mp1/pm/psci.c
index 5aac318d..5240d716 100644
--- a/core/arch/arm/plat-stm32mp1/pm/psci.c
+++ b/core/arch/arm/plat-stm32mp1/pm/psci.c
@@ -8,6 +8,7 @@
 #include <drivers/stm32mp1_rcc.h>
 #include <io.h>
 #include <kernel/cache_helpers.h>
+#include <kernel/delay.h>
 #include <kernel/generic_boot.h>
 #include <kernel/interrupt.h>
 #include <kernel/misc.h>
@@ -110,19 +111,27 @@ void stm32mp_register_online_cpu(void)
 	unlock_state_access(excep);
 }
 
-#define GIC_SEC_SGI_0		8
+#define GICD_SGIR		0xF00
+static void raise_sgi0_as_secure(void)
+{
+	dsb_ishst();
+	write32(GIC_NON_SEC_SGI_0 | SHIFT_U32(TARGET_CPU1_GIC_MASK, 16),
+		get_gicd_base() + GICD_SGIR);
+}
 
 static void release_secondary_early_hpen(size_t __unused pos)
 {
+	/* Need to send SIG#0 over Group0 after individual core 1 reset */
+	raise_sgi0_as_secure();
+	udelay(20);
 
 	write32(TEE_LOAD_ADDR,
 		stm32mp_bkpreg(BCKR_CORE1_BRANCH_ADDRESS));
 	write32(BOOT_API_A7_CORE1_MAGIC_NUMBER,
 		stm32mp_bkpreg(BCKR_CORE1_MAGIC_NUMBER));
 
-	dmb();
-	isb();
-	itr_raise_sgi(GIC_SEC_SGI_0, BIT(pos));
+	dsb_ishst();
+	itr_raise_sgi(GIC_SEC_SGI_0, TARGET_CPU1_GIC_MASK);
 }
 
 /* Override default psci_cpu_on() with platform specific sequence */
@@ -167,6 +176,31 @@ int psci_cpu_on(uint32_t core_id, uint32_t entry, uint32_t context_id)
 
 	return rc;
 }
+
+/* Override default psci_cpu_off() with platform specific sequence */
+int psci_cpu_off(void)
+{
+	unsigned int pos = get_core_pos();
+	uint32_t excep;
+
+	if (pos == 0) {
+		EMSG("PSCI_CPU_OFF not supported for core #0");
+		return PSCI_RET_INTERNAL_FAILURE;
+	}
+
+	DMSG("core %u", pos);
+
+	excep = lock_state_access();
+
+	assert(core_state[pos] == CORE_ON);
+	core_state[pos] = CORE_OFF;
+
+	unlock_state_access(excep);
+
+	thread_mask_exceptions(THREAD_EXCP_ALL);
+	stm32_pm_cpu_power_down_wfi();
+	panic();
+}
 #endif
 
 /* Override default psci_cpu_on() with platform supported features */