arch/arm/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) 2012 ARM Limited
  12  *
  13  * Author: Will Deacon <[email protected]>
  14  */
  15
  16 #define pr_fmt(fmt) "psci: " fmt
  17
  18 #include <linux/init.h>
  19 #include <linux/of.h>
  20
  21 #include <asm/compiler.h>
  22 #include <asm/errno.h>
  23 #include <asm/opcodes-sec.h>
  24 #include <asm/opcodes-virt.h>
  25 #include <asm/psci.h>
  26
  27 struct psci_operations psci_ops;
  28
  29 static int (*invoke_psci_fn)(u32, u32, u32, u32);
  30
  31 enum psci_function {
  32         PSCI_FN_CPU_SUSPEND,
  33         PSCI_FN_CPU_ON,
  34         PSCI_FN_CPU_OFF,
  35         PSCI_FN_MIGRATE,
  36         PSCI_FN_MAX,
  37 };
  38
  39 static u32 psci_function_id[PSCI_FN_MAX];
  40
  41 #define PSCI_RET_SUCCESS                0
  42 #define PSCI_RET_EOPNOTSUPP             -1
  43 #define PSCI_RET_EINVAL                 -2
  44 #define PSCI_RET_EPERM                  -3
  45
  46 static int psci_to_linux_errno(int errno)
  47 {
  48         switch (errno) {
  49         case PSCI_RET_SUCCESS:
  50                 return 0;
  51         case PSCI_RET_EOPNOTSUPP:
  52                 return -EOPNOTSUPP;
  53         case PSCI_RET_EINVAL:
  54                 return -EINVAL;
  55         case PSCI_RET_EPERM:
  56                 return -EPERM;
  57         };
  58
  59         return -EINVAL;
  60 }
  61
  62 #define PSCI_POWER_STATE_ID_MASK        0xffff
  63 #define PSCI_POWER_STATE_ID_SHIFT       0
  64 #define PSCI_POWER_STATE_TYPE_MASK      0x1
  65 #define PSCI_POWER_STATE_TYPE_SHIFT     16
  66 #define PSCI_POWER_STATE_AFFL_MASK      0x3
  67 #define PSCI_POWER_STATE_AFFL_SHIFT     24
  68
  69 static u32 psci_power_state_pack(struct psci_power_state state)
  70 {
  71         return  ((state.id & PSCI_POWER_STATE_ID_MASK)
  72                         << PSCI_POWER_STATE_ID_SHIFT)   |
  73                 ((state.type & PSCI_POWER_STATE_TYPE_MASK)
  74                         << PSCI_POWER_STATE_TYPE_SHIFT) |
  75                 ((state.affinity_level & PSCI_POWER_STATE_AFFL_MASK)
  76                         << PSCI_POWER_STATE_AFFL_SHIFT);
  77 }
  78
  79 /*
  80  * The following two functions are invoked via the invoke_psci_fn pointer
  81  * and will not be inlined, allowing us to piggyback on the AAPCS.
  82  */
  83 static noinline int __invoke_psci_fn_hvc(u32 function_id, u32 arg0, u32 arg1,
  84                                          u32 arg2)
  85 {
  86         asm volatile(
  87                         __asmeq("%0", "r0")
  88                         __asmeq("%1", "r1")
  89                         __asmeq("%2", "r2")
  90                         __asmeq("%3", "r3")
  91                         __HVC(0)
  92                 : "+r" (function_id)
  93                 : "r" (arg0), "r" (arg1), "r" (arg2));
  94
  95         return function_id;
  96 }
  97
  98 static noinline int __invoke_psci_fn_smc(u32 function_id, u32 arg0, u32 arg1,
  99                                          u32 arg2)
 100 {
 101         asm volatile(
 102                         __asmeq("%0", "r0")
 103                         __asmeq("%1", "r1")
 104                         __asmeq("%2", "r2")
 105                         __asmeq("%3", "r3")
 106                         __SMC(0)
 107                 : "+r" (function_id)
 108                 : "r" (arg0), "r" (arg1), "r" (arg2));
 109
 110         return function_id;
 111 }
 112
 113 static int psci_cpu_suspend(struct psci_power_state state,
 114                             unsigned long entry_point)
 115 {
 116         int err;
 117         u32 fn, power_state;
 118
 119         fn = psci_function_id[PSCI_FN_CPU_SUSPEND];
 120         power_state = psci_power_state_pack(state);
 121         err = invoke_psci_fn(fn, power_state, entry_point, 0);
 122         return psci_to_linux_errno(err);
 123 }
 124
 125 static int psci_cpu_off(struct psci_power_state state)
 126 {
 127         int err;
 128         u32 fn, power_state;
 129
 130         fn = psci_function_id[PSCI_FN_CPU_OFF];
 131         power_state = psci_power_state_pack(state);
 132         err = invoke_psci_fn(fn, power_state, 0, 0);
 133         return psci_to_linux_errno(err);
 134 }
 135
 136 static int psci_cpu_on(unsigned long cpuid, unsigned long entry_point)
 137 {
 138         int err;
 139         u32 fn;
 140
 141         fn = psci_function_id[PSCI_FN_CPU_ON];
 142         err = invoke_psci_fn(fn, cpuid, entry_point, 0);
 143         return psci_to_linux_errno(err);
 144 }
 145
 146 static int psci_migrate(unsigned long cpuid)
 147 {
 148         int err;
 149         u32 fn;
 150
 151         fn = psci_function_id[PSCI_FN_MIGRATE];
 152         err = invoke_psci_fn(fn, cpuid, 0, 0);
 153         return psci_to_linux_errno(err);
 154 }
 155
 156 static const struct of_device_id psci_of_match[] __initconst = {
 157         { .compatible = "arm,psci",     },
 158         {},
 159 };
 160
 161 static int __init psci_init(void)
 162 {
 163         struct device_node *np;
 164         const char *method;
 165         u32 id;
 166
 167         np = of_find_matching_node(NULL, psci_of_match);
 168         if (!np)
 169                 return 0;
 170
 171         pr_info("probing function IDs from device-tree\n");
 172
 173         if (of_property_read_string(np, "method", &method)) {
 174                 pr_warning("missing \"method\" property\n");
 175                 goto out_put_node;
 176         }
 177
 178         if (!strcmp("hvc", method)) {
 179                 invoke_psci_fn = __invoke_psci_fn_hvc;
 180         } else if (!strcmp("smc", method)) {
 181                 invoke_psci_fn = __invoke_psci_fn_smc;
 182         } else {
 183                 pr_warning("invalid \"method\" property: %s\n", method);
 184                 goto out_put_node;
 185         }
 186
 187         if (!of_property_read_u32(np, "cpu_suspend", &id)) {
 188                 psci_function_id[PSCI_FN_CPU_SUSPEND] = id;
 189                 psci_ops.cpu_suspend = psci_cpu_suspend;
 190         }
 191
 192         if (!of_property_read_u32(np, "cpu_off", &id)) {
 193                 psci_function_id[PSCI_FN_CPU_OFF] = id;
 194                 psci_ops.cpu_off = psci_cpu_off;
 195         }
 196
 197         if (!of_property_read_u32(np, "cpu_on", &id)) {
 198                 psci_function_id[PSCI_FN_CPU_ON] = id;
 199                 psci_ops.cpu_on = psci_cpu_on;
 200         }
 201
 202         if (!of_property_read_u32(np, "migrate", &id)) {
 203                 psci_function_id[PSCI_FN_MIGRATE] = id;
 204                 psci_ops.migrate = psci_migrate;
 205         }
 206
 207 out_put_node:
 208         of_node_put(np);
 209         return 0;
 210 }
 211 early_initcall(psci_init);