psci: preserve target affinity level during suspend

This patch adds support to save and restore the target affinity level
specified during a cpu_suspend psci call. This ensures that we
traverse only through the affinity levels that we originally intended
to after resuming from suspend.

Change-Id: I0900ae49a50b496da137cfec8f158da0397ec56c

4 files changed, 73 insertions, 1 deletions

diff --git a/common/psci/psci_afflvl_suspend.c b/common/psci/psci_afflvl_suspend.c
index 6fb60f4..b2dc732 100644
--- a/common/psci/psci_afflvl_suspend.c
+++ b/common/psci/psci_afflvl_suspend.c
@@ -44,6 +44,37 @@ 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
+ ******************************************************************************/
+void psci_set_suspend_afflvl(aff_map_node *node, int afflvl)
+{
+	/*
+	 * 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);
+
+	/*
+	 * 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.
+	 */
+	psci_suspend_context[node->data].suspend_level = afflvl;
+}
+
+/*******************************************************************************
+ * This function gets the affinity level till which the current cpu was powered
+ * down during a cpu_suspend call.
+ ******************************************************************************/
+int psci_get_suspend_afflvl(aff_map_node *node)
+{
+	/* Return the target affinity level */
+	return psci_suspend_context[node->data].suspend_level;
+}
+
+/*******************************************************************************
  * The next three functions implement a handler for each supported affinity
  * level which is called when that affinity level is about to be suspended.
  ******************************************************************************/
@@ -336,6 +367,9 @@ int psci_afflvl_suspend(unsigned long mpidr,
 			  end_afflvl,
 			  PSCI_STATE_SUSPEND);
 
+	/* 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,
diff --git a/common/psci/psci_common.c b/common/psci/psci_common.c
index 705c5d7..e9028cc 100644
--- a/common/psci/psci_common.c
+++ b/common/psci/psci_common.c
@@ -72,6 +72,38 @@ aff_limits_node psci_aff_limits[MPIDR_MAX_AFFLVL + 1];
 plat_pm_ops *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.
+ ******************************************************************************/
+int get_power_on_target_afflvl(unsigned long mpidr)
+{
+	aff_map_node *node;
+	unsigned int state;
+
+	/* Retrieve our node from the topology tree */
+	node = psci_get_aff_map_node(mpidr & 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.
+	 */
+	state = psci_get_state(node->state);
+	if (state == PSCI_STATE_ON_PENDING)
+		return get_max_afflvl();
+
+	if (state == PSCI_STATE_SUSPEND)
+		return psci_get_suspend_afflvl(node);
+
+	return PSCI_E_INVALID_PARAMS;
+}
+
+/*******************************************************************************
  * Simple routine to retrieve the maximum affinity level supported by the
  * platform and check that it makes sense.
  ******************************************************************************/
diff --git a/common/psci/psci_entry.S b/common/psci/psci_entry.S
index 6532556..a6a1a6f 100644
--- a/common/psci/psci_entry.S
+++ b/common/psci/psci_entry.S
@@ -72,7 +72,10 @@ psci_aff_common_finish_entry:
 	 * level 0.
 	 * ---------------------------------------------
 	 */
-	bl	get_max_afflvl
+	mov	x0, x19
+	bl	get_power_on_target_afflvl
+	cmp	x0, xzr
+	b.lt	_panic
 	mov	x3, x23
 	mov	x2, x0
 	mov	x0, x19
diff --git a/common/psci/psci_private.h b/common/psci/psci_private.h
index 8016ad2..1cc7104 100644
--- a/common/psci/psci_private.h
+++ b/common/psci/psci_private.h
@@ -107,6 +107,7 @@ 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(mpidr_aff_map_nodes, int, int, unsigned int);
 extern int psci_validate_mpidr(unsigned long, int);
+extern int get_power_on_target_afflvl(unsigned long mpidr);
 extern void psci_afflvl_power_on_finish(unsigned long,
 						int,
 						int,
@@ -145,6 +146,8 @@ 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 int psci_afflvl_suspend(unsigned long,
 			       unsigned long,
 			       unsigned long,