feat(psci): add support for PSCI_SET_SUSPEND_MODE

This patch adds a PSCI_SET_SUSPEND_MODE handler that validates the
request per section 5.20.2 of the PSCI spec (DEN0022D.b), and updates
the suspend mode to the requested mode.

This is conditionally compiled into the build depending on the value of
the `PSCI_OS_INIT_MODE` build option.

Change-Id: Iebf65f5f7846aef6b8643ad6082db99b4dcc4bef
Signed-off-by: Wing Li <wingers@google.com>

include/lib/psci/psci.h

@@ -59,6 +59,7 @@
 #define PSCI_NODE_HW_STATE_AARCH64	U(0xc400000d)
 #define PSCI_SYSTEM_SUSPEND_AARCH32	U(0x8400000E)
 #define PSCI_SYSTEM_SUSPEND_AARCH64	U(0xc400000E)
+#define PSCI_SET_SUSPEND_MODE		U(0x8400000F)
 #define PSCI_STAT_RESIDENCY_AARCH32	U(0x84000010)
 #define PSCI_STAT_RESIDENCY_AARCH64	U(0xc4000010)
 #define PSCI_STAT_COUNT_AARCH32		U(0x84000011)
@@ -73,9 +74,17 @@
  * Number of PSCI calls (above) implemented
  */
 #if ENABLE_PSCI_STAT
-#define PSCI_NUM_CALLS			U(22)
+#if PSCI_OS_INIT_MODE
+#define PSCI_NUM_CALLS			U(30)
 #else
-#define PSCI_NUM_CALLS			U(18)
+#define PSCI_NUM_CALLS			U(29)
+#endif
+#else
+#if PSCI_OS_INIT_MODE
+#define PSCI_NUM_CALLS			U(26)
+#else
+#define PSCI_NUM_CALLS			U(25)
+#endif
 #endif
 
 /* The macros below are used to identify PSCI calls from the SMC function ID */
@@ -347,6 +356,9 @@ u_register_t psci_migrate_info_up_cpu(void);
 int psci_node_hw_state(u_register_t target_cpu,
 		       unsigned int power_level);
 int psci_features(unsigned int psci_fid);
+#if PSCI_OS_INIT_MODE
+int psci_set_suspend_mode(unsigned int mode);
+#endif
 void __dead2 psci_power_down_wfi(void);
 void psci_arch_setup(void);
 

include/lib/psci/psci_lib.h

@@ -92,6 +92,7 @@ void psci_prepare_next_non_secure_ctx(
 int psci_stop_other_cores(unsigned int wait_ms,
 			  void (*stop_func)(u_register_t mpidr));
 bool psci_is_last_on_cpu_safe(void);
+bool psci_are_all_cpus_on_safe(void);
 void psci_pwrdown_cpu(unsigned int power_level);
 
 #endif /* __ASSEMBLER__ */

lib/psci/psci_common.c

@@ -76,6 +76,14 @@ CASSERT((PLAT_MAX_PWR_LVL <= PSCI_MAX_PWR_LVL) &&
 	(PLAT_MAX_PWR_LVL >= PSCI_CPU_PWR_LVL),
 	assert_platform_max_pwrlvl_check);
 
+#if PSCI_OS_INIT_MODE
+/*******************************************************************************
+ * The power state coordination mode used in CPU_SUSPEND.
+ * Defaults to platform-coordinated mode.
+ ******************************************************************************/
+suspend_mode_t psci_suspend_mode = PLAT_COORD;
+#endif
+
 /*
  * The plat_local_state used by the platform is one of these types: RUN,
  * RETENTION and OFF. The platform can define further sub-states for each type
@@ -154,7 +162,7 @@ void psci_query_sys_suspend_pwrstate(psci_power_state_t *state_info)
 }
 
 /*******************************************************************************
- * This function verifies that the all the other cores in the system have been
+ * This function verifies that all the other cores in the system have been
  * turned OFF and the current CPU is the last running CPU in the system.
  * Returns true, if the current CPU is the last ON CPU or false otherwise.
  ******************************************************************************/
@@ -178,6 +186,23 @@ bool psci_is_last_on_cpu(void)
 	return true;
 }
 
+/*******************************************************************************
+ * This function verifies that all cores in the system have been turned ON.
+ * Returns true, if all CPUs are ON or false otherwise.
+ ******************************************************************************/
+static bool psci_are_all_cpus_on(void)
+{
+	unsigned int cpu_idx;
+
+	for (cpu_idx = 0; cpu_idx < psci_plat_core_count; cpu_idx++) {
+		if (psci_get_aff_info_state_by_idx(cpu_idx) == AFF_STATE_OFF) {
+			return false;
+		}
+	}
+
+	return true;
+}
+
 /*******************************************************************************
  * Routine to return the maximum power level to traverse to after a cpu has
  * been physically powered up. It is expected to be called immediately after
@@ -1050,3 +1075,29 @@ bool psci_is_last_on_cpu_safe(void)
 
 	return true;
 }
+
+/*******************************************************************************
+ * This function verifies that all cores in the system have been turned ON.
+ * Returns true, if all CPUs are ON or false otherwise.
+ *
+ * This API has following differences with psci_are_all_cpus_on
+ *  1. PSCI states are locked
+ ******************************************************************************/
+bool psci_are_all_cpus_on_safe(void)
+{
+	unsigned int this_core = plat_my_core_pos();
+	unsigned int parent_nodes[PLAT_MAX_PWR_LVL] = {0};
+
+	psci_get_parent_pwr_domain_nodes(this_core, PLAT_MAX_PWR_LVL, parent_nodes);
+
+	psci_acquire_pwr_domain_locks(PLAT_MAX_PWR_LVL, parent_nodes);
+
+	if (!psci_are_all_cpus_on()) {
+		psci_release_pwr_domain_locks(PLAT_MAX_PWR_LVL, parent_nodes);
+		return false;
+	}
+
+	psci_release_pwr_domain_locks(PLAT_MAX_PWR_LVL, parent_nodes);
+
+	return true;
+}

