diff --git a/lib/psci/psci_common.c b/lib/psci/psci_common.c index ea1a01d..5ab15c6 100644 --- a/lib/psci/psci_common.c +++ b/lib/psci/psci_common.c @@ -775,7 +775,7 @@ * suspend. */ if (psci_get_aff_info_state() == AFF_STATE_OFF) { - ERROR("Unexpected affinity info state"); + ERROR("Unexpected affinity info state.\n"); panic(); } diff --git a/plat/qemu/common/aarch32/plat_helpers.S b/plat/qemu/common/aarch32/plat_helpers.S index aebcfa7..15e860b 100644 --- a/plat/qemu/common/aarch32/plat_helpers.S +++ b/plat/qemu/common/aarch32/plat_helpers.S @@ -72,8 +72,14 @@ /* 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 13a5ee4..dbcdc2d 100644 --- a/plat/qemu/common/aarch64/plat_helpers.S +++ b/plat/qemu/common/aarch64/plat_helpers.S @@ -70,6 +70,12 @@ 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 fb56622..2c358ea 100644 --- a/plat/qemu/common/qemu_gicv2.c +++ b/plat/qemu/common/qemu_gicv2.c @@ -37,3 +37,8 @@ /* 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 28572c5..0d35446 100644 --- a/plat/qemu/common/qemu_gicv3.c +++ b/plat/qemu/common/qemu_gicv3.c @@ -44,3 +44,9 @@ 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 116211c..cf80009 100644 --- a/plat/qemu/common/qemu_pm.c +++ b/plat/qemu/common/qemu_pm.c @@ -152,9 +152,18 @@ * 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 @@ .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 71ea4de..4dc62f5 100644 --- a/plat/qemu/common/qemu_private.h +++ b/plat/qemu/common/qemu_private.h @@ -32,5 +32,6 @@ void plat_qemu_gic_init(void); void qemu_pwr_gic_on_finish(void); +void qemu_pwr_gic_off(void); #endif /* QEMU_PRIVATE_H */