Preserve PSCI cpu_suspend 'power_state' parameter.

This patch saves the 'power_state' parameter prior to suspending
a cpu and invalidates it upon its resumption. The 'affinity level'
and 'state id' fields of this parameter can be read using a set of
public and private apis. Validation of power state parameter is
introduced which checks for SBZ bits are zero.
This change also takes care of flushing the parameter from the cache
to main memory. This ensures that it is available after cpu reset
when the caches and mmu are turned off. The earlier support for
saving only the 'affinity level' field of the 'power_state' parameter
has also been reworked.

Fixes ARM-Software/tf-issues#26
Fixes ARM-Software/tf-issues#130

Change-Id: Ic007ccb5e39bf01e0b67390565d3b4be33f5960a

5 files changed, 83 insertions, 25 deletions

diff --git a/services/std_svc/psci/psci_afflvl_suspend.c b/services/std_svc/psci/psci_afflvl_suspend.c
index ca521ff3..fc6fe1f4 100644
--- a/services/std_svc/psci/psci_afflvl_suspend.c
+++ b/services/std_svc/psci/psci_afflvl_suspend.c
@@ -46,10 +46,10 @@ typedef int (*afflvl_suspend_handler)(unsigned long,
 				      unsigned int);
 
 /*******************************************************************************
- * This function sets the affinity level till which the current cpu is being
- * powered down to during a cpu_suspend call
+ * This function sets the power state of the current cpu while
+ * powering down during a cpu_suspend call
  ******************************************************************************/
