diff --git a/.github/workflows/test-build.yml b/.github/workflows/test-build.yml
index 319d182e1..ff5835473 100644
--- a/.github/workflows/test-build.yml
+++ b/.github/workflows/test-build.yml
@@ -40,7 +40,7 @@ jobs:
 
       - name: Select config
         run: |
-          cp ${{inputs.config-file}} .config && make include/target.h
+          cp ${{inputs.config-file}} .config
 
       - name: Build tools
         run: |
diff --git a/Makefile b/Makefile
index d1dc6bc23..9a2a24341 100644
--- a/Makefile
+++ b/Makefile
@@ -218,15 +218,15 @@ wolfboot.elf: include/target.h $(LSCRIPT) $(OBJS) $(LIBS) $(BINASSEMBLE) FORCE
 	$(Q)$(LD) $(LDFLAGS) $(LSCRIPT_FLAGS) $(SECURE_LDFLAGS) $(LD_START_GROUP) $(OBJS) $(LIBS) $(LD_END_GROUP) -o $@
 
 $(LSCRIPT): $(LSCRIPT_IN) FORCE
-	@(test $(LSCRIPT_IN) != NONE) || (echo "Error: no linker script" \
+	$(Q)(test $(LSCRIPT_IN) != NONE) || (echo "Error: no linker script" \
 		"configuration found. If you selected Encryption and RAM_CODE, then maybe" \
 		"the encryption algorithm is not yet supported with bootloader updates." \
 		&& false)
-	@(test -r $(LSCRIPT_IN)) || (echo "Error: no RAM/ChaCha linker script found." \
+	$(Q)(test -r $(LSCRIPT_IN)) || (echo "Error: no RAM/ChaCha linker script found." \
 		"If you selected Encryption and RAM_CODE, ensure that you have a" \
 		"custom linker script (i.e. $(TARGET)_chacha_ram.ld). Please read " \
 		"docs/encrypted_partitions.md for more information" && false)
-	@cat $(LSCRIPT_IN) | \
+	$(Q)cat $(LSCRIPT_IN) | \
 		sed -e "s/@ARCH_FLASH_OFFSET@/$(ARCH_FLASH_OFFSET)/g" | \
 		sed -e "s/@BOOTLOADER_PARTITION_SIZE@/$(BOOTLOADER_PARTITION_SIZE)/g" | \
 		sed -e "s/@WOLFBOOT_ORIGIN@/$(WOLFBOOT_ORIGIN)/g" | \
diff --git a/arch.mk b/arch.mk
index 02dd276d2..b7914d79d 100644
--- a/arch.mk
+++ b/arch.mk
@@ -146,6 +146,9 @@ ifeq ($(ARCH),ARM)
     else
       WOLFBOOT_ORIGIN=0x08000000
     endif
+    ifneq ($(TZEN),1)
+      LSCRIPT_IN=hal/$(TARGET)-ns.ld
+    endif
   endif
 
   ifeq ($(TARGET),stm32u5)
diff --git a/config/examples/stm32l5-nonsecure-dualbank.config b/config/examples/stm32l5-nonsecure-dualbank.config
index 778962832..d367d95d7 100644
--- a/config/examples/stm32l5-nonsecure-dualbank.config
+++ b/config/examples/stm32l5-nonsecure-dualbank.config
@@ -3,7 +3,7 @@ TZEN?=0
 TARGET?=stm32l5
 SIGN?=ECC256
 HASH?=SHA256
-DEBUG?=1
+DEBUG?=0
 VTOR?=1
 CORTEX_M0?=0
 CORTEX_M33?=1
@@ -18,8 +18,8 @@ V?=0
 SPMATH?=1
 RAM_CODE?=0
 DUALBANK_SWAP?=1
-WOLFBOOT_PARTITION_SIZE?=0x38000
-WOLFBOOT_SECTOR_SIZE?=0x800
-WOLFBOOT_PARTITION_BOOT_ADDRESS?=0x08008000
-WOLFBOOT_PARTITION_UPDATE_ADDRESS?=0x08048000
+WOLFBOOT_PARTITION_SIZE?=0x30000
+WOLFBOOT_SECTOR_SIZE?=0x2000
+WOLFBOOT_PARTITION_BOOT_ADDRESS?=0x08010000
+WOLFBOOT_PARTITION_UPDATE_ADDRESS?=0x08110000
 WOLFBOOT_PARTITION_SWAP_ADDRESS?=0xFFFFFFFF
diff --git a/config/examples/stm32u5-nonsecure-dualbank.config b/config/examples/stm32u5-nonsecure-dualbank.config
index e1dc91401..aa7ffeda1 100644
--- a/config/examples/stm32u5-nonsecure-dualbank.config
+++ b/config/examples/stm32u5-nonsecure-dualbank.config
@@ -18,8 +18,8 @@ V?=0
 SPMATH?=1
 RAM_CODE?=0
 DUALBANK_SWAP?=1
-WOLFBOOT_PARTITION_SIZE?=0x38000
+WOLFBOOT_PARTITION_SIZE?=0x30000
 WOLFBOOT_SECTOR_SIZE?=0x2000
-WOLFBOOT_PARTITION_BOOT_ADDRESS?=0x08008000
-WOLFBOOT_PARTITION_UPDATE_ADDRESS?=0x08108000
+WOLFBOOT_PARTITION_BOOT_ADDRESS?=0x08010000
+WOLFBOOT_PARTITION_UPDATE_ADDRESS?=0x08110000
 WOLFBOOT_PARTITION_SWAP_ADDRESS?=0xFFFFFFFF
diff --git a/hal/stm32l5-ns.ld b/hal/stm32l5-ns.ld
new file mode 100644
index 000000000..ee4634d27
--- /dev/null
+++ b/hal/stm32l5-ns.ld
@@ -0,0 +1,50 @@
+MEMORY
+{
+    FLASH (rx) : ORIGIN = 0x08000000, LENGTH = @BOOTLOADER_PARTITION_SIZE@
+    RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 0x00020000 /* mapping TCM only */
+}
+
+SECTIONS
+{
+    .text :
+    {
+        _start_text = .;
+        KEEP(*(.isr_vector))
+        *(.text*)
+        *(.rodata*)
+        . = ALIGN(4);
+        _end_text = .;
+    } > FLASH
+
+    .edidx :
+    {
+        . = ALIGN(4);
+        *(.ARM.exidx*)
+    } > FLASH
+
+    _stored_data = .;
+    .data : AT (_stored_data)
+    {
+        _start_data = .;
+        KEEP(*(.data*))
+        . = ALIGN(4);
+        KEEP(*(.ramcode))
+        . = ALIGN(4);
+        _end_data = .;
+    } > RAM
+
+    .bss (NOLOAD) :
+    {
+        _start_bss = .;
+        __bss_start__ = .;
+        *(.bss*)
+        *(COMMON)
+        . = ALIGN(4);
+        _end_bss = .;
+        __bss_end__ = .;
+        _end = .;
+    } > RAM
+    . = ALIGN(4);
+}
+
+END_STACK = ORIGIN(RAM) + LENGTH(RAM);
diff --git a/hal/stm32l5.c b/hal/stm32l5.c
index 246034a6d..999ea2c63 100644
--- a/hal/stm32l5.c
+++ b/hal/stm32l5.c
@@ -97,8 +97,9 @@ int RAMFUNCTION hal_flash_write(uint32_t address, const uint8_t *data, int len)
         *cr &= ~FLASH_CR_PG;
         i+=8;
     }
-
+#if defined (__ARM_FEATURE_CMSE) && (__ARM_FEATURE_CMSE == 3U)
     hal_tz_release_nonsecure_area();
+#endif
     return 0;
 }
 
