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>
diff --git a/lib/psci/psci_main.c b/lib/psci/psci_main.c
index a631f3f..4b5fc81 100644
--- a/lib/psci/psci_main.c
+++ b/lib/psci/psci_main.c
@@ -370,6 +370,39 @@
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.
******************************************************************************/
@@ -452,6 +485,12 @@
case PSCI_FEATURES:
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:
@@ -515,6 +554,12 @@
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);