-void psci_set_suspend_afflvl(aff_map_node *node, int afflvl)
+void psci_set_suspend_power_state(aff_map_node *node, unsigned int power_state)
 {
 	/*
 	 * Check that nobody else is calling this function on our behalf &
@@ -58,22 +58,69 @@ void psci_set_suspend_afflvl(aff_map_node *node, int afflvl)
 	assert(node->mpidr == (read_mpidr() & MPIDR_AFFINITY_MASK));
 	assert(node->level == MPIDR_AFFLVL0);
 
+	/* Save PSCI power state parameter for the core in suspend context */
+	psci_suspend_context[node->data].power_state = power_state;
+
 	/*
-	 * Store the affinity level we are powering down to in our context.
-	 * The cache flush in the suspend code will ensure that this info
-	 * is available immediately upon resuming.
+	 * Flush the suspend data to PoC since it will be accessed while
+	 * returning back from suspend with the caches turned off
 	 */
-	psci_suspend_context[node->data].suspend_level = afflvl;
+	flush_dcache_range(
+		(unsigned long)&psci_suspend_context[node->data],
+		sizeof(suspend_context));
 }
 
 /*******************************************************************************
+ * 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
+ ******************************************************************************/
+int psci_get_suspend_afflvl(unsigned long mpidr)
+{
+	aff_map_node *node;
+
+	node = psci_get_aff_map_node(mpidr & MPIDR_AFFINITY_MASK,
+			MPIDR_AFFLVL0);
+	assert(node);
+
+	return psci_get_aff_map_node_suspend_afflvl(node);
+}
+
+
+/*******************************************************************************
  * This function gets the affinity level till which the current cpu was powered
- * down during a cpu_suspend call.
+ * down during a cpu_suspend call. Returns PSCI_INVALID_DATA if the
+ * power state saved for the node is invalid
+ ******************************************************************************/
+int psci_get_aff_map_node_suspend_afflvl(aff_map_node *node)
+{
+	unsigned int power_state;
+
+	assert(node->level == MPIDR_AFFLVL0);
+
+	power_state = psci_suspend_context[node->data].power_state;
+	return ((power_state == PSCI_INVALID_DATA) ?
+				power_state : psci_get_pstate_afflvl(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
  ******************************************************************************/
-int psci_get_suspend_afflvl(aff_map_node *node)
+int psci_get_suspend_stateid(unsigned long mpidr)
 {
-	/* Return the target affinity level */
-	return psci_suspend_context[node->data].suspend_level;
+	aff_map_node *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 = psci_suspend_context[node->data].power_state;
+	return ((power_state == PSCI_INVALID_DATA) ?
+					power_state : psci_get_pstate_id(power_state));
 }
 
 /*******************************************************************************
@@ -94,6 +141,9 @@ static int psci_afflvl0_suspend(unsigned long mpidr,
 	/* Sanity check to safeguard against data corruption */
 	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);
+
 	/*
 	 * Generic management: Store the re-entry information for the non-secure
 	 * world and allow the secure world to suspend itself
@@ -376,10 +426,6 @@ int psci_afflvl_suspend(unsigned long mpidr,
 				  end_afflvl,
 				  mpidr_nodes);
 
-
-	/* Save the affinity level till which this cpu can be powered down */
-	psci_set_suspend_afflvl(mpidr_nodes[MPIDR_AFFLVL0], end_afflvl);
-
 	/* Perform generic, architecture and platform specific handling */
 	rc = psci_call_suspend_handlers(mpidr_nodes,
 					start_afflvl,
@@ -461,10 +507,14 @@ static unsigned int psci_afflvl0_suspend_finish(unsigned long mpidr,
 	 * error, it's expected to assert within
 	 */
 	if (psci_spd_pm && psci_spd_pm->svc_suspend) {
-		suspend_level = psci_get_suspend_afflvl(cpu_node);
+		suspend_level = psci_get_aff_map_node_suspend_afflvl(cpu_node);
+		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);
+
 	/*
 	 * Generic management: Now we just need to retrieve the
 	 * information that we had stashed away during the suspend
diff --git a/services/std_svc/psci/psci_common.c b/services/std_svc/psci/psci_common.c
index 236309cf..8b49b778 100644
--- a/services/std_svc/psci/psci_common.c
+++ b/services/std_svc/psci/psci_common.c
@@ -91,6 +91,7 @@ int get_power_on_target_afflvl(unsigned long mpidr)
 {
 	aff_map_node *node;
 	unsigned int state;
+	int afflvl;
 
 	/* Retrieve our node from the topology tree */
 	node = psci_get_aff_map_node(mpidr & MPIDR_AFFINITY_MASK,
@@ -106,9 +107,11 @@ int get_power_on_target_afflvl(unsigned long mpidr)
 	if (state == PSCI_STATE_ON_PENDING)
 		return get_max_afflvl();
 
-	if (state == PSCI_STATE_SUSPEND)
-		return psci_get_suspend_afflvl(node);
-
+	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;
 }
 
diff --git a/services/std_svc/psci/psci_main.c b/services/std_svc/psci/psci_main.c
index 2d61ec08..c90929dd 100644
--- a/services/std_svc/psci/psci_main.c
+++ b/services/std_svc/psci/psci_main.c
@@ -85,6 +85,10 @@ int psci_cpu_suspend(unsigned int power_state,
 	unsigned long mpidr;
 	unsigned int target_afflvl, pstate_type;
 
+	/* Check SBZ bits in power state are zero */
+	if (psci_validate_power_state(power_state))
+		return PSCI_E_INVALID_PARAMS;
+
 	/* Sanity check the requested state */
 	target_afflvl = psci_get_pstate_afflvl(power_state);
 	if (target_afflvl > MPIDR_MAX_AFFLVL)
diff --git a/services/std_svc/psci/psci_private.h b/services/std_svc/psci/psci_private.h
index 2d9d12ba..8cb3aab8 100644
--- a/services/std_svc/psci/psci_private.h
+++ b/services/std_svc/psci/psci_private.h
@@ -74,10 +74,8 @@ typedef struct {
  * across cpu_suspend calls which enter the power down state.
  ******************************************************************************/
 typedef struct {
-	/* Align the suspend level to allow per-cpu lockless access */
-	int suspend_level
-	__attribute__((__aligned__(CACHE_WRITEBACK_GRANULE)));
-} suspend_context;
+	unsigned int power_state;
+} __aligned(CACHE_WRITEBACK_GRANULE) suspend_context;
 
 typedef aff_map_node (*mpidr_aff_map_nodes[MPIDR_MAX_AFFLVL]);
 typedef unsigned int (*afflvl_power_on_finisher)(unsigned long,
@@ -147,8 +145,9 @@ extern int psci_afflvl_on(unsigned long,
 extern int psci_afflvl_off(unsigned long, int, int);
 
 /* Private exported functions from psci_affinity_suspend.c */
-extern void psci_set_suspend_afflvl(aff_map_node *node, int afflvl);
-extern int psci_get_suspend_afflvl(aff_map_node *node);
+extern void psci_set_suspend_power_state(aff_map_node *node,
+					unsigned int power_state);
+extern int psci_get_aff_map_node_suspend_afflvl(aff_map_node *node);
 extern int psci_afflvl_suspend(unsigned long,
 			       unsigned long,
 			       unsigned long,
diff --git a/services/std_svc/psci/psci_setup.c b/services/std_svc/psci/psci_setup.c
index e3a5d5d0..4525d78d 100644
--- a/services/std_svc/psci/psci_setup.c
+++ b/services/std_svc/psci/psci_setup.c
@@ -183,6 +183,8 @@ static void psci_init_aff_map_node(unsigned long mpidr,
 		assert(psci_ns_einfo_idx < PSCI_NUM_AFFS);
 
 		psci_aff_map[idx].data = psci_ns_einfo_idx;
+		/* Invalidate the suspend context for the node */
+		psci_suspend_context[psci_ns_einfo_idx].power_state = PSCI_INVALID_DATA;
 		psci_ns_einfo_idx++;
 
 		/*