Merge pull request #324 from soby-mathew/sm/sys_suspend

PSCI: Add SYSTEM_SUSPEND API support

4 files changed, 106 insertions, 4 deletions

diff --git a/services/std_svc/psci/psci_common.c b/services/std_svc/psci/psci_common.c
index 63d84760..1b74ff2c 100644
--- a/services/std_svc/psci/psci_common.c
+++ b/services/std_svc/psci/psci_common.c
@@ -92,6 +92,38 @@ uint32_t psci_find_max_phys_off_afflvl(uint32_t start_afflvl,
 }
 
 /*******************************************************************************
+ * This function verifies that the all the other cores in the system have been
+ * turned OFF and the current CPU is the last running CPU in the system.
+ * Returns 1 (true) if the current CPU is the last ON CPU or 0 (false)
+ * otherwise.
+ ******************************************************************************/
+unsigned int psci_is_last_on_cpu(void)
+{
+	unsigned long mpidr = read_mpidr_el1() & MPIDR_AFFINITY_MASK;
+	unsigned int i;
+
+	for (i = psci_aff_limits[MPIDR_AFFLVL0].min;
+			i <= psci_aff_limits[MPIDR_AFFLVL0].max; i++) {
+
+		assert(psci_aff_map[i].level == MPIDR_AFFLVL0);
+
+		if (!(psci_aff_map[i].state & PSCI_AFF_PRESENT))
+			continue;
+
+		if (psci_aff_map[i].mpidr == mpidr) {
+			assert(psci_get_state(&psci_aff_map[i])
+					== PSCI_STATE_ON);
+			continue;
+		}
+
+		if (psci_get_state(&psci_aff_map[i]) != PSCI_STATE_OFF)
+			return 0;
+	}
+
+	return 1;
+}
+
+/*******************************************************************************
  * This function saves the highest affinity level which is in OFF state. The
  * affinity instance with which the level is associated is determined by the
  * caller.
diff --git a/services/std_svc/psci/psci_main.c b/services/std_svc/psci/psci_main.c
index fcd3b552..b389287b 100644
--- a/services/std_svc/psci/psci_main.c
+++ b/services/std_svc/psci/psci_main.c
@@ -31,9 +31,10 @@
 #include <arch.h>
 #include <arch_helpers.h>
 #include <assert.h>
+#include <debug.h>
+#include <platform.h>
 #include <runtime_svc.h>
 #include <std_svc.h>
-#include <debug.h>
 #include "psci_private.h"
 
 /*******************************************************************************
@@ -167,6 +168,62 @@ int psci_cpu_suspend(unsigned int power_state,
 	return PSCI_E_SUCCESS;
 }
 
+int psci_system_suspend(unsigned long entrypoint,
+			unsigned long context_id)
+{
+	int rc;
+	unsigned int power_state;
+	entry_point_info_t ep;
+
+	/* Validate the entrypoint using platform pm_ops */
+	if (psci_plat_pm_ops->validate_ns_entrypoint) {
+		rc = psci_plat_pm_ops->validate_ns_entrypoint(entrypoint);
+		if (rc != PSCI_E_SUCCESS) {
+			assert(rc == PSCI_E_INVALID_PARAMS);
+			return PSCI_E_INVALID_PARAMS;
+		}
+	}
+
+	/* Check if the current CPU is the last ON CPU in the system */
+	if (!psci_is_last_on_cpu())
+		return PSCI_E_DENIED;
+
+	/*
+	 * Verify and derive the re-entry information for
+	 * the non-secure world from the non-secure state from
+	 * where this call originated.
+	 */
+	rc = psci_get_ns_ep_info(&ep, entrypoint, context_id);
+	if (rc != PSCI_E_SUCCESS)
+		return rc;
+
+	/*
+	 * Assert that the required pm_ops hook is implemented to ensure that
+	 * the capability detected during psci_setup() is valid.
+	 */
+	assert(psci_plat_pm_ops->get_sys_suspend_power_state);
+
+	/*
+	 * Query the platform for the power_state required for system suspend
+	 */
+	power_state = psci_plat_pm_ops->get_sys_suspend_power_state();
+
+	/* Save PSCI power state parameter for the core in suspend context */
+	psci_set_suspend_power_state(power_state);
+
+	/*
+	 * Do what is needed to enter the power down state. Upon success,
+	 * enter the final wfi which will power down this cpu.
+	 */
+	psci_afflvl_suspend(&ep,
+			    MPIDR_AFFLVL0,
+			    PLATFORM_MAX_AFFLVL);
+
+	/* Reset PSCI power state parameter for the core. */
+	psci_set_suspend_power_state(PSCI_INVALID_DATA);
+	return PSCI_E_SUCCESS;
+}
+
 int psci_cpu_off(void)
 {
 	int rc;
@@ -357,6 +414,9 @@ uint64_t psci_smc_handler(uint32_t smc_fid,
 		case PSCI_MIG_INFO_UP_CPU_AARCH32:
 			SMC_RET1(handle, psci_migrate_info_up_cpu());
 
+		case PSCI_SYSTEM_SUSPEND_AARCH32:
+			SMC_RET1(handle, psci_system_suspend(x1, x2));
+
 		case PSCI_SYSTEM_OFF:
 			psci_system_off();
 			/* We should never return from psci_system_off() */
@@ -390,6 +450,9 @@ uint64_t psci_smc_handler(uint32_t smc_fid,
 		case PSCI_MIG_INFO_UP_CPU_AARCH64:
 			SMC_RET1(handle, psci_migrate_info_up_cpu());
 
+		case PSCI_SYSTEM_SUSPEND_AARCH64:
+			SMC_RET1(handle, psci_system_suspend(x1, x2));
+
 		default:
 			break;
 		}
diff --git a/services/std_svc/psci/psci_private.h b/services/std_svc/psci/psci_private.h
index 62a0efc8..2955de7a 100644
--- a/services/std_svc/psci/psci_private.h
+++ b/services/std_svc/psci/psci_private.h
@@ -69,7 +69,8 @@
 			define_psci_cap(PSCI_CPU_ON_AARCH64) |		\
 			define_psci_cap(PSCI_AFFINITY_INFO_AARCH64) |	\
 			define_psci_cap(PSCI_MIG_AARCH64) |		\
-			define_psci_cap(PSCI_MIG_INFO_UP_CPU_AARCH64))
+			define_psci_cap(PSCI_MIG_INFO_UP_CPU_AARCH64) |	\
+			define_psci_cap(PSCI_SYSTEM_SUSPEND_AARCH64))
 
 
 /*******************************************************************************
@@ -102,6 +103,7 @@ typedef void (*afflvl_power_on_finisher_t)(aff_map_node_t *);
  ******************************************************************************/
 extern const plat_pm_ops_t *psci_plat_pm_ops;
 extern aff_map_node_t psci_aff_map[PSCI_NUM_AFFS];
