Add PSCI service specific per-CPU data

This patch adds a structure defined by the PSCI service to the per-CPU data
array. The structure is used to save the 'power_state' parameter specified
during a 'cpu_suspend' call on the current CPU. This parameter was being saved
in the cpu node in the PSCI topology tree earlier.

The existing API to return the state id specified during a PSCI CPU_SUSPEND call
i.e. psci_get_suspend_stateid(mpidr) has been renamed to
psci_get_suspend_stateid_by_mpidr(mpidr). The new psci_get_suspend_stateid() API
returns the state id of the current cpu.

The psci_get_suspend_afflvl() API has been changed to return the target affinity
level of the current CPU. This was specified using the 'mpidr' parameter in the
old implementation.

The behaviour of the get_power_on_target_afflvl() has been tweaked such that
traversal of the PSCI topology tree to locate the affinity instance node for the
current CPU is done only in the debug build as it is an expensive operation.

Change-Id: Iaad49db75abda471f6a82d697ee6e0df554c4caf

5 files changed, 59 insertions, 79 deletions

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 @@ typedef int (*afflvl_suspend_handler_t)(aff_map_node_t *,
 				      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 @@ static int psci_afflvl0_suspend(aff_map_node_t *cpu_node,
 	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 @@ static unsigned int psci_afflvl0_suspend_finish(aff_map_node_t *cpu_node)
 	 * 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 @@ const plat_pm_ops_t *psci_plat_pm_ops;
 /*******************************************************************************
  * 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 @@ psci_aff_common_finish_entry:
 	 * 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 @@ typedef struct aff_map_node {
 	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 @@ unsigned short psci_get_phys_state(aff_map_node_t *node);
 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_on(unsigned long,
 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 @@ static void psci_init_aff_map_node(unsigned long mpidr,
 		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 @@ static void psci_init_aff_map_node(unsigned long mpidr,
 		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);