diff --git a/hal/stm32h5.c b/hal/stm32h5.c
index e35f35ac..f69c3488 100644
--- a/hal/stm32h5.c
+++ b/hal/stm32h5.c
@@ -366,10 +366,23 @@ static void clock_pll_on(void)
 }
 
 #if (TZ_SECURE())
+
+#define NVIC_ISER_BASE (0xE000E100)
+#define NVIC_ICER_BASE (0xE000E180)
+#define NVIC_IPRI_BASE (0xE000E400)
+#define NVIC_USART3_IRQ 60
+
+/* Cortex M-33 has an extra register to set up non-secure interrupts */
+#define NVIC_ITNS_BASE (0xE000E380)
+
+
+
 static void periph_unsecure(void)
 {
     uint32_t pin;
     volatile uint32_t reg;
+    volatile uint32_t *nvic_itns;
+    uint32_t nvic_reg_pos, nvic_reg_off;
 
     /*Enable clock for User LED GPIOs */
     RCC_AHB2_CLOCK_ER|= LED_AHB2_ENABLE;
@@ -412,6 +425,13 @@ static void periph_unsecure(void)
         TZSC_SECCFGR1 = reg;
     }
 
+    /* Set USART3 interrupt as non-secure */
+    nvic_reg_pos = NVIC_USART3_IRQ / 32;
+    nvic_reg_off = NVIC_USART3_IRQ % 32;
+    nvic_itns = ((volatile uint32_t *)(NVIC_ITNS_BASE + 4 * nvic_reg_pos));
+    *nvic_itns |= (1 << nvic_reg_off);
+
+
     /* Disable GPIOs clock used previously for accessing SECCFGR registers */
 #if 0
     RCC_AHB2_CLOCK_ER &= ~GPIOA_AHB2_CLOCK_ER;
diff --git a/hal/stm32h5.h b/hal/stm32h5.h
index 17182045..c0176e41 100644
--- a/hal/stm32h5.h
+++ b/hal/stm32h5.h
@@ -23,7 +23,9 @@
 #ifndef STM32H5_DEF_INCLUDED
 #define STM32H5_DEF_INCLUDED
 /* Assembly helpers */
+#ifndef DMB
 #define DMB() __asm__ volatile ("dmb")
+#endif
 #define ISB() __asm__ volatile ("isb")
 #define DSB() __asm__ volatile ("dsb")
 
@@ -409,8 +411,14 @@
 #define UART_CR3_HDSEL          (1 << 3)
 #define UART_CR3_DEM            (1 << 14)
 #define UART_CR3_IREN           (1 << 1)
+#define UART_CR3_RXFTIE         (1 << 28)
 #define UART_ISR_TX_EMPTY       (1 << 7)
 #define UART_ISR_RX_NOTEMPTY    (1 << 5)
+#define UART_EPE                (1 << 0)    /* Parity error */
+#define UART_EFE                (1 << 1)    /* Framing error */
+#define UART_ENE                (1 << 2)    /* Noise error */
+#define UART_ORE                (1 << 3)    /* Overrun error */
+
 
 
 /* OTP FLASH AREA */
diff --git a/hal/uart/uart_drv_stm32h5.c b/hal/uart/uart_drv_stm32h5.c
index 453c1e49..e53bf0a2 100644
--- a/hal/uart/uart_drv_stm32h5.c
+++ b/hal/uart/uart_drv_stm32h5.c
@@ -141,11 +141,18 @@ static int uart1_init(uint32_t bitrate, uint8_t data, char parity, uint8_t stop)
     return 0;
 }
 
