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_suspend.c b/common/psci/psci_afflvl_suspend.c
index 810075b..186f048 100644
--- a/common/psci/psci_afflvl_suspend.c
+++ b/common/psci/psci_afflvl_suspend.c
@@ -226,112 +226,149 @@
};
/*******************************************************************************
- * This function implements the core of the processing required to suspend a cpu
- * It'S assumed that along with suspending the cpu, higher affinity levels will
- * be suspended as far as possible. Suspending a cpu is equivalent to physically
- * powering it down, but the cpu is still available to the OS for scheduling.
- * We first need to determine the new state off all the affinity instances in
- * the mpidr corresponding to the target cpu. Action will be taken on the basis
- * of this new state. To do the state change we first need to acquire the locks
- * for all the implemented affinity level to be able to snapshot the system
- * state. Then we need to start suspending affinity levels from the lowest to
- * the highest (e.g. a cpu needs to be suspended before a cluster can be). To
- * achieve this flow, we start acquiring the locks from the highest to the
- * lowest affinity level. Once we reach affinity level 0, we do the state change
- * followed by the actions corresponding to the new state for affinity level 0.
- * Actions as per the updated state for higher affinity levels are performed as
- * we unwind back to highest affinity level.
+ * This function takes an array of pointers to affinity instance nodes in the
+ * topology tree and calls the suspend handler for the corresponding affinity
+ * levels
+ ******************************************************************************/
+static int psci_call_suspend_handlers(mpidr_aff_map_nodes mpidr_nodes,
+ int start_afflvl,
+ int end_afflvl,
+ unsigned long mpidr,
+ unsigned long entrypoint,
+ unsigned long context_id,
+ unsigned int power_state)
+{
+ int rc = PSCI_E_INVALID_PARAMS, level;
+ aff_map_node *node;
+
+ for (level = start_afflvl; level <= end_afflvl; level++) {
+ node = mpidr_nodes[level];
+ if (node == NULL)
+ continue;
+
+ /*
+ * TODO: In case of an error should there be a way
+ * of restoring what we might have torn down at
+ * lower affinity levels.
+ */
+ rc = psci_afflvl_suspend_handlers[level](mpidr,
+ node,
+ entrypoint,
+ context_id,
+ power_state);
+ if (rc != PSCI_E_SUCCESS)
+ break;
+ }
+
+ return rc;
+}
+
+/*******************************************************************************
+ * Top level handler which is called when a cpu wants to suspend its execution.
+ * It is assumed that along with turning the cpu off, higher affinity levels
+ * until the target affinity level will be turned off as well. It traverses
+ * through all the affinity levels performing generic, architectural, platform
+ * setup and state management e.g. for a cluster that's to be suspended, it will
+ * call the platform specific code which will disable coherency at the
+ * interconnect level if the cpu is the last in the cluster. For a cpu it could
+ * mean programming the power controller etc.
+ *
+ * The state of all the relevant affinity levels is changed prior to calling the
+ * affinity level specific handlers as their actions would depend upon the state
+ * the affinity level is about to enter.
+ *
+ * The affinity level specific handlers are called in ascending order i.e. from
+ * the lowest to the highest affinity level implemented by the platform because
+ * to turn off affinity level X it is neccesary to turn off affinity level X - 1
+ * first.
+ *
+ * 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,
unsigned long context_id,
unsigned int power_state,
- int cur_afflvl,
- int tgt_afflvl)
+ int start_afflvl,
+ int end_afflvl)
{
- int rc = PSCI_E_SUCCESS, level;
- unsigned int prev_state, next_state;
- aff_map_node *aff_node;
+ int rc = PSCI_E_SUCCESS;
+ unsigned int prev_state;
+ mpidr_aff_map_nodes mpidr_nodes;
mpidr &= MPIDR_AFFINITY_MASK;
/*
- * Some affinity instances at levels between the current and
- * target levels could be absent in the mpidr. Skip them and
- * start from the first present instance.
+ * 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.
*/
- level = psci_get_first_present_afflvl(mpidr,
- cur_afflvl,
- tgt_afflvl,
- &aff_node);
+ rc = psci_get_aff_map_nodes(mpidr,
+ start_afflvl,
+ end_afflvl,
+ mpidr_nodes);
+ if (rc != PSCI_E_SUCCESS)
+ return rc;
/*
- * Return if there are no more affinity instances beyond this
- * level to process. Else ensure that the returned affinity
- * node makes sense.
+ * 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.
*/
- if (aff_node == NULL)
- return rc;
-
- assert(level == aff_node->level);
+ psci_acquire_afflvl_locks(mpidr,
+ start_afflvl,
+ end_afflvl,
+ mpidr_nodes);
/*
- * This function acquires the lock corresponding to each
- * affinity level so that state management can be done safely.
+ * Keep the old cpu state handy. It will be used to restore the
+ * system to its original state in case something goes wrong
*/
- bakery_lock_get(mpidr, &aff_node->lock);
-
- /* Keep the old state and the next one handy */
- prev_state = psci_get_state(aff_node->state);
- next_state = PSCI_STATE_SUSPEND;
+ prev_state = psci_get_state(mpidr_nodes[MPIDR_AFFLVL0]->state);
/*
- * We start from the highest affinity level and work our way
- * downwards to the lowest i.e. MPIDR_AFFLVL0.
+ * State management: Update the state of each affinity instance
+ * between the start and end affinity levels
*/
- if (aff_node->level == tgt_afflvl) {
- psci_change_state(mpidr,
- tgt_afflvl,
- get_max_afflvl(),
- next_state);
- } else {
- rc = psci_afflvl_suspend(mpidr,
- entrypoint,
- context_id,
- power_state,
- level - 1,
- tgt_afflvl);
- if (rc != PSCI_E_SUCCESS) {
- psci_set_state(aff_node->state, prev_state);
- goto exit;
- }
- }
+ psci_change_state(mpidr_nodes,
+ start_afflvl,
+ end_afflvl,
+ PSCI_STATE_SUSPEND);
+
+ /* Perform generic, architecture and platform specific handling */
+ rc = psci_call_suspend_handlers(mpidr_nodes,
+ start_afflvl,
+ end_afflvl,
+ mpidr,
+ entrypoint,
+ context_id,
+ power_state);
/*
- * Perform generic, architecture and platform specific
- * handling
+ * If an error is returned by a handler then restore the cpu state
+ * to its original value. If the cpu state is restored then that
+ * should result in the state of the higher affinity levels to
+ * get restored as well.
+ * TODO: We are not undoing any architectural or platform specific
+ * operations that might have completed before encountering the
+ * error. The system might not be in a stable state.
*/
- rc = psci_afflvl_suspend_handlers[level](mpidr,
- aff_node,
- entrypoint,
- context_id,
- power_state);
- if (rc != PSCI_E_SUCCESS) {
- psci_set_state(aff_node->state, prev_state);
- goto exit;
- }
+ if (rc != PSCI_E_SUCCESS)
+ psci_change_state(mpidr_nodes,
+ start_afflvl,
+ end_afflvl,
+ prev_state);
/*
- * If all has gone as per plan then this cpu should be
- * marked as OFF
+ * Release the locks corresponding to each affinity level in the
+ * reverse order to which they were acquired.
*/
- if (level == MPIDR_AFFLVL0) {
- next_state = psci_get_state(aff_node->state);
- assert(next_state == PSCI_STATE_SUSPEND);
- }
+ psci_release_afflvl_locks(mpidr,
+ start_afflvl,
+ end_afflvl,
+ mpidr_nodes);
-exit:
- bakery_lock_release(mpidr, &aff_node->lock);
return rc;
}
@@ -340,13 +377,16 @@
* are called by the common finisher routine in psci_common.c.
******************************************************************************/
static unsigned int psci_afflvl0_suspend_finish(unsigned long mpidr,
- aff_map_node *cpu_node,
- unsigned int prev_state)
+ aff_map_node *cpu_node)
{
- unsigned int index, plat_state, rc = 0;
+ unsigned int index, plat_state, state, rc = PSCI_E_SUCCESS;
assert(cpu_node->level == MPIDR_AFFLVL0);
+ /* Ensure we have been woken up from a suspended state */
+ state = psci_get_state(cpu_node->state);
+ assert(state == PSCI_STATE_SUSPEND);
+
/*
* Plat. management: Perform the platform specific actions
* before we change the state of the cpu e.g. enabling the
@@ -355,7 +395,9 @@
* situation.
*/
if (psci_plat_pm_ops->affinst_suspend_finish) {
- 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_suspend_finish(mpidr,
cpu_node->level,
plat_state);
@@ -396,11 +438,9 @@
}
static unsigned int psci_afflvl1_suspend_finish(unsigned long mpidr,
- aff_map_node *cluster_node,
- unsigned int prev_state)
+ aff_map_node *cluster_node)
{
- unsigned int rc = 0;
- unsigned int plat_state;
+ unsigned int plat_state, rc = PSCI_E_SUCCESS;
assert(cluster_node->level == MPIDR_AFFLVL1);
@@ -413,7 +453,9 @@
* situation.
*/
if (psci_plat_pm_ops->affinst_suspend_finish) {
- plat_state = psci_get_phys_state(prev_state);
+
+ /* Get the physical state of this cpu */
+ plat_state = psci_get_aff_phys_state(cluster_node);
rc = psci_plat_pm_ops->affinst_suspend_finish(mpidr,
cluster_node->level,
plat_state);
@@ -425,11 +467,9 @@
static unsigned int psci_afflvl2_suspend_finish(unsigned long mpidr,
- aff_map_node *system_node,
- unsigned int target_afflvl)
+ 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);
@@ -448,7 +488,9 @@
* situation.
*/
if (psci_plat_pm_ops->affinst_suspend_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_suspend_finish(mpidr,
system_node->level,
plat_state);