Merge pull request #50 from vikramkanigiri/vk/tf-issues#26

Preserve PSCI cpu_suspend 'power_state' parameter.

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__*/
 
 

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
+ * This function sets the power state of the current cpu while
- * powered down to during a cpu_suspend call
+ * 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.
+	 * Flush the suspend data to PoC since it will be accessed while
-	 * The cache flush in the suspend code will ensure that this info
+	 * returning back from suspend with the caches turned off
-	 * is available immediately upon resuming.
 	 */
-	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
+ * This function gets the affinity level till which a cpu is powered down
- * down during a cpu_suspend call.
+ * 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 */
+	aff_map_node *node;
-	return psci_suspend_context[node->data].suspend_level;
+
+	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 @@ 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

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)
+	if (state == PSCI_STATE_SUSPEND) {
-		return psci_get_suspend_afflvl(node);
+		afflvl = psci_get_aff_map_node_suspend_afflvl(node);
-
+		assert(afflvl != PSCI_INVALID_DATA);
+		return afflvl;
+	}
 	return PSCI_E_INVALID_PARAMS;
 }
 

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)

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 */
+	unsigned int power_state;
-	int suspend_level
+} __aligned(CACHE_WRITEBACK_GRANULE) suspend_context;
-	__attribute__((__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 void psci_set_suspend_power_state(aff_map_node *node,
-extern int psci_get_suspend_afflvl(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,

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