+static void uart1_clear_errors(void)
+{
+    UART1_ICR = UART1_ISR & (UART_ENE | UART_EPE | UART_ORE | UART_EFE);
+}
+
 static int uart1_tx(const uint8_t c)
 {
     volatile uint32_t reg;
     do {
         reg = UART1_ISR;
+        if (reg & (UART_ENE | UART_EPE | UART_ORE | UART_EFE))
+            uart1_clear_errors();
     } while ((reg & UART_ISR_TX_EMPTY) == 0);
     UART1_TDR = c;
     return 1;
@@ -156,6 +163,8 @@ static int uart1_rx(uint8_t *c)
     volatile uint32_t reg;
     int i = 0;
     reg = UART1_ISR;
+    if (reg & (UART_ENE | UART_EPE | UART_ORE | UART_EFE))
+        uart1_clear_errors();
     if (reg & UART_ISR_RX_NOTEMPTY) {
         *c = (uint8_t)UART1_RDR;
         return 1;
@@ -209,11 +218,18 @@ static int uart3_init(uint32_t bitrate, uint8_t data, char parity, uint8_t stop)
     return 0;
 }
 
+static void uart3_clear_errors(void)
+{
+    UART3_ICR = UART3_ISR & (UART_ENE | UART_EPE | UART_ORE | UART_EFE);
+}
+
 static int uart3_tx(const uint8_t c)
 {
     volatile uint32_t reg;
     do {
         reg = UART3_ISR;
+        if (reg & (UART_ENE | UART_EPE | UART_ORE | UART_EFE))
+            uart3_clear_errors();
     } while ((reg & UART_ISR_TX_EMPTY) == 0);
     UART3_TDR = c;
     return 1;
@@ -224,6 +240,8 @@ static int uart3_rx(uint8_t *c)
     volatile uint32_t reg;
     int i = 0;
     reg = UART3_ISR;
+    if (reg & (UART_ENE | UART_EPE | UART_ORE | UART_EFE))
+        uart3_clear_errors();
     if (reg & UART_ISR_RX_NOTEMPTY) {
         *c = (uint8_t)UART3_RDR;
         return 1;
diff --git a/test-app/app_stm32h5.c b/test-app/app_stm32h5.c
index 8b6ac289..c25d4ca2 100644
--- a/test-app/app_stm32h5.c
+++ b/test-app/app_stm32h5.c
@@ -28,6 +28,7 @@
 #include <sys/stat.h>
 #include "system.h"
 #include "hal.h"
+#include "hal/stm32h5.h"
 #include "uart_drv.h"
 #include "wolfboot/wolfboot.h"
 #include "wolfcrypt/benchmark/benchmark.h"
@@ -45,10 +46,19 @@ extern const CK_FUNCTION_LIST wolfpkcs11nsFunctionList;
 
 volatile unsigned int jiffies = 0;
 
+/* Usart irq-based read function */
+static uint8_t uart_buf_rx[256];
+static uint32_t uart_rx_bytes = 0;
+static uint32_t uart_processed = 0;
+static int uart_rx_isr(unsigned char *c, int len);
+static int uart_poll(void);
+
 #define LED_BOOT_PIN (4) /* PG4 - Nucleo - Red Led */
 #define LED_USR_PIN  (0) /* PB0 - Nucleo - Green Led */
 #define LED_EXTRA_PIN  (4) /* PF4 - Nucleo - Orange Led */
 
+#define NVIC_USART3_IRQN (60)
+
 /*Non-Secure */
 #define RCC_BASE            (0x44020C00)   /* RM0481 - Table 3 */
 #define GPIOG_BASE 0x42021800
@@ -176,6 +186,9 @@ static int cmd_random(const char *args);
 static int cmd_benchmark(const char *args);
 static int cmd_test(const char *args);
 static int cmd_timestamp(const char *args);
+static int cmd_update(const char *args);
+static int cmd_update_xmodem(const char *args);
+static int cmd_reboot(const char *args);
 
 
 
@@ -201,9 +214,165 @@ struct console_command COMMANDS[] =
      { cmd_timestamp, "timestamp", "print the current timestamp"},
      { cmd_benchmark, "benchmark", "run the wolfCrypt benchmark"},
      { cmd_test, "test", "run the wolfCrypt test"},
+     { cmd_update_xmodem, "update", "update the firmware via XMODEM"},
+     { cmd_reboot, "reboot", "reboot the system"},
      { NULL, "", ""}
 };
 
+#define AIRCR *(volatile uint32_t *)(0xE000ED0C)
+#define AIRCR_VKEY (0x05FA << 16)
+#   define AIRCR_SYSRESETREQ (1 << 2)
+
+int cmd_reboot(const char *args)
+{
+    (void)args;
+    AIRCR = AIRCR_SYSRESETREQ | AIRCR_VKEY;
+    while(1)
+        asm volatile("wfi");
+    return 0; /* Never happens */
+}
+
+#define XSOH 0x01
+#define XEOT 0x04
+#define XACK 0x06
+#define XNAK 0x15
+#define XCAN 0x18
+
+
+static uint8_t crc8(uint8_t *data, size_t len)
+{
+    uint8_t checksum = 0;
+    for (int i = 0; i < len; i++) {
+        checksum += data[i];
+    }
+    return checksum;
+}
+
+#define XMODEM_PAYLOAD_SIZE 128
+#define XMODEM_PACKET_SIZE (3 + XMODEM_PAYLOAD_SIZE + 1)
+#define XMODEM_TIMEOUT   1000 /* milliseconds */
+
+static void xcancel(void)
+{
+    int i;
+    for (i = 0; i < 10; i++)
+        uart_tx(XCAN);
+}
+
+static uint8_t xpkt_payload[XMODEM_PAYLOAD_SIZE];
+
+static int cmd_update_xmodem(const char *args)
+{
+    int ret = -1;
+    uint8_t xpkt[XMODEM_PACKET_SIZE];
+    uint32_t dst_flash = (uint32_t)WOLFBOOT_PARTITION_UPDATE_ADDRESS;
+    uint8_t pkt_num = 0, pkt_num_expected=0xFF;
+    uint32_t pkt_size = XMODEM_PACKET_SIZE;
+    uint32_t update_ver = 0;
+    uint32_t now = jiffies;
+    uint32_t i = 0;
+    uint8_t pkt_num_inv;
+    uint8_t crc, calc_crc;
+
+
+    printf("Erasing update partition...");
+    fflush(stdout);
+    hal_flash_unlock();
+    //hal_flash_erase(dst_flash, WOLFBOOT_PARTITION_SIZE);
+    printf("Done.\r\n");
+
+    printf("Waiting for XMODEM transfer...\r\n");
+
+    while (1) {
+        now = jiffies;
+        i = 0;
+
+        while(i < XMODEM_PACKET_SIZE) {
+            ret = uart_rx_isr(&xpkt[i], XMODEM_PACKET_SIZE - i);
+            if (ret == 0) {
+                if(jiffies > (now + XMODEM_TIMEOUT)) {
+                    now = jiffies;
+                    if (i == 0)
+                        uart_tx(XNAK);
+                    i = 0;
+                } else {
+                    asm volatile("wfi");
+                }
+            } else {
+                now = jiffies;
+                i += ret;
+            }
+        }
+
+        if (xpkt[0] == XEOT) {
+            ret = 0;
+            uart_tx(XACK);
+            extra_led_on();
+            break;
+        }
+        if (xpkt[0] != XSOH) {
+            continue;
+        }
+        pkt_num = xpkt[1];
+        pkt_num_inv = ~xpkt[2];
+        if (pkt_num == pkt_num_inv) {
+            if (pkt_num_expected == 0xFF) /* re-sync */
+                (pkt_num_expected = pkt_num);
+            else if (pkt_num_expected != pkt_num) {
+                uart_tx(XNAK);
+                continue;
+            }
+            if ((pkt_num / 0x10) & 0x01)
+                extra_led_on();
+            else
+                extra_led_off();
+
+            /* Packet is valid */
+            crc = xpkt[XMODEM_PACKET_SIZE - 1];
+            calc_crc = crc8(xpkt, XMODEM_PACKET_SIZE - 1);
+            if (crc == calc_crc) {
+                uint32_t t_size;
+                /* CRC is valid */
+                memcpy(xpkt_payload, xpkt + 3, XMODEM_PAYLOAD_SIZE);
+                ret = hal_flash_write(dst_flash, xpkt_payload, XMODEM_PAYLOAD_SIZE);
+                if (ret != 0) {
+                    xcancel();
+                    printf("Error writing to flash\r\n");
+                    break;
+                }
+                uart_tx(XACK);
+                pkt_num++;
+                pkt_num_expected++;
+                dst_flash += XMODEM_PAYLOAD_SIZE;
+                t_size = *((uint32_t *)(WOLFBOOT_PARTITION_UPDATE_ADDRESS + 4));
+                if ((uint32_t)dst_flash >= (WOLFBOOT_PARTITION_UPDATE_ADDRESS + t_size)) {
+                    ret = 0;
+                    extra_led_off();
+                    break;
+                }
+                uart_tx(XACK);
+            } else {
+                uart_tx(XNAK);
+            }
+        } else {
+            uart_tx(XNAK); /* invalid packet number received */
+        }
+    }
+    for (i = 0; i < 10; i++)
+        uart_tx('\r');
+
+    printf("End of transfer. ret: %d\r\n", ret);
+    update_ver = wolfBoot_update_firmware_version();
+    if (update_ver != 0) {
+        printf("New firmware version: 0x%lx\r\n", update_ver);
+    } else {
+        printf("No valid image in update partition\r\n");
+    }
+
+    hal_flash_lock();
+    return ret;
+}
+
 static int cmd_help(const char *args)
 {
     int i;
@@ -231,6 +400,12 @@ static int cmd_info(const char *args)
     printf("Firmware version : 0x%lx\r\n", wolfBoot_current_firmware_version());
     if (update_fw_version != 0) {
         printf("Candidate firmware version : 0x%lx\r\n", update_fw_version);
+        if (update_fw_version > cur_fw_version) {
+            wolfBoot_update_trigger();
+            printf("'reboot' to initiate update.\r\n");
+        } else {
+            printf("Update image older than current.\r\n");
+        }
     } else {
         printf("No image in update partition.\r\n");
     }
@@ -443,7 +618,7 @@ static void console_loop(void)
         fflush(stdout);
         idx = 0;
         do {
-            ret = uart_rx((uint8_t *)&c);
+            ret = uart_rx_isr((uint8_t *)&c, 1);
             if (ret > 0) {
                 if (c == '\r')
                     break;
@@ -453,20 +628,51 @@ static void console_loop(void)
         if (idx > 0) {
             cmd[idx] = 0;
             if (parse_cmd(cmd) == -2) {
-                printf("Unknown command\r\n");
+                printf("Unknown command: %s\r\n", cmd);
             }
         }
     }
 }
 
+void isr_usart3(void)
+{
+    volatile uint32_t reg;
+    usr_led_on();
+    reg = UART3_ISR;
+    if (reg & UART_ISR_RX_NOTEMPTY) {
+        if (uart_rx_bytes >= 255)
+            reg = UART3_RDR;
+        else
+            uart_buf_rx[uart_rx_bytes++] = (unsigned char)(UART3_RDR & 0xFF);
+    }
+}
+
+static int uart_rx_isr(unsigned char *c, int len)
+{
+    UART3_CR1 &= ~UART_ISR_RX_NOTEMPTY;
+    if (len > (uart_rx_bytes - uart_processed))
+        len = (uart_rx_bytes - uart_processed);
+    if (len > 0) {
+        memcpy(c, uart_buf_rx + uart_processed, len);
+        uart_processed += len;
+        if (uart_processed >= uart_rx_bytes) {
+            uart_processed = 0;
+            uart_rx_bytes = 0;
+            usr_led_off();
+        }
+    }
+    UART3_CR1 |= UART_ISR_RX_NOTEMPTY;
+    return len;
+}
+
+static int uart_poll(void)
+{
+    return (uart_rx_bytes > uart_processed)?1:0;
+}
 
 void main(void)
 {
     int ret;
-    uint32_t rand;
-    uint32_t i;
-    uint32_t klen = 200;
-    int otherkey_slot;
     uint32_t app_version;
 
 
@@ -478,7 +684,15 @@ void main(void)
 
     app_version = wolfBoot_current_firmware_version();
 
+
+    nvic_irq_setprio(NVIC_USART3_IRQN, 0);
+    nvic_irq_enable(NVIC_USART3_IRQN);
+
     uart_init(115200, 8, 'N', 1);
+    UART3_CR1 |= UART_ISR_RX_NOTEMPTY;
+    UART3_CR3 |= UART_CR3_RXFTIE;
+
+
     printf("========================\r\n");
     printf("STM32H5 wolfBoot demo Application\r\n");
     printf("Copyright 2024 wolfSSL Inc\r\n");
@@ -523,18 +737,7 @@ __attribute__((weak)) int _read(int file, char *ptr, int len)
   (void)file;
   int DataIdx;
   int ret;
-
-  for (DataIdx = 0; DataIdx < len; DataIdx++)
-  {
-      do {
-          ret = uart_rx((uint8_t *)ptr);
-          if (ret > 0)
-              ptr++;
-      } while (ret == 0);
-      if (ret == 0)
-          break;
-  }
-  return DataIdx;
+  return -1;
 }
 
 int _write(int file, char *ptr, int len)
diff --git a/test-app/startup_arm.c b/test-app/startup_arm.c
index 77dc92f1..ae33b461 100644
--- a/test-app/startup_arm.c
+++ b/test-app/startup_arm.c
@@ -33,6 +33,10 @@ extern unsigned int _start_heap;
 extern void isr_tim2(void);
 #endif
 
+#ifdef PLATFORM_stm32h5
+extern void isr_usart3(void);
+#endif
+
 #ifdef APP_HAS_SYSTICK
 extern void isr_systick(void);
 #endif
@@ -294,6 +298,139 @@ void (* const IV[])(void) =
     isr_empty, //	UCPD1_IRQHandler
     isr_empty, //	ICACHE_IRQHandler
     isr_empty, //	OTFDEC1_IRQHandler
+               //
+#elif defined(PLATFORM_stm32h5)
+    isr_empty, //	WWDG_IRQHandler
+    isr_empty, //	PVD_PVM_IRQHandler
+    isr_empty, //	RTC_IRQHandler
+    isr_empty, //	RTC_S_IRQHandler
+    isr_empty, //	TAMP_IRQHandler
+    isr_empty, //	RAMCFG_IRQHandler
+    isr_empty, //	FLASH_IRQHandler
+    isr_empty, //	FLASH_S_IRQHandler
+    isr_empty, //	GTZC_IRQHandler
+    isr_empty, //	RCC_IRQHandler
+    isr_empty, //	RCC_S_IRQHandler
+    isr_empty, //	EXTI0_IRQHandler
+    isr_empty, //	EXTI1_IRQHandler
+    isr_empty, //	EXTI2_IRQHandler
+    isr_empty, //	EXTI3_IRQHandler
+    isr_empty, //	EXTI4_IRQHandler
+    isr_empty, //	EXTI5_IRQHandler
+    isr_empty, //	EXTI6_IRQHandler
+    isr_empty, //	EXTI7_IRQHandler
+    isr_empty, //	EXTI8_IRQHandler
+    isr_empty, //	EXTI9_IRQHandler
+    isr_empty, //	EXTI10_IRQHandler
+    isr_empty, //	EXTI11_IRQHandler
+    isr_empty, //	EXTI12_IRQHandler
+    isr_empty, //	EXTI13_IRQHandler
+    isr_empty, //	EXTI14_IRQHandler
+    isr_empty, //	EXTI15_IRQHandler
+    isr_empty, //	GPDMA1CH0_IRQHandler
+    isr_empty, //	GPDMA1CH1_IRQHandler
+    isr_empty, //	GPDMA1CH2_IRQHandler
+    isr_empty, //	GPDMA1CH3_IRQHandler
+    isr_empty, //	GPDMA1CH4_IRQHandler
+    isr_empty, //	GPDMA1CH5_IRQHandler
+    isr_empty, //	GPDMA1CH6_IRQHandler
+    isr_empty, //	GPDMA1CH7_IRQHandler
+    isr_empty, //	IWDG_IRQHandler
+    isr_empty, //	SAES_IRQHandler
+    isr_empty, //	ADC1_IRQHandler
+    isr_empty, //	DAC1_IRQHandler
+    isr_empty, //	FDCAN1_IT0_IRQHandler
+    isr_empty, //	FDCAN1_IT1_IRQHandler
+    isr_empty, //	TIM1_BRK_IRQHandler
+    isr_empty, //	TIM1_UP_IRQHandler
+    isr_empty, //	TIM1_TRG_COM_IRQHandler
+    isr_empty, //	TIM1_CC_IRQHandler
+    isr_empty, //	TIM2_IRQHandler
+    isr_empty, //	TIM3_IRQHandler
+    isr_empty, //	TIM4_IRQHandler
+    isr_empty, //	TIM5_IRQHandler
+    isr_empty, //	TIM6_IRQHandler
+    isr_empty, //	TIM7_IRQHandler
+    isr_empty, //	I2C1_EV_IRQHandler
+    isr_empty, //	I2C1_ER_IRQHandler
+    isr_empty, //	I2C2_EV_IRQHandler
+    isr_empty, //	I2C2_ER_IRQHandler
+    isr_empty, //	SPI1_IRQHandler
+    isr_empty, //	SPI2_IRQHandler
+    isr_empty, //	SPI3_IRQHandler
+    isr_empty, //	USART1_IRQHandler
+    isr_empty, //	USART2_IRQHandler
+    isr_usart3, //	USART3_IRQHandler
+    isr_empty, //	UART4_IRQHandler
+    isr_empty, //	UART5_IRQHandler
+    isr_empty, //	LPUART1_IRQHandler
+    isr_empty, //	LPTIM1_IRQHandler
+    isr_empty, //	TIM8_BRK_IRQHandler
+    isr_empty, //	TIM8_UP_IRQHandler
+    isr_empty, //	TIM8_TRG_COM_IRQHandler
+    isr_empty, //	TIM8_CC_IRQHandler
+    isr_empty, //	ADC2_IRQHandler
+    isr_empty, //	LPTIM2_IRQHandler
+    isr_empty, //	TIM15_IRQHandler
+    isr_empty, //	TIM16_IRQHandler
+    isr_empty, //	TIM17_IRQHandler
+    isr_empty, //	USB_FS_IRQHandler
+    isr_empty, //	CRS_IRQHandler
+    isr_empty, //	UCPD1_IRQHandler
+    isr_empty, //	FMC_IRQHandler
+    isr_empty, //	OCTOSPI1_IRQHandler
+    isr_empty, //	SDMMC1_IRQHandler
+    isr_empty, //	I2C3_EV_IRQHandler
+    isr_empty, //	I2C3_ER_IRQHandler
+    isr_empty, //	SPI4_IRQHandler
+    isr_empty, //	SPI5_IRQHandler
+    isr_empty, //	SPI6_IRQHandler
+    isr_empty, //	USART6_IRQHandler
+    isr_empty, //	USART10_IRQHandler
+    isr_empty, //	USART11_IRQHandler
+    isr_empty, //	SAI1_IRQHandler
+    isr_empty, //	SAI2_IRQHandler
+    isr_empty, //   GPDMA2CH0_IRQHandler
+    isr_empty, //   GPDMA2CH1_IRQHandler
+    isr_empty, //   GPDMA2CH2_IRQHandler
+    isr_empty, //   GPDMA2CH3_IRQHandler
+    isr_empty, //   GPDMA2CH4_IRQHandler
+    isr_empty, //   GPDMA2CH5_IRQHandler
+    isr_empty, //   GPDMA2CH6_IRQHandler
+    isr_empty, //   GPDMA2CH7_IRQHandler
+    isr_empty, //	UART7_IRQHandler
+    isr_empty, //	UART8_IRQHandler
+    isr_empty, //	UART9_IRQHandler
+    isr_empty, //	UART12_IRQHandler
+    isr_empty, //	SDMMC2_IRQHandler
+    isr_empty, //   FPU_IRQHandler
+    isr_empty, //   ICACHE_IRQHandler
+    isr_empty, //   DCACHE_IRQHandler
+    isr_empty, //   ETH1_IRQHandler
+    isr_empty, //   DCMI_PSSI_IRQHandler
+    isr_empty, //   FDCAN2_IT0_IRQHandler
+    isr_empty, //   FDCAN2_IT1_IRQHandler
+    isr_empty, //   CORDIC_IRQHandler
+    isr_empty, //   FMAC_IRQHandler
+    isr_empty, //   DTS_IRQHandler
+    isr_empty, //   RNG_IRQHandler
+    isr_empty, //   OTFDEC1_IRQHandler
+    isr_empty, //   AES_IRQHandler
+    isr_empty, //   HASH_IRQHandler
+    isr_empty, //   PKA_IRQHandler
+    isr_empty, //   CEC_IRQHandler
+    isr_empty, //   TIM12_IRQHandler
+    isr_empty, //   TIM13_IRQHandler
+    isr_empty, //   TIM14_IRQHandler
+    isr_empty, //   I3C1_EV_IRQHandler
+    isr_empty, //   I3C1_ER_IRQHandler
+    isr_empty, //   I2C4_EV_IRQHandler
+    isr_empty, //   I2C4_ER_IRQHandler
+    isr_empty, //   LPTIM3_IRQHandler
+    isr_empty, //   LPTIM4_IRQHandler
+    isr_empty, //   LPTIM5_IRQHandler
+    isr_empty, //   LPTIM6_IRQHandler
+
 #elif defined(STM32) /* For STM32 */
     isr_empty,              // NVIC_WWDG_IRQ 0
     isr_empty,              // PVD_IRQ 1
diff --git a/test-app/system.h b/test-app/system.h
index 441b5691..e6876232 100644
--- a/test-app/system.h
+++ b/test-app/system.h
@@ -28,12 +28,13 @@
 #define PLL_FULL_MASK (0x7F037FFF)
 
 /* Assembly helpers */
+#ifndef DMB
 #define DMB() asm volatile ("dmb");
+#endif
 #define WFI() asm volatile ("wfi");
 
 /* Master clock setting */
 void clock_config(void);
-void flash_set_waitstates(void);
 
 
 /* NVIC */