From 09464ca0aaaa2b6dbad1bdf8dcb7dcc45383ff95 Mon Sep 17 00:00:00 2001
From: David Garske <david@wolfssl.com>
Date: Mon, 22 Jan 2024 17:35:06 -0800
Subject: [PATCH 01/19] API's to support OTP flash read/write on the STM32H7.
 Enabled with `FLASH_OTP_ROT`.

---
 hal/stm32h7.c | 135 ++++++++++++++++++++++++++++++++++++++++++++++++--
 1 file changed, 130 insertions(+), 5 deletions(-)

diff --git a/hal/stm32h7.c b/hal/stm32h7.c
index cbd9b99cc..1284a35d3 100644
--- a/hal/stm32h7.c
+++ b/hal/stm32h7.c
@@ -205,17 +205,21 @@
 /*** QSPI ***/
 /* See hal/spi/spi_drv_stm32.c */
 
+
 /*** FLASH ***/
 #define SYSCFG_APB4_CLOCK_ER_VAL    (1 << 0) /* RM0433 - 7.7.48 - RCC_APB4ENR - SYSCFGEN */
 
 #define FLASH_BASE          (0x52002000)   /* RM0433 - Table 8 */
 #define FLASH_ACR           (*(volatile uint32_t *)(FLASH_BASE + 0x00)) /* RM0433 - 3.9.1 - FLASH_ACR */
-#define FLASH_OPTSR_CUR     (*(volatile uint32_t *)(FLASH_BASE + 0x1C))
+
+#define FLASH_OPTKEYR       (*(volatile uint32_t *)(FLASH_BASE + 0x08)) /* FLASH option key register */
+#define FLASH_OPTCR         (*(volatile uint32_t *)(FLASH_BASE + 0x18)) /* FLASH option control register */
+#define FLASH_OPTSR_CUR     (*(volatile uint32_t *)(FLASH_BASE + 0x1C)) /* FLASH option status register */
 
 /* Bank 1 */
 #define FLASH_KEYR1         (*(volatile uint32_t *)(FLASH_BASE + 0x04)) /* RM0433 - 3.9.2 - FLASH_KEYR 1 */
-#define FLASH_SR1           (*(volatile uint32_t *)(FLASH_BASE + 0x10)) /* RM0433 - 3.9.5 - FLASH_SR 1 */
 #define FLASH_CR1           (*(volatile uint32_t *)(FLASH_BASE + 0x0C)) /* RM0433 - 3.9.4 - FLASH_CR 1 */
+#define FLASH_SR1           (*(volatile uint32_t *)(FLASH_BASE + 0x10)) /* RM0433 - 3.9.5 - FLASH_SR 1 */
 
 /* Bank 2 */
 #define FLASH_KEYR2         (*(volatile uint32_t *)(FLASH_BASE + 0x104)) /* RM0433 - 3.9.24 - FLASH_KEYR 2 */
@@ -255,12 +259,38 @@
 
 #define FLASH_OPTSR_CUR_BSY                 (1 << 0)
 
+#define FLASH_OPTCR_OPTLOCK                 (1 << 0)  /* lock option configuration bit */
+#define FLASH_OPTCR_OPTSTART                (1 << 1)  /* Option byte start change option configuration bit */
+#define FLASH_OPTCR_MER                     (1 << 4)  /* Mass erase request */
+#define FLASH_OPTCR_PG_OTP                  (1 << 5)  /* OTP program control bit */
+#define FLASH_OPTCR_OPTCHANGEERRIE          (1 << 30) /* Option byte change error interrupt enable bit */
+#define FLASH_OPTCR_SWAP_BANK               (1 << 31) /* Bank swapping option configuration bit */
+
 #define FLASH_CR_SNB_SHIFT                  8     /* SNB bits 10:8 */
 #define FLASH_CR_SNB_MASK                   0x7   /* SNB bits 10:8 - 3 bits */
 
-#define FLASH_KEY1                          (0x45670123)
-#define FLASH_KEY2                          (0xCDEF89AB)
-
+#define FLASH_KEY1                          (0x45670123U)
+#define FLASH_KEY2                          (0xCDEF89ABU)
+
+#define FLASH_OPT_KEY1                      (0x08192A3BU)
+#define FLASH_OPT_KEY2                      (0x4C5D6E7FU)
+
+#ifdef FLASH_OTP_ROT
+    #ifndef FLASH_OTP_BASE
+    #define FLASH_OTP_BASE 0x08FFF000
+    #endif
+    #ifndef FLASH_OTP_END
+    #define FLASH_OTP_END  0x08FFF3FF
+    #endif
+    #ifndef OTP_SIZE
+    #define OTP_SIZE       1024
+    #endif
+    #ifndef OTP_BLOCKS
+    #define OTP_BLOCKS     16
+    #endif
+
+    #define OTP_BLOCK_SIZE (OTP_SIZE / OTP_BLOCKS) /* 64 bytes */
+#endif
 
 /* STM32H7: Due to ECC functionality, it is not possible to write partition/sector
  * flags and signature more than once. This flags_cache is used to intercept write operations and
@@ -777,3 +807,98 @@ void hal_prepare_boot(void)
 #endif
     clock_pll_off();
 }
+
+#ifdef FLASH_OTP_ROT
+
+static void hal_flash_wait_otp(void)
+{
+    /* Wait for the FLASH operation to complete by polling on QW flag to be reset. */
+    while ( (FLASH_SR1 & FLASH_SR_QW) == FLASH_SR_QW ) {
+        /* TODO: check timeout */
+    }
+
+    /* Check FLASH End of Operation flag */
+    if ( (FLASH_SR1 & FLASH_SR_EOP) == FLASH_SR_EOP ) {
+        FLASH_SR1 &= FLASH_SR_EOP; /* Clear FLASH End of Operation pending bit */
+    }
+}
+
+static void hal_flash_otp_unlock(void)
+{
+    if ((FLASH_OPTCR & FLASH_OPTCR_OPTLOCK) != 0U) {
+        FLASH_OPTKEYR = FLASH_OPT_KEY1;
+        FLASH_OPTKEYR = FLASH_OPT_KEY2;
+    }
+}
+
+static void hal_flash_otp_lock(void)
+{
+    /* Set the OPTLOCK Bit to lock the FLASH Option Byte Registers access */
+    FLASH_OPTCR |= FLASH_OPTCR_OPTLOCK;
+}
+
+int hal_flash_otp_write(uint32_t flashAddress, uint16_t* data, uint16_t length)
+{
+    volatile uint16_t tmp;
+    uint16_t idx = 0;
+    if (!(flashAddress >= FLASH_OTP_BASE && flashAddress <= FLASH_OTP_END)) {
+        return -1;
+    }
+
+    hal_flash_unlock();
+    hal_flash_otp_unlock();
+
+    while (idx < length && flashAddress <= FLASH_OTP_END-1) {
+        /* Clear errors */
+        flash_clear_errors(0); /* bank 1 */
+        /* Wait for last operation to be completed */
+        hal_flash_wait_otp();
+
+        FLASH_OPTCR &= ~(FLASH_OPTCR_OPTLOCK); /* unlock FLASH_OPTCR register */
+
+        /* Set OTP_PG bit */
+        FLASH_OPTCR |= FLASH_OPTCR_PG_OTP;
+
+        ISB();
+        DSB();
+
+        /* Program an OTP word (16 bits) */
+        *(volatile uint16_t*)flashAddress = *(volatile uint16_t*)data;
+
+        /* Read it back */
+        tmp = *(volatile uint16_t*)flashAddress;
+        (void)tmp; /* avoid unused warnings */
+        flashAddress += sizeof(uint16_t);
+        data++;
+        idx += sizeof(uint16_t);
+
+        /* Wait for last operation to be completed */
+        hal_flash_wait_otp();
+
+        /* clear OTP_PG bit */
+        FLASH_OPTCR &= ~FLASH_OPTCR_PG_OTP;
+    }
+
+    hal_flash_otp_lock();
+    hal_flash_lock();
+    return 0;
+}
+
+int hal_flash_otp_read(uint32_t flashAddress, uint16_t* data, uint32_t length)
+{
+    uint32_t i;
+    if (!(flashAddress >= FLASH_OTP_BASE && flashAddress <= FLASH_OTP_END)) {
+        return -1;
+    }
+    for (i = 0;
+        (i < length) && (flashAddress <= (FLASH_OTP_END-1));
+        i += sizeof(uint16_t))
+    {
+        *data = *(volatile uint16_t*)flashAddress;
+        flashAddress += sizeof(uint16_t);
+        data++;
+    }
+    return 0;
+}
+
+#endif /* FLASH_OTP_ROT */

From 62a5b9f8fa10455e9b0df22e4e19ca77ed60d225 Mon Sep 17 00:00:00 2001
From: Daniele Lacamera <dan@danielinux.net>
Date: Fri, 10 May 2024 14:26:45 +0200
Subject: [PATCH 02/19] Export hal functions for OTP read/write access

---
 hal/stm32h7.c   | 307 ++-------------------------------------------
 hal/stm32h7.h   | 323 ++++++++++++++++++++++++++++++++++++++++++++++++
 include/hal.h   |   7 ++
 options.mk      |   6 +
 tools/config.mk |   4 +-
 5 files changed, 346 insertions(+), 301 deletions(-)
 create mode 100644 hal/stm32h7.h

diff --git a/hal/stm32h7.c b/hal/stm32h7.c
index 1284a35d3..62c9e4ca1 100644
--- a/hal/stm32h7.c
+++ b/hal/stm32h7.c
@@ -19,302 +19,7 @@
  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1335, USA
  */
 
-#include <stdint.h>
-#include "image.h"
-#include "hal.h"
-
-/* Assembly helpers */
-#define DMB() __asm__ volatile ("dmb")
-#define ISB() __asm__ volatile ("isb")
-#define DSB() __asm__ volatile ("dsb")
-
-/* STM32 H7 register configuration */
-/*** RCC ***/
-#define RCC_BASE            (0x58024400) /* RM0433 - Table 8 */
-#define RCC_CR              (*(volatile uint32_t *)(RCC_BASE + 0x00))  /* RM0433 - 7.7.2 */
-#define RCC_PLLCKSELR       (*(volatile uint32_t *)(RCC_BASE + 0x28))  /* RM0433 - 7.7.11 */
-#define RCC_PLLCFGR         (*(volatile uint32_t *)(RCC_BASE + 0x2C))  /* RM0433 - 7.7.12 */
-#define RCC_PLL1DIVR        (*(volatile uint32_t *)(RCC_BASE + 0x30))  /* RM0433 - 7.7.13 */
-
-#define RCC_CFGR            (*(volatile uint32_t *)(RCC_BASE + 0x10))  /* RM0433 - 7.7.7 */
-#define RCC_D1CFGR          (*(volatile uint32_t *)(RCC_BASE + 0x18))  /* RM0433 - 7.7.8 */
-#define RCC_D2CFGR          (*(volatile uint32_t *)(RCC_BASE + 0x1C))  /* RM0433 - 7.7.8 */
-#define RCC_D3CFGR          (*(volatile uint32_t *)(RCC_BASE + 0x20))  /* RM0433 - 7.7.9 */
-
-#define RCC_D2CCIP2R        (*(volatile uint32_t *)(RCC_BASE + 0x54))  /* RM0433 - 8.7.21 */
-
-#define APB1_CLOCK_LRST     (*(volatile uint32_t *)(RCC_BASE + 0x90))  /* RM0433 - 8.7.33 - RCC_APB1LRSTR */
-
-#define AHB4_CLOCK_ENR      (*(volatile uint32_t *)(RCC_BASE + 0xE0))  /* RM0433 - 8.7.43 */
-#define APB1_CLOCK_LER      (*(volatile uint32_t *)(RCC_BASE + 0xE8))  /* RM0433 - 8.7.45 - RCC_APB1LENR */
-#define APB1_CLOCK_HER      (*(volatile uint32_t *)(RCC_BASE + 0xEC))  /* RM0433 - 8.7.46 */
-#define APB2_CLOCK_ER       (*(volatile uint32_t *)(RCC_BASE + 0xF0))  /* RM0433 - 8.7.47 */
-#define AHB3_CLOCK_ER       (*(volatile uint32_t *)(RCC_BASE + 0xD4))  /* RM0433 - 8.7.40 */
-
-#define RCC_CR_PLL1RDY              (1 << 25)
-#define RCC_CR_PLL1ON               (1 << 24)
-#define RCC_CR_HSEBYP               (1 << 18)
-#define RCC_CR_HSERDY               (1 << 17)
-#define RCC_CR_HSEON                (1 << 16)
-#define RCC_CR_HSIRDY               (1 << 2)
-#define RCC_CR_HSION                (1 << 0)
-
-#define RCC_CFGR_SW_HSISYS          0x0
-#define RCC_CFGR_SW_PLL             0x3
-#define RCC_PLLCFGR_DIVR1EN        (1 << 18)
-#define RCC_PLLCFGR_DIVQ1EN        (1 << 17)
-#define RCC_PLLCFGR_DIVP1EN        (1 << 16)
-
-#define RCC_PLLCFGR_PLL1VCOSEL     (1 << 1)
-
-#define RCC_PLLCFGR_PLL1RGE_2_4    0x1
-#define RCC_PLLCFGR_PLL1RGE_SHIFT  0x2
-
-#define RCC_PLLCKSELR_DIVM1         (1 << 4)
-#define RCC_PLLCKSELR_PLLSRC_HSI     0x0
-#define RCC_PLLCKSELR_PLLSRC_HSE     0x2
-
-#define RCC_PLLCKSELR_DIVM1_NONE     0x1
-
-/*** GPIO ***/
-#define GPIOA_BASE (0x58020000)
-#define GPIOB_BASE (0x58020400)
-#define GPIOC_BASE (0x58020800)
-#define GPIOD_BASE (0x58020C00)
-#define GPIOE_BASE (0x58021000)
-#define GPIOF_BASE (0x58021400)
-#define GPIOG_BASE (0x58021800)
-
-#define GPIO_MODE(base)  (*(volatile uint32_t *)((base) + 0x00))
-#define GPIO_OTYPE(base) (*(volatile uint32_t *)((base) + 0x04))
-#define GPIO_OSPD(base)  (*(volatile uint32_t *)((base) + 0x08))
-#define GPIO_PUPD(base)  (*(volatile uint32_t *)((base) + 0x0c))
-#define GPIO_ODR(base)   (*(volatile uint32_t *)((base) + 0x14))
-#define GPIO_BSRR(base)  (*(volatile uint32_t *)((base) + 0x18))
-#define GPIO_AFRL(base)  (*(volatile uint32_t *)((base) + 0x20))
-#define GPIO_AFRH(base)  (*(volatile uint32_t *)((base) + 0x24))
-
-
-/*** PWR ***/
-#define PWR_BASE             (0x58024800) /* RM0433 - Table 8 */
-#define PWR_CSR1             (*(volatile uint32_t *)(PWR_BASE + 0x04))  /* RM0433 - 5.8.x */
-#define PWR_CSR1_ACTVOSRDY   (1 << 13)
-#define PWR_CR3              (*(volatile uint32_t *)(PWR_BASE + 0x0C))  /* RM0433 - 5.8.4 */
-#define PWR_CR3_SCUEN        (1 << 2)
-#define PWR_CR3_LDOEN        (1 << 1)
-#define PWR_D3CR             (*(volatile uint32_t *)(PWR_BASE + 0x18))  /* RM0433 - 5.8.6 */
-#define PWR_D3CR_VOSRDY      (1 << 13)
-#define PWR_D3CR_VOS_SHIFT   (14)
-#define PWR_D3CR_VOS_SCALE_1 (3)
-
-#define SYSCFG_BASE          (0x58000400) /* RM0433 - Table 8 */
-#define SYSCFG_PMCR          (*(volatile uint32_t *)(SYSCFG_BASE + 0x04))  /* RM0433 - 5.8.4 */
-#define SYSCFG_PWRCR         (*(volatile uint32_t *)(SYSCFG_BASE + 0x04))  /* RM0433 - 5.8.4 */
-#define SYSCFG_UR0           (*(volatile uint32_t *)(SYSCFG_BASE + 0x300))  /* RM0433 - 12.3.1.2 */
-#define SYSCFG_PWRCR_ODEN    (1 << 0)
-#define SYSCFG_UR0_BKS       (1 << 0)   /* bank swap */
-
-/*** APB PRESCALER ***/
-#define RCC_PRESCALER_DIV_NONE 0
-#define RCC_PRESCALER_DIV_2 8
-
-
-/*** UART ***/
-#ifndef CLOCK_SPEED
-#define CLOCK_SPEED 64000000UL /* 120MHz pclk1, 64MHz HSI */
-#endif
-#ifndef BAUD_RATE
-#define BAUD_RATE   115200
-#endif
-#ifndef UART_PORT
-#define UART_PORT   3 /* default to Nucleo VCOM port */
-#endif
-
-#if UART_PORT == 3
-/* USART3 Base address (connected to ST virtual com port on Nucleo board) */
-#define UART_BASE      (0x40004800)
-#define UART_GPIO_BASE GPIOD_BASE
-#define UART_TX_PIN    8 /* PD8, USART Transmit pin */
-#define UART_RX_PIN    9 /* PD9, USART Receive pin */
-#define UART_PIN_AF    7 /* AF stands for Alternate Function. USART TX/RX */
-#elif UART_PORT == 5
-/* USART5 Base address */
-#define UART_BASE      (0x40005000)
-#define UART_GPIO_BASE GPIOB_BASE
-#define UART_TX_PIN    13 /* PB13, USART Transmit pin */
-#define UART_RX_PIN    12 /* PB12, USART Receive pin */
-#define UART_PIN_AF    14 /* AF stands for Alternate Function. USART TX/RX */
-#else
-/* USART2 Base address (chosen because of its pin layout on Nucleo board) */
-#define UART_BASE      (0x40004400)
-#define UART_GPIO_BASE GPIOD_BASE
-#define UART_TX_PIN    5 /* PD5, USART Transmit pin */
-#define UART_RX_PIN    6 /* PD6, USART Receive pin */
-#define UART_PIN_AF    7 /* AF stands for Alternate Function. USART TX/RX */
-#endif
-
-/* UART/USART: Defining register start addresses. */
-#define UART_CR1(base)    (*(volatile uint32_t *)((base) + 0x00))
-#define UART_CR2(base)    (*(volatile uint32_t *)((base) + 0x04))
-#define UART_CR3(base)    (*(volatile uint32_t *)((base) + 0x08))
-#define UART_BRR(base)    (*(volatile uint32_t *)((base) + 0x0C))
-#define UART_RQR(base)    (*(volatile uint32_t *)((base) + 0x18))
-#define UART_ISR(base)    (*(volatile uint32_t *)((base) + 0x1C))
-#define UART_ICR(base)    (*(volatile uint32_t *)((base) + 0x20))
-#define UART_RDR(base)    (*(volatile uint32_t *)((base) + 0x24))
-#define UART_TDR(base)    (*(volatile uint32_t *)((base) + 0x28))
-#define UART_PRESC(base)  (*(volatile uint32_t *)((base) + 0x2C))
-
-/* UART/USART: Defining register bit placement for CR1 and ISR register for readability. */
-#define UART_CR1_UART_ENABLE                (1 << 0)
-#define UART_CR1_TX_ENABLE                  (1 << 3)
-#define UART_CR1_RX_ENABLE                  (1 << 2)
-#define UART_CR1_M1                         (1 << 28)
-#define UART_CR1_M0                         (1 << 12)
-#define UART_CR1_PARITY_ENABLED             (1 << 10)
-#define UART_CR1_PARITY_ODD                 (1 << 9)
-#define UART_CR1_FIFOEN                     (1 << 29)
-#define UART_CR1_OVER8                      (1 << 15)
-
-#define UART_CR2_STOP_MASK                  (0x3 << 12)
-#define UART_CR2_STOP(bits)                 (((bits) & 0x3) << 12)
-#define UART_CR2_LINEN                      (1 << 14)
-#define UART_CR2_CLKEN                      (1 << 11)
-
-#define UART_CR3_SCEN                       (1 << 5)
-#define UART_CR3_HDSEL                      (1 << 3)
-#define UART_CR3_IREN                       (1 << 1)
-
-#define UART_ISR_TX_FIFO_NOT_FULL           (1 << 7) /* Transmit Data Empty (TXE) or TX FIFO Not Full (TXFNF) */
-#define UART_ISR_RX_FIFO_NOT_EMPTY          (1 << 5)
-#define UART_ISR_TRANSMISSION_COMPLETE      (1 << 6)
-
-/* RCC: Defining register bit placement for APB1, APB2, AHB1 and AHB4 register for readability. */
-#define RCC_APB1_USART2_EN                  (1 << 17)
-#define RCC_APB1_USART3_EN                  (1 << 18)
-#define RCC_APB1_UART4_EN                   (1 << 19)
-#define RCC_APB1_UART5_EN                   (1 << 20)
-#define RCC_APB1_UART7_EN                   (1 << 30)
-#define RCC_APB1_UART8_EN                   (1 << 31)
-#define RCC_APB2_USART1_EN                  (1 << 4)
-#define RCC_APB2_USART6_EN                  (1 << 5)
-
-#define RCC_AHB4_GPIOB_EN                   (1 << 1)
-#define RCC_AHB4_GPIOD_EN                   (1 << 3)
-
-/*** QSPI ***/
-/* See hal/spi/spi_drv_stm32.c */
-
-
-/*** FLASH ***/
-#define SYSCFG_APB4_CLOCK_ER_VAL    (1 << 0) /* RM0433 - 7.7.48 - RCC_APB4ENR - SYSCFGEN */
-
-#define FLASH_BASE          (0x52002000)   /* RM0433 - Table 8 */
-#define FLASH_ACR           (*(volatile uint32_t *)(FLASH_BASE + 0x00)) /* RM0433 - 3.9.1 - FLASH_ACR */
-
-#define FLASH_OPTKEYR       (*(volatile uint32_t *)(FLASH_BASE + 0x08)) /* FLASH option key register */
-#define FLASH_OPTCR         (*(volatile uint32_t *)(FLASH_BASE + 0x18)) /* FLASH option control register */
-#define FLASH_OPTSR_CUR     (*(volatile uint32_t *)(FLASH_BASE + 0x1C)) /* FLASH option status register */
-
-/* Bank 1 */
-#define FLASH_KEYR1         (*(volatile uint32_t *)(FLASH_BASE + 0x04)) /* RM0433 - 3.9.2 - FLASH_KEYR 1 */
-#define FLASH_CR1           (*(volatile uint32_t *)(FLASH_BASE + 0x0C)) /* RM0433 - 3.9.4 - FLASH_CR 1 */
-#define FLASH_SR1           (*(volatile uint32_t *)(FLASH_BASE + 0x10)) /* RM0433 - 3.9.5 - FLASH_SR 1 */
-
-/* Bank 2 */
-#define FLASH_KEYR2         (*(volatile uint32_t *)(FLASH_BASE + 0x104)) /* RM0433 - 3.9.24 - FLASH_KEYR 2 */
-#define FLASH_SR2           (*(volatile uint32_t *)(FLASH_BASE + 0x110)) /* RM0433 - 3.9.26 - FLASH_SR 2 */
-#define FLASH_CR2           (*(volatile uint32_t *)(FLASH_BASE + 0x10C)) /* RM0433 - 3.9.25 - FLASH_CR 2 */
-
-/* Flash Configuration */
-#define FLASHMEM_ADDRESS_SPACE    (0x08000000UL)
-#define FLASH_PAGE_SIZE           (0x20000) /* 128KB */
-#define FLASH_BANK2_BASE          (0x08100000UL) /*!< Base address of : (up to 1 MB) Flash Bank2 accessible over AXI */
-#define FLASH_BANK2_BASE_REL      (FLASH_BANK2_BASE - FLASHMEM_ADDRESS_SPACE)
-#define FLASH_TOP                 (0x081FFFFFUL) /*!< FLASH end address  */
-
-/* Register values */
-#define FLASH_ACR_LATENCY_MASK              (0x07)
-#define FLASH_SR_BSY                        (1 << 0)
-#define FLASH_SR_WBNE                       (1 << 1)
-#define FLASH_SR_QW                         (1 << 2)
-#define FLASH_SR_WRPERR                     (1 << 17)
-#define FLASH_SR_PGSERR                     (1 << 18)
-#define FLASH_SR_STRBERR                    (1 << 19)
-#define FLASH_SR_INCERR                     (1 << 21)
-#define FLASH_SR_OPERR                      (1 << 22)
-#define FLASH_SR_RDPERR                     (1 << 23)
-#define FLASH_SR_RDSERR                     (1 << 24)
-#define FLASH_SR_SNECCERR                   (1 << 25)
-#define FLASH_SR_DBECCERR                   (1 << 26)
-#define FLASH_SR_EOP                        (1 << 16)
-
-#define FLASH_CR_LOCK                       (1 << 0) /* RM0433 - 3.7.5 - FLASH_CR */
-#define FLASH_CR_STRT                       (1 << 7)
-#define FLASH_CR_PSIZE                      (1 << 4)
-#define FLASH_CR_BER                        (1 << 3)
-#define FLASH_CR_SER                        (1 << 2)
-#define FLASH_CR_PG                         (1 << 1)
-#define FLASH_CR2_SPSS2                     (1 << 14)
-
-#define FLASH_OPTSR_CUR_BSY                 (1 << 0)
-
-#define FLASH_OPTCR_OPTLOCK                 (1 << 0)  /* lock option configuration bit */
-#define FLASH_OPTCR_OPTSTART                (1 << 1)  /* Option byte start change option configuration bit */
-#define FLASH_OPTCR_MER                     (1 << 4)  /* Mass erase request */
-#define FLASH_OPTCR_PG_OTP                  (1 << 5)  /* OTP program control bit */
-#define FLASH_OPTCR_OPTCHANGEERRIE          (1 << 30) /* Option byte change error interrupt enable bit */
-#define FLASH_OPTCR_SWAP_BANK               (1 << 31) /* Bank swapping option configuration bit */
-
-#define FLASH_CR_SNB_SHIFT                  8     /* SNB bits 10:8 */
-#define FLASH_CR_SNB_MASK                   0x7   /* SNB bits 10:8 - 3 bits */
-
-#define FLASH_KEY1                          (0x45670123U)
-#define FLASH_KEY2                          (0xCDEF89ABU)
-
-#define FLASH_OPT_KEY1                      (0x08192A3BU)
-#define FLASH_OPT_KEY2                      (0x4C5D6E7FU)
-
-#ifdef FLASH_OTP_ROT
-    #ifndef FLASH_OTP_BASE
-    #define FLASH_OTP_BASE 0x08FFF000
-    #endif
-    #ifndef FLASH_OTP_END
-    #define FLASH_OTP_END  0x08FFF3FF
-    #endif
-    #ifndef OTP_SIZE
-    #define OTP_SIZE       1024
-    #endif
-    #ifndef OTP_BLOCKS
-    #define OTP_BLOCKS     16
-    #endif
-
-    #define OTP_BLOCK_SIZE (OTP_SIZE / OTP_BLOCKS) /* 64 bytes */
-#endif
-
-/* STM32H7: Due to ECC functionality, it is not possible to write partition/sector
- * flags and signature more than once. This flags_cache is used to intercept write operations and
- * ensures that the sector is always erased before each write.
- */
-
-#define STM32H7_SECTOR_SIZE 0x20000
-
-#if defined(WOLFBOOT_PARTITION_SIZE) && \
-    (WOLFBOOT_PARTITION_SIZE < (2 * STM32H7_SECTOR_SIZE))
-#   error "Please use a bigger WOLFBOOT_PARTITION_SIZE, since the last 128KB on each partition will be reserved for bootloader flags"
-#endif
-
-#define STM32H7_PART_BOOT_END   (WOLFBOOT_PARTITION_BOOT_ADDRESS + WOLFBOOT_PARTITION_SIZE)
-#define STM32H7_PART_UPDATE_END (WOLFBOOT_PARTITION_UPDATE_ADDRESS + WOLFBOOT_PARTITION_SIZE)
-#define STM32H7_WORD_SIZE (32)
-#define STM32H7_PART_BOOT_FLAGS_PAGE_ADDRESS \
-    (((STM32H7_PART_BOOT_END - 1)   / STM32H7_SECTOR_SIZE) * STM32H7_SECTOR_SIZE)
-#define STM32H7_PART_UPDATE_FLAGS_PAGE_ADDRESS \
-    (((STM32H7_PART_UPDATE_END - 1) / STM32H7_SECTOR_SIZE) * STM32H7_SECTOR_SIZE)
-#define STM32H7_BOOT_FLAGS_PAGE(x) \
-    ((x >= STM32H7_PART_BOOT_FLAGS_PAGE_ADDRESS)   && (x < STM32H7_PART_BOOT_END))
-#define STM32H7_UPDATE_FLAGS_PAGE(x) \
-    ((x >= STM32H7_PART_UPDATE_FLAGS_PAGE_ADDRESS) && (x < STM32H7_PART_UPDATE_END))
+#include "hal/stm32h7.h"
 
 static uint32_t stm32h7_cache[STM32H7_WORD_SIZE / sizeof(uint32_t)];
 
