diff --git a/options.mk b/options.mk
index 1ffd073a88..0beb5737e4 100644
--- a/options.mk
+++ b/options.mk
@@ -652,6 +652,7 @@ ifeq ($(WOLFCRYPT_TZ_PKCS11),1)
   CFLAGS+=-DCK_CALLABLE="__attribute__((cmse_nonsecure_entry))"
   CFLAGS+=-Ilib/wolfPKCS11
   CFLAGS+=-DWP11_HASH_PIN_COST=3
+  LDFLAGS+=--specs=nano.specs
   WOLFCRYPT_OBJS+=src/pkcs11_store.o
   WOLFCRYPT_OBJS+=src/pkcs11_callable.o
   WOLFCRYPT_OBJS+=./lib/wolfssl/wolfcrypt/src/pwdbased.o
diff --git a/src/pkcs11_store.c b/src/pkcs11_store.c
index 6c52c41546..f3ae1a4f46 100644
--- a/src/pkcs11_store.c
+++ b/src/pkcs11_store.c
@@ -66,19 +66,16 @@ 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);
+    static uint32_t heapsize = (uintptr_t)&_heap_size;
     void *old_heap = heap;
     if (((incr >> 2) << 2) != incr)
         incr = ((incr >> 2) + 1) << 2;
 
-    if (heap == NULL)
+    if (heap == NULL) {
         heap = (unsigned char *)&_start_heap;
-    else
+        old_heap = heap;
+    } else
         heap += incr;
-    if (((uint32_t)heap - (uint32_t)(&_start_heap)) > heapsize) {
-        heap -= incr;
-        return NULL;
-    }
     return old_heap;
 }
 #endif