diff --git a/Makefile b/Makefile
index d1f7e07565..aa72d96108 100644
--- a/Makefile
+++ b/Makefile
@@ -384,6 +384,7 @@ keys: $(PRIVATE_KEY)
 
 clean:
 	$(Q)rm -f src/*.o hal/*.o hal/spi/*.o test-app/*.o src/x86/*.o
+	$(Q)rm -f src/wc_secure_calls.o
 	$(Q)rm -f $(WOLFBOOT_LIB_WOLFSSL)/wolfcrypt/src/*.o $(WOLFBOOT_LIB_WOLFTPM)/src/*.o $(WOLFBOOT_LIB_WOLFTPM)/hal/*.o $(WOLFBOOT_LIB_WOLFTPM)/examples/pcr/*.o
 	$(Q)rm -f $(WOLFBOOT_LIB_WOLFSSL)/wolfcrypt/src/port/Renesas/*.o
 	$(Q)rm -f wolfboot.bin wolfboot.elf wolfboot.map test-update.rom wolfboot.hex
diff --git a/arch.mk b/arch.mk
index caf0fa6ec5..17b1ba123c 100644
--- a/arch.mk
+++ b/arch.mk
@@ -1172,6 +1172,11 @@ WOLFCRYPT_OBJS+=$(WOLFBOOT_LIB_WOLFSSL)/wolfcrypt/src/logging.o
 # Debug UART
 ifeq ($(DEBUG_UART),1)
   CFLAGS+=-DDEBUG_UART
+
+  # If this target has a UART driver, add it to the OBJS
+  ifneq (,$(wildcard hal/uart/uart_drv_$(TARGET).c))
+    OBJS+=hal/uart/uart_drv_$(TARGET).o
+  endif
 endif
 
 ifeq ($(NXP_CUSTOM_DCD),1)
diff --git a/config/examples/stm32h5-dualbank.config b/config/examples/stm32h5-dualbank.config
index ebd4e55dc0..40cd1e0267 100644
--- a/config/examples/stm32h5-dualbank.config
+++ b/config/examples/stm32h5-dualbank.config
@@ -18,11 +18,12 @@ V?=0
 SPMATH?=1
 RAM_CODE?=0
 DUALBANK_SWAP?=1
-WOLFBOOT_PARTITION_SIZE?=0xC0000
+WOLFBOOT_PARTITION_SIZE?=0xA0000
 WOLFBOOT_SECTOR_SIZE?=0x2000
-WOLFBOOT_PARTITION_BOOT_ADDRESS?=0x08040000
-WOLFBOOT_PARTITION_UPDATE_ADDRESS?=0x8140000
+WOLFBOOT_PARTITION_BOOT_ADDRESS?=0x08060000
+WOLFBOOT_PARTITION_UPDATE_ADDRESS?=0x08160000
 WOLFBOOT_PARTITION_SWAP_ADDRESS?=0xFFFFFFFF
 FLAGS_HOME=0
 DISABLE_BACKUP=0
 IMAGE_HEADER_SIZE?=1024
+ARMORED=1
diff --git a/config/examples/stm32h5-tz-dualbank-otp-lms.config b/config/examples/stm32h5-tz-dualbank-otp-lms.config
index f14e7435b7..7f76f6af3d 100644
--- a/config/examples/stm32h5-tz-dualbank-otp-lms.config
+++ b/config/examples/stm32h5-tz-dualbank-otp-lms.config
@@ -18,10 +18,10 @@ V?=0
 SPMATH?=1
 RAM_CODE?=1
 DUALBANK_SWAP?=1
-WOLFBOOT_PARTITION_SIZE?=0xC0000
+WOLFBOOT_PARTITION_SIZE?=0xA0000
 WOLFBOOT_SECTOR_SIZE?=0x2000
-WOLFBOOT_PARTITION_BOOT_ADDRESS?=0x08040000
-WOLFBOOT_PARTITION_UPDATE_ADDRESS?=0x8140000
+WOLFBOOT_PARTITION_BOOT_ADDRESS?=0x08060000
+WOLFBOOT_PARTITION_UPDATE_ADDRESS?=0x08160000
 WOLFBOOT_PARTITION_SWAP_ADDRESS?=0xFFFFFFFF
 FLAGS_HOME=0
 DISABLE_BACKUP=0
diff --git a/config/examples/stm32h5-tz-dualbank-otp.config b/config/examples/stm32h5-tz-dualbank-otp.config
index 5f7cd1c228..b09b08fbae 100644
--- a/config/examples/stm32h5-tz-dualbank-otp.config
+++ b/config/examples/stm32h5-tz-dualbank-otp.config
@@ -18,10 +18,10 @@ V?=0
 SPMATH?=1
 RAM_CODE?=1
 DUALBANK_SWAP?=1
-WOLFBOOT_PARTITION_SIZE?=0xC0000
+WOLFBOOT_PARTITION_SIZE?=0xA0000
 WOLFBOOT_SECTOR_SIZE?=0x2000
-WOLFBOOT_PARTITION_BOOT_ADDRESS?=0x08040000
-WOLFBOOT_PARTITION_UPDATE_ADDRESS?=0x8140000
+WOLFBOOT_PARTITION_BOOT_ADDRESS?=0x08060000
+WOLFBOOT_PARTITION_UPDATE_ADDRESS?=0x08160000
 WOLFBOOT_PARTITION_SWAP_ADDRESS?=0xFFFFFFFF
 FLAGS_HOME=0
 DISABLE_BACKUP=0
diff --git a/config/examples/stm32h5-tz.config b/config/examples/stm32h5-tz.config
index a446942adb..66c1bae751 100644
--- a/config/examples/stm32h5-tz.config
+++ b/config/examples/stm32h5-tz.config
@@ -16,15 +16,16 @@ NVM_FLASH_WRITEONCE?=1
 WOLFBOOT_VERSION?=1
 V?=0
 SPMATH?=1
-RAM_CODE?=0
+RAM_CODE?=1
 DUALBANK_SWAP?=0
-WOLFBOOT_PARTITION_SIZE?=0xC0000
+WOLFBOOT_PARTITION_SIZE?=0xA0000
 WOLFBOOT_SECTOR_SIZE?=0x2000
-WOLFBOOT_PARTITION_BOOT_ADDRESS?=0x08040000
-WOLFBOOT_PARTITION_UPDATE_ADDRESS?=0x8140000
-WOLFBOOT_PARTITION_SWAP_ADDRESS?=0x08010000
+WOLFBOOT_PARTITION_BOOT_ADDRESS?=0x08060000
+WOLFBOOT_PARTITION_UPDATE_ADDRESS?=0x08100000
+WOLFBOOT_PARTITION_SWAP_ADDRESS?=0x081A0000
 FLAGS_HOME=0
 DISABLE_BACKUP=0
 WOLFCRYPT_TZ=1
 WOLFCRYPT_TZ_PKCS11=1
 IMAGE_HEADER_SIZE?=1024
+ARMORED=1
diff --git a/config/examples/stm32h5.config b/config/examples/stm32h5.config
index ab141fe6b6..61a340fd3c 100644
--- a/config/examples/stm32h5.config
+++ b/config/examples/stm32h5.config
@@ -18,11 +18,12 @@ V?=0
 SPMATH?=1
 RAM_CODE?=0
 DUALBANK_SWAP?=0
-WOLFBOOT_PARTITION_SIZE?=0xC0000
+WOLFBOOT_PARTITION_SIZE?=0xA0000
 WOLFBOOT_SECTOR_SIZE?=0x2000
-WOLFBOOT_PARTITION_BOOT_ADDRESS?=0x08040000
-WOLFBOOT_PARTITION_UPDATE_ADDRESS?=0x8140000
-WOLFBOOT_PARTITION_SWAP_ADDRESS?=0x08010000
+WOLFBOOT_PARTITION_BOOT_ADDRESS?=0x08060000
+WOLFBOOT_PARTITION_UPDATE_ADDRESS?=0x08100000
+WOLFBOOT_PARTITION_SWAP_ADDRESS?=0x081A0000
 FLAGS_HOME=0
 DISABLE_BACKUP=0
 IMAGE_HEADER_SIZE?=1024
+ARMORED=1
diff --git a/docs/Targets.md b/docs/Targets.md
index 599b7229ec..08d506102b 100644
--- a/docs/Targets.md
+++ b/docs/Targets.md
@@ -885,6 +885,7 @@ arm-none-eabi-gdb
 (gdb) target remote:3333
 ```
 
+
 ## STM32H5
 
 Like [STM32L5](#stm32l5) and [STM32U5](#stm32u5), STM32H5 support is also demonstrated
@@ -902,8 +903,8 @@ For more information, see [/docs/flash-OTP.md](/docs/flash-OTP.md).
 The implementation shows how to switch from secure application to non-secure application,
 thanks to the system isolation performed, which splits the internal Flash and internal
 SRAM memories into two parts:
- - the first 256KB are used by wolfboot running in secure mode and the secure application
- - the remaining available space is used for non-secure application and update partition
+ - the first 384KB are used by wolfboot running in secure mode and the secure application
+ - the remaining available space (640KB) is used for non-secure application and update partition
 
 The example configuration for this scenario is available in [/config/examples/stm32h5.config](/config/examples/stm32h5.config).
 
@@ -913,14 +914,14 @@ The example configuration for this scenario is available in [/config/examples/st
 
 `STM32_Programmer_CLI -c port=swd -ob TZEN=0xB4`
 
-- set the option bytes to enable flash secure protection of first 256KB:
-`STM32_Programmer_CLI -c port=swd -ob SECWM1_PSTRT=0x0 SECWM1_PEND=0x1F SECWM2_PSTRT=0x1F SECWM2_PEND=0x0`
+- set the option bytes to enable flash secure protection of first 384KB and remainder as non-secure:
+`STM32_Programmer_CLI -c port=swd -ob SECWM1_PSTRT=0x0 SECWM1_PEND=0x2F SECWM2_PSTRT=0x2F SECWM2_PEND=0x0`
 
 - flash the wolfboot image to the secure partition:
 `STM32_Programmer_CLI -c port=swd -d wolfboot.bin 0x0C000000`
 
 - flash the application image to the non-secure partition:
-`STM32_Programmer_CLI -c port=swd -d test-app/image_v1_signed.bin 0x08040000`
+`STM32_Programmer_CLI -c port=swd -d test-app/image_v1_signed.bin 0x08060000`
 
 For a full list of all the option bytes tested with this configuration, refer to [STM32-TZ.md](/docs/STM32-TZ.md).
 
@@ -946,35 +947,26 @@ For DUALBANK with TrustZone use `stm32h5-tz-dualbank-otp.config`.
 
 DUALBANK configuration (Tested on NUCLEO-STM32H563ZI):
 
+```
 BANK A: 0x08000000 to 0x080FFFFFF (1MB)
 BANK B: 0x08100000 to 0x081FFFFFF (1MB)
+```
 
 First of all, ensure that the `SWAP_BANK` option byte is off when running wolfBoot
 for the first time:
-
-```
-STM32_Programmer_CLI -c port=swd -ob SWAP_BANK=0
-```
+`STM32_Programmer_CLI -c port=swd -ob SWAP_BANK=0`
 
 It is a good idea to start with an empty flash, by erasing all sectors via:
+`STM32_Programmer_CLI -c port=swd -e 0 255`
 
-```
-STM32_Programmer_CLI -c port=swd -e 0 255
-```
 Compile wolfBoot with `make`. The file `factory.bin` contains both wolfboot and the
 version 1 of the application, and can be uploaded to the board at the beginning
 of the first bank using `STM32_Programmer_CLI` tool:
-
-```
-STM32_Programmer_CLI -c port=swd -d factory.bin 0x08000000
-```
+`STM32_Programmer_CLI -c port=swd -d factory.bin 0x08000000`
 
 Optionally, you can upload another copy of wolfboot.bin to the beginning of the second bank.
-Wolfboot should take care of copying itself to the second bank upon first boot if you don't.:
-
-```
-STM32_Programmer_CLI -c port=swd -d wolfboot.bin 0x08100000
-```
+Wolfboot should take care of copying itself to the second bank upon first boot if you don't:
+`STM32_Programmer_CLI -c port=swd -d wolfboot.bin 0x08100000`
 
 After uploading the images, reboot your board. The green LED should indicate that v1 of the
 test application is running.
@@ -982,14 +974,28 @@ test application is running.
 To initiate an update, sign a new version of the app and upload the v3 to the update partition
 on the second bank:
 
-```
+```sh
 tools/keytools/sign --ecc256 test-app/image.bin wolfboot_signing_private_key.der 3
-STM32_Programmer_CLI -c port=swd -d test-app/image_v3_signed.bin 0x08110000
+STM32_Programmer_CLI -c port=swd -d test-app/image_v3_signed.bin 0x08160000
 ```
 
 Reboot the board to initiate an update via DUALBANK hw-assisted swap.
 Any version except the first one will also turn on the orange LED.
 
+### STM32H5 Debugging
+
+
+OpenOCD: `openocd -s /usr/local/share/openocd/scripts -f board/st_nucleo_h5.cfg`
+
+```sh
+arm-none-eabi-gdb
+source .gdbinit
+add-symbol-file test-app/image.elf 0x08060000
+mon reset init
+b main
+c
+```
+
 
 ## STM32H7
 
diff --git a/docs/flash-OTP.md b/docs/flash-OTP.md
index 4b2bcca2f8..33a4e1ac2a 100644
--- a/docs/flash-OTP.md
+++ b/docs/flash-OTP.md
@@ -155,7 +155,7 @@ reboot : reboot the system
   - Sign a new version of the firmware: `./tools/keytools/sign --lms test-app/image.bin wolfboot_signing_private_key.der 2`
   - Run "update" command on the shell and wait for xmodem transfer
   - Use serial terminal that supports xmodem like "minicom" or "CoolTerm".
-    * Run `minicom` on `/dev/ttyACM0` and start file transfer using "CTRL+A; S"
+    * Run `minicom` on `/dev/ttyACM0` and start file transfer using "CTRL+A; S" -> `minicom -D /dev/ttyACM0 -b 115200`
     * Select xmodem then navigate to the new signed firmware file `test-app/image_v2_signed.bin`
   - During the transfer, the yellow LED will flash.
   - The green LED is dim because it's sync with the UART RX
diff --git a/hal/stm32_tz.c b/hal/stm32_tz.c
index 0ffcfb2bde..1152817c3a 100644
--- a/hal/stm32_tz.c
+++ b/hal/stm32_tz.c
@@ -213,31 +213,34 @@ void hal_tz_release_nonsecure_area(void)
 #define GTZC1_BASE             (0x50032400)
 #define GTZC1_TZSC             (*(volatile uint32_t *)(GTZC1_BASE + 0x00))
 #define GTZC1_TZIC             (*(volatile uint32_t *)(GTZC1_BASE + 0x0400))
-#define GTZC1_MPCBB1_S         ((volatile uint32_t *)(GTZC1_BASE + 0x0800 + 0x100))
-#define GTZC1_MPCBB2_S         ((volatile uint32_t *)(GTZC1_BASE + 0x0C00 + 0x100))
-#define GTZC1_MPCBB3_S         ((volatile uint32_t *)(GTZC1_BASE + 0x1000 + 0x100))
+#define GTZC1_MPCBB1_SECCFGR   ((volatile uint32_t *)(GTZC1_BASE + 0x0800 + 0x100))
+#define GTZC1_MPCBB2_SECCFGR   ((volatile uint32_t *)(GTZC1_BASE + 0x0C00 + 0x100))
+#define GTZC1_MPCBB3_SECCFGR   ((volatile uint32_t *)(GTZC1_BASE + 0x1000 + 0x100))
 
-#define SET_GTZC1_MPCBBx_S_VCTR(bank,n,val) \
-    (*((volatile uint32_t *)(GTZC1_MPCBB##bank##_S) + n ))= val
+#define SET_GTZC1_MPCBBx_SECCFGR_VCTR(bank,n,val) \
+    (*((volatile uint32_t *)(GTZC1_MPCBB##bank##_SECCFGR) + n )) = val
 
 void hal_gtzc_init(void)
 {
     int i;
-    /* One bit in the bitmask: 512B */
+    /* One bit in the bitmask: 512B
+     * 1: Secure access only to block
+     * 0: Non-secure access only to block
+     */
 
     /* Configure SRAM1 as secure (Low 256 KB) */
     for (i = 0; i < 16; i++) {
-        SET_GTZC1_MPCBBx_S_VCTR(1, i, 0xFFFFFFFF);
+        SET_GTZC1_MPCBBx_SECCFGR_VCTR(1, i, 0xFFFFFFFF);
     }
 
     /* Configure SRAM2 as secure (64 KB) */
     for (i = 0; i < 4; i++) {
-        SET_GTZC1_MPCBBx_S_VCTR(2, i, 0xFFFFFFFF);
+        SET_GTZC1_MPCBBx_SECCFGR_VCTR(2, i, 0xFFFFFFFF);
     }
 
     /* Configure SRAM3 as non-secure (320 KB) */
     for (i = 0; i < 20; i++) {
-        SET_GTZC1_MPCBBx_S_VCTR(3, i, 0x0);
+        SET_GTZC1_MPCBBx_SECCFGR_VCTR(3, i, 0x0);
     }
 }
 