+extern aff_limits_node_t psci_aff_limits[MPIDR_MAX_AFFLVL + 1];
 extern uint32_t psci_caps;
 
 /*******************************************************************************
@@ -140,6 +142,7 @@ void psci_set_max_phys_off_afflvl(uint32_t afflvl);
 uint32_t psci_find_max_phys_off_afflvl(uint32_t start_afflvl,
 				       uint32_t end_afflvl,
 				       aff_map_node_t *mpidr_nodes[]);
+unsigned int psci_is_last_on_cpu(void);
 int psci_spd_migrate_info(uint64_t *mpidr);
 
 /* Private exported functions from psci_setup.c */
diff --git a/services/std_svc/psci/psci_setup.c b/services/std_svc/psci/psci_setup.c
index 5ff24d5b..01b559cf 100644
--- a/services/std_svc/psci/psci_setup.c
+++ b/services/std_svc/psci/psci_setup.c
@@ -55,7 +55,7 @@ static cpu_context_t psci_ns_context[PLATFORM_CORE_COUNT];
  * level i.e. start index and end index needs to be present. 'psci_aff_limits'
  * stores this information.
  ******************************************************************************/
-static aff_limits_node_t psci_aff_limits[MPIDR_MAX_AFFLVL + 1];
+aff_limits_node_t psci_aff_limits[MPIDR_MAX_AFFLVL + 1];
 
 /******************************************************************************
  * Define the psci capability variable.
@@ -385,8 +385,12 @@ int32_t psci_setup(void)
 		psci_caps |=  define_psci_cap(PSCI_CPU_OFF);
 	if (psci_plat_pm_ops->affinst_on && psci_plat_pm_ops->affinst_on_finish)
 		psci_caps |=  define_psci_cap(PSCI_CPU_ON_AARCH64);
-	if (psci_plat_pm_ops->affinst_suspend && psci_plat_pm_ops->affinst_suspend_finish)
+	if (psci_plat_pm_ops->affinst_suspend &&
+			psci_plat_pm_ops->affinst_suspend_finish) {
 		psci_caps |=  define_psci_cap(PSCI_CPU_SUSPEND_AARCH64);
+		if (psci_plat_pm_ops->get_sys_suspend_power_state)
+			psci_caps |=  define_psci_cap(PSCI_SYSTEM_SUSPEND_AARCH64);
+	}
 	if (psci_plat_pm_ops->system_off)
 		psci_caps |=  define_psci_cap(PSCI_SYSTEM_OFF);
 	if (psci_plat_pm_ops->system_reset)