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