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