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 @@
 };
 
 /*******************************************************************************
- * 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 @@
 };
 
 /*******************************************************************************
- * 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);
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);
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 @@
 }
 
 /*******************************************************************************
+ * 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 @@
 }
 
 /*******************************************************************************
- * 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 @@
 }
 
 /*******************************************************************************
+ * 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 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 @@
 		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 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 @@
 	}
 
 	/* 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 @@
 				 entrypoint,
 				 context_id,
 				 power_state,
-				 tgt_afflvl,
-				 MPIDR_AFFLVL0);
+				 MPIDR_AFFLVL0,
+				 target_afflvl);
 
 exit:
 	if (rc != PSCI_E_SUCCESS)
@@ -120,7 +124,7 @@
 	 * 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 @@
 	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_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_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 @@
 }
 
 /*******************************************************************************
+ * 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