diff --git a/services/std_svc/psci/psci_afflvl_suspend.c b/services/std_svc/psci/psci_afflvl_suspend.c
index 70f90a1..a123dc3 100644
--- a/services/std_svc/psci/psci_afflvl_suspend.c
+++ b/services/std_svc/psci/psci_afflvl_suspend.c
@@ -34,6 +34,7 @@
 #include <arch_helpers.h>
 #include <context.h>
 #include <context_mgmt.h>
+#include <cpu_data.h>
 #include <platform.h>
 #include <runtime_svc.h>
 #include <stddef.h>
@@ -45,76 +46,59 @@
 				      unsigned int);
 
 /*******************************************************************************
- * This function sets the power state of the current cpu while
- * powering down during a cpu_suspend call
+ * This function saves the power state parameter passed in the current PSCI
+ * cpu_suspend call in the per-cpu data array.
  ******************************************************************************/
-void psci_set_suspend_power_state(aff_map_node_t *node, unsigned int power_state)
+void psci_set_suspend_power_state(unsigned int power_state)
 {
-	/*
-	 * Check that nobody else is calling this function on our behalf &
-	 * this information is being set only in the cpu node
-	 */
-	assert(node->mpidr == (read_mpidr() & MPIDR_AFFINITY_MASK));
-	assert(node->level == MPIDR_AFFLVL0);
-
-	/*
-	 * Save PSCI power state parameter for the core in suspend context.
-	 * The node is in always-coherent RAM so it does not need to be flushed
-	 */
-	node->power_state = power_state;
+	set_cpu_data(psci_svc_cpu_data.power_state, power_state);
+	flush_cpu_data(psci_svc_cpu_data.power_state);
 }
 
 /*******************************************************************************
- * This function gets the affinity level till which a cpu is powered down
- * during a cpu_suspend call. Returns PSCI_INVALID_DATA if the
- * power state saved for the node is invalid
+ * This function gets the affinity level till which the current cpu could be
+ * powered down during a cpu_suspend call. Returns PSCI_INVALID_DATA if the
+ * power state is invalid.
  ******************************************************************************/
-int psci_get_suspend_afflvl(unsigned long mpidr)
+int psci_get_suspend_afflvl()
 {
-	aff_map_node_t *node;
+	unsigned int power_state;
 
-	node = psci_get_aff_map_node(mpidr & MPIDR_AFFINITY_MASK,
-			MPIDR_AFFLVL0);
-	assert(node);
+	power_state = get_cpu_data(psci_svc_cpu_data.power_state);
 
-	return psci_get_aff_map_node_suspend_afflvl(node);
+	return ((power_state == PSCI_INVALID_DATA) ?
+		power_state : psci_get_pstate_afflvl(power_state));
 }
 
-
 /*******************************************************************************
- * This function gets the affinity level till which the current cpu was powered
- * down during a cpu_suspend call. Returns PSCI_INVALID_DATA if the
- * power state saved for the node is invalid
+ * This function gets the state id of the current cpu from the power state
+ * parameter saved in the per-cpu data array. Returns PSCI_INVALID_DATA if the
+ * power state saved is invalid.
  ******************************************************************************/
-int psci_get_aff_map_node_suspend_afflvl(aff_map_node_t *node)
+int psci_get_suspend_stateid()
 {
 	unsigned int power_state;
 
-	assert(node->level == MPIDR_AFFLVL0);
+	power_state = get_cpu_data(psci_svc_cpu_data.power_state);
 
-	power_state = node->power_state;
 	return ((power_state == PSCI_INVALID_DATA) ?
-				power_state : psci_get_pstate_afflvl(power_state));
+		power_state : psci_get_pstate_id(power_state));
 }
 
 /*******************************************************************************
- * This function gets the state id of a cpu stored in suspend context
- * while powering down during a cpu_suspend call. Returns 0xFFFFFFFF
- * if the power state saved for the node is invalid
+ * This function gets the state id of the cpu specified by the 'mpidr' parameter
+ * from the power state parameter saved in the per-cpu data array. Returns
+ * PSCI_INVALID_DATA if the power state saved is invalid.
  ******************************************************************************/
