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

diff --git a/include/psci.h b/include/psci.h
index 5c66789ee9ac1a363b2b21e3b4a34fc86d272639..e29079372f6b255277034c723af190a7b183efde 100644 (file)
--- a/include/psci.h
+++ b/include/psci.h
@@ -73,6 +73,7 @@
 #define PSTATE_ID_MASK         0xffff
 #define PSTATE_TYPE_MASK       0x1
 #define PSTATE_AFF_LVL_MASK    0x3
+#define PSTATE_VALID_MASK     0xFCFE0000
 
 #define PSTATE_TYPE_STANDBY    0x0
 #define PSTATE_TYPE_POWERDOWN  0x1
@@ -118,9 +119,14 @@
 #define PSCI_STATE_ON_PENDING  0x2
 #define PSCI_STATE_SUSPEND     0x3
 
+#define PSCI_INVALID_DATA -1
+
 #define get_phys_state(x)      (x != PSCI_STATE_ON ? \
                                 PSCI_STATE_OFF : PSCI_STATE_ON)
 
+#define psci_validate_power_state(pstate) (pstate & PSTATE_VALID_MASK)
+
+
 /* Number of affinity instances whose state this psci imp. can track */
 #define PSCI_NUM_AFFS          32ull
 
@@ -182,6 +188,9 @@ extern int psci_cpu_on(unsigned long,
 extern void psci_aff_on_finish_entry(void);
 extern void psci_aff_suspend_finish_entry(void);
 extern void psci_register_spd_pm_hook(const spd_pm_ops *);
+extern int psci_get_suspend_stateid(unsigned long mpidr);
+extern int psci_get_suspend_afflvl(unsigned long mpidr);
+
 #endif /*__ASSEMBLY__*/
 
 

diff --git a/services/std_svc/psci/psci_afflvl_suspend.c b/services/std_svc/psci/psci_afflvl_suspend.c
index ca521ff38e0ef7f16ae12f0304c157d2c39a4a0e..fc6fe1f4c6f7229fad8c46630d89609e6aee6a73 100644 (file)
--- 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 236309cf9efe6c5d8b23b9e6e8d1cdda4d0dcf77..8b49b7786a0554d4b6781e16cc0fd468141a41d5 100644 (file)
--- 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 2d61ec0896ab564b92f4ebcbb1ff260460260c11..c90929ddee934bf522f936529e2ae6bac30d37a2 100644 (file)
--- 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 2d9d12ba87c72fae2c9c9693bcb1249004e0e32d..8cb3aab8d73cbcb40dd0301911a7535b5c880398 100644 (file)
--- 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 e3a5d5d08ceb4facb2f813b1f6746584fba49946..4525d78dff56a4047b85e6b439d38d59f7dce4b2 100644 (file)
--- 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++;
 
                /*