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/include/lib/psci/psci.h b/include/lib/psci/psci.h
index b56e98b5f327261ef5f17300791b786dbff65ae6..8aaf4879ea82bc020a800021dd27e68113bcc8e9 100644 (file)
--- a/include/lib/psci/psci.h
+++ b/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);
 

diff --git a/include/lib/psci/psci_lib.h b/include/lib/psci/psci_lib.h
index 3edc50b6cec122a680d0deeb5e09d7cfec0943fb..4b244ec3347f1421f79511ce2fe6c51b09e7cbfb 100644 (file)
--- a/include/lib/psci/psci_lib.h
+++ b/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__ */

diff --git a/lib/psci/psci_common.c b/lib/psci/psci_common.c
index f233be1f10ba7a9076fe04fbd51c6d0ea58d5dba..5ce562feb70a9e82958c81bd1bfe282d3e0f6842 100644 (file)
--- a/lib/psci/psci_common.c
+++ b/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;
+}

diff --git a/lib/psci/psci_main.c b/lib/psci/psci_main.c
index a631f3ffbf8498fd35d4bb8dac58c6710c9025c1..4b5fc81d78d8e693d2e93fd34af21e84599dea75 100644 (file)
--- a/lib/psci/psci_main.c
+++ b/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);

diff --git a/lib/psci/psci_private.h b/lib/psci/psci_private.h
index 6ca9ef694e763ecfe4abc820f691e26750c66191..73b161edcda8adb7a771a4e05826922f19b748d4 100644 (file)
--- a/lib/psci/psci_private.h
+++ b/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

diff --git a/lib/psci/psci_setup.c b/lib/psci/psci_setup.c
index 3cb4f7e43e779de97c18e5a761daa162c047d763..16d6e45319b9385b409f19c3903e4a123f6fc798 100644 (file)
--- a/lib/psci/psci_setup.c
+++ b/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);