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; }