@@ -288,7 +291,7 @@ void hal_tz_sau_init(void)
     /* SAU is set up before staging. Set up all areas as secure. */
 
     /* Non-secure callable: NSC functions area */
-    sau_init_region(0, 0x0C038000, 0x0C040000, 1);
+    sau_init_region(0, 0x0C040000, 0x0C05FFFF, 1);
 
     /* Secure: application flash area (first bank) */
     sau_init_region(1, WOLFBOOT_PARTITION_BOOT_ADDRESS, FLASH_BANK2_BASE - 1, 0);
diff --git a/hal/stm32h5.c b/hal/stm32h5.c
index 499bb17d8b..fb6cd3faa6 100644
--- a/hal/stm32h5.c
+++ b/hal/stm32h5.c
@@ -27,21 +27,23 @@
 #include "hal/stm32h5.h"
 #include "hal/armv8m_tz.h"
 
+#include "uart_drv.h"
+
 #define PLL_SRC_HSE 1
 
 #if TZ_SECURE()
 
 /* This function assumes that the boot and the update
  * partitions are at the same address in the two banks,
- * regardless wheather DUALBANK_SWAP is active or not.
+ * regardless if DUALBANK_SWAP is active or not.
  */
 static int is_flash_nonsecure(uint32_t address)
 {
-    uint32_t in_bank_offset = address & 0x000FFFFF;
-    if (in_bank_offset >= (WOLFBOOT_PARTITION_BOOT_ADDRESS - FLASHMEM_ADDRESS_SPACE))
+    uint32_t in_bank_offset = (address & 0x000FFFFF);
+    if (in_bank_offset >= (WOLFBOOT_PARTITION_BOOT_ADDRESS - FLASHMEM_ADDRESS_SPACE)) {
         return 1;
-    else
-        return 0;
+    }
+    return 0;
 }
 #endif
 
