summaryrefslogtreecommitdiffstats
path: root/commands/smc.c
diff options
context:
space:
mode:
Diffstat (limited to 'commands/smc.c')
-rw-r--r--commands/smc.c162
1 files changed, 162 insertions, 0 deletions
diff --git a/commands/smc.c b/commands/smc.c
new file mode 100644
index 0000000000..84102f3249
--- /dev/null
+++ b/commands/smc.c
@@ -0,0 +1,162 @@
+// SPDX-License-Identifier: GPL-2.0
+
+#include <common.h>
+#include <command.h>
+#include <getopt.h>
+#include <dma.h>
+#include <linux/iopoll.h>
+#include <asm/barebox-arm-head.h>
+
+#include <asm/psci.h>
+#include <asm/secure.h>
+#include <linux/arm-smccc.h>
+
+#define STACK_SIZE 100
+#define HANDSHAKE_MAGIC 0xBA12EB0C
+#define ERROR_MAGIC 0xDEADBEEF
+
+struct cpu_context {
+ unsigned long stack[STACK_SIZE];
+ long handshake;
+};
+
+static void noinline cpu_handshake(long *handshake)
+{
+ struct arm_smccc_res res;
+
+ writel(HANDSHAKE_MAGIC, handshake);
+
+ arm_smccc_smc(ARM_PSCI_0_2_FN_CPU_OFF,
+ 0, 0, 0, 0, 0, 0, 0, &res);
+
+ writel(ERROR_MAGIC, handshake);
+
+ while (1)
+ ;
+}
+
+static void __naked second_entry(unsigned long arg0)
+{
+ struct cpu_context *context = (void*)arg0;
+
+ arm_cpu_lowlevel_init();
+ arm_setup_stack((unsigned long)&context->stack[STACK_SIZE]);
+ barrier();
+
+ cpu_handshake(&context->handshake);
+}
+
+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 struct cpu_context *context;
+
+static int do_smc(int argc, char *argv[])
+{
+ long ret;
+ int opt;
+ struct arm_smccc_res res = {
+ .a0 = 0xdeadbee0,
+ .a1 = 0xdeadbee1,
+ .a2 = 0xdeadbee2,
+ .a3 = 0xdeadbee3,
+ };
+
+ if (argc < 2)
+ return COMMAND_ERROR_USAGE;
+
+ while ((opt = getopt(argc, argv, "nic")) > 0) {
+ switch (opt) {
+ case 'n':
+ if (!IS_ENABLED(CONFIG_ARM_SECURE_MONITOR)) {
+ printf("secure monitor support not compiled in\n");
+ return COMMAND_ERROR;
+ }
+
+ armv7_secure_monitor_install();
+ break;
+ case 'i':
+ arm_smccc_smc(ARM_PSCI_0_2_FN_PSCI_VERSION,
+ 0, 0, 0, 0, 0, 0, 0, &res);
+ printf("found psci version %ld.%ld\n", res.a0 >> 16, res.a0 & 0xffff);
+ break;
+ case 'c':
+ if (!context)
+ context = dma_alloc_coherent(sizeof(*context),
+ DMA_ADDRESS_BROKEN);
+
+ if (!context) {
+ printf("Out of memory\n");
+ return COMMAND_ERROR;
+ }
+
+ arm_smccc_smc(ARM_PSCI_0_2_FN_CPU_ON,
+ 1, (unsigned long)second_entry, (unsigned long)context,
+ 0, 0, 0, 0, &res);
+ ret = (long)res.a0;
+ printf("CPU_ON returns with: %s\n", psci_xlate_str(ret));
+ if (ret) {
+ unsigned long magic = readl(&context->handshake);
+ if (magic == ERROR_MAGIC)
+ printf("Turning off CPU had failed\n");
+ return COMMAND_ERROR;
+ }
+
+ readl_poll_timeout(&context->handshake, ret,
+ ret != HANDSHAKE_MAGIC, USEC_PER_SEC);
+ if (ret == 0) {
+ printf("Second CPU handshake timed out.\n");
+ return COMMAND_ERROR;
+ }
+
+ printf("Second CPU handshake successful.\n");
+ }
+ }
+
+
+ return 0;
+}
+BAREBOX_CMD_HELP_START(smc)
+BAREBOX_CMD_HELP_TEXT("Secure monitor code test command")
+BAREBOX_CMD_HELP_TEXT("")
+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_END
+
+BAREBOX_CMD_START(smc)
+ .cmd = do_smc,
+ BAREBOX_CMD_DESC("secure monitor test command")
+ BAREBOX_CMD_HELP(cmd_smc_help)
+ BAREBOX_CMD_GROUP(CMD_GRP_MISC)
+BAREBOX_CMD_END