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/errno.h>
  24 #include <asm/psci.h>
  25 #include <asm/smp_plat.h>
  26
  27 #define PSCI_POWER_STATE_TYPE_STANDBY           0
  28 #define PSCI_POWER_STATE_TYPE_POWER_DOWN        1
  29
  30 struct psci_power_state {
  31         u16     id;
  32         u8      type;
  33         u8      affinity_level;
  34 };
  35
  36 struct psci_operations {
  37         int (*cpu_suspend)(struct psci_power_state state,
  38                            unsigned long entry_point);
  39         int (*cpu_off)(struct psci_power_state state);
  40         int (*cpu_on)(unsigned long cpuid, unsigned long entry_point);
  41         int (*migrate)(unsigned long cpuid);
  42 };
  43
  44 static struct psci_operations psci_ops;
  45
  46 static int (*invoke_psci_fn)(u64, u64, u64, u64);
  47
  48 enum psci_function {
  49         PSCI_FN_CPU_SUSPEND,
  50         PSCI_FN_CPU_ON,
  51         PSCI_FN_CPU_OFF,
  52         PSCI_FN_MIGRATE,
  53         PSCI_FN_MAX,
  54 };
  55
  56 static u32 psci_function_id[PSCI_FN_MAX];
  57
  58 #define PSCI_RET_SUCCESS                0
  59 #define PSCI_RET_EOPNOTSUPP             -1
  60 #define PSCI_RET_EINVAL                 -2
  61 #define PSCI_RET_EPERM                  -3
  62
  63 static int psci_to_linux_errno(int errno)
  64 {
  65         switch (errno) {
  66         case PSCI_RET_SUCCESS:
  67                 return 0;
  68         case PSCI_RET_EOPNOTSUPP:
  69                 return -EOPNOTSUPP;
  70         case PSCI_RET_EINVAL:
  71                 return -EINVAL;
  72         case PSCI_RET_EPERM:
  73                 return -EPERM;
  74         };
  75
  76         return -EINVAL;
  77 }
  78
  79 #define PSCI_POWER_STATE_ID_MASK        0xffff
  80 #define PSCI_POWER_STATE_ID_SHIFT       0
  81 #define PSCI_POWER_STATE_TYPE_MASK      0x1
  82 #define PSCI_POWER_STATE_TYPE_SHIFT     16
  83 #define PSCI_POWER_STATE_AFFL_MASK      0x3
  84 #define PSCI_POWER_STATE_AFFL_SHIFT     24
  85
  86 static u32 psci_power_state_pack(struct psci_power_state state)
  87 {
  88         return  ((state.id & PSCI_POWER_STATE_ID_MASK)
  89                         << PSCI_POWER_STATE_ID_SHIFT)   |
  90                 ((state.type & PSCI_POWER_STATE_TYPE_MASK)
  91                         << PSCI_POWER_STATE_TYPE_SHIFT) |
  92                 ((state.affinity_level & PSCI_POWER_STATE_AFFL_MASK)
  93                         << PSCI_POWER_STATE_AFFL_SHIFT);
  94 }
  95
  96 /*
  97  * The following two functions are invoked via the invoke_psci_fn pointer
  98  * and will not be inlined, allowing us to piggyback on the AAPCS.
  99  */
 100 static noinline int __invoke_psci_fn_hvc(u64 function_id, u64 arg0, u64 arg1,
 101                                          u64 arg2)
 102 {
 103         asm volatile(
 104                         __asmeq("%0", "x0")
 105                         __asmeq("%1", "x1")
 106                         __asmeq("%2", "x2")
 107                         __asmeq("%3", "x3")
 108                         "hvc    #0\n"
 109                 : "+r" (function_id)
 110                 : "r" (arg0), "r" (arg1), "r" (arg2));
 111
 112         return function_id;
 113 }
 114
 115 static noinline int __invoke_psci_fn_smc(u64 function_id, u64 arg0, u64 arg1,
 116                                          u64 arg2)
 117 {
 118         asm volatile(
 119                         __asmeq("%0", "x0")
 120                         __asmeq("%1", "x1")
 121                         __asmeq("%2", "x2")
 122                         __asmeq("%3", "x3")
 123                         "smc    #0\n"
 124                 : "+r" (function_id)
 125                 : "r" (arg0), "r" (arg1), "r" (arg2));
 126
 127         return function_id;
 128 }
 129
 130 static int psci_cpu_suspend(struct psci_power_state state,
 131                             unsigned long entry_point)
 132 {
 133         int err;
 134         u32 fn, power_state;
 135
 136         fn = psci_function_id[PSCI_FN_CPU_SUSPEND];
 137         power_state = psci_power_state_pack(state);
 138         err = invoke_psci_fn(fn, power_state, entry_point, 0);
 139         return psci_to_linux_errno(err);
 140 }
 141
 142 static int psci_cpu_off(struct psci_power_state state)
 143 {
 144         int err;
 145         u32 fn, power_state;
 146
 147         fn = psci_function_id[PSCI_FN_CPU_OFF];
 148         power_state = psci_power_state_pack(state);
 149         err = invoke_psci_fn(fn, power_state, 0, 0);
 150         return psci_to_linux_errno(err);
 151 }
 152
 153 static int psci_cpu_on(unsigned long cpuid, unsigned long entry_point)
 154 {
 155         int err;
 156         u32 fn;
 157
 158         fn = psci_function_id[PSCI_FN_CPU_ON];
 159         err = invoke_psci_fn(fn, cpuid, entry_point, 0);
 160         return psci_to_linux_errno(err);
 161 }
 162
 163 static int psci_migrate(unsigned long cpuid)
 164 {
 165         int err;
 166         u32 fn;
 167
 168         fn = psci_function_id[PSCI_FN_MIGRATE];
 169         err = invoke_psci_fn(fn, cpuid, 0, 0);
 170         return psci_to_linux_errno(err);
 171 }
 172
 173 static const struct of_device_id psci_of_match[] __initconst = {
 174         { .compatible = "arm,psci",     },
 175         {},
 176 };
 177
 178 int __init psci_init(void)
 179 {
 180         struct device_node *np;
 181         const char *method;
 182         u32 id;
 183         int err = 0;
 184
 185         np = of_find_matching_node(NULL, psci_of_match);
 186         if (!np)
 187                 return -ENODEV;
 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                 err = -ENXIO;
 194                 goto out_put_node;
 195         }
 196
 197         if (!strcmp("hvc", method)) {
 198                 invoke_psci_fn = __invoke_psci_fn_hvc;
 199         } else if (!strcmp("smc", method)) {
 200                 invoke_psci_fn = __invoke_psci_fn_smc;
 201         } else {
 202                 pr_warning("invalid \"method\" property: %s\n", method);
 203                 err = -EINVAL;
 204                 goto out_put_node;
 205         }
 206
 207         if (!of_property_read_u32(np, "cpu_suspend", &id)) {
 208                 psci_function_id[PSCI_FN_CPU_SUSPEND] = id;
 209                 psci_ops.cpu_suspend = psci_cpu_suspend;
 210         }
 211
 212         if (!of_property_read_u32(np, "cpu_off", &id)) {
 213                 psci_function_id[PSCI_FN_CPU_OFF] = id;
 214                 psci_ops.cpu_off = psci_cpu_off;
 215         }
 216
 217         if (!of_property_read_u32(np, "cpu_on", &id)) {
 218                 psci_function_id[PSCI_FN_CPU_ON] = id;
 219                 psci_ops.cpu_on = psci_cpu_on;
 220         }
 221
 222         if (!of_property_read_u32(np, "migrate", &id)) {
 223                 psci_function_id[PSCI_FN_MIGRATE] = id;
 224                 psci_ops.migrate = psci_migrate;
 225         }
 226
 227 out_put_node:
 228         of_node_put(np);
 229         return err;
 230 }
 231
 232 #ifdef CONFIG_SMP
 233
 234 static int __init smp_psci_init_cpu(struct device_node *dn, int cpu)
 235 {
 236         return 0;
 237 }
 238
 239 static int __init smp_psci_prepare_cpu(int cpu)
 240 {
 241         int err;
 242
 243         if (!psci_ops.cpu_on) {
 244                 pr_err("no cpu_on method, not booting CPU%d\n", cpu);
 245                 return -ENODEV;
 246         }
 247
 248         err = psci_ops.cpu_on(cpu_logical_map(cpu), __pa(secondary_holding_pen));
 249         if (err) {
 250                 pr_err("failed to boot CPU%d (%d)\n", cpu, err);
 251                 return err;
 252         }
 253
 254         return 0;
 255 }
 256
 257 const struct smp_enable_ops smp_psci_ops __initconst = {
 258         .name           = "psci",
 259         .init_cpu       = smp_psci_init_cpu,
 260         .prepare_cpu    = smp_psci_prepare_cpu,
 261 };
 262
 263 #endif