lib/psci/psci_main.c

@@ -370,6 +370,39 @@ int psci_features(unsigned int psci_fid)
 	return PSCI_E_SUCCESS;
 }
 
+#if PSCI_OS_INIT_MODE
+int psci_set_suspend_mode(unsigned int mode)
+{
+	if (psci_suspend_mode == mode) {
+		return PSCI_E_SUCCESS;
+	}
+
+	if (mode == PLAT_COORD) {
+		/* Check if the current CPU is the last ON CPU in the system */
+		if (!psci_is_last_on_cpu_safe()) {
+			return PSCI_E_DENIED;
+		}
+	}
+
+	if (mode == OS_INIT) {
+		/*
+		 * Check if all CPUs in the system are ON or if the current
+		 * CPU is the last ON CPU in the system.
+		 */
+		if (!(psci_are_all_cpus_on_safe() ||
+		      psci_is_last_on_cpu_safe())) {
+			return PSCI_E_DENIED;
+		}
+	}
+
+	psci_suspend_mode = mode;
+	psci_flush_dcache_range((uintptr_t)&psci_suspend_mode,
+				sizeof(psci_suspend_mode));
+
+	return PSCI_E_SUCCESS;
+}
+#endif
+
 /*******************************************************************************
  * PSCI top level handler for servicing SMCs.
  ******************************************************************************/
@@ -453,6 +486,12 @@ u_register_t psci_smc_handler(uint32_t smc_fid,
 			ret = (u_register_t)psci_features(r1);
 			break;
 
+#if PSCI_OS_INIT_MODE
+		case PSCI_SET_SUSPEND_MODE:
+			ret = (u_register_t)psci_set_suspend_mode(r1);
+			break;
+#endif
+
 #if ENABLE_PSCI_STAT
 		case PSCI_STAT_RESIDENCY_AARCH32:
 			ret = psci_stat_residency(r1, r2);
@@ -515,6 +554,12 @@ u_register_t psci_smc_handler(uint32_t smc_fid,
 			ret = (u_register_t)psci_system_suspend(x1, x2);
 			break;
 
+#if PSCI_OS_INIT_MODE
+		case PSCI_SET_SUSPEND_MODE:
+			ret = (u_register_t)psci_set_suspend_mode(x1);
+			break;
+#endif
+
 #if ENABLE_PSCI_STAT
 		case PSCI_STAT_RESIDENCY_AARCH64:
 			ret = psci_stat_residency(x1, (unsigned int) x2);

lib/psci/psci_private.h

@@ -163,6 +163,16 @@ typedef struct cpu_pwr_domain_node {
 	spinlock_t cpu_lock;
 } cpu_pd_node_t;
 
+#if PSCI_OS_INIT_MODE
+/*******************************************************************************
+ * The supported power state coordination modes that can be used in CPU_SUSPEND.
+ ******************************************************************************/
+typedef enum suspend_mode {
+	PLAT_COORD = 0,
+	OS_INIT = 1
+} suspend_mode_t;
+#endif
+
 /*******************************************************************************
  * The following are helpers and declarations of locks.
  ******************************************************************************/
@@ -260,6 +270,9 @@ extern non_cpu_pd_node_t psci_non_cpu_pd_nodes[PSCI_NUM_NON_CPU_PWR_DOMAINS];
 extern cpu_pd_node_t psci_cpu_pd_nodes[PLATFORM_CORE_COUNT];
 extern unsigned int psci_caps;
 extern unsigned int psci_plat_core_count;
+#if PSCI_OS_INIT_MODE
+extern suspend_mode_t psci_suspend_mode;
+#endif
 
 /*******************************************************************************
  * SPD's power management hooks registered with PSCI

lib/psci/psci_setup.c

@@ -254,6 +254,9 @@ int __init psci_setup(const psci_lib_args_t *lib_args)
 			psci_caps |=  define_psci_cap(PSCI_CPU_SUSPEND_AARCH64);
 		if (psci_plat_pm_ops->get_sys_suspend_power_state != NULL)
 			psci_caps |=  define_psci_cap(PSCI_SYSTEM_SUSPEND_AARCH64);
+#if PSCI_OS_INIT_MODE
+		psci_caps |= define_psci_cap(PSCI_SET_SUSPEND_MODE);
+#endif
 	}
 	if (psci_plat_pm_ops->system_off != NULL)
 		psci_caps |=  define_psci_cap(PSCI_SYSTEM_OFF);