diff --git a/include/user_settings.h b/include/user_settings.h
index ec78d1f42..229995ded 100644
--- a/include/user_settings.h
+++ b/include/user_settings.h
@@ -53,6 +53,11 @@ extern int tolower(int c);
 #   define WOLFBOOT_TPM_PARMENC /* used in this file to gate features */
 #endif
 
+#ifdef WOLFCRYPT_SECURE_MODE
+    int hal_trng_get_entropy(unsigned char *out, unsigned len);
+    #define CUSTOM_RAND_GENERATE_SEED hal_trng_get_entropy
+#endif
+
 /* ED25519 and SHA512 */
 #ifdef WOLFBOOT_SIGN_ED25519
 #   define HAVE_ED25519
@@ -61,7 +66,6 @@ extern int tolower(int c);
 #   define NO_ED25519_EXPORT
 #   define WOLFSSL_SHA512
 #   define USE_SLOW_SHA512
-#   define NO_RSA
 #endif
 
 /* ED448 */
@@ -71,15 +75,15 @@ extern int tolower(int c);
 #   define ED448_SMALL
 #   define NO_ED448_SIGN
 #   define NO_ED448_EXPORT
-#   define NO_RSA
 #   define WOLFSSL_SHA3
 #   define WOLFSSL_SHAKE256
 #endif
 
