Merge tag 'arm64-fixes' of git://git.kernel.org/pub/scm/linux/kernel/git/arm64/linux

[firefly-linux-kernel-4.4.55.git] / drivers / firmware / psci.c

diff --git a/drivers/firmware/psci.c b/drivers/firmware/psci.c
index 42700f09a8c5845f418709c8802c08d6043c2f10..d24f35d74b27079afeae5c08d601c3bcd899dee6 100644 (file)
--- a/drivers/firmware/psci.c
+++ b/drivers/firmware/psci.c
@@ -20,23 +20,25 @@
 #include <linux/printk.h>
 #include <linux/psci.h>
 #include <linux/reboot.h>
+#include <linux/suspend.h>
 
 #include <uapi/linux/psci.h>
 
 #include <asm/cputype.h>
 #include <asm/system_misc.h>
 #include <asm/smp_plat.h>
+#include <asm/suspend.h>
 
 /*
  * While a 64-bit OS can make calls with SMC32 calling conventions, for some
- * calls it is necessary to use SMC64 to pass or return 64-bit values. For such
- * calls PSCI_0_2_FN_NATIVE(x) will choose the appropriate (native-width)
- * function ID.
+ * calls it is necessary to use SMC64 to pass or return 64-bit values.
+ * For such calls PSCI_FN_NATIVE(version, name) will choose the appropriate
+ * (native-width) function ID.
  */
 #ifdef CONFIG_64BIT
-#define PSCI_0_2_FN_NATIVE(name)       PSCI_0_2_FN64_##name
+#define PSCI_FN_NATIVE(version, name)  PSCI_##version##_FN64_##name
 #else