-int psci_get_suspend_stateid(unsigned long mpidr)
+int psci_get_suspend_stateid_by_mpidr(unsigned long mpidr)
 {
-	aff_map_node_t *node;
 	unsigned int power_state;
 
-	node = psci_get_aff_map_node(mpidr & MPIDR_AFFINITY_MASK,
-			MPIDR_AFFLVL0);
-	assert(node);
-	assert(node->level == MPIDR_AFFLVL0);
+	power_state = get_cpu_data_by_mpidr(mpidr,
+					    psci_svc_cpu_data.power_state);
 
-	power_state = node->power_state;
 	return ((power_state == PSCI_INVALID_DATA) ?
-					power_state : psci_get_pstate_id(power_state));
+		power_state : psci_get_pstate_id(power_state));
 }
 
 /*******************************************************************************
@@ -136,7 +120,7 @@
 	assert(cpu_node->level == MPIDR_AFFLVL0);
 
 	/* Save PSCI power state parameter for the core in suspend context */
-	psci_set_suspend_power_state(cpu_node, power_state);
+	psci_set_suspend_power_state(power_state);
 
 	/*
 	 * Generic management: Store the re-entry information for the non-secure
@@ -451,13 +435,13 @@
 	 * error, it's expected to assert within
 	 */
 	if (psci_spd_pm && psci_spd_pm->svc_suspend) {
-		suspend_level = psci_get_aff_map_node_suspend_afflvl(cpu_node);
+		suspend_level = psci_get_suspend_afflvl();
 		assert (suspend_level != PSCI_INVALID_DATA);
 		psci_spd_pm->svc_suspend_finish(suspend_level);
 	}
 
 	/* Invalidate the suspend context for the node */
-	psci_set_suspend_power_state(cpu_node, PSCI_INVALID_DATA);
+	psci_set_suspend_power_state(PSCI_INVALID_DATA);
 
 	/*
 	 * Generic management: Now we just need to retrieve the
diff --git a/services/std_svc/psci/psci_common.c b/services/std_svc/psci/psci_common.c
index 2fd1764..9daf6f0 100644
--- a/services/std_svc/psci/psci_common.c
+++ b/services/std_svc/psci/psci_common.c
@@ -61,37 +61,39 @@
 /*******************************************************************************
  * Routine to return the maximum affinity level to traverse to after a cpu has
  * been physically powered up. It is expected to be called immediately after
- * reset from assembler code. It has to find its 'aff_map_node' instead of
- * getting it as an argument.
- * TODO: Calling psci_get_aff_map_node() with the MMU disabled is slow. Add
- * support to allow faster access to the target affinity level.
+ * reset from assembler code.
  ******************************************************************************/
-int get_power_on_target_afflvl(unsigned long mpidr)
+int get_power_on_target_afflvl()
 {
-	aff_map_node_t *node;
-	unsigned int state;
 	int afflvl;
 
+#if DEBUG
+	unsigned int state;
+	aff_map_node_t *node;
+
 	/* Retrieve our node from the topology tree */
-	node = psci_get_aff_map_node(mpidr & MPIDR_AFFINITY_MASK,
-			MPIDR_AFFLVL0);
+	node = psci_get_aff_map_node(read_mpidr_el1() & MPIDR_AFFINITY_MASK,
+				     MPIDR_AFFLVL0);
 	assert(node);
 
 	/*
-	 * Return the maximum supported affinity level if this cpu was off.
-	 * Call the handler in the suspend code if this cpu had been suspended.
-	 * Any other state is invalid.
+	 * Sanity check the state of the cpu. It should be either suspend or "on
+	 * pending"
 	 */
 	state = psci_get_state(node);
-	if (state == PSCI_STATE_ON_PENDING)
-		return get_max_afflvl();
+	assert(state == PSCI_STATE_SUSPEND || state == PSCI_STATE_ON_PENDING);
+#endif
 
-	if (state == PSCI_STATE_SUSPEND) {
-		afflvl = psci_get_aff_map_node_suspend_afflvl(node);
-		assert(afflvl != PSCI_INVALID_DATA);
-		return afflvl;
-	}
-	return PSCI_E_INVALID_PARAMS;
+	/*
+	 * Assume that this cpu was suspended and retrieve its target affinity
+	 * level. If it is invalid then it could only have been turned off
+	 * earlier. get_max_afflvl() will return the highest affinity level a
+	 * cpu can be turned off to.
+	 */
+	afflvl = psci_get_suspend_afflvl();
+	if (afflvl == PSCI_INVALID_DATA)
+		afflvl = get_max_afflvl();
+	return afflvl;
 }
 
 /*******************************************************************************
diff --git a/services/std_svc/psci/psci_entry.S b/services/std_svc/psci/psci_entry.S
index 68b917e..4b2b106 100644
--- a/services/std_svc/psci/psci_entry.S
+++ b/services/std_svc/psci/psci_entry.S
@@ -134,18 +134,13 @@
 	 * level 0.
 	 * ---------------------------------------------
 	 */
