diff --git a/include/bl31/cpu_data.h b/include/bl31/cpu_data.h index 9015c4b..355160b 100644 --- a/include/bl31/cpu_data.h +++ b/include/bl31/cpu_data.h @@ -44,6 +44,7 @@ #include #include +#include #include /******************************************************************************* @@ -63,12 +64,12 @@ * by components that have per-cpu members. The member access macros should be * used for this. ******************************************************************************/ - typedef struct cpu_data { void *cpu_context[2]; #if CRASH_REPORTING uint64_t crash_buf[CPU_DATA_CRASH_BUF_SIZE >> 3]; #endif + struct psci_cpu_data psci_svc_cpu_data; } __aligned(CACHE_WRITEBACK_GRANULE) cpu_data_t; #if CRASH_REPORTING diff --git a/include/bl31/services/psci.h b/include/bl31/services/psci.h index 6512dfb..a89ba4e 100644 --- a/include/bl31/services/psci.h +++ b/include/bl31/services/psci.h @@ -131,6 +131,14 @@ #include +/******************************************************************************* + * Structure used to store per-cpu information relevant to the PSCI service. + * It is populated in the per-cpu data array. In return we get a guarantee that + * this information will not reside on a cache line shared with another cpu. + ******************************************************************************/ +typedef struct psci_cpu_data { + uint32_t power_state; +} psci_cpu_data_t; /******************************************************************************* * Structure populated by platform specific code to export routines which @@ -192,8 +200,9 @@ void psci_aff_on_finish_entry(void); void psci_aff_suspend_finish_entry(void); void psci_register_spd_pm_hook(const spd_pm_ops_t *); -int psci_get_suspend_stateid(unsigned long mpidr); -int psci_get_suspend_afflvl(unsigned long mpidr); +int psci_get_suspend_stateid_by_mpidr(unsigned long); +int psci_get_suspend_stateid(void); +int psci_get_suspend_afflvl(void); uint64_t psci_smc_handler(uint32_t smc_fid, uint64_t x1, diff --git a/services/std_svc/psci/psci_afflvl_suspend.c b/services/std_svc/psci/psci_afflvl_suspend.c index 70f90a1..a123dc3 100644 --- a/services/std_svc/psci/psci_afflvl_suspend.c +++ b/services/std_svc/psci/psci_afflvl_suspend.c @@ -34,6 +34,7 @@ #include #include #include +#include #include #include #include @@ -45,76 +46,59 @@ unsigned int); /******************************************************************************* - * This function sets the power state of the current cpu while - * powering down during a cpu_suspend call + * This function saves the power state parameter passed in the current PSCI + * cpu_suspend call in the per-cpu data array. ******************************************************************************/ -void psci_set_suspend_power_state(aff_map_node_t *node, unsigned int power_state) +void psci_set_suspend_power_state(unsigned int power_state) { - /* - * Check that nobody else is calling this function on our behalf & - * this information is being set only in the cpu node - */ - assert(node->mpidr == (read_mpidr() & MPIDR_AFFINITY_MASK)); - assert(node->level == MPIDR_AFFLVL0); - - /* - * Save PSCI power state parameter for the core in suspend context. - * The node is in always-coherent RAM so it does not need to be flushed - */ - node->power_state = power_state; + set_cpu_data(psci_svc_cpu_data.power_state, power_state); + flush_cpu_data(psci_svc_cpu_data.power_state); } /******************************************************************************* - * This function gets the affinity level till which a cpu is powered down - * during a cpu_suspend call. Returns PSCI_INVALID_DATA if the - * power state saved for the node is invalid + * This function gets the affinity level till which the current cpu could be + * powered down during a cpu_suspend call. Returns PSCI_INVALID_DATA if the + * power state is invalid. ******************************************************************************/ -int psci_get_suspend_afflvl(unsigned long mpidr) -{ - aff_map_node_t *node; - - node = psci_get_aff_map_node(mpidr & MPIDR_AFFINITY_MASK, - MPIDR_AFFLVL0); - assert(node); - - return psci_get_aff_map_node_suspend_afflvl(node); -} - - -/******************************************************************************* - * This function gets the affinity level till which the current cpu was powered - * down during a cpu_suspend call. Returns PSCI_INVALID_DATA if the - * power state saved for the node is invalid - ******************************************************************************/ -int psci_get_aff_map_node_suspend_afflvl(aff_map_node_t *node) +int psci_get_suspend_afflvl() { unsigned int power_state; - assert(node->level == MPIDR_AFFLVL0); + power_state = get_cpu_data(psci_svc_cpu_data.power_state); - power_state = node->power_state; return ((power_state == PSCI_INVALID_DATA) ? - power_state : psci_get_pstate_afflvl(power_state)); + power_state : psci_get_pstate_afflvl(power_state)); } /******************************************************************************* - * This function gets the state id of a cpu stored in suspend context - * while powering down during a cpu_suspend call. Returns 0xFFFFFFFF - * if the power state saved for the node is invalid + * This function gets the state id of the current cpu from the power state + * parameter saved in the per-cpu data array. Returns PSCI_INVALID_DATA if the + * power state saved is invalid. ******************************************************************************/ -int psci_get_suspend_stateid(unsigned long mpidr) +int psci_get_suspend_stateid() { - aff_map_node_t *node; unsigned int power_state; - node = psci_get_aff_map_node(mpidr & MPIDR_AFFINITY_MASK, - MPIDR_AFFLVL0); - assert(node); - assert(node->level == MPIDR_AFFLVL0); + power_state = get_cpu_data(psci_svc_cpu_data.power_state); - power_state = node->power_state; return ((power_state == PSCI_INVALID_DATA) ? - power_state : psci_get_pstate_id(power_state)); + power_state : psci_get_pstate_id(power_state)); +} + +/******************************************************************************* + * This function gets the state id of the cpu specified by the 'mpidr' parameter + * from the power state parameter saved in the per-cpu data array. Returns + * PSCI_INVALID_DATA if the power state saved is invalid. + ******************************************************************************/ +int psci_get_suspend_stateid_by_mpidr(unsigned long mpidr) +{ + unsigned int power_state; + + power_state = get_cpu_data_by_mpidr(mpidr, + psci_svc_cpu_data.power_state); + + return ((power_state == PSCI_INVALID_DATA) ? + power_state : psci_get_pstate_id(power_state)); } /******************************************************************************* @@ -136,7 +120,7 @@ assert(cpu_node->level == MPIDR_AFFLVL0); /* Save PSCI power state parameter for the core in suspend context */ - psci_set_suspend_power_state(cpu_node, power_state); + psci_set_suspend_power_state(power_state); /* * Generic management: Store the re-entry information for the non-secure @@ -451,13 +435,13 @@ * error, it's expected to assert within */ if (psci_spd_pm && psci_spd_pm->svc_suspend) { - suspend_level = psci_get_aff_map_node_suspend_afflvl(cpu_node); + suspend_level = psci_get_suspend_afflvl(); assert (suspend_level != PSCI_INVALID_DATA); psci_spd_pm->svc_suspend_finish(suspend_level); } /* Invalidate the suspend context for the node */ - psci_set_suspend_power_state(cpu_node, PSCI_INVALID_DATA); + psci_set_suspend_power_state(PSCI_INVALID_DATA); /* * Generic management: Now we just need to retrieve the diff --git a/services/std_svc/psci/psci_common.c b/services/std_svc/psci/psci_common.c index 2fd1764..9daf6f0 100644 --- a/services/std_svc/psci/psci_common.c +++ b/services/std_svc/psci/psci_common.c @@ -61,37 +61,39 @@ /******************************************************************************* * Routine to return the maximum affinity level to traverse to after a cpu has * been physically powered up. It is expected to be called immediately after - * reset from assembler code. It has to find its 'aff_map_node' instead of - * getting it as an argument. - * TODO: Calling psci_get_aff_map_node() with the MMU disabled is slow. Add - * support to allow faster access to the target affinity level. + * reset from assembler code. ******************************************************************************/ -int get_power_on_target_afflvl(unsigned long mpidr) +int get_power_on_target_afflvl() { - aff_map_node_t *node; - unsigned int state; int afflvl; +#if DEBUG + unsigned int state; + aff_map_node_t *node; + /* Retrieve our node from the topology tree */ - node = psci_get_aff_map_node(mpidr & MPIDR_AFFINITY_MASK, - MPIDR_AFFLVL0); + node = psci_get_aff_map_node(read_mpidr_el1() & MPIDR_AFFINITY_MASK, + MPIDR_AFFLVL0); assert(node); /* - * Return the maximum supported affinity level if this cpu was off. - * Call the handler in the suspend code if this cpu had been suspended. - * Any other state is invalid. + * Sanity check the state of the cpu. It should be either suspend or "on + * pending" */ state = psci_get_state(node); - if (state == PSCI_STATE_ON_PENDING) - return get_max_afflvl(); + assert(state == PSCI_STATE_SUSPEND || state == PSCI_STATE_ON_PENDING); +#endif - if (state == PSCI_STATE_SUSPEND) { - afflvl = psci_get_aff_map_node_suspend_afflvl(node); - assert(afflvl != PSCI_INVALID_DATA); - return afflvl; - } - return PSCI_E_INVALID_PARAMS; + /* + * Assume that this cpu was suspended and retrieve its target affinity + * level. If it is invalid then it could only have been turned off + * earlier. get_max_afflvl() will return the highest affinity level a + * cpu can be turned off to. + */ + afflvl = psci_get_suspend_afflvl(); + if (afflvl == PSCI_INVALID_DATA) + afflvl = get_max_afflvl(); + return afflvl; } /******************************************************************************* diff --git a/services/std_svc/psci/psci_entry.S b/services/std_svc/psci/psci_entry.S index 68b917e..4b2b106 100644 --- a/services/std_svc/psci/psci_entry.S +++ b/services/std_svc/psci/psci_entry.S @@ -134,18 +134,13 @@ * level 0. * --------------------------------------------- */ - mrs x0, mpidr_el1 bl get_power_on_target_afflvl - cmp x0, xzr - b.lt _panic mov x2, x23 mov x1, x0 mov x0, #MPIDR_AFFLVL0 bl psci_afflvl_power_on_finish b el3_exit -_panic: - b _panic /* -------------------------------------------- * This function is called to indicate to the diff --git a/services/std_svc/psci/psci_private.h b/services/std_svc/psci/psci_private.h index 4bf9107..158a5f7 100644 --- a/services/std_svc/psci/psci_private.h +++ b/services/std_svc/psci/psci_private.h @@ -52,7 +52,6 @@ unsigned short ref_count; unsigned char state; unsigned char level; - unsigned int power_state; bakery_lock_t lock; } aff_map_node_t; @@ -85,7 +84,7 @@ void psci_set_state(aff_map_node_t *node, unsigned short state); unsigned long mpidr_set_aff_inst(unsigned long, unsigned char, int); int psci_validate_mpidr(unsigned long, int); -int get_power_on_target_afflvl(unsigned long mpidr); +int get_power_on_target_afflvl(void); void psci_afflvl_power_on_finish(int, int, afflvl_power_on_finisher_t *); @@ -119,15 +118,13 @@ int psci_afflvl_off(int, int); /* Private exported functions from psci_affinity_suspend.c */ -void psci_set_suspend_power_state(aff_map_node_t *node, - unsigned int power_state); -int psci_get_aff_map_node_suspend_afflvl(aff_map_node_t *node); int psci_afflvl_suspend(unsigned long, unsigned long, unsigned int, int, int); unsigned int psci_afflvl_suspend_finish(int, int); +void psci_set_suspend_power_state(unsigned int power_state); /* Private exported functions from psci_helpers.S */ void psci_do_pwrdown_cache_maintenance(uint32_t affinity_level); diff --git a/services/std_svc/psci/psci_setup.c b/services/std_svc/psci/psci_setup.c index 68f19a0..9e4955d 100644 --- a/services/std_svc/psci/psci_setup.c +++ b/services/std_svc/psci/psci_setup.c @@ -189,9 +189,6 @@ if (state & PSCI_AFF_PRESENT) psci_set_state(&psci_aff_map[idx], PSCI_STATE_OFF); - /* Invalidate the suspend context for the node */ - psci_aff_map[idx].power_state = PSCI_INVALID_DATA; - /* * Associate a non-secure context with this affinity * instance through the context management library. @@ -199,6 +196,11 @@ linear_id = platform_get_core_pos(mpidr); assert(linear_id < PLATFORM_CORE_COUNT); + /* Invalidate the suspend context for the node */ + set_cpu_data_by_index(linear_id, + psci_svc_cpu_data.power_state, + PSCI_INVALID_DATA); + cm_set_context_by_mpidr(mpidr, (void *) &psci_ns_context[linear_id], NON_SECURE);