@@ -49,12 +51,22 @@ static int is_flash_nonsecure(uint32_t address)
 static void RAMFUNCTION flash_set_waitstates(unsigned int waitstates)
 {
     uint32_t reg = FLASH_ACR;
-    if ((reg & FLASH_ACR_LATENCY_MASK) < waitstates)
-        do {
-            FLASH_ACR =  (reg & ~(FLASH_ACR_LATENCY_MASK | (FLASH_ACR_WRHIGHFREQ_MASK << FLASH_ACR_WRHIGHFREQ_SHIFT))) |
-                         waitstates | (0x02 << FLASH_ACR_WRHIGHFREQ_SHIFT) ;
+    uint32_t wrhighfreq = 1; /* default flash signal delay */
+
+    if ((reg & FLASH_ACR_LATENCY_MASK) < waitstates) {
+        /* clear wrhighfreq and latency */
+        reg &= ~(FLASH_ACR_LATENCY_MASK |
+                (FLASH_ACR_WRHIGHFREQ_MASK << FLASH_ACR_WRHIGHFREQ_SHIFT));
+        if (waitstates > 3) { /* wait states 4 and 5 require = 2 */
+            wrhighfreq = 2;
         }
-        while ((FLASH_ACR & FLASH_ACR_LATENCY_MASK) != waitstates);
+        reg |= (waitstates | (wrhighfreq << FLASH_ACR_WRHIGHFREQ_SHIFT));
+        FLASH_ACR = reg;
+        ISB();
+        DMB();
+        /* wait for the register to be updated */
+        while (FLASH_ACR != reg);
+    }
 }
 
 void RAMFUNCTION hal_flash_wait_complete(uint8_t bank)
@@ -538,10 +550,15 @@ static void fork_bootloader(void)
 }
 #endif
 
