1 files changed, 194 insertions, 37 deletions

diff --git a/arch/arm64/kernel/psci.c b/arch/arm64/kernel/psci.c
index ea4828a4aa96..9e9798f91172 100644
--- a/arch/arm64/kernel/psci.c
+++ b/arch/arm64/kernel/psci.c
@@ -18,12 +18,17 @@
 #include <linux/init.h>
 #include <linux/of.h>
 #include <linux/smp.h>
+#include <linux/reboot.h>
+#include <linux/pm.h>
+#include <linux/delay.h>
+#include <uapi/linux/psci.h>
 
 #include <asm/compiler.h>
 #include <asm/cpu_ops.h>
 #include <asm/errno.h>
 #include <asm/psci.h>
 #include <asm/smp_plat.h>
+#include <asm/system_misc.h>
 
 #define PSCI_POWER_STATE_TYPE_STANDBY           0
 #define PSCI_POWER_STATE_TYPE_POWER_DOWN        1
@@ -40,58 +45,52 @@ struct psci_operations {
         int (*cpu_off)(struct psci_power_state state);
         int (*cpu_on)(unsigned long cpuid, unsigned long entry_point);
         int (*migrate)(unsigned long cpuid);
+        int (*affinity_info)(unsigned long target_affinity,
+                        unsigned long lowest_affinity_level);
+        int (*migrate_info_type)(void);
 };
 
 static struct psci_operations psci_ops;
 
 static int (*invoke_psci_fn)(u64, u64, u64, u64);
+typedef int (*psci_initcall_t)(const struct device_node *);
 
 enum psci_function {
         PSCI_FN_CPU_SUSPEND,
         PSCI_FN_CPU_ON,
         PSCI_FN_CPU_OFF,
         PSCI_FN_MIGRATE,
+        PSCI_FN_AFFINITY_INFO,
+        PSCI_FN_MIGRATE_INFO_TYPE,
         PSCI_FN_MAX,
 };
 
 static u32 psci_function_id[PSCI_FN_MAX];
 
-#define PSCI_RET_SUCCESS                0
-#define PSCI_RET_EOPNOTSUPP             -1
-#define PSCI_RET_EINVAL                 -2
-#define PSCI_RET_EPERM                  -3
-
 static int psci_to_linux_errno(int errno)
 {
         switch (errno) {
         case PSCI_RET_SUCCESS:
                 return 0;
-        case PSCI_RET_EOPNOTSUPP:
+        case PSCI_RET_NOT_SUPPORTED:
                 return -EOPNOTSUPP;
-        case PSCI_RET_EINVAL:
+        case PSCI_RET_INVALID_PARAMS:
                 return -EINVAL;
-        case PSCI_RET_EPERM:
+        case PSCI_RET_DENIED:
                 return -EPERM;
         };
 
         return -EINVAL;
 }
 
