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 #include <linux/reboot.h> 22 #include <linux/pm.h> 23 #include <linux/delay.h> 24 #include <uapi/linux/psci.h> 25 26 #include <asm/compiler.h> 27 #include <asm/cpu_ops.h> 28 #include <asm/errno.h> 29 #include <asm/psci.h> 30 #include <asm/smp_plat.h> 31 #include <asm/system_misc.h> 32 33 #define PSCI_POWER_STATE_TYPE_STANDBY 0 34 #define PSCI_POWER_STATE_TYPE_POWER_DOWN 1 35 36 struct psci_power_state { 37 u16 id; 38 u8 type; 39 u8 affinity_level; 40 }; 41 42 struct psci_operations { 43 int (*cpu_suspend)(struct psci_power_state state, 44 unsigned long entry_point); 45 int (*cpu_off)(struct psci_power_state state); 46 int (*cpu_on)(unsigned long cpuid, unsigned long entry_point); 47 int (*migrate)(unsigned long cpuid); 48 int (*affinity_info)(unsigned long target_affinity, 49 unsigned long lowest_affinity_level); 50 int (*migrate_info_type)(void); 51 }; 52 53 static struct psci_operations psci_ops; 54 55 static int (*invoke_psci_fn)(u64, u64, u64, u64); 56 typedef int (*psci_initcall_t)(const struct device_node *); 57 58 enum psci_function { 59 PSCI_FN_CPU_SUSPEND, 60 PSCI_FN_CPU_ON, 61 PSCI_FN_CPU_OFF, 62 PSCI_FN_MIGRATE, 63 PSCI_FN_AFFINITY_INFO, 64 PSCI_FN_MIGRATE_INFO_TYPE, 65 PSCI_FN_MAX, 66 }; 67 68 static u32 psci_function_id[PSCI_FN_MAX]; 69 70 static int psci_to_linux_errno(int errno) 71 { 72 switch (errno) { 73 case PSCI_RET_SUCCESS: 74 return 0; 75 case PSCI_RET_NOT_SUPPORTED: 76 return -EOPNOTSUPP; 77 case PSCI_RET_INVALID_PARAMS: 78 return -EINVAL; 79 case PSCI_RET_DENIED: 80 return -EPERM; 81 }; 82 83 return -EINVAL; 84 } 85 86 static u32 psci_power_state_pack(struct psci_power_state state) 87 { 88 return ((state.id << PSCI_0_2_POWER_STATE_ID_SHIFT) 89 & PSCI_0_2_POWER_STATE_ID_MASK) | 90 ((state.type << PSCI_0_2_POWER_STATE_TYPE_SHIFT) 91 & PSCI_0_2_POWER_STATE_TYPE_MASK) | 92 ((state.affinity_level << PSCI_0_2_POWER_STATE_AFFL_SHIFT) 93 & PSCI_0_2_POWER_STATE_AFFL_MASK); 94 } 95 96 /* 97 * The following two functions are invoked via the invoke_psci_fn pointer 98 * and will not be inlined, allowing us to piggyback on the AAPCS. 99 */ 100 static noinline int __invoke_psci_fn_hvc(u64 function_id, u64 arg0, u64 arg1, 101 u64 arg2) 102 { 103 asm volatile( 104 __asmeq("%0", "x0") 105 __asmeq("%1", "x1") 106 __asmeq("%2", "x2") 107 __asmeq("%3", "x3") 108 "hvc #0\n" 109 : "+r" (function_id) 110 : "r" (arg0), "r" (arg1), "r" (arg2)); 111 112 return function_id; 113 } 114 115 static noinline int __invoke_psci_fn_smc(u64 function_id, u64 arg0, u64 arg1, 116 u64 arg2) 117 { 118 asm volatile( 119 __asmeq("%0", "x0") 120 __asmeq("%1", "x1") 121 __asmeq("%2", "x2") 122 __asmeq("%3", "x3") 123 "smc #0\n" 124 : "+r" (function_id) 125 : "r" (arg0), "r" (arg1), "r" (arg2)); 126 127 return function_id; 128 } 129 130 static int psci_get_version(void) 131 { 132 int err; 133 134 err = invoke_psci_fn(PSCI_0_2_FN_PSCI_VERSION, 0, 0, 0); 135 return err; 136 } 137 138 static int psci_cpu_suspend(struct psci_power_state state, 139 unsigned long entry_point) 140 { 141 int err; 142 u32 fn, power_state; 143 144 fn = psci_function_id[PSCI_FN_CPU_SUSPEND]; 145 power_state = psci_power_state_pack(state); 146 err = invoke_psci_fn(fn, power_state, entry_point, 0); 147 return psci_to_linux_errno(err); 148 } 149 150 static int psci_cpu_off(struct psci_power_state state) 151 { 152 int err; 153 u32 fn, power_state; 154 155 fn = psci_function_id[PSCI_FN_CPU_OFF]; 156 power_state = psci_power_state_pack(state); 157 err = invoke_psci_fn(fn, power_state, 0, 0); 158 return psci_to_linux_errno(err); 159 } 160 161 static int psci_cpu_on(unsigned long cpuid, unsigned long entry_point) 162 { 163 int err; 164 u32 fn; 165 166 fn = psci_function_id[PSCI_FN_CPU_ON]; 167 err = invoke_psci_fn(fn, cpuid, entry_point, 0); 168 return psci_to_linux_errno(err); 169 } 170 171 static int psci_migrate(unsigned long cpuid) 172 { 173 int err; 174 u32 fn; 175 176 fn = psci_function_id[PSCI_FN_MIGRATE]; 177 err = invoke_psci_fn(fn, cpuid, 0, 0); 178 return psci_to_linux_errno(err); 179 } 180 181 static int psci_affinity_info(unsigned long target_affinity, 182 unsigned long lowest_affinity_level) 183 { 184 int err; 185 u32 fn; 186 187 fn = psci_function_id[PSCI_FN_AFFINITY_INFO]; 188 err = invoke_psci_fn(fn, target_affinity, lowest_affinity_level, 0); 189 return err; 190 } 191 192 static int psci_migrate_info_type(void) 193 { 194 int err; 195 u32 fn; 196 197 fn = psci_function_id[PSCI_FN_MIGRATE_INFO_TYPE]; 198 err = invoke_psci_fn(fn, 0, 0, 0); 199 return err; 200 } 201 202 static int get_set_conduit_method(struct device_node *np) 203 { 204 const char *method; 205 206 pr_info("probing for conduit method from DT.\n"); 207 208 if (of_property_read_string(np, "method", &method)) { 209 pr_warn("missing \"method\" property\n"); 210 return -ENXIO; 211 } 212 213 if (!strcmp("hvc", method)) { 214 invoke_psci_fn = __invoke_psci_fn_hvc; 215 } else if (!strcmp("smc", method)) { 216 invoke_psci_fn = __invoke_psci_fn_smc; 217 } else { 218 pr_warn("invalid \"method\" property: %s\n", method); 219 return -EINVAL; 220 } 221 return 0; 222 } 223 224 static void psci_sys_reset(enum reboot_mode reboot_mode, const char *cmd) 225 { 226 invoke_psci_fn(PSCI_0_2_FN_SYSTEM_RESET, 0, 0, 0); 227 } 228 229 static void psci_sys_poweroff(void) 230 { 231 invoke_psci_fn(PSCI_0_2_FN_SYSTEM_OFF, 0, 0, 0); 232 } 233 234 /* 235 * PSCI Function IDs for v0.2+ are well defined so use 236 * standard values. 237 */ 238 static int __init psci_0_2_init(struct device_node *np) 239 { 240 int err, ver; 241 242 err = get_set_conduit_method(np); 243 244 if (err) 245 goto out_put_node; 246 247 ver = psci_get_version(); 248 249 if (ver == PSCI_RET_NOT_SUPPORTED) { 250 /* PSCI v0.2 mandates implementation of PSCI_ID_VERSION. */ 251 pr_err("PSCI firmware does not comply with the v0.2 spec.\n"); 252 err = -EOPNOTSUPP; 253 goto out_put_node; 254 } else { 255 pr_info("PSCIv%d.%d detected in firmware.\n", 256 PSCI_VERSION_MAJOR(ver), 257 PSCI_VERSION_MINOR(ver)); 258 259 if (PSCI_VERSION_MAJOR(ver) == 0 && 260 PSCI_VERSION_MINOR(ver) < 2) { 261 err = -EINVAL; 262 pr_err("Conflicting PSCI version detected.\n"); 263 goto out_put_node; 264 } 265 } 266 267 pr_info("Using standard PSCI v0.2 function IDs\n"); 268 psci_function_id[PSCI_FN_CPU_SUSPEND] = PSCI_0_2_FN64_CPU_SUSPEND; 269 psci_ops.cpu_suspend = psci_cpu_suspend; 270 271 psci_function_id[PSCI_FN_CPU_OFF] = PSCI_0_2_FN_CPU_OFF; 272 psci_ops.cpu_off = psci_cpu_off; 273 274 psci_function_id[PSCI_FN_CPU_ON] = PSCI_0_2_FN64_CPU_ON; 275 psci_ops.cpu_on = psci_cpu_on; 276 277 psci_function_id[PSCI_FN_MIGRATE] = PSCI_0_2_FN64_MIGRATE; 278 psci_ops.migrate = psci_migrate; 279 280 psci_function_id[PSCI_FN_AFFINITY_INFO] = PSCI_0_2_FN64_AFFINITY_INFO; 281 psci_ops.affinity_info = psci_affinity_info; 282 283 psci_function_id[PSCI_FN_MIGRATE_INFO_TYPE] = 284 PSCI_0_2_FN_MIGRATE_INFO_TYPE; 285 psci_ops.migrate_info_type = psci_migrate_info_type; 286 287 arm_pm_restart = psci_sys_reset; 288 289 pm_power_off = psci_sys_poweroff; 290 291 out_put_node: 292 of_node_put(np); 293 return err; 294 } 295 296 /* 297 * PSCI < v0.2 get PSCI Function IDs via DT. 298 */ 299 static int __init psci_0_1_init(struct device_node *np) 300 { 301 u32 id; 302 int err; 303 304 err = get_set_conduit_method(np); 305 306 if (err) 307 goto out_put_node; 308 309 pr_info("Using PSCI v0.1 Function IDs from DT\n"); 310 311 if (!of_property_read_u32(np, "cpu_suspend", &id)) { 312 psci_function_id[PSCI_FN_CPU_SUSPEND] = id; 313 psci_ops.cpu_suspend = psci_cpu_suspend; 314 } 315 316 if (!of_property_read_u32(np, "cpu_off", &id)) { 317 psci_function_id[PSCI_FN_CPU_OFF] = id; 318 psci_ops.cpu_off = psci_cpu_off; 319 } 320 321 if (!of_property_read_u32(np, "cpu_on", &id)) { 322 psci_function_id[PSCI_FN_CPU_ON] = id; 323 psci_ops.cpu_on = psci_cpu_on; 324 } 325 326 if (!of_property_read_u32(np, "migrate", &id)) { 327 psci_function_id[PSCI_FN_MIGRATE] = id; 328 psci_ops.migrate = psci_migrate; 329 } 330 331 out_put_node: 332 of_node_put(np); 333 return err; 334 } 335 336 static const struct of_device_id psci_of_match[] __initconst = { 337 { .compatible = "arm,psci", .data = psci_0_1_init}, 338 { .compatible = "arm,psci-0.2", .data = psci_0_2_init}, 339 {}, 340 }; 341 342 int __init psci_init(void) 343 { 344 struct device_node *np; 345 const struct of_device_id *matched_np; 346 psci_initcall_t init_fn; 347 348 np = of_find_matching_node_and_match(NULL, psci_of_match, &matched_np); 349 350 if (!np) 351 return -ENODEV; 352 353 init_fn = (psci_initcall_t)matched_np->data; 354 return init_fn(np); 355 } 356 357 #ifdef CONFIG_SMP 358 359 static int __init cpu_psci_cpu_init(struct device_node *dn, unsigned int cpu) 360 { 361 return 0; 362 } 363 364 static int __init cpu_psci_cpu_prepare(unsigned int cpu) 365 { 366 if (!psci_ops.cpu_on) { 367 pr_err("no cpu_on method, not booting CPU%d\n", cpu); 368 return -ENODEV; 369 } 370 371 return 0; 372 } 373 374 static int cpu_psci_cpu_boot(unsigned int cpu) 375 { 376 int err = psci_ops.cpu_on(cpu_logical_map(cpu), __pa(secondary_entry)); 377 if (err) 378 pr_err("failed to boot CPU%d (%d)\n", cpu, err); 379 380 return err; 381 } 382 383 #ifdef CONFIG_HOTPLUG_CPU 384 static int cpu_psci_cpu_disable(unsigned int cpu) 385 { 386 /* Fail early if we don't have CPU_OFF support */ 387 if (!psci_ops.cpu_off) 388 return -EOPNOTSUPP; 389 return 0; 390 } 391 392 static void cpu_psci_cpu_die(unsigned int cpu) 393 { 394 int ret; 395 /* 396 * There are no known implementations of PSCI actually using the 397 * power state field, pass a sensible default for now. 398 */ 399 struct psci_power_state state = { 400 .type = PSCI_POWER_STATE_TYPE_POWER_DOWN, 401 }; 402 403 ret = psci_ops.cpu_off(state); 404 405 pr_crit("unable to power off CPU%u (%d)\n", cpu, ret); 406 } 407 408 static int cpu_psci_cpu_kill(unsigned int cpu) 409 { 410 int err, i; 411 412 if (!psci_ops.affinity_info) 413 return 1; 414 /* 415 * cpu_kill could race with cpu_die and we can 416 * potentially end up declaring this cpu undead 417 * while it is dying. So, try again a few times. 418 */ 419 420 for (i = 0; i < 10; i++) { 421 err = psci_ops.affinity_info(cpu_logical_map(cpu), 0); 422 if (err == PSCI_0_2_AFFINITY_LEVEL_OFF) { 423 pr_info("CPU%d killed.\n", cpu); 424 return 1; 425 } 426 427 msleep(10); 428 pr_info("Retrying again to check for CPU kill\n"); 429 } 430 431 pr_warn("CPU%d may not have shut down cleanly (AFFINITY_INFO reports %d)\n", 432 cpu, err); 433 /* Make op_cpu_kill() fail. */ 434 return 0; 435 } 436 #endif 437 #endif 438 439 const struct cpu_operations cpu_psci_ops = { 440 .name = "psci", 441 #ifdef CONFIG_SMP 442 .cpu_init = cpu_psci_cpu_init, 443 .cpu_prepare = cpu_psci_cpu_prepare, 444 .cpu_boot = cpu_psci_cpu_boot, 445 #ifdef CONFIG_HOTPLUG_CPU 446 .cpu_disable = cpu_psci_cpu_disable, 447 .cpu_die = cpu_psci_cpu_die, 448 .cpu_kill = cpu_psci_cpu_kill, 449 #endif 450 #endif 451 }; 452 453