-#include "uart_drv.h"
 void hal_init(void)
 {
     clock_pll_on();
+
+#ifdef DEBUG_UART
+    uart_init(115200, 8, 'N', 1);
+    uart_write("wolfBoot Init\n", 14);
+#endif
+
 #if TZ_SECURE()
     hal_gtzc_init();
     hal_tz_sau_init();
diff --git a/hal/stm32h5.h b/hal/stm32h5.h
index 4363c4fe7b..d6d38b07b2 100644
--- a/hal/stm32h5.h
+++ b/hal/stm32h5.h
@@ -22,6 +22,9 @@
 
 #ifndef STM32H5_DEF_INCLUDED
 #define STM32H5_DEF_INCLUDED
+
+#define PERIPH_CLOCK_FREQ (64000000)
+
 /* Assembly helpers */
 #ifndef DMB
 #define DMB() __asm__ volatile ("dmb")
@@ -343,6 +346,25 @@
 #define GPIOG_BASE 0x42021800
 #endif
 
+#define GPIOB_MODE  (*(volatile uint32_t *)(GPIOB_BASE + 0x00))
+#define GPIOB_OTYPE (*(volatile uint32_t *)(GPIOB_BASE + 0x04))
+#define GPIOB_OSPD  (*(volatile uint32_t *)(GPIOB_BASE + 0x08))
+#define GPIOB_PUPD  (*(volatile uint32_t *)(GPIOB_BASE + 0x0c))
+#define GPIOB_ODR   (*(volatile uint32_t *)(GPIOB_BASE + 0x14))
+#define GPIOB_BSRR  (*(volatile uint32_t *)(GPIOB_BASE + 0x18))
+#define GPIOB_AFL   (*(volatile uint32_t *)(GPIOB_BASE + 0x20))
+#define GPIOB_AFH   (*(volatile uint32_t *)(GPIOB_BASE + 0x24))
+
+#define GPIOD_MODE  (*(volatile uint32_t *)(GPIOD_BASE + 0x00))
+#define GPIOD_OTYPE (*(volatile uint32_t *)(GPIOD_BASE + 0x04))
+#define GPIOD_OSPD  (*(volatile uint32_t *)(GPIOD_BASE + 0x08))
+#define GPIOD_PUPD  (*(volatile uint32_t *)(GPIOD_BASE + 0x0c))
+#define GPIOD_ODR   (*(volatile uint32_t *)(GPIOD_BASE + 0x14))
+#define GPIOD_BSRR  (*(volatile uint32_t *)(GPIOD_BASE + 0x18))
+#define GPIOD_AFL   (*(volatile uint32_t *)(GPIOD_BASE + 0x20))
+#define GPIOD_AFH   (*(volatile uint32_t *)(GPIOD_BASE + 0x24))
+
+
 /* RCC AHB2 Clock Enable Register */
 #define RCC_AHB2_CLOCK_ER (*(volatile uint32_t *)(RCC_BASE + 0x8C ))
 #define GPIOA_AHB2_CLOCK_ER (1 << 0)
@@ -364,7 +386,13 @@
 #define RCC_APB1L_CLOCK_ER (*(volatile uint32_t *)(RCC_BASE + 0x9C))
 #define UART3_APB1L_CLOCK_ER_VAL (1 << 18)
 
-/* UART1 */
+#define RCC_AHB2ENR1_CLOCK_ER (*(volatile uint32_t *)(RCC_BASE + 0x8C ))
+
+#define GPIOB_AHB2ENR1_CLOCK_ER (1 << 1)
+#define GPIOD_AHB2ENR1_CLOCK_ER (1 << 3)
+
+
+/* UART */
 #if (TZ_SECURE())
 #define UART1 (0x54002400) /* Using LPUART1 */
 #define UART3 (0x50005800) /* Using USART3 */
@@ -373,25 +401,25 @@
 #define UART3 (0x40004800)
 #endif
 
-#define UART1_CR1      (*(volatile uint32_t *)(UART1 + 0x00))
-#define UART1_CR2      (*(volatile uint32_t *)(UART1 + 0x04))
-#define UART1_CR3      (*(volatile uint32_t *)(UART1 + 0x08))
-#define UART1_BRR      (*(volatile uint32_t *)(UART1 + 0x0c))
-#define UART1_ISR      (*(volatile uint32_t *)(UART1 + 0x1c))
-#define UART1_ICR      (*(volatile uint32_t *)(UART1 + 0x20))
-#define UART1_RDR      (*(volatile uint32_t *)(UART1 + 0x24))
-#define UART1_TDR      (*(volatile uint32_t *)(UART1 + 0x28))
-#define UART1_PRE      (*(volatile uint32_t *)(UART1 + 0x2C))
-
-#define UART3_CR1      (*(volatile uint32_t *)(UART3 + 0x00))
-#define UART3_CR2      (*(volatile uint32_t *)(UART3 + 0x04))
-#define UART3_CR3      (*(volatile uint32_t *)(UART3 + 0x08))
-#define UART3_BRR      (*(volatile uint32_t *)(UART3 + 0x0c))
-#define UART3_ISR      (*(volatile uint32_t *)(UART3 + 0x1c))
-#define UART3_ICR      (*(volatile uint32_t *)(UART3 + 0x20))
-#define UART3_RDR      (*(volatile uint32_t *)(UART3 + 0x24))
-#define UART3_TDR      (*(volatile uint32_t *)(UART3 + 0x28))
-#define UART3_PRE      (*(volatile uint32_t *)(UART3 + 0x2C))
+/* USE_UART1
+ * Set to 0 for VCP over USB
+ * Set to 1 for Arduino D0, D1 pins on nucleo
+ */
+#if defined(USE_UART1) && USE_UART1 == 1
+#define USE_UART UART1
+#else
+#define USE_UART UART3
+#endif
+
+#define UART_CR1(base) (*(volatile uint32_t *)((base) + 0x00))
+#define UART_CR2(base) (*(volatile uint32_t *)((base) + 0x04))
+#define UART_CR3(base) (*(volatile uint32_t *)((base) + 0x08))
+#define UART_BRR(base) (*(volatile uint32_t *)((base) + 0x0c))
+#define UART_ISR(base) (*(volatile uint32_t *)((base) + 0x1c))
+#define UART_ICR(base) (*(volatile uint32_t *)((base) + 0x20))
+#define UART_RDR(base) (*(volatile uint32_t *)((base) + 0x24))
+#define UART_TDR(base) (*(volatile uint32_t *)((base) + 0x28))
+#define UART_PRE(base) (*(volatile uint32_t *)((base) + 0x2C))
 
 #define UART_CR1_UART_ENABLE    (1 << 0)
 #define UART_CR1_OVER8          (1 << 15)
@@ -415,7 +443,6 @@
 #define UART_ORE                (1 << 3)    /* Overrun error */
 
 
-
 /* OTP FLASH AREA */
 #define FLASH_OTP_BASE 0x08FFF000
 #define FLASH_OTP_END  0x08FFF7FF
diff --git a/hal/stm32h5.ld b/hal/stm32h5.ld
index dca4119bb9..7807163818 100644
--- a/hal/stm32h5.ld
+++ b/hal/stm32h5.ld
@@ -1,11 +1,12 @@
 MEMORY
 {
-    FLASH (rx) :    ORIGIN = @WOLFBOOT_ORIGIN@, LENGTH = @BOOTLOADER_PARTITION_SIZE@ - 0x1C000
+    /* If FLASH_KEYVAULT or FLASH_NSC length is adjusted FLASH length needs adjusted too */
+    FLASH (rx) :    ORIGIN = @WOLFBOOT_ORIGIN@, LENGTH = @BOOTLOADER_PARTITION_SIZE@ - 0x20000
     RAM (rwx) :     ORIGIN = 0x30000000, LENGTH = 0x20000
     RAM_KV (rw):    ORIGIN = 0x30020000, LENGTH = 0x10000
     RAM_HEAP (rw):  ORIGIN = 0x30030000, LENGTH = 0x10000 /* 64KB Heap for wolfcrypt/PKCS11 */
-    FLASH_KEYVAULT(rw): ORIGIN = @WOLFBOOT_ORIGIN@ + 0x24000, LENGTH = 0x14000
-    FLASH_NSC(rx): ORIGIN = @WOLFBOOT_ORIGIN@ + 0x38000, LENGTH = 0x8000
+    FLASH_KEYVAULT(rw): ORIGIN = @WOLFBOOT_ORIGIN@ + LENGTH(FLASH), LENGTH = 0x1C000
+    FLASH_NSC(rx): ORIGIN = @WOLFBOOT_ORIGIN@ + LENGTH(FLASH) + LENGTH(FLASH_KEYVAULT), LENGTH = 0x4000
 }
 
 SECTIONS
@@ -28,11 +29,10 @@ SECTIONS
 
     .gnu.sgstubs :
     {
-        . += 0x400;
-        . = ALIGN(4);
+        . = ALIGN(32);
         *(.gnu.sgstubs*)   /* Secure Gateway stubs */
         . = ALIGN(4);
-    } >FLASH_NSC
+    } > FLASH_NSC
 
     _stored_data = .;
     .data : AT (_stored_data)
