diff --git a/hal/stm32h5.c b/hal/stm32h5.c
index 5127ff4da..672ef5417 100644
--- a/hal/stm32h5.c
+++ b/hal/stm32h5.c
@@ -216,7 +216,7 @@ int RAMFUNCTION hal_flash_erase(uint32_t address, int len)
         if ((FLASH_OPTSR_CUR & FLASH_OPTSR_SWAP_BANK) >> 31)
             bnksel = !bnksel;
 
-#if !TZ_SECURE() && !defined(__FLASH_OTP_PRIMER)
+#if !TZ_SECURE() && !defined(__FLASH_OTP_PRIMER) && defined(DEBUG)
         printf("Erasing bank %d, page %d\r\n", bnksel, (p - base) >> 13);
 #endif
 
diff --git a/test-app/ARM-stm32h5-ns.ld b/test-app/ARM-stm32h5-ns.ld
index cf53bb037..b8d713375 100644
--- a/test-app/ARM-stm32h5-ns.ld
+++ b/test-app/ARM-stm32h5-ns.ld
@@ -50,4 +50,5 @@ SECTIONS
 }
 
 PROVIDE(_start_heap = _end);
+PROVIDE(_heap_size = 4K);
 PROVIDE(_end_stack  = ORIGIN(RAM) + LENGTH(RAM));
diff --git a/test-app/ARM-stm32h5.ld b/test-app/ARM-stm32h5.ld
index 6e1091772..a225a39ab 100644
--- a/test-app/ARM-stm32h5.ld
+++ b/test-app/ARM-stm32h5.ld
@@ -50,4 +50,5 @@ SECTIONS
 }
 
 PROVIDE(_start_heap = _end);
+PROVIDE(_heap_size = 4K);
 PROVIDE(_end_stack  = ORIGIN(RAM) + LENGTH(RAM));
diff --git a/test-app/Makefile b/test-app/Makefile
index 7cbb11dcf..cff2fd28c 100644
--- a/test-app/Makefile
+++ b/test-app/Makefile
@@ -157,7 +157,11 @@ ifeq ($(TARGET),stm32h5)
   ifeq ($(TZEN),1)
     LSCRIPT_TEMPLATE=ARM-stm32h5-ns.ld
     APP_OBJS+=wcs/wolfcrypt_secure.o
-    APP_OBJS+=../lib/wolfssl/wolfcrypt/src/logging.o
+    ifeq ($(WOLFCRYPT_TZ),1)
+      APP_OBJS+=../lib/wolfssl/wolfcrypt/src/logging.o
+      APP_OBJS+=../lib/wolfssl/wolfcrypt/benchmark/benchmark.o
+      APP_OBJS+=../lib/wolfssl/wolfcrypt/test/test.o
+    endif
   else
     LSCRIPT_TEMPLATE=ARM-stm32h5.ld
   endif
@@ -173,8 +177,6 @@ ifeq ($(TARGET),stm32h5)
   else
     APP_OBJS+=../src/keystore.o
   endif
-  APP_OBJS+=../lib/wolfssl/wolfcrypt/benchmark/benchmark.o
-  APP_OBJS+=../lib/wolfssl/wolfcrypt/test/test.o
 endif
 
 ifeq ($(TARGET),stm32u5)
diff --git a/test-app/app_stm32h5.c b/test-app/app_stm32h5.c
index b9dff1514..91e431260 100644
--- a/test-app/app_stm32h5.c
+++ b/test-app/app_stm32h5.c
@@ -31,15 +31,15 @@
 #include "hal/stm32h5.h"
 #include "uart_drv.h"
 #include "wolfboot/wolfboot.h"
-#include "wolfcrypt/benchmark/benchmark.h"
-#include "wolfcrypt/test/test.h"
 #include "keystore.h"
 
 #ifdef SECURE_PKCS11
 #include "wcs/user_settings.h"
-#include <wolfssl/wolfcrypt/settings.h>
-#include <wolfssl/wolfcrypt/wc_pkcs11.h>
-#include <wolfssl/wolfcrypt/random.h>
+#include "wolfssl/wolfcrypt/settings.h"
+#include "wolfssl/wolfcrypt/wc_pkcs11.h"
+#include "wolfssl/wolfcrypt/random.h"
+#include "wolfcrypt/benchmark/benchmark.h"
+#include "wolfcrypt/test/test.h"
 extern const char pkcs11_library_name[];
 extern const CK_FUNCTION_LIST wolfpkcs11nsFunctionList;
 #endif
@@ -487,6 +487,7 @@ static int cmd_success(const char *args)
 
 static int cmd_random(const char *args)
 {
+#ifdef WOLFCRYPT_SECURE_MODE
     WC_RNG rng;
     int ret;
     uint32_t rand;
@@ -505,6 +506,9 @@ static int cmd_random(const char *args)
     printf("Today's lucky number: 0x%08lX\r\n", rand);
     printf("Brought to you by wolfCrypt's DRBG fed by HW TRNG in Secure world\r\n");
     wc_FreeRng(&rng);
+#else
+    printf("Feature only supported with WOLFCRYPT_TZ=1\n");
+#endif
     return 0;
 }
 
@@ -521,6 +525,7 @@ static int cmd_timestamp(const char *args)
 static int cmd_login_pkcs11(const char *args)
 {
     int ret = -1;
+#ifdef SECURE_PKCS11
     unsigned int devId = 0;
     Pkcs11Token token;
     Pkcs11Dev PKCS11_d;
@@ -535,7 +540,6 @@ static int cmd_login_pkcs11(const char *args)
         return 0;
     }
 
-#ifdef SECURE_PKCS11
     printf("PKCS11 Login\r\n");
 
     printf("Initializing wolfCrypt...");
@@ -608,26 +612,30 @@ static int cmd_login_pkcs11(const char *args)
         }
 #endif
     }
-
-#endif /* SECURE_PKCS11 */
     if (ret == 0) {
         printf("PKCS11 initialization completed successfully.\r\n");
         pkcs11_initialized = 1;
     }
+#else
+    printf("Feature only supported with WOLFCRYPT_TZ=1\n");
+#endif /* SECURE_PKCS11 */
     return ret;
 }
 
 static int cmd_benchmark(const char *args)
 {
-
+#ifdef WOLFCRYPT_SECURE_MODE
     benchmark_test(NULL);
+#endif
     return 0;
 }
 
 /* Test command */
 static int cmd_test(const char *args)
 {
+#ifdef WOLFCRYPT_SECURE_MODE
     wolfcrypt_test(NULL);
+#endif
     return 0;
 }
 
@@ -836,3 +844,27 @@ int _fstat(int file, struct stat *st)
   return 0;
 }
 
+#ifndef WOLFCRYPT_SECURE_MODE
+/* Back-end for malloc, used for token handling */
+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 = (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;
+    else
+        heap += incr;
+    if (((uint32_t)heap - (uint32_t)(&_start_heap)) > heapsize) {
+        heap -= incr;
+        return NULL;
+    }
+    return old_heap;
+}
+#endif
\ No newline at end of file
diff --git a/test-app/wcs/user_settings.h b/test-app/wcs/user_settings.h
index 9e9f0a5fb..9140f2b17 100644
--- a/test-app/wcs/user_settings.h
+++ b/test-app/wcs/user_settings.h
@@ -133,7 +133,9 @@ extern int tolower(int c);
 
 #define BENCH_EMBEDDED
 
+#ifdef SECURE_PKCS11
 #define CUSTOM_RAND_GENERATE_BLOCK wcs_get_random
+#endif
 
 /* Disable VLAs */
 #define WOLFSSL_SP_NO_DYN_STACK