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 297