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

7 files changed, 601 insertions, 332 deletions

diff --git a/common/psci/psci_afflvl_off.c b/common/psci/psci_afflvl_off.c
index b62ae29..22685ba 100644
--- a/common/psci/psci_afflvl_off.c
+++ b/common/psci/psci_afflvl_off.c
@@ -162,97 +162,135 @@ static const afflvl_off_handler psci_afflvl_off_handlers[] = {
 };
 
 /*******************************************************************************
- * This function implements the core of the processing required to turn a cpu
- * off. It's assumed that along with turning the cpu off, higher affinity levels
- * will be turned off as far as possible. 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 turning
- * affinity levels off from the lowest to the highest (e.g. a cpu needs to be
- * off before a cluster can be turned off). 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 off handler for the corresponding affinity
+ * levels
+ ******************************************************************************/
+static int psci_call_off_handlers(mpidr_aff_map_nodes mpidr_nodes,
+				  int start_afflvl,
+				  int end_afflvl,
+				  unsigned long mpidr)
+{
+	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_off_handlers[level](mpidr, node);
+		if (rc != PSCI_E_SUCCESS)
+			break;
+	}
+
+	return rc;
+}
+
+/*******************************************************************************
+ * Top level handler which is called when a cpu wants to power itself down.
+ * It's assumed that along with turning the cpu off, higher affinity levels will
+ * be turned off as far as possible. It traverses through all the affinity
+ * levels performing generic, architectural, platform setup and state management
+ * e.g. for a cluster that's to be powered off, 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
+ * 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_off(unsigned long mpidr,
-		    int cur_afflvl,
-		    int tgt_afflvl)
+		    int start_afflvl,
+		    int end_afflvl)
 {
-	int rc = PSCI_E_SUCCESS, level;
-	unsigned int next_state, prev_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. In either case, we cannot return back
+	 * to the caller as it would not know what to do.
 	 */
