From 6b6d5611a7be3e6a56d8325e9a46d56ec45904ee Mon Sep 17 00:00:00 2001
From: David Garske <david@wolfssl.com>
Date: Tue, 10 Mar 2020 11:30:22 -0700
Subject: [PATCH] Added support for initializting the GICv2 interrupt
 controller. This is required for QNX kernel boot.

---
 IDE/XilinxSDK/.cproject  |  6 ++++--
 hal/zynq.c               |  2 +-
 src/boot_aarch64.c       | 19 +++++++++++++------
 src/boot_aarch64_start.S | 37 +++++++++++++++++++++++++++++++++++++
 4 files changed, 55 insertions(+), 9 deletions(-)

diff --git a/IDE/XilinxSDK/.cproject b/IDE/XilinxSDK/.cproject
index 91fc2a93..8d82f455 100644
--- a/IDE/XilinxSDK/.cproject
+++ b/IDE/XilinxSDK/.cproject
@@ -36,6 +36,7 @@
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/src}&quot;"/>
 								</option>
 								<option id="xilinx.gnu.compiler.symbols.defined.980385157" name="Defined symbols (-D)" superClass="xilinx.gnu.compiler.symbols.defined" valueType="definedSymbols">
+									<listOptionValue builtIn="false" value="USE_BUILTIN_STARTUP"/>
 									<listOptionValue builtIn="false" value="DEBUG=1"/>
 									<listOptionValue builtIn="false" value="__WOLFBOOT"/>
 									<listOptionValue builtIn="false" value="PART_UPDATE_EXT=1"/>
@@ -94,7 +95,7 @@
 						</toolChain>
 					</folderInfo>
 					<sourceEntries>
-						<entry excluding="src/boot_aarch64_start.S|tools|test-app|src/xmalloc_ecc.c|src/vector_riscv.S|src/update_flash.c|src/update_flash_hwswap.c|src/boot_riscv.c|src/boot_arm.c|hal" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/>
+						<entry excluding="tools|test-app|src/xmalloc_ecc.c|src/vector_riscv.S|src/update_flash.c|src/update_flash_hwswap.c|src/boot_riscv.c|src/boot_arm.c|hal" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/>
 						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="hal"/>
 					</sourceEntries>
 				</configuration>
@@ -130,6 +131,7 @@
 								</option>
 								<option id="xilinx.gnu.compiler.inferred.swplatform.flags.1801302027" name="Software Platform Inferred Flags" superClass="xilinx.gnu.compiler.inferred.swplatform.flags" value=" " valueType="string"/>
 								<option id="xilinx.gnu.compiler.symbols.defined.1649249146" name="Defined symbols (-D)" superClass="xilinx.gnu.compiler.symbols.defined" valueType="definedSymbols">
+									<listOptionValue builtIn="false" value="USE_BUILTIN_STARTUP"/>
 									<listOptionValue builtIn="false" value="__WOLFBOOT"/>
 									<listOptionValue builtIn="false" value="PART_UPDATE_EXT=1"/>
 									<listOptionValue builtIn="false" value="PART_SWAP_EXT=1"/>
@@ -190,7 +192,7 @@
 						</toolChain>
 					</folderInfo>
 					<sourceEntries>
-						<entry excluding="src/boot_aarch64_start.S|tools|test-app|src/xmalloc_ecc.c|src/vector_riscv.S|src/update_flash.c|src/update_flash_hwswap.c|src/boot_riscv.c|src/boot_arm.c|hal" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/>
+						<entry excluding="tools|test-app|src/xmalloc_ecc.c|src/vector_riscv.S|src/update_flash.c|src/update_flash_hwswap.c|src/boot_riscv.c|src/boot_arm.c|hal" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/>
 						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="hal"/>
 					</sourceEntries>
 				</configuration>
diff --git a/hal/zynq.c b/hal/zynq.c
index 0b651301..79835251 100644
--- a/hal/zynq.c
+++ b/hal/zynq.c
@@ -798,7 +798,7 @@ void hal_init(void)
     uint32_t cpu_freq = 0;
 
 #ifdef DEBUG_ZYNQ