-/* ECC and SHA256 */
-#if defined(WOLFBOOT_SIGN_ECC256) ||\
-    defined(WOLFBOOT_SIGN_ECC384) ||\
-    defined(WOLFBOOT_SIGN_ECC521)
+/* ECC */
+#if defined(WOLFBOOT_SIGN_ECC256) || \
+    defined(WOLFBOOT_SIGN_ECC384) || \
+    defined(WOLFBOOT_SIGN_ECC521) || \
+    defined(WOLFCRYPT_SECURE_MODE)
 
 #   define HAVE_ECC
 #   define ECC_TIMING_RESISTANT
@@ -93,136 +97,124 @@ extern int tolower(int c);
 #      define FREESCALE_LTC_TFM
 #   endif
 
-/* SP MATH */
-#   if !defined(USE_FAST_MATH) && !defined(WOLFSSL_SP_MATH_ALL)
+
+/* Some ECC options are disabled to reduce size */
+#   if !defined(WOLFCRYPT_SECURE_MODE)
+#       if !defined(WOLFBOOT_TPM)
+#          define NO_ECC_SIGN
+#          define NO_ECC_DHE
+#          define NO_ECC_EXPORT
+#          define NO_ECC_KEY_EXPORT
+#       else
+#           define HAVE_ECC_KEY_EXPORT
+#       endif
+#   else
+#       define HAVE_ECC_SIGN
+#       define HAVE_ECC_CDH
 #       define WOLFSSL_SP
 #       define WOLFSSL_SP_MATH
 #       define WOLFSSL_SP_SMALL
+#       define SP_WORD_SIZE 32
 #       define WOLFSSL_HAVE_SP_ECC
+#       define WOLFSSL_KEY_GEN
+#       define HAVE_ECC_KEY_EXPORT
 #   endif
 
+    /* SP MATH */
+#   if !defined(USE_FAST_MATH) && !defined(WOLFSSL_SP_MATH_ALL)
+#       define WOLFSSL_SP
+#       define WOLFSSL_SP_MATH
+#       define WOLFSSL_SP_SMALL
+#       define WOLFSSL_HAVE_SP_ECC
+#   endif
 
-/* ECC options disabled to reduce size */
-#ifndef WOLFCRYPT_SECURE_MODE
-#   define HAVE_ECC
-#   define NO_ECC_SIGN
-#   define NO_ECC_EXPORT
-#   define NO_ECC_KEY_EXPORT
-#   define NO_ASN
-#else
-#   define HAVE_ECC_SIGN
-//#   define HAVE_ECC_CDH
-#   define WOLFSSL_SP
-#   define WOLFSSL_SP_MATH
-#   define WOLFSSL_SP_SMALL
-#   define SP_WORD_SIZE 32
-#   define WOLFSSL_HAVE_SP_ECC
-//#   define WOLFSSL_SP_MATH_ALL
-#  define WOLFSSL_KEY_GEN
-#  define HAVE_ECC_KEY_EXPORT
-
-int hal_trng_get_entropy(unsigned char *out, unsigned len);
-#   define CUSTOM_RAND_GENERATE_SEED hal_trng_get_entropy
-#endif
 
 /* Curve */
