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
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 @@
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 @@
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 @@
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 @@
* 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 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_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,