-#define PSCI_POWER_STATE_ID_MASK        0xffff
-#define PSCI_POWER_STATE_ID_SHIFT       0
-#define PSCI_POWER_STATE_TYPE_MASK      0x1
-#define PSCI_POWER_STATE_TYPE_SHIFT     16
-#define PSCI_POWER_STATE_AFFL_MASK      0x3
-#define PSCI_POWER_STATE_AFFL_SHIFT     24
-
 static u32 psci_power_state_pack(struct psci_power_state state)
 {
-        return  ((state.id & PSCI_POWER_STATE_ID_MASK)
-                        << PSCI_POWER_STATE_ID_SHIFT)   |
-                ((state.type & PSCI_POWER_STATE_TYPE_MASK)
-                        << PSCI_POWER_STATE_TYPE_SHIFT) |
-                ((state.affinity_level & PSCI_POWER_STATE_AFFL_MASK)
-                        << PSCI_POWER_STATE_AFFL_SHIFT);
+        return ((state.id << PSCI_0_2_POWER_STATE_ID_SHIFT)
+                        & PSCI_0_2_POWER_STATE_ID_MASK) |
+                ((state.type << PSCI_0_2_POWER_STATE_TYPE_SHIFT)
+                 & PSCI_0_2_POWER_STATE_TYPE_MASK) |
+                ((state.affinity_level << PSCI_0_2_POWER_STATE_AFFL_SHIFT)
+                 & PSCI_0_2_POWER_STATE_AFFL_MASK);
 }
 
 /*
@@ -128,6 +127,14 @@ static noinline int __invoke_psci_fn_smc(u64 function_id, u64 arg0, u64 arg1,
         return function_id;
 }
 
+static int psci_get_version(void)
+{
+        int err;
+
+        err = invoke_psci_fn(PSCI_0_2_FN_PSCI_VERSION, 0, 0, 0);
+        return err;
+}
+
 static int psci_cpu_suspend(struct psci_power_state state,
                             unsigned long entry_point)
 {
@@ -171,26 +178,36 @@ static int psci_migrate(unsigned long cpuid)
         return psci_to_linux_errno(err);
 }
 
-static const struct of_device_id psci_of_match[] __initconst = {
-        { .compatible = "arm,psci",     },
-        {},
-};
+static int psci_affinity_info(unsigned long target_affinity,
+                unsigned long lowest_affinity_level)
+{
+        int err;
+        u32 fn;
+
+        fn = psci_function_id[PSCI_FN_AFFINITY_INFO];
+        err = invoke_psci_fn(fn, target_affinity, lowest_affinity_level, 0);
+        return err;
+}
 
-void __init psci_init(void)
+static int psci_migrate_info_type(void)
 {
-        struct device_node *np;
-        const char *method;
-        u32 id;
+        int err;
+        u32 fn;
 
-        np = of_find_matching_node(NULL, psci_of_match);
-        if (!np)
-                return;
+        fn = psci_function_id[PSCI_FN_MIGRATE_INFO_TYPE];
+        err = invoke_psci_fn(fn, 0, 0, 0);
+        return err;
+}
 
-        pr_info("probing function IDs from device-tree\n");
+static int get_set_conduit_method(struct device_node *np)
+{
+        const char *method;
+
+        pr_info("probing for conduit method from DT.\n");
 
         if (of_property_read_string(np, "method", &method)) {
-                pr_warning("missing \"method\" property\n");
-                goto out_put_node;
+                pr_warn("missing \"method\" property\n");
+                return -ENXIO;
         }
 
         if (!strcmp("hvc", method)) {
@@ -198,10 +215,99 @@ void __init psci_init(void)
         } else if (!strcmp("smc", method)) {
                 invoke_psci_fn = __invoke_psci_fn_smc;
         } else {
-                pr_warning("invalid \"method\" property: %s\n", method);
+                pr_warn("invalid \"method\" property: %s\n", method);
+                return -EINVAL;
+        }
+        return 0;
+}
+
+static void psci_sys_reset(enum reboot_mode reboot_mode, const char *cmd)
+{
+        invoke_psci_fn(PSCI_0_2_FN_SYSTEM_RESET, 0, 0, 0);
+}
+
+static void psci_sys_poweroff(void)
+{
+        invoke_psci_fn(PSCI_0_2_FN_SYSTEM_OFF, 0, 0, 0);
+}
+
+/*
+ * PSCI Function IDs for v0.2+ are well defined so use
+ * standard values.
+ */
+static int psci_0_2_init(struct device_node *np)
+{
+        int err, ver;
+
+        err = get_set_conduit_method(np);
+
+        if (err)
+                goto out_put_node;
+
+        ver = psci_get_version();
+
+        if (ver == PSCI_RET_NOT_SUPPORTED) {
+                /* PSCI v0.2 mandates implementation of PSCI_ID_VERSION. */
+                pr_err("PSCI firmware does not comply with the v0.2 spec.\n");
+                err = -EOPNOTSUPP;
                 goto out_put_node;
+        } else {
+                pr_info("PSCIv%d.%d detected in firmware.\n",
+                                PSCI_VERSION_MAJOR(ver),
+                                PSCI_VERSION_MINOR(ver));
+
+                if (PSCI_VERSION_MAJOR(ver) == 0 &&
+                                PSCI_VERSION_MINOR(ver) < 2) {
+                        err = -EINVAL;
+                        pr_err("Conflicting PSCI version detected.\n");
+                        goto out_put_node;
+                }
         }
 
+        pr_info("Using standard PSCI v0.2 function IDs\n");
+        psci_function_id[PSCI_FN_CPU_SUSPEND] = PSCI_0_2_FN64_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_FN64_CPU_ON;
+        psci_ops.cpu_on = psci_cpu_on;
+
+        psci_function_id[PSCI_FN_MIGRATE] = PSCI_0_2_FN64_MIGRATE;
+        psci_ops.migrate = psci_migrate;
+
+        psci_function_id[PSCI_FN_AFFINITY_INFO] = PSCI_0_2_FN64_AFFINITY_INFO;
+        psci_ops.affinity_info = psci_affinity_info;
+
+        psci_function_id[PSCI_FN_MIGRATE_INFO_TYPE] =
+                PSCI_0_2_FN_MIGRATE_INFO_TYPE;
+        psci_ops.migrate_info_type = psci_migrate_info_type;
+
+        arm_pm_restart = psci_sys_reset;
+
+        pm_power_off = psci_sys_poweroff;
+
+out_put_node:
+        of_node_put(np);
+        return err;
+}
+
+/*
+ * PSCI < v0.2 get PSCI Function IDs via DT.
+ */
+static int psci_0_1_init(struct device_node *np)
+{
+        u32 id;
+        int err;
+
+        err = get_set_conduit_method(np);
+
+        if (err)
+                goto out_put_node;
+
+        pr_info("Using PSCI v0.1 Function IDs from DT\n");
+
         if (!of_property_read_u32(np, "cpu_suspend", &id)) {
                 psci_function_id[PSCI_FN_CPU_SUSPEND] = id;
                 psci_ops.cpu_suspend = psci_cpu_suspend;
@@ -224,7 +330,28 @@ void __init psci_init(void)
 
 out_put_node:
         of_node_put(np);
-        return;
+        return err;
+}
+
+static const struct of_device_id psci_of_match[] __initconst = {
+        { .compatible = "arm,psci",     .data = psci_0_1_init},
+        { .compatible = "arm,psci-0.2", .data = psci_0_2_init},
+        {},
+};
+
+int __init psci_init(void)
+{
+        struct device_node *np;
+        const struct of_device_id *matched_np;
+        psci_initcall_t init_fn;
+
+        np = of_find_matching_node_and_match(NULL, psci_of_match, &matched_np);
+
+        if (!np)
+                return -ENODEV;
+
+        init_fn = (psci_initcall_t)matched_np->data;
+        return init_fn(np);
 }
 
 #ifdef CONFIG_SMP
@@ -277,6 +404,35 @@ static void cpu_psci_cpu_die(unsigned int cpu)
 
         pr_crit("unable to power off CPU%u (%d)\n", cpu, ret);
 }
