diff --git a/.github/workflows/test-configs.yml b/.github/workflows/test-configs.yml
index 5a79597407..25a2c21dd2 100644
--- a/.github/workflows/test-configs.yml
+++ b/.github/workflows/test-configs.yml
@@ -320,6 +320,12 @@ jobs:
       arch: arm
       config-file: ./config/examples/stm32h5-tz.config
 
+  stm32h5_tz_dualbank_test:
+    uses: ./.github/workflows/test-build.yml
+    with:
+      arch: arm
+      config-file: ./config/examples/stm32h5-tz-dualbank.config
+
   stm32h5_tz_dualbank_otp_test:
     uses: ./.github/workflows/test-build.yml
     with:
diff --git a/config/examples/stm32h5-tz-dualbank.config b/config/examples/stm32h5-tz-dualbank.config
new file mode 100644
index 0000000000..3eaf2f16ad
--- /dev/null
+++ b/config/examples/stm32h5-tz-dualbank.config
@@ -0,0 +1,31 @@
+ARCH?=ARM
+TZEN?=1
+TARGET?=stm32h5
+SIGN?=ECC256
+HASH?=SHA256
+DEBUG?=0
+VTOR?=1
+CORTEX_M0?=0
+CORTEX_M33?=1
+NO_ASM?=0
+NO_MPU=1
+EXT_FLASH?=0
+SPI_FLASH?=0
+ALLOW_DOWNGRADE?=0
+NVM_FLASH_WRITEONCE?=1
+WOLFBOOT_VERSION?=1
+V?=0
+SPMATH?=1
+RAM_CODE?=1
+DUALBANK_SWAP?=1
+WOLFBOOT_PARTITION_SIZE?=0xA0000
+WOLFBOOT_SECTOR_SIZE?=0x2000
+WOLFBOOT_PARTITION_BOOT_ADDRESS?=0x08060000
+WOLFBOOT_PARTITION_UPDATE_ADDRESS?=0x0C160000
+WOLFBOOT_PARTITION_SWAP_ADDRESS?=0xFFFFFFFF
+FLAGS_HOME=0
+DISABLE_BACKUP=0
+WOLFCRYPT_TZ=1
+WOLFCRYPT_TZ_PKCS11=1
+ARMORED=1
+IMAGE_HEADER_SIZE?=1024
diff --git a/hal/stm32h5.c b/hal/stm32h5.c
index fe60a72bf9..aa83f51441 100644
--- a/hal/stm32h5.c
+++ b/hal/stm32h5.c
@@ -510,14 +510,30 @@ static void RAMFUNCTION stm32h5_reboot(void)
 void RAMFUNCTION hal_flash_dualbank_swap(void)
 {
     uint32_t cur_opts;
+#ifdef WOLFCRYPT_SECURE_MODE
+    uint8_t wolfboot_final_sector =
+        (WOLFBOOT_PARTITION_BOOT_ADDRESS - FLASHMEM_ADDRESS_SPACE) / WOLFBOOT_SECTOR_SIZE - 1;
+    uint8_t partition_final_sector =
+        wolfboot_final_sector + (WOLFBOOT_PARTITION_SIZE / WOLFBOOT_SECTOR_SIZE);
+#endif
     cur_opts = (FLASH_OPTSR_CUR & FLASH_OPTSR_SWAP_BANK) >> 31;
     hal_flash_clear_errors(0);
     hal_flash_unlock();
     hal_flash_opt_unlock();
-    if (cur_opts)
+    if (cur_opts) {
         FLASH_OPTSR_PRG &= ~(FLASH_OPTSR_SWAP_BANK);
-    else
+#ifdef WOLFCRYPT_SECURE_MODE
+        FLASH_SECWM1R_PRG = wolfboot_final_sector << FLASH_SECWM_END_SHIFT;
+        FLASH_SECWM2R_PRG = partition_final_sector << FLASH_SECWM_END_SHIFT;
+#endif
+    }
+    else {
         FLASH_OPTSR_PRG |= FLASH_OPTSR_SWAP_BANK;
+#ifdef WOLFCRYPT_SECURE_MODE
+        FLASH_SECWM1R_PRG = partition_final_sector << FLASH_SECWM_END_SHIFT;
+        FLASH_SECWM2R_PRG = wolfboot_final_sector << FLASH_SECWM_END_SHIFT;
+#endif
+    }
 
     FLASH_OPTCR |= FLASH_OPTCR_OPTSTRT;
     DMB();
diff --git a/hal/stm32h5.h b/hal/stm32h5.h
index d80add0736..88dbf11fd0 100644
--- a/hal/stm32h5.h
+++ b/hal/stm32h5.h
@@ -212,6 +212,14 @@
 #define FLASH_SECBB2       ((volatile uint32_t *)(FLASH_BASE + 0x1A0)) /* Array */
 #define FLASH_SECBB_NREGS  4    /* Array length for the two above */
 
+#define FLASH_SECWM1R_CUR  (*(volatile uint32_t *)(FLASH_BASE + 0x0E0))
+#define FLASH_SECWM1R_PRG  (*(volatile uint32_t *)(FLASH_BASE + 0x0E4))
+#define FLASH_SECWM2R_CUR  (*(volatile uint32_t *)(FLASH_BASE + 0x1E0))
+#define FLASH_SECWM2R_PRG  (*(volatile uint32_t *)(FLASH_BASE + 0x1E4))
+
+#define FLASH_SECWM_STRT_SHIFT 0
+#define FLASH_SECWM_END_SHIFT  16
+
 #define FLASH_NS_BASE          (0x40022000)   //RM0481 - Table 3
 #define FLASH_NS_KEYR        (*(volatile uint32_t *)(FLASH_NS_BASE + 0x08))
 #define FLASH_NS_OPTKEYR     (*(volatile uint32_t *)(FLASH_NS_BASE + 0x0C))
diff --git a/test-app/app_stm32h5.c b/test-app/app_stm32h5.c
index 7f1e4eb91e..bd3fa5e5e2 100644
--- a/test-app/app_stm32h5.c
+++ b/test-app/app_stm32h5.c
@@ -373,7 +373,6 @@ static int cmd_update_xmodem(const char *args)
                 if (dst_offset >= t_size) {
                     eot_expected = 1;
                 }
-                /*uart_tx(XACK);*/
             } else {
                 uart_tx(XNAK);
             }