-	mrs	x0, mpidr_el1
 	bl	get_power_on_target_afflvl
-	cmp	x0, xzr
-	b.lt	_panic
 	mov	x2, x23
 	mov	x1, x0
 	mov	x0, #MPIDR_AFFLVL0
 	bl	psci_afflvl_power_on_finish
 
 	b	el3_exit
-_panic:
-	b	_panic
 
 	/* --------------------------------------------
 	 * This function is called to indicate to the
diff --git a/services/std_svc/psci/psci_private.h b/services/std_svc/psci/psci_private.h
index 4bf9107..158a5f7 100644
--- a/services/std_svc/psci/psci_private.h
+++ b/services/std_svc/psci/psci_private.h
@@ -52,7 +52,6 @@
 	unsigned short ref_count;
 	unsigned char state;
 	unsigned char level;
-	unsigned int power_state;
 	bakery_lock_t lock;
 } aff_map_node_t;
 
@@ -85,7 +84,7 @@
 void psci_set_state(aff_map_node_t *node, unsigned short state);
 unsigned long mpidr_set_aff_inst(unsigned long, unsigned char, int);
 int psci_validate_mpidr(unsigned long, int);
-int get_power_on_target_afflvl(unsigned long mpidr);
+int get_power_on_target_afflvl(void);
 void psci_afflvl_power_on_finish(int,
 				int,
 				afflvl_power_on_finisher_t *);
@@ -119,15 +118,13 @@
 int psci_afflvl_off(int, int);
 
 /* Private exported functions from psci_affinity_suspend.c */
-void psci_set_suspend_power_state(aff_map_node_t *node,
-				unsigned int power_state);
-int psci_get_aff_map_node_suspend_afflvl(aff_map_node_t *node);
 int psci_afflvl_suspend(unsigned long,
 			unsigned long,
 			unsigned int,
 			int,
 			int);
 unsigned int psci_afflvl_suspend_finish(int, int);
+void psci_set_suspend_power_state(unsigned int power_state);
 
 /* Private exported functions from psci_helpers.S */
 void psci_do_pwrdown_cache_maintenance(uint32_t affinity_level);
diff --git a/services/std_svc/psci/psci_setup.c b/services/std_svc/psci/psci_setup.c
index 68f19a0..9e4955d 100644
--- a/services/std_svc/psci/psci_setup.c
+++ b/services/std_svc/psci/psci_setup.c
@@ -189,9 +189,6 @@
 		if (state & PSCI_AFF_PRESENT)
 			psci_set_state(&psci_aff_map[idx], PSCI_STATE_OFF);
 
-		/* Invalidate the suspend context for the node */
-		psci_aff_map[idx].power_state = PSCI_INVALID_DATA;
-
 		/*
 		 * Associate a non-secure context with this affinity
 		 * instance through the context management library.
@@ -199,6 +196,11 @@
 		linear_id = platform_get_core_pos(mpidr);
 		assert(linear_id < PLATFORM_CORE_COUNT);
 
+		/* Invalidate the suspend context for the node */
+		set_cpu_data_by_index(linear_id,
+				      psci_svc_cpu_data.power_state,
+				      PSCI_INVALID_DATA);
+
 		cm_set_context_by_mpidr(mpidr,
 					(void *) &psci_ns_context[linear_id],
 					NON_SECURE);