qemu: Implement PSCI_CPU_OFF.

This is based on the rpi implementation from
https://review.trustedfirmware.org/c/TF-A/trusted-firmware-a/+/2746.

Signed-off-by: Andrew Walbran <qwandor@google.com>
Change-Id: I5fe324fcd9d5e232091e01267ea12147c46bc9c1

6 files changed, 37 insertions, 3 deletions

diff --git a/plat/qemu/common/aarch32/plat_helpers.S b/plat/qemu/common/aarch32/plat_helpers.S
index aebcfa78f..15e860b42 100644
--- a/plat/qemu/common/aarch32/plat_helpers.S
+++ b/plat/qemu/common/aarch32/plat_helpers.S
@@ -72,8 +72,14 @@ func plat_secondary_cold_boot_setup
 	/* Wait until we have a go */
 poll_mailbox:
 	ldr	r1, [r2, r0]
-        cmp     r1, #0
+        cmp     r1, #PLAT_QEMU_HOLD_STATE_WAIT
         beq     1f
+
+	/* Clear the mailbox again ready for next time. */
+	mov r1, #PLAT_QEMU_HOLD_STATE_WAIT
+	str r1, [r2, r0]
+
+	/* Jump to the provided entrypoint. */
 	mov_imm	r0, PLAT_QEMU_TRUSTED_MAILBOX_BASE
 	ldr	r1, [r0]
 	bx	r1
diff --git a/plat/qemu/common/aarch64/plat_helpers.S b/plat/qemu/common/aarch64/plat_helpers.S
index 13a5ee461..dbcdc2d39 100644
--- a/plat/qemu/common/aarch64/plat_helpers.S
+++ b/plat/qemu/common/aarch64/plat_helpers.S
@@ -70,6 +70,12 @@ func plat_secondary_cold_boot_setup
 poll_mailbox:
 	ldr	x1, [x2, x0]
 	cbz	x1, 1f
+
+	/* Clear the mailbox again ready for next time. */
+	mov x1, #PLAT_QEMU_HOLD_STATE_WAIT
+	str x1, [x2, x0]
+
+	/* Jump to the provided entrypoint. */
 	mov_imm	x0, PLAT_QEMU_TRUSTED_MAILBOX_BASE
 	ldr	x1, [x0]
 	br	x1
diff --git a/plat/qemu/common/qemu_gicv2.c b/plat/qemu/common/qemu_gicv2.c
index fb566227a..2c358ea1a 100644
--- a/plat/qemu/common/qemu_gicv2.c
+++ b/plat/qemu/common/qemu_gicv2.c
@@ -37,3 +37,8 @@ void qemu_pwr_gic_on_finish(void)
 	/* Enable the gic cpu interface */
 	gicv2_cpuif_enable();
 }
+
+void qemu_pwr_gic_off(void)
+{
+	gicv2_cpuif_disable();
+}
diff --git a/plat/qemu/common/qemu_gicv3.c b/plat/qemu/common/qemu_gicv3.c
index 28572c5ef..0d35446bd 100644
--- a/plat/qemu/common/qemu_gicv3.c
+++ b/plat/qemu/common/qemu_gicv3.c
@@ -44,3 +44,9 @@ void qemu_pwr_gic_on_finish(void)
 	gicv3_rdistif_init(plat_my_core_pos());
 	gicv3_cpuif_enable(plat_my_core_pos());
 }
+
+void qemu_pwr_gic_off(void)
+{
+	gicv3_cpuif_disable(plat_my_core_pos());
+	gicv3_rdistif_off(plat_my_core_pos());
+}
diff --git a/plat/qemu/common/qemu_pm.c b/plat/qemu/common/qemu_pm.c
index 116211c51..cf800096f 100644
--- a/plat/qemu/common/qemu_pm.c
+++ b/plat/qemu/common/qemu_pm.c
@@ -152,9 +152,18 @@ static int qemu_pwr_domain_on(u_register_t mpidr)
  * Platform handler called when a power domain is about to be turned off. The
  * target_state encodes the power state that each level should transition to.
  ******************************************************************************/
-void qemu_pwr_domain_off(const psci_power_state_t *target_state)
+static void qemu_pwr_domain_off(const psci_power_state_t *target_state)
 {
-	assert(0);
+	qemu_pwr_gic_off();
+}
+
+void __dead2 plat_secondary_cold_boot_setup(void);
+
+static void __dead2
+qemu_pwr_domain_pwr_down_wfi(const psci_power_state_t *target_state)
+{
+	disable_mmu_el3();
+	plat_secondary_cold_boot_setup();
 }
 
 /*******************************************************************************
@@ -209,6 +218,7 @@ static const plat_psci_ops_t plat_qemu_psci_pm_ops = {
 	.cpu_standby = qemu_cpu_standby,
 	.pwr_domain_on = qemu_pwr_domain_on,
 	.pwr_domain_off = qemu_pwr_domain_off,
+	.pwr_domain_pwr_down_wfi = qemu_pwr_domain_pwr_down_wfi,
 	.pwr_domain_suspend = qemu_pwr_domain_suspend,
 	.pwr_domain_on_finish = qemu_pwr_domain_on_finish,
 	.pwr_domain_suspend_finish = qemu_pwr_domain_suspend_finish,
diff --git a/plat/qemu/common/qemu_private.h b/plat/qemu/common/qemu_private.h
index 71ea4de10..4dc62f539 100644
--- a/plat/qemu/common/qemu_private.h
+++ b/plat/qemu/common/qemu_private.h
@@ -32,5 +32,6 @@ void qemu_console_init(void);
 
 void plat_qemu_gic_init(void);
 void qemu_pwr_gic_on_finish(void);
+void qemu_pwr_gic_off(void);
 
 #endif /* QEMU_PRIVATE_H */