gxbb: Workaround for PSCI_CPU_OFF
There seems to be a problem where SCP can't turn CPU0 off. Instead of
returning PSCI_E_DENIED or crashing make CPU0 wait in a WFE loop. This
way all CPUs have a consistent behaviour from the point of view of the
caller.
Change-Id: I5c8c266ca3b69c9e7a4f5ae70afeea5dd36a0825
Signed-off-by: Antonio Nino Diaz <antonio.ninodiaz@arm.com>
diff --git a/plat/meson/gxbb/gxbb_pm.c b/plat/meson/gxbb/gxbb_pm.c
index 632a53f..930b5e1 100644
--- a/plat/meson/gxbb/gxbb_pm.c
+++ b/plat/meson/gxbb/gxbb_pm.c
@@ -25,6 +25,7 @@
#define SCPI_SYSTEM_REBOOT 1
static uintptr_t gxbb_sec_entrypoint;
+static volatile uint32_t gxbb_cpu0_go;
static void gxbb_program_mailbox(u_register_t mpidr, uint64_t value)
{
@@ -83,6 +84,22 @@
static int32_t gxbb_pwr_domain_on(u_register_t mpidr)
{
+ unsigned int core = plat_gxbb_calc_core_pos(mpidr);
+
+ /* CPU0 can't be turned OFF, emulate it with a WFE loop */
+ if (core == GXBB_PRIMARY_CPU) {
+ VERBOSE("BL31: Releasing CPU0 from wait loop...\n");
+
+ gxbb_cpu0_go = 1;
+ flush_dcache_range((uintptr_t)&gxbb_cpu0_go, sizeof(gxbb_cpu0_go));
+ dsb();
+ isb();
+
+ sev();
+
+ return PSCI_E_SUCCESS;
+ }
+
gxbb_program_mailbox(mpidr, gxbb_sec_entrypoint);
scpi_set_css_power_state(mpidr,
SCPI_POWER_ON, SCPI_POWER_ON, SCPI_POWER_ON);
@@ -94,9 +111,18 @@
static void gxbb_pwr_domain_on_finish(const psci_power_state_t *target_state)
{
+ unsigned int core = plat_gxbb_calc_core_pos(read_mpidr_el1());
+
assert(target_state->pwr_domain_state[MPIDR_AFFLVL0] ==
PLAT_LOCAL_STATE_OFF);
+ if (core == GXBB_PRIMARY_CPU) {
+ gxbb_cpu0_go = 0;
+ flush_dcache_range((uintptr_t)&gxbb_cpu0_go, sizeof(gxbb_cpu0_go));
+ dsb();
+ isb();
+ }
+
gicv2_pcpu_distif_init();
gicv2_cpuif_enable();
}
@@ -112,10 +138,37 @@
gicv2_cpuif_disable();
+ /* CPU0 can't be turned OFF, emulate it with a WFE loop */
+ if (core == GXBB_PRIMARY_CPU)
+ return;
+
scpi_set_css_power_state(mpidr,
SCPI_POWER_OFF, SCPI_POWER_ON, SCPI_POWER_ON);
}
+static void __dead2 gxbb_pwr_domain_pwr_down_wfi(const psci_power_state_t
+ *target_state)
+{
+ unsigned int core = plat_gxbb_calc_core_pos(read_mpidr_el1());
+
+ /* CPU0 can't be turned OFF, emulate it with a WFE loop */
+ if (core == GXBB_PRIMARY_CPU) {
+ VERBOSE("BL31: CPU0 entering wait loop...\n");
+
+ while (gxbb_cpu0_go == 0)
+ wfe();
+
+ VERBOSE("BL31: CPU0 resumed.\n");
+
+ write_rmr_el3(RMR_EL3_RR_BIT | RMR_EL3_AA64_BIT);
+ }
+
+ dsbsy();
+
+ for (;;)
+ wfi();
+}
+
/*******************************************************************************
* Platform handlers and setup function.
******************************************************************************/
@@ -123,6 +176,7 @@
.pwr_domain_on = gxbb_pwr_domain_on,
.pwr_domain_on_finish = gxbb_pwr_domain_on_finish,
.pwr_domain_off = gxbb_pwr_domain_off,
+ .pwr_domain_pwr_down_wfi = gxbb_pwr_domain_pwr_down_wfi,
.system_off = gxbb_system_off,
.system_reset = gxbb_system_reset,
};
@@ -132,5 +186,6 @@
{
gxbb_sec_entrypoint = sec_entrypoint;
*psci_ops = &gxbb_ops;
+ gxbb_cpu0_go = 0;
return 0;
}