Merge pull request #50 from vikramkanigiri/vk/tf-issues#26
Preserve PSCI cpu_suspend 'power_state' parameter.
diff --git a/include/psci.h b/include/psci.h
index 5c66789..e290793 100644
--- 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 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 ca521ff..fc6fe1f 100644
--- a/services/std_svc/psci/psci_afflvl_suspend.c
+++ b/services/std_svc/psci/psci_afflvl_suspend.c
@@ -46,10 +46,10 @@
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 @@
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 @@
/* 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 @@
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 @@
* 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 236309c..8b49b77 100644
--- a/services/std_svc/psci/psci_common.c
+++ b/services/std_svc/psci/psci_common.c
@@ -91,6 +91,7 @@
{
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 @@
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 2d61ec0..c90929d 100644
--- a/services/std_svc/psci/psci_main.c
+++ b/services/std_svc/psci/psci_main.c
@@ -85,6 +85,10 @@
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 2d9d12b..8cb3aab 100644
--- a/services/std_svc/psci/psci_private.h
+++ b/services/std_svc/psci/psci_private.h
@@ -74,10 +74,8 @@
* 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_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 e3a5d5d..4525d78 100644
--- a/services/std_svc/psci/psci_setup.c
+++ b/services/std_svc/psci/psci_setup.c
@@ -183,6 +183,8 @@
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++;
/*