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/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++;
/*