Juno: Add support for PSCI cpu_suspend api

This patch adds support for the PSCI cpu_suspend api to allow entry
into low power states until affinity level 1 i.e. cluster. It mainly
ensures that a consolidated power off command which specifies the
level to which each affinity level should be powered down is sent to
the SCP.

Change-Id: I84201f5c06e9c41c9e1fbc6438d12ab5f65ee429

plat/juno/plat_pm.c

@@ -28,6 +28,7 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
+#include <assert.h>
 #include <arch_helpers.h>
 #include <cci400.h>
 #include <platform.h>
@@ -37,6 +38,13 @@
 #include "juno_private.h"
 #include "scpi.h"
 
+typedef struct {
+	/* Align the suspend level to allow per-cpu lockless access */
+	uint32_t state[MPIDR_MAX_AFFLVL]  __aligned(CACHE_WRITEBACK_GRANULE);
+} scp_pstate;
+
+static scp_pstate target_pstate[PLATFORM_CORE_COUNT];
+
 int pm_on(unsigned long mpidr,
 		   unsigned long sec_entrypoint,
 		   unsigned long ns_entrypoint,
@@ -90,6 +98,16 @@ int pm_on_finish(unsigned long mpidr, unsigned int afflvl, unsigned int state)
 		/* Juno todo: Is this setup only needed after a cold boot? */
 		gic_pcpu_distif_setup(GICD_BASE);
 
+		/*
+		 * Clear the mailbox for each cpu
+		 */
+		unsigned long *mbox = (unsigned long *)(unsigned long)(
+			TRUSTED_MAILBOXES_BASE +
+			(platform_get_core_pos(mpidr) << TRUSTED_MAILBOX_SHIFT)
+			);
+		*mbox = 0;
+		flush_dcache_range((unsigned long)mbox, sizeof(*mbox));
+
 		break;
 	}
 
@@ -97,23 +115,34 @@ int pm_on_finish(unsigned long mpidr, unsigned int afflvl, unsigned int state)
 }
 
 /*******************************************************************************
- * Handler called when an affinity instance is about to be turned off. The
- * level and mpidr determine the affinity instance. The 'state' arg. allows the
- * platform to decide whether the cluster is being turned off and take apt
- * actions.
+ * Common function called while turning a cpu off or suspending it. It is called
+ * for each affinity level until the target affinity level. It keeps track of
+ * the power state that needs to be programmed through the SCP firmware for each
+ * affinity level. Once the target affinity level is reached it uses the MHU
+ * channel to ask the SCP to perform the power operations for each affinity
+ * level accumulated so far.
  *
  * CAUTION: There is no guarantee that caches will remain turned on across calls
  * to this function as each affinity level is dealt with. So do not write & read
  * global variables across calls. It will be wise to do flush a write to the
  * global to prevent unpredictable results.
  ******************************************************************************/