-#ifdef WOLFBOOT_SIGN_ECC256
-#   define HAVE_ECC256
-#   define FP_MAX_BITS (256 + 32)
-#elif defined(WOLFBOOT_SIGN_ECC384)
-#   define HAVE_ECC384
-#   define FP_MAX_BITS (384 * 2)
-#   if !defined(USE_FAST_MATH) && !defined(WOLFSSL_SP_MATH_ALL)
-#       define WOLFSSL_SP_384
-#       define WOLFSSL_SP_NO_256
-#   endif
-#   if !defined(WOLFBOOT_TPM_PARMENC)
-#       define NO_ECC256
-#   endif
-#elif defined(WOLFBOOT_SIGN_ECC521)
-#   define HAVE_ECC521
-#   define FP_MAX_BITS (528 * 2)
-#   if !defined(USE_FAST_MATH) && !defined(WOLFSSL_SP_MATH_ALL)
-#       define WOLFSSL_SP_521
-#       define WOLFSSL_SP_NO_256
-#   endif
-#   if !defined(WOLFBOOT_TPM_PARMENC)
-#       define NO_ECC256
+#   ifdef WOLFBOOT_SIGN_ECC256
+#       define HAVE_ECC256
+#       define FP_MAX_BITS (256 + 32)
+#       if !defined(USE_FAST_MATH) && !defined(WOLFSSL_SP_MATH_ALL)
+#           define WOLFSSL_SP_NO_384
+#           define WOLFSSL_SP_NO_521
+#       endif
+#   elif defined(WOLFBOOT_SIGN_ECC384)
+#       define HAVE_ECC384
+#       define FP_MAX_BITS (384 * 2)
+#       if !defined(USE_FAST_MATH) && !defined(WOLFSSL_SP_MATH_ALL)
+#           define WOLFSSL_SP_384
+#           define WOLFSSL_SP_NO_256
+#       endif
+#       if !defined(WOLFBOOT_TPM_PARMENC)
+#           define NO_ECC256
+#       endif
+#   elif defined(WOLFBOOT_SIGN_ECC521)
+#       define HAVE_ECC521
+#       define FP_MAX_BITS (528 * 2)
+#       if !defined(USE_FAST_MATH) && !defined(WOLFSSL_SP_MATH_ALL)
+#           define WOLFSSL_SP_521
+#           define WOLFSSL_SP_NO_256
+#       endif
+#       if !defined(WOLFBOOT_TPM_PARMENC)
+#           define NO_ECC256
+#       endif
 #   endif
-#endif
-#   define NO_RSA
-
 #endif /* WOLFBOOT_SIGN_ECC521 || WOLFBOOT_SIGN_ECC384 || WOLFBOOT_SIGN_ECC256 */
 
-#ifdef WOLFBOOT_SIGN_RSA2048
+
+#if defined(WOLFBOOT_SIGN_RSA2048) || \
+    defined(WOLFBOOT_SIGN_RSA3072) || \
+    defined(WOLFBOOT_SIGN_RSA4096) || \
+    defined(WOLFCRYPT_SECURE_MODE)
+#   define WC_RSA_BLINDING 
+#   define WC_RSA_DIRECT
 #   define RSA_LOW_MEM
-#   ifndef WOLFBOOT_TPM
+#   define WC_ASN_HASH_SHA256
+#   if !defined(WOLFBOOT_TPM) && !defined(WOLFCRYPT_SECURE_MODE)
 #       define WOLFSSL_RSA_VERIFY_INLINE
 #       define WOLFSSL_RSA_VERIFY_ONLY
-#   endif
-#   if !defined(WOLFBOOT_TPM_PARMENC)
 #       define WC_NO_RSA_OAEP
 #   endif
-#   define FP_MAX_BITS (2048 * 2)
-    /* sp math */
 #   if !defined(USE_FAST_MATH) && !defined(WOLFSSL_SP_MATH_ALL)
 #       define WOLFSSL_HAVE_SP_RSA
 #       define WOLFSSL_SP
 #       define WOLFSSL_SP_SMALL
 #       define WOLFSSL_SP_MATH
+#   endif
+#   ifdef WOLFBOOT_SIGN_RSA2048
+#       define FP_MAX_BITS (2048 * 2)
 #       define WOLFSSL_SP_NO_3072
 #       define WOLFSSL_SP_NO_4096
+#       define WOLFSSL_SP_2048
 #   endif
-#   define WC_ASN_HASH_SHA256
-#endif
-
-#ifdef WOLFBOOT_SIGN_RSA3072
-#   define RSA_LOW_MEM
-#   define WOLFSSL_RSA_VERIFY_INLINE
-#   define WOLFSSL_RSA_VERIFY_ONLY
-#   define WC_NO_RSA_OAEP
-#   define FP_MAX_BITS (3072 * 2)
-    /* sp math */
-#   if !defined(USE_FAST_MATH) && !defined(WOLFSSL_SP_MATH_ALL)
-#       define WOLFSSL_HAVE_SP_RSA
-#       define WOLFSSL_SP
-#       define WOLFSSL_SP_SMALL
-#       define WOLFSSL_SP_MATH
+#   ifdef WOLFBOOT_SIGN_RSA3072
+#       define FP_MAX_BITS (3072 * 2)
 #       define WOLFSSL_SP_NO_2048
 #       define WOLFSSL_SP_NO_4096