@@ -809,8 +514,7 @@ void hal_prepare_boot(void)
 }
 
 #ifdef FLASH_OTP_ROT
-
-static void hal_flash_wait_otp(void)
+static void flash_otp_wait(void)
 {
     /* Wait for the FLASH operation to complete by polling on QW flag to be reset. */
     while ( (FLASH_SR1 & FLASH_SR_QW) == FLASH_SR_QW ) {
@@ -837,6 +541,8 @@ static void hal_flash_otp_lock(void)
     FLASH_OPTCR |= FLASH_OPTCR_OPTLOCK;
 }
 
+/* Public API */
+
 int hal_flash_otp_write(uint32_t flashAddress, uint16_t* data, uint16_t length)
 {
     volatile uint16_t tmp;
@@ -852,7 +558,7 @@ int hal_flash_otp_write(uint32_t flashAddress, uint16_t* data, uint16_t length)
         /* Clear errors */
         flash_clear_errors(0); /* bank 1 */
         /* Wait for last operation to be completed */
-        hal_flash_wait_otp();
+        flash_otp_wait();
 
         FLASH_OPTCR &= ~(FLASH_OPTCR_OPTLOCK); /* unlock FLASH_OPTCR register */
 
@@ -873,7 +579,7 @@ int hal_flash_otp_write(uint32_t flashAddress, uint16_t* data, uint16_t length)
         idx += sizeof(uint16_t);
 
         /* Wait for last operation to be completed */
-        hal_flash_wait_otp();
+        flash_otp_wait();
 
         /* clear OTP_PG bit */
         FLASH_OPTCR &= ~FLASH_OPTCR_PG_OTP;
@@ -902,3 +608,4 @@ int hal_flash_otp_read(uint32_t flashAddress, uint16_t* data, uint32_t length)
 }
 
 #endif /* FLASH_OTP_ROT */
+
diff --git a/hal/stm32h7.h b/hal/stm32h7.h
new file mode 100644
index 000000000..894e276f7
--- /dev/null
+++ b/hal/stm32h7.h
@@ -0,0 +1,323 @@
+/* stm32h7.h
+ *
+ * Copyright (C) 2021 wolfSSL Inc.
+ *
+ * This file is part of wolfBoot.
+ *
+ * wolfBoot is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * wolfBoot is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1335, USA
+ */
+
+
+#ifndef HAL_STM32H7_H
+#define HAL_STM32H7_H
+
+#include <stdint.h>
+#include "image.h"
+#include "hal.h"
+
+/* Assembly helpers */
+#define DMB() __asm__ volatile ("dmb")
+#define ISB() __asm__ volatile ("isb")
+#define DSB() __asm__ volatile ("dsb")
+
+/* STM32 H7 register configuration */
+/*** RCC ***/
+#define RCC_BASE            (0x58024400) /* RM0433 - Table 8 */
+#define RCC_CR              (*(volatile uint32_t *)(RCC_BASE + 0x00))  /* RM0433 - 7.7.2 */
+#define RCC_PLLCKSELR       (*(volatile uint32_t *)(RCC_BASE + 0x28))  /* RM0433 - 7.7.11 */
+#define RCC_PLLCFGR         (*(volatile uint32_t *)(RCC_BASE + 0x2C))  /* RM0433 - 7.7.12 */
+#define RCC_PLL1DIVR        (*(volatile uint32_t *)(RCC_BASE + 0x30))  /* RM0433 - 7.7.13 */
+
+#define RCC_CFGR            (*(volatile uint32_t *)(RCC_BASE + 0x10))  /* RM0433 - 7.7.7 */
+#define RCC_D1CFGR          (*(volatile uint32_t *)(RCC_BASE + 0x18))  /* RM0433 - 7.7.8 */
+#define RCC_D2CFGR          (*(volatile uint32_t *)(RCC_BASE + 0x1C))  /* RM0433 - 7.7.8 */
+#define RCC_D3CFGR          (*(volatile uint32_t *)(RCC_BASE + 0x20))  /* RM0433 - 7.7.9 */
+
+#define RCC_D2CCIP2R        (*(volatile uint32_t *)(RCC_BASE + 0x54))  /* RM0433 - 8.7.21 */
+
+#define APB1_CLOCK_LRST     (*(volatile uint32_t *)(RCC_BASE + 0x90))  /* RM0433 - 8.7.33 - RCC_APB1LRSTR */
+
+#define AHB4_CLOCK_ENR      (*(volatile uint32_t *)(RCC_BASE + 0xE0))  /* RM0433 - 8.7.43 */
+#define APB1_CLOCK_LER      (*(volatile uint32_t *)(RCC_BASE + 0xE8))  /* RM0433 - 8.7.45 - RCC_APB1LENR */
+#define APB1_CLOCK_HER      (*(volatile uint32_t *)(RCC_BASE + 0xEC))  /* RM0433 - 8.7.46 */
+#define APB2_CLOCK_ER       (*(volatile uint32_t *)(RCC_BASE + 0xF0))  /* RM0433 - 8.7.47 */
+#define AHB3_CLOCK_ER       (*(volatile uint32_t *)(RCC_BASE + 0xD4))  /* RM0433 - 8.7.40 */
+
+#define RCC_CR_PLL1RDY              (1 << 25)
+#define RCC_CR_PLL1ON               (1 << 24)
+#define RCC_CR_HSEBYP               (1 << 18)
+#define RCC_CR_HSERDY               (1 << 17)
+#define RCC_CR_HSEON                (1 << 16)
+#define RCC_CR_HSIRDY               (1 << 2)
+#define RCC_CR_HSION                (1 << 0)
+
+#define RCC_CFGR_SW_HSISYS          0x0
+#define RCC_CFGR_SW_PLL             0x3
+#define RCC_PLLCFGR_DIVR1EN        (1 << 18)
+#define RCC_PLLCFGR_DIVQ1EN        (1 << 17)
+#define RCC_PLLCFGR_DIVP1EN        (1 << 16)
+
+#define RCC_PLLCFGR_PLL1VCOSEL     (1 << 1)
+
+#define RCC_PLLCFGR_PLL1RGE_2_4    0x1
+#define RCC_PLLCFGR_PLL1RGE_SHIFT  0x2
+
+#define RCC_PLLCKSELR_DIVM1         (1 << 4)
+#define RCC_PLLCKSELR_PLLSRC_HSI     0x0
+#define RCC_PLLCKSELR_PLLSRC_HSE     0x2
+
+#define RCC_PLLCKSELR_DIVM1_NONE     0x1
+
+/*** GPIO ***/
+#define GPIOA_BASE (0x58020000)
+#define GPIOB_BASE (0x58020400)
+#define GPIOC_BASE (0x58020800)
+#define GPIOD_BASE (0x58020C00)
+#define GPIOE_BASE (0x58021000)
+#define GPIOF_BASE (0x58021400)
+#define GPIOG_BASE (0x58021800)
+
+#define GPIO_MODE(base)  (*(volatile uint32_t *)((base) + 0x00))
+#define GPIO_OTYPE(base) (*(volatile uint32_t *)((base) + 0x04))
+#define GPIO_OSPD(base)  (*(volatile uint32_t *)((base) + 0x08))
+#define GPIO_PUPD(base)  (*(volatile uint32_t *)((base) + 0x0c))
+#define GPIO_ODR(base)   (*(volatile uint32_t *)((base) + 0x14))
+#define GPIO_BSRR(base)  (*(volatile uint32_t *)((base) + 0x18))
+#define GPIO_AFRL(base)  (*(volatile uint32_t *)((base) + 0x20))
+#define GPIO_AFRH(base)  (*(volatile uint32_t *)((base) + 0x24))
+
+
+/*** PWR ***/
+#define PWR_BASE             (0x58024800) /* RM0433 - Table 8 */
+#define PWR_CSR1             (*(volatile uint32_t *)(PWR_BASE + 0x04))  /* RM0433 - 5.8.x */
+#define PWR_CSR1_ACTVOSRDY   (1 << 13)
+#define PWR_CR3              (*(volatile uint32_t *)(PWR_BASE + 0x0C))  /* RM0433 - 5.8.4 */
+#define PWR_CR3_SCUEN        (1 << 2)
+#define PWR_CR3_LDOEN        (1 << 1)
+#define PWR_D3CR             (*(volatile uint32_t *)(PWR_BASE + 0x18))  /* RM0433 - 5.8.6 */
+#define PWR_D3CR_VOSRDY      (1 << 13)
+#define PWR_D3CR_VOS_SHIFT   (14)
+#define PWR_D3CR_VOS_SCALE_1 (3)
+
+#define SYSCFG_BASE          (0x58000400) /* RM0433 - Table 8 */
+#define SYSCFG_PMCR          (*(volatile uint32_t *)(SYSCFG_BASE + 0x04))  /* RM0433 - 5.8.4 */
+#define SYSCFG_PWRCR         (*(volatile uint32_t *)(SYSCFG_BASE + 0x04))  /* RM0433 - 5.8.4 */
+#define SYSCFG_UR0           (*(volatile uint32_t *)(SYSCFG_BASE + 0x300))  /* RM0433 - 12.3.1.2 */
+#define SYSCFG_PWRCR_ODEN    (1 << 0)
+#define SYSCFG_UR0_BKS       (1 << 0)   /* bank swap */
+
+/*** APB PRESCALER ***/
+#define RCC_PRESCALER_DIV_NONE 0
+#define RCC_PRESCALER_DIV_2 8
+
+
+/*** UART ***/
+#ifndef CLOCK_SPEED
+#define CLOCK_SPEED 64000000UL /* 120MHz pclk1, 64MHz HSI */
+#endif
+#ifndef BAUD_RATE
+#define BAUD_RATE   115200
+#endif
+#ifndef UART_PORT
+#define UART_PORT   3 /* default to Nucleo VCOM port */
+#endif
+
+#if UART_PORT == 3
+/* USART3 Base address (connected to ST virtual com port on Nucleo board) */
+#define UART_BASE      (0x40004800)
+#define UART_GPIO_BASE GPIOD_BASE
+#define UART_TX_PIN    8 /* PD8, USART Transmit pin */
+#define UART_RX_PIN    9 /* PD9, USART Receive pin */
+#define UART_PIN_AF    7 /* AF stands for Alternate Function. USART TX/RX */
+#elif UART_PORT == 5
+/* USART5 Base address */
+#define UART_BASE      (0x40005000)
+#define UART_GPIO_BASE GPIOB_BASE
+#define UART_TX_PIN    13 /* PB13, USART Transmit pin */
+#define UART_RX_PIN    12 /* PB12, USART Receive pin */
+#define UART_PIN_AF    14 /* AF stands for Alternate Function. USART TX/RX */
+#else
+/* USART2 Base address (chosen because of its pin layout on Nucleo board) */
+#define UART_BASE      (0x40004400)
+#define UART_GPIO_BASE GPIOD_BASE
+#define UART_TX_PIN    5 /* PD5, USART Transmit pin */
+#define UART_RX_PIN    6 /* PD6, USART Receive pin */
+#define UART_PIN_AF    7 /* AF stands for Alternate Function. USART TX/RX */
+#endif
+
+/* UART/USART: Defining register start addresses. */
+#define UART_CR1(base)    (*(volatile uint32_t *)((base) + 0x00))
+#define UART_CR2(base)    (*(volatile uint32_t *)((base) + 0x04))
+#define UART_CR3(base)    (*(volatile uint32_t *)((base) + 0x08))
+#define UART_BRR(base)    (*(volatile uint32_t *)((base) + 0x0C))
+#define UART_RQR(base)    (*(volatile uint32_t *)((base) + 0x18))
+#define UART_ISR(base)    (*(volatile uint32_t *)((base) + 0x1C))
+#define UART_ICR(base)    (*(volatile uint32_t *)((base) + 0x20))
+#define UART_RDR(base)    (*(volatile uint32_t *)((base) + 0x24))
+#define UART_TDR(base)    (*(volatile uint32_t *)((base) + 0x28))
+#define UART_PRESC(base)  (*(volatile uint32_t *)((base) + 0x2C))
+
+/* UART/USART: Defining register bit placement for CR1 and ISR register for readability. */
+#define UART_CR1_UART_ENABLE                (1 << 0)
+#define UART_CR1_TX_ENABLE                  (1 << 3)
+#define UART_CR1_RX_ENABLE                  (1 << 2)
+#define UART_CR1_M1                         (1 << 28)
+#define UART_CR1_M0                         (1 << 12)
+#define UART_CR1_PARITY_ENABLED             (1 << 10)
+#define UART_CR1_PARITY_ODD                 (1 << 9)
+#define UART_CR1_FIFOEN                     (1 << 29)
+#define UART_CR1_OVER8                      (1 << 15)
+
+#define UART_CR2_STOP_MASK                  (0x3 << 12)
+#define UART_CR2_STOP(bits)                 (((bits) & 0x3) << 12)
+#define UART_CR2_LINEN                      (1 << 14)
+#define UART_CR2_CLKEN                      (1 << 11)
+
+#define UART_CR3_SCEN                       (1 << 5)
+#define UART_CR3_HDSEL                      (1 << 3)
+#define UART_CR3_IREN                       (1 << 1)
+
+#define UART_ISR_TX_FIFO_NOT_FULL           (1 << 7) /* Transmit Data Empty (TXE) or TX FIFO Not Full (TXFNF) */
+#define UART_ISR_RX_FIFO_NOT_EMPTY          (1 << 5)
+#define UART_ISR_TRANSMISSION_COMPLETE      (1 << 6)
+
+/* RCC: Defining register bit placement for APB1, APB2, AHB1 and AHB4 register for readability. */
+#define RCC_APB1_USART2_EN                  (1 << 17)
+#define RCC_APB1_USART3_EN                  (1 << 18)
+#define RCC_APB1_UART4_EN                   (1 << 19)
+#define RCC_APB1_UART5_EN                   (1 << 20)
+#define RCC_APB1_UART7_EN                   (1 << 30)
+#define RCC_APB1_UART8_EN                   (1 << 31)
+#define RCC_APB2_USART1_EN                  (1 << 4)
+#define RCC_APB2_USART6_EN                  (1 << 5)
+
+#define RCC_AHB4_GPIOB_EN                   (1 << 1)
+#define RCC_AHB4_GPIOD_EN                   (1 << 3)
+
+/*** QSPI ***/
+/* See hal/spi/spi_drv_stm32.c */
+
+
+/*** FLASH ***/
+#define SYSCFG_APB4_CLOCK_ER_VAL    (1 << 0) /* RM0433 - 7.7.48 - RCC_APB4ENR - SYSCFGEN */
+
+#define FLASH_BASE          (0x52002000)   /* RM0433 - Table 8 */
+#define FLASH_ACR           (*(volatile uint32_t *)(FLASH_BASE + 0x00)) /* RM0433 - 3.9.1 - FLASH_ACR */
+
+#define FLASH_OPTKEYR       (*(volatile uint32_t *)(FLASH_BASE + 0x08)) /* FLASH option key register */
+#define FLASH_OPTCR         (*(volatile uint32_t *)(FLASH_BASE + 0x18)) /* FLASH option control register */
+#define FLASH_OPTSR_CUR     (*(volatile uint32_t *)(FLASH_BASE + 0x1C)) /* FLASH option status register */
+
+/* Bank 1 */
+#define FLASH_KEYR1         (*(volatile uint32_t *)(FLASH_BASE + 0x04)) /* RM0433 - 3.9.2 - FLASH_KEYR 1 */
+#define FLASH_CR1           (*(volatile uint32_t *)(FLASH_BASE + 0x0C)) /* RM0433 - 3.9.4 - FLASH_CR 1 */
+#define FLASH_SR1           (*(volatile uint32_t *)(FLASH_BASE + 0x10)) /* RM0433 - 3.9.5 - FLASH_SR 1 */
+
+/* Bank 2 */
+#define FLASH_KEYR2         (*(volatile uint32_t *)(FLASH_BASE + 0x104)) /* RM0433 - 3.9.24 - FLASH_KEYR 2 */
+#define FLASH_SR2           (*(volatile uint32_t *)(FLASH_BASE + 0x110)) /* RM0433 - 3.9.26 - FLASH_SR 2 */
+#define FLASH_CR2           (*(volatile uint32_t *)(FLASH_BASE + 0x10C)) /* RM0433 - 3.9.25 - FLASH_CR 2 */
+
+/* Flash Configuration */
+#define FLASHMEM_ADDRESS_SPACE    (0x08000000UL)
+#define FLASH_PAGE_SIZE           (0x20000) /* 128KB */
+#define FLASH_BANK2_BASE          (0x08100000UL) /*!< Base address of : (up to 1 MB) Flash Bank2 accessible over AXI */
+#define FLASH_BANK2_BASE_REL      (FLASH_BANK2_BASE - FLASHMEM_ADDRESS_SPACE)
+#define FLASH_TOP                 (0x081FFFFFUL) /*!< FLASH end address  */
+
+/* Register values */
+#define FLASH_ACR_LATENCY_MASK              (0x07)
+#define FLASH_SR_BSY                        (1 << 0)
+#define FLASH_SR_WBNE                       (1 << 1)
+#define FLASH_SR_QW                         (1 << 2)
+#define FLASH_SR_WRPERR                     (1 << 17)
+#define FLASH_SR_PGSERR                     (1 << 18)
+#define FLASH_SR_STRBERR                    (1 << 19)
+#define FLASH_SR_INCERR                     (1 << 21)
+#define FLASH_SR_OPERR                      (1 << 22)
+#define FLASH_SR_RDPERR                     (1 << 23)
+#define FLASH_SR_RDSERR                     (1 << 24)
+#define FLASH_SR_SNECCERR                   (1 << 25)
+#define FLASH_SR_DBECCERR                   (1 << 26)
+#define FLASH_SR_EOP                        (1 << 16)
+
+#define FLASH_CR_LOCK                       (1 << 0) /* RM0433 - 3.7.5 - FLASH_CR */
+#define FLASH_CR_STRT                       (1 << 7)
+#define FLASH_CR_PSIZE                      (1 << 4)
+#define FLASH_CR_BER                        (1 << 3)
+#define FLASH_CR_SER                        (1 << 2)
+#define FLASH_CR_PG                         (1 << 1)
+#define FLASH_CR2_SPSS2                     (1 << 14)
+
+#define FLASH_OPTSR_CUR_BSY                 (1 << 0)
+
+#define FLASH_OPTCR_OPTLOCK                 (1 << 0)  /* lock option configuration bit */
+#define FLASH_OPTCR_OPTSTART                (1 << 1)  /* Option byte start change option configuration bit */
+#define FLASH_OPTCR_MER                     (1 << 4)  /* Mass erase request */
+#define FLASH_OPTCR_PG_OTP                  (1 << 5)  /* OTP program control bit */
+#define FLASH_OPTCR_OPTCHANGEERRIE          (1 << 30) /* Option byte change error interrupt enable bit */
+#define FLASH_OPTCR_SWAP_BANK               (1 << 31) /* Bank swapping option configuration bit */
+
+#define FLASH_CR_SNB_SHIFT                  8     /* SNB bits 10:8 */
+#define FLASH_CR_SNB_MASK                   0x7   /* SNB bits 10:8 - 3 bits */
+
+#define FLASH_KEY1                          (0x45670123U)
+#define FLASH_KEY2                          (0xCDEF89ABU)
+
+#define FLASH_OPT_KEY1                      (0x08192A3BU)
+#define FLASH_OPT_KEY2                      (0x4C5D6E7FU)
+
+#ifdef FLASH_OTP_ROT
+    #ifndef FLASH_OTP_BASE
+    #define FLASH_OTP_BASE 0x08FFF000
+    #endif
+    #ifndef FLASH_OTP_END
+    #define FLASH_OTP_END  0x08FFF3FF
+    #endif
+    #ifndef OTP_SIZE
+    #define OTP_SIZE       1024
+    #endif
+    #ifndef OTP_BLOCKS
+    #define OTP_BLOCKS     16
+    #endif
+
+    #define OTP_BLOCK_SIZE (OTP_SIZE / OTP_BLOCKS) /* 64 bytes */
+#endif
+
+/* STM32H7: Due to ECC functionality, it is not possible to write partition/sector
+ * flags and signature more than once. This flags_cache is used to intercept write operations and
+ * ensures that the sector is always erased before each write.
+ */
+
+#define STM32H7_SECTOR_SIZE 0x20000
+
+#if defined(WOLFBOOT_PARTITION_SIZE) && \
+    (WOLFBOOT_PARTITION_SIZE < (2 * STM32H7_SECTOR_SIZE))
+#   error "Please use a bigger WOLFBOOT_PARTITION_SIZE, since the last 128KB on each partition will be reserved for bootloader flags"
+#endif
+
+#define STM32H7_PART_BOOT_END   (WOLFBOOT_PARTITION_BOOT_ADDRESS + WOLFBOOT_PARTITION_SIZE)
+#define STM32H7_PART_UPDATE_END (WOLFBOOT_PARTITION_UPDATE_ADDRESS + WOLFBOOT_PARTITION_SIZE)
+#define STM32H7_WORD_SIZE (32)
+#define STM32H7_PART_BOOT_FLAGS_PAGE_ADDRESS \
+    (((STM32H7_PART_BOOT_END - 1)   / STM32H7_SECTOR_SIZE) * STM32H7_SECTOR_SIZE)
+#define STM32H7_PART_UPDATE_FLAGS_PAGE_ADDRESS \
+    (((STM32H7_PART_UPDATE_END - 1) / STM32H7_SECTOR_SIZE) * STM32H7_SECTOR_SIZE)
+#define STM32H7_BOOT_FLAGS_PAGE(x) \
+    ((x >= STM32H7_PART_BOOT_FLAGS_PAGE_ADDRESS)   && (x < STM32H7_PART_BOOT_END))
+#define STM32H7_UPDATE_FLAGS_PAGE(x) \
+    ((x >= STM32H7_PART_UPDATE_FLAGS_PAGE_ADDRESS) && (x < STM32H7_PART_UPDATE_END))
+
+#endif /* HAL_STM32H7_H */
diff --git a/include/hal.h b/include/hal.h
index 8f3e1ce5c..813eef83d 100644
--- a/include/hal.h
+++ b/include/hal.h
@@ -130,6 +130,13 @@ int hal_trng_get_entropy(unsigned char *out, unsigned len);
 
 #endif
 
+#ifdef FLASH_OTP_ROT
+
+int hal_flash_otp_write(uint32_t flashAddress, uint16_t* data, uint16_t length);
+int hal_flash_otp_read(uint32_t flashAddress, uint16_t* data, uint32_t length);
+
+#endif
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/options.mk b/options.mk
index d0361559b..2d2e9af68 100644
--- a/options.mk
+++ b/options.mk
@@ -1,5 +1,11 @@
 WOLFCRYPT_OBJS+=./lib/wolfssl/wolfcrypt/src/asn.o
 
+# Support for Built-in ROT into OTP flash memory
+ifeq ($(FLASH_OTP_ROT),1)
+    CFLAGS+=-D"FLASH_OTP_ROT"
+endif
+
+# Support for TPM signature verification
 ifeq ($(WOLFBOOT_TPM_VERIFY),1)
   WOLFTPM:=1
   CFLAGS+=-D"WOLFBOOT_TPM_VERIFY"
diff --git a/tools/config.mk b/tools/config.mk
index e37fd9048..731eea25f 100644
--- a/tools/config.mk
+++ b/tools/config.mk
@@ -78,6 +78,7 @@ ifeq ($(ARCH),)
   FORCE_32BIT=0
   DISK_LOCK?=0
   DISK_LOCK_PASSWORD?=
+  FLASH_OTP_ROT?=0
 endif
 
 CONFIG_VARS:= ARCH TARGET SIGN HASH MCUXSDK MCUXPRESSO MCUXPRESSO_CPU MCUXPRESSO_DRIVERS \
@@ -98,4 +99,5 @@ CONFIG_VARS:= ARCH TARGET SIGN HASH MCUXSDK MCUXPRESSO MCUXPRESSO_CPU MCUXPRESSO
 	WOLFBOOT_UNIVERSAL_KEYSTORE \
 	XMSS_PARAMS \
 	ELF \
-	NXP_CUSTOM_DCD NXP_CUSTOM_DCD_OBJS
+	NXP_CUSTOM_DCD NXP_CUSTOM_DCD_OBJS \
+	FLASH_OTP_ROT

From d1eabc2e3137c4578ebb14761109c4ac9a24aa91 Mon Sep 17 00:00:00 2001
From: Daniele Lacamera <dan@danielinux.net>
Date: Fri, 10 May 2024 15:12:15 +0200
Subject: [PATCH 03/19] Keystore module for OTP in FLASH

---
 Makefile                 |   6 +-
 include/keystore.h       |   2 +
 src/flash_otp_keystore.c | 128 +++++++++++++++++++++++++++++++++++++++
 tools/keytools/keygen.c  |   2 +-
 4 files changed, 136 insertions(+), 2 deletions(-)
 create mode 100644 src/flash_otp_keystore.c

diff --git a/Makefile b/Makefile
index acfcd4edf..656218d68 100644
--- a/Makefile
+++ b/Makefile
@@ -35,7 +35,11 @@ ifeq ($(SIGN),NONE)
   PRIVATE_KEY=
 else
   PRIVATE_KEY=wolfboot_signing_private_key.der
-  OBJS+=./src/keystore.o
+  ifeq ($(FLASH_OTP_ROT),1)
+        OBJS+=./src/flash_otp_keystore.o
+  else
+        OBJS+=./src/keystore.o
+  endif
 endif
 
 WOLFCRYPT_OBJS:=
diff --git a/include/keystore.h b/include/keystore.h
index 5839e017f..a2cfc91e1 100644
--- a/include/keystore.h
+++ b/include/keystore.h
@@ -43,6 +43,8 @@ struct keystore_slot {
     uint8_t  pubkey[KEYSTORE_PUBKEY_SIZE];
 };
 
+#define SIZEOF_KEYSTORE_SLOT (32 + KEYSTORE_PUBKEY_SIZE)
+
 /* KeyStore API */
 int keystore_num_pubkeys(void);
 #if defined(WOLFBOOT_RENESAS_SCEPROTECT) ||\
diff --git a/src/flash_otp_keystore.c b/src/flash_otp_keystore.c
new file mode 100644
index 000000000..2c708f923
--- /dev/null
+++ b/src/flash_otp_keystore.c
@@ -0,0 +1,128 @@
+/* flash_otp_keystore.c
+ *
+ * Implementation for Flash based OTP keystore used as trust anchor
+ *
+ *
+ * Copyright (C) 2024 wolfSSL Inc.
+ *
+ * This file is part of wolfBoot.
+ *
+ * wolfBoot is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * wolfBoot is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1335, USA
+ */
+
+#include <stdint.h>
+#include <string.h>
+#include "wolfboot/wolfboot.h"
+#include "keystore.h"
+#include "hal.h"
+
+#ifdef FLASH_OTP_ROT
+
+#ifdef TARGET_stm32h7
+#include "hal/stm32h7.h"
+#endif
+
+#define OTP_HDR_SIZE 16
+
+struct wolfBoot_otp_hdr_size {
+    char keystore_hdr_magic[8];
+    uint16_t item_count;
+    uint16_t flags;
+    uint32_t version;
+};
+
+static const char KEYSTORE_HDR_MAGIC[8] = "WOLFBOOT";
+
+#if !defined(KEYSTORE_ANY) && (KEYSTORE_PUBKEY_SIZE != KEYSTORE_PUBKEY_SIZE_ECC256)
+	#error Key algorithm mismatch. Remove old keys via 'make keysclean'
+#else
+
+#define KEYSTORE_MAX_PUBKEYS ((OTP_SIZE - OTP_HDR_SIZE) / SIZEOF_KEYSTORE_SLOT)
+
+#if (KEYSTORE_MAX_PUBKEYS < 1)
+    #error "No space for keystore in OTP with current algorithm"
+#endif
+
+int keystore_num_pubkeys(void)
+{
+    uint8_t otp_header[OTP_HDR_SIZE];
+    struct wolfBoot_otp_hdr_size *hdr = (struct wolfBoot_otp_hdr_size *)otp_header;
+    if (hal_flash_otp_read(FLASH_OTP_BASE, (void *)otp_header, OTP_HDR_SIZE) != 0)
+        return 0;
+    if (memcmp(hdr->keystore_hdr_magic, KEYSTORE_HDR_MAGIC, 8) != 0) {
+        return 0;
+    }
+    if (hdr->item_count > KEYSTORE_MAX_PUBKEYS)
+        return 0;
+    return hdr->item_count;
+}
+
+static uint16_t otp_slot_item_cache[SIZEOF_KEYSTORE_SLOT/2];
+
+uint8_t *keystore_get_buffer(int id)
+{
+    struct keystore_slot *slot;
+    if (id >= keystore_num_pubkeys())
+        return (uint8_t *)0;
+    if (hal_flash_otp_read(FLASH_OTP_BASE +
+                OTP_HDR_SIZE + id * SIZEOF_KEYSTORE_SLOT, otp_slot_item_cache,
+                SIZEOF_KEYSTORE_SLOT) != 0)
+        return (uint8_t *)0;
+    slot = (struct keystore_slot *)otp_slot_item_cache;
+    return slot->pubkey;
+}
+
+int keystore_get_size(int id)
+{
+    struct keystore_slot *slot;
+    if (id >= keystore_num_pubkeys())
+        return -1;
+    if (hal_flash_otp_read(FLASH_OTP_BASE +
+                OTP_HDR_SIZE + id * SIZEOF_KEYSTORE_SLOT, otp_slot_item_cache,
+                SIZEOF_KEYSTORE_SLOT) != 0)
+        return -1;
+    slot = (struct keystore_slot *)otp_slot_item_cache;
+    return slot->pubkey_size;
+}
+
+uint32_t keystore_get_mask(int id)
+{
+    struct keystore_slot *slot;
+    if (id >= keystore_num_pubkeys())
+        return 0;
+    if (hal_flash_otp_read(FLASH_OTP_BASE +
+                OTP_HDR_SIZE + id * SIZEOF_KEYSTORE_SLOT, otp_slot_item_cache,
+                SIZEOF_KEYSTORE_SLOT) != 0)
+        return 0;
+    slot = (struct keystore_slot *)otp_slot_item_cache;
+    return slot->part_id_mask;
+}
+
+uint32_t keystore_get_key_type(int id)
+{
+    struct keystore_slot *slot;
+    if (id >= keystore_num_pubkeys())
+        return -1;
+    if (hal_flash_otp_read(FLASH_OTP_BASE +
+                OTP_HDR_SIZE + id * SIZEOF_KEYSTORE_SLOT, otp_slot_item_cache,
+                SIZEOF_KEYSTORE_SLOT) != 0)
+        return -1;
+    slot = (struct keystore_slot *)otp_slot_item_cache;
+    return slot->key_type;
+}
+
+#endif /* Keystore public key size check */
+
+#endif /* FLASH_OTP_ROT */
diff --git a/tools/keytools/keygen.c b/tools/keytools/keygen.c
index 45d1e2723..a97c55212 100644
--- a/tools/keytools/keygen.c
+++ b/tools/keytools/keygen.c
@@ -233,7 +233,7 @@ const char Keystore_API[] =
                 "uint32_t keystore_get_mask(int id)\n"
                 "{\n"
                 "    if (id >= keystore_num_pubkeys())\n"
-                "        return -1;\n"
+                "        return 0;\n"
                 "    return (int)PubKeys[id].part_id_mask;\n"
                 "}\n\n"
                 "uint32_t keystore_get_key_type(int id)\n"

From 32578808f4066b78e995ed390055ebc7f29cc150 Mon Sep 17 00:00:00 2001
From: Daniele Lacamera <dan@danielinux.net>
Date: Fri, 10 May 2024 15:31:19 +0200
Subject: [PATCH 04/19] Added guard to OTP module for NO_SIGN

---
 src/flash_otp_keystore.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/flash_otp_keystore.c b/src/flash_otp_keystore.c
index 2c708f923..795410474 100644
--- a/src/flash_otp_keystore.c
+++ b/src/flash_otp_keystore.c
@@ -28,7 +28,7 @@
 #include "keystore.h"
 #include "hal.h"
 
-#ifdef FLASH_OTP_ROT
+#if defined(FLASH_OTP_ROT) && !defined(WOLFBOOT_NO_SIGN)
 
 #ifdef TARGET_stm32h7
 #include "hal/stm32h7.h"
@@ -125,4 +125,4 @@ uint32_t keystore_get_key_type(int id)
 
 #endif /* Keystore public key size check */
 
-#endif /* FLASH_OTP_ROT */
+#endif /* FLASH_OTP_ROT && !WOLFBOOT_NO_SIGN */

From 572e1157bc428b8e1a494f3dc864e11f97c65eb7 Mon Sep 17 00:00:00 2001
From: Daniele Lacamera <dan@danielinux.net>
Date: Mon, 13 May 2024 13:15:25 +0200
Subject: [PATCH 05/19] Added "otp-keystore-primer" tool

---
 .gitignore                               |   1 +
 Makefile                                 |  16 +-
 hal/stm32h7.c                            |  17 +-
 include/hal.h                            |   4 +-
 include/otp_keystore.h                   |  70 +++++
 include/wolfboot/wolfboot.h              |   1 +
 src/flash_otp_keystore.c                 |  29 +-
 tools/keytools/otp/Makefile              |  36 +++
 tools/keytools/otp/otp-keystore-primer.c |  61 ++++
 tools/keytools/otp/startup.c             | 337 +++++++++++++++++++++++
 tools/keytools/otp/target.ld             |  45 +++
 11 files changed, 580 insertions(+), 37 deletions(-)
 create mode 100644 include/otp_keystore.h
 create mode 100644 tools/keytools/otp/Makefile
 create mode 100644 tools/keytools/otp/otp-keystore-primer.c
 create mode 100644 tools/keytools/otp/startup.c
 create mode 100644 tools/keytools/otp/target.ld

diff --git a/.gitignore b/.gitignore
index fdbf486ac..bd0d75afb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -69,6 +69,7 @@ tools/keytools/keygen.exe
 tools/keytools/x64
 tools/keytools/Debug
 tools/keytools/Release
+tools/keytools/otp/otp-keystore-primer
 
 # delta binaries
 tools/delta/bmdiff
diff --git a/Makefile b/Makefile
index 656218d68..7c0d5f93f 100644
--- a/Makefile
+++ b/Makefile
@@ -127,6 +127,10 @@ ifeq ($(TARGET),nxp_t1024)
 	MAIN_TARGET:=factory_wstage1.bin
 endif
 
+ifeq ($(FLASH_OTP_ROT),1)
+	MAIN_TARGET:=include/target.h tools/keytools/otp/otp-keystore-primer factory.bin
+endif
+
 ASFLAGS:=$(CFLAGS)
 BOOTLOADER_PARTITION_SIZE?=$$(( $(WOLFBOOT_PARTITION_BOOT_ADDRESS) - $(ARCH_FLASH_OFFSET)))
 
@@ -184,6 +188,7 @@ $(PRIVATE_KEY):
 	$(Q)$(MAKE) keytools_check
 	$(Q)(test $(SIGN) = NONE) || ("$(KEYGEN_TOOL)" $(KEYGEN_OPTIONS) -g $(PRIVATE_KEY)) || true
 	$(Q)(test $(SIGN) = NONE) && (echo "// SIGN=NONE" >  src/keystore.c) || true
+	$(Q)(test $(FLASH_OTP_ROT) = 0) || (make -C tools/keytools/otp) || true
 
 keytools: include/target.h
 	@echo "Building key tools"
@@ -239,7 +244,7 @@ wolfboot_stage1.bin: wolfboot.elf stage1/loader_stage1.bin
 	$(Q) cp stage1/loader_stage1.bin wolfboot_stage1.bin
 
 wolfboot.elf: include/target.h $(LSCRIPT) $(OBJS) $(LIBS) $(BINASSEMBLE) FORCE
-	$(Q)(test $(SIGN) = NONE) || (grep -q $(SIGN_ALG) src/keystore.c) || \
+	$(Q)(test $(SIGN) = NONE) || (test $(FLASH_OTP_ROT) = 1) || (grep -q $(SIGN_ALG) src/keystore.c) || \
 		(echo "Key mismatch: please run 'make distclean' to remove all keys if you want to change algorithm" && false)
 	@echo "\t[LD] $@"
 	@echo $(OBJS)
@@ -279,6 +284,8 @@ hex: wolfboot.hex
 
 src/keystore.c: $(PRIVATE_KEY)
 
+flash_keystore: $(PRIVATE_KEY) src/flash_otp_keystore.o
+
 keys: $(PRIVATE_KEY)
 
 clean:
@@ -302,6 +309,7 @@ utilsclean: clean
 	$(Q)$(MAKE) -C tools/test-update-server -s clean
 	$(Q)$(MAKE) -C tools/uart-flash-server -s clean
 	$(Q)$(MAKE) -C tools/unit-tests -s clean
+	$(Q)$(MAKE) -C tools/keytools/otp -s clean
 
 keysclean: clean
 	$(Q)rm -f *.pem *.der tags ./src/*_pub_key.c ./src/keystore.c include/target.h
@@ -359,6 +367,12 @@ cppcheck:
 		--suppress="objectIndex" --suppress="comparePointers" \
 		--error-exitcode=89 --std=c89 src/*.c hal/*.c hal/spi/*.c hal/uart/*.c
 
+otp: tools/keytools/otp/otp-keystore-primer.bin
+
+tools/keytools/otp/otp-keystore-primer.bin: FORCE
+	make -C tools/keytools/otp clean
+	make -C tools/keytools/otp
+
 %.o:%.c
 	@echo "\t[CC-$(ARCH)] $@"
 	$(Q)$(CC) $(CFLAGS) -c $(OUTPUT_FLAG) $@ $^
diff --git a/hal/stm32h7.c b/hal/stm32h7.c
index 62c9e4ca1..3c1b92dff 100644
--- a/hal/stm32h7.c
+++ b/hal/stm32h7.c
@@ -543,7 +543,7 @@ static void hal_flash_otp_lock(void)
 
 /* Public API */
 
-int hal_flash_otp_write(uint32_t flashAddress, uint16_t* data, uint16_t length)
+int hal_flash_otp_write(uint32_t flashAddress, const void* data, uint16_t length)
 {
     volatile uint16_t tmp;
     uint16_t idx = 0;
@@ -569,20 +569,23 @@ int hal_flash_otp_write(uint32_t flashAddress, uint16_t* data, uint16_t length)
         DSB();
 
         /* Program an OTP word (16 bits) */
-        *(volatile uint16_t*)flashAddress = *(volatile uint16_t*)data;
+        *(volatile uint16_t*)flashAddress = *(const uint16_t*)data;
 
+#if 0
         /* Read it back */
         tmp = *(volatile uint16_t*)flashAddress;
         (void)tmp; /* avoid unused warnings */
-        flashAddress += sizeof(uint16_t);
-        data++;
-        idx += sizeof(uint16_t);
+#endif
 
         /* Wait for last operation to be completed */
         flash_otp_wait();
 
         /* clear OTP_PG bit */
         FLASH_OPTCR &= ~FLASH_OPTCR_PG_OTP;
+
+        flashAddress += sizeof(uint16_t);
+        data++;
+        idx += sizeof(uint16_t);
     }
 
     hal_flash_otp_lock();
@@ -590,7 +593,7 @@ int hal_flash_otp_write(uint32_t flashAddress, uint16_t* data, uint16_t length)
     return 0;
 }
 