+
+static int cpu_psci_cpu_kill(unsigned int cpu)
+{
+        int err, i;
+
+        if (!psci_ops.affinity_info)
+                return 1;
+        /*
+         * cpu_kill could race with cpu_die and we can
+         * potentially end up declaring this cpu undead
+         * while it is dying. So, try again a few times.
+         */
+
+        for (i = 0; i < 10; i++) {
+                err = psci_ops.affinity_info(cpu_logical_map(cpu), 0);
+                if (err == PSCI_0_2_AFFINITY_LEVEL_OFF) {
+                        pr_info("CPU%d killed.\n", cpu);
+                        return 1;
+                }
+
+                msleep(10);
+                pr_info("Retrying again to check for CPU kill\n");
+        }
+
+        pr_warn("CPU%d may not have shut down cleanly (AFFINITY_INFO reports %d)\n",
+                        cpu, err);
+        /* Make op_cpu_kill() fail. */
+        return 0;
+}
 #endif
 
 const struct cpu_operations cpu_psci_ops = {
@@ -287,6 +443,7 @@ const struct cpu_operations cpu_psci_ops = {
 #ifdef CONFIG_HOTPLUG_CPU
         .cpu_disable    = cpu_psci_cpu_disable,
         .cpu_die        = cpu_psci_cpu_die,
+        .cpu_kill       = cpu_psci_cpu_kill,
 #endif
 };