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>

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 void __dead2 gxbb_system_off(void)
 
 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 int32_t gxbb_pwr_domain_on(u_register_t mpidr)
 
 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 @@ static void gxbb_pwr_domain_off(const psci_power_state_t *target_state)
 
 	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 @@ static const plat_psci_ops_t gxbb_ops = {
 	.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 @@ int plat_setup_psci_ops(uintptr_t sec_entrypoint,
 {
 	gxbb_sec_entrypoint = sec_entrypoint;
 	*psci_ops = &gxbb_ops;
+	gxbb_cpu0_go = 0;
 	return 0;
 }