-    xil_printf("wolfBoot Secure Boot\n");
+    xil_printf("\nwolfBoot Secure Boot\n");
 #endif
 
     asm volatile("msr cntfrq_el0, %0" : : "r" (cpu_freq) : "memory");
diff --git a/src/boot_aarch64.c b/src/boot_aarch64.c
index caabfad4..484fc284 100644
--- a/src/boot_aarch64.c
+++ b/src/boot_aarch64.c
@@ -31,6 +31,7 @@ static volatile unsigned int cpu_id;
 extern unsigned int *END_STACK;
 
 extern void main(void);
+extern void gicv2_init_secure(void);
 
 void boot_entry_C(void) 
 {
@@ -59,21 +60,27 @@ void RAMFUNCTION do_boot(const uint32_t *app_offset, const uint32_t* dts_offset)
 void RAMFUNCTION do_boot(const uint32_t *app_offset)
 #endif
 {
-    /* Set application address via x4 */
-    asm volatile("mov x4, %0" : : "r"(app_offset));
+	/* Set application address via x4 */
+	asm volatile("mov x4, %0" : : "r"(app_offset));
 
 #ifdef MMU
-    /* move the dts pointer to x0 (as first argument) */
-    asm volatile("mov x0, %0" : : "r"(dts_offset));
+	/* Move the dts pointer to x5 (as first argument) */
+	asm volatile("mov x5, %0" : : "r"(dts_offset));
 #else
-    asm volatile("mov x0, xzr");
+	asm volatile("mov x5, xzr");
 #endif
 
-    /* zero registers x1, x2, x3 */
+    /* Initialize GICv2 for Kernel */
+    gicv2_init_secure();
+
+    /* Zero registers x1, x2, x3 */
     asm volatile("mov x3, xzr");
     asm volatile("mov x2, xzr");
     asm volatile("mov x1, xzr");
 
+    /* Move the dts pointer to x0 (as first argument) */
+    asm volatile("mov x0, x5");
+
     /* Unconditionally jump to app_entry at x4 */
     asm volatile("br x4");
 }
diff --git a/src/boot_aarch64_start.S b/src/boot_aarch64_start.S
index f3345e81..16c3e8dd 100644
--- a/src/boot_aarch64_start.S
+++ b/src/boot_aarch64_start.S
@@ -19,6 +19,17 @@
  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1335, USA
  */
 
+
+#define GICD_BASE       0xF9010000
+#define GICD_CTLR       0x0000
+#define GICD_TYPER      0x0004
+#define GICD_SGIR       0x0F00
+#define GICD_IGROUPRn   0x0080
+
+#define GICC_BASE       0xF9020000
+#define GICC_PMR        0x0004
+
+#ifndef USE_BUILTIN_STARTUP
 .section ".boot"
 .global _vector_table
 _vector_table:
@@ -61,4 +72,30 @@ _vector_table:
 8:  mov sp, x1         // set stack pointer
     bl boot_entry_C   // boot_entry_C never returns
     b   7b            // go to sleep anyhow in case.
+#endif /* USE_BUILTIN_STARTUP */
 
+
+/* Initialize GIC 400 (GICv2) */
+.global gicv2_init_secure
+gicv2_init_secure:
+    ldr  x0, =GICD_BASE
+    mov  w9, #0x3            /* EnableGrp0 | EnableGrp1 */
+    str  w9, [x0, GICD_CTLR] /* Secure GICD_CTLR */
+    ldr  w9, [x0, GICD_TYPER]
+    and  w10, w9, #0x1f      /* ITLinesNumber */
+    cbz  w10, 1f             /* No SPIs */
+    add  x11, x0, GICD_IGROUPRn
+    mov  w9, #~0             /* Config SPIs as Grp1 */
+    str  w9, [x11], #0x4
+0:	str  w9, [x11], #0x4
+    sub	 w10, w10, #0x1
+    cbnz w10, 0b
+
+    ldr  x1, =GICC_BASE      /* GICC_CTLR */
+    mov	 w0, #3              /* EnableGrp0 | EnableGrp1 */
+    str	 w0, [x1]
+
+    mov	 w0, #1 << 7         /* Allow NS access to GICC_PMR */
+    str	 w0, [x1, #4]        /* GICC_PMR */
+1:
+	ret