diff --git a/include/lib/aarch64/arch_helpers.h b/include/lib/aarch64/arch_helpers.h index 7d24a5378351d60b8fe0f5bbe91e4d7de5964585..65941e6cfc9b0aee45870760ddcd55075e4eacd0 100644 --- a/include/lib/aarch64/arch_helpers.h +++ b/include/lib/aarch64/arch_helpers.h @@ -270,6 +270,8 @@ DEFINE_SYSREG_RW_FUNCS(cntvoff_el2) DEFINE_SYSREG_RW_FUNCS(vpidr_el2) DEFINE_SYSREG_RW_FUNCS(vmpidr_el2) +DEFINE_SYSREG_READ_FUNC(isr_el1) + /* GICv3 System Registers */ DEFINE_RENAME_SYSREG_RW_FUNCS(icc_sre_el1, ICC_SRE_EL1) diff --git a/services/std_svc/psci/psci_afflvl_off.c b/services/std_svc/psci/psci_afflvl_off.c index d1b7e88d72e8dd366bc6015af01b44ad45d3c966..ceb51f83e9d37ac31144111ac40295ede3dc8cbf 100644 --- a/services/std_svc/psci/psci_afflvl_off.c +++ b/services/std_svc/psci/psci_afflvl_off.c @@ -236,5 +236,13 @@ exit: end_afflvl, mpidr_nodes); + /* + * Check if all actions needed to safely power down this cpu have + * successfully completed. Enter a wfi loop which will allow the + * power controller to physically power down this cpu. + */ + if (rc == PSCI_E_SUCCESS) + psci_power_down_wfi(); + return rc; } diff --git a/services/std_svc/psci/psci_afflvl_suspend.c b/services/std_svc/psci/psci_afflvl_suspend.c index 35f9e4a5a42d36047948a18de836b456cfd6db88..9ede65d015126b4113e475ea59ad4b591e4fba46 100644 --- a/services/std_svc/psci/psci_afflvl_suspend.c +++ b/services/std_svc/psci/psci_afflvl_suspend.c @@ -257,6 +257,7 @@ void psci_afflvl_suspend(entry_point_info_t *ep, int start_afflvl, int end_afflvl) { + int skip_wfi = 0; mpidr_aff_map_nodes_t mpidr_nodes; unsigned int max_phys_off_afflvl; @@ -280,6 +281,16 @@ void psci_afflvl_suspend(entry_point_info_t *ep, end_afflvl, mpidr_nodes); + /* + * We check if there are any pending interrupts after the delay + * introduced by lock contention to increase the chances of early + * detection that a wake-up interrupt has fired. + */ + if (read_isr_el1()) { + skip_wfi = 1; + goto exit; + } + /* * Call the cpu suspend handler registered by the Secure Payload * Dispatcher to let it do any bookeeping. If the handler encounters an @@ -323,6 +334,7 @@ void psci_afflvl_suspend(entry_point_info_t *ep, */ psci_set_max_phys_off_afflvl(PSCI_INVALID_DATA); +exit: /* * Release the locks corresponding to each affinity level in the * reverse order to which they were acquired. @@ -330,6 +342,8 @@ void psci_afflvl_suspend(entry_point_info_t *ep, psci_release_afflvl_locks(start_afflvl, end_afflvl, mpidr_nodes); + if (!skip_wfi) + psci_power_down_wfi(); } /******************************************************************************* diff --git a/services/std_svc/psci/psci_main.c b/services/std_svc/psci/psci_main.c index 7c686949f73c143998dd7f9e2fbddc4d8281d654..91d16f46a91c62804b07c1900672c8f4c4880e2a 100644 --- a/services/std_svc/psci/psci_main.c +++ b/services/std_svc/psci/psci_main.c @@ -161,8 +161,6 @@ int psci_cpu_suspend(unsigned int power_state, MPIDR_AFFLVL0, target_afflvl); - psci_power_down_wfi(); - /* Reset PSCI power state parameter for the core. */ psci_set_suspend_power_state(PSCI_INVALID_DATA); return PSCI_E_SUCCESS; @@ -181,14 +179,6 @@ int psci_cpu_off(void) */ rc = psci_afflvl_off(MPIDR_AFFLVL0, target_afflvl); - /* - * Check if all actions needed to safely power down this cpu have - * successfully completed. Enter a wfi loop which will allow the - * power controller to physically power down this cpu. - */ - if (rc == PSCI_E_SUCCESS) - psci_power_down_wfi(); - /* * The only error cpu_off can return is E_DENIED. So check if that's * indeed the case.