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 void __init psci_init(void) 180 { 181 struct device_node *np; 182 const char *method; 183 u32 id; 184 185 np = of_find_matching_node(NULL, psci_of_match); 186 if (!np) 187 return; 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 goto out_put_node; 194 } 195 196 if (!strcmp("hvc", method)) { 197 invoke_psci_fn = __invoke_psci_fn_hvc; 198 } else if (!strcmp("smc", method)) { 199 invoke_psci_fn = __invoke_psci_fn_smc; 200 } else { 201 pr_warning("invalid \"method\" property: %s\n", method); 202 goto out_put_node; 203 } 204 205 if (!of_property_read_u32(np, "cpu_suspend", &id)) { 206 psci_function_id[PSCI_FN_CPU_SUSPEND] = id; 207 psci_ops.cpu_suspend = psci_cpu_suspend; 208 } 209 210 if (!of_property_read_u32(np, "cpu_off", &id)) { 211 psci_function_id[PSCI_FN_CPU_OFF] = id; 212 psci_ops.cpu_off = psci_cpu_off; 213 } 214 215 if (!of_property_read_u32(np, "cpu_on", &id)) { 216 psci_function_id[PSCI_FN_CPU_ON] = id; 217 psci_ops.cpu_on = psci_cpu_on; 218 } 219 220 if (!of_property_read_u32(np, "migrate", &id)) { 221 psci_function_id[PSCI_FN_MIGRATE] = id; 222 psci_ops.migrate = psci_migrate; 223 } 224 225 out_put_node: 226 of_node_put(np); 227 return; 228 } 229 230 #ifdef CONFIG_SMP 231 232 static int __init cpu_psci_cpu_init(struct device_node *dn, unsigned int cpu) 233 { 234 return 0; 235 } 236 237 static int __init cpu_psci_cpu_prepare(unsigned int cpu) 238 { 239 if (!psci_ops.cpu_on) { 240 pr_err("no cpu_on method, not booting CPU%d\n", cpu); 241 return -ENODEV; 242 } 243 244 return 0; 245 } 246 247 static int cpu_psci_cpu_boot(unsigned int cpu) 248 { 249 int err = psci_ops.cpu_on(cpu_logical_map(cpu), __pa(secondary_entry)); 250 if (err) 251 pr_err("failed to boot CPU%d (%d)\n", cpu, err); 252 253 return err; 254 } 255 256 #ifdef CONFIG_HOTPLUG_CPU 257 static int cpu_psci_cpu_disable(unsigned int cpu) 258 { 259 /* Fail early if we don't have CPU_OFF support */ 260 if (!psci_ops.cpu_off) 261 return -EOPNOTSUPP; 262 return 0; 263 } 264 265 static void cpu_psci_cpu_die(unsigned int cpu) 266 { 267 int ret; 268 /* 269 * There are no known implementations of PSCI actually using the 270 * power state field, pass a sensible default for now. 271 */ 272 struct psci_power_state state = { 273 .type = PSCI_POWER_STATE_TYPE_POWER_DOWN, 274 }; 275 276 ret = psci_ops.cpu_off(state); 277 278 pr_crit("unable to power off CPU%u (%d)\n", cpu, ret); 279 } 280 #endif 281 282 const struct cpu_operations cpu_psci_ops = { 283 .name = "psci", 284 .cpu_init = cpu_psci_cpu_init, 285 .cpu_prepare = cpu_psci_cpu_prepare, 286 .cpu_boot = cpu_psci_cpu_boot, 287 #ifdef CONFIG_HOTPLUG_CPU 288 .cpu_disable = cpu_psci_cpu_disable, 289 .cpu_die = cpu_psci_cpu_die, 290 #endif 291 }; 292 293 #endif 294