diff --git a/include/bl31/services/psci1.0/psci.h b/include/bl31/services/psci1.0/psci.h index d79ab74..fe23711 100644 --- a/include/bl31/services/psci1.0/psci.h +++ b/include/bl31/services/psci1.0/psci.h @@ -256,13 +256,12 @@ ******************************************************************************/ typedef struct plat_psci_ops { void (*cpu_standby)(plat_local_state_t cpu_state); - int (*pwr_domain_on)(u_register_t mpidr, - unsigned long sec_entrypoint); + int (*pwr_domain_on)(u_register_t mpidr); void (*pwr_domain_off)(const psci_power_state_t *target_state); - void (*pwr_domain_suspend)(unsigned long sec_entrypoint, - const psci_power_state_t *target_state); + void (*pwr_domain_suspend)(const psci_power_state_t *target_state); void (*pwr_domain_on_finish)(const psci_power_state_t *target_state); - void (*pwr_domain_suspend_finish)(const psci_power_state_t *target_state); + void (*pwr_domain_suspend_finish)( + const psci_power_state_t *target_state); void (*system_off)(void) __dead2; void (*system_reset)(void) __dead2; int (*validate_power_state)(unsigned int power_state, @@ -302,8 +301,7 @@ unsigned long, unsigned long); void __dead2 psci_power_down_wfi(void); -void psci_cpu_on_finish_entry(void); -void psci_cpu_suspend_finish_entry(void); +void psci_entrypoint(void); void psci_register_spd_pm_hook(const spd_pm_ops_t *); uint64_t psci_smc_handler(uint32_t smc_fid, diff --git a/include/plat/common/psci1.0/platform.h b/include/plat/common/psci1.0/platform.h index a961863..d29fcfc 100644 --- a/include/plat/common/psci1.0/platform.h +++ b/include/plat/common/psci1.0/platform.h @@ -181,7 +181,8 @@ /******************************************************************************* * Mandatory PSCI functions (BL3-1) ******************************************************************************/ -int plat_setup_psci_ops(const struct plat_psci_ops **); +int plat_setup_psci_ops(uintptr_t sec_entrypoint, + const struct plat_psci_ops **); const unsigned char *plat_get_power_domain_tree_desc(void); /******************************************************************************* diff --git a/services/std_svc/psci1.0/psci_common.c b/services/std_svc/psci1.0/psci_common.c index 70cc98d..0b885cd 100644 --- a/services/std_svc/psci1.0/psci_common.c +++ b/services/std_svc/psci1.0/psci_common.c @@ -189,7 +189,7 @@ * been physically powered up. It is expected to be called immediately after * reset from assembler code. ******************************************************************************/ -int get_power_on_target_pwrlvl(void) +static int get_power_on_target_pwrlvl(void) { int pwrlvl; @@ -651,11 +651,26 @@ * code to enable the gic cpu interface and for a cluster it will enable * coherency at the interconnect level in addition to gic cpu interface. ******************************************************************************/ -void psci_power_up_finish(int end_pwrlvl, - pwrlvl_power_on_finisher_t power_on_handler) +void psci_power_up_finish(void) { unsigned int cpu_idx = plat_my_core_pos(); psci_power_state_t state_info = { {PSCI_LOCAL_STATE_RUN} }; + int end_pwrlvl; + + /* + * Verify that we have been explicitly turned ON or resumed from + * suspend. + */ + if (psci_get_aff_info_state() == AFF_STATE_OFF) { + ERROR("Unexpected affinity info state"); + panic(); + } + + /* + * Get the maximum power domain level to traverse to after this cpu + * has been physically powered up. + */ + end_pwrlvl = get_power_on_target_pwrlvl(); /* * This function acquires the lock corresponding to each power level so @@ -668,9 +683,21 @@ psci_get_target_local_pwr_states(end_pwrlvl, &state_info); /* - * Perform generic, architecture and platform specific handling. + * This CPU could be resuming from suspend or it could have just been + * turned on. To distinguish between these 2 cases, we examine the + * affinity state of the CPU: + * - If the affinity state is ON_PENDING then it has just been + * turned on. + * - Else it is resuming from suspend. + * + * Depending on the type of warm reset identified, choose the right set + * of power management handler and perform the generic, architecture + * and platform specific handling. */ - power_on_handler(cpu_idx, &state_info); + if (psci_get_aff_info_state() == AFF_STATE_ON_PENDING) + psci_cpu_on_finish(cpu_idx, &state_info); + else + psci_cpu_suspend_finish(cpu_idx, &state_info); /* * Set the requested and target state of this CPU and all the higher diff --git a/services/std_svc/psci1.0/psci_entry.S b/services/std_svc/psci1.0/psci_entry.S index 4e7456d..73c3377 100644 --- a/services/std_svc/psci1.0/psci_entry.S +++ b/services/std_svc/psci1.0/psci_entry.S @@ -34,25 +34,16 @@ #include #include - .globl psci_cpu_on_finish_entry - .globl psci_cpu_suspend_finish_entry + .globl psci_entrypoint .globl psci_power_down_wfi - /* ----------------------------------------------------- - * This cpu has been physically powered up. Depending - * upon whether it was resumed from suspend or simply - * turned on, call the common power on finisher with - * the handlers (chosen depending upon original state). - * ----------------------------------------------------- + /* -------------------------------------------------------------------- + * This CPU has been physically powered up. It is either resuming from + * suspend or has simply been turned on. In both cases, call the power + * on finisher. + * -------------------------------------------------------------------- */ -func psci_cpu_on_finish_entry - adr x23, psci_cpu_on_finish - b psci_power_up_entry - -psci_cpu_suspend_finish_entry: - adr x23, psci_cpu_suspend_finish - -psci_power_up_entry: +func psci_entrypoint /* * On the warm boot path, most of the EL3 initialisations performed by * 'el3_entrypoint_common' must be skipped: @@ -98,12 +89,10 @@ mov x0, #DISABLE_DCACHE bl bl31_plat_enable_mmu - bl get_power_on_target_pwrlvl - mov x1, x23 bl psci_power_up_finish b el3_exit -endfunc psci_cpu_on_finish_entry +endfunc psci_entrypoint /* -------------------------------------------- * This function is called to indicate to the diff --git a/services/std_svc/psci1.0/psci_on.c b/services/std_svc/psci1.0/psci_on.c index 542cc23..d68198f 100644 --- a/services/std_svc/psci1.0/psci_on.c +++ b/services/std_svc/psci1.0/psci_on.c @@ -89,7 +89,6 @@ int end_pwrlvl) { int rc; - unsigned long psci_entrypoint; unsigned int target_idx = plat_core_pos_by_mpidr(target_cpu); /* @@ -126,16 +125,12 @@ /* * Perform generic, architecture and platform specific handling. */ - /* Set the secure world (EL3) re-entry point after BL1 */ - psci_entrypoint = (unsigned long) psci_cpu_on_finish_entry; - /* * Plat. management: Give the platform the current state * of the target cpu to allow it to perform the necessary * steps to power on. */ - rc = psci_plat_pm_ops->pwr_domain_on((u_register_t)target_cpu, - psci_entrypoint); + rc = psci_plat_pm_ops->pwr_domain_on((u_register_t)target_cpu); assert(rc == PSCI_E_SUCCESS || rc == PSCI_E_INTERN_FAIL); if (rc == PSCI_E_SUCCESS) diff --git a/services/std_svc/psci1.0/psci_private.h b/services/std_svc/psci1.0/psci_private.h index 8d08df4..e2e32c7 100644 --- a/services/std_svc/psci1.0/psci_private.h +++ b/services/std_svc/psci1.0/psci_private.h @@ -166,9 +166,6 @@ spinlock_t cpu_lock; } cpu_pd_node_t; -typedef void (*pwrlvl_power_on_finisher_t)(unsigned int cpu_idx, - psci_power_state_t *state_info); - /******************************************************************************* * Data prototypes ******************************************************************************/ @@ -190,10 +187,8 @@ psci_power_state_t *state_info); void psci_query_sys_suspend_pwrstate(psci_power_state_t *state_info); int psci_validate_mpidr(unsigned long mpidr); -int get_power_on_target_pwrlvl(void); void psci_init_req_local_pwr_states(void); -void psci_power_up_finish(int end_pwrlvl, - pwrlvl_power_on_finisher_t power_on_handler); +void psci_power_up_finish(void); int psci_get_ns_ep_info(entry_point_info_t *ep, uint64_t entrypoint, uint64_t context_id); void psci_get_parent_pwr_domain_nodes(unsigned int cpu_idx, diff --git a/services/std_svc/psci1.0/psci_setup.c b/services/std_svc/psci1.0/psci_setup.c index 599c09b..ce4da95 100644 --- a/services/std_svc/psci1.0/psci_setup.c +++ b/services/std_svc/psci1.0/psci_setup.c @@ -245,7 +245,8 @@ */ psci_set_pwr_domains_to_run(PLAT_MAX_PWR_LVL); - plat_setup_psci_ops(&psci_plat_pm_ops); + plat_setup_psci_ops((uintptr_t)psci_entrypoint, + &psci_plat_pm_ops); assert(psci_plat_pm_ops); /* Initialize the psci capability */ diff --git a/services/std_svc/psci1.0/psci_suspend.c b/services/std_svc/psci1.0/psci_suspend.c index 2e4270f..71e4778 100644 --- a/services/std_svc/psci1.0/psci_suspend.c +++ b/services/std_svc/psci1.0/psci_suspend.c @@ -133,7 +133,6 @@ { int skip_wfi = 0; unsigned int idx = plat_my_core_pos(); - unsigned long psci_entrypoint; /* * This function must only be called on platforms where the @@ -167,22 +166,16 @@ */ psci_do_state_coordination(end_pwrlvl, state_info); - psci_entrypoint = 0; - if (is_power_down_state) { + if (is_power_down_state) psci_suspend_to_pwrdown_start(end_pwrlvl, ep, state_info); - /* Set the secure world (EL3) re-entry point after BL1. */ - psci_entrypoint = - (unsigned long) psci_cpu_suspend_finish_entry; - } - /* * Plat. management: Allow the platform to perform the * necessary actions to turn off this cpu e.g. set the * platform defined mailbox with the psci entrypoint, * program the power controller etc. */ - psci_plat_pm_ops->pwr_domain_suspend(psci_entrypoint, state_info); + psci_plat_pm_ops->pwr_domain_suspend(state_info); exit: /*