+#       define WOLFSSL_SP_3072
 #   endif
-#   define WC_ASN_HASH_SHA256
-#endif
 
-#ifdef WOLFBOOT_SIGN_RSA4096
-#   define RSA_LOW_MEM
-#   define WOLFSSL_RSA_VERIFY_INLINE
-#   define WOLFSSL_RSA_VERIFY_ONLY
-#   define WC_NO_RSA_OAEP
-#   define FP_MAX_BITS (4096 * 2)
-    /* sp math */
-#   if !defined(USE_FAST_MATH) && !defined(WOLFSSL_SP_MATH_ALL)
-#       define WOLFSSL_HAVE_SP_RSA
-#       define WOLFSSL_SP
-#       define WOLFSSL_SP_SMALL
-#       define WOLFSSL_SP_MATH
-#       define WOLFSSL_SP_4096
+#   ifdef WOLFBOOT_SIGN_RSA4096
+#       define FP_MAX_BITS (4096 * 2)
 #       define WOLFSSL_SP_NO_2048
 #       define WOLFSSL_SP_NO_3072
+#       define WOLFSSL_SP_4096
 #   endif
-#   define WC_ASN_HASH_SHA256
-#endif
+#else
+#   define NO_RSA
+#endif /* RSA */
 
 #ifdef WOLFBOOT_HASH_SHA3_384
 #   define WOLFSSL_SHA3
-#   if defined(NO_RSA) && !defined(WOLFBOOT_TPM_PARMENC)
+#   if defined(NO_RSA) && !defined(WOLFBOOT_TPM) && \
+    !defined(WOLFCRYPT_SECURE_MODE)
 #       define NO_SHA256
 #   endif
 #endif
 
 #ifdef WOLFBOOT_HASH_SHA384
 #   define WOLFSSL_SHA384
-#   if defined(NO_RSA) && !defined(WOLFBOOT_TPM_PARMENC)
+#   if defined(NO_RSA) && !defined(WOLFBOOT_TPM) && \
+    !defined(WOLFCRYPT_SECURE_MODE)
 #       define NO_SHA256
 #   endif
 #endif
@@ -267,8 +259,6 @@ int hal_trng_get_entropy(unsigned char *out, unsigned len);
 # 	define HAVE_SCRYPT
 # 	define HAVE_AESGCM
 	typedef unsigned long time_t;
-#else
-#   define NO_HMAC
 #endif
 
 #ifndef HAVE_PWDBASED
@@ -297,7 +287,6 @@ int hal_trng_get_entropy(unsigned char *out, unsigned len);
         /* Configure RNG seed */
         #define CUSTOM_RAND_GENERATE_SEED(buf, sz) ({(void)buf; (void)sz; 0;}) /* stub, not used */
         #define WC_RNG_SEED_CB
-        #define HAVE_HASHDRBG
     #endif
 
     #ifdef WOLFTPM_MMIO
@@ -321,33 +310,39 @@ int hal_trng_get_entropy(unsigned char *out, unsigned len);
     #endif
 #endif
 
+#if !defined(WOLFCRYPT_SECURE_MODE) && !defined(WOLFBOOT_TPM_PARMENC)
+    #define WC_NO_RNG
+    #define WC_NO_HASHDRBG
+    #define NO_AES_CBC
+#else
+    #define HAVE_HASHDRBG
+    #define WOLFSSL_AES_CFB
+#endif
+
+
 #if !defined(ENCRYPT_WITH_AES128) && !defined(ENCRYPT_WITH_AES256) && \
     !defined(WOLFBOOT_TPM_PARMENC) && !defined(WOLFCRYPT_SECURE_MODE)
     #define NO_AES
 #endif
 
-#if !defined(WOLFBOOT_TPM_PARMENC) && !defined(WOLFCRYPT_SECURE_MODE)
-    #define NO_HMAC
-    #define WC_NO_RNG
-    #define WC_NO_HASHDRBG
-    #define NO_DEV_RANDOM
-    #define NO_ECC_KEY_EXPORT
-#endif
-
-/* Disables - For minimum wolfCrypt build */
-#ifndef WOLFBOOT_TPM
-#   if !defined(ENCRYPT_WITH_AES128) && !defined(ENCRYPT_WITH_AES256) && !defined(WOLFCRYPT_SECURE_MODE)
-#       define NO_AES
+#if !defined(WOLFBOOT_TPM) && !defined(WOLFCRYPT_SECURE_MODE)
+#   define NO_HMAC
+#   define WC_NO_RNG
+#   define WC_NO_HASHDRBG
+#   define NO_DEV_RANDOM
+#   define NO_ECC_KEY_EXPORT
+#   ifdef NO_RSA
+#       define NO_ASN
 #   endif
 #endif
 
 #define NO_CMAC
