diff --git a/include/lib/psci/psci_lib.h b/include/lib/psci/psci_lib.h index 76c1a8d..1ac45ad 100644 --- a/include/lib/psci/psci_lib.h +++ b/include/lib/psci/psci_lib.h @@ -89,6 +89,8 @@ void psci_register_spd_pm_hook(const spd_pm_ops_t *pm); void psci_prepare_next_non_secure_ctx( entry_point_info_t *next_image_info); +int psci_stop_other_cores(unsigned int wait_ms, + void (*stop_func)(u_register_t mpidr)); #endif /* __ASSEMBLER__ */ #endif /* PSCI_LIB_H */ diff --git a/lib/psci/psci_common.c b/lib/psci/psci_common.c index cced276..6d81377 100644 --- a/lib/psci/psci_common.c +++ b/lib/psci/psci_common.c @@ -12,6 +12,7 @@ #include #include #include +#include #include #include #include @@ -973,3 +974,49 @@ psci_do_pwrdown_cache_maintenance(power_level); #endif } + +/******************************************************************************* + * This function invokes the callback 'stop_func()' with the 'mpidr' of each + * online PE. Caller can pass suitable method to stop a remote core. + * + * 'wait_ms' is the timeout value in milliseconds for the other cores to + * transition to power down state. Passing '0' makes it non-blocking. + * + * The function returns 'PSCI_E_DENIED' if some cores failed to stop within the + * given timeout. + ******************************************************************************/ +int psci_stop_other_cores(unsigned int wait_ms, + void (*stop_func)(u_register_t mpidr)) +{ + unsigned int idx, this_cpu_idx; + + this_cpu_idx = plat_my_core_pos(); + + /* Invoke stop_func for each core */ + for (idx = 0U; idx < psci_plat_core_count; idx++) { + /* skip current CPU */ + if (idx == this_cpu_idx) { + continue; + } + + /* Check if the CPU is ON */ + if (psci_get_aff_info_state_by_idx(idx) == AFF_STATE_ON) { + (*stop_func)(psci_cpu_pd_nodes[idx].mpidr); + } + } + + /* Need to wait for other cores to shutdown */ + if (wait_ms != 0U) { + while ((wait_ms-- != 0U) && (psci_is_last_on_cpu() != 0U)) { + mdelay(1U); + } + + if (psci_is_last_on_cpu() != 0U) { + WARN("Failed to stop all cores!\n"); + psci_print_power_domain_map(); + return PSCI_E_DENIED; + } + } + + return PSCI_E_SUCCESS; +}