psci: rectify and homogenise generic code
This patch performs a major rework of the psci generic implementation
to achieve the following:
1. replace recursion with iteration where possible to aid code
readability e.g. affinity instance states are changed iteratively
instead of recursively.
2. acquire pointers to affinity instance nodes at the beginning of a
psci operation. All subsequent actions use these pointers instead
of calling psci_get_aff_map_node() repeatedly e.g. management of
locks has been abstracted under functions which use these pointers
to ensure correct ordering. Helper functions have been added to
create these abstractions.
3. assertions have been added to cpu level handlers to ensure correct
state transition
4. the affinity level extents specified to various functions have the
same meaning i.e. start level is always less than the end level.
Change-Id: If0508c3a7b20ea3ddda2a66128429382afc3dfc8
diff --git a/common/psci/psci_afflvl_on.c b/common/psci/psci_afflvl_on.c
index 81d46bf..c9c3b2c 100644
--- a/common/psci/psci_afflvl_on.c
+++ b/common/psci/psci_afflvl_on.c
@@ -207,84 +207,119 @@
};
/*******************************************************************************
- * This function implements the core of the processing required to turn a cpu
- * on. It avoids recursion to traverse from the lowest to the highest affinity
- * level unlike the off/suspend/pon_finisher functions. It does ensure that the
- * locks are picked in the same order as the order routines to avoid deadlocks.
- * The flow is: Take all the locks until the highest affinity level, Call the
- * handlers for turning an affinity level on & finally change the state of the
- * affinity level.
+ * This function takes an array of pointers to affinity instance nodes in the
+ * topology tree and calls the on handler for the corresponding affinity
+ * levels
+ ******************************************************************************/
+static int psci_call_on_handlers(mpidr_aff_map_nodes target_cpu_nodes,
+ int start_afflvl,
+ int end_afflvl,
+ unsigned long target_cpu,
+ unsigned long entrypoint,
+ unsigned long context_id)
+{
+ int rc = PSCI_E_INVALID_PARAMS, level;
+ aff_map_node *node;
+
+ for (level = end_afflvl; level >= start_afflvl; level--) {
+ node = target_cpu_nodes[level];
+ if (node == NULL)
+ continue;
+
+ /*
+ * TODO: In case of an error should there be a way
+ * of undoing what we might have setup at higher
+ * affinity levels.
+ */
+ rc = psci_afflvl_on_handlers[level](target_cpu,
+ node,
+ entrypoint,
+ context_id);
+ if (rc != PSCI_E_SUCCESS)
+ break;
+ }
+
+ return rc;
+}
+
+/*******************************************************************************
+ * Generic handler which is called to physically power on a cpu identified by
+ * its mpidr. It traverses through all the affinity levels performing generic,
+ * architectural, platform setup and state management e.g. for a cpu that is
+ * to be powered on, it will ensure that enough information is stashed for it
+ * to resume execution in the non-secure security state.
+ *
+ * The state of all the relevant affinity levels is changed after calling the
+ * affinity level specific handlers as their actions would depend upon the state
+ * the affinity level is currently in.
+ *
+ * The affinity level specific handlers are called in descending order i.e. from
+ * the highest to the lowest affinity level implemented by the platform because
+ * to turn on affinity level X it is neccesary to turn on affinity level X + 1
+ * first.
******************************************************************************/
int psci_afflvl_on(unsigned long target_cpu,
unsigned long entrypoint,
unsigned long context_id,
- int current_afflvl,
- int target_afflvl)
+ int start_afflvl,
+ int end_afflvl)
{
- unsigned int prev_state, next_state;
- int rc = PSCI_E_SUCCESS, level;
- aff_map_node *aff_node;
+ int rc = PSCI_E_SUCCESS;
+ mpidr_aff_map_nodes target_cpu_nodes;
unsigned long mpidr = read_mpidr() & MPIDR_AFFINITY_MASK;
/*
- * This loop acquires the lock corresponding to each
- * affinity level so that by the time we hit the lowest
- * affinity level, the system topology is snapshot and
- * state management can be done safely.
+ * 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.
*/
- for (level = current_afflvl; level >= target_afflvl; level--) {
- aff_node = psci_get_aff_map_node(target_cpu, level);
- if (aff_node)
- bakery_lock_get(mpidr, &aff_node->lock);
- }
+ rc = psci_get_aff_map_nodes(target_cpu,
+ start_afflvl,
+ end_afflvl,
+ target_cpu_nodes);
+ if (rc != PSCI_E_SUCCESS)
+ return rc;
+
/*
- * Perform generic, architecture and platform specific
- * handling
+ * This function acquires the lock corresponding to each affinity
+ * level so that by the time all locks are taken, the system topology
+ * is snapshot and state management can be done safely.
*/
- for (level = current_afflvl; level >= target_afflvl; level--) {
+ psci_acquire_afflvl_locks(mpidr,
+ start_afflvl,
+ end_afflvl,
+ target_cpu_nodes);
- /* Grab the node for each affinity level once again */
- aff_node = psci_get_aff_map_node(target_cpu, level);
- if (aff_node) {
-
- /* Keep the old state and the next one handy */
- prev_state = psci_get_state(aff_node->state);
- rc = psci_afflvl_on_handlers[level](target_cpu,
- aff_node,
- entrypoint,
- context_id);
- if (rc != PSCI_E_SUCCESS) {
- psci_set_state(aff_node->state, prev_state);
- goto exit;
- }
- }
- }
+ /* Perform generic, architecture and platform specific handling. */
+ rc = psci_call_on_handlers(target_cpu_nodes,
+ start_afflvl,
+ end_afflvl,
+ target_cpu,
+ entrypoint,
+ context_id);
+ if (rc != PSCI_E_SUCCESS)
+ goto exit;
/*
- * State management: Update the states since this is the
- * target affinity level requested.
+ * State management: Update the state of each affinity instance
+ * between the start and end affinity levels
*/
- psci_change_state(target_cpu,
- target_afflvl,
- get_max_afflvl(),
+ psci_change_state(target_cpu_nodes,
+ start_afflvl,
+ end_afflvl,
PSCI_STATE_ON_PENDING);
exit:
/*
* This loop releases the lock corresponding to each affinity level
- * in the reverse order. It also checks the final state of the cpu.
+ * in the reverse order to which they were acquired.
*/
- for (level = target_afflvl; level <= current_afflvl; level++) {
- aff_node = psci_get_aff_map_node(target_cpu, level);
- if (aff_node) {
- if (level == MPIDR_AFFLVL0) {
- next_state = psci_get_state(aff_node->state);
- assert(next_state == PSCI_STATE_ON_PENDING);
- }
- bakery_lock_release(mpidr, &aff_node->lock);
- }
- }
+ psci_release_afflvl_locks(mpidr,
+ start_afflvl,
+ end_afflvl,
+ target_cpu_nodes);
return rc;
}
@@ -294,13 +329,16 @@
* are called by the common finisher routine in psci_common.c.
******************************************************************************/
static unsigned int psci_afflvl0_on_finish(unsigned long mpidr,
- aff_map_node *cpu_node,
- unsigned int prev_state)
+ aff_map_node *cpu_node)
{
- unsigned int index, plat_state, rc = PSCI_E_SUCCESS;
+ unsigned int index, plat_state, state, rc = PSCI_E_SUCCESS;
assert(cpu_node->level == MPIDR_AFFLVL0);
+ /* Ensure we have been explicitly woken up by another cpu */
+ state = psci_get_state(cpu_node->state);
+ assert(state == PSCI_STATE_ON_PENDING);
+
/*
* Plat. management: Perform the platform specific actions
* for this cpu e.g. enabling the gic or zeroing the mailbox
@@ -309,8 +347,8 @@
*/
if (psci_plat_pm_ops->affinst_on_finish) {
- /* Get the previous physical state of this cpu */
- plat_state = psci_get_phys_state(prev_state);
+ /* Get the physical state of this cpu */
+ plat_state = psci_get_phys_state(state);
rc = psci_plat_pm_ops->affinst_on_finish(mpidr,
cpu_node->level,
plat_state);
@@ -346,11 +384,9 @@
}
static unsigned int psci_afflvl1_on_finish(unsigned long mpidr,
- aff_map_node *cluster_node,
- unsigned int prev_state)
+ aff_map_node *cluster_node)
{
- unsigned int rc = PSCI_E_SUCCESS;
- unsigned int plat_state;
+ unsigned int plat_state, rc = PSCI_E_SUCCESS;
assert(cluster_node->level == MPIDR_AFFLVL1);
@@ -363,7 +399,9 @@
* situation.
*/
if (psci_plat_pm_ops->affinst_on_finish) {
- plat_state = psci_get_phys_state(prev_state);
+
+ /* Get the physical state of this cluster */
+ plat_state = psci_get_aff_phys_state(cluster_node);
rc = psci_plat_pm_ops->affinst_on_finish(mpidr,
cluster_node->level,
plat_state);
@@ -375,11 +413,9 @@
static unsigned int psci_afflvl2_on_finish(unsigned long mpidr,
- aff_map_node *system_node,
- unsigned int prev_state)
+ aff_map_node *system_node)
{
- int rc = PSCI_E_SUCCESS;
- unsigned int plat_state;
+ unsigned int plat_state, rc = PSCI_E_SUCCESS;
/* Cannot go beyond this affinity level */
assert(system_node->level == MPIDR_AFFLVL2);
@@ -398,7 +434,9 @@
* situation.
*/
if (psci_plat_pm_ops->affinst_on_finish) {
- plat_state = psci_get_phys_state(system_node->state);
+
+ /* Get the physical state of the system */
+ plat_state = psci_get_aff_phys_state(system_node);
rc = psci_plat_pm_ops->affinst_on_finish(mpidr,
system_node->level,
plat_state);