diff --git a/hal/nrf5340.c b/hal/nrf5340.c
index c5ae4641..c4110c73 100644
--- a/hal/nrf5340.c
+++ b/hal/nrf5340.c
@@ -25,8 +25,13 @@
 
 #include "image.h"
 #include "string.h"
+#include "printf.h"
 #include "nrf5340.h"
 
+#ifdef TEST_FLASH
+static int test_flash(void);
+#endif
+
 #ifdef DEBUG_UART
 
 #ifndef UART_SEL
@@ -97,6 +102,7 @@ void uart_write(const char* buf, unsigned int sz)
 }
 #endif /* DEBUG_UART */
 
+/* Non-volatile memory controller - base address is 0x0 */
 static void RAMFUNCTION flash_wait_complete(void)
 {
     while (NVMC_READY == 0)
@@ -107,13 +113,17 @@ int RAMFUNCTION hal_flash_write(uint32_t address, const uint8_t *data, int len)
 {
     int i = 0;
     uint32_t *src, *dst;
-
+#ifdef DEBUG_FLASH
+    wolfBoot_printf("Internal Flash Write: addr 0x%x, len %d\n", address, len);
+#endif
     while (i < len) {
         if ((len - i > 3) && ((((address + i) & 0x03) == 0)  &&
                       ((((uint32_t)data) + i) & 0x03) == 0)) {
             src = (uint32_t *)data;
             dst = (uint32_t *)address;
+            /* set both secure and non-secure registers */
             NVMC_CONFIG = NVMC_CONFIG_WEN;
+            NVMC_CONFIGNS = NVMC_CONFIG_WEN;
             flash_wait_complete();
             dst[i >> 2] = src[i >> 2];
             flash_wait_complete();
@@ -125,7 +135,9 @@ int RAMFUNCTION hal_flash_write(uint32_t address, const uint8_t *data, int len)
             dst = (uint32_t *)(address - off);
             val = dst[i >> 2];
             vbytes[off] = data[i];
+            /* set both secure and non-secure registers */
             NVMC_CONFIG = NVMC_CONFIG_WEN;
+            NVMC_CONFIGNS = NVMC_CONFIG_WEN;
             flash_wait_complete();
             dst[i >> 2] = val;
             flash_wait_complete();
@@ -139,10 +151,15 @@ int RAMFUNCTION hal_flash_erase(uint32_t address, int len)
 {
     uint32_t end = address + len - 1;
     uint32_t p;
+#ifdef DEBUG_FLASH
+    wolfBoot_printf("Internal Flash Erase: addr 0x%x, len %d\n", address, len);
+#endif
     for (p = address; p <= end; p += FLASH_PAGE_SIZE) {
+        /* set both secure and non-secure registers */
         NVMC_CONFIG = NVMC_CONFIG_EEN;
+        NVMC_CONFIGNS = NVMC_CONFIG_EEN;
         flash_wait_complete();
-        NVMC_ERASEPAGE = p;
+        *(volatile uint32_t *)p = 0xFFFFFFFF;
         flash_wait_complete();
     }
     return 0;
@@ -182,13 +199,68 @@ void hal_init(void)
     uart_init();
     uart_write("wolfBoot HAL Init\n", 18);
 #endif
+
+#ifdef TEST_FLASH
+    if (test_flash() != 0) {
+        wolfBoot_printf("Internal flash Test Failed!\n");
+    }
+#endif
 }
 
 void hal_prepare_boot(void)
 {
-#if 0
-    CLOCK_HFCLKSTOP = 1;
+
+}
+
+/* Test for internal flash erase/write */
+/* Use TEST_EXT_FLASH to test external QSPI flash (see qspi_flash.c) */
+#ifdef TEST_FLASH
+
+#ifndef TEST_ADDRESS
+#define TEST_ADDRESS (0xFA000UL)
 #endif
+
+/* #define TEST_FLASH_READONLY */
+
+static int test_flash(void)
+{
+    int ret = 0;
+    uint32_t i, len;
+    uint8_t* pagePtr = (uint8_t*)TEST_ADDRESS;
+    static uint8_t pageData[WOLFBOOT_SECTOR_SIZE];
+
+    wolfBoot_printf("Internal flash test at 0x%x\n", TEST_ADDRESS);
+
+    /* Setup test data */
+    for (i=0; i<sizeof(pageData); i++) {
+        ((uint8_t*)pageData)[i] = (i & 0xff);
+    }
+
+#ifndef TEST_FLASH_READONLY
+    /* Erase sector */
+    hal_flash_unlock();
+    ret = hal_flash_erase(TEST_ADDRESS, WOLFBOOT_SECTOR_SIZE);
+    hal_flash_lock();
+    if (ret != 0) {
+        wolfBoot_printf("Erase Sector failed: Ret %d\n", ret);
+        return ret;
+    }
+
+    /* Write Page */
+    ret = hal_flash_write(TEST_ADDRESS, (uint8_t*)pageData, sizeof(pageData));
+    wolfBoot_printf("Write Page: Ret %d\n", ret);
+#endif /* !TEST_FLASH_READONLY */
+
+    /* Compare Page */
+    ret = memcmp((void*)TEST_ADDRESS, pageData, sizeof(pageData));
+    if (ret != 0) {
+        wolfBoot_printf("Check Data @ %d failed\n", ret);
+        return ret;
+    }
+
+    wolfBoot_printf("Internal Flash Test Passed\n");
+    return ret;
 }
+#endif /* TEST_FLASH */
 
 #endif /* TARGET_* */
diff --git a/hal/nrf5340.h b/hal/nrf5340.h
index 397bfc54..30ef8ad9 100644
--- a/hal/nrf5340.h
+++ b/hal/nrf5340.h
@@ -35,17 +35,23 @@ void sleep_us(unsigned int us);
 
 /* Non-volatile memory controller */
 #ifdef TZEN
-#define NVMC_BASE       (0x50039000)
+#define NVMC_BASE        (0x50039000)
 #else
-#define NVMC_BASE       (0x40039000)
+#define NVMC_BASE        (0x40039000)
 #endif
-#define NVMC_CONFIG     *((volatile uint32_t *)(NVMC_BASE + 0x504))
-#define NVMC_ERASEPAGE  *((volatile uint32_t *)(NVMC_BASE + 0x508))
-#define NVMC_READY      *((volatile uint32_t *)(NVMC_BASE + 0x400))
-#define NVMC_CONFIG_REN 0 /* read only access */
-#define NVMC_CONFIG_WEN 1 /* write enable */
-#define NVMC_CONFIG_EEN 2 /* erase enable */
-#define NVMC_CONFIG_PEE 3 /* partial erase enable */
+#define NVMC_READY       *((volatile uint32_t *)(NVMC_BASE + 0x400))
+#define NVMC_READYNEXT   *((volatile uint32_t *)(NVMC_BASE + 0x408))
+#define NVMC_CONFIG      *((volatile uint32_t *)(NVMC_BASE + 0x504))
+#define NVMC_CONFIGNS    *((volatile uint32_t *)(NVMC_BASE + 0x584))
+#define NVMC_WRITEUICRNS *((volatile uint32_t *)(NVMC_BASE + 0x588)) /* User information configuration registers (UICR) */
+
+#define NVMC_CONFIG_REN  0 /* read only access */
+#define NVMC_CONFIG_WEN  1 /* write enable */
+#define NVMC_CONFIG_EEN  2 /* erase enable */
+#define NVMC_CONFIG_PEE  3 /* partial erase enable - secure only */
+
+#define NVMC_WRITEUICRNS_SET 1
+#define NVMC_WRITEUICRNS_KEY 0xAFBE5A70
 
 #define FLASH_PAGE_SIZE (4096)
 
diff --git a/hal/spi/spi_drv_nrf5340.c b/hal/spi/spi_drv_nrf5340.c
index be0c0c80..7789997a 100644
--- a/hal/spi/spi_drv_nrf5340.c
+++ b/hal/spi/spi_drv_nrf5340.c
@@ -246,6 +246,7 @@ void spi_init(int polarity, int phase)
     /* make sure interrupts are disabled */
     QSPI_INTENCLR = 1; /* write "1" to disable READY interrupt */
 
+#ifdef DEBUG_QSPI
     /* Display QSPI config */
     reg = QSPI_IFCONFIG0;
     wolfBoot_printf(
@@ -255,9 +256,12 @@ void spi_init(int polarity, int phase)
         QSPI_CLK_FREQ_DIV+1,
         (reg & QSPI_IFCONFIG0_ADDRMODE_32BIT) ? 32 : 24,
         (reg & QSPI_IFCONFIG0_PPSIZE_512)     ? 512 : 256);
+#endif
 
     /* Activate QSPI */
+#ifdef DEBUG_QSPI
     wolfBoot_printf("QSPI Activate\n");
+#endif
     QSPI_EVENTS_READY = 0; /* clear events */
     QSPI_TASKS_ACTIVATE = 1;
     qspi_wait_ready();
diff --git a/src/qspi_flash.c b/src/qspi_flash.c
index 527076c2..f274dca2 100644
--- a/src/qspi_flash.c
+++ b/src/qspi_flash.c
@@ -154,7 +154,7 @@ static int qspi_flash_read_id(uint8_t* id, uint32_t idSz)
     ret = qspi_command_simple(QSPI_MODE_READ, READ_ID_CMD, data, 3);
 
 #ifdef DEBUG_QSPI
-    wolfBoot_printf("Flash ID (ret %d): 0x%02x 0x%02x 0x%02x\n",
+    wolfBoot_printf("QSPI Flash ID (ret %d): 0x%02x 0x%02x 0x%02x\n",
         ret, data[0], data[1], data[2]);
 #endif
 
@@ -177,7 +177,7 @@ static int qspi_write_enable(void)
     /* send write enable */
     ret = qspi_command_simple(QSPI_MODE_WRITE, WRITE_ENABLE_CMD, NULL, 0);
 #if defined(DEBUG_QSPI) && DEBUG_QSPI > 1
-    wolfBoot_printf("Write Enable: Ret %d\n", ret);
+    wolfBoot_printf("QSPI Write Enable: Ret %d\n", ret);
 #endif
 
     /* wait until write enabled and not busy */
@@ -191,13 +191,13 @@ static int qspi_write_enable(void)
     }
     if (timeout >= QSPI_FLASH_READY_TRIES) {
 #ifdef DEBUG_QSPI
-        wolfBoot_printf("Flash WE Timeout!\n");
+        wolfBoot_printf("QSPI Flash WE Timeout!\n");
 #endif
         return -1; /* timeout */
     }
 
 #if defined(DEBUG_QSPI) && DEBUG_QSPI > 1
-    wolfBoot_printf("Write Enabled: %s\n",
+    wolfBoot_printf("QSPI Write Enabled: %s\n",
         (status & FLASH_SR_WRITE_EN) ? "yes" : "no");
 #endif
 
@@ -208,7 +208,7 @@ static int qspi_write_disable(void)
 {
     int ret = qspi_command_simple(QSPI_MODE_WRITE, WRITE_DISABLE_CMD, NULL, 0);
 #if defined(DEBUG_QSPI) && DEBUG_QSPI > 1
-    wolfBoot_printf("Write Disable: Ret %d\n", ret);
+    wolfBoot_printf("QSPI Write Disable: Ret %d\n", ret);
 #endif
     return ret;
 }
@@ -225,7 +225,7 @@ static int qspi_status(uint8_t* status)
     ret = qspi_command_simple(QSPI_MODE_READ, READ_SR_CMD, data, 1);
 #if defined(DEBUG_QSPI) && DEBUG_QSPI > 1
     if (status == NULL || last_status != data[0]) {
-        wolfBoot_printf("Status (ret %d): %02x -> %02x\n",
+        wolfBoot_printf("QSPI Status (ret %d): %02x -> %02x\n",
             ret, last_status, data[0]);
     }
     last_status = data[0];
@@ -251,7 +251,7 @@ static int qspi_wait_ready(void)
     }
 
 #ifdef DEBUG_QSPI
-    wolfBoot_printf("Flash Ready Timeout!\n");
+    wolfBoot_printf("QSPI Flash Ready Timeout!\n");
 #endif
     return -1;
 }
@@ -304,7 +304,7 @@ static int qspi_enter_4byte_addr(void)
     if (ret == 0) {
         ret = qspi_command_simple(QSPI_MODE_WRITE, ENTER_4B_ADDR_MODE_CMD, NULL, 0);
 #ifdef DEBUG_QSPI
-        wolfBoot_printf("Enter 4-byte address mode: Ret %d\n", ret);
+        wolfBoot_printf("QSPI: Enter 4-byte address mode: Ret %d\n", ret);
 #endif
         if (ret == 0) {
             ret = qspi_wait_ready(); /* Wait for not busy */
@@ -319,7 +319,7 @@ static int qspi_exit_4byte_addr(void)
     if (ret == 0) {
         ret = qspi_command_simple(QSPI_MODE_WRITE, EXIT_4B_ADDR_MODE_CMD, NULL, 0);
 #ifdef DEBUG_QSPI
-        wolfBoot_printf("Enter 4-byte address mode: Ret %d\n", ret);
+        wolfBoot_printf("QSPI: Enter 4-byte address mode: Ret %d\n", ret);
 #endif
         if (ret == 0) {
             ret = qspi_wait_ready(); /* Wait for not busy */
@@ -344,7 +344,9 @@ uint16_t spi_flash_probe(void)
 #endif
 
 #ifdef TEST_EXT_FLASH
-    test_ext_flash();
+    if (test_ext_flash() != 0) {
+        wolfBoot_printf("QSPI flash test failed!\n");
+    }
 #endif
     return 0;
 }
@@ -366,7 +368,7 @@ int spi_flash_sector_erase(uint32_t address)
             NULL, 0, QSPI_DATA_MODE_NONE                   /* Data */
         );
 #ifdef DEBUG_QSPI
-        wolfBoot_printf("Flash Erase: Ret %d, Address 0x%x\n", ret, address);
+        wolfBoot_printf("QSPI Flash Erase: Ret %d, Address 0x%x\n", ret, address);
 #endif
         if (ret == 0) {
             ret = qspi_wait_ready(); /* Wait for not busy */
@@ -391,7 +393,7 @@ int spi_flash_read(uint32_t address, void *data, int len)
 
     if (address > FLASH_DEVICE_SIZE) {
 #ifdef DEBUG_QSPI
-        wolfBoot_printf("Flash Read: Invalid address (0x%x > 0x%x max)\n",
+        wolfBoot_printf("QSPI Flash Read: Invalid address (0x%x > 0x%x max)\n",
             address, FLASH_DEVICE_SIZE);
 #endif
         return -1;
@@ -406,7 +408,7 @@ int spi_flash_read(uint32_t address, void *data, int len)
     );
 
 #ifdef DEBUG_QSPI
-    wolfBoot_printf("Flash Read: Ret %d, Address 0x%x, Len %d, Cmd 0x%x\n",
+    wolfBoot_printf("QSPI Flash Read: Ret %d, Address 0x%x, Len %d, Cmd 0x%x\n",
         ret, address, len, FLASH_READ_CMD);
 #endif
 
@@ -440,7 +442,7 @@ int spi_flash_write(uint32_t address, const void *data, int len)
                 xferSz, QSPI_DATA_MODE                     /* Data */
             );
 #ifdef DEBUG_QSPI
-            wolfBoot_printf("Flash Write: Ret %d, Addr 0x%x, Len %d, Cmd 0x%x\n",
+            wolfBoot_printf("QSPI Flash Write: Ret %d, Addr 0x%x, Len %d, Cmd 0x%x\n",
                 ret, addr, xferSz, FLASH_WRITE_CMD);
 #endif
             if (ret != 0)
@@ -469,11 +471,12 @@ void spi_flash_release(void)
 #endif /* QSPI_FLASH || OCTOSPI_FLASH */
 
 
+/* Test for external QSPI flash */
 #ifdef TEST_EXT_FLASH
 
 #ifndef TEST_EXT_ADDRESS
     /* Start Address for test - 2MB */
-    #define TEST_EXT_ADDRESS (2 * 1024 * 1024)
+    #define TEST_EXT_ADDRESS (2UL * 1024UL * 1024UL)
 #endif
 
 static int test_ext_flash(void)
@@ -483,6 +486,8 @@ static int test_ext_flash(void)
     uint8_t pageData[FLASH_PAGE_SIZE];
     uint32_t wait = 0;
 
+    wolfBoot_printf("QSPI Flash Test at 0x%x\n", TEST_EXT_ADDRESS);
+
 #ifndef TEST_FLASH_READONLY
     /* Erase sector */
     ret = ext_flash_erase(TEST_EXT_ADDRESS, FLASH_SECTOR_SIZE);
@@ -501,7 +506,6 @@ static int test_ext_flash(void)
     ret = ext_flash_read(TEST_EXT_ADDRESS, pageData, sizeof(pageData));
     wolfBoot_printf("Page Read: Ret %d\n", ret);
 
-    wolfBoot_printf("Checking...\n");
     /* Check data */
     for (i=0; i<sizeof(pageData); i++) {
     #if defined(DEBUG_QSPI) && DEBUG_QSPI > 1
@@ -509,11 +513,11 @@ static int test_ext_flash(void)
     #endif
         if (pageData[i] != (i & 0xff)) {
             wolfBoot_printf("Check Data @ %d failed\n", i);
-            return -1;
+            return -i;
         }
     }
 
-    wolfBoot_printf("Flash Test Passed\n");
+    wolfBoot_printf("QSPI Flash Test Passed\n");
     return ret;
 }
 #endif /* TEST_EXT_FLASH */