arch/arm64/kernel/psci.c

   1 /*
   2  * This program is free software; you can redistribute it and/or modify
   3  * it under the terms of the GNU General Public License version 2 as
   4  * published by the Free Software Foundation.
   5  *
   6  * This program is distributed in the hope that it will be useful,
   7  * but WITHOUT ANY WARRANTY; without even the implied warranty of
   8  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   9  * GNU General Public License for more details.
  10  *
  11  * Copyright (C) 2013 ARM Limited
  12  *
  13  * Author: Will Deacon <will.deacon@arm.com>
  14  */
  15
  16 #define pr_fmt(fmt) "psci: " fmt
  17
  18 #include <linux/init.h>
  19 #include <linux/of.h>
  20 #include <linux/smp.h>
  21
  22 #include <asm/compiler.h>
  23 #include <asm/cpu_ops.h>
  24 #include <asm/errno.h>
  25 #include <asm/psci.h>
  26 #include <asm/smp_plat.h>
  27
  28 #define PSCI_POWER_STATE_TYPE_STANDBY           0
  29 #define PSCI_POWER_STATE_TYPE_POWER_DOWN        1
  30
  31 struct psci_power_state {
  32         u16     id;
  33         u8      type;
  34         u8      affinity_level;
  35 };
  36
  37 struct psci_operations {
  38         int (*cpu_suspend)(struct psci_power_state state,
  39                            unsigned long entry_point);
  40         int (*cpu_off)(struct psci_power_state state);
  41         int (*cpu_on)(unsigned long cpuid, unsigned long entry_point);
  42         int (*migrate)(unsigned long cpuid);
  43 };
  44
  45 static struct psci_operations psci_ops;
  46
  47 static int (*invoke_psci_fn)(u64, u64, u64, u64);
  48
  49 enum psci_function {
  50         PSCI_FN_CPU_SUSPEND,
  51         PSCI_FN_CPU_ON,
  52         PSCI_FN_CPU_OFF,
  53         PSCI_FN_MIGRATE,
  54         PSCI_FN_MAX,
  55 };
  56
  57 static u32 psci_function_id[PSCI_FN_MAX];
  58
  59 #define PSCI_RET_SUCCESS                0
  60 #define PSCI_RET_EOPNOTSUPP             -1
  61 #define PSCI_RET_EINVAL                 -2
  62 #define PSCI_RET_EPERM                  -3
  63
  64 static int psci_to_linux_errno(int errno)
  65 {
  66         switch (errno) {
  67         case PSCI_RET_SUCCESS:
  68                 return 0;
  69         case PSCI_RET_EOPNOTSUPP:
  70                 return -EOPNOTSUPP;
  71         case PSCI_RET_EINVAL:
  72                 return -EINVAL;
  73         case PSCI_RET_EPERM:
  74                 return -EPERM;
  75         };
  76
  77         return -EINVAL;
  78 }
  79
  80 #define PSCI_POWER_STATE_ID_MASK        0xffff
  81 #define PSCI_POWER_STATE_ID_SHIFT       0
  82 #define PSCI_POWER_STATE_TYPE_MASK      0x1
  83 #define PSCI_POWER_STATE_TYPE_SHIFT     16
  84 #define PSCI_POWER_STATE_AFFL_MASK      0x3
  85 #define PSCI_POWER_STATE_AFFL_SHIFT     24
  86
  87 static u32 psci_power_state_pack(struct psci_power_state state)
  88 {
  89         return  ((state.id & PSCI_POWER_STATE_ID_MASK)
  90                         << PSCI_POWER_STATE_ID_SHIFT)   |
  91                 ((state.type & PSCI_POWER_STATE_TYPE_MASK)
  92                         << PSCI_POWER_STATE_TYPE_SHIFT) |
  93                 ((state.affinity_level & PSCI_POWER_STATE_AFFL_MASK)
  94                         << PSCI_POWER_STATE_AFFL_SHIFT);
  95 }
  96
  97 /*
  98  * The following two functions are invoked via the invoke_psci_fn pointer
  99  * and will not be inlined, allowing us to piggyback on the AAPCS.
 100  */
 101 static noinline int __invoke_psci_fn_hvc(u64 function_id, u64 arg0, u64 arg1,
 102                                          u64 arg2)
 103 {
 104         asm volatile(
 105                         __asmeq("%0", "x0")
 106                         __asmeq("%1", "x1")
 107                         __asmeq("%2", "x2")
 108                         __asmeq("%3", "x3")
 109                         "hvc    #0\n"
 110                 : "+r" (function_id)
 111                 : "r" (arg0), "r" (arg1), "r" (arg2));
 112
 113         return function_id;
 114 }
 115
 116 static noinline int __invoke_psci_fn_smc(u64 function_id, u64 arg0, u64 arg1,
 117                                          u64 arg2)
 118 {
 119         asm volatile(
 120                         __asmeq("%0", "x0")
 121                         __asmeq("%1", "x1")
 122                         __asmeq("%2", "x2")
 123                         __asmeq("%3", "x3")
 124                         "smc    #0\n"
 125                 : "+r" (function_id)
 126                 : "r" (arg0), "r" (arg1), "r" (arg2));
 127
 128         return function_id;
 129 }
 130
 131 static int psci_cpu_suspend(struct psci_power_state state,
 132                             unsigned long entry_point)
 133 {
 134         int err;
 135         u32 fn, power_state;
 136
 137         fn = psci_function_id[PSCI_FN_CPU_SUSPEND];
 138         power_state = psci_power_state_pack(state);
 139         err = invoke_psci_fn(fn, power_state, entry_point, 0);
 140         return psci_to_linux_errno(err);
 141 }
 142
 143 static int psci_cpu_off(struct psci_power_state state)
 144 {
 145         int err;
 146         u32 fn, power_state;
 147
 148         fn = psci_function_id[PSCI_FN_CPU_OFF];
 149         power_state = psci_power_state_pack(state);
 150         err = invoke_psci_fn(fn, power_state, 0, 0);
 151         return psci_to_linux_errno(err);
 152 }
 153
 154 static int psci_cpu_on(unsigned long cpuid, unsigned long entry_point)
 155 {
 156         int err;
 157         u32 fn;
 158
 159         fn = psci_function_id[PSCI_FN_CPU_ON];
 160         err = invoke_psci_fn(fn, cpuid, entry_point, 0);
 161         return psci_to_linux_errno(err);
 162 }
 163
 164 static int psci_migrate(unsigned long cpuid)
 165 {
 166         int err;
 167         u32 fn;
 168
 169         fn = psci_function_id[PSCI_FN_MIGRATE];
 170         err = invoke_psci_fn(fn, cpuid, 0, 0);
 171         return psci_to_linux_errno(err);
 172 }
 173
 174 static const struct of_device_id psci_of_match[] __initconst = {
 175         { .compatible = "arm,psci",     },
 176         {},
 177 };
 178
 179 void __init psci_init(void)
 180 {
 181         struct device_node *np;
 182         const char *method;
 183         u32 id;
 184
 185         np = of_find_matching_node(NULL, psci_of_match);
 186         if (!np)
 187                 return;
 188
 189         pr_info("probing function IDs from device-tree\n");
 190
 191         if (of_property_read_string(np, "method", &method)) {
 192                 pr_warning("missing \"method\" property\n");
 193                 goto out_put_node;
 194         }
 195
 196         if (!strcmp("hvc", method)) {
 197                 invoke_psci_fn = __invoke_psci_fn_hvc;
 198         } else if (!strcmp("smc", method)) {
 199                 invoke_psci_fn = __invoke_psci_fn_smc;
 200         } else {
 201                 pr_warning("invalid \"method\" property: %s\n", method);
 202                 goto out_put_node;
 203         }
 204
 205         if (!of_property_read_u32(np, "cpu_suspend", &id)) {
 206                 psci_function_id[PSCI_FN_CPU_SUSPEND] = id;
 207                 psci_ops.cpu_suspend = psci_cpu_suspend;
 208         }
 209
 210         if (!of_property_read_u32(np, "cpu_off", &id)) {
 211                 psci_function_id[PSCI_FN_CPU_OFF] = id;
 212                 psci_ops.cpu_off = psci_cpu_off;
 213         }
 214
 215         if (!of_property_read_u32(np, "cpu_on", &id)) {
 216                 psci_function_id[PSCI_FN_CPU_ON] = id;
 217                 psci_ops.cpu_on = psci_cpu_on;
 218         }
 219
 220         if (!of_property_read_u32(np, "migrate", &id)) {
 221                 psci_function_id[PSCI_FN_MIGRATE] = id;
 222                 psci_ops.migrate = psci_migrate;
 223         }
 224
 225 out_put_node:
 226         of_node_put(np);
 227         return;
 228 }
 229
 230 #ifdef CONFIG_SMP
 231
 232 static int __init cpu_psci_cpu_init(struct device_node *dn, unsigned int cpu)
 233 {
 234         return 0;
 235 }
 236
 237 static int __init cpu_psci_cpu_prepare(unsigned int cpu)
 238 {
 239         if (!psci_ops.cpu_on) {
 240                 pr_err("no cpu_on method, not booting CPU%d\n", cpu);
 241                 return -ENODEV;
 242         }
 243
 244         return 0;
 245 }
 246
 247 static int cpu_psci_cpu_boot(unsigned int cpu)
 248 {
 249         int err = psci_ops.cpu_on(cpu_logical_map(cpu), __pa(secondary_entry));
 250         if (err)
 251                 pr_err("failed to boot CPU%d (%d)\n", cpu, err);
 252
 253         return err;
 254 }
 255
 256 #ifdef CONFIG_HOTPLUG_CPU
 257 static int cpu_psci_cpu_disable(unsigned int cpu)
 258 {
 259         /* Fail early if we don't have CPU_OFF support */
 260         if (!psci_ops.cpu_off)
 261                 return -EOPNOTSUPP;
 262         return 0;
 263 }
 264
 265 static void cpu_psci_cpu_die(unsigned int cpu)
 266 {
 267         int ret;
 268         /*
 269          * There are no known implementations of PSCI actually using the
 270          * power state field, pass a sensible default for now.
 271          */
 272         struct psci_power_state state = {
 273                 .type = PSCI_POWER_STATE_TYPE_POWER_DOWN,
 274         };
 275
 276         ret = psci_ops.cpu_off(state);
 277
 278         pr_crit("unable to power off CPU%u (%d)\n", cpu, ret);
 279 }
 280 #endif
 281
 282 const struct cpu_operations cpu_psci_ops = {
 283         .name           = "psci",
 284         .cpu_init       = cpu_psci_cpu_init,
 285         .cpu_prepare    = cpu_psci_cpu_prepare,
 286         .cpu_boot       = cpu_psci_cpu_boot,
 287 #ifdef CONFIG_HOTPLUG_CPU
 288         .cpu_disable    = cpu_psci_cpu_disable,
 289         .cpu_die        = cpu_psci_cpu_die,
 290 #endif
 291 };
 292
 293 #endif