diff options
Diffstat (limited to 'arch/arm/cpu')
-rw-r--r-- | arch/arm/cpu/Makefile | 1 | ||||
-rw-r--r-- | arch/arm/cpu/psci-client.c | 190 | ||||
-rw-r--r-- | arch/arm/cpu/psci.c | 46 |
3 files changed, 233 insertions, 4 deletions
diff --git a/arch/arm/cpu/Makefile b/arch/arm/cpu/Makefile index e0b16747ad..09b3bc2eea 100644 --- a/arch/arm/cpu/Makefile +++ b/arch/arm/cpu/Makefile @@ -16,6 +16,7 @@ pbl-$(CONFIG_BOARD_ARM_GENERIC_DT_AARCH64) += board-dt-2nd-aarch64.o obj-pbl-y += setupc$(S64).o cache$(S64).o obj-$(CONFIG_BOOTM_OPTEE) += start-kernel-optee.o +obj-$(CONFIG_ARM_PSCI_CLIENT) += psci-client.o # # Any variants can be called as start-armxyz.S diff --git a/arch/arm/cpu/psci-client.c b/arch/arm/cpu/psci-client.c new file mode 100644 index 0000000000..b5d0d37497 --- /dev/null +++ b/arch/arm/cpu/psci-client.c @@ -0,0 +1,190 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * Copyright (C) 2017 Masahiro Yamada <yamada.masahiro@socionext.com> + * Copyright (C) 2019 Ahmad Fatoum, Pengutronix + */ + +#include <common.h> +#include <init.h> +#include <driver.h> +#include <asm/psci.h> +#include <asm/secure.h> +#include <poweroff.h> +#include <restart.h> +#include <linux/arm-smccc.h> + +static struct restart_handler restart; + +static void __noreturn psci_invoke_noreturn(int function) +{ + int ret; + + ret = psci_invoke(function, 0, 0, 0, NULL); + + pr_err("psci command failed: %s\n", strerror(-ret)); + hang(); +} + +static void __noreturn psci_poweroff(struct poweroff_handler *handler) +{ + psci_invoke_noreturn(ARM_PSCI_0_2_FN_SYSTEM_OFF); +} + +static void __noreturn psci_restart(struct restart_handler *rst) +{ + psci_invoke_noreturn(ARM_PSCI_0_2_FN_SYSTEM_RESET); +} + +static u32 version; +int psci_get_version(void) +{ + if (!version) + return -EPROBE_DEFER; + + return version; +} + +static u32 (*psci_invoke_fn)(ulong, ulong, ulong, ulong); + +static int psci_xlate_error(s32 errnum) +{ + switch (errnum) { + case ARM_PSCI_RET_NOT_SUPPORTED: + return -ENOTSUPP; // Operation not supported + case ARM_PSCI_RET_INVAL: + return -EINVAL; // Invalid argument + case ARM_PSCI_RET_DENIED: + return -EPERM; // Operation not permitted + case ARM_PSCI_RET_ALREADY_ON: + return -EBUSY; // CPU already on + case ARM_PSCI_RET_ON_PENDING: + return -EALREADY; // CPU_ON in progress + case ARM_PSCI_RET_INTERNAL_FAILURE: + return -EIO; // Internal failure + case ARM_PSCI_RET_NOT_PRESENT: + return -ESRCH; // Trusted OS not present on core + case ARM_PSCI_RET_DISABLED: + return -ENODEV; // CPU is disabled + case ARM_PSCI_RET_INVALID_ADDRESS: + return -EACCES; // Bad address + default: + return errnum; + }; +} + +int psci_invoke(ulong function, ulong arg0, ulong arg1, ulong arg2, + ulong *result) +{ + ulong ret; + if (!psci_invoke_fn) + return -EPROBE_DEFER; + + ret = psci_invoke_fn(function, arg0, arg1, arg2); + if (result) + *result = ret; + + switch (function) { + case ARM_PSCI_0_2_FN_PSCI_VERSION: + case ARM_PSCI_1_0_FN64_STAT_RESIDENCY: + case ARM_PSCI_1_0_FN64_STAT_COUNT: + /* These don't return an error code */ + return 0; + } + + return psci_xlate_error(ret); +} + +static u32 invoke_psci_fn_hvc(ulong function, ulong arg0, ulong arg1, ulong arg2) +{ + struct arm_smccc_res res; + arm_smccc_hvc(function, arg0, arg1, arg2, 0, 0, 0, 0, &res); + return res.a0; +} + +static u32 invoke_psci_fn_smc(ulong function, ulong arg0, ulong arg1, ulong arg2) +{ + struct arm_smccc_res res; + arm_smccc_smc(function, arg0, arg1, arg2, 0, 0, 0, 0, &res); + return res.a0; +} + +static int of_psci_do_fixup(struct device_node *root, void *context) +{ + return of_psci_fixup(root, *(u32 *)context); +} + +static int __init psci_probe(struct device_d *dev) +{ + const char *method; + ulong of_version, actual_version; + int ret; + + ret = dev_get_drvdata(dev, (const void **)&of_version); + if (ret) + return -ENODEV; + + ret = of_property_read_string(dev->device_node, "method", &method); + if (ret) { + dev_warn(dev, "missing \"method\" property\n"); + return -ENXIO; + } + + if (!strcmp(method, "hvc")) { + psci_invoke_fn = invoke_psci_fn_hvc; + } else if (!strcmp(method, "smc")) { + psci_invoke_fn = invoke_psci_fn_smc; + } else { + pr_warn("invalid \"method\" property: %s\n", method); + return -EINVAL; + } + + + if (of_version < ARM_PSCI_VER(0,2)) { + version = of_version; + + dev_info(dev, "assuming version %u.%u\n", + version >> 16, version & 0xffff); + dev_dbg(dev, "Not registering reset handler due to PSCI version\n"); + + return 0; + } + + psci_invoke(ARM_PSCI_0_2_FN_PSCI_VERSION, 0, 0, 0, &actual_version); + version = actual_version; + + dev_info(dev, "detected version %u.%u\n", + version >> 16, version & 0xffff); + + if (actual_version != of_version) + of_register_fixup(of_psci_do_fixup, &version); + + ret = poweroff_handler_register_fn(psci_poweroff); + if (ret) + dev_warn(dev, "error registering poweroff handler: %s\n", + strerror(-ret)); + + restart.name = "psci"; + restart.restart = psci_restart; + restart.priority = 400; + + ret = restart_handler_register(&restart); + if (ret) + dev_warn(dev, "error registering restart handler: %s\n", + strerror(-ret)); + + return ret; +} + +static __maybe_unused struct of_device_id psci_dt_ids[] = { + { .compatible = "arm,psci", .data = (void*)ARM_PSCI_VER(0,1) }, + { .compatible = "arm,psci-0.2", .data = (void*)ARM_PSCI_VER(0,2) }, + { .compatible = "arm,psci-1.0", .data = (void*)ARM_PSCI_VER(1,0) }, + { /* sentinel */ }, +}; + +static struct driver_d psci_driver = { + .name = "psci", + .probe = psci_probe, + .of_compatible = DRV_OF_COMPAT(psci_dt_ids), +}; +coredevice_platform_driver(psci_driver); diff --git a/arch/arm/cpu/psci.c b/arch/arm/cpu/psci.c index a976ddbb5c..22ce1dfd0e 100644 --- a/arch/arm/cpu/psci.c +++ b/arch/arm/cpu/psci.c @@ -228,7 +228,7 @@ static int armv7_psci_init(void) } device_initcall(armv7_psci_init); -#ifdef DEBUG +#ifdef CONFIG_ARM_PSCI_DEBUG #include <command.h> #include <getopt.h> @@ -248,8 +248,41 @@ void second_entry(void) while (1); } +static const char *psci_xlate_str(long err) +{ + static char errno_string[sizeof "error 0x123456789ABCDEF0"]; + + switch(err) + { + case ARM_PSCI_RET_SUCCESS: + return "Success"; + case ARM_PSCI_RET_NOT_SUPPORTED: + return "Operation not supported"; + case ARM_PSCI_RET_INVAL: + return "Invalid argument"; + case ARM_PSCI_RET_DENIED: + return "Operation not permitted"; + case ARM_PSCI_RET_ALREADY_ON: + return "CPU already on"; + case ARM_PSCI_RET_ON_PENDING: + return "CPU_ON in progress"; + case ARM_PSCI_RET_INTERNAL_FAILURE: + return "Internal failure"; + case ARM_PSCI_RET_NOT_PRESENT: + return "Trusted OS not present on core"; + case ARM_PSCI_RET_DISABLED: + return "CPU is disabled"; + case ARM_PSCI_RET_INVALID_ADDRESS: + return "Bad address"; + } + + sprintf(errno_string, "error 0x%lx", err); + return errno_string; +} + static int do_smc(int argc, char *argv[]) { + long ret; int opt; struct arm_smccc_res res = { .a0 = 0xdeadbee0, @@ -258,7 +291,10 @@ static int do_smc(int argc, char *argv[]) .a3 = 0xdeadbee3, }; - while ((opt = getopt(argc, argv, "nicz")) > 0) { + if (argc < 2) + return COMMAND_ERROR_USAGE; + + while ((opt = getopt(argc, argv, "nic")) > 0) { switch (opt) { case 'n': armv7_secure_monitor_install(); @@ -271,7 +307,10 @@ static int do_smc(int argc, char *argv[]) case 'c': arm_smccc_smc(ARM_PSCI_0_2_FN_CPU_ON, 1, (unsigned long)second_entry, 0, 0, 0, 0, 0, &res); - break; + ret = (long)res.a0; + printf("CPU_ON returns with: %s\n", psci_xlate_str(ret)); + if (ret) + return COMMAND_ERROR; } } @@ -285,7 +324,6 @@ BAREBOX_CMD_HELP_TEXT("Options:") BAREBOX_CMD_HELP_OPT ("-n", "Install secure monitor and switch to nonsecure mode") BAREBOX_CMD_HELP_OPT ("-i", "Show information about installed PSCI version") BAREBOX_CMD_HELP_OPT ("-c", "Start secondary CPU core") -BAREBOX_CMD_HELP_OPT ("-z", "Turn off secondary CPU core") BAREBOX_CMD_HELP_END BAREBOX_CMD_START(smc) |