From b70987f86cfa22ecf3314e6d5c2a25e11e8be2b5 Mon Sep 17 00:00:00 2001
From: Hideki Miyazaki <hide@wolfssl.com>
Date: Sat, 25 Jan 2025 11:28:55 +0900
Subject: [PATCH] revert simple start-up gard UART int NO_QNX definition

---
 config/examples/raspi3.config |  3 +-
 hal/raspi3.c                  |  7 +++--
 hal/raspi3.h                  | 28 ++++++++++++++++++
 options.mk                    |  4 ++-
 src/boot_aarch64.c            |  2 +-
 src/boot_aarch64_start.S      | 54 ++++++++++++++++++++++++++++++++++-
 6 files changed, 91 insertions(+), 7 deletions(-)
 create mode 100644 hal/raspi3.h

diff --git a/config/examples/raspi3.config b/config/examples/raspi3.config
index 13ef84d8a..60ca6bb3c 100644
--- a/config/examples/raspi3.config
+++ b/config/examples/raspi3.config
@@ -4,12 +4,13 @@ SIGN?=RSA4096
 HASH?=SHA3
 DEBUG?=1
 VTOR?=1
-NO_XIP?=1
 SPMATH?=1
 IMAGE_HEADER_SIZE?=1024
 PKA?=1
 WOLFTPM?=0
 DEBUG_UART?=0
+NO_XIP?=1
+NO_QNX?=1
 WOLFBOOT_SECTOR_SIZE=0x400
 WOLFBOOT_NO_PARTITIONS=1
 WOLFBOOT_LOAD_ADDRESS?=0x3080000
diff --git a/hal/raspi3.c b/hal/raspi3.c
index b0d7d85a3..febbb561b 100644
--- a/hal/raspi3.c
+++ b/hal/raspi3.c
@@ -102,6 +102,7 @@ static inline void delay(int32_t count)
         asm volatile("nop");
     }
 }
+#if defined(DEBUG_UART)
 /**
  * write message to mailbox
  */
@@ -213,7 +214,7 @@ void uart_init()
     /* enable UART0 transfer & receive*/
     *UART0_CR = (1 << 0) | (1 << 8) | (1 << 9);
 }
-
+#endif
 void* hal_get_primary_address(void)
 {
     return (void*)&kernel_addr;
@@ -300,7 +301,7 @@ void hal_init(void)
         "0123456789abcdef";
     wolfBoot_set_encrypt_key((uint8_t *)enc_key,(uint8_t *)(enc_key +  32));
     #endif
-
+#if defined(DEBUG_UART)
     uart_init();
 
     /* length of the message */
@@ -341,7 +342,7 @@ void hal_init(void)
     wolfBoot_printf("\n M2MC clock : %d Hz", getclocks(13));
     wolfBoot_printf("\n PIXEL_BVB clock : %d Hz\n", getclocks(14));
 #endif
-
+#endif
 }
 
 void hal_prepare_boot(void)
diff --git a/hal/raspi3.h b/hal/raspi3.h
new file mode 100644
index 000000000..6328a94cc
--- /dev/null
+++ b/hal/raspi3.h
@@ -0,0 +1,28 @@
+/* raspi3.h
+ *
+ * Copyright (C) 2025 wolfSSL Inc.
+ *
+ * This file is part of wolfBoot.
+ *
+ * wolfBoot is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * wolfBoot is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1335, USA
+ */
+
+#ifndef _RASPI3_H_
+#define _RASPI3_H_
+
+#define USE_BUILTIN_STARTUP
+#define USE_SIMPLE_STARTUP
+
+#endif /* _RASPI3_H_ */
\ No newline at end of file
diff --git a/options.mk b/options.mk
index 43396e0e8..561552621 100644
--- a/options.mk
+++ b/options.mk
@@ -563,7 +563,9 @@ endif
 ifeq ($(NO_XIP),1)
   CFLAGS+=-D"NO_XIP"
 endif
-
+ifeq ($(NO_QNX),1)
+  CFLAGS+=-D"NO_QNX"
+endif
 
 ifeq ($(ALLOW_DOWNGRADE),1)
   CFLAGS+= -D"ALLOW_DOWNGRADE"
diff --git a/src/boot_aarch64.c b/src/boot_aarch64.c
index dec78f6d9..f36690a4a 100644
--- a/src/boot_aarch64.c
+++ b/src/boot_aarch64.c
@@ -1,6 +1,6 @@
 /* boot_aarch64.c
  *
- * Copyright (C) 2024 wolfSSL Inc.
+ * Copyright (C) 2025 wolfSSL Inc.
  *
  * This file is part of wolfBoot.
  *
diff --git a/src/boot_aarch64_start.S b/src/boot_aarch64_start.S
index 79cc3f67d..0a62b71d4 100644
--- a/src/boot_aarch64_start.S
+++ b/src/boot_aarch64_start.S
@@ -30,6 +30,10 @@
 #include "hal/nxp_ls1028a.h"
 #endif
 
+#ifdef TARGET_raspi3
+#include "hal/raspi3.h"
+#endif
+
 /* GICv2 Register Offsets */
 #ifndef GICD_BASE
 #define GICD_BASE       0xF9010000
@@ -1028,7 +1032,55 @@ FPUStatus:
     .skip 1
 
 .align 8
-
+#elif defined(USE_SIMPLE_STARTUP)
+.section ".boot"
+.global _vector_table
+_vector_table:
+   mov x21, x0        // read ATAG/FDT address
+
+4: ldr x1, =_vector_table // get start of .text in x1
+    // Read current EL
+    mrs     x0, CurrentEL
+    and     x0, x0, #0x0C
+
+    // EL == 3?
+    cmp     x0, #12
+    bne     2f
+3:  mrs     x2, scr_el3
+    orr     x2, x2, 0x0F    // scr_el3 |= NS|IRQ|FIQ|EA
+    msr     scr_el3, x2
+
+    msr cptr_el3, xzr       // enable FP/SIMD
+
+    // EL == 1?
+2:  cmp     x0, #4
+    beq     1f
+
+    // EL == 2?
+    mov x2, #3 << 20
+    msr cptr_el2, x2           /* Enable FP/SIMD */
+    b 0f
+
+1:  mov x0, #3 << 20
+    msr cpacr_el1, x0           // Enable FP/SIMD for EL1
+    msr     sp_el1, x1
+
+   /* Suspend slave CPUs */
+0: mrs x3, mpidr_el1  // read MPIDR_EL1
+   and x3, x3, #3     // CPUID = MPIDR_EL1 & 0x03
+   cbz x3, 8f         // if 0, branch forward
+7: wfi                // infinite sleep
+   b    7b
+
+8:  mov sp, x1         // set stack pointer
+    bl boot_entry_C   // boot_entry_C never returns
+    b   7b            // go to sleep anyhow in case.
+#if 0
+.section ".boot"
+.global _vector_table
+_vector_table:
+    bl boot_entry_C   // boot_entry_C never returns
+#endif
 #endif /* !USE_BUILTIN_STARTUP */