+#define NO_DH
 #define NO_CODING
 #define WOLFSSL_NO_PEM
 #define NO_ASN_TIME
 #define NO_RC4
 #define NO_SHA
-#define NO_DH
 #define NO_DSA
 #define NO_MD4
 #define NO_RABBIT
@@ -366,14 +361,6 @@ int hal_trng_get_entropy(unsigned char *out, unsigned len);
 #define WOLFSSL_IGNORE_FILE_WARN
 #define NO_ERROR_STRINGS
 
-#ifndef WOLFCRYPT_SECURE_MODE
-    #define WC_NO_RNG
-    #define WC_NO_HASHDRBG
-    #define NO_AES_CBC
-#else
-    #define HAVE_HASHDRBG
-#endif
-
 #define BENCH_EMBEDDED
 #define NO_CRYPT_TEST
 #define NO_CRYPT_BENCHMARK
@@ -396,7 +383,7 @@ int hal_trng_get_entropy(unsigned char *out, unsigned len);
 #       define WOLFSSL_SP_NO_MALLOC
 #       define WOLFSSL_SP_NO_DYN_STACK
 #   endif
-#   if !defined(ARCH_SIM) && !defined(SECURE_PKCS11)
+#   if !defined(ARCH_SIM) && !defined(WOLFCRYPT_SECURE_MODE)
 #       define WOLFSSL_NO_MALLOC
 #   endif
 #else
@@ -415,7 +402,7 @@ int hal_trng_get_entropy(unsigned char *out, unsigned len);
     #define XPRINTF uart_printf
 #endif
 
-#ifdef SECURE_PKCS11
+#ifdef WOLFCRYPT_SECURE_MODE
 typedef unsigned long time_t;
 #endif
 
diff --git a/options.mk b/options.mk
index ebda29bf2..73ab878c1 100644
--- a/options.mk
+++ b/options.mk
@@ -1,3 +1,5 @@
+WOLFCRYPT_OBJS+=./lib/wolfssl/wolfcrypt/src/asn.o
+
 ifeq ($(WOLFBOOT_TPM_VERIFY),1)
   WOLFTPM:=1
   CFLAGS+=-D"WOLFBOOT_TPM_VERIFY"
@@ -187,6 +189,9 @@ ifeq ($(SIGN),ED448)
     endif
   endif
 
+ifeq ($(SECURE_PKCS11),1)
+endif
+
 
   ifneq ($(HASH),SHA3)
     WOLFCRYPT_OBJS+=./lib/wolfssl/wolfcrypt/src/sha3.o
@@ -209,7 +214,6 @@ ifneq ($(findstring RSA2048,$(SIGN)),)
     $(RSA_EXTRA_OBJS) \
     $(MATH_OBJS) \
     ./lib/wolfssl/wolfcrypt/src/rsa.o \
-    ./lib/wolfssl/wolfcrypt/src/asn.o \
     ./lib/wolfssl/wolfcrypt/src/hash.o \
     ./lib/wolfssl/wolfcrypt/src/memory.o \
     ./lib/wolfssl/wolfcrypt/src/wolfmath.o \
@@ -249,7 +253,6 @@ ifneq ($(findstring RSA3072,$(SIGN)),)
     $(RSA_EXTRA_OBJS) \
     $(MATH_OBJS) \
     ./lib/wolfssl/wolfcrypt/src/rsa.o \
-    ./lib/wolfssl/wolfcrypt/src/asn.o \
     ./lib/wolfssl/wolfcrypt/src/hash.o \
     ./lib/wolfssl/wolfcrypt/src/memory.o \
     ./lib/wolfssl/wolfcrypt/src/wolfmath.o \
@@ -293,7 +296,6 @@ ifneq ($(findstring RSA4096,$(SIGN)),)
     $(RSA_EXTRA_OBJS) \
     $(MATH_OBJS) \
     ./lib/wolfssl/wolfcrypt/src/rsa.o \
