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 int __init psci_init(void)
 180 {
 181         struct device_node *np;
 182         const char *method;
 183         u32 id;
 184         int err = 0;
 185
 186         np = of_find_matching_node(NULL, psci_of_match);
 187         if (!np)
 188                 return -ENODEV;
 189
 190         pr_info("probing function IDs from device-tree\n");
 191
 192         if (of_property_read_string(np, "method", &method)) {
 193                 pr_warning("missing \"method\" property\n");
 194                 err = -ENXIO;
 195                 goto out_put_node;
 196         }
 197
 198         if (!strcmp("hvc", method)) {
 199                 invoke_psci_fn = __invoke_psci_fn_hvc;
 200         } else if (!strcmp("smc", method)) {
 201                 invoke_psci_fn = __invoke_psci_fn_smc;
 202         } else {
 203                 pr_warning("invalid \"method\" property: %s\n", method);
 204                 err = -EINVAL;
 205                 goto out_put_node;
 206         }
 207
 208         if (!of_property_read_u32(np, "cpu_suspend", &id)) {
 209                 psci_function_id[PSCI_FN_CPU_SUSPEND] = id;
 210                 psci_ops.cpu_suspend = psci_cpu_suspend;
 211         }
 212
 213         if (!of_property_read_u32(np, "cpu_off", &id)) {
 214                 psci_function_id[PSCI_FN_CPU_OFF] = id;
 215                 psci_ops.cpu_off = psci_cpu_off;
 216         }
 217
 218         if (!of_property_read_u32(np, "cpu_on", &id)) {
 219                 psci_function_id[PSCI_FN_CPU_ON] = id;
 220                 psci_ops.cpu_on = psci_cpu_on;
 221         }
 222
 223         if (!of_property_read_u32(np, "migrate", &id)) {
 224                 psci_function_id[PSCI_FN_MIGRATE] = id;
 225                 psci_ops.migrate = psci_migrate;
 226         }
 227
 228 out_put_node:
 229         of_node_put(np);
 230         return err;
 231 }
 232
 233 #ifdef CONFIG_SMP
 234
 235 static int __init cpu_psci_cpu_init(struct device_node *dn, unsigned int cpu)
 236 {
 237         return 0;
 238 }
 239
 240 static int __init cpu_psci_cpu_prepare(unsigned int cpu)
 241 {
 242         if (!psci_ops.cpu_on) {
 243                 pr_err("no cpu_on method, not booting CPU%d\n", cpu);
 244                 return -ENODEV;
 245         }
 246
 247         return 0;
 248 }
 249
 250 static int cpu_psci_cpu_boot(unsigned int cpu)
 251 {
 252         int err = psci_ops.cpu_on(cpu_logical_map(cpu), __pa(secondary_entry));
 253         if (err)
 254                 pr_err("psci: failed to boot CPU%d (%d)\n", cpu, err);
 255
 256         return err;
 257 }
 258
 259 #ifdef CONFIG_HOTPLUG_CPU
 260 static int cpu_psci_cpu_disable(unsigned int cpu)
 261 {
 262         /* Fail early if we don't have CPU_OFF support */
 263         if (!psci_ops.cpu_off)
 264                 return -EOPNOTSUPP;
 265         return 0;
 266 }
 267
 268 static void cpu_psci_cpu_die(unsigned int cpu)
 269 {
 270         int ret;
 271         /*
 272          * There are no known implementations of PSCI actually using the
 273          * power state field, pass a sensible default for now.
 274          */
 275         struct psci_power_state state = {
 276                 .type = PSCI_POWER_STATE_TYPE_POWER_DOWN,
 277         };
 278
 279         ret = psci_ops.cpu_off(state);
 280
 281         pr_crit("psci: unable to power off CPU%u (%d)\n", cpu, ret);
 282 }
 283 #endif
 284
 285 const struct cpu_operations cpu_psci_ops = {
 286         .name           = "psci",
 287         .cpu_init       = cpu_psci_cpu_init,
 288         .cpu_prepare    = cpu_psci_cpu_prepare,
 289         .cpu_boot       = cpu_psci_cpu_boot,
 290 #ifdef CONFIG_HOTPLUG_CPU
 291         .cpu_disable    = cpu_psci_cpu_disable,
 292         .cpu_die        = cpu_psci_cpu_die,
 293 #endif
 294 };
 295
 296 #endif