-int hal_flash_otp_read(uint32_t flashAddress, uint16_t* data, uint32_t length)
+int hal_flash_otp_read(uint32_t flashAddress, void* data, uint32_t length)
 {
     uint32_t i;
     if (!(flashAddress >= FLASH_OTP_BASE && flashAddress <= FLASH_OTP_END)) {
@@ -600,7 +603,7 @@ int hal_flash_otp_read(uint32_t flashAddress, uint16_t* data, uint32_t length)
         (i < length) && (flashAddress <= (FLASH_OTP_END-1));
         i += sizeof(uint16_t))
     {
-        *data = *(volatile uint16_t*)flashAddress;
+        *(uint16_t *)data = *(volatile uint16_t*)flashAddress;
         flashAddress += sizeof(uint16_t);
         data++;
     }
diff --git a/include/hal.h b/include/hal.h
index 813eef83d..73d2db1dd 100644
--- a/include/hal.h
+++ b/include/hal.h
@@ -132,8 +132,8 @@ int hal_trng_get_entropy(unsigned char *out, unsigned len);
 
 #ifdef FLASH_OTP_ROT
 
-int hal_flash_otp_write(uint32_t flashAddress, uint16_t* data, uint16_t length);
-int hal_flash_otp_read(uint32_t flashAddress, uint16_t* data, uint32_t length);
+int hal_flash_otp_write(uint32_t flashAddress, const void* data, uint16_t length);
+int hal_flash_otp_read(uint32_t flashAddress, void* data, uint32_t length);
 
 #endif
 
diff --git a/include/otp_keystore.h b/include/otp_keystore.h
new file mode 100644
index 000000000..2f8a47030
--- /dev/null
+++ b/include/otp_keystore.h
@@ -0,0 +1,70 @@
+/* otp_keystore.h
+ *
+ * Helper for storing/retrieving Trust Anchor to/from OTP flash
+ *
+ *
+ * Copyright (C) 2024 wolfSSL Inc.
+ *
+ * This file is part of wolfBoot.
+ *
+ * wolfBoot is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * wolfBoot is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1335, USA
+ */
+
+
+#ifndef OTP_KEYSTORE_H
+#define OTP_KEYSTORE_H
+
+#if defined(FLASH_OTP_ROT) && !defined(WOLFBOOT_NO_SIGN)
+/* Specific includes for supported targets
+ * (needed for OTP_SIZE)
+ */
+#ifdef TARGET_stm32h7
+    #include "hal/stm32h7.h"
+#else
+    #error "Unsupported target for OTP"
+#endif
+
+#include "keystore.h"
+
+#define OTP_HDR_SIZE 16
+
+struct __attribute__((packed)) wolfBoot_otp_hdr {
+    char keystore_hdr_magic[8];
+    uint16_t item_count;
+    uint16_t flags;
+    uint32_t version;
+};
+
+static const char KEYSTORE_HDR_MAGIC[8] = "WOLFBOOT";
+
+#if !defined(KEYSTORE_ANY) && (KEYSTORE_PUBKEY_SIZE != KEYSTORE_PUBKEY_SIZE_ECC256)
+	#error Key algorithm mismatch. Remove old keys via 'make keysclean'
+#else
+
+#define KEYSTORE_MAX_PUBKEYS ((OTP_SIZE - OTP_HDR_SIZE) / SIZEOF_KEYSTORE_SLOT)
+
+#if (OTP_SIZE == 0)
+#error WRONG OTP SIZE
+#endif
+
+#if (KEYSTORE_MAX_PUBKEYS < 1)
+    #error "No space for any keystores in OTP with current algorithm"
+#endif
+
+#endif /* KEYSTORE_ANY */
+
+#endif /* FLASH_OTP_ROT */ 
+
+#endif /* OTP_KEYSTORE_H */
diff --git a/include/wolfboot/wolfboot.h b/include/wolfboot/wolfboot.h
index 93b99f1bc..e6be66fd9 100644
--- a/include/wolfboot/wolfboot.h
+++ b/include/wolfboot/wolfboot.h
@@ -330,6 +330,7 @@ int wolfBoot_set_encrypt_key(const uint8_t *key, const uint8_t *nonce);
 int wolfBoot_get_encrypt_key(uint8_t *key, uint8_t *nonce);
 int wolfBoot_erase_encrypt_key(void);
 
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/src/flash_otp_keystore.c b/src/flash_otp_keystore.c
index 795410474..b5cca0b0b 100644
--- a/src/flash_otp_keystore.c
+++ b/src/flash_otp_keystore.c
@@ -27,38 +27,14 @@
 #include "wolfboot/wolfboot.h"
 #include "keystore.h"
 #include "hal.h"
+#include "otp_keystore.h"
 
 #if defined(FLASH_OTP_ROT) && !defined(WOLFBOOT_NO_SIGN)
 
-#ifdef TARGET_stm32h7
-#include "hal/stm32h7.h"
-#endif
-
-#define OTP_HDR_SIZE 16
-
-struct wolfBoot_otp_hdr_size {
-    char keystore_hdr_magic[8];
-    uint16_t item_count;
-    uint16_t flags;
-    uint32_t version;
-};
-
-static const char KEYSTORE_HDR_MAGIC[8] = "WOLFBOOT";
-
-#if !defined(KEYSTORE_ANY) && (KEYSTORE_PUBKEY_SIZE != KEYSTORE_PUBKEY_SIZE_ECC256)
-	#error Key algorithm mismatch. Remove old keys via 'make keysclean'
-#else
-
-#define KEYSTORE_MAX_PUBKEYS ((OTP_SIZE - OTP_HDR_SIZE) / SIZEOF_KEYSTORE_SLOT)
-
-#if (KEYSTORE_MAX_PUBKEYS < 1)
-    #error "No space for keystore in OTP with current algorithm"
-#endif
-
 int keystore_num_pubkeys(void)
 {
     uint8_t otp_header[OTP_HDR_SIZE];
-    struct wolfBoot_otp_hdr_size *hdr = (struct wolfBoot_otp_hdr_size *)otp_header;
+    struct wolfBoot_otp_hdr *hdr = (struct wolfBoot_otp_hdr *)otp_header;
     if (hal_flash_otp_read(FLASH_OTP_BASE, (void *)otp_header, OTP_HDR_SIZE) != 0)
         return 0;
     if (memcmp(hdr->keystore_hdr_magic, KEYSTORE_HDR_MAGIC, 8) != 0) {
@@ -123,6 +99,5 @@ uint32_t keystore_get_key_type(int id)
     return slot->key_type;
 }
 
-#endif /* Keystore public key size check */
 
 #endif /* FLASH_OTP_ROT && !WOLFBOOT_NO_SIGN */
diff --git a/tools/keytools/otp/Makefile b/tools/keytools/otp/Makefile
new file mode 100644
index 000000000..5141c61a2
--- /dev/null
+++ b/tools/keytools/otp/Makefile
@@ -0,0 +1,36 @@
+-include ../../../.config
+-include ../../../tools/config.mk
+-include ../../../options.mk
+-include ../../../wcs/pkcs11.mk
+
+TARGET?=none
+ARCH?=ARM
+CROSS_COMPILE?=arm-none-eabi-
+CFLAGS+=-O0 -ggdb
+CFLAGS+=-I. -I../../../ -I../../../include
+CFLAGS+=-I./wcs
+CFLAGS+=-DFLASH_OTP_ROT
+OBJS+=startup.o otp-keystore-primer.o ../../../src/keystore.o
+LSCRIPT=target.ld
+LDFLAGS+=$(CFLAGS) -T$(LSCRIPT) -lc -Wl,-Map=otp-keystore-primer.map
+
+ifeq ($(TARGET),stm32h7)
+    CFLAGS+=-DTARGET_stm32h7
+    CFLAGS+=-mcpu=cortex-m7 -ffunction-sections -fdata-sections -fno-common -ffreestanding -nostartfiles
+    OBJS+=../../../hal/stm32h7.o
+endif
+CC=$(CROSS_COMPILE)gcc
+OBJCOPY?=$(CROSS_COMPILE)objcopy
+SIZE?=$(CROSS_COMPILE)size
+
+
+otp-keystore-primer.bin: otp-keystore-primer.elf
+	@$(OBJCOPY) -O binary $(^) $(@)
+
+otp-keystore-primer.elf: $(OBJS)
+	@$(CC) -o otp-keystore-primer.elf $(LDFLAGS) $(CFLAGS) $(OBJS)
+	@$(SIZE) $(@)
+	
+
+clean:
+	@rm -rf $(OBJS) *.bin *.elf
diff --git a/tools/keytools/otp/otp-keystore-primer.c b/tools/keytools/otp/otp-keystore-primer.c
new file mode 100644
index 000000000..fc4a31d3c
--- /dev/null
+++ b/tools/keytools/otp/otp-keystore-primer.c
@@ -0,0 +1,61 @@
+/* otp-keystore-primer.c
+ *
+ * Primer app to provision public keys into OTP flash
+ *
+ *
+ * Copyright (C) 2024 wolfSSL Inc.
+ *
+ * This file is part of wolfBoot.
+ *
+ * wolfBoot is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * wolfBoot is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1335, USA
+ */
+#include <stdint.h>
+#include <string.h>
+#include "hal.h"
+#include "otp_keystore.h"
+
+extern struct keystore_slot PubKeys[];
+
+void main(void)
+{
+    int n_keys = keystore_num_pubkeys();
+    int i;
+    struct wolfBoot_otp_hdr hdr;
+    memcpy(hdr.keystore_hdr_magic, KEYSTORE_HDR_MAGIC, 8);
+    hdr.item_count = n_keys;
+    hdr.flags = 0;
+    hdr.version = WOLFBOOT_VERSION;
+
+    /* Sanity check to avoid writing an empty keystore */
+    if (n_keys < 1) {
+        while(1)
+            ;
+    }
+
+    /* Write the header to the beginning of the OTP memory */
+    hal_flash_otp_write(FLASH_OTP_BASE, (uint16_t *)&hdr, sizeof(hdr));
+
+    for (i = 0; i < n_keys; i++) {
+        /* Write each public key to its slot in OTP */
+        hal_flash_otp_write(FLASH_OTP_BASE +
+                OTP_HDR_SIZE + i * SIZEOF_KEYSTORE_SLOT, (uint16_t *)&PubKeys[i],
+                sizeof(struct keystore_slot));
+    }
+
+    /* Done! */
+    while(1)
+        ;
+
+}
diff --git a/tools/keytools/otp/startup.c b/tools/keytools/otp/startup.c
new file mode 100644
index 000000000..b0509bbf1
--- /dev/null
+++ b/tools/keytools/otp/startup.c
@@ -0,0 +1,337 @@
+/* startup_arm.c
+ *
+ * Test bare-metal blinking led application
+ *
+ * Copyright (C) 2021 wolfSSL Inc.
+ *
+ * This file is part of wolfBoot.
+ *
+ * wolfBoot is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * wolfBoot is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1335, USA
+ */
+
+extern unsigned int _stored_data;
+extern unsigned int _start_data;
+extern unsigned int _end_data;
+extern unsigned int _start_bss;
+extern unsigned int _end_bss;
+extern unsigned int _end_stack;
+extern unsigned int _start_heap;
+
+
+#ifndef STACK_PAINTING
+#define STACK_PAINTING 0
+#endif
+
+static volatile unsigned int avail_mem = 0;
+#if STACK_PAINTING
+static unsigned int sp;
+#endif
+
+extern void main(void);
+
+void isr_reset(void) {
+    register unsigned int *src, *dst;
+    src = (unsigned int *) &_stored_data;
+    dst = (unsigned int *) &_start_data;
+    while (dst < (unsigned int *)&_end_data) {
+        *dst = *src;
+        dst++;
+        src++;
+    }
+
+    dst = &_start_bss;
+    while (dst < (unsigned int *)&_end_bss) {
+        *dst = 0U;
+        dst++;
+    }
+
+    avail_mem = &_end_stack - &_start_heap;
+#if STACK_PAINTING
+    {
+        asm volatile("mrs %0, msp" : "=r"(sp));
+        dst = ((unsigned int *)(&_end_stack)) - (8192 / sizeof(unsigned int)); ;
+        while ((unsigned int)dst < sp) {
+            *dst = 0xDEADC0DE;
+            dst++;
+        }
+    }
+#endif
+
+
+    main();
+}
+
+void isr_fault(void)
+{
+    /* Panic. */
+    while(1) ;;
+
+}
+
+void isr_memfault(void)
+{
+    /* Panic. */
+    while(1) ;;
+}
+
+void isr_busfault(void)
+{
+    /* Panic. */
+    while(1) ;;
+}
+
+void isr_usagefault(void)
+{
+    /* Panic. */
+    while(1) ;;
+}
+
+
+void isr_empty(void)
+{
+
+}
+
+
+
+__attribute__ ((section(".isr_vector")))
+void (* const IV[])(void) =
+{
+	(void (*)(void))(&_end_stack),
+	isr_reset,                   // Reset
+	isr_fault,                   // NMI
+	isr_fault,                   // HardFault
+	isr_memfault,                // MemFault
+	isr_busfault,                // BusFault
+	isr_usagefault,              // UsageFault
+	0,                           // SecureFault
+	0,                          // reserved
+	0,                          // reserved
+	0,                          // reserved
+	isr_empty,                   // SVC
+	isr_empty,                   // DebugMonitor
+	0,                           // reserved
+	isr_empty,                   // PendSV
+	isr_empty,                   // SysTick
+
+/* Device specific IRQs for LM3S */
+
+#ifdef LM3S
+    isr_empty,                     // GPIO Port A
+    isr_empty,                     // GPIO Port B
+    isr_empty,                     // GPIO Port C
+    isr_empty,                     // GPIO Port D
+    isr_empty,                     // GPIO Port E
+    isr_empty,                     // UART0 Rx and Tx
+    isr_empty,                     // UART1 Rx and Tx
+    isr_empty,                     // SSI0 Rx and Tx
+    isr_empty,                     // I2C0 Master and Slave
+    isr_empty,                     // PWM Fault
+    isr_empty,                     // PWM Generator 0
+    isr_empty,                     // PWM Generator 1
+    isr_empty,                     // PWM Generator 2
+    isr_empty,                     // Quadrature Encoder 0
+    isr_empty,                     // ADC Sequence 0
+    isr_empty,                     // ADC Sequence 1
+    isr_empty,                     // ADC Sequence 2
+    isr_empty,                     // ADC Sequence 3
+    isr_empty,                     // Watchdog timer
+    isr_empty,                     // Timer 0 subtimer A
+    isr_empty,                     // Timer 0 subtimer B
+    isr_empty,                     // Timer 1 subtimer A
+    isr_empty,                     // Timer 1 subtimer B
+    isr_empty,                     // Timer 2 subtimer A
+    isr_empty,                     // Timer 3 subtimer B
+    isr_empty,                     // Analog Comparator 0
+    isr_empty,                     // Analog Comparator 1
+    isr_empty,                     // Analog Comparator 2
+    isr_empty,                     // System Control (PLL, OSC, BO)
+    isr_empty,                     // FLASH Control
+    isr_empty,                     // GPIO Port F
+    isr_empty,                     // GPIO Port G
+    isr_empty,                     // GPIO Port H
+    isr_empty,                     // UART2 Rx and Tx
+    isr_empty,                     // SSI1 Rx and Tx
+    isr_empty,                     // Timer 3 subtimer A
+    isr_empty,                     // Timer 3 subtimer B
+    isr_empty,                     // I2C1 Master and Slave
+    isr_empty,                     // Quadrature Encoder 1
+    isr_empty,                     // CAN0
+    isr_empty,                     // CAN1
+    isr_empty,                     // CAN2
+    isr_empty,                     // Ethernet
+    isr_empty,                     // Hibernate
+#elif (defined(PLATFORM_stm32l5) ||defined(PLATFORM_stm32u5))   /* Fill with extra unused handlers */
+    isr_empty, //	WWDG_IRQHandler
+    isr_empty, //	PVD_PVM_IRQHandler
+    isr_empty, //	RTC_IRQHandler
+    isr_empty, //	RTC_S_IRQHandler
+    isr_empty, //	TAMP_IRQHandler
+    isr_empty, //	TAMP_S_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, //	DMAMUX1_IRQHandler
+    isr_empty, //	DMAMUX1_S_IRQHandler
+    isr_empty, //	DMA1_Channel1_IRQHandler
+    isr_empty, //	DMA1_Channel2_IRQHandler
+    isr_empty, //	DMA1_Channel3_IRQHandler
+    isr_empty, //	DMA1_Channel4_IRQHandler
+    isr_empty, //	DMA1_Channel5_IRQHandler
+    isr_empty, //	DMA1_Channel6_IRQHandler
+    isr_empty, //	DMA1_Channel7_IRQHandler
+    isr_empty, //	DMA1_Channel8_IRQHandler
+    isr_empty, //	ADC1_2_IRQHandler
+    isr_empty, //	DAC_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, //	TIM8_BRK_IRQHandler
+    isr_empty, //	TIM8_UP_IRQHandler
+    isr_empty, //	TIM8_TRG_COM_IRQHandler
+    isr_empty, //	TIM8_CC_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, //	USART1_IRQHandler
+    isr_empty, //	USART2_IRQHandler
+    isr_empty, //	USART3_IRQHandler
+    isr_empty, //	UART4_IRQHandler
+    isr_empty, //	UART5_IRQHandler
+    isr_empty, //	LPUART1_IRQHandler
+    isr_empty, //	LPTIM1_IRQHandler
+    isr_empty, //	LPTIM2_IRQHandler
+    isr_empty, //	TIM15_IRQHandler
+    isr_empty, //	TIM16_IRQHandler
+    isr_empty, //	TIM17_IRQHandler
+    isr_empty, //	COMP_IRQHandler
+    isr_empty, //	USB_FS_IRQHandler
+    isr_empty, //	CRS_IRQHandler
+    isr_empty, //	FMC_IRQHandler
+    isr_empty, //	OCTOSPI1_IRQHandler
+    isr_empty, //	0
+    isr_empty, //	SDMMC1_IRQHandler
+    isr_empty, //	0
+    isr_empty, //	DMA2_Channel1_IRQHandler
+    isr_empty, //	DMA2_Channel2_IRQHandler
+    isr_empty, //	DMA2_Channel3_IRQHandler
+    isr_empty, //	DMA2_Channel4_IRQHandler
+    isr_empty, //	DMA2_Channel5_IRQHandler
+    isr_empty, //	DMA2_Channel6_IRQHandler
+    isr_empty, //	DMA2_Channel7_IRQHandler
+    isr_empty, //	DMA2_Channel8_IRQHandler
+    isr_empty, //	I2C3_EV_IRQHandler
+    isr_empty, //	I2C3_ER_IRQHandler
+    isr_empty, //	SAI1_IRQHandler
+    isr_empty, //	SAI2_IRQHandler
+    isr_empty, //	TSC_IRQHandler
+    isr_empty, //	AES_IRQHandler
+    isr_empty, //	RNG_IRQHandler
+    isr_empty, //	FPU_IRQHandler
+    isr_empty, //	HASH_IRQHandler
+    isr_empty, //	PKA_IRQHandler
+    isr_empty, //	LPTIM3_IRQHandler
+    isr_empty, //	SPI3_IRQHandler
+    isr_empty, //	I2C4_ER_IRQHandler
+    isr_empty, //	I2C4_EV_IRQHandler
+    isr_empty, //	DFSDM1_FLT0_IRQHandler
+    isr_empty, //	DFSDM1_FLT1_IRQHandler
+    isr_empty, //	DFSDM1_FLT2_IRQHandler
+    isr_empty, //	DFSDM1_FLT3_IRQHandler
+    isr_empty, //	UCPD1_IRQHandler
+    isr_empty, //	ICACHE_IRQHandler
+    isr_empty, //	OTFDEC1_IRQHandler
+#elif defined(STM32) /* For STM32 */
+    isr_empty,              // NVIC_WWDG_IRQ 0
+    isr_empty,              // PVD_IRQ 1
+    isr_empty,              // TAMP_STAMP_IRQ 2
+    isr_empty,              // RTC_WKUP_IRQ 3
+    isr_empty,              // FLASH_IRQ 4
+    isr_empty,              // RCC_IRQ 5
+    isr_empty,              // EXTI0_IRQ 6
+    isr_empty,              // EXTI1_IRQ 7
+    isr_empty,              // EXTI2_IRQ 8
+    isr_empty,              // EXTI3_IRQ 9
+    isr_empty,              // EXTI4_IRQ 10
+    isr_empty,              // DMA1_STREAM0_IRQ 11
+    isr_empty,              // DMA1_STREAM1_IRQ 12
+    isr_empty,              // DMA1_STREAM2_IRQ 13
+    isr_empty,              // DMA1_STREAM3_IRQ 14
+    isr_empty,              // DMA1_STREAM4_IRQ 15
+    isr_empty,              // DMA1_STREAM5_IRQ 16
+    isr_empty,              // DMA1_STREAM6_IRQ 17
+    isr_empty,              // ADC_IRQ 18
+    isr_empty,              // CAN1_TX_IRQ 19
+    isr_empty,              // CAN1_RX0_IRQ 20
+    isr_empty,              // CAN1_RX1_IRQ 21
+    isr_empty,              // CAN1_SCE_IRQ 22
+    isr_empty,              // EXTI9_5_IRQ 23
+    isr_empty,              // TIM1_BRK_TIM9_IRQ 24
+    isr_empty,              // TIM1_UP_TIM10_IRQ 25
+    isr_empty,              // TIM1_TRG_COM_TIM11_IRQ 26
+    isr_empty,              // TIM1_CC_IRQ 27
+    isr_empty,               // TIM2_IRQ 28
+    isr_empty,              // TIM3_IRQ 29
+    isr_empty,              // TIM4_IRQ 30
+    isr_empty,              // I2C1_EV_IRQ 31
+    isr_empty,              // I2C1_ER_IRQ 32
+    isr_empty,              // I2C2_EV_IRQ 33
+    isr_empty,              // I2C2_ER_IRQ 34
+    isr_empty,              // SPI1_IRQ 35
+    isr_empty,              // SPI2_IRQ 36
+    isr_empty,              // USART1_IRQ 37
+    isr_empty,              // USART2_IRQ 38
+    isr_empty,              // USART3_IRQ 39
+    isr_empty,              // EXTI15_10_IRQ 40
+    isr_empty,              // RTC_ALARM_IRQ 41
+    isr_empty,              // USB_FS_WKUP_IRQ 42
+    isr_empty,              // TIM8_BRK_TIM12_IRQ 43
+    isr_empty,              // TIM8_UP_TIM13_IRQ 44
+    isr_empty,              // TIM8_TRG_COM_TIM14_IRQ 45
+    isr_empty,              // TIM8_CC_IRQ 46
+    isr_empty,              // DMA1_STREAM7_IRQ 47
+
+#endif
+};
diff --git a/tools/keytools/otp/target.ld b/tools/keytools/otp/target.ld
new file mode 100644
index 000000000..466a89305
--- /dev/null
+++ b/tools/keytools/otp/target.ld
@@ -0,0 +1,45 @@
+MEMORY
+{
+    FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 0x20000
+    RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 0x00020000 /* mapping Low-memory only */
+}
+
+SECTIONS
+{
+    .text :
+    {
+        _start_text = .;
+        KEEP(*(.isr_vector))
+        *(.init)
+        *(.fini)
+        *(.text*)
+        *(.rodata*)
+        . = ALIGN(4);
+        _end_text = .;
+    } > 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*)
+        *(COMMON)
+        . = ALIGN(4);
+        _end_bss = .;
+        _end = .;
+    } > RAM
+}
+
+PROVIDE(_start_heap = _end);
+PROVIDE(_end_stack  = ORIGIN(RAM) + LENGTH(RAM));