-int pm_off(unsigned long mpidr, unsigned int afflvl, unsigned int state)
+static int juno_power_down_common(unsigned long mpidr,
+				  unsigned int linear_id,
+				  unsigned int cur_afflvl,
+				  unsigned int tgt_afflvl,
+				  unsigned int state)
 {
-	 /* We're only interested in power off states */
-	if (state != PSCI_STATE_OFF)
-		return PSCI_E_SUCCESS;
+	/* Record which power state this affinity level is supposed to enter */
+	if (state == PSCI_STATE_OFF) {
+		target_pstate[linear_id].state[cur_afflvl] = scpi_power_off;
+	} else {
+		assert(cur_afflvl != MPIDR_AFFLVL0);
+		target_pstate[linear_id].state[cur_afflvl] = scpi_power_on;
+		goto exit;
+	}
 
-	switch (afflvl) {
+	switch (cur_afflvl) {
 	case MPIDR_AFFLVL1:
 		/* Cluster is to be turned off, so disable coherency */
 		cci_disable_coherency(mpidr);
@@ -127,30 +156,106 @@ int pm_off(unsigned long mpidr, unsigned int afflvl, unsigned int state)
 		/* Prevent interrupts from spuriously waking up this cpu */
 		gic_cpuif_deactivate(GICC_BASE);
 
-		/*
-		 * Ask SCP to power down CPU.
-		 *
-		 * Note, we also ask for cluster power down as well because we
-		 * know the SCP will only actually do that if this is the last
-		 * CPU going down, and also, that final power down won't happen
-		 * until this CPU executes the WFI instruction after the PSCI
-		 * framework has done it's thing.
-		 */
-		scpi_set_css_power_state(mpidr, scpi_power_off, scpi_power_off,
-						scpi_power_retention);
 		break;
 	}
 
+exit:
+	/*
+	 * If this is the target affinity level then we need to ask the SCP
+	 * to power down the appropriate components depending upon their state
+	 */
+	if (cur_afflvl == tgt_afflvl) {
+		scpi_set_css_power_state(mpidr,
+					 target_pstate[linear_id].state[MPIDR_AFFLVL0],
+					 target_pstate[linear_id].state[MPIDR_AFFLVL1],
+					 scpi_power_on);
+
+		/* Reset the states */
+		target_pstate[linear_id].state[MPIDR_AFFLVL0] = scpi_power_on;
+		target_pstate[linear_id].state[MPIDR_AFFLVL1] = scpi_power_on;
+	}
+
 	return PSCI_E_SUCCESS;
 }
 
+/*******************************************************************************
+ * Handler called when an affinity instance is about to be turned off. The
+ * level and mpidr determine the affinity instance. The 'state' arg. allows the
+ * platform to decide whether the cluster is being turned off and take apt
+ * actions.
+ *
+ * CAUTION: There is no guarantee that caches will remain turned on across calls
+ * to this function as each affinity level is dealt with. So do not write & read
+ * global variables across calls. It will be wise to do flush a write to the
+ * global to prevent unpredictable results.
+ ******************************************************************************/
+int pm_off(unsigned long mpidr, unsigned int afflvl, unsigned int state)
+{
+	return juno_power_down_common(mpidr,
+				      platform_get_core_pos(mpidr),
+				      afflvl,
+				      plat_get_max_afflvl(),
+				      state);
+}
+
+/*******************************************************************************
+ * Handler called when an affinity instance is about to be suspended. The
+ * level and mpidr determine the affinity instance. The 'state' arg. allows the
+ * platform to decide whether the cluster is being turned off and take apt
+ * actions. The 'sec_entrypoint' determines the address in BL3-1 from where
+ * execution should resume.
+ *
+ * CAUTION: There is no guarantee that caches will remain turned on across calls
+ * to this function as each affinity level is dealt with. So do not write & read
+ * global variables across calls. It will be wise to do flush a write to the
+ * global to prevent unpredictable results.
+ ******************************************************************************/
+int pm_suspend(unsigned long mpidr,
+	       unsigned long sec_entrypoint,
+	       unsigned long ns_entrypoint,
+	       unsigned int afflvl,
+	       unsigned int state)
+{
+	uint32_t tgt_afflvl;
+
+	tgt_afflvl = psci_get_suspend_afflvl(mpidr);
+	assert(tgt_afflvl != PSCI_INVALID_DATA);
+
+	/*
+	 * Setup mailbox with address for CPU entrypoint when it next powers up
+	 */
+	if (afflvl == MPIDR_AFFLVL0) {
+		unsigned long *mbox = (unsigned long *)(unsigned long)(
+			TRUSTED_MAILBOXES_BASE +
+			(platform_get_core_pos(mpidr) << TRUSTED_MAILBOX_SHIFT)
+			);
+		*mbox = sec_entrypoint;
+		flush_dcache_range((unsigned long)mbox, sizeof(*mbox));
+	}
+
+	return juno_power_down_common(mpidr,
+				      platform_get_core_pos(mpidr),
+				      afflvl,
+				      tgt_afflvl,
+				      state);
+}
+
+int pm_suspend_finish(unsigned long mpidr,
+		      unsigned int afflvl,
+		      unsigned int state)
+{
+	return pm_on_finish(mpidr, afflvl, state);
+}
+
 /*******************************************************************************
  * Export the platform handlers to enable psci to invoke them
  ******************************************************************************/
 static const plat_pm_ops_t pm_ops = {
 	.affinst_on		= pm_on,
 	.affinst_on_finish	= pm_on_finish,
-	.affinst_off		= pm_off
+	.affinst_off		= pm_off,
+	.affinst_suspend	= pm_suspend,
+	.affinst_suspend_finish	= pm_suspend_finish
 };
 
 /*******************************************************************************