diff --git a/hal/uart/uart_drv_lpc.c b/hal/uart/uart_drv_lpc.c
index 7f05318923..644a35743d 100644
--- a/hal/uart/uart_drv_lpc.c
+++ b/hal/uart/uart_drv_lpc.c
@@ -113,4 +113,14 @@ int uart_init(uint32_t bitrate, uint8_t data, char parity, uint8_t stop)
     return 0;
 }
 
+#ifdef DEBUG_UART
+void uart_write(const char *buf, unsigned int len)
+{
+    while (len--) {
+        uart_tx(*buf);
+        buf++;
+    }
+}
+#endif
+
 #endif /* UART_FLASH && TARGET_lpc */
diff --git a/hal/uart/uart_drv_stm32f4.c b/hal/uart/uart_drv_stm32f4.c
index 758d0dcf91..b1b5ad2581 100644
--- a/hal/uart/uart_drv_stm32f4.c
+++ b/hal/uart/uart_drv_stm32f4.c
@@ -109,8 +109,8 @@ static void uart_pins_setup(void)
     GPIO_MODE = reg | (2 << (UART_TX_PIN * 2));
 
     /* The alternate function register is split across two 32bit
-     * registers (AFL, AFH). AFL covers pins 0 through 7, and 
-     * AFH covers pins 8 through 15. The code below determines 
+     * registers (AFL, AFH). AFL covers pins 0 through 7, and
+     * AFH covers pins 8 through 15. The code below determines
      * which register to use at compile time based on the chosen
      * pin number
     */
@@ -199,4 +199,14 @@ int uart_init(uint32_t bitrate, uint8_t data, char parity, uint8_t stop)
     return 0;
 }
 
+#ifdef DEBUG_UART
+void uart_write(const char *buf, unsigned int len)
+{
+    while (len--) {
+        uart_tx(*buf);
+        buf++;
+    }
+}
+#endif
+
 #endif /* TARGET_stm32f4 */
diff --git a/hal/uart/uart_drv_stm32h5.c b/hal/uart/uart_drv_stm32h5.c
index e80c006f9a..d36c1fc9cf 100644
--- a/hal/uart/uart_drv_stm32h5.c
+++ b/hal/uart/uart_drv_stm32h5.c
@@ -30,38 +30,8 @@
 #include <stdint.h>
 #include "hal/stm32h5.h"
 
-/* USE_UART1
- * Set to 0 for VCP over USB
- * Set to 1 for Arduino D0, D1 pins on nucleo
- * */
-#define USE_UART1 0
-
-#define RCC_AHB2ENR1_CLOCK_ER (*(volatile uint32_t *)(RCC_BASE + 0x8C ))
-#define GPIOB_AHB2ENR1_CLOCK_ER (1 << 1)
-#define GPIOD_AHB2ENR1_CLOCK_ER (1 << 3)
-
-
-#define GPIOB_MODE  (*(volatile uint32_t *)(GPIOB_BASE + 0x00))
-#define GPIOB_OTYPE (*(volatile uint32_t *)(GPIOB_BASE + 0x04))
-#define GPIOB_OSPD  (*(volatile uint32_t *)(GPIOB_BASE + 0x08))
-#define GPIOB_PUPD  (*(volatile uint32_t *)(GPIOB_BASE + 0x0c))
-#define GPIOB_ODR   (*(volatile uint32_t *)(GPIOB_BASE + 0x14))
-#define GPIOB_BSRR  (*(volatile uint32_t *)(GPIOB_BASE + 0x18))
-#define GPIOB_AFL   (*(volatile uint32_t *)(GPIOB_BASE + 0x20))
-#define GPIOB_AFH   (*(volatile uint32_t *)(GPIOB_BASE + 0x24))
-
-#define GPIOD_MODE  (*(volatile uint32_t *)(GPIOD_BASE + 0x00))
-#define GPIOD_OTYPE (*(volatile uint32_t *)(GPIOD_BASE + 0x04))
-#define GPIOD_OSPD  (*(volatile uint32_t *)(GPIOD_BASE + 0x08))
-#define GPIOD_PUPD  (*(volatile uint32_t *)(GPIOD_BASE + 0x0c))
-#define GPIOD_ODR   (*(volatile uint32_t *)(GPIOD_BASE + 0x14))
-#define GPIOD_BSRR  (*(volatile uint32_t *)(GPIOD_BASE + 0x18))
-#define GPIOD_AFL   (*(volatile uint32_t *)(GPIOD_BASE + 0x20))
-#define GPIOD_AFH   (*(volatile uint32_t *)(GPIOD_BASE + 0x24))
-
-#define CLOCK_FREQ (64000000)
-
-static void uart1_pins_setup(void)
+#if defined(USE_UART1) && USE_UART == 1
+static void uart_pins_setup(void)
 {
     uint32_t reg;
     RCC_AHB2ENR1_CLOCK_ER|= GPIOB_AHB2ENR1_CLOCK_ER;
@@ -78,8 +48,8 @@ static void uart1_pins_setup(void)
     GPIOB_AFH = reg | (UART1_PIN_AF << ((UART1_RX_PIN) * 4));
 
 }