From 5f2a3d5ac7327029c53f21fc4256807858be1815 Mon Sep 17 00:00:00 2001
From: Daniele Lacamera <dan@danielinux.net>
Date: Mon, 13 May 2024 18:17:21 +0200
Subject: [PATCH 06/19] Fix pointer increment

---
 hal/stm32h7.c | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/hal/stm32h7.c b/hal/stm32h7.c
index 3c1b92dff..f5e7475c7 100644
--- a/hal/stm32h7.c
+++ b/hal/stm32h7.c
@@ -547,6 +547,7 @@ int hal_flash_otp_write(uint32_t flashAddress, const void* data, uint16_t length
 {
     volatile uint16_t tmp;
     uint16_t idx = 0;
+    const uint16_t *pdata = (const uint16_t *)data;
     if (!(flashAddress >= FLASH_OTP_BASE && flashAddress <= FLASH_OTP_END)) {
         return -1;
     }
@@ -569,13 +570,11 @@ int hal_flash_otp_write(uint32_t flashAddress, const void* data, uint16_t length
         DSB();
 
         /* Program an OTP word (16 bits) */
-        *(volatile uint16_t*)flashAddress = *(const uint16_t*)data;
+        *(volatile uint16_t*)flashAddress = *pdata;
 
-#if 0
         /* Read it back */
         tmp = *(volatile uint16_t*)flashAddress;
         (void)tmp; /* avoid unused warnings */
-#endif
 
         /* Wait for last operation to be completed */
         flash_otp_wait();
@@ -584,7 +583,7 @@ int hal_flash_otp_write(uint32_t flashAddress, const void* data, uint16_t length
         FLASH_OPTCR &= ~FLASH_OPTCR_PG_OTP;
 
         flashAddress += sizeof(uint16_t);
-        data++;
+        pdata++;
         idx += sizeof(uint16_t);
     }
 
@@ -596,6 +595,7 @@ int hal_flash_otp_write(uint32_t flashAddress, const void* data, uint16_t length
 int hal_flash_otp_read(uint32_t flashAddress, void* data, uint32_t length)
 {
     uint32_t i;
+    uint16_t *pdata = (uint16_t *)data;
     if (!(flashAddress >= FLASH_OTP_BASE && flashAddress <= FLASH_OTP_END)) {
         return -1;
     }
@@ -603,9 +603,9 @@ int hal_flash_otp_read(uint32_t flashAddress, void* data, uint32_t length)
         (i < length) && (flashAddress <= (FLASH_OTP_END-1));
         i += sizeof(uint16_t))
     {
-        *(uint16_t *)data = *(volatile uint16_t*)flashAddress;
+        *pdata = *(volatile uint16_t*)flashAddress;
         flashAddress += sizeof(uint16_t);
-        data++;
+        pdata++;
     }
     return 0;
 }

From 02bfe8a6a1c9f12e89a8293aa668a04d1e10c68c Mon Sep 17 00:00:00 2001
From: Daniele Lacamera <dan@danielinux.net>
Date: Tue, 14 May 2024 18:02:37 +0200
Subject: [PATCH 07/19] Added OTP support for STM32H5

---
 hal/stm32h5.c                            | 96 ++++++++++++++++++++++--
 hal/stm32h5.h                            | 13 +++-
 include/keystore.h                       |  4 +-
 include/otp_keystore.h                   |  2 +
 include/wolfboot/wolfboot.h              |  4 +
 src/flash_otp_keystore.c                 |  2 +-
 tools/keytools/otp/Makefile              |  7 +-
 tools/keytools/otp/otp-keystore-primer.c |  4 +
 8 files changed, 118 insertions(+), 14 deletions(-)

diff --git a/hal/stm32h5.c b/hal/stm32h5.c
index 08f126e2f..2dd704f8f 100644
--- a/hal/stm32h5.c
+++ b/hal/stm32h5.c
@@ -37,7 +37,7 @@ static void RAMFUNCTION flash_set_waitstates(unsigned int waitstates)
         while ((FLASH_ACR & FLASH_ACR_LATENCY_MASK) != waitstates);
 }
 
-void RAMFUNCTION hal_flash_wait_complete(uint8_t bank)
+static void RAMFUNCTION hal_flash_wait_complete(void)
 {
     while ((FLASH_SR & FLASH_SR_BSY) == FLASH_SR_BSY)
         ;
@@ -48,6 +48,17 @@ void RAMFUNCTION hal_flash_wait_complete(uint8_t bank)
 
 }
 
+static void RAMFUNCTION hal_flash_wait_buffer_empty(void)
+{
+    while ((FLASH_SR & FLASH_SR_DBNE) == FLASH_SR_DBNE)
+        ;
+#if (TZ_SECURE())
+    while ((FLASH_NS_SR & FLASH_SR_DBNE) == FLASH_SR_DBNE)
+        ;
+#endif
+
+}
+
 void RAMFUNCTION hal_flash_clear_errors(uint8_t bank)
 {
     FLASH_CCR |= ( FLASH_CCR_CLR_WBNE | FLASH_CCR_CLR_DBNE | FLASH_CCR_CLR_INCE|
@@ -85,7 +96,7 @@ int RAMFUNCTION hal_flash_write(uint32_t address, const uint8_t *data, int len)
         dst[i >> 2] = dword[0];
         ISB();
         dst[(i >> 2) + 1] = dword[1];
-        hal_flash_wait_complete(0);
+        hal_flash_wait_complete();
         if ((*sr & FLASH_SR_EOP) != 0)
             *sr |= FLASH_SR_EOP;
         *cr &= ~FLASH_CR_PG;
@@ -99,7 +110,7 @@ int RAMFUNCTION hal_flash_write(uint32_t address, const uint8_t *data, int len)
 
 void RAMFUNCTION hal_flash_unlock(void)
 {
-    hal_flash_wait_complete(0);
+    hal_flash_wait_complete();
     if ((FLASH_CR & FLASH_CR_LOCK) != 0) {
         FLASH_KEYR = FLASH_KEY1;
         DMB();
@@ -112,14 +123,14 @@ void RAMFUNCTION hal_flash_unlock(void)
 
 void RAMFUNCTION hal_flash_lock(void)
 {
-    hal_flash_wait_complete(0);
+    hal_flash_wait_complete();
     if ((FLASH_CR & FLASH_CR_LOCK) == 0)
         FLASH_CR |= FLASH_CR_LOCK;
 }
 
 void RAMFUNCTION hal_flash_opt_unlock(void)
 {
-    hal_flash_wait_complete(0);
+    hal_flash_wait_complete();
     if ((FLASH_OPTCR & FLASH_OPTCR_OPTLOCK) != 0) {
         FLASH_OPTKEYR = FLASH_OPTKEY1;
         DMB();
@@ -134,7 +145,7 @@ void RAMFUNCTION hal_flash_opt_unlock(void)
 void RAMFUNCTION hal_flash_opt_lock(void)
 {
     FLASH_OPTCR |= FLASH_OPTCR_OPTSTRT;
-    hal_flash_wait_complete(0);
+    hal_flash_wait_complete();
     if ((FLASH_OPTCR & FLASH_OPTCR_OPTLOCK) == 0)
         FLASH_OPTCR |= FLASH_OPTCR_OPTLOCK;
 }
@@ -149,7 +160,7 @@ int RAMFUNCTION hal_flash_erase(uint32_t address, int len)
     if (len == 0)
         return -1;
 
-    if (address < ARCH_FLASH_OFFSET)
+    if (address < 0x08000000)
         return -1;
 
     end_address = address + len - 1;
@@ -176,7 +187,7 @@ int RAMFUNCTION hal_flash_erase(uint32_t address, int len)
         FLASH_CR = reg;
         DMB();
         FLASH_CR |= FLASH_CR_STRT;
-        hal_flash_wait_complete(0);
+        hal_flash_wait_complete();
     }
     /* If the erase operation is completed, disable the associated bits */
     FLASH_CR &= ~FLASH_CR_SER ;
@@ -421,3 +432,72 @@ void hal_prepare_boot(void)
 #endif
 }
 
+#ifdef FLASH_OTP_ROT
+
+/* Public API */
+
+int hal_flash_otp_write(uint32_t flashAddress, const void* data, uint16_t length)
+{
+    volatile uint16_t tmp;
+    uint16_t *pdata = (uint16_t *)data;
+    uint16_t idx = 0, len_align;
+    uint16_t last_word;
+    if (!(flashAddress >= FLASH_OTP_BASE && flashAddress <= FLASH_OTP_END)) {
+        return -1;
+    }
+
+    hal_flash_wait_complete();
+    hal_flash_wait_buffer_empty();
+    hal_flash_unlock();
+    hal_flash_clear_errors(0);
+
+
+    /* Truncate to 2B alignment */
+    length = (length / 2 * 2);
+
+    while (idx < length && flashAddress <= FLASH_OTP_END-1) {
+        hal_flash_wait_complete();
+        /* Set PG bit */
+        FLASH_CR |= FLASH_CR_PG;
+        /* Program an OTP word (32 bits) */
+        *(volatile uint16_t*)flashAddress = *pdata;
+        ISB();
+        DSB();
+        /* Read it back */
+        tmp = *(volatile uint16_t*)flashAddress;
+        if (tmp != *pdata) {
+            /* Provisioning failed. OTP already programmed? */
+            while(1)
+                ;
+        }
+
+        /* Clear PG bit */
+        FLASH_CR &= ~FLASH_CR_PG;
+        flashAddress += sizeof(uint16_t);
+        pdata++;
+        idx += sizeof(uint16_t);
+    }
+
+    hal_flash_lock();
+    return 0;
+}
+
+int hal_flash_otp_read(uint32_t flashAddress, void* data, uint32_t length)
+{
+    uint16_t i;
+    uint16_t *pdata = (uint16_t *)data;
+    if (!(flashAddress >= FLASH_OTP_BASE && flashAddress <= FLASH_OTP_END)) {
+        return -1;
+    }
+    for (i = 0;
+        (i < length) && (flashAddress <= (FLASH_OTP_END-1));
+        i += sizeof(uint16_t))
+    {
+        *pdata = *(volatile uint16_t*)flashAddress;
+        flashAddress += sizeof(uint16_t);
+        pdata++;
+    }
+    return 0;
+}
+
+#endif /* FLASH_OTP_ROT */
diff --git a/hal/stm32h5.h b/hal/stm32h5.h
index 16a698639..5e64222ad 100644
--- a/hal/stm32h5.h
+++ b/hal/stm32h5.h
@@ -355,18 +355,25 @@
 #define RCC_APB2_CLOCK_ER (*(volatile uint32_t *)(RCC_BASE + 0xA4))
 #define UART1_APB2_CLOCK_ER_VAL (1 << 14)
 
+
+/* OTP FLASH AREA */
+#define FLASH_OTP_BASE 0x08FFF000
+#define FLASH_OTP_END  0x08FFF7FF
+#define OTP_SIZE             2048
+#define OTP_BLOCKS             32
+
+/* UART1 pin configuration */
 #define UART1_PIN_AF 8
 #define UART1_RX_PIN 8
 #define UART1_TX_PIN 7
 
+/* GPIO secure configuration */
 #define GPIO_SECCFGR(base) (*(volatile uint32_t *)(base + 0x30))
-
-
-
 #define LED_AHB2_ENABLE (GPIOG_AHB2_CLOCK_ER | GPIOB_AHB2_CLOCK_ER | \
         GPIOF_AHB2_CLOCK_ER)
 #define LED_BOOT_PIN (4)  /* PG4 - Nucleo board - Orange Led */
 #define LED_USR_PIN (0)   /* PB0  - Nucleo board  - Green Led */
 #define LED_EXTRA_PIN (4) /* PF4 - Nucleo board - Blue Led */
 
+
 #endif /* STM32H5_DEF_INCLUDED */
diff --git a/include/keystore.h b/include/keystore.h
index a2cfc91e1..12882579c 100644
--- a/include/keystore.h
+++ b/include/keystore.h
@@ -35,6 +35,7 @@ extern "C" {
     #define KEYSTORE_PUBKEY_SIZE 576 /* Max is RSA 4096 */
 #endif
 
+
 struct keystore_slot {
     uint32_t slot_id;
     uint32_t key_type;
@@ -43,7 +44,8 @@ struct keystore_slot {
     uint8_t  pubkey[KEYSTORE_PUBKEY_SIZE];
 };
 
-#define SIZEOF_KEYSTORE_SLOT (32 + KEYSTORE_PUBKEY_SIZE)
+#define KEYSTORE_HDR_SIZE 16
+#define SIZEOF_KEYSTORE_SLOT (KEYSTORE_HDR_SIZE + KEYSTORE_PUBKEY_SIZE)
 
 /* KeyStore API */
 int keystore_num_pubkeys(void);
diff --git a/include/otp_keystore.h b/include/otp_keystore.h
index 2f8a47030..6a5499708 100644
--- a/include/otp_keystore.h
+++ b/include/otp_keystore.h
@@ -32,6 +32,8 @@
  */
 #ifdef TARGET_stm32h7
     #include "hal/stm32h7.h"
+#elif defined TARGET_stm32h5
+    #include "hal/stm32h5.h"
 #else
     #error "Unsupported target for OTP"
 #endif
diff --git a/include/wolfboot/wolfboot.h b/include/wolfboot/wolfboot.h
index e6be66fd9..529b98400 100644
--- a/include/wolfboot/wolfboot.h
+++ b/include/wolfboot/wolfboot.h
@@ -175,6 +175,10 @@ extern "C" {
     #endif
 #endif
 
+#endif
+
+#if defined __WOLFBOOT || defined __FLASH_OTP_PRIMER
+
  /* Authentication configuration */
  #if defined(WOLFBOOT_NO_SIGN)
  #   define HDR_IMG_TYPE_AUTH HDR_IMG_TYPE_AUTH_NONE
diff --git a/src/flash_otp_keystore.c b/src/flash_otp_keystore.c
index b5cca0b0b..d7db68115 100644
--- a/src/flash_otp_keystore.c
+++ b/src/flash_otp_keystore.c
@@ -45,7 +45,7 @@ int keystore_num_pubkeys(void)
     return hdr->item_count;
 }
 
-static uint16_t otp_slot_item_cache[SIZEOF_KEYSTORE_SLOT/2];
+static uint8_t otp_slot_item_cache[SIZEOF_KEYSTORE_SLOT];
 
 uint8_t *keystore_get_buffer(int id)
 {
diff --git a/tools/keytools/otp/Makefile b/tools/keytools/otp/Makefile
index 5141c61a2..0d91e57b2 100644
--- a/tools/keytools/otp/Makefile
+++ b/tools/keytools/otp/Makefile
@@ -9,7 +9,7 @@ CROSS_COMPILE?=arm-none-eabi-
 CFLAGS+=-O0 -ggdb
 CFLAGS+=-I. -I../../../ -I../../../include
 CFLAGS+=-I./wcs
-CFLAGS+=-DFLASH_OTP_ROT
+CFLAGS+=-DFLASH_OTP_ROT -D__FLASH_OTP_PRIMER
 OBJS+=startup.o otp-keystore-primer.o ../../../src/keystore.o
 LSCRIPT=target.ld
 LDFLAGS+=$(CFLAGS) -T$(LSCRIPT) -lc -Wl,-Map=otp-keystore-primer.map
@@ -19,6 +19,11 @@ ifeq ($(TARGET),stm32h7)
     CFLAGS+=-mcpu=cortex-m7 -ffunction-sections -fdata-sections -fno-common -ffreestanding -nostartfiles
     OBJS+=../../../hal/stm32h7.o
 endif
+ifeq ($(TARGET),stm32h5)
+    CFLAGS+=-DTARGET_stm32h5
+    CFLAGS+=-mcpu=cortex-m33 -ffunction-sections -fdata-sections -fno-common -ffreestanding -nostartfiles
+    OBJS+=../../../hal/stm32h5.o
+endif
 CC=$(CROSS_COMPILE)gcc
 OBJCOPY?=$(CROSS_COMPILE)objcopy
 SIZE?=$(CROSS_COMPILE)size
diff --git a/tools/keytools/otp/otp-keystore-primer.c b/tools/keytools/otp/otp-keystore-primer.c
index fc4a31d3c..d64f2ebb6 100644
--- a/tools/keytools/otp/otp-keystore-primer.c
+++ b/tools/keytools/otp/otp-keystore-primer.c
@@ -23,6 +23,7 @@
  */
 #include <stdint.h>
 #include <string.h>
+#include "wolfboot/wolfboot.h"
 #include "hal.h"
 #include "otp_keystore.h"
 
@@ -33,6 +34,9 @@ void main(void)
     int n_keys = keystore_num_pubkeys();
     int i;
     struct wolfBoot_otp_hdr hdr;
+
+    hal_init();
+
     memcpy(hdr.keystore_hdr_magic, KEYSTORE_HDR_MAGIC, 8);
     hdr.item_count = n_keys;
     hdr.flags = 0;

From 34652ffebfaafb0590e67ddb77f57c1f878c732d Mon Sep 17 00:00:00 2001
From: Daniele Lacamera <dan@danielinux.net>
Date: Tue, 14 May 2024 18:41:20 +0200
Subject: [PATCH 08/19] Reverted hal_flash_ functions moved to static

---
 hal/stm32h5.c | 22 +++++++++++-----------
 1 file changed, 11 insertions(+), 11 deletions(-)

diff --git a/hal/stm32h5.c b/hal/stm32h5.c
index 2dd704f8f..e3a030cb3 100644
--- a/hal/stm32h5.c
+++ b/hal/stm32h5.c
@@ -37,7 +37,7 @@ static void RAMFUNCTION flash_set_waitstates(unsigned int waitstates)
         while ((FLASH_ACR & FLASH_ACR_LATENCY_MASK) != waitstates);
 }
 
-static void RAMFUNCTION hal_flash_wait_complete(void)
+void RAMFUNCTION hal_flash_wait_complete(uint8_t bank)
 {
     while ((FLASH_SR & FLASH_SR_BSY) == FLASH_SR_BSY)
         ;
@@ -48,7 +48,7 @@ static void RAMFUNCTION hal_flash_wait_complete(void)
 
 }
 
-static void RAMFUNCTION hal_flash_wait_buffer_empty(void)
+static void RAMFUNCTION hal_flash_wait_buffer_empty(uint8_t bank)
 {
     while ((FLASH_SR & FLASH_SR_DBNE) == FLASH_SR_DBNE)
         ;
@@ -96,7 +96,7 @@ int RAMFUNCTION hal_flash_write(uint32_t address, const uint8_t *data, int len)
         dst[i >> 2] = dword[0];
         ISB();
         dst[(i >> 2) + 1] = dword[1];
-        hal_flash_wait_complete();
+        hal_flash_wait_complete(0);
         if ((*sr & FLASH_SR_EOP) != 0)
             *sr |= FLASH_SR_EOP;
         *cr &= ~FLASH_CR_PG;
@@ -110,7 +110,7 @@ int RAMFUNCTION hal_flash_write(uint32_t address, const uint8_t *data, int len)
 
 void RAMFUNCTION hal_flash_unlock(void)
 {
-    hal_flash_wait_complete();
+    hal_flash_wait_complete(0);
     if ((FLASH_CR & FLASH_CR_LOCK) != 0) {
         FLASH_KEYR = FLASH_KEY1;
         DMB();
@@ -123,14 +123,14 @@ void RAMFUNCTION hal_flash_unlock(void)
 
 void RAMFUNCTION hal_flash_lock(void)
 {
-    hal_flash_wait_complete();
+    hal_flash_wait_complete(0);
     if ((FLASH_CR & FLASH_CR_LOCK) == 0)
         FLASH_CR |= FLASH_CR_LOCK;
 }
 
 void RAMFUNCTION hal_flash_opt_unlock(void)
 {
-    hal_flash_wait_complete();
+    hal_flash_wait_complete(0);
     if ((FLASH_OPTCR & FLASH_OPTCR_OPTLOCK) != 0) {
         FLASH_OPTKEYR = FLASH_OPTKEY1;
         DMB();
@@ -145,7 +145,7 @@ void RAMFUNCTION hal_flash_opt_unlock(void)
 void RAMFUNCTION hal_flash_opt_lock(void)
 {
     FLASH_OPTCR |= FLASH_OPTCR_OPTSTRT;
-    hal_flash_wait_complete();
+    hal_flash_wait_complete(0);
     if ((FLASH_OPTCR & FLASH_OPTCR_OPTLOCK) == 0)
         FLASH_OPTCR |= FLASH_OPTCR_OPTLOCK;
 }
@@ -187,7 +187,7 @@ int RAMFUNCTION hal_flash_erase(uint32_t address, int len)
         FLASH_CR = reg;
         DMB();
         FLASH_CR |= FLASH_CR_STRT;
-        hal_flash_wait_complete();
+        hal_flash_wait_complete(0);
     }
     /* If the erase operation is completed, disable the associated bits */
     FLASH_CR &= ~FLASH_CR_SER ;
@@ -446,8 +446,8 @@ int hal_flash_otp_write(uint32_t flashAddress, const void* data, uint16_t length
         return -1;
     }
 
-    hal_flash_wait_complete();
-    hal_flash_wait_buffer_empty();
+    hal_flash_wait_complete(0);
+    hal_flash_wait_buffer_empty(0);
     hal_flash_unlock();
     hal_flash_clear_errors(0);
 
@@ -456,7 +456,7 @@ int hal_flash_otp_write(uint32_t flashAddress, const void* data, uint16_t length
     length = (length / 2 * 2);
 
     while (idx < length && flashAddress <= FLASH_OTP_END-1) {
-        hal_flash_wait_complete();
+        hal_flash_wait_complete(0);
         /* Set PG bit */
         FLASH_CR |= FLASH_CR_PG;
         /* Program an OTP word (32 bits) */

From 9dd8b7cf976f310ed1db1699bf5c2a1645cf861c Mon Sep 17 00:00:00 2001
From: Daniele Lacamera <dan@danielinux.net>
Date: Wed, 15 May 2024 09:48:23 +0200
Subject: [PATCH 09/19] Renamed FLASH_OTP_ROT to FLASH_OTP_KEYSTORE

---
 Makefile                    |  8 ++++----
 hal/stm32h5.c               |  4 ++--
 hal/stm32h7.c               |  4 ++--
 hal/stm32h7.h               | 28 +++++++++++++---------------
 include/hal.h               |  2 +-
 include/otp_keystore.h      |  4 ++--
 options.mk                  |  4 ++--
 src/flash_otp_keystore.c    |  4 ++--
 tools/config.mk             |  4 ++--
 tools/keytools/otp/Makefile |  2 +-
 10 files changed, 31 insertions(+), 33 deletions(-)

diff --git a/Makefile b/Makefile
index 7c0d5f93f..a36936d93 100644
--- a/Makefile
+++ b/Makefile
@@ -35,7 +35,7 @@ ifeq ($(SIGN),NONE)
   PRIVATE_KEY=
 else
   PRIVATE_KEY=wolfboot_signing_private_key.der
-  ifeq ($(FLASH_OTP_ROT),1)
+  ifeq ($(FLASH_OTP_KEYSTORE),1)
         OBJS+=./src/flash_otp_keystore.o
   else
         OBJS+=./src/keystore.o
@@ -127,7 +127,7 @@ ifeq ($(TARGET),nxp_t1024)
 	MAIN_TARGET:=factory_wstage1.bin
 endif
 
-ifeq ($(FLASH_OTP_ROT),1)
+ifeq ($(FLASH_OTP_KEYSTORE),1)
 	MAIN_TARGET:=include/target.h tools/keytools/otp/otp-keystore-primer factory.bin
 endif
 
@@ -188,7 +188,7 @@ $(PRIVATE_KEY):
 	$(Q)$(MAKE) keytools_check
 	$(Q)(test $(SIGN) = NONE) || ("$(KEYGEN_TOOL)" $(KEYGEN_OPTIONS) -g $(PRIVATE_KEY)) || true
 	$(Q)(test $(SIGN) = NONE) && (echo "// SIGN=NONE" >  src/keystore.c) || true
-	$(Q)(test $(FLASH_OTP_ROT) = 0) || (make -C tools/keytools/otp) || true
+	$(Q)(test $(FLASH_OTP_KEYSTORE) = 0) || (make -C tools/keytools/otp) || true
 
 keytools: include/target.h
 	@echo "Building key tools"
@@ -244,7 +244,7 @@ wolfboot_stage1.bin: wolfboot.elf stage1/loader_stage1.bin
 	$(Q) cp stage1/loader_stage1.bin wolfboot_stage1.bin
 
 wolfboot.elf: include/target.h $(LSCRIPT) $(OBJS) $(LIBS) $(BINASSEMBLE) FORCE
-	$(Q)(test $(SIGN) = NONE) || (test $(FLASH_OTP_ROT) = 1) || (grep -q $(SIGN_ALG) src/keystore.c) || \
+	$(Q)(test $(SIGN) = NONE) || (test $(FLASH_OTP_KEYSTORE) = 1) || (grep -q $(SIGN_ALG) src/keystore.c) || \
 		(echo "Key mismatch: please run 'make distclean' to remove all keys if you want to change algorithm" && false)
 	@echo "\t[LD] $@"
 	@echo $(OBJS)
diff --git a/hal/stm32h5.c b/hal/stm32h5.c
index e3a030cb3..22841ec42 100644
--- a/hal/stm32h5.c
+++ b/hal/stm32h5.c
@@ -432,7 +432,7 @@ void hal_prepare_boot(void)
 #endif
 }
 
-#ifdef FLASH_OTP_ROT
+#ifdef FLASH_OTP_KEYSTORE
 
 /* Public API */
 
@@ -500,4 +500,4 @@ int hal_flash_otp_read(uint32_t flashAddress, void* data, uint32_t length)
     return 0;
 }
 
-#endif /* FLASH_OTP_ROT */
+#endif /* FLASH_OTP_KEYSTORE */
diff --git a/hal/stm32h7.c b/hal/stm32h7.c
index f5e7475c7..6d8fef9ed 100644
--- a/hal/stm32h7.c
+++ b/hal/stm32h7.c
@@ -513,7 +513,7 @@ void hal_prepare_boot(void)
     clock_pll_off();
 }
 
-#ifdef FLASH_OTP_ROT
+#ifdef FLASH_OTP_KEYSTORE
 static void flash_otp_wait(void)
 {
     /* Wait for the FLASH operation to complete by polling on QW flag to be reset. */
@@ -610,5 +610,5 @@ int hal_flash_otp_read(uint32_t flashAddress, void* data, uint32_t length)
     return 0;
 }
 
-#endif /* FLASH_OTP_ROT */
+#endif /* FLASH_OTP_KEYSTORE */
 
diff --git a/hal/stm32h7.h b/hal/stm32h7.h
index 894e276f7..7cfb50de9 100644
--- a/hal/stm32h7.h
+++ b/hal/stm32h7.h
@@ -279,22 +279,20 @@
 #define FLASH_OPT_KEY1                      (0x08192A3BU)
 #define FLASH_OPT_KEY2                      (0x4C5D6E7FU)
 
-#ifdef FLASH_OTP_ROT
-    #ifndef FLASH_OTP_BASE
-    #define FLASH_OTP_BASE 0x08FFF000
-    #endif
-    #ifndef FLASH_OTP_END
-    #define FLASH_OTP_END  0x08FFF3FF
-    #endif
-    #ifndef OTP_SIZE
-    #define OTP_SIZE       1024
-    #endif
-    #ifndef OTP_BLOCKS
-    #define OTP_BLOCKS     16
-    #endif
-
-    #define OTP_BLOCK_SIZE (OTP_SIZE / OTP_BLOCKS) /* 64 bytes */
+#ifndef FLASH_OTP_BASE
+#define FLASH_OTP_BASE 0x08FFF000
 #endif
+#ifndef FLASH_OTP_END
+#define FLASH_OTP_END  0x08FFF3FF
+#endif
+#ifndef OTP_SIZE
+#define OTP_SIZE       1024
+#endif
+#ifndef OTP_BLOCKS
+#define OTP_BLOCKS     16
+#endif
+
+#define OTP_BLOCK_SIZE (OTP_SIZE / OTP_BLOCKS) /* 64 bytes */
 
 /* STM32H7: Due to ECC functionality, it is not possible to write partition/sector
  * flags and signature more than once. This flags_cache is used to intercept write operations and
diff --git a/include/hal.h b/include/hal.h
index 73d2db1dd..d42409af9 100644
--- a/include/hal.h
+++ b/include/hal.h
@@ -130,7 +130,7 @@ int hal_trng_get_entropy(unsigned char *out, unsigned len);
 
 #endif
 
-#ifdef FLASH_OTP_ROT
+#ifdef FLASH_OTP_KEYSTORE
 
 int hal_flash_otp_write(uint32_t flashAddress, const void* data, uint16_t length);
 int hal_flash_otp_read(uint32_t flashAddress, void* data, uint32_t length);
diff --git a/include/otp_keystore.h b/include/otp_keystore.h
index 6a5499708..f03be4673 100644
--- a/include/otp_keystore.h
+++ b/include/otp_keystore.h
@@ -26,7 +26,7 @@
 #ifndef OTP_KEYSTORE_H
 #define OTP_KEYSTORE_H
 
-#if defined(FLASH_OTP_ROT) && !defined(WOLFBOOT_NO_SIGN)
+#if defined(FLASH_OTP_KEYSTORE) && !defined(WOLFBOOT_NO_SIGN)
 /* Specific includes for supported targets
  * (needed for OTP_SIZE)
  */
@@ -67,6 +67,6 @@ static const char KEYSTORE_HDR_MAGIC[8] = "WOLFBOOT";
 
 #endif /* KEYSTORE_ANY */
 
-#endif /* FLASH_OTP_ROT */ 
+#endif /* FLASH_OTP_KEYSTORE */ 
 
 #endif /* OTP_KEYSTORE_H */
diff --git a/options.mk b/options.mk
index 2d2e9af68..422d9339b 100644
--- a/options.mk
+++ b/options.mk
@@ -1,8 +1,8 @@
 WOLFCRYPT_OBJS+=./lib/wolfssl/wolfcrypt/src/asn.o
 
 # Support for Built-in ROT into OTP flash memory
-ifeq ($(FLASH_OTP_ROT),1)
-    CFLAGS+=-D"FLASH_OTP_ROT"
+ifeq ($(FLASH_OTP_KEYSTORE),1)
+    CFLAGS+=-D"FLASH_OTP_KEYSTORE"
 endif
 
 # Support for TPM signature verification
diff --git a/src/flash_otp_keystore.c b/src/flash_otp_keystore.c
index d7db68115..671d61a29 100644
--- a/src/flash_otp_keystore.c
+++ b/src/flash_otp_keystore.c
@@ -29,7 +29,7 @@
 #include "hal.h"
 #include "otp_keystore.h"
 
-#if defined(FLASH_OTP_ROT) && !defined(WOLFBOOT_NO_SIGN)
+#if defined(FLASH_OTP_KEYSTORE) && !defined(WOLFBOOT_NO_SIGN)
 
 int keystore_num_pubkeys(void)
 {
@@ -100,4 +100,4 @@ uint32_t keystore_get_key_type(int id)
 }
 
 
-#endif /* FLASH_OTP_ROT && !WOLFBOOT_NO_SIGN */
+#endif /* FLASH_OTP_KEYSTORE && !WOLFBOOT_NO_SIGN */
diff --git a/tools/config.mk b/tools/config.mk
index 731eea25f..4b1e8e88f 100644
--- a/tools/config.mk
+++ b/tools/config.mk
@@ -78,7 +78,7 @@ ifeq ($(ARCH),)
   FORCE_32BIT=0
   DISK_LOCK?=0
   DISK_LOCK_PASSWORD?=
-  FLASH_OTP_ROT?=0
+  FLASH_OTP_KEYSTORE?=0
 endif
 
 CONFIG_VARS:= ARCH TARGET SIGN HASH MCUXSDK MCUXPRESSO MCUXPRESSO_CPU MCUXPRESSO_DRIVERS \
@@ -100,4 +100,4 @@ CONFIG_VARS:= ARCH TARGET SIGN HASH MCUXSDK MCUXPRESSO MCUXPRESSO_CPU MCUXPRESSO
 	XMSS_PARAMS \
 	ELF \
 	NXP_CUSTOM_DCD NXP_CUSTOM_DCD_OBJS \
-	FLASH_OTP_ROT
+	FLASH_OTP_KEYSTORE
diff --git a/tools/keytools/otp/Makefile b/tools/keytools/otp/Makefile
index 0d91e57b2..807d4e572 100644
--- a/tools/keytools/otp/Makefile
+++ b/tools/keytools/otp/Makefile
@@ -9,7 +9,7 @@ CROSS_COMPILE?=arm-none-eabi-
 CFLAGS+=-O0 -ggdb
 CFLAGS+=-I. -I../../../ -I../../../include
 CFLAGS+=-I./wcs
-CFLAGS+=-DFLASH_OTP_ROT -D__FLASH_OTP_PRIMER
+CFLAGS+=-DFLASH_OTP_KEYSTORE -D__FLASH_OTP_PRIMER
 OBJS+=startup.o otp-keystore-primer.o ../../../src/keystore.o
 LSCRIPT=target.ld
 LDFLAGS+=$(CFLAGS) -T$(LSCRIPT) -lc -Wl,-Map=otp-keystore-primer.map

From fd80688a8e3805ebf58a9a2670d7a9d8352c6119 Mon Sep 17 00:00:00 2001
From: Daniele Lacamera <dan@danielinux.net>
Date: Wed, 15 May 2024 10:47:55 +0200
Subject: [PATCH 10/19] Added documentation on FLASH_OTP_KEYSTORE

---
 docs/Targets.md              |  5 ++++
 docs/compile.md              |  4 ++++
 docs/flash-OTP.md            | 44 ++++++++++++++++++++++++++++++++++++
 tools/keytools/otp/README.md |  6 +++++
 4 files changed, 59 insertions(+)
 create mode 100644 docs/flash-OTP.md
 create mode 100644 tools/keytools/otp/README.md

diff --git a/docs/Targets.md b/docs/Targets.md
index d26eaa520..53ace4a8e 100644
--- a/docs/Targets.md
+++ b/docs/Targets.md
@@ -850,6 +850,11 @@ arm-none-eabi-gdb
 Like [STM32L5](#stm32l5) and [STM32U5](#stm32u5), STM32H5 support is also demonstrated
 through different scenarios.
 
+Additionally, wolfBoot can be compiled with `FLASH_OTP_KEYSTORE` option, to store
+the public key(s) used for firmware authentication into a dedicated, one-time
+programmable flash area that can be write protected.
+For more information, see [/docs/flash-OTP.md](/docs/flash-OTP.md).
+
 ### Scenario 1: TrustZone enabled, staging non-secure application
 
 #### Example description
diff --git a/docs/compile.md b/docs/compile.md
index b58c39800..0cf28afe7 100644
--- a/docs/compile.md
+++ b/docs/compile.md
@@ -279,6 +279,10 @@ You can also manually override the fill bytes using `FILL_BYTE=` at build-time.
 
 Note: if you are using an external FLASH (e.g. SPI) in combination with a flash with inverted logic, ensure that you store all the flags in one partition, by using the `FLAGS_HOME=1` option described above.
 
+### Using One-time programmable (OTP) flash as keystore
+
+By default, keys are directly incorporated in the firmware image. To store the keys in a separate, one-time programmable (OTP) flash memory, use the `FLASH_OTP_KEYSTORE=1` option.
+For more information, see [/docs/OTP-keystore.md](/docs/OTP-keystore.md).
 
 ### Using Mac OS/X
 
diff --git a/docs/flash-OTP.md b/docs/flash-OTP.md
new file mode 100644
index 000000000..140012319
--- /dev/null
+++ b/docs/flash-OTP.md
@@ -0,0 +1,44 @@
+## Using One-Time Programmable (OTP) flash area for keystore
+
+Some microcontrollers provide a special area in flash memory that can
+only be written once and cannot be erased.
+
+This feature comes particularly handy when you want to store the public keys required
+to authenticate the firmware update images, which has exactly the same requirements. A public
+key is a cryptographic key that can be freely distributed and is used to verify the signature
+of the firmware update image. By storing the public keys in the OTP area, you can ensure that
+they are immutable and cannot be tampered with.
+
+### Compiling wolfBoot to access OTP as keystore
+
+To use the OTP area as a keystore, you need to compile wolfBoot with the `FLASH_OTP_KEYSTORE`
+option enabled. This option is disabled by default, which means that the keystore is incorporated into
+the wolfBoot binary itself.
+
+When wolfBoot uses the OTP area as a keystore, it reads the public keys from the OTP area at runtime.
+The public keys are stored in the OTP area, after an initial 16-byte header that contains the number of
+keys stored, the size of each key, and other information.
+
+In order for wolfBoot to start authenticating the firmware images at boot and upon update, the public keys
+must be provisioned to the OTP area in a separate step, as described in the next section.
+
+### Provisioning the public keys to the OTP area
+
+After enabling the `FLASH_OTP_KEYSTORE` option in your `.config` file, when you compile wolfBoot by running "make",
+an additional application called `otp-keystore-primer` is generated under `tools/keytools/otp`. This application is used to
+provision the public keys to the OTP area. By flashing this application to the microcontroller, the public keys contained 
+in your keystore (previously generated by `keygen`) are written to the OTP area.
+
+The `otp-keystore-primer` application is generated with the public keys embedded in it. The keys are retrieved from the `keystore.c` file,
+generated by the `keygen` command. The `otp-keystore-primer` application reads the public keys from the `keystore.c` file and writes them to the OTP area.
+
+After generating a new `keystore.c` with the `keygen` application, you can generate the `otp-keystore-primer` application again, by running `make otp`.
+
+> [!WARNING]
+> The `otp-keystore-primer` application is a one-time use application. Once the application runs on your target, the public keys are written to the OTP area,
+> and it will be impossible to erase them. Therefore, it is important to ensure that the public keys are correct before provisioning them to the OTP area,
+> and that the associated private keys are stored securely. Accidentally losing the private keys will render the public keys stored in the OTP area useless.
+
+> [!CAUTION]
+> ** Be very careful when using the `otp-keystore-primer` application. Use it at your own risk. **
+
diff --git a/tools/keytools/otp/README.md b/tools/keytools/otp/README.md
new file mode 100644
index 000000000..3ccf0294f
--- /dev/null
+++ b/tools/keytools/otp/README.md
@@ -0,0 +1,6 @@
+## OTP keystore primer application
+
+This application is used to provision the public keys into a dedicated FLASH OTP
+area. For more information about its usage, please refer to [/docs/flash-OTP.md](/docs/flash-OTP.md).
+
+

From 8b62697f6c0d12edaa60360be86e9b9a907b0a23 Mon Sep 17 00:00:00 2001
From: Daniele Lacamera <dan@danielinux.net>
Date: Thu, 16 May 2024 16:17:48 +0200
Subject: [PATCH 11/19] Fixed FLASH range config for SAU in TZ+DUALBANK

+ added non-secure area at boot for OTP to read trust anchor if OTP
  feature is enabled.
---
 hal/stm32_tz.c | 71 +++++++++++++++++++++++++++++++++++++++-----------
 hal/stm32h5.c  |  1 +
 2 files changed, 57 insertions(+), 15 deletions(-)

diff --git a/hal/stm32_tz.c b/hal/stm32_tz.c
index 2bd3d8af2..7f1f6534b 100644
--- a/hal/stm32_tz.c
+++ b/hal/stm32_tz.c
@@ -60,29 +60,67 @@ static void RAMFUNCTION hal_flash_nonsecure_lock(void)
         FLASH_NS_CR |= FLASH_CR_LOCK;
 }
 
+static int is_range_nonsecure(uint32_t address, int len)
+{
+#ifndef DUALBANK_SWAP
+    /* The non secure area begins at the BOOT partition */
+    uint32_t min = WOLFBOOT_PARTITION_BOOT_ADDRESS;
+    uint32_t max = FLASH_TOP + 1;
+    uint32_t end;
+    if (len < 0)
+        return 0;
+    end = (uint32_t)(address + len);
+    if ((address >= min) && (end <= max))
+        return 1;
+    return 0;
+#else
+    /* In this case, the secure area is in the lower side of both banks. */
+    uint32_t boot_offset = WOLFBOOT_PARTITION_BOOT_ADDRESS - ARCH_FLASH_OFFSET;
+    uint32_t min1 = WOLFBOOT_PARTITION_BOOT_ADDRESS;
+    uint32_t max1 = FLASH_BANK2_BASE + 1;
+    uint32_t min2 = WOLFBOOT_PARTITION_UPDATE_ADDRESS;
+    uint32_t max2 = FLASH_TOP + 1;
+    uint32_t end;
+    if (len < 0)
+        return 0;
+    end = (uint32_t)(address + len);
+    if (((address >= min1) && (end <= max1)) ||
+            ((address >= min2) && (end <= max2)) )
+        return 1;
+    return 0;
+#endif
+}
+
+
 void hal_tz_claim_nonsecure_area(uint32_t address, int len)
 {
     int page_n, reg_idx;
     uint32_t reg;
     uint32_t end = address + len;
+    uint32_t bank = 0;
+    int pos;
 
-
-    if (address < FLASH_BANK2_BASE)
+    if (!is_range_nonsecure(address, len))
         return;
-    if (end > (FLASH_TOP + 1))
-        return;
-
-    hal_flash_wait_complete(0);
-    hal_flash_clear_errors(0);
     while (address < end) {
-        page_n = (address - FLASH_BANK2_BASE) / FLASH_PAGE_SIZE;
+        if (address < FLASH_BANK2_BASE) {
+            page_n = (address - ARCH_FLASH_OFFSET) / FLASH_PAGE_SIZE;
+            bank = 1;
+        } else {
+            page_n = (address - FLASH_BANK2_BASE) / FLASH_PAGE_SIZE;
+            bank = 2;
+        }
         reg_idx = page_n / 32;
-        int pos;
         pos = page_n % 32;
+        hal_flash_wait_complete(bank);
+        hal_flash_clear_errors(bank);
         hal_flash_nonsecure_unlock();
-        FLASH_SECBB2[reg_idx] |= ( 1 << pos);
+        if (bank == 1)
+            FLASH_SECBB1[reg_idx] |= ( 1 << pos);
+        else
+            FLASH_SECBB2[reg_idx] |= ( 1 << pos);
         ISB();
-        hal_flash_wait_complete(0);
+        hal_flash_wait_complete(bank);
         hal_flash_nonsecure_lock();
         /* Erase claimed non-secure page, in secure mode */
 #ifndef PLATFORM_stm32h5
@@ -96,7 +134,7 @@ void hal_tz_claim_nonsecure_area(uint32_t address, int len)
         DMB();
         FLASH_CR |= FLASH_CR_STRT;
         ISB();
-        hal_flash_wait_complete(0);
+        hal_flash_wait_complete(bank);
         address += FLASH_PAGE_SIZE;
     }
 #ifndef PLATFORM_stm32h5
@@ -198,15 +236,15 @@ void hal_gtzc_init(void)
 void hal_tz_sau_init(void)
 {
     uint32_t page_n = 0;
-    /* WIP: SAU is set up before staging */
+    /* SAU is set up before staging. Set up all areas as secure. */
     /* Non-secure callable: NSC functions area */
     sau_init_region(0, 0x0C038000, 0x0C040000, 1);
 
     /* Non-Secure: application flash area (first bank) */
-    sau_init_region(1, 0x08040000, 0x080FFFFF, 0);
+    sau_init_region(1, WOLFBOOT_PARTITION_BOOT_ADDRESS, FLASH_BANK2_BASE - 1, 0);
 
     /* Non-Secure: application flash area (second bank) */
-    sau_init_region(2, 0x08140000, 0x081FFFFF, 0);
+    sau_init_region(2, WOLFBOOT_PARTITION_UPDATE_ADDRESS, FLASH_TOP -1, 0);
 
     /* Secure RAM regions in SRAM1/SRAM2 */
     sau_init_region(3, 0x30000000, 0x3004FFFF, 1);
@@ -217,6 +255,9 @@ void hal_tz_sau_init(void)
     /* Non-secure: internal peripherals */
     sau_init_region(5, 0x40000000, 0x4FFFFFFF, 0);
 
+    /* Set as non-secure: OTP + RO area */
+    sau_init_region(6, 0x08FFF000, 0x08FFFFFF, 0);
+
     /* Enable SAU */
     SAU_CTRL = SAU_INIT_CTRL_ENABLE;
 
diff --git a/hal/stm32h5.c b/hal/stm32h5.c
index 22841ec42..474d11e5d 100644
--- a/hal/stm32h5.c
+++ b/hal/stm32h5.c
@@ -385,6 +385,7 @@ static void fork_bootloader(void)
     uint32_t r = 0, w = 0;
     int i;
 
+
 #if TZ_SECURE()
     data = (uint32_t)((data & (~FLASHMEM_ADDRESS_SPACE)) | FLASH_SECURE_MMAP_BASE);
     dst = (uint32_t)((dst & (~FLASHMEM_ADDRESS_SPACE)) | FLASH_SECURE_MMAP_BASE);

From 307e3b426c223616a5e2e4746c5c6bf9c6bb0df5 Mon Sep 17 00:00:00 2001
From: Daniele Lacamera <dan@danielinux.net>
Date: Mon, 20 May 2024 10:58:43 +0200
Subject: [PATCH 12/19] otp_keystore_primer: fixed provisioning + readonly

---
 hal/stm32h5.c                            | 65 ++++++++++++++++++++----
 hal/stm32h5.h                            |  4 ++
 hal/stm32h7.c                            |  6 +++
 include/hal.h                            |  1 +
 tools/keytools/otp/Makefile              | 12 ++---
 tools/keytools/otp/otp-keystore-primer.c |  6 +++
 6 files changed, 77 insertions(+), 17 deletions(-)

diff --git a/hal/stm32h5.c b/hal/stm32h5.c
index 474d11e5d..11c6a9de8 100644
--- a/hal/stm32h5.c
+++ b/hal/stm32h5.c
@@ -435,38 +435,80 @@ void hal_prepare_boot(void)
 
 #ifdef FLASH_OTP_KEYSTORE
 
+#define FLASH_OTP_BLOCK_SIZE (64)
+
 /* Public API */
 
+int hal_flash_otp_set_readonly(uint32_t flashAddress, uint16_t length)
+{
+    uint32_t start_block = (flashAddress - FLASH_OTP_BASE) / FLASH_OTP_BLOCK_SIZE;
+    uint32_t count = length / FLASH_OTP_BLOCK_SIZE;
+    uint32_t bmap = 0;
+    unsigned int i;
+    if (start_block + count > 32)
+        return -1;
+
+    if ((length % FLASH_OTP_BLOCK_SIZE) != 0)
+    {
+        count++;
+    }
+
+    /* Turn on the bits */
+    for (i = start_block; i < (start_block + count); i++) {
+        bmap |= (1 << i);
+    }
+    /* Enable OTP write protection for the selected blocks */
+    while ((bmap & FLASH_OTPBLR_CUR) != bmap) {
+        FLASH_OTPBLR_PRG |= bmap;
+        ISB();
+        DSB();
+    }
+    return 0;
+}
+
 int hal_flash_otp_write(uint32_t flashAddress, const void* data, uint16_t length)
 {
-    volatile uint16_t tmp;
+    volatile uint16_t tmp_msw, tmp_lsw;
     uint16_t *pdata = (uint16_t *)data;
     uint16_t idx = 0, len_align;
     uint16_t last_word;
+    uint32_t blr_bitmap = 0;
     if (!(flashAddress >= FLASH_OTP_BASE && flashAddress <= FLASH_OTP_END)) {
         return -1;
     }
 
+    /* Reject misaligned destination address */
+    if ((flashAddress & 0x01) != 0) {
+        return -1;
+    }
+
     hal_flash_wait_complete(0);
     hal_flash_wait_buffer_empty(0);
     hal_flash_unlock();
     hal_flash_clear_errors(0);
 
-
     /* Truncate to 2B alignment */
     length = (length / 2 * 2);
 
-    while (idx < length && flashAddress <= FLASH_OTP_END-1) {
+    while ((idx < length) && (flashAddress <= FLASH_OTP_END-1)) {
         hal_flash_wait_complete(0);
         /* Set PG bit */
         FLASH_CR |= FLASH_CR_PG;
-        /* Program an OTP word (32 bits) */
-        *(volatile uint16_t*)flashAddress = *pdata;
+        /* Program an OTP word (16 bits) */
+        *(volatile uint16_t*)flashAddress = pdata[0];
+        /* Program a second OTP word (16 bits) */
+        *(volatile uint16_t*)(flashAddress + sizeof(uint16_t)) = pdata[1];
         ISB();
         DSB();
+
+        /* Wait until not busy */
+        while ((FLASH_SR & FLASH_SR_BSY) != 0)
+            ;
+
         /* Read it back */
-        tmp = *(volatile uint16_t*)flashAddress;
-        if (tmp != *pdata) {
+        tmp_msw = *(volatile uint16_t*)flashAddress;
+        tmp_lsw = *(volatile uint16_t*)(flashAddress + sizeof(uint16_t));
+        if ((tmp_msw != pdata[0]) || (tmp_lsw != pdata[1])) {
             /* Provisioning failed. OTP already programmed? */
             while(1)
                 ;
@@ -474,11 +516,12 @@ int hal_flash_otp_write(uint32_t flashAddress, const void* data, uint16_t length
 
         /* Clear PG bit */
         FLASH_CR &= ~FLASH_CR_PG;
-        flashAddress += sizeof(uint16_t);
-        pdata++;
-        idx += sizeof(uint16_t);
-    }
 
+        /* Advance to next two words */
+        flashAddress += (2 * sizeof(uint16_t));
+        pdata += 2;
+        idx += (2 * sizeof(uint16_t));
+    }
     hal_flash_lock();
     return 0;
 }
diff --git a/hal/stm32h5.h b/hal/stm32h5.h
index 5e64222ad..754565271 100644
--- a/hal/stm32h5.h
+++ b/hal/stm32h5.h
@@ -207,6 +207,8 @@
 #define FLASH_SR          (*(volatile uint32_t *)(FLASH_BASE + 0x24))
 #define FLASH_CR          (*(volatile uint32_t *)(FLASH_BASE + 0x2C))
 
+
+
 #define FLASH_SECBB1       ((volatile uint32_t *)(FLASH_BASE + 0x0A0)) /* Array */
 #define FLASH_SECBB2       ((volatile uint32_t *)(FLASH_BASE + 0x1A0)) /* Array */
 #define FLASH_SECBB_NREGS  4    /* Array length for the two above */
@@ -239,6 +241,8 @@
 /* Both secure + non secure */
 #define FLASH_OPTCR       (*(volatile uint32_t *)(FLASH_BASE + 0x1C))
 #define FLASH_OPSR        (*(volatile uint32_t *)(FLASH_BASE + 0x18))
+#define FLASH_OTPBLR_CUR  (*(volatile uint32_t *)(FLASH_BASE + 0x90))
+#define FLASH_OTPBLR_PRG  (*(volatile uint32_t *)(FLASH_BASE + 0x94))
 
 #define FLASH_OPSR_DATA_OP          (1 << 21)
 #define FLASH_OPSR_BK_OP            (1 << 22)
diff --git a/hal/stm32h7.c b/hal/stm32h7.c
index 6d8fef9ed..d3cc21709 100644
--- a/hal/stm32h7.c
+++ b/hal/stm32h7.c
@@ -543,6 +543,12 @@ static void hal_flash_otp_lock(void)
 
 /* Public API */
 
+int hal_flash_otp_set_readonly(uint32_t flashAddress, uint16_t length)
+{
+    /* TODO: set WP on OTP if needed */
+    return 0;
+}
+
 int hal_flash_otp_write(uint32_t flashAddress, const void* data, uint16_t length)
 {
     volatile uint16_t tmp;
diff --git a/include/hal.h b/include/hal.h
index d42409af9..2bb836021 100644
--- a/include/hal.h
+++ b/include/hal.h
@@ -133,6 +133,7 @@ int hal_trng_get_entropy(unsigned char *out, unsigned len);
 #ifdef FLASH_OTP_KEYSTORE
 
 int hal_flash_otp_write(uint32_t flashAddress, const void* data, uint16_t length);
+int hal_flash_otp_set_readonly(uint32_t flashAddress, uint16_t length);
 int hal_flash_otp_read(uint32_t flashAddress, void* data, uint32_t length);
 
 #endif
diff --git a/tools/keytools/otp/Makefile b/tools/keytools/otp/Makefile
index 807d4e572..a1d524de9 100644
--- a/tools/keytools/otp/Makefile
+++ b/tools/keytools/otp/Makefile
@@ -10,19 +10,19 @@ CFLAGS+=-O0 -ggdb
 CFLAGS+=-I. -I../../../ -I../../../include
 CFLAGS+=-I./wcs
 CFLAGS+=-DFLASH_OTP_KEYSTORE -D__FLASH_OTP_PRIMER
-OBJS+=startup.o otp-keystore-primer.o ../../../src/keystore.o
+PRI_KS_OBJS+=startup.o otp-keystore-primer.o ../../../src/keystore.o
 LSCRIPT=target.ld
 LDFLAGS+=$(CFLAGS) -T$(LSCRIPT) -lc -Wl,-Map=otp-keystore-primer.map
 
 ifeq ($(TARGET),stm32h7)
     CFLAGS+=-DTARGET_stm32h7
     CFLAGS+=-mcpu=cortex-m7 -ffunction-sections -fdata-sections -fno-common -ffreestanding -nostartfiles
-    OBJS+=../../../hal/stm32h7.o
+    PRI_KS_OBJS+=../../../hal/stm32h7.o
 endif
 ifeq ($(TARGET),stm32h5)
     CFLAGS+=-DTARGET_stm32h5
     CFLAGS+=-mcpu=cortex-m33 -ffunction-sections -fdata-sections -fno-common -ffreestanding -nostartfiles
-    OBJS+=../../../hal/stm32h5.o
+    PRI_KS_OBJS+=../../../hal/stm32h5.o
 endif
 CC=$(CROSS_COMPILE)gcc
 OBJCOPY?=$(CROSS_COMPILE)objcopy
@@ -32,10 +32,10 @@ SIZE?=$(CROSS_COMPILE)size
 otp-keystore-primer.bin: otp-keystore-primer.elf
 	@$(OBJCOPY) -O binary $(^) $(@)
 
-otp-keystore-primer.elf: $(OBJS)
-	@$(CC) -o otp-keystore-primer.elf $(LDFLAGS) $(CFLAGS) $(OBJS)
+otp-keystore-primer.elf: $(PRI_KS_OBJS)
+	@$(CC) -o otp-keystore-primer.elf $(LDFLAGS) $(CFLAGS) $(PRI_KS_OBJS)
 	@$(SIZE) $(@)
 	
 
 clean:
-	@rm -rf $(OBJS) *.bin *.elf
+	@rm -rf $(PRI_KS_OBJS) *.bin *.elf
diff --git a/tools/keytools/otp/otp-keystore-primer.c b/tools/keytools/otp/otp-keystore-primer.c
index d64f2ebb6..ed15c40e6 100644
--- a/tools/keytools/otp/otp-keystore-primer.c
+++ b/tools/keytools/otp/otp-keystore-primer.c
@@ -34,6 +34,7 @@ void main(void)
     int n_keys = keystore_num_pubkeys();
     int i;
     struct wolfBoot_otp_hdr hdr;
+    uint32_t tot_len;
 
     hal_init();
 
@@ -58,6 +59,11 @@ void main(void)
                 sizeof(struct keystore_slot));
     }
 
+    /* Protect the OTP area just written */
+    tot_len = OTP_HDR_SIZE + n_keys * SIZEOF_KEYSTORE_SLOT;
+    hal_flash_otp_set_readonly(FLASH_OTP_BASE, tot_len);
+
+
     /* Done! */
     while(1)
         ;

From 84282c9bb16e22995d7662c0ac24ba034d50b5b4 Mon Sep 17 00:00:00 2001
From: Daniele Lacamera <dan@danielinux.net>
Date: Mon, 20 May 2024 11:00:32 +0200
Subject: [PATCH 13/19] Added full setup on stm32h5

---
 .../examples/stm32h5-tz-dualbank-otp.config   | 30 +++++++++++++++++++
 1 file changed, 30 insertions(+)
 create mode 100644 config/examples/stm32h5-tz-dualbank-otp.config

diff --git a/config/examples/stm32h5-tz-dualbank-otp.config b/config/examples/stm32h5-tz-dualbank-otp.config
new file mode 100644
index 000000000..e4a13939f
--- /dev/null
+++ b/config/examples/stm32h5-tz-dualbank-otp.config
@@ -0,0 +1,30 @@
+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?=0
+DUALBANK_SWAP?=1
+WOLFBOOT_PARTITION_SIZE?=0xC0000
+WOLFBOOT_SECTOR_SIZE?=0x2000
+WOLFBOOT_PARTITION_BOOT_ADDRESS?=0x08040000
+WOLFBOOT_PARTITION_UPDATE_ADDRESS?=0x8140000
+WOLFBOOT_PARTITION_SWAP_ADDRESS?=0xFFFFFFFF
+FLAGS_HOME=0
+DISABLE_BACKUP=0
+FLASH_OTP_KEYSTORE=1
+WOLFCRYPT_TZ=1
+WOLFCRYPT_TZ_PKCS11=1

From 4a57fd229620db21c2aa0695098e6196db3bcccd Mon Sep 17 00:00:00 2001
From: Daniele Lacamera <dan@danielinux.net>
Date: Mon, 20 May 2024 11:03:31 +0200
Subject: [PATCH 14/19] Added build test for H5 full config (TZ+dualbank+OTP)

---
 .github/workflows/test-configs.yml | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/.github/workflows/test-configs.yml b/.github/workflows/test-configs.yml
index 05f568b57..66f989613 100644
--- a/.github/workflows/test-configs.yml
+++ b/.github/workflows/test-configs.yml
@@ -257,6 +257,12 @@ jobs:
       arch: arm
       config-file: ./config/examples/stm32h5-wolfcrypt-tz.config
 
+  stm32h5_tz_dualbank_otp:
+    uses: ./.github/workflows/test-build.yml
+    with:
+      arch: arm
+      config-file: ./config/examples/stm32h5-tz-dualbank-otp.config
+
   stm32h7_test:
     uses: ./.github/workflows/test-build.yml
     with:

From df4dd3dadcbaca1a7f4133e526b753d938196225 Mon Sep 17 00:00:00 2001
From: Daniele Lacamera <dan@danielinux.net>
Date: Fri, 24 May 2024 13:02:51 +0200
Subject: [PATCH 15/19] Fixed size for memcmp/copy used in fork_bootloader

---
 hal/stm32h5.c | 18 ++++++++++++------
 hal/stm32h5.h |  2 +-
 2 files changed, 13 insertions(+), 7 deletions(-)

diff --git a/hal/stm32h5.c b/hal/stm32h5.c
index 11c6a9de8..c102234d5 100644
--- a/hal/stm32h5.c
+++ b/hal/stm32h5.c
@@ -376,7 +376,9 @@ void RAMFUNCTION hal_flash_dualbank_swap(void)
     stm32h5_reboot();
 }
 
-static uint8_t bootloader_copy_mem[BOOTLOADER_SIZE];
+
+#define BOOTLOADER_COPY_MEM_SIZE 0x1000
+static uint8_t bootloader_copy_mem[BOOTLOADER_COPY_MEM_SIZE];
 
 static void fork_bootloader(void)
 {
@@ -395,13 +397,17 @@ static void fork_bootloader(void)
     if (memcmp((void *)data, (const char*)dst, BOOTLOADER_SIZE) == 0)
         return;
 
-    /* Read the wolfBoot image in RAM */
-    memcpy(bootloader_copy_mem, (void*)data, BOOTLOADER_SIZE);
-
-    /* Mass-erase */
     hal_flash_unlock();
+    /* Mass-erase second block */
     hal_flash_erase(dst, BOOTLOADER_SIZE);
-    hal_flash_write(dst, bootloader_copy_mem, BOOTLOADER_SIZE);
+    /* Read the wolfBoot image in RAM */
+    for (i = 0; i < BOOTLOADER_SIZE;
+            i += BOOTLOADER_COPY_MEM_SIZE) {
+        memcpy(bootloader_copy_mem, (void*)(data + i),
+                BOOTLOADER_COPY_MEM_SIZE);
+        hal_flash_write(dst + i, bootloader_copy_mem,
+                BOOTLOADER_COPY_MEM_SIZE);
+    }
     hal_flash_lock();
 }
 #endif
diff --git a/hal/stm32h5.h b/hal/stm32h5.h
index 754565271..b2bc4daea 100644
--- a/hal/stm32h5.h
+++ b/hal/stm32h5.h
@@ -326,7 +326,7 @@
 #define FLASHMEM_ADDRESS_SPACE    (0x08000000)
 #define FLASH_PAGE_SIZE           (0x2000)      /* 8KB */
 #define FLASH_BANK2_BASE          (0x08100000) /*!< Base address of Flash Bank2     */
-#define BOOTLOADER_SIZE           (0x8000)
+#define BOOTLOADER_SIZE           (WOLFBOOT_PARTITION_BOOT_ADDRESS - FLASHMEM_ADDRESS_SPACE)
 #define FLASH_TOP                 (0x081FFFFF) /*!< FLASH end address (sector 127)  */
 
 #define FLASH_KEY1                            (0x45670123U)

From d573efa8c65c966a4ea037f8f17f5fb9b3babe6f Mon Sep 17 00:00:00 2001
From: Daniele Lacamera <dan@danielinux.net>
Date: Fri, 24 May 2024 13:23:22 +0200
Subject: [PATCH 16/19] Initialize wolfPKCS11 also in "_hwswap" mode

---
 src/update_flash_hwswap.c | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/src/update_flash_hwswap.c b/src/update_flash_hwswap.c
index 14db052c1..d2f758be6 100644
--- a/src/update_flash_hwswap.c
+++ b/src/update_flash_hwswap.c
@@ -27,6 +27,9 @@
 #include "hal.h"
 #include "spi_flash.h"
 #include "wolfboot/wolfboot.h"
+#ifdef SECURE_PKCS11
+int WP11_Library_Init(void);
+#endif
 
 extern void hal_flash_dualbank_swap(void);
 
@@ -92,6 +95,9 @@ void RAMFUNCTION wolfBoot_start(void)
             hal_flash_lock();
         }
     }
+#ifdef SECURE_PKCS11
+    WP11_Library_Init();
+#endif
     hal_prepare_boot();
     do_boot((void *)(WOLFBOOT_PARTITION_BOOT_ADDRESS + IMAGE_HEADER_SIZE));
 }

From 75cd725c9d2284994468963014e3bf8659dd9d70 Mon Sep 17 00:00:00 2001
From: Daniele Lacamera <dan@danielinux.net>
Date: Fri, 24 May 2024 16:37:25 +0200
Subject: [PATCH 17/19] Fix linker warnings when building with otp

---
 Makefile                     | 33 ++++++++++++++++++---------------
 include/wolfboot/wolfboot.h  |  2 +-
 tools/keytools/otp/Makefile  | 18 +++++++++++++-----
 tools/keytools/otp/target.ld |  1 +
 4 files changed, 33 insertions(+), 21 deletions(-)

diff --git a/Makefile b/Makefile
index a36936d93..2f05f425a 100644
--- a/Makefile
+++ b/Makefile
@@ -26,10 +26,10 @@ SIGN_ALG=
 OBJCOPY_FLAGS=
 
 OBJS:= \
-	./hal/$(TARGET).o \
 	./src/string.o \
 	./src/image.o \
-	./src/libwolfboot.o
+	./src/libwolfboot.o \
+	./hal/$(TARGET).o
 
 ifeq ($(SIGN),NONE)
   PRIVATE_KEY=
@@ -85,50 +85,50 @@ TARGET_H_TEMPLATE:=include/target.h.in
 ifeq ($(TZEN),1)
 ifeq ($(TARGET),stm32l5)
 	# Don't build a contiguous image
-	MAIN_TARGET:=wolfboot.bin test-app/image_v1_signed.bin
+    MAIN_TARGET:=wolfboot.bin test-app/image_v1_signed.bin
 endif
 
 ifeq ($(TARGET),stm32u5)
 	# Don't build a contiguous image
-	MAIN_TARGET:=wolfboot.bin test-app/image_v1_signed.bin
+    MAIN_TARGET:=wolfboot.bin test-app/image_v1_signed.bin
 endif
 
 ifeq ($(TARGET),stm32h5)
 	# Don't build a contiguous image
-	MAIN_TARGET:=wolfboot.bin test-app/image_v1_signed.bin
+    MAIN_TARGET:=wolfboot.bin test-app/image_v1_signed.bin
 endif
 endif # TZEN=1
 
 ifeq ($(TARGET),x86_64_efi)
-	MAIN_TARGET:=wolfboot.efi
+    MAIN_TARGET:=wolfboot.efi
 endif
 
 ifeq ($(FSP), 1)
-	MAIN_TARGET:=wolfboot_stage1.bin
+    MAIN_TARGET:=wolfboot_stage1.bin
 endif
 
 ifeq ($(TARGET),library)
-	CFLAGS+=-g
-	MAIN_TARGET:=test-lib
+    CFLAGS+=-g
+    MAIN_TARGET:=test-lib
 endif
 
 ifeq ($(TARGET),raspi3)
-	MAIN_TARGET:=wolfboot.bin
+    MAIN_TARGET:=wolfboot.bin
 endif
 
 ifeq ($(TARGET),sim)
-	MAIN_TARGET:=wolfboot.bin tools/bin-assemble/bin-assemble test-app/image_v1_signed.bin internal_flash.dd
+    MAIN_TARGET:=wolfboot.bin tools/bin-assemble/bin-assemble test-app/image_v1_signed.bin internal_flash.dd
 endif
 
 ifeq ($(TARGET),nxp_p1021)
-	MAIN_TARGET:=factory_wstage1.bin
+    MAIN_TARGET:=factory_wstage1.bin
 endif
 ifeq ($(TARGET),nxp_t1024)
-	MAIN_TARGET:=factory_wstage1.bin
+    MAIN_TARGET:=factory_wstage1.bin
 endif
 
 ifeq ($(FLASH_OTP_KEYSTORE),1)
-	MAIN_TARGET:=include/target.h tools/keytools/otp/otp-keystore-primer factory.bin
+    MAIN_TARGET+=tools/keytools/otp/otp-keystore-primer.bin
 endif
 
 ASFLAGS:=$(CFLAGS)
@@ -177,11 +177,14 @@ standalone:
 	$(Q)$(OBJCOPY) $(OBJCOPY_FLAGS) -O binary test-app/image.elf standalone.bin
 	$(Q)$(SIZE) test-app/image.elf
 
+
 include tools/test.mk
 include tools/test-enc.mk
 include tools/test-delta.mk
 include tools/test-renode.mk
 
+hal/$(TARGET).o:
+
 keytools_check: keytools FORCE
 
 $(PRIVATE_KEY):
@@ -367,7 +370,7 @@ cppcheck:
 		--suppress="objectIndex" --suppress="comparePointers" \
 		--error-exitcode=89 --std=c89 src/*.c hal/*.c hal/spi/*.c hal/uart/*.c
 
-otp: tools/keytools/otp/otp-keystore-primer.bin
+otp: tools/keytools/otp/otp-keystore-primer.bin FORCE
 
 tools/keytools/otp/otp-keystore-primer.bin: FORCE
 	make -C tools/keytools/otp clean
diff --git a/include/wolfboot/wolfboot.h b/include/wolfboot/wolfboot.h
index 529b98400..a5985cc51 100644
--- a/include/wolfboot/wolfboot.h
+++ b/include/wolfboot/wolfboot.h
@@ -177,7 +177,7 @@ extern "C" {
 
 #endif
 
-#if defined __WOLFBOOT || defined __FLASH_OTP_PRIMER
+#if defined(__WOLFBOOT) || defined (__FLASH_OTP_PRIMER)
 
  /* Authentication configuration */
  #if defined(WOLFBOOT_NO_SIGN)
diff --git a/tools/keytools/otp/Makefile b/tools/keytools/otp/Makefile
index a1d524de9..756b33747 100644
--- a/tools/keytools/otp/Makefile
+++ b/tools/keytools/otp/Makefile
@@ -2,6 +2,10 @@
 -include ../../../tools/config.mk
 -include ../../../options.mk
 -include ../../../wcs/pkcs11.mk
+V?=0
+ifeq ($(V),0)
+  Q=@
+endif
 
 TARGET?=none
 ARCH?=ARM
@@ -28,14 +32,18 @@ CC=$(CROSS_COMPILE)gcc
 OBJCOPY?=$(CROSS_COMPILE)objcopy
 SIZE?=$(CROSS_COMPILE)size
 
-
 otp-keystore-primer.bin: otp-keystore-primer.elf
-	@$(OBJCOPY) -O binary $(^) $(@)
+	$(Q)$(OBJCOPY) -O binary $(^) $(@)
 
 otp-keystore-primer.elf: $(PRI_KS_OBJS)
-	@$(CC) -o otp-keystore-primer.elf $(LDFLAGS) $(CFLAGS) $(PRI_KS_OBJS)
-	@$(SIZE) $(@)
+	$(Q)$(CC) -o otp-keystore-primer.elf $(LDFLAGS) $(CFLAGS) $(PRI_KS_OBJS)
+	$(Q)$(SIZE) $(@)
+
+%.o: %.c
+	$(Q)$(CC) $(CFLAGS) -c -o $@ $<
+
+
 	
 
 clean:
-	@rm -rf $(PRI_KS_OBJS) *.bin *.elf
+	$(Q)rm -rf $(PRI_KS_OBJS) *.bin *.elf
diff --git a/tools/keytools/otp/target.ld b/tools/keytools/otp/target.ld
index 466a89305..342349360 100644
--- a/tools/keytools/otp/target.ld
+++ b/tools/keytools/otp/target.ld
@@ -4,6 +4,7 @@ MEMORY
     RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 0x00020000 /* mapping Low-memory only */
 }
 
+ENTRY(isr_reset);
 SECTIONS
 {
     .text :

From 9b378d005b2a1821d7c21fce94e88fa082a21889 Mon Sep 17 00:00:00 2001
From: Daniele Lacamera <dan@danielinux.net>
Date: Fri, 24 May 2024 16:41:36 +0200
Subject: [PATCH 18/19] Reviewer requests: fix portability of "packed"

+ separate object for hal in otp-keystore-primer
---
 include/otp_keystore.h      |  9 ++++++++-
 tools/keytools/otp/Makefile | 11 +++++++++--
 2 files changed, 17 insertions(+), 3 deletions(-)

diff --git a/include/otp_keystore.h b/include/otp_keystore.h
index f03be4673..8c66724b0 100644
--- a/include/otp_keystore.h
+++ b/include/otp_keystore.h
@@ -42,7 +42,14 @@
 
 #define OTP_HDR_SIZE 16
 
-struct __attribute__((packed)) wolfBoot_otp_hdr {
+#if (defined(__IAR_SYSTEMS_ICC__) && (__IAR_SYSTEMS_ICC__ > 8)) || \
+        defined(__GNUC__)
+    #define KEYSTORE_HDR_PACKED __attribute__((packed))
+#else
+    #define KEYSTORE_HDR_PACKED
+#endif
+
+struct KEYSTORE_HDR_PACKED wolfBoot_otp_hdr {
     char keystore_hdr_magic[8];
     uint16_t item_count;
     uint16_t flags;
diff --git a/tools/keytools/otp/Makefile b/tools/keytools/otp/Makefile
index 756b33747..363d56fd1 100644
--- a/tools/keytools/otp/Makefile
+++ b/tools/keytools/otp/Makefile
@@ -21,12 +21,12 @@ LDFLAGS+=$(CFLAGS) -T$(LSCRIPT) -lc -Wl,-Map=otp-keystore-primer.map
 ifeq ($(TARGET),stm32h7)
     CFLAGS+=-DTARGET_stm32h7
     CFLAGS+=-mcpu=cortex-m7 -ffunction-sections -fdata-sections -fno-common -ffreestanding -nostartfiles
-    PRI_KS_OBJS+=../../../hal/stm32h7.o
+    PRI_KS_OBJS+=stm32h7.o
 endif
 ifeq ($(TARGET),stm32h5)
     CFLAGS+=-DTARGET_stm32h5
     CFLAGS+=-mcpu=cortex-m33 -ffunction-sections -fdata-sections -fno-common -ffreestanding -nostartfiles
-    PRI_KS_OBJS+=../../../hal/stm32h5.o
+    PRI_KS_OBJS+=stm32h5.o
 endif
 CC=$(CROSS_COMPILE)gcc
 OBJCOPY?=$(CROSS_COMPILE)objcopy
@@ -42,6 +42,13 @@ otp-keystore-primer.elf: $(PRI_KS_OBJS)
 %.o: %.c
 	$(Q)$(CC) $(CFLAGS) -c -o $@ $<
 
+stm32h7.o: ../../../hal/stm32h7.c
+	$(Q)$(CC) $(CFLAGS) -c -o $@ $<
+
+
+stm32h5.o: ../../../hal/stm32h5.c
+	$(Q)$(CC) $(CFLAGS) -c -o $@ $<
+
 
 	
 

From 8834e344a931bb4b779eb5f6c7b239880c536b30 Mon Sep 17 00:00:00 2001
From: Daniele Lacamera <dan@danielinux.net>
Date: Fri, 24 May 2024 17:01:01 +0200
Subject: [PATCH 19/19] Fixed build error due to missing keystore.c in otp

---
 Makefile | 10 +++++++---
 1 file changed, 7 insertions(+), 3 deletions(-)

diff --git a/Makefile b/Makefile
index 2f05f425a..4b73633b3 100644
--- a/Makefile
+++ b/Makefile
@@ -36,9 +36,9 @@ ifeq ($(SIGN),NONE)
 else
   PRIVATE_KEY=wolfboot_signing_private_key.der
   ifeq ($(FLASH_OTP_KEYSTORE),1)
-        OBJS+=./src/flash_otp_keystore.o
+    OBJS+=./src/flash_otp_keystore.o
   else
-        OBJS+=./src/keystore.o
+    OBJS+=./src/keystore.o
   endif
 endif
 
@@ -287,7 +287,11 @@ hex: wolfboot.hex
 
 src/keystore.c: $(PRIVATE_KEY)
 
-flash_keystore: $(PRIVATE_KEY) src/flash_otp_keystore.o
+flash_keystore: src/flash_otp_keystore.o
+
+src/flash_otp_keystore.o: $(PRIVATE_KEY) src/flash_otp_keystore.c
+	$(Q)$(MAKE) src/keystore.c
+	$(Q)$(CC) -c $(CFLAGS) src/flash_otp_keystore.c -o $(@)
 
 keys: $(PRIVATE_KEY)