-#define PSCI_0_2_FN_NATIVE(name)       PSCI_0_2_FN_##name
+#define PSCI_FN_NATIVE(version, name)  PSCI_##version##_FN_##name
 #endif
 
 /*
@@ -70,6 +72,41 @@ enum psci_function {
 
 static u32 psci_function_id[PSCI_FN_MAX];
 
+#define PSCI_0_2_POWER_STATE_MASK              \
+                               (PSCI_0_2_POWER_STATE_ID_MASK | \
+                               PSCI_0_2_POWER_STATE_TYPE_MASK | \
+                               PSCI_0_2_POWER_STATE_AFFL_MASK)
+
+#define PSCI_1_0_EXT_POWER_STATE_MASK          \
+                               (PSCI_1_0_EXT_POWER_STATE_ID_MASK | \
+                               PSCI_1_0_EXT_POWER_STATE_TYPE_MASK)
+
+static u32 psci_cpu_suspend_feature;
+
+static inline bool psci_has_ext_power_state(void)
+{
+       return psci_cpu_suspend_feature &
+                               PSCI_1_0_FEATURES_CPU_SUSPEND_PF_MASK;
+}
+
+bool psci_power_state_loses_context(u32 state)
+{
+       const u32 mask = psci_has_ext_power_state() ?
+                                       PSCI_1_0_EXT_POWER_STATE_TYPE_MASK :
+                                       PSCI_0_2_POWER_STATE_TYPE_MASK;
+
+       return state & mask;
+}
+
+bool psci_power_state_is_valid(u32 state)
+{
+       const u32 valid_mask = psci_has_ext_power_state() ?
+                              PSCI_1_0_EXT_POWER_STATE_MASK :
+                              PSCI_0_2_POWER_STATE_MASK;
+
+       return !(state & ~valid_mask);
+}
+
 static int psci_to_linux_errno(int errno)
 {
        switch (errno) {
@@ -78,6 +115,7 @@ static int psci_to_linux_errno(int errno)
        case PSCI_RET_NOT_SUPPORTED:
                return -EOPNOTSUPP;
        case PSCI_RET_INVALID_PARAMS:
+       case PSCI_RET_INVALID_ADDRESS:
                return -EINVAL;
        case PSCI_RET_DENIED:
                return -EPERM;
@@ -134,7 +172,7 @@ static int psci_migrate(unsigned long cpuid)
 static int psci_affinity_info(unsigned long target_affinity,
                unsigned long lowest_affinity_level)
 {
-       return invoke_psci_fn(PSCI_0_2_FN_NATIVE(AFFINITY_INFO),
+       return invoke_psci_fn(PSCI_FN_NATIVE(0_2, AFFINITY_INFO),
                              target_affinity, lowest_affinity_level, 0);
 }
 
@@ -145,7 +183,7 @@ static int psci_migrate_info_type(void)
 
 static unsigned long psci_migrate_info_up_cpu(void)
 {
-       return invoke_psci_fn(PSCI_0_2_FN_NATIVE(MIGRATE_INFO_UP_CPU),
+       return invoke_psci_fn(PSCI_FN_NATIVE(0_2, MIGRATE_INFO_UP_CPU),
                              0, 0, 0);
 }
 
@@ -181,6 +219,49 @@ static void psci_sys_poweroff(void)
        invoke_psci_fn(PSCI_0_2_FN_SYSTEM_OFF, 0, 0, 0);
 }
 
+static int __init psci_features(u32 psci_func_id)
+{
+       return invoke_psci_fn(PSCI_1_0_FN_PSCI_FEATURES,
+                             psci_func_id, 0, 0);
+}
+
+static int psci_system_suspend(unsigned long unused)
+{
+       return invoke_psci_fn(PSCI_FN_NATIVE(1_0, SYSTEM_SUSPEND),
+                             virt_to_phys(cpu_resume), 0, 0);
+}
+
+static int psci_system_suspend_enter(suspend_state_t state)
+{
+       return cpu_suspend(0, psci_system_suspend);
+}
+
+static const struct platform_suspend_ops psci_suspend_ops = {
+       .valid          = suspend_valid_only_mem,
+       .enter          = psci_system_suspend_enter,
+};
+
+static void __init psci_init_system_suspend(void)
+{
+       int ret;
+
+       if (!IS_ENABLED(CONFIG_SUSPEND))
+               return;
+
+       ret = psci_features(PSCI_FN_NATIVE(1_0, SYSTEM_SUSPEND));
+
+       if (ret != PSCI_RET_NOT_SUPPORTED)
+               suspend_set_ops(&psci_suspend_ops);
+}
+
+static void __init psci_init_cpu_suspend(void)
+{
+       int feature = psci_features(psci_function_id[PSCI_FN_CPU_SUSPEND]);
+
+       if (feature != PSCI_RET_NOT_SUPPORTED)
+               psci_cpu_suspend_feature = feature;
+}
+
 /*
  * Detect the presence of a resident Trusted OS which may cause CPU_OFF to
  * return DENIED (which would be fatal).
@@ -224,16 +305,17 @@ static void __init psci_init_migrate(void)
 static void __init psci_0_2_set_functions(void)
 {
        pr_info("Using standard PSCI v0.2 function IDs\n");
-       psci_function_id[PSCI_FN_CPU_SUSPEND] = PSCI_0_2_FN_NATIVE(CPU_SUSPEND);
+       psci_function_id[PSCI_FN_CPU_SUSPEND] =
+                                       PSCI_FN_NATIVE(0_2, CPU_SUSPEND);
        psci_ops.cpu_suspend = psci_cpu_suspend;
 
        psci_function_id[PSCI_FN_CPU_OFF] = PSCI_0_2_FN_CPU_OFF;
        psci_ops.cpu_off = psci_cpu_off;
 
-       psci_function_id[PSCI_FN_CPU_ON] = PSCI_0_2_FN_NATIVE(CPU_ON);
+       psci_function_id[PSCI_FN_CPU_ON] = PSCI_FN_NATIVE(0_2, CPU_ON);
        psci_ops.cpu_on = psci_cpu_on;
 
-       psci_function_id[PSCI_FN_MIGRATE] = PSCI_0_2_FN_NATIVE(MIGRATE);
+       psci_function_id[PSCI_FN_MIGRATE] = PSCI_FN_NATIVE(0_2, MIGRATE);
        psci_ops.migrate = psci_migrate;
 
        psci_ops.affinity_info = psci_affinity_info;
@@ -265,6 +347,11 @@ static int __init psci_probe(void)
 
        psci_init_migrate();
 
+       if (PSCI_VERSION_MAJOR(ver) >= 1) {
+               psci_init_cpu_suspend();
+               psci_init_system_suspend();
+       }
+
        return 0;
 }
 
@@ -340,6 +427,7 @@ out_put_node:
 static const struct of_device_id const psci_of_match[] __initconst = {
        { .compatible = "arm,psci",     .data = psci_0_1_init},
        { .compatible = "arm,psci-0.2", .data = psci_0_2_init},
+       { .compatible = "arm,psci-1.0", .data = psci_0_2_init},
        {},
 };