-	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);
+	assert (rc == PSCI_E_SUCCESS);
+
 	/*
-	 * 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_OFF;
+	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_off(mpidr, 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_OFF);
+
+	/* Perform generic, architecture and platform specific handling */
+	rc = psci_call_off_handlers(mpidr_nodes,
+				    start_afflvl,
+				    end_afflvl,
+				    mpidr);
 
 	/*
-	 * 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_off_handlers[level](mpidr, aff_node);
-	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_OFF);
-	}
+	psci_release_afflvl_locks(mpidr,
+				  start_afflvl,
+				  end_afflvl,
+				  mpidr_nodes);
 
-exit:
-	bakery_lock_release(mpidr, &aff_node->lock);
 	return rc;
 }
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 @@ static const afflvl_on_handler psci_afflvl_on_handlers[] = {
 };
 
 /*******************************************************************************
- * 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--) {
-
-		/* 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;
-			}
-		}
-	}
+	psci_acquire_afflvl_locks(mpidr,
+				  start_afflvl,
+				  end_afflvl,
+				  target_cpu_nodes);
+
+	/* 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 @@ exit:
  * 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 @@ static unsigned int psci_afflvl0_on_finish(unsigned long mpidr,
 	 */
 	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_afflvl0_on_finish(unsigned long mpidr,
 }
 
 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 @@ static unsigned int psci_afflvl1_on_finish(unsigned long mpidr,
 	 * 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_afflvl1_on_finish(unsigned long mpidr,
 
 
 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 @@ static unsigned int psci_afflvl2_on_finish(unsigned long mpidr,
 	 * 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);
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 @@ static const afflvl_suspend_handler psci_afflvl_suspend_handlers[] = {
 };
 
 /*******************************************************************************
- * 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 @@ exit:
  * 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 @@ static unsigned int psci_afflvl0_suspend_finish(unsigned long mpidr,
 	 * 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_afflvl0_suspend_finish(unsigned long mpidr,
 }
 
 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 @@ static unsigned int psci_afflvl1_suspend_finish(unsigned long mpidr,
 	 * 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_afflvl1_suspend_finish(unsigned long mpidr,
 
 
 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 @@ static unsigned int psci_afflvl2_suspend_finish(unsigned long mpidr,
 	 * 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);
diff --git a/common/psci/psci_common.c b/common/psci/psci_common.c
index ba0a379..c37658b 100644
--- a/common/psci/psci_common.c
+++ b/common/psci/psci_common.c
@@ -111,6 +111,62 @@ unsigned long mpidr_set_aff_inst(unsigned long mpidr,
 }
 
 /*******************************************************************************
+ * This function sanity checks a range of affinity levels.
+ ******************************************************************************/
+int psci_check_afflvl_range(int start_afflvl, int end_afflvl)
+{
+	/* Sanity check the parameters passed */
+	if (end_afflvl > MPIDR_MAX_AFFLVL)
+		return PSCI_E_INVALID_PARAMS;
+
+	if (start_afflvl < MPIDR_AFFLVL0)
+		return PSCI_E_INVALID_PARAMS;
+
+	if (end_afflvl < start_afflvl)
+		return PSCI_E_INVALID_PARAMS;
+
+	return PSCI_E_SUCCESS;
+}
+
+/*******************************************************************************
+ * This function is passed an array of pointers to affinity level nodes in the
+ * 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,
+			       int end_afflvl,
+			       mpidr_aff_map_nodes mpidr_nodes)
+{
+	int level;
+
+	for (level = start_afflvl; level <= end_afflvl; level++) {
+		if (mpidr_nodes[level] == NULL)
+			continue;
+		bakery_lock_get(mpidr, &mpidr_nodes[level]->lock);
+	}
+}
+
+/*******************************************************************************
+ * This function is passed an array of pointers to affinity level nodes in the
+ * 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,
+			       int end_afflvl,
+			       mpidr_aff_map_nodes mpidr_nodes)
+{
+	int level;
+
+	for (level = end_afflvl; level >= start_afflvl; level--) {
+		if (mpidr_nodes[level] == NULL)
+			continue;
+		bakery_lock_release(mpidr, &mpidr_nodes[level]->lock);
+	}
+}
+
+/*******************************************************************************
  * Simple routine to determine whether an affinity instance at a given level
  * in an mpidr exists or not.
  ******************************************************************************/
@@ -158,37 +214,44 @@ int psci_get_first_present_afflvl(unsigned long mpidr,
 }
 
 /*******************************************************************************
- * Recursively change the affinity state between the current and target affinity
+ * Iteratively change the affinity state between the current and target affinity
  * levels. The target state matters only if we are starting from affinity level
  * 0 i.e. a cpu otherwise the state depends upon the state of the lower affinity
  * levels.
  ******************************************************************************/
-int psci_change_state(unsigned long mpidr,
-		      int cur_afflvl,
-		      int tgt_afflvl,
+int psci_change_state(mpidr_aff_map_nodes mpidr_nodes,
+		      int start_afflvl,
+		      int end_afflvl,
 		      unsigned int tgt_state)
 {
-	int rc = PSCI_E_SUCCESS;
+	int rc = PSCI_E_SUCCESS, level;
 	unsigned int state;
-	aff_map_node *aff_node;
-
-	/* Sanity check the affinity levels */
-	assert(tgt_afflvl >= cur_afflvl);
-
-	aff_node = psci_get_aff_map_node(mpidr, cur_afflvl);
-	assert(aff_node);
+	aff_map_node *node;
 
-	/* TODO: Check whether the affinity level is present or absent*/
+	/*
+	 * Get a temp pointer to the node. It is not possible that affinity
+	 * level 0 is missing. Simply ignore higher missing levels.
+	 */
+	for (level = start_afflvl; level <= end_afflvl; level++) {
 
-	if (cur_afflvl == MPIDR_AFFLVL0) {
-		psci_set_state(aff_node->state, tgt_state);
-	} else {
-		state = psci_calculate_affinity_state(aff_node);
-		psci_set_state(aff_node->state, state);
+		node = mpidr_nodes[level];
+		if (level == MPIDR_AFFLVL0) {
+			assert(node);
+			psci_set_state(node->state, tgt_state);
+		} else {
+			if (node == NULL)
+				continue;
+			state = psci_calculate_affinity_state(node);
+			psci_set_state(node->state, state);
+		}
 	}
 
-	if (cur_afflvl != tgt_afflvl)
-		psci_change_state(mpidr, cur_afflvl + 1, tgt_afflvl, tgt_state);
+	/* If all went well then the cpu should be in the target state */
+	if (start_afflvl == MPIDR_AFFLVL0) {
+		node = mpidr_nodes[MPIDR_AFFLVL0];
+		state = psci_get_state(node->state);
+		assert(tgt_state == state);
+	}
 
 	return rc;
 }
@@ -443,92 +506,112 @@ unsigned int psci_get_aff_phys_state(aff_map_node *aff_node)
 }
 
 /*******************************************************************************
+ * This function takes an array of pointers to affinity instance nodes in the
+ * topology tree and calls the physical power on handler for the corresponding
+ * affinity levels
+ ******************************************************************************/
+static int psci_call_power_on_handlers(mpidr_aff_map_nodes mpidr_nodes,
+				       int start_afflvl,
+				       int end_afflvl,
+				       afflvl_power_on_finisher *pon_handlers,
+				       unsigned long mpidr)
+{
+	int rc = PSCI_E_INVALID_PARAMS, level;
+	aff_map_node *node;
+
+	for (level = end_afflvl; level >= start_afflvl; level--) {
+		node = mpidr_nodes[level];
+		if (node == NULL)
+			continue;
+
+		/*
+		 * If we run into any trouble while powering up an
+		 * affinity instance, then there is no recovery path
+		 * so simply return an error and let the caller take
+		 * care of the situation.
+		 */
+		rc = pon_handlers[level](mpidr, node);
+		if (rc != PSCI_E_SUCCESS)
+			break;
+	}
+
+	return rc;
+}
+
+/*******************************************************************************
  * Generic handler which is called when a cpu is physically powered on. It
- * recurses through all the affinity levels performing generic, architectural,
+ * traverses through all the affinity levels performing generic, architectural,
  * platform setup and state management e.g. for a cluster that's been powered
  * on, it will call the platform specific code which will enable coherency at
  * the interconnect level. For a cpu it could mean turning on the MMU etc.
  *
- * This function traverses from the lowest to the highest affinity level
- * implemented by the platform. Since it's recursive, for each call the
- * 'cur_afflvl' & 'tgt_afflvl' parameters keep track of which level we are at
- * and which level we need to get to respectively. Locks are picked up along the
- * way so that when the lowest affinity level is hit, state management can be
- * safely done. Prior to this, each affinity level does it's bookeeping as per
- * the state out of reset.
+ * 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 exiting from.
+ *
+ * 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.
  *
  * CAUTION: This function is called with coherent stacks so that coherency and
  * the mmu can be turned on safely.
  ******************************************************************************/
-unsigned int psci_afflvl_power_on_finish(unsigned long mpidr,
-					 int cur_afflvl,
-					 int tgt_afflvl,
-					 afflvl_power_on_finisher *pon_handlers)
+void psci_afflvl_power_on_finish(unsigned long mpidr,
+				 int start_afflvl,
+				 int end_afflvl,
+				 afflvl_power_on_finisher *pon_handlers)
 {
-	unsigned int prev_state, next_state, rc = PSCI_E_SUCCESS;
-	aff_map_node *aff_node;
-	int level;
+	mpidr_aff_map_nodes mpidr_nodes;
+	int rc;
 
 	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. Either case is an irrecoverable error.
 	 */
-	level = psci_get_first_present_afflvl(mpidr,
-					      cur_afflvl,
-					      tgt_afflvl,
-					      &aff_node);
-	/*
-	 * Return if there are no more affinity instances beyond this
-	 * level to process. Else ensure that the returned affinity
-	 * node makes sense.
-	 */
-	if (aff_node == NULL)
-		return rc;
-
-	assert(level == aff_node->level);
+	rc = psci_get_aff_map_nodes(mpidr,
+				    start_afflvl,
+				    end_afflvl,
+				    mpidr_nodes);
+	assert (rc == PSCI_E_SUCCESS);
 
 	/*
-	 * This function acquires the lock corresponding to each
-	 * affinity level so that by the time we hit the highest
-	 * affinity level, the system topology is snapshot and state
-	 * management can be done safely.
+	 * 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.
 	 */
-	bakery_lock_get(mpidr, &aff_node->lock);
-
-	/* Keep the old and new state handy */
-	prev_state = psci_get_state(aff_node->state);
-	next_state = PSCI_STATE_ON;
+	psci_acquire_afflvl_locks(mpidr,
+				  start_afflvl,
+				  end_afflvl,
+				  mpidr_nodes);
 
 	/* Perform generic, architecture and platform specific handling */
-	rc = pon_handlers[level](mpidr, aff_node, prev_state);
-	if (rc != PSCI_E_SUCCESS) {
-		psci_set_state(aff_node->state, prev_state);
-		goto exit;
-	}
+	rc = psci_call_power_on_handlers(mpidr_nodes,
+					 start_afflvl,
+					 end_afflvl,
+					 pon_handlers,
+					 mpidr);
+	assert (rc == PSCI_E_SUCCESS);
 
 	/*
-	 * State management: Update the states if this is the highest
-	 * affinity level requested else pass the job to the next level.
+	 * State management: Update the state of each affinity instance
+	 * between the start and end affinity levels
 	 */
-	if (aff_node->level != tgt_afflvl) {
-		rc = psci_afflvl_power_on_finish(mpidr,
-						 level + 1,
-						 tgt_afflvl,
-						 pon_handlers);
-	} else {
-		psci_change_state(mpidr, MPIDR_AFFLVL0, tgt_afflvl, next_state);
-	}
-
-	/* If all has gone as per plan then this cpu should be marked as ON */
-	if (level == MPIDR_AFFLVL0) {
-		next_state = psci_get_state(aff_node->state);
-		assert(next_state == PSCI_STATE_ON);
-	}
+	psci_change_state(mpidr_nodes,
+			  start_afflvl,
+			  end_afflvl,
+			  PSCI_STATE_ON);
 
-exit:
-	bakery_lock_release(mpidr, &aff_node->lock);
-	return rc;
+	/*
+	 * 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,
+				  end_afflvl,
+				  mpidr_nodes);
 }
diff --git a/common/psci/psci_main.c b/common/psci/psci_main.c
index fbf864b..f73fc49 100644
--- a/common/psci/psci_main.c
+++ b/common/psci/psci_main.c
@@ -45,7 +45,7 @@ int psci_cpu_on(unsigned long target_cpu,
 
 {
 	int rc;
-	unsigned int start_afflvl, target_afflvl;
+	unsigned int start_afflvl, end_afflvl;
 
 	/* Determine if the cpu exists of not */
 	rc = psci_validate_mpidr(target_cpu, MPIDR_AFFLVL0);
@@ -53,13 +53,17 @@ int psci_cpu_on(unsigned long target_cpu,
 		goto exit;
 	}
 
-	start_afflvl = get_max_afflvl();
-	target_afflvl = MPIDR_AFFLVL0;
+	/*
+	 * To turn this cpu on, specify which affinity
+	 * levels need to be turned on
+	 */
+	start_afflvl = MPIDR_AFFLVL0;
+	end_afflvl = get_max_afflvl();
 	rc = psci_afflvl_on(target_cpu,
 			    entrypoint,
 			    context_id,
 			    start_afflvl,
-			    target_afflvl);
+			    end_afflvl);
 
 exit:
 	return rc;
@@ -76,7 +80,7 @@ int psci_cpu_suspend(unsigned int power_state,
 {
 	int rc;
 	unsigned long mpidr;
-	unsigned int tgt_afflvl, pstate_type;
+	unsigned int target_afflvl, pstate_type;
 
 	/* TODO: Standby states are not supported at the moment */
 	pstate_type = psci_get_pstate_type(power_state);
@@ -86,8 +90,8 @@ int psci_cpu_suspend(unsigned int power_state,
 	}
 
 	/* Sanity check the requested state */
-	tgt_afflvl = psci_get_pstate_afflvl(power_state);
-	if (tgt_afflvl > MPIDR_MAX_AFFLVL) {
+	target_afflvl = psci_get_pstate_afflvl(power_state);
+	if (target_afflvl > MPIDR_MAX_AFFLVL) {
 		rc = PSCI_E_INVALID_PARAMS;
 		goto exit;
 	}
@@ -97,8 +101,8 @@ int psci_cpu_suspend(unsigned int power_state,
 				 entrypoint,
 				 context_id,
 				 power_state,
-				 tgt_afflvl,
-				 MPIDR_AFFLVL0);
+				 MPIDR_AFFLVL0,
+				 target_afflvl);
 
 exit:
 	if (rc != PSCI_E_SUCCESS)
@@ -120,7 +124,7 @@ int psci_cpu_off(void)
 	 * management is done immediately followed by cpu, cluster ...
 	 * ..target_afflvl specific actions as this function unwinds back.
 	 */
-	rc = psci_afflvl_off(mpidr, target_afflvl, MPIDR_AFFLVL0);
+	rc = psci_afflvl_off(mpidr, MPIDR_AFFLVL0, target_afflvl);
 
 	/*
 	 * The only error cpu_off can return is E_DENIED. So check if that's
diff --git a/common/psci/psci_private.h b/common/psci/psci_private.h
index e2100f8..7338f1c 100644
--- a/common/psci/psci_private.h
+++ b/common/psci/psci_private.h
@@ -84,9 +84,9 @@ typedef struct {
 	int max;
 } aff_limits_node;
 
+typedef aff_map_node *mpidr_aff_map_nodes[MPIDR_MAX_AFFLVL];
 typedef unsigned int (*afflvl_power_on_finisher)(unsigned long,
-						 aff_map_node *,
-						 unsigned int);
+						 aff_map_node *);
 
 /*******************************************************************************
  * Data prototypes
@@ -110,9 +110,9 @@ extern unsigned int psci_get_aff_phys_state(aff_map_node *);
 extern unsigned int psci_calculate_affinity_state(aff_map_node *);
 extern void psci_get_ns_entry_info(unsigned int index);
 extern unsigned long mpidr_set_aff_inst(unsigned long,unsigned char, int);
-extern int psci_change_state(unsigned long, int, int, unsigned int);
+extern int psci_change_state(mpidr_aff_map_nodes, int, int, unsigned int);
 extern int psci_validate_mpidr(unsigned long, int);
-extern unsigned int psci_afflvl_power_on_finish(unsigned long,
+extern void psci_afflvl_power_on_finish(unsigned long,
 						int,
 						int,
 						afflvl_power_on_finisher *);
@@ -122,7 +122,21 @@ extern int psci_set_ns_entry_info(unsigned int index,
 extern int psci_get_first_present_afflvl(unsigned long,
 					 int, int,
 					 aff_map_node **);
+extern int psci_check_afflvl_range(int start_afflvl, int end_afflvl);
+extern void psci_acquire_afflvl_locks(unsigned long mpidr,
+				      int start_afflvl,
+				      int end_afflvl,
+				      mpidr_aff_map_nodes mpidr_nodes);
+extern void psci_release_afflvl_locks(unsigned long mpidr,
+				      int start_afflvl,
+				      int end_afflvl,
+				      mpidr_aff_map_nodes mpidr_nodes);
+
 /* Private exported functions from psci_setup.c */
+extern int psci_get_aff_map_nodes(unsigned long mpidr,
+				  int start_afflvl,
+				  int end_afflvl,
+				  mpidr_aff_map_nodes mpidr_nodes);
 extern aff_map_node *psci_get_aff_map_node(unsigned long, int);
 
 /* Private exported functions from psci_affinity_on.c */
diff --git a/common/psci/psci_setup.c b/common/psci/psci_setup.c
index 8220b30..decc18d 100644
--- a/common/psci/psci_setup.c
+++ b/common/psci/psci_setup.c
@@ -87,6 +87,56 @@ aff_map_node *psci_get_aff_map_node(unsigned long mpidr, int aff_lvl)
 }
 
 /*******************************************************************************
+ * This function populates an array with nodes corresponding to a given range of
+ * affinity levels in an mpidr. It returns successfully only when the affinity
+ * levels are correct, the mpidr is valid i.e. no affinity level is absent from
+ * the topology tree & the affinity instance at level 0 is not absent.
+ ******************************************************************************/
+int psci_get_aff_map_nodes(unsigned long mpidr,
+			   int start_afflvl,
+			   int end_afflvl,
+			   mpidr_aff_map_nodes mpidr_nodes)
+{
+	int rc = PSCI_E_INVALID_PARAMS, level;
+	aff_map_node *node;
+
+	rc = psci_check_afflvl_range(start_afflvl, end_afflvl);
+	if (rc != PSCI_E_SUCCESS)
+		return rc;
+
+	for (level = start_afflvl; level <= end_afflvl; level++) {
+
+		/*
+		 * Grab the node for each affinity level. No affinity level
+		 * can be missing as that would mean that the topology tree
+		 * is corrupted.
+		 */
+		node = psci_get_aff_map_node(mpidr, level);
+		if (node == NULL) {
+			rc = PSCI_E_INVALID_PARAMS;
+			break;
+		}
+
+		/*
+		 * Skip absent affinity levels unless it's afffinity level 0.
+		 * An absent cpu means that the mpidr is invalid. Save the
+		 * pointer to the node for the present affinity level
+		 */
+		if (!(node->state & PSCI_AFF_PRESENT)) {
+			if (level == MPIDR_AFFLVL0) {
+				rc = PSCI_E_INVALID_PARAMS;
+				break;
+			}
+
+			mpidr_nodes[level] = NULL;
+		} else
+			mpidr_nodes[level] = node;
+	}
+
+	return rc;
+}
+
+/*******************************************************************************
  * Function which initializes the 'aff_map_node' corresponding to an affinity
  * level instance. Each node has a unique mpidr, level and bakery lock. The data
  * field is opaque and holds affinity level specific data e.g. for affinity