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

lib/psci/psci_common.c

@@ -775,7 +775,7 @@ void psci_warmboot_entrypoint(void)
 	 * suspend.
 	 */
 	if (psci_get_aff_info_state() == AFF_STATE_OFF) {
-		ERROR("Unexpected affinity info state");
+		ERROR("Unexpected affinity info state.\n");
 		panic();
 	}
 

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

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

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();
+}

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());
+}

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,

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 */