diff --git a/hal/stm32h5.c b/hal/stm32h5.c
index 474d11e5..11c6a9de 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 5e64222a..75456527 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 6d8fef9e..d3cc2170 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 d42409af..2bb83602 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 807d4e57..a1d524de 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 d64f2ebb..ed15c40e 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)
         ;