-
-static void uart3_pins_setup(void)
+#else
+static void uart_pins_setup(void)
 {
     uint32_t reg;
     RCC_AHB2ENR1_CLOCK_ER|= GPIOD_AHB2ENR1_CLOCK_ER;
@@ -96,186 +66,103 @@ static void uart3_pins_setup(void)
     GPIOD_AFH = reg | (UART3_PIN_AF << ((UART3_RX_PIN - 8) * 4));
 
 }
+#endif
 
-static int uart1_init(uint32_t bitrate, uint8_t data, char parity, uint8_t stop)
+static int uart_base_init(uint32_t base, uint32_t bitrate, uint8_t data,
+    char parity, uint8_t stop)
 {
     uint32_t reg;
-    /* Enable pins and configure for AF */
-    uart1_pins_setup();
-
-    reg = RCC_CCIPR3 & (~ (RCC_CCIPR3_LPUART1SEL_MASK << RCC_CCIPR3_LPUART1SEL_SHIFT));
-    RCC_CCIPR3 = reg | (0 << RCC_CCIPR3_LPUART1SEL_SHIFT); /* PLL2 */
 
     /* Configure clock */
-    UART1_BRR |= (uint16_t)(CLOCK_FREQ / bitrate) + 1;
+    UART_BRR(base) = (uint16_t)(PERIPH_CLOCK_FREQ / bitrate) + 1;
 
     /* Configure data bits */
     if (data == 8)
-        UART1_CR1 &= ~UART_CR1_SYMBOL_LEN;
+        UART_CR1(base) &= ~UART_CR1_SYMBOL_LEN;
     else
-        UART1_CR1 |= UART_CR1_SYMBOL_LEN;
+        UART_CR1(base) |= UART_CR1_SYMBOL_LEN;
 
     /* Configure parity */
     switch (parity) {
         case 'O':
-            UART1_CR1 |= UART_CR1_PARITY_ODD;
+            UART_CR1(base) |= UART_CR1_PARITY_ODD;
             /* fall through to enable parity */
             /* FALL THROUGH */
         case 'E':
-            UART1_CR1 |= UART_CR1_PARITY_ENABLED;
+            UART_CR1(base) |= UART_CR1_PARITY_ENABLED;
             break;
         default:
-            UART1_CR1 &= ~(UART_CR1_PARITY_ENABLED | UART_CR1_PARITY_ODD);
+            UART_CR1(base) &= ~(UART_CR1_PARITY_ENABLED | UART_CR1_PARITY_ODD);
     }
     /* Set stop bits */
-    reg = UART1_CR2 & ~UART_CR2_STOPBITS;
+    reg = UART_CR2(base) & ~UART_CR2_STOPBITS;
     if (stop > 1)
-        UART1_CR2 = reg & (2 << 12);
+        UART_CR2(base) = reg & (2 << 12);
     else
-        UART1_CR2 = reg;
+        UART_CR2(base) = reg;
 
     /* Prescaler to DIV1 */
-    UART1_PRE |= 2;
+    UART_PRE(base) |= 2;
 
     /* Configure for RX+TX, turn on. */
-    UART1_CR1 |= UART_CR1_TX_ENABLE | UART_CR1_RX_ENABLE | UART_CR1_UART_ENABLE;
+    UART_CR1(base) |= UART_CR1_TX_ENABLE | UART_CR1_RX_ENABLE | UART_CR1_UART_ENABLE;
 
     return 0;
 }
 
-static void uart1_clear_errors(void)
+static void uart_clear_errors(uint32_t base)
 {
-    UART1_ICR = UART1_ISR & (UART_ENE | UART_EPE | UART_ORE | UART_EFE);
+    UART_ICR(base) = UART_ISR(base) & (UART_ENE | UART_EPE | UART_ORE | UART_EFE);
 }
 
-static int uart1_tx(const uint8_t c)
+int uart_tx(const uint8_t c)
 {
     volatile uint32_t reg;
     do {
-        reg = UART1_ISR;
+        reg = UART_ISR(USE_UART);
         if (reg & (UART_ENE | UART_EPE | UART_ORE | UART_EFE))
-            uart1_clear_errors();
+        uart_clear_errors(USE_UART);
     } while ((reg & UART_ISR_TX_EMPTY) == 0);
-    UART1_TDR = c;
+    UART_TDR(USE_UART) = c;
     return 1;
 }
 
-static int uart1_rx(uint8_t *c)
+int uart_rx(uint8_t *c)
 {
     volatile uint32_t reg;
     int i = 0;
-    reg = UART1_ISR;
+    reg = UART_ISR(USE_UART);
     if (reg & (UART_ENE | UART_EPE | UART_ORE | UART_EFE))
-        uart1_clear_errors();
+        uart_clear_errors(USE_UART);
     if (reg & UART_ISR_RX_NOTEMPTY) {
-        *c = (uint8_t)UART1_RDR;
+        *c = (uint8_t)UART_RDR(USE_UART);
         return 1;
     }
     return 0;
 }
 
