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