summaryrefslogtreecommitdiffstats
path: root/commands/smc.c
blob: 2a53e1b64752a796cd72f81be23cd9984c4cb4b9 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
// SPDX-License-Identifier: GPL-2.0-only

#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