-static int uart3_init(uint32_t bitrate, uint8_t data, char parity, uint8_t stop)
+int uart_init(uint32_t bitrate, uint8_t data, char parity, uint8_t stop)
 {
     uint32_t reg;
-    /* Enable pins and configure for AF */
-    uart3_pins_setup();
-
+    uart_pins_setup();
+#if defined(USE_UART1) && USE_UART == 1
+    reg = RCC_CCIPR3 & (~ (RCC_CCIPR3_LPUART1SEL_MASK << RCC_CCIPR3_LPUART1SEL_SHIFT));
+    RCC_CCIPR3 = reg | (0 << RCC_CCIPR3_LPUART1SEL_SHIFT); /* PLL2 */
+#else
     reg = RCC_CCIPR1 & (~ (RCC_CCIPR1_USART3SEL_MASK << RCC_CCIPR1_USART3SEL_SHIFT));
     RCC_CCIPR1 = reg | (0 << RCC_CCIPR1_USART3SEL_SHIFT); /* PLL2 */
-
-    /* Configure clock */
-    UART3_BRR = (uint16_t)(CLOCK_FREQ / bitrate) + 1;
-
-    /* Configure data bits */
-    if (data == 8)
-        UART3_CR1 &= ~UART_CR1_SYMBOL_LEN;
-    else
-        UART3_CR1 |= UART_CR1_SYMBOL_LEN;
-
-    /* Configure parity */
-    switch (parity) {
-        case 'O':
-            UART3_CR1 |= UART_CR1_PARITY_ODD;
-            /* fall through to enable parity */
-            /* FALL THROUGH */
-        case 'E':
-            UART3_CR1 |= UART_CR1_PARITY_ENABLED;
-            break;
-        default:
-            UART3_CR1 &= ~(UART_CR1_PARITY_ENABLED | UART_CR1_PARITY_ODD);
-    }
-    /* Set stop bits */
-    reg = UART3_CR2 & ~UART_CR2_STOPBITS;
-    if (stop > 1)
-        UART3_CR2 = reg & (2 << 12);
-    else
-        UART3_CR2 = reg;
-
-    /* Prescaler to DIV1 */
-    UART3_PRE |= 2;
-
-    /* Configure for RX+TX, turn on. */
-    UART3_CR1 |= UART_CR1_TX_ENABLE | UART_CR1_RX_ENABLE | UART_CR1_UART_ENABLE;
-
-    return 0;
-}
-
-static void uart3_clear_errors(void)
-{
-    UART3_ICR = UART3_ISR & (UART_ENE | UART_EPE | UART_ORE | UART_EFE);
-}
-
-static int uart3_tx(const uint8_t c)
-{
-    volatile uint32_t reg;
-    do {
-        reg = UART3_ISR;
-        if (reg & (UART_ENE | UART_EPE | UART_ORE | UART_EFE))
-            uart3_clear_errors();
-    } while ((reg & UART_ISR_TX_EMPTY) == 0);
-    UART3_TDR = c;
-    return 1;
-}
-
-static int uart3_rx(uint8_t *c)
-{
-    volatile uint32_t reg;
-    int i = 0;
-    reg = UART3_ISR;
-    if (reg & (UART_ENE | UART_EPE | UART_ORE | UART_EFE))
-        uart3_clear_errors();
-    if (reg & UART_ISR_RX_NOTEMPTY) {
-        *c = (uint8_t)UART3_RDR;
-        return 1;
-    }
-    return 0;
-}
-
-int uart_init(uint32_t bitrate, uint8_t data, char parity, uint8_t stop)
-{
-#if USE_UART1
-    return uart1_init(bitrate, data, parity, stop);
-#else
-    return uart3_init(bitrate, data, parity, stop);
 #endif
+    return uart_base_init(USE_UART, bitrate, data, parity, stop);
 }
 
-int uart_tx(const uint8_t c)
+#ifdef DEBUG_UART
+void uart_write(const char *buf, unsigned int len)
 {
-#if USE_UART1
-    return uart1_tx(c);
-#else
-    return uart3_tx(c);
-#endif
+    while (len--) {
+        uart_tx(*buf);
+        buf++;
+    }
 }
-
-int uart_rx(uint8_t *c)
-{
-#if USE_UART1
-    return uart1_rx(c);
-#else
-    return uart3_rx(c);
 #endif
-}
 
 #endif /* TARGET_stm32h5 */
diff --git a/hal/uart/uart_drv_stm32l0.c b/hal/uart/uart_drv_stm32l0.c
index 6629e563ed..a5d3cfa6b1 100644
--- a/hal/uart/uart_drv_stm32l0.c
+++ b/hal/uart/uart_drv_stm32l0.c
@@ -169,4 +169,14 @@ int uart_rx(uint8_t *c, int len)
     return 0;
 }
 
+#ifdef DEBUG_UART
+void uart_write(const char *buf, unsigned int len)
+{
+    while (len--) {
+        uart_tx(*buf);
+        buf++;
+    }
+}
+#endif
+
 #endif /* TARGET_stm32l0 */
diff --git a/hal/uart/uart_drv_stm32l5.c b/hal/uart/uart_drv_stm32l5.c
index c342997b29..34506cb294 100644
--- a/hal/uart/uart_drv_stm32l5.c
+++ b/hal/uart/uart_drv_stm32l5.c
@@ -158,4 +158,14 @@ int uart_rx(uint8_t *c, int len)
     return 0;
 }
 
+#ifdef DEBUG_UART
+void uart_write(const char *buf, unsigned int len)
+{
+    while (len--) {
+        uart_tx(*buf);
+        buf++;
+    }
+}
+#endif
+
 #endif /* TARGET_stm32l5 */
diff --git a/hal/uart/uart_drv_stm32wb.c b/hal/uart/uart_drv_stm32wb.c
index 9c4e80b00e..28d0481f13 100644
--- a/hal/uart/uart_drv_stm32wb.c
+++ b/hal/uart/uart_drv_stm32wb.c
@@ -145,4 +145,14 @@ int uart_init(uint32_t bitrate, uint8_t data, char parity, uint8_t stop)
     return 0;
 }
 
+#ifdef DEBUG_UART
+void uart_write(const char *buf, unsigned int len)
+{
+    while (len--) {
+        uart_tx(*buf);
+        buf++;
+    }
+}
+#endif
+
 #endif /* TARGET_stm32wb */
diff --git a/include/uart_drv.h b/include/uart_drv.h
index e525f31eb4..a7a81ec8c9 100644
--- a/include/uart_drv.h
+++ b/include/uart_drv.h
@@ -29,4 +29,8 @@ int uart_tx(const uint8_t c);
 int uart_rx(uint8_t *c);
 int uart_init(uint32_t bitrate, uint8_t data, char parity, uint8_t stop);
 
+#ifdef DEBUG_UART
+void uart_write(const char *buf, unsigned int len);
+#endif
+
 #endif /* H_HAL_UART_ */
diff --git a/src/libwolfboot.c b/src/libwolfboot.c
index 146271e400..d3c2d13c50 100644
--- a/src/libwolfboot.c
+++ b/src/libwolfboot.c
@@ -200,7 +200,7 @@ static uint8_t NVM_CACHE[NVM_CACHE_SIZE] XALIGNED(16);
 static int nvm_cached_sector = 0;
 static uint8_t get_base_offset(uint8_t *base, uintptr_t off)
 {
-    return *(base - off); /* ignore array bounds error */
+    return *(uint8_t*)((uintptr_t)base - off); /* ignore array bounds error */
 }
 
 void WEAKFUNCTION hal_cache_invalidate(void)
diff --git a/src/pkcs11_store.c b/src/pkcs11_store.c
index 5602f840f2..c15cf4aa65 100644
--- a/src/pkcs11_store.c
+++ b/src/pkcs11_store.c
@@ -38,7 +38,7 @@
 #endif
 
 #ifndef KEYVAULT_MAX_ITEMS
-    #define KEYVAULT_MAX_ITEMS 0x17 /* Total memory: 0x18000, 22 items + 2 sector overhead */
+    #define KEYVAULT_MAX_ITEMS 20 /* Total memory: 0x16000 (20 items) + 2 sector overhead = 0x18000 */
 #endif
 
 /* Internal errors from wolfPKCS11 */
@@ -55,24 +55,29 @@
 
 
 #ifndef UNIT_TEST
-extern uint32_t *_flash_keyvault; /* From linker script: origin of vault flash */
-static uint8_t *vault_base = (uint8_t *)&_flash_keyvault;
 
-/* Back-end for malloc, used by wolfPKCS11 */
+/* From linker script: origin and size of vault flash */
+extern uint32_t _flash_keyvault;
+extern uint32_t _flash_keyvault_size;
+
+#define vault_base ((uint8_t*)&_flash_keyvault)
+#define vault_size ((uint32_t)&_flash_keyvault_size)
+
 
