test-runner: SMP support
Start secondary CPUs before running tests. Currently hardcoded to 4 CPUs
and current qemu gic address.
Bug: 116851819
Change-Id: I68011ccb8d0cff02e888ac2bae69d5f1aa9de4ab
diff --git a/test-runner/arm64/asm.S b/test-runner/arm64/asm.S
index 2b396ef..2f32751 100644
--- a/test-runner/arm64/asm.S
+++ b/test-runner/arm64/asm.S
@@ -24,6 +24,19 @@
.text
+#define CPU_COUNT (4)
+#define STACK_SIZE (4096)
+
+#define GICD_BASE (0x08000000)
+#define GICD_SGIR (0xf00)
+
+.macro get_cpu_num, reg
+ /* Get cpu number */
+ mrs \reg, mpidr_el1
+ bic \reg, \reg, #(1<<24) /* Clear MT */
+ bic \reg, \reg, #(3<<30) /* Clear RES1 and U */
+.endm
+
.globl _start
_start:
/* Set exception vector base */
@@ -39,9 +52,17 @@
msr vbar_el1, x0
vbar_setup_done:
+ get_cpu_num x0
+
/* Setup stack */
- adr x0, stack
- mov sp, x0
+ mov x1, #STACK_SIZE
+ mul x1, x1, x0
+ adr x2, stack
+ add x2, x2, x1
+ adr x1, stack_space_end
+ cmp x2, x1
+ bhi error
+ mov sp, x2
/* Jump to c-code */
bl boot
@@ -61,11 +82,6 @@
.quad 0x20026 /* ADP_Stopped_ApplicationExit */
.quad 2 /* exit code */
-.globl trusty_idle
-trusty_idle:
- wfi
- ret
-
/*
* Catch exceptions (except sync-sp_el0 as we don't use that mode and it would
* jump to our entry point) and exit in case there is a bug.
@@ -91,7 +107,56 @@
exception 0x700
exception 0x780
+.globl trusty_idle
+trusty_idle:
+ // x1 = event_poll
+ adr x0, skip_cpu0_wfi // x0 = &skip_cpu0_wfi
+ get_cpu_num x2 // x2 = cpunum
+ cbz x2, cpu0_idle // if cpunum == 0 goto cpu0_idle
+
+ // skip_cpu0_wfi = cpunum (any value non-0 would work)
+ stlr x2, [x0]
+
+ // Send int 15 to cpu 0 to take it out of wfi
+ ldr x3, =GICD_BASE
+ ldr x4, =0x1800f
+ str x4, [x3, GICD_SGIR]
+
+ b wfi
+
+cpu0_idle:
+ cbz x1, wfi // only clear and skip event_poll wfi
+ ldaxr x3, [x0] // x1 = skip_cpu0_wfi
+ stxr w4, xzr, [x0] // skip_cpu0_wfi = 0
+ cbnz x3, no_wfi
+wfi:
+ wfi
+no_wfi:
+ ret
+
+.globl arch_start_secondary_cpus
+arch_start_secondary_cpus:
+ sub sp, sp, #8
+ mov x1, #1
+ str x1, [sp]
+start_secondary_cpu_loop:
+ ldr x0, =0xC4000003 /* psci CPU_ON */
+ adr x2, _start
+ mov x3, #0
+ smc 0
+ ldr x1, [sp]
+ add x1, x1, #1
+ str x1, [sp]
+ cmp x1, #CPU_COUNT
+ blo start_secondary_cpu_loop
+ add sp, sp, #8
+ ret
.data
- .space 4096
+ .space STACK_SIZE
stack:
+ .space STACK_SIZE * (CPU_COUNT - 1)
+stack_space_end:
+
+skip_cpu0_wfi:
+ .long 0
diff --git a/test-runner/include/test-runner-arch.h b/test-runner/include/test-runner-arch.h
index 6ea4a5f..4a26e28 100644
--- a/test-runner/include/test-runner-arch.h
+++ b/test-runner/include/test-runner-arch.h
@@ -55,6 +55,8 @@
int host_write(int handle, const void* data, size_t size);
int host_system(const char* cmd);
+int arch_start_secondary_cpus(void);
+
/*
* Boot next operating system.
*/
diff --git a/test-runner/test-runner.c b/test-runner/test-runner.c
index 87d5728..dd966a9 100644
--- a/test-runner/test-runner.c
+++ b/test-runner/test-runner.c
@@ -57,9 +57,9 @@
* responsible for reporting the error. It currently returns to the host with
* 2 as the exit code.
*/
-void boot(void) {
+void boot(int cpu) {
int ret;
- int stdout;
+ static int stdout;
int chan;
int status;
char cmdline[256];
@@ -71,10 +71,21 @@
.base = test_result,
.len = sizeof(test_result),
};
- struct trusty_dev trusty_dev;
+ static struct trusty_dev trusty_dev;
struct trusty_ipc_dev* ipc_dev;
struct trusty_ipc_chan test_chan;
+ if (cpu) {
+ while (true) {
+ ret = trusty_dev_nop(&trusty_dev);
+ if (!ret) {
+ trusty_idle(&trusty_dev, false);
+ } else {
+ write_str(stdout, "Secondary cpu unexpected error code\n");
+ }
+ }
+ }
+
/* Read test arguments from host (port name of test server to connect to) */
cmdline_len = host_get_cmdline(cmdline, sizeof(cmdline));
if (!starts_with(boottest_cmd, cmdline, cmdline_len)) {
@@ -105,6 +116,12 @@
return;
}
+ ret = arch_start_secondary_cpus();
+ if (ret) {
+ write_str(stdout, "Failed to start secondary CPUs\n");
+ return;
+ }
+
/* Create connection to test server */
trusty_ipc_chan_init(&test_chan, ipc_dev);
chan = trusty_ipc_connect(&test_chan, port, true);
