diff --git a/services/std_svc/psci/psci_afflvl_on.c b/services/std_svc/psci/psci_afflvl_on.c index 04529d2..b778c5e 100644 --- a/services/std_svc/psci/psci_afflvl_on.c +++ b/services/std_svc/psci/psci_afflvl_on.c @@ -40,9 +40,7 @@ #include "psci_private.h" typedef int (*afflvl_on_handler_t)(unsigned long target_cpu, - aff_map_node_t *node, - unsigned long ns_entrypoint, - unsigned long context_id); + aff_map_node_t *node); /******************************************************************************* * This function checks whether a cpu which has been requested to be turned on @@ -66,14 +64,9 @@ * TODO: Split this code across separate handlers for each type of setup? ******************************************************************************/ static int psci_afflvl0_on(unsigned long target_cpu, - aff_map_node_t *cpu_node, - unsigned long ns_entrypoint, - unsigned long context_id) + aff_map_node_t *cpu_node) { unsigned long psci_entrypoint; - uint32_t ns_scr_el3 = read_scr_el3(); - uint32_t ns_sctlr_el1 = read_sctlr_el1(); - int rc; /* Sanity check to safeguard against data corruption */ assert(cpu_node->level == MPIDR_AFFLVL0); @@ -86,16 +79,6 @@ if (psci_spd_pm && psci_spd_pm->svc_on) psci_spd_pm->svc_on(target_cpu); - /* - * Arch. management: Derive the re-entry information for - * the non-secure world from the non-secure state from - * where this call originated. - */ - rc = psci_save_ns_entry(target_cpu, ns_entrypoint, context_id, - ns_scr_el3, ns_sctlr_el1); - if (rc != PSCI_E_SUCCESS) - return rc; - /* Set the secure world (EL3) re-entry point after BL1 */ psci_entrypoint = (unsigned long) psci_aff_on_finish_entry; @@ -119,9 +102,7 @@ * TODO: Split this code across separate handlers for each type of setup? ******************************************************************************/ static int psci_afflvl1_on(unsigned long target_cpu, - aff_map_node_t *cluster_node, - unsigned long ns_entrypoint, - unsigned long context_id) + aff_map_node_t *cluster_node) { unsigned long psci_entrypoint; @@ -155,9 +136,7 @@ * TODO: Split this code across separate handlers for each type of setup? ******************************************************************************/ static int psci_afflvl2_on(unsigned long target_cpu, - aff_map_node_t *system_node, - unsigned long ns_entrypoint, - unsigned long context_id) + aff_map_node_t *system_node) { unsigned long psci_entrypoint; @@ -201,9 +180,7 @@ static int psci_call_on_handlers(aff_map_node_t *target_cpu_nodes[], int start_afflvl, int end_afflvl, - unsigned long target_cpu, - unsigned long entrypoint, - unsigned long context_id) + unsigned long target_cpu) { int rc = PSCI_E_INVALID_PARAMS, level; aff_map_node_t *node; @@ -219,9 +196,7 @@ * affinity levels. */ rc = psci_afflvl_on_handlers[level](target_cpu, - node, - entrypoint, - context_id); + node); if (rc != PSCI_E_SUCCESS) break; } @@ -246,8 +221,7 @@ * first. ******************************************************************************/ int psci_afflvl_on(unsigned long target_cpu, - unsigned long entrypoint, - unsigned long context_id, + entry_point_info_t *ep, int start_afflvl, int end_afflvl) { @@ -290,20 +264,23 @@ rc = psci_call_on_handlers(target_cpu_nodes, start_afflvl, end_afflvl, - target_cpu, - entrypoint, - context_id); + target_cpu); /* * This function updates the state of each affinity instance * corresponding to the mpidr in the range of affinity levels * specified. */ - if (rc == PSCI_E_SUCCESS) + if (rc == PSCI_E_SUCCESS) { psci_do_afflvl_state_mgmt(start_afflvl, end_afflvl, target_cpu_nodes, PSCI_STATE_ON_PENDING); + /* + * Store the re-entry information for the non-secure world. + */ + cm_init_context(target_cpu, ep); + } exit: /* diff --git a/services/std_svc/psci/psci_afflvl_suspend.c b/services/std_svc/psci/psci_afflvl_suspend.c index d30bf39..4988e67 100644 --- a/services/std_svc/psci/psci_afflvl_suspend.c +++ b/services/std_svc/psci/psci_afflvl_suspend.c @@ -41,8 +41,6 @@ #include "psci_private.h" typedef int (*afflvl_suspend_handler_t)(aff_map_node_t *node, - unsigned long ns_entrypoint, - unsigned long context_id, unsigned int power_state); /******************************************************************************* @@ -106,14 +104,9 @@ * level which is called when that affinity level is about to be suspended. ******************************************************************************/ static int psci_afflvl0_suspend(aff_map_node_t *cpu_node, - unsigned long ns_entrypoint, - unsigned long context_id, unsigned int power_state) { unsigned long psci_entrypoint; - uint32_t ns_scr_el3 = read_scr_el3(); - uint32_t ns_sctlr_el1 = read_sctlr_el1(); - int rc; /* Sanity check to safeguard against data corruption */ assert(cpu_node->level == MPIDR_AFFLVL0); @@ -122,8 +115,7 @@ psci_set_suspend_power_state(power_state); /* - * Generic management: Store the re-entry information for the non-secure - * world and allow the secure world to suspend itself + * Generic management: Allow the Secure world to suspend itself */ /* @@ -134,14 +126,6 @@ if (psci_spd_pm && psci_spd_pm->svc_suspend) psci_spd_pm->svc_suspend(power_state); - /* - * Generic management: Store the re-entry information for the - * non-secure world - */ - rc = psci_save_ns_entry(read_mpidr_el1(), ns_entrypoint, context_id, - ns_scr_el3, ns_sctlr_el1); - if (rc != PSCI_E_SUCCESS) - return rc; /* Set the secure world (EL3) re-entry point after BL1 */ psci_entrypoint = (unsigned long) psci_aff_suspend_finish_entry; @@ -167,8 +151,6 @@ } static int psci_afflvl1_suspend(aff_map_node_t *cluster_node, - unsigned long ns_entrypoint, - unsigned long context_id, unsigned int power_state) { unsigned int plat_state; @@ -203,8 +185,6 @@ static int psci_afflvl2_suspend(aff_map_node_t *system_node, - unsigned long ns_entrypoint, - unsigned long context_id, unsigned int power_state) { unsigned int plat_state; @@ -259,8 +239,6 @@ static int psci_call_suspend_handlers(aff_map_node_t *mpidr_nodes[], int start_afflvl, int end_afflvl, - unsigned long entrypoint, - unsigned long context_id, unsigned int power_state) { int rc = PSCI_E_INVALID_PARAMS, level; @@ -277,8 +255,6 @@ * lower affinity levels. */ rc = psci_afflvl_suspend_handlers[level](node, - entrypoint, - context_id, power_state); if (rc != PSCI_E_SUCCESS) break; @@ -306,8 +282,7 @@ * to turn off affinity level X it is neccesary to turn off affinity level X - 1 * first. ******************************************************************************/ -int psci_afflvl_suspend(unsigned long entrypoint, - unsigned long context_id, +int psci_afflvl_suspend(entry_point_info_t *ep, unsigned int power_state, int start_afflvl, int end_afflvl) @@ -356,12 +331,15 @@ /* Stash the highest affinity level that will be turned off */ psci_set_max_phys_off_afflvl(max_phys_off_afflvl); + /* + * Store the re-entry information for the non-secure world. + */ + cm_init_context(read_mpidr_el1(), ep); + /* Perform generic, architecture and platform specific handling */ rc = psci_call_suspend_handlers(mpidr_nodes, start_afflvl, end_afflvl, - entrypoint, - context_id, power_state); /* diff --git a/services/std_svc/psci/psci_common.c b/services/std_svc/psci/psci_common.c index 0a1cdf9..507b56e 100644 --- a/services/std_svc/psci/psci_common.c +++ b/services/std_svc/psci/psci_common.c @@ -290,15 +290,14 @@ /******************************************************************************* * This function determines the full entrypoint information for the requested - * PSCI entrypoint on power on/resume and saves this in the non-secure CPU - * cpu_context, ready for when the core boots. + * PSCI entrypoint on power on/resume and returns it. ******************************************************************************/ -int psci_save_ns_entry(uint64_t mpidr, - uint64_t entrypoint, uint64_t context_id, - uint32_t ns_scr_el3, uint32_t ns_sctlr_el1) +int psci_get_ns_ep_info(entry_point_info_t *ep, + uint64_t entrypoint, uint64_t context_id) { uint32_t ep_attr, mode, sctlr, daif, ee; - entry_point_info_t ep; + uint32_t ns_scr_el3 = read_scr_el3(); + uint32_t ns_sctlr_el1 = read_sctlr_el1(); sctlr = ns_scr_el3 & SCR_HCE_BIT ? read_sctlr_el2() : ns_sctlr_el1; ee = 0; @@ -308,11 +307,11 @@ ep_attr |= EP_EE_BIG; ee = 1; } - SET_PARAM_HEAD(&ep, PARAM_EP, VERSION_1, ep_attr); + SET_PARAM_HEAD(ep, PARAM_EP, VERSION_1, ep_attr); - ep.pc = entrypoint; - memset(&ep.args, 0, sizeof(ep.args)); - ep.args.arg0 = context_id; + ep->pc = entrypoint; + memset(&ep->args, 0, sizeof(ep->args)); + ep->args.arg0 = context_id; /* * Figure out whether the cpu enters the non-secure address space @@ -329,7 +328,7 @@ mode = ns_scr_el3 & SCR_HCE_BIT ? MODE_EL2 : MODE_EL1; - ep.spsr = SPSR_64(mode, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS); + ep->spsr = SPSR_64(mode, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS); } else { mode = ns_scr_el3 & SCR_HCE_BIT ? MODE32_hyp : MODE32_svc; @@ -340,12 +339,9 @@ */ daif = DAIF_ABT_BIT | DAIF_IRQ_BIT | DAIF_FIQ_BIT; - ep.spsr = SPSR_MODE32(mode, entrypoint & 0x1, ee, daif); + ep->spsr = SPSR_MODE32(mode, entrypoint & 0x1, ee, daif); } - /* initialise an entrypoint to set up the CPU context */ - cm_init_context(mpidr, &ep); - return PSCI_E_SUCCESS; } diff --git a/services/std_svc/psci/psci_main.c b/services/std_svc/psci/psci_main.c index 2e700e8..7fce5fa 100644 --- a/services/std_svc/psci/psci_main.c +++ b/services/std_svc/psci/psci_main.c @@ -45,6 +45,7 @@ { int rc; unsigned int start_afflvl, end_afflvl; + entry_point_info_t ep; /* Determine if the cpu exists of not */ rc = psci_validate_mpidr(target_cpu, MPIDR_AFFLVL0); @@ -53,14 +54,23 @@ } /* + * Verify and derive the re-entry information for + * the non-secure world from the non-secure state from + * where this call originated. + */ + rc = psci_get_ns_ep_info(&ep, entrypoint, context_id); + if (rc != PSCI_E_SUCCESS) + return rc; + + + /* * To turn this cpu on, specify which affinity * levels need to be turned on */ start_afflvl = MPIDR_AFFLVL0; end_afflvl = get_max_afflvl(); rc = psci_afflvl_on(target_cpu, - entrypoint, - context_id, + &ep, start_afflvl, end_afflvl); @@ -79,6 +89,7 @@ { int rc; unsigned int target_afflvl, pstate_type; + entry_point_info_t ep; /* Check SBZ bits in power state are zero */ if (psci_validate_power_state(power_state)) @@ -106,12 +117,20 @@ } /* + * Verify and derive the re-entry information for + * the non-secure world from the non-secure state from + * where this call originated. + */ + rc = psci_get_ns_ep_info(&ep, entrypoint, context_id); + if (rc != PSCI_E_SUCCESS) + return rc; + + /* * Do what is needed to enter the power down state. Upon success, * enter the final wfi which will power down this cpu else return * an error. */ - rc = psci_afflvl_suspend(entrypoint, - context_id, + rc = psci_afflvl_suspend(&ep, power_state, MPIDR_AFFLVL0, target_afflvl); diff --git a/services/std_svc/psci/psci_private.h b/services/std_svc/psci/psci_private.h index 9a8ef73..3a4ee3c 100644 --- a/services/std_svc/psci/psci_private.h +++ b/services/std_svc/psci/psci_private.h @@ -33,6 +33,7 @@ #include #include +#include #include /* @@ -101,9 +102,8 @@ void psci_afflvl_power_on_finish(int, int, afflvl_power_on_finisher_t *); -int psci_save_ns_entry(uint64_t mpidr, - uint64_t entrypoint, uint64_t context_id, - uint32_t caller_scr_el3, uint32_t caller_sctlr_el1); +int psci_get_ns_ep_info(entry_point_info_t *ep, + uint64_t entrypoint, uint64_t context_id); int psci_check_afflvl_range(int start_afflvl, int end_afflvl); void psci_do_afflvl_state_mgmt(uint32_t start_afflvl, uint32_t end_afflvl, @@ -129,21 +129,20 @@ aff_map_node_t *psci_get_aff_map_node(unsigned long, int); /* Private exported functions from psci_affinity_on.c */ -int psci_afflvl_on(unsigned long, - unsigned long, - unsigned long, - int, - int); +int psci_afflvl_on(unsigned long target_cpu, + entry_point_info_t *ep, + int start_afflvl, + int end_afflvl); /* Private exported functions from psci_affinity_off.c */ int psci_afflvl_off(int, int); /* Private exported functions from psci_affinity_suspend.c */ -int psci_afflvl_suspend(unsigned long, - unsigned long, - unsigned int, - int, - int); +int psci_afflvl_suspend(entry_point_info_t *ep, + unsigned int power_state, + int start_afflvl, + int end_afflvl); + unsigned int psci_afflvl_suspend_finish(int, int); void psci_set_suspend_power_state(unsigned int power_state);