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 the current cpu was powered
- * down during a cpu_suspend call.
+ * 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(aff_map_node *node)
+int psci_get_suspend_afflvl(unsigned long mpidr)
{
- /* Return the target affinity level */
- return psci_suspend_context[node->data].suspend_level;
+ 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. 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_stateid(unsigned long mpidr)
+{
+ 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