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 6fb60f4aa104d3e6eaea4643cbf4b2b5402d8528..b2dc732f7983708a7babec335202930d1bf3a181 100644 (file)
--- a/common/psci/psci_afflvl_suspend.c
+++ b/common/psci/psci_afflvl_suspend.c
@@ -43,6 +43,37 @@ typedef int (*afflvl_suspend_handler)(unsigned long,
                                      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 705c5d7182f3fb9f265bafefcede609ccdac503c..e9028ccdf2127811dc851ab390b64f453b87f2ed 100644 (file)
--- a/common/psci/psci_common.c
+++ b/common/psci/psci_common.c
@@ -71,6 +71,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 65325560b5a01bd19193423cd0d47f4f9308a7bd..a6a1a6f00828f40cb0646ca9074e300b0e21db0e 100644 (file)
--- 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 8016ad29a1b8d08b9f99ff254181709d89782edd..1cc71049ffc4f07628139be857292f18033623f1 100644 (file)
--- 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,