test-runner: Add gicv3 support using doorbell interrupt

Bug: 122357256
Change-Id: I88045d242d7846f78ede4a4955a6d29a163a2652

6 files changed, 109 insertions, 2 deletions

diff --git a/ql-tipc/include/trusty/smcall.h b/ql-tipc/include/trusty/smcall.h
index db136d0..2589a26 100644
--- a/ql-tipc/include/trusty/smcall.h
+++ b/ql-tipc/include/trusty/smcall.h
@@ -100,7 +100,12 @@
 #define SMC_FC_RESERVED SMC_FASTCALL_NR(SMC_ENTITY_SECURE_MONITOR, 0)
 #define SMC_FC_FIQ_EXIT SMC_FASTCALL_NR(SMC_ENTITY_SECURE_MONITOR, 1)
 #define SMC_FC_REQUEST_FIQ SMC_FASTCALL_NR(SMC_ENTITY_SECURE_MONITOR, 2)
+
+#define TRUSTY_IRQ_TYPE_NORMAL (0)
+#define TRUSTY_IRQ_TYPE_PER_CPU (1)
+#define TRUSTY_IRQ_TYPE_DOORBELL (2)
 #define SMC_FC_GET_NEXT_IRQ SMC_FASTCALL_NR(SMC_ENTITY_SECURE_MONITOR, 3)
+
 #define SMC_FC_FIQ_ENTER SMC_FASTCALL_NR(SMC_ENTITY_SECURE_MONITOR, 4)
 
 #define SMC_FC64_SET_FIQ_HANDLER SMC_FASTCALL64_NR(SMC_ENTITY_SECURE_MONITOR, 5)
diff --git a/test-runner/arm64/arch.c b/test-runner/arm64/arch.c
new file mode 100644
index 0000000..0077c26
--- /dev/null
+++ b/test-runner/arm64/arch.c
@@ -0,0 +1,71 @@
+/*
+ * Copyright (C) 2019 The Android Open Source Project
+ *
+ * Permission is hereby granted, free of charge, to any person
+ * obtaining a copy of this software and associated documentation
+ * files (the "Software"), to deal in the Software without
+ * restriction, including without limitation the rights to use, copy,
+ * modify, merge, publish, distribute, sublicense, and/or sell copies
+ * of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <test-runner-arch.h>
+
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdint.h>
+#include <trusty/smc.h>
+#include <trusty/smcall.h>
+
+#if GIC_VERSION > 2
+#define GICD_BASE (0x08000000)
+#define GICD_CTLR (GICD_BASE + 0x000)
+
+#define GICR_BASE (0x080A0000)
+#define GICR_SGI_BASE (GICR_BASE + 0x10000)
+#define GICR_ISENABLER0 (GICR_SGI_BASE + 0x0100)
+#define GICR_CPU_OFFSET(cpu) ((cpu)*0x20000)
+
+#define REG32(addr) ((volatile uint32_t*)(uintptr_t)(addr))
+#define GICDREG_READ(reg) (*REG32((reg)))
+#define GICDREG_WRITE(reg, val) (*REG32((reg)) = (val))
+
+#define GICRREG_READ(cpu, reg) (*REG32((reg) + GICR_CPU_OFFSET(cpu)))
+#define GICRREG_WRITE(cpu, reg, val) \
+    (*REG32((reg) + GICR_CPU_OFFSET(cpu)) = (val))
+
+static uint32_t doorbell_irq;
+#endif
+
+void boot_arm64(int cpu) {
+#if GIC_VERSION > 2
+    if (!cpu) {
+        GICDREG_WRITE(GICD_CTLR, 2); /* Enable Non-secure group 1 interrupt */
+        doorbell_irq = smc(SMC_FC_GET_NEXT_IRQ, 0, TRUSTY_IRQ_TYPE_DOORBELL, 0);
+    }
+    if (doorbell_irq >= 32) {
+        /*
+         * We only support per-cpu doorbell interrupts which are all enabled by
+         * GICR_ISENABLER0.
+         */
+        return;
+    }
+    GICRREG_WRITE(cpu, GICR_ISENABLER0, 1U << doorbell_irq);
+    GICRREG_WRITE(cpu, GICR_ISENABLER0, 1U << 0); /* skip_cpu0_wfi interrupt */
+    __asm__ volatile("msr icc_igrpen1_el1, %0" ::"r"(1UL));
+#endif
+    boot(cpu);
+}
diff --git a/test-runner/arm64/asm.S b/test-runner/arm64/asm.S
index e9309a5..e038968 100644
--- a/test-runner/arm64/asm.S
+++ b/test-runner/arm64/asm.S
@@ -75,7 +75,7 @@ vbar_setup_done:
     mov sp, x2
 
     /* Jump to c-code */