+/* Back-end for malloc, used by wolfPKCS11 */
 extern unsigned int _start_heap; /* From linker script: heap memory */
 extern unsigned int _heap_size;  /* From linker script: heap limit */
 
 void * _sbrk(unsigned int incr)
 {
-    static unsigned char *heap = (unsigned char *)&_start_heap;
-    static uint32_t heapsize = (uintptr_t)&_heap_size;
+    static uint8_t *heap = NULL;
+    static uint32_t heapsize = (uint32_t)&_heap_size;
     void *old_heap = heap;
     if (((incr >> 2) << 2) != incr)
         incr = ((incr >> 2) + 1) << 2;
 
     if (heap == NULL) {
-        heap = (unsigned char *)&_start_heap;
+        heap = (uint8_t*)&_start_heap;
         old_heap = heap;
     } else
         heap += incr;
@@ -175,7 +180,6 @@ static void cache_commit(uint32_t offset)
 {
     hal_flash_unlock();
 
-
     /* Write backup sector first */
     hal_flash_erase((uintptr_t)BACKUP_SECTOR_ADDRESS, WOLFBOOT_SECTOR_SIZE);
     hal_flash_write((uintptr_t)BACKUP_SECTOR_ADDRESS, cached_sector, WOLFBOOT_SECTOR_SIZE);
@@ -229,7 +233,7 @@ static void delete_object(int32_t type, uint32_t tok_id, uint32_t obj_id)
     memcpy(cached_sector, vault_base, WOLFBOOT_SECTOR_SIZE);
 
     while ((uintptr_t)hdr < ((uintptr_t)cached_sector + WOLFBOOT_SECTOR_SIZE)) {
-        if ((hdr->token_id == tok_id) && (hdr->object_id == obj_id) && 
+        if ((hdr->token_id == tok_id) && (hdr->object_id == obj_id) &&
                 (hdr->type == type)) {
             hdr->token_id = PKCS11_INVALID_ID;
             hdr->object_id = PKCS11_INVALID_ID;
diff --git a/src/wc_callable.c b/src/wc_callable.c
index d4304bcb3a..a267f1c1d4 100644
--- a/src/wc_callable.c
+++ b/src/wc_callable.c
@@ -29,20 +29,18 @@
 #include "hal.h"
 #include <stdint.h>
 
+static WC_RNG wcs_rng;
+
 int __attribute__((cmse_nonsecure_entry))
 wcs_get_random(uint8_t *rand, uint32_t size)
 {
-    int ret;
-    WC_RNG wcs_rng;
-    wc_InitRng(&wcs_rng);
-    ret = wc_RNG_GenerateBlock(&wcs_rng, rand, size);
-    wc_FreeRng(&wcs_rng);
-    return ret;
+    return wc_RNG_GenerateBlock(&wcs_rng, rand, size);
 }
 
 void wcs_Init(void)
 {
     hal_trng_init();
+    wc_InitRng(&wcs_rng);
 }
 
 #endif
diff --git a/test-app/Makefile b/test-app/Makefile
index d447226320..a7a5b5a9f1 100644
--- a/test-app/Makefile
+++ b/test-app/Makefile
@@ -6,12 +6,12 @@
 TARGET?=none
 ARCH?=ARM
 MCUXPRESSO_CMSIS?=$(MCUXPRESSO)/CMSIS
-CFLAGS+=-I. -I..
 ifeq ($(TZEN),1)
   # wcs directory contains a user_settings.h, which will conflict with
   # the one in the include directory if the test app needs it (e.g. wolfHSM)
   CFLAGS+=-I./wcs
 endif
+CFLAGS+=-I. -I..
 DEBUG?=1
 DELTA_DATA_SIZE?=2000
 USE_GCC?=1
diff --git a/test-app/app_stm32h5.c b/test-app/app_stm32h5.c
index 08bd8b12f5..ed20152674 100644
--- a/test-app/app_stm32h5.c
+++ b/test-app/app_stm32h5.c
@@ -704,18 +704,18 @@ void isr_usart3(void)
 {
     volatile uint32_t reg;
     usr_led_on();
-    reg = UART3_ISR;
+    reg = UART_ISR(UART3);
     if (reg & UART_ISR_RX_NOTEMPTY) {
         if (uart_rx_bytes >= 1023)
-            reg = UART3_RDR;
+            reg = UART_RDR(UART3);
         else
-            uart_buf_rx[uart_rx_bytes++] = (unsigned char)(UART3_RDR & 0xFF);
+            uart_buf_rx[uart_rx_bytes++] = (unsigned char)(UART_RDR(UART3) & 0xFF);
     }
 }
 
 static int uart_rx_isr(unsigned char *c, int len)
 {
-    UART3_CR1 &= ~UART_ISR_RX_NOTEMPTY;
+    UART_CR1(UART3) &= ~UART_ISR_RX_NOTEMPTY;
     if (len > (uart_rx_bytes - uart_processed))
         len = (uart_rx_bytes - uart_processed);
     if (len > 0) {
@@ -727,7 +727,7 @@ static int uart_rx_isr(unsigned char *c, int len)
             usr_led_off();
         }
     }
-    UART3_CR1 |= UART_ISR_RX_NOTEMPTY;
+    UART_CR1(UART3) |= UART_ISR_RX_NOTEMPTY;
     return len;
 }
 
@@ -750,14 +750,12 @@ void main(void)
 
     app_version = wolfBoot_current_firmware_version();
 
-
     nvic_irq_setprio(NVIC_USART3_IRQN, 0);
     nvic_irq_enable(NVIC_USART3_IRQN);
 
     uart_init(115200, 8, 'N', 1);
-    UART3_CR1 |= UART_ISR_RX_NOTEMPTY;
-    UART3_CR3 |= UART_CR3_RXFTIE;
-
+    UART_CR1(UART3) |= UART_ISR_RX_NOTEMPTY;
+    UART_CR3(UART3) |= UART_CR3_RXFTIE;
 
     printf("========================\r\n");
     printf("STM32H5 wolfBoot demo Application\r\n");
@@ -768,7 +766,6 @@ void main(void)
 
     console_loop();
 
-
     while(1)
         ;
 
diff --git a/test-app/wcs/user_settings.h b/test-app/wcs/user_settings.h
index fb85bf6f86..f3796f33c7 100644
--- a/test-app/wcs/user_settings.h
+++ b/test-app/wcs/user_settings.h
@@ -136,7 +136,13 @@ extern int tolower(int c);
 #define BENCH_EMBEDDED
 
 #ifdef SECURE_PKCS11
-#define CUSTOM_RAND_GENERATE_BLOCK wcs_get_random
+
+static inline int wcs_cmse_get_random(unsigned char* output, int sz)
+{
+    return wcs_get_random(output, sz);
+}
+
+#define CUSTOM_RAND_GENERATE_BLOCK wcs_cmse_get_random
 #endif
 
 /* Disable VLAs */