-    ./lib/wolfssl/wolfcrypt/src/asn.o \
     ./lib/wolfssl/wolfcrypt/src/hash.o \
     ./lib/wolfssl/wolfcrypt/src/memory.o \
     ./lib/wolfssl/wolfcrypt/src/wolfmath.o \
@@ -379,11 +381,6 @@ ifeq ($(SIGN),LMS)
   endif
 endif
 
-
-ifeq ($(USE_GCC_HEADLESS),1)
-  CFLAGS+="-Wstack-usage=$(STACK_USAGE)"
-endif
-
 ifeq ($(RAM_CODE),1)
   CFLAGS+= -D"RAM_CODE"
 endif
@@ -544,12 +541,15 @@ ifeq ($(SECURE_PKCS11),1)
   OBJS+=src/pkcs11_store.o
   OBJS+=src/pkcs11_callable.o
   WOLFCRYPT_OBJS+=./lib/wolfssl/wolfcrypt/src/aes.o
+  WOLFCRYPT_OBJS+=./lib/wolfssl/wolfcrypt/src/rsa.o
   WOLFCRYPT_OBJS+=./lib/wolfssl/wolfcrypt/src/pwdbased.o
   WOLFCRYPT_OBJS+=./lib/wolfssl/wolfcrypt/src/hmac.o
+  WOLFCRYPT_OBJS+=./lib/wolfssl/wolfcrypt/src/dh.o
   WOLFCRYPT_OBJS+=./lib/wolfPKCS11/src/crypto.o \
 		./lib/wolfPKCS11/src/internal.o \
 		./lib/wolfPKCS11/src/slot.o \
 		./lib/wolfPKCS11/src/wolfpkcs11.o
+  STACK_USAGE=12596
 endif
 
 OBJS+=$(PUBLIC_KEY_OBJS)
@@ -677,6 +677,10 @@ endif
 
 CFLAGS+=$(CFLAGS_EXTRA)
 
+ifeq ($(USE_GCC_HEADLESS),1)
+  CFLAGS+="-Wstack-usage=$(STACK_USAGE)"
+endif
+
 ifeq ($(SIGN_ALG),)
   SIGN_ALG=$(SIGN)
 endif
diff --git a/tools/test.mk b/tools/test.mk
index df84bf521..5817e6812 100644
--- a/tools/test.mk
+++ b/tools/test.mk
@@ -74,6 +74,7 @@ $(BINASSEMBLE):
 test-size: FORCE
 	$(Q)make clean
 	$(Q)make wolfboot.bin
+	$(Q)$(CROSS_COMPILE)strip wolfboot.elf
 	$(Q)FP=`$(SIZE) -A wolfboot.elf | awk ' /Total/ {print $$2;}'`; echo SIZE: $$FP LIMIT: $$LIMIT; test $$FP -le $$LIMIT
 
 # Testbed actions
@@ -949,13 +950,13 @@ test-size-all:
 	make keysclean
 	make test-size SIGN=ECC256 NO_ASM=1 LIMIT=13706
 	make keysclean
-	make test-size SIGN=RSA2048 LIMIT=11186
+	make test-size SIGN=RSA2048 LIMIT=11226
 	make keysclean
-	make test-size SIGN=RSA2048 NO_ASM=1 LIMIT=11166
+	make test-size SIGN=RSA2048 NO_ASM=1 LIMIT=11202
 	make keysclean
-	make test-size SIGN=RSA4096 LIMIT=11550
+	make test-size SIGN=RSA4096 LIMIT=11586
 	make keysclean
-	make test-size SIGN=RSA4096 NO_ASM=1 LIMIT=11466
+	make test-size SIGN=RSA4096 NO_ASM=1 LIMIT=11502
 	make keysclean
 	make test-size SIGN=ECC384 LIMIT=17566
 	make keysclean
@@ -963,7 +964,7 @@ test-size-all:
 	make keysclean
 	make test-size SIGN=ED448 LIMIT=13414
 	make keysclean
-	make test-size SIGN=RSA3072 LIMIT=11386
+	make test-size SIGN=RSA3072 LIMIT=11422
 	make keysclean
-	make test-size SIGN=RSA3072 NO_ASM=1 LIMIT=11258
+	make test-size SIGN=RSA3072 NO_ASM=1 LIMIT=11294
 	make keysclean