-    bl boot
+    bl boot_arm64
     /* fall-through */
 
 error:
@@ -126,10 +126,16 @@ trusty_idle:
     // skip_cpu0_wfi = cpunum (any value non-0 would work)
     stlr x2, [x0]
 
+#if GIC_VERSION > 2
+    // Send int 0 to cpu 0 to take it out of wfi
+    ldr x4, =(0 << 24) | 1
+    msr icc_sgi1r_el1, x4
+#else
     // Send int 15 to cpu 0 to take it out of wfi
     ldr x3, =GICD_BASE
     ldr x4, =0x1800f
     str x4, [x3, GICD_SGIR]
+#endif
 
     cbnz x1, no_wfi // also use event_poll to skip wfi on secondary cpus
     b wfi
@@ -144,6 +150,22 @@ wfi:
 no_wfi:
     ret
 
+#if GIC_VERSION > 2
+.globl trusty_local_irq_disable
+trusty_local_irq_disable:
+    /*
+     * Clear doorbell interrupt from trusty or ipi sent from secondary cores in
+     * trusty_idle above.
+     */
+    mrs x0, icc_iar1_el1
+    cmp x0, #1020
+    b.hs .trusty_local_irq_disable_done
+    msr icc_eoir1_el1, x0
+    b trusty_local_irq_disable
+.trusty_local_irq_disable_done:
+    ret
+#endif
+
 .globl arch_start_secondary_cpus
 arch_start_secondary_cpus:
     sub sp, sp, #16
diff --git a/test-runner/arm64/rules.mk b/test-runner/arm64/rules.mk
index 48198d1..dd1ada0 100644
--- a/test-runner/arm64/rules.mk
+++ b/test-runner/arm64/rules.mk
@@ -22,8 +22,12 @@
 # SOFTWARE.
 #
 
+MODULE_DEFINES += \
+	GIC_VERSION=$(GIC_VERSION) \
+
 MODULE_SRCS += \
 	$(LOCAL_DIR)/$(ARCH)/asm.S \
+	$(LOCAL_DIR)/$(ARCH)/arch.c \
 	$(LOCAL_DIR)/$(ARCH)/boot.c \
 	$(LOCAL_DIR)/$(ARCH)/trusty_mem.c \
 	$(QL_TIPC)/arch/arm/trusty_dev.c \
diff --git a/test-runner/include/test-runner-arch.h b/test-runner/include/test-runner-arch.h
index 4a26e28..467653d 100644
--- a/test-runner/include/test-runner-arch.h
+++ b/test-runner/include/test-runner-arch.h
@@ -58,6 +58,11 @@ int host_system(const char* cmd);
 int arch_start_secondary_cpus(void);
 
 /*
+ * Called from arch to enter test-runner on each cpu.
+ */
+void boot(int cpu);
+
+/*
  * Boot next operating system.
  */
 void boot_next(void);
diff --git a/test-runner/test-runner-sysdeps.c b/test-runner/test-runner-sysdeps.c
index e488531..4ed015b 100644
--- a/test-runner/test-runner-sysdeps.c
+++ b/test-runner/test-runner-sysdeps.c
@@ -89,7 +89,7 @@ void trusty_lock(struct trusty_dev* dev) {}
 
 void trusty_unlock(struct trusty_dev* dev) {}
 
-void trusty_local_irq_disable(unsigned long* state) {}
+__attribute__((weak)) void trusty_local_irq_disable(unsigned long* state) {}
 
 void trusty_local_irq_restore(unsigned long* state) {}