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

PSCI: Add SYSTEM_SUSPEND API support

docs/firmware-design.md

@@ -755,7 +755,7 @@ required support.
 |`CPU_FREEZE`           | No      |                                           |
 |`CPU_DEFAULT_SUSPEND`  | No      |                                           |
 |`CPU_HW_STATE`         | No      |                                           |
-|`SYSTEM_SUSPEND`       | No      |                                           |
+|`SYSTEM_SUSPEND`       | Yes*    |                                           |
 |`PSCI_SET_SUSPEND_MODE`| No      |                                           |
 |`PSCI_STAT_RESIDENCY`  | No      |                                           |
 |`PSCI_STAT_COUNT`      | No      |                                           |

docs/porting-guide.md

@@ -1238,8 +1238,8 @@ affinity level 0 (CPU), the platform port should power down affinity level 1
 #### plat_pm_ops.affinst_suspend()
 
 Perform the platform specific setup to power off an affinity instance of the
-calling CPU. It is called by the PSCI `CPU_SUSPEND` API
-implementation.
+calling CPU. It is called by the PSCI `CPU_SUSPEND` API and `SYSTEM_SUSPEND`
+API implementation
 
 The `affinity level` (second argument) and `state` (third argument) have a
 similar meaning as described in the `affinst_on()` operation. They are used to
@@ -1268,14 +1268,14 @@ The `affinity level` (first argument) and `state` (second argument) have a
 similar meaning as described in the previous operations. The generic code
 expects the handler to succeed.
 
-#### plat_pm_ops.affinst_on_suspend()
+#### plat_pm_ops.affinst_suspend_finish()
 
 This function is called by the PSCI implementation after the calling CPU is
 powered on and released from reset in response to an asynchronous wakeup
 event, for example a timer interrupt that was programmed by the CPU during the
-`CPU_SUSPEND` call. It performs the platform-specific setup required to
-restore the saved state for this CPU to resume execution in the normal world
-and also provide secure runtime firmware services.
+`CPU_SUSPEND` call or `SYSTEM_SUSPEND` call. It performs the platform-specific
+setup required to restore the saved state for this CPU to resume execution
+in the normal world and also provide secure runtime firmware services.
 
 The `affinity level` (first argument) and `state` (second argument) have a
 similar meaning as described in the previous operations. The generic code
@@ -1291,11 +1291,20 @@ world PSCI client.
 
 #### plat_pm_ops.validate_ns_entrypoint()
 
-This function is called by the PSCI implementation during the `CPU_SUSPEND`
-and `CPU_ON` calls to validate the non-secure `entry_point` parameter passed
-by the normal world. If the `entry_point` is known to be invalid, the platform
-must return PSCI_E_INVALID_PARAMS as error, which is propagated back to the
-normal world PSCI client.
+This function is called by the PSCI implementation during the `CPU_SUSPEND`,
+`SYSTEM_SUSPEND` and `CPU_ON` calls to validate the non-secure `entry_point`
+parameter passed by the normal world. If the `entry_point` is known to be
+invalid, the platform must return PSCI_E_INVALID_PARAMS as error, which is
+propagated back to the normal world PSCI client.
+
+#### plat_pm_ops.get_sys_suspend_power_state()
+
+This function is called by the PSCI implementation during the `SYSTEM_SUSPEND`
+call to return the `power_state` parameter. This allows the platform to encode
+the appropriate State-ID field within the `power_state` parameter which can be
+utilized in `affinst_suspend()` to suspend to system affinity level. The
+`power_state` parameter should be in the same format as specified by the
+PSCI specification for the CPU_SUSPEND API.
 
 BL3-1 platform initialization code must also detect the system topology and
 the state of each affinity instance in the topology. This information is

include/bl31/services/psci.h

@@ -62,6 +62,8 @@
 #define PSCI_SYSTEM_OFF			0x84000008
 #define PSCI_SYSTEM_RESET		0x84000009
 #define PSCI_FEATURES			0x8400000A
+#define PSCI_SYSTEM_SUSPEND_AARCH32	0x8400000E
+#define PSCI_SYSTEM_SUSPEND_AARCH64	0xc400000E
 
 /* Macro to help build the psci capabilities bitfield */
 #define define_psci_cap(x)		(1 << (x & 0x1f))
@@ -69,7 +71,7 @@
 /*
  * Number of PSCI calls (above) implemented
  */
-#define PSCI_NUM_CALLS			16
+#define PSCI_NUM_CALLS			18
 
 /*******************************************************************************
  * PSCI Migrate and friends
@@ -93,12 +95,16 @@
 #define PSTATE_TYPE_STANDBY	0x0
 #define PSTATE_TYPE_POWERDOWN	0x1
 
-#define psci_get_pstate_id(pstate)	((pstate >> PSTATE_ID_SHIFT) & \
+#define psci_get_pstate_id(pstate)	(((pstate) >> PSTATE_ID_SHIFT) & \
 					PSTATE_ID_MASK)
-#define psci_get_pstate_type(pstate)	((pstate >> PSTATE_TYPE_SHIFT) & \
+#define psci_get_pstate_type(pstate)	(((pstate) >> PSTATE_TYPE_SHIFT) & \
 					PSTATE_TYPE_MASK)
-#define psci_get_pstate_afflvl(pstate)	((pstate >> PSTATE_AFF_LVL_SHIFT) & \
+#define psci_get_pstate_afflvl(pstate)	(((pstate) >> PSTATE_AFF_LVL_SHIFT) & \
 					PSTATE_AFF_LVL_MASK)
+#define psci_make_powerstate(state_id, type, afflvl) \
+			(((state_id) & PSTATE_ID_MASK) << PSTATE_ID_SHIFT) |\
+			(((type) & PSTATE_TYPE_MASK) << PSTATE_TYPE_SHIFT) |\
+			(((afflvl) & PSTATE_AFF_LVL_MASK) << PSTATE_AFF_LVL_SHIFT)
 
 /*******************************************************************************
  * PSCI CPU_FEATURES feature flag specific defines
@@ -193,6 +199,7 @@ typedef struct plat_pm_ops {
 	void (*system_reset)(void) __dead2;
 	int (*validate_power_state)(unsigned int power_state);
 	int (*validate_ns_entrypoint)(unsigned long ns_entrypoint);
+	unsigned int (*get_sys_suspend_power_state)(void);
 } plat_pm_ops_t;
 
 /*******************************************************************************

services/std_svc/psci/psci_common.c

@@ -91,6 +91,38 @@ uint32_t psci_find_max_phys_off_afflvl(uint32_t start_afflvl,
 	return max_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

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;
 		}

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

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)