diff --git a/services/std_svc/psci/psci_afflvl_off.c b/services/std_svc/psci/psci_afflvl_off.c index 30f2bd1..a8904e9 100644 --- a/services/std_svc/psci/psci_afflvl_off.c +++ b/services/std_svc/psci/psci_afflvl_off.c @@ -34,13 +34,13 @@ #include #include "psci_private.h" -typedef int (*afflvl_off_handler_t)(unsigned long, aff_map_node_t *); +typedef int (*afflvl_off_handler_t)(aff_map_node_t *); /******************************************************************************* * The next three functions implement a handler for each supported affinity * level which is called when that affinity level is turned off. ******************************************************************************/ -static int psci_afflvl0_off(unsigned long mpidr, aff_map_node_t *cpu_node) +static int psci_afflvl0_off(aff_map_node_t *cpu_node) { unsigned int plat_state; int rc; @@ -98,7 +98,7 @@ /* Get the current physical state of this cpu */ plat_state = psci_get_phys_state(cpu_node); - rc = psci_plat_pm_ops->affinst_off(mpidr, + rc = psci_plat_pm_ops->affinst_off(read_mpidr_el1(), cpu_node->level, plat_state); } @@ -106,7 +106,7 @@ return rc; } -static int psci_afflvl1_off(unsigned long mpidr, aff_map_node_t *cluster_node) +static int psci_afflvl1_off(aff_map_node_t *cluster_node) { int rc = PSCI_E_SUCCESS; unsigned int plat_state; @@ -136,14 +136,14 @@ * program the power controller etc. */ if (psci_plat_pm_ops->affinst_off) - rc = psci_plat_pm_ops->affinst_off(mpidr, + rc = psci_plat_pm_ops->affinst_off(read_mpidr_el1(), cluster_node->level, plat_state); return rc; } -static int psci_afflvl2_off(unsigned long mpidr, aff_map_node_t *system_node) +static int psci_afflvl2_off(aff_map_node_t *system_node) { int rc = PSCI_E_SUCCESS; unsigned int plat_state; @@ -167,7 +167,7 @@ * at this affinity level */ if (psci_plat_pm_ops->affinst_off) - rc = psci_plat_pm_ops->affinst_off(mpidr, + rc = psci_plat_pm_ops->affinst_off(read_mpidr_el1(), system_node->level, plat_state); return rc; @@ -186,8 +186,7 @@ ******************************************************************************/ static int psci_call_off_handlers(mpidr_aff_map_nodes_t mpidr_nodes, int start_afflvl, - int end_afflvl, - unsigned long mpidr) + int end_afflvl) { int rc = PSCI_E_INVALID_PARAMS, level; aff_map_node_t *node; @@ -202,7 +201,7 @@ * of restoring what we might have torn down at * lower affinity levels. */ - rc = psci_afflvl_off_handlers[level](mpidr, node); + rc = psci_afflvl_off_handlers[level](node); if (rc != PSCI_E_SUCCESS) break; } @@ -232,14 +231,12 @@ * CAUTION: This function is called with coherent stacks so that coherency can * be turned off and caches can be flushed safely. ******************************************************************************/ -int psci_afflvl_off(unsigned long mpidr, - int start_afflvl, +int psci_afflvl_off(int start_afflvl, int end_afflvl) { int rc = PSCI_E_SUCCESS; mpidr_aff_map_nodes_t mpidr_nodes; - mpidr &= MPIDR_AFFINITY_MASK;; /* * Collect the pointers to the nodes in the topology tree for @@ -248,7 +245,7 @@ * levels are incorrect. In either case, we cannot return back * to the caller as it would not know what to do. */ - rc = psci_get_aff_map_nodes(mpidr, + rc = psci_get_aff_map_nodes(read_mpidr_el1() & MPIDR_AFFINITY_MASK, start_afflvl, end_afflvl, mpidr_nodes); @@ -259,23 +256,20 @@ * level so that by the time all locks are taken, the system topology * is snapshot and state management can be done safely. */ - psci_acquire_afflvl_locks(mpidr, - start_afflvl, + psci_acquire_afflvl_locks(start_afflvl, end_afflvl, mpidr_nodes); /* Perform generic, architecture and platform specific handling */ rc = psci_call_off_handlers(mpidr_nodes, start_afflvl, - end_afflvl, - mpidr); + end_afflvl); /* * Release the locks corresponding to each affinity level in the * reverse order to which they were acquired. */ - psci_release_afflvl_locks(mpidr, - start_afflvl, + psci_release_afflvl_locks(start_afflvl, end_afflvl, mpidr_nodes); diff --git a/services/std_svc/psci/psci_afflvl_on.c b/services/std_svc/psci/psci_afflvl_on.c index d91db96..d620172 100644 --- a/services/std_svc/psci/psci_afflvl_on.c +++ b/services/std_svc/psci/psci_afflvl_on.c @@ -285,7 +285,6 @@ { int rc = PSCI_E_SUCCESS; mpidr_aff_map_nodes_t target_cpu_nodes; - unsigned long mpidr = read_mpidr() & MPIDR_AFFINITY_MASK; /* * Collect the pointers to the nodes in the topology tree for @@ -306,8 +305,7 @@ * level so that by the time all locks are taken, the system topology * is snapshot and state management can be done safely. */ - psci_acquire_afflvl_locks(mpidr, - start_afflvl, + psci_acquire_afflvl_locks(start_afflvl, end_afflvl, target_cpu_nodes); @@ -323,8 +321,7 @@ * This loop releases the lock corresponding to each affinity level * in the reverse order to which they were acquired. */ - psci_release_afflvl_locks(mpidr, - start_afflvl, + psci_release_afflvl_locks(start_afflvl, end_afflvl, target_cpu_nodes); @@ -335,8 +332,7 @@ * The following functions finish an earlier affinity power on request. They * are called by the common finisher routine in psci_common.c. ******************************************************************************/ -static unsigned int psci_afflvl0_on_finish(unsigned long mpidr, - aff_map_node_t *cpu_node) +static unsigned int psci_afflvl0_on_finish(aff_map_node_t *cpu_node) { unsigned int plat_state, state, rc; @@ -356,7 +352,7 @@ /* Get the physical state of this cpu */ plat_state = get_phys_state(state); - rc = psci_plat_pm_ops->affinst_on_finish(mpidr, + rc = psci_plat_pm_ops->affinst_on_finish(read_mpidr_el1(), cpu_node->level, plat_state); assert(rc == PSCI_E_SUCCESS); @@ -399,8 +395,7 @@ return rc; } -static unsigned int psci_afflvl1_on_finish(unsigned long mpidr, - aff_map_node_t *cluster_node) +static unsigned int psci_afflvl1_on_finish(aff_map_node_t *cluster_node) { unsigned int plat_state, rc = PSCI_E_SUCCESS; @@ -418,7 +413,7 @@ /* Get the physical state of this cluster */ plat_state = psci_get_phys_state(cluster_node); - rc = psci_plat_pm_ops->affinst_on_finish(mpidr, + rc = psci_plat_pm_ops->affinst_on_finish(read_mpidr_el1(), cluster_node->level, plat_state); assert(rc == PSCI_E_SUCCESS); @@ -431,8 +426,7 @@ } -static unsigned int psci_afflvl2_on_finish(unsigned long mpidr, - aff_map_node_t *system_node) +static unsigned int psci_afflvl2_on_finish(aff_map_node_t *system_node) { unsigned int plat_state, rc = PSCI_E_SUCCESS; @@ -456,7 +450,7 @@ /* Get the physical state of the system */ plat_state = psci_get_phys_state(system_node); - rc = psci_plat_pm_ops->affinst_on_finish(mpidr, + rc = psci_plat_pm_ops->affinst_on_finish(read_mpidr_el1(), system_node->level, plat_state); assert(rc == PSCI_E_SUCCESS); diff --git a/services/std_svc/psci/psci_afflvl_suspend.c b/services/std_svc/psci/psci_afflvl_suspend.c index ea90389..0977198 100644 --- a/services/std_svc/psci/psci_afflvl_suspend.c +++ b/services/std_svc/psci/psci_afflvl_suspend.c @@ -38,8 +38,7 @@ #include #include "psci_private.h" -typedef int (*afflvl_suspend_handler_t)(unsigned long, - aff_map_node_t *, +typedef int (*afflvl_suspend_handler_t)(aff_map_node_t *, unsigned long, unsigned long, unsigned int); @@ -121,8 +120,7 @@ * The next three functions implement a handler for each supported affinity * level which is called when that affinity level is about to be suspended. ******************************************************************************/ -static int psci_afflvl0_suspend(unsigned long mpidr, - aff_map_node_t *cpu_node, +static int psci_afflvl0_suspend(aff_map_node_t *cpu_node, unsigned long ns_entrypoint, unsigned long context_id, unsigned int power_state) @@ -214,7 +212,7 @@ if (psci_plat_pm_ops->affinst_suspend) { plat_state = psci_get_phys_state(cpu_node); - rc = psci_plat_pm_ops->affinst_suspend(mpidr, + rc = psci_plat_pm_ops->affinst_suspend(read_mpidr_el1(), psci_entrypoint, ns_entrypoint, cpu_node->level, @@ -224,8 +222,7 @@ return rc; } -static int psci_afflvl1_suspend(unsigned long mpidr, - aff_map_node_t *cluster_node, +static int psci_afflvl1_suspend(aff_map_node_t *cluster_node, unsigned long ns_entrypoint, unsigned long context_id, unsigned int power_state) @@ -267,7 +264,7 @@ * platform handler prototype the same. */ psci_entrypoint = (unsigned long) psci_aff_suspend_finish_entry; - rc = psci_plat_pm_ops->affinst_suspend(mpidr, + rc = psci_plat_pm_ops->affinst_suspend(read_mpidr_el1(), psci_entrypoint, ns_entrypoint, cluster_node->level, @@ -278,8 +275,7 @@ } -static int psci_afflvl2_suspend(unsigned long mpidr, - aff_map_node_t *system_node, +static int psci_afflvl2_suspend(aff_map_node_t *system_node, unsigned long ns_entrypoint, unsigned long context_id, unsigned int power_state) @@ -313,7 +309,7 @@ * platform handler prototype the same. */ psci_entrypoint = (unsigned long) psci_aff_suspend_finish_entry; - rc = psci_plat_pm_ops->affinst_suspend(mpidr, + rc = psci_plat_pm_ops->affinst_suspend(read_mpidr_el1(), psci_entrypoint, ns_entrypoint, system_node->level, @@ -337,7 +333,6 @@ static int psci_call_suspend_handlers(mpidr_aff_map_nodes_t mpidr_nodes, int start_afflvl, int end_afflvl, - unsigned long mpidr, unsigned long entrypoint, unsigned long context_id, unsigned int power_state) @@ -355,8 +350,7 @@ * of restoring what we might have torn down at * lower affinity levels. */ - rc = psci_afflvl_suspend_handlers[level](mpidr, - node, + rc = psci_afflvl_suspend_handlers[level](node, entrypoint, context_id, power_state); @@ -389,8 +383,7 @@ * CAUTION: This function is called with coherent stacks so that coherency can * be turned off and caches can be flushed safely. ******************************************************************************/ -int psci_afflvl_suspend(unsigned long mpidr, - unsigned long entrypoint, +int psci_afflvl_suspend(unsigned long entrypoint, unsigned long context_id, unsigned int power_state, int start_afflvl, @@ -399,15 +392,13 @@ int rc = PSCI_E_SUCCESS; mpidr_aff_map_nodes_t mpidr_nodes; - mpidr &= MPIDR_AFFINITY_MASK; - /* * Collect the pointers to the nodes in the topology tree for * each affinity instance in the mpidr. If this function does * not return successfully then either the mpidr or the affinity * levels are incorrect. */ - rc = psci_get_aff_map_nodes(mpidr, + rc = psci_get_aff_map_nodes(read_mpidr_el1() & MPIDR_AFFINITY_MASK, start_afflvl, end_afflvl, mpidr_nodes); @@ -419,8 +410,7 @@ * level so that by the time all locks are taken, the system topology * is snapshot and state management can be done safely. */ - psci_acquire_afflvl_locks(mpidr, - start_afflvl, + psci_acquire_afflvl_locks(start_afflvl, end_afflvl, mpidr_nodes); @@ -428,7 +418,6 @@ rc = psci_call_suspend_handlers(mpidr_nodes, start_afflvl, end_afflvl, - mpidr, entrypoint, context_id, power_state); @@ -437,8 +426,7 @@ * Release the locks corresponding to each affinity level in the * reverse order to which they were acquired. */ - psci_release_afflvl_locks(mpidr, - start_afflvl, + psci_release_afflvl_locks(start_afflvl, end_afflvl, mpidr_nodes); @@ -449,8 +437,7 @@ * The following functions finish an earlier affinity suspend request. They * are called by the common finisher routine in psci_common.c. ******************************************************************************/ -static unsigned int psci_afflvl0_suspend_finish(unsigned long mpidr, - aff_map_node_t *cpu_node) +static unsigned int psci_afflvl0_suspend_finish(aff_map_node_t *cpu_node) { unsigned int plat_state, state, rc; int32_t suspend_level; @@ -472,7 +459,7 @@ /* Get the physical state of this cpu */ plat_state = get_phys_state(state); - rc = psci_plat_pm_ops->affinst_suspend_finish(mpidr, + rc = psci_plat_pm_ops->affinst_suspend_finish(read_mpidr_el1(), cpu_node->level, plat_state); assert(rc == PSCI_E_SUCCESS); @@ -516,8 +503,7 @@ return rc; } -static unsigned int psci_afflvl1_suspend_finish(unsigned long mpidr, - aff_map_node_t *cluster_node) +static unsigned int psci_afflvl1_suspend_finish(aff_map_node_t *cluster_node) { unsigned int plat_state, rc = PSCI_E_SUCCESS; @@ -535,7 +521,7 @@ /* Get the physical state of this cpu */ plat_state = psci_get_phys_state(cluster_node); - rc = psci_plat_pm_ops->affinst_suspend_finish(mpidr, + rc = psci_plat_pm_ops->affinst_suspend_finish(read_mpidr_el1(), cluster_node->level, plat_state); assert(rc == PSCI_E_SUCCESS); @@ -548,8 +534,7 @@ } -static unsigned int psci_afflvl2_suspend_finish(unsigned long mpidr, - aff_map_node_t *system_node) +static unsigned int psci_afflvl2_suspend_finish(aff_map_node_t *system_node) { unsigned int plat_state, rc = PSCI_E_SUCCESS;; @@ -573,7 +558,7 @@ /* Get the physical state of the system */ plat_state = psci_get_phys_state(system_node); - rc = psci_plat_pm_ops->affinst_suspend_finish(mpidr, + rc = psci_plat_pm_ops->affinst_suspend_finish(read_mpidr_el1(), system_node->level, plat_state); assert(rc == PSCI_E_SUCCESS); diff --git a/services/std_svc/psci/psci_common.c b/services/std_svc/psci/psci_common.c index ed29fbe..3c79a5e 100644 --- a/services/std_svc/psci/psci_common.c +++ b/services/std_svc/psci/psci_common.c @@ -156,8 +156,7 @@ * topology tree for an mpidr. It picks up locks for each affinity level bottom * up in the range specified. ******************************************************************************/ -void psci_acquire_afflvl_locks(unsigned long mpidr, - int start_afflvl, +void psci_acquire_afflvl_locks(int start_afflvl, int end_afflvl, mpidr_aff_map_nodes_t mpidr_nodes) { @@ -175,8 +174,7 @@ * topology tree for an mpidr. It releases the lock for each affinity level top * down in the range specified. ******************************************************************************/ -void psci_release_afflvl_locks(unsigned long mpidr, - int start_afflvl, +void psci_release_afflvl_locks(int start_afflvl, int end_afflvl, mpidr_aff_map_nodes_t mpidr_nodes) { @@ -353,8 +351,7 @@ static int psci_call_power_on_handlers(mpidr_aff_map_nodes_t mpidr_nodes, int start_afflvl, int end_afflvl, - afflvl_power_on_finisher_t *pon_handlers, - unsigned long mpidr) + afflvl_power_on_finisher_t *pon_handlers) { int rc = PSCI_E_INVALID_PARAMS, level; aff_map_node_t *node; @@ -370,7 +367,7 @@ * so simply return an error and let the caller take * care of the situation. */ - rc = pon_handlers[level](mpidr, node); + rc = pon_handlers[level](node); if (rc != PSCI_E_SUCCESS) break; } @@ -397,23 +394,20 @@ * CAUTION: This function is called with coherent stacks so that coherency and * the mmu can be turned on safely. ******************************************************************************/ -void psci_afflvl_power_on_finish(unsigned long mpidr, - int start_afflvl, +void psci_afflvl_power_on_finish(int start_afflvl, int end_afflvl, afflvl_power_on_finisher_t *pon_handlers) { mpidr_aff_map_nodes_t mpidr_nodes; int rc; - mpidr &= MPIDR_AFFINITY_MASK; - /* * Collect the pointers to the nodes in the topology tree for * each affinity instance in the mpidr. If this function does * not return successfully then either the mpidr or the affinity * levels are incorrect. Either case is an irrecoverable error. */ - rc = psci_get_aff_map_nodes(mpidr, + rc = psci_get_aff_map_nodes(read_mpidr_el1() & MPIDR_AFFINITY_MASK, start_afflvl, end_afflvl, mpidr_nodes); @@ -425,8 +419,7 @@ * level so that by the time all locks are taken, the system topology * is snapshot and state management can be done safely. */ - psci_acquire_afflvl_locks(mpidr, - start_afflvl, + psci_acquire_afflvl_locks(start_afflvl, end_afflvl, mpidr_nodes); @@ -434,8 +427,7 @@ rc = psci_call_power_on_handlers(mpidr_nodes, start_afflvl, end_afflvl, - pon_handlers, - mpidr); + pon_handlers); if (rc != PSCI_E_SUCCESS) panic(); @@ -443,8 +435,7 @@ * This loop releases the lock corresponding to each affinity level * in the reverse order to which they were acquired. */ - psci_release_afflvl_locks(mpidr, - start_afflvl, + psci_release_afflvl_locks(start_afflvl, end_afflvl, mpidr_nodes); } diff --git a/services/std_svc/psci/psci_entry.S b/services/std_svc/psci/psci_entry.S index 5628d79..1ffde06 100644 --- a/services/std_svc/psci/psci_entry.S +++ b/services/std_svc/psci/psci_entry.S @@ -58,8 +58,6 @@ adr x23, psci_afflvl_suspend_finishers psci_aff_common_finish_entry: - adr x22, psci_afflvl_power_on_finish - /* --------------------------------------------- * Initialise the pcpu cache pointer for the CPU * --------------------------------------------- @@ -92,11 +90,10 @@ bl get_power_on_target_afflvl cmp x0, xzr b.lt _panic - mov x3, x23 - mov x2, x0 - mov x1, #MPIDR_AFFLVL0 - mrs x0, mpidr_el1 - blr x22 + mov x2, x23 + mov x1, x0 + mov x0, #MPIDR_AFFLVL0 + bl psci_afflvl_power_on_finish /* -------------------------------------------- * Give ourselves a stack allocated in Normal diff --git a/services/std_svc/psci/psci_main.c b/services/std_svc/psci/psci_main.c index 2d7b018..d68f3d0 100644 --- a/services/std_svc/psci/psci_main.c +++ b/services/std_svc/psci/psci_main.c @@ -78,7 +78,6 @@ unsigned long context_id) { int rc; - unsigned long mpidr; unsigned int target_afflvl, pstate_type; /* Check SBZ bits in power state are zero */ @@ -111,9 +110,7 @@ * enter the final wfi which will power down this cpu else return * an error. */ - mpidr = read_mpidr(); - rc = psci_afflvl_suspend(mpidr, - entrypoint, + rc = psci_afflvl_suspend(entrypoint, context_id, power_state, MPIDR_AFFLVL0, @@ -127,18 +124,15 @@ int psci_cpu_off(void) { int rc; - unsigned long mpidr; int target_afflvl = get_max_afflvl(); - mpidr = read_mpidr(); - /* * Traverse from the highest to the lowest affinity level. When the * lowest affinity level is hit, all the locks are acquired. State * management is done immediately followed by cpu, cluster ... * ..target_afflvl specific actions as this function unwinds back. */ - rc = psci_afflvl_off(mpidr, MPIDR_AFFLVL0, target_afflvl); + rc = psci_afflvl_off(MPIDR_AFFLVL0, target_afflvl); /* * Check if all actions needed to safely power down this cpu have diff --git a/services/std_svc/psci/psci_private.h b/services/std_svc/psci/psci_private.h index f534087..06db63f 100644 --- a/services/std_svc/psci/psci_private.h +++ b/services/std_svc/psci/psci_private.h @@ -62,8 +62,7 @@ } aff_limits_node_t; typedef aff_map_node_t (*mpidr_aff_map_nodes_t[MPIDR_MAX_AFFLVL]); -typedef unsigned int (*afflvl_power_on_finisher_t)(unsigned long, - aff_map_node_t *); +typedef unsigned int (*afflvl_power_on_finisher_t)(aff_map_node_t *); /******************************************************************************* * Data prototypes @@ -87,20 +86,17 @@ 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); -void psci_afflvl_power_on_finish(unsigned long, - int, +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_check_afflvl_range(int start_afflvl, int end_afflvl); -void psci_acquire_afflvl_locks(unsigned long mpidr, - int start_afflvl, +void psci_acquire_afflvl_locks(int start_afflvl, int end_afflvl, mpidr_aff_map_nodes_t mpidr_nodes); -void psci_release_afflvl_locks(unsigned long mpidr, - int start_afflvl, +void psci_release_afflvl_locks(int start_afflvl, int end_afflvl, mpidr_aff_map_nodes_t mpidr_nodes); @@ -119,7 +115,7 @@ int); /* Private exported functions from psci_affinity_off.c */ -int psci_afflvl_off(unsigned long, int, int); +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, @@ -127,11 +123,10 @@ int psci_get_aff_map_node_suspend_afflvl(aff_map_node_t *node); int psci_afflvl_suspend(unsigned long, unsigned long, - unsigned long, unsigned int, int, int); -unsigned int psci_afflvl_suspend_finish(unsigned long, int, int); +unsigned int psci_afflvl_suspend_finish(int, int); #endif /* __PSCI_PRIVATE_H__ */