diff options
| -rw-r--r-- | include/bl31/services/psci.h | 3 | ||||
| -rw-r--r-- | services/std_svc/psci/psci_afflvl_off.c | 30 | ||||
| -rw-r--r-- | services/std_svc/psci/psci_afflvl_suspend.c | 28 | ||||
| -rw-r--r-- | services/std_svc/psci/psci_common.c | 80 | ||||
| -rw-r--r-- | services/std_svc/psci/psci_helpers.S | 28 | ||||
| -rw-r--r-- | services/std_svc/psci/psci_private.h | 4 | ||||
| -rw-r--r-- | services/std_svc/psci/psci_setup.c | 9 |
7 files changed, 168 insertions, 14 deletions
diff --git a/include/bl31/services/psci.h b/include/bl31/services/psci.h index a89ba4ec..88b2107c 100644 --- a/include/bl31/services/psci.h +++ b/include/bl31/services/psci.h @@ -138,6 +138,8 @@ ******************************************************************************/ typedef struct psci_cpu_data { uint32_t power_state; + uint32_t max_phys_off_afflvl; /* Highest affinity level in physically + powered off state */ } psci_cpu_data_t; /******************************************************************************* @@ -203,6 +205,7 @@ void psci_register_spd_pm_hook(const spd_pm_ops_t *); int psci_get_suspend_stateid_by_mpidr(unsigned long); int psci_get_suspend_stateid(void); int psci_get_suspend_afflvl(void); +uint32_t psci_get_max_phys_off_afflvl(void); uint64_t psci_smc_handler(uint32_t smc_fid, uint64_t x1, diff --git a/services/std_svc/psci/psci_afflvl_off.c b/services/std_svc/psci/psci_afflvl_off.c index 6bf12db3..231721e7 100644 --- a/services/std_svc/psci/psci_afflvl_off.c +++ b/services/std_svc/psci/psci_afflvl_off.c @@ -102,10 +102,9 @@ static int psci_afflvl1_off(aff_map_node_t *cluster_node) /* * Arch. Management. Flush all levels of caches to PoC if - * the cluster is to be shutdown + * the cluster is to be shutdown. */ - if (plat_state == PSCI_STATE_OFF) - dcsw_op_all(DCCISW); + psci_do_pwrdown_cache_maintenance(MPIDR_AFFLVL1); /* * Plat. Management. Allow the platform to do its cluster @@ -134,7 +133,11 @@ static int psci_afflvl2_off(aff_map_node_t *system_node) */ plat_state = psci_get_phys_state(system_node); - /* No arch. and generic bookeeping to do here currently */ + /* + * Arch. Management. Flush all levels of caches to PoC if + * the system is to be shutdown. + */ + psci_do_pwrdown_cache_maintenance(MPIDR_AFFLVL2); /* * Plat. Management : Allow the platform to do its bookeeping @@ -207,7 +210,7 @@ int psci_afflvl_off(int start_afflvl, { int rc = PSCI_E_SUCCESS; mpidr_aff_map_nodes_t mpidr_nodes; - + unsigned int max_phys_off_afflvl; /* * Collect the pointers to the nodes in the topology tree for @@ -240,12 +243,29 @@ int psci_afflvl_off(int start_afflvl, end_afflvl, mpidr_nodes, PSCI_STATE_OFF); + + max_phys_off_afflvl = psci_find_max_phys_off_afflvl(start_afflvl, + end_afflvl, + mpidr_nodes); + assert(max_phys_off_afflvl != PSCI_INVALID_DATA); + + /* Stash the highest affinity level that will enter the OFF state. */ + psci_set_max_phys_off_afflvl(max_phys_off_afflvl); + /* Perform generic, architecture and platform specific handling */ rc = psci_call_off_handlers(mpidr_nodes, start_afflvl, end_afflvl); /* + * Invalidate the entry for the highest affinity level stashed earlier. + * This ensures that any reads of this variable outside the power + * up/down sequences return PSCI_INVALID_DATA. + * + */ + psci_set_max_phys_off_afflvl(PSCI_INVALID_DATA); + + /* * Release the locks corresponding to each affinity level in the * reverse order to which they were acquired. */ diff --git a/services/std_svc/psci/psci_afflvl_suspend.c b/services/std_svc/psci/psci_afflvl_suspend.c index 4fb640a2..54f26348 100644 --- a/services/std_svc/psci/psci_afflvl_suspend.c +++ b/services/std_svc/psci/psci_afflvl_suspend.c @@ -193,10 +193,9 @@ static int psci_afflvl1_suspend(aff_map_node_t *cluster_node, /* * Arch. management: Flush all levels of caches to PoC if the - * cluster is to be shutdown + * cluster is to be shutdown. */ - if (plat_state == PSCI_STATE_OFF) - dcsw_op_all(DCCISW); + psci_do_pwrdown_cache_maintenance(MPIDR_AFFLVL1); /* * Plat. Management. Allow the platform to do its cluster @@ -242,6 +241,12 @@ static int psci_afflvl2_suspend(aff_map_node_t *system_node, plat_state = psci_get_phys_state(system_node); /* + * Arch. management: Flush all levels of caches to PoC if the + * system is to be shutdown. + */ + psci_do_pwrdown_cache_maintenance(MPIDR_AFFLVL2); + + /* * Plat. Management : Allow the platform to do its bookeeping * at this affinity level */ @@ -333,6 +338,7 @@ int psci_afflvl_suspend(unsigned long entrypoint, { int rc = PSCI_E_SUCCESS; mpidr_aff_map_nodes_t mpidr_nodes; + unsigned int max_phys_off_afflvl; /* * Collect the pointers to the nodes in the topology tree for @@ -365,6 +371,15 @@ int psci_afflvl_suspend(unsigned long entrypoint, end_afflvl, mpidr_nodes, PSCI_STATE_SUSPEND); + + max_phys_off_afflvl = psci_find_max_phys_off_afflvl(start_afflvl, + end_afflvl, + mpidr_nodes); + assert(max_phys_off_afflvl != PSCI_INVALID_DATA); + + /* Stash the highest affinity level that will be turned off */ + psci_set_max_phys_off_afflvl(max_phys_off_afflvl); + /* Perform generic, architecture and platform specific handling */ rc = psci_call_suspend_handlers(mpidr_nodes, start_afflvl, @@ -374,6 +389,13 @@ int psci_afflvl_suspend(unsigned long entrypoint, power_state); /* + * Invalidate the entry for the highest affinity level stashed earlier. + * This ensures that any reads of this variable outside the power + * up/down sequences return PSCI_INVALID_DATA. + */ + psci_set_max_phys_off_afflvl(PSCI_INVALID_DATA); + + /* * Release the locks corresponding to each affinity level in the * reverse order to which they were acquired. */ diff --git a/services/std_svc/psci/psci_common.c b/services/std_svc/psci/psci_common.c index 94855061..e9d6e5b6 100644 --- a/services/std_svc/psci/psci_common.c +++ b/services/std_svc/psci/psci_common.c @@ -59,6 +59,66 @@ __attribute__ ((section("tzfw_coherent_mem"))); const plat_pm_ops_t *psci_plat_pm_ops; /******************************************************************************* + * This function is passed an array of pointers to affinity level nodes in the + * topology tree for an mpidr. It iterates through the nodes to find the highest + * affinity level which is marked as physically powered off. + ******************************************************************************/ +uint32_t psci_find_max_phys_off_afflvl(uint32_t start_afflvl, + uint32_t end_afflvl, + mpidr_aff_map_nodes_t mpidr_nodes) +{ + uint32_t max_afflvl = PSCI_INVALID_DATA; + + for (; start_afflvl <= end_afflvl; start_afflvl++) { + if (mpidr_nodes[start_afflvl] == NULL) + continue; + + if (psci_get_phys_state(mpidr_nodes[start_afflvl]) == + PSCI_STATE_OFF) + max_afflvl = start_afflvl; + } + + return max_afflvl; +} + +/******************************************************************************* + * This function saves the highest affinity level which is in OFF state. The + * affinity instance with which the level is associated is determined by the + * caller. + ******************************************************************************/ +void psci_set_max_phys_off_afflvl(uint32_t afflvl) +{ + set_cpu_data(psci_svc_cpu_data.max_phys_off_afflvl, afflvl); + + /* + * Ensure that the saved value is flushed to main memory and any + * speculatively pre-fetched stale copies are invalidated from the + * caches of other cpus in the same coherency domain. This ensures that + * the value can be safely read irrespective of the state of the data + * cache. + */ + flush_cpu_data(psci_svc_cpu_data.max_phys_off_afflvl); +} + +/******************************************************************************* + * This function reads the saved highest affinity level which is in OFF + * state. The affinity instance with which the level is associated is determined + * by the caller. + ******************************************************************************/ +uint32_t psci_get_max_phys_off_afflvl(void) +{ + /* + * Ensure that the last update of this value in this cpu's cache is + * flushed to main memory and any speculatively pre-fetched stale copies + * are invalidated from the caches of other cpus in the same coherency + * domain. This ensures that the value is always read from the main + * memory when it was written before the data cache was enabled. + */ + flush_cpu_data(psci_svc_cpu_data.max_phys_off_afflvl); + return get_cpu_data(psci_svc_cpu_data.max_phys_off_afflvl); +} + +/******************************************************************************* * 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. @@ -418,6 +478,8 @@ void psci_afflvl_power_on_finish(int start_afflvl, { mpidr_aff_map_nodes_t mpidr_nodes; int rc; + unsigned int max_phys_off_afflvl; + /* * Collect the pointers to the nodes in the topology tree for @@ -441,6 +503,17 @@ void psci_afflvl_power_on_finish(int start_afflvl, end_afflvl, mpidr_nodes); + max_phys_off_afflvl = psci_find_max_phys_off_afflvl(start_afflvl, + end_afflvl, + mpidr_nodes); + assert(max_phys_off_afflvl != PSCI_INVALID_DATA); + + /* + * Stash the highest affinity level that will come out of the OFF or + * SUSPEND states. + */ + psci_set_max_phys_off_afflvl(max_phys_off_afflvl); + /* Perform generic, architecture and platform specific handling */ rc = psci_call_power_on_handlers(mpidr_nodes, start_afflvl, @@ -460,6 +533,13 @@ void psci_afflvl_power_on_finish(int start_afflvl, PSCI_STATE_ON); /* + * Invalidate the entry for the highest affinity level stashed earlier. + * This ensures that any reads of this variable outside the power + * up/down sequences return PSCI_INVALID_DATA + */ + psci_set_max_phys_off_afflvl(PSCI_INVALID_DATA); + + /* * This loop releases the lock corresponding to each affinity level * in the reverse order to which they were acquired. */ diff --git a/services/std_svc/psci/psci_helpers.S b/services/std_svc/psci/psci_helpers.S index 21b5688c..91c31725 100644 --- a/services/std_svc/psci/psci_helpers.S +++ b/services/std_svc/psci/psci_helpers.S @@ -30,7 +30,9 @@ #include <arch.h> #include <asm_macros.S> +#include <assert_macros.S> #include <platform_def.h> +#include <psci.h> .globl psci_do_pwrdown_cache_maintenance .globl psci_do_pwrup_cache_maintenance @@ -38,18 +40,31 @@ /* ----------------------------------------------------------------------- * void psci_do_pwrdown_cache_maintenance(uint32_t affinity level); * - * This function performs cache maintenance before this cpu is powered - * off. The levels of cache affected are determined by the affinity level - * which is passed as the argument. Additionally, this function also - * ensures that stack memory is correctly flushed out to avoid coherency - * issues due to a change in its memory attributes after the data cache - * is disabled. + * This function performs cache maintenance if the specified affinity + * level is the equal to the level of the highest affinity instance which + * will be/is physically powered off. The levels of cache affected are + * determined by the affinity level which is passed as the argument i.e. + * level 0 results in a flush of the L1 cache. Both the L1 and L2 caches + * are flushed for a higher affinity level. + * + * Additionally, this function also ensures that stack memory is correctly + * flushed out to avoid coherency issues due to a change in its memory + * attributes after the data cache is disabled. * ----------------------------------------------------------------------- */ func psci_do_pwrdown_cache_maintenance stp x29, x30, [sp,#-16]! stp x19, x20, [sp,#-16]! + mov x19, x0 + bl psci_get_max_phys_off_afflvl +#if ASM_ASSERTION + cmp x0, #PSCI_INVALID_DATA + ASM_ASSERT(ne) +#endif + cmp x0, x19 + b.ne 1f + /* --------------------------------------------- * Disable the Data Cache. * --------------------------------------------- @@ -127,6 +142,7 @@ do_stack_maintenance: sub x1, sp, x0 bl inv_dcache_range +1: ldp x19, x20, [sp], #16 ldp x29, x30, [sp], #16 ret diff --git a/services/std_svc/psci/psci_private.h b/services/std_svc/psci/psci_private.h index 73690b85..bbc8c323 100644 --- a/services/std_svc/psci/psci_private.h +++ b/services/std_svc/psci/psci_private.h @@ -103,6 +103,10 @@ void psci_release_afflvl_locks(int start_afflvl, int end_afflvl, mpidr_aff_map_nodes_t mpidr_nodes); void psci_print_affinity_map(void); +void psci_set_max_phys_off_afflvl(uint32_t afflvl); +uint32_t psci_find_max_phys_off_afflvl(uint32_t start_afflvl, + uint32_t end_afflvl, + mpidr_aff_map_nodes_t mpidr_nodes); /* Private exported functions from psci_setup.c */ int psci_get_aff_map_nodes(unsigned long mpidr, diff --git a/services/std_svc/psci/psci_setup.c b/services/std_svc/psci/psci_setup.c index 9e4955dc..b49b6e81 100644 --- a/services/std_svc/psci/psci_setup.c +++ b/services/std_svc/psci/psci_setup.c @@ -201,6 +201,15 @@ static void psci_init_aff_map_node(unsigned long mpidr, psci_svc_cpu_data.power_state, PSCI_INVALID_DATA); + /* + * There is no state associated with the current execution + * context so ensure that any reads of the highest affinity + * level in a powered down state return PSCI_INVALID_DATA. + */ + set_cpu_data_by_index(linear_id, + psci_svc_cpu_data.max_phys_off_afflvl, + PSCI_INVALID_DATA); + cm_set_context_by_mpidr(mpidr, (void *) &psci_ns_context[linear_id], NON_SECURE); |