From ae7d23bf4152f1dc7d1114830473b447b64c204b Mon Sep 17 00:00:00 2001
From: Daniele Lacamera <root@danielinux.net>
Date: Mon, 30 Mar 2026 13:59:22 +0200
Subject: [PATCH] Fix stm32h5 OTP readonly block rounding

F/1475
---
 hal/stm32h5.c                   | 10 ++---
 include/hal_otp.h               | 12 ++++++
 tools/unit-tests/Makefile       |  5 ++-
 tools/unit-tests/unit-hal-otp.c | 73 +++++++++++++++++++++++++++++++++
 4 files changed, 92 insertions(+), 8 deletions(-)
 create mode 100644 include/hal_otp.h
 create mode 100644 tools/unit-tests/unit-hal-otp.c

diff --git a/hal/stm32h5.c b/hal/stm32h5.c
index 69bac84a..02e2997c 100644
--- a/hal/stm32h5.c
+++ b/hal/stm32h5.c
@@ -25,6 +25,7 @@
 #include <string.h>
 
 #include "hal.h"
+#include "hal_otp.h"
 #include "hal/stm32h5.h"
 #include "hal/armv8m_tz.h"
 
@@ -764,20 +765,15 @@ void hal_prepare_boot(void)
 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 count = hal_otp_blocks_for_length(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);
+        bmap |= (1U << i);
     }
     /* Enable OTP write protection for the selected blocks */
     while ((bmap & FLASH_OTPBLR_CUR) != bmap) {
diff --git a/include/hal_otp.h b/include/hal_otp.h
new file mode 100644
index 00000000..1cee0a04
--- /dev/null
+++ b/include/hal_otp.h
@@ -0,0 +1,12 @@
+#ifndef WOLFBOOT_HAL_OTP_H
+#define WOLFBOOT_HAL_OTP_H
+
+#include <stdint.h>
+
+static inline uint32_t hal_otp_blocks_for_length(uint32_t length,
+    uint32_t block_size)
+{
+    return (length + block_size - 1U) / block_size;
+}
+
+#endif /* WOLFBOOT_HAL_OTP_H */
diff --git a/tools/unit-tests/Makefile b/tools/unit-tests/Makefile
index ac2562eb..5edd46ca 100644
--- a/tools/unit-tests/Makefile
+++ b/tools/unit-tests/Makefile
@@ -49,7 +49,7 @@ TESTS:=unit-parser unit-extflash unit-string unit-spi-flash unit-aes128 \
        unit-update-flash-enc unit-update-ram unit-pkcs11_store unit-psa_store unit-disk \
        unit-update-disk unit-multiboot unit-boot-x86-fsp unit-qspi-flash unit-tpm-rsa-exp \
        unit-image-nopart unit-image-sha384 unit-image-sha3-384 unit-store-sbrk \
-       unit-tpm-blob unit-policy-sign unit-sdhci-response-bits
+       unit-tpm-blob unit-policy-sign unit-sdhci-response-bits unit-hal-otp
 
 all: $(TESTS)
 
@@ -149,6 +149,9 @@ unit-sdhci-response-bits: ../../include/target.h unit-sdhci-response-bits.c
 	gcc -o $@ $^ $(CFLAGS) -ffunction-sections -fdata-sections $(LDFLAGS) \
 		-Wl,--gc-sections
 
+unit-hal-otp: ../../include/target.h unit-hal-otp.c
+	gcc -o $@ $^ $(CFLAGS) $(LDFLAGS)
+
 unit-aes128: ../../include/target.h unit-extflash.c
 	gcc -o $@ $^ $(CFLAGS) $(LDFLAGS)
 
diff --git a/tools/unit-tests/unit-hal-otp.c b/tools/unit-tests/unit-hal-otp.c
new file mode 100644
index 00000000..8aa224d1
--- /dev/null
+++ b/tools/unit-tests/unit-hal-otp.c
@@ -0,0 +1,73 @@
+/* unit-hal-otp.c
+ *
+ * Unit tests for OTP block rounding helpers.
+ *
+ * Copyright (C) 2026 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 <check.h>
+#include <stdint.h>
+
+#include "../../include/hal_otp.h"
+
+#define FLASH_OTP_BLOCK_SIZE 64U
+#define OTP_BLOCKS 32U
+
+START_TEST(test_rounds_partial_block_before_bounds_check)
+{
+    uint32_t start_block = OTP_BLOCKS - 1U;
+    uint32_t count = hal_otp_blocks_for_length(1U, FLASH_OTP_BLOCK_SIZE);
+
+    ck_assert_uint_eq(count, 1U);
+    ck_assert_uint_gt(start_block + count, OTP_BLOCKS - 1U);
+}
+END_TEST
+
+START_TEST(test_exact_multiple_keeps_exact_block_count)
+{
+    ck_assert_uint_eq(
+        hal_otp_blocks_for_length(FLASH_OTP_BLOCK_SIZE * 2U, FLASH_OTP_BLOCK_SIZE),
+        2U);
+}
+END_TEST
+
+static Suite *hal_otp_suite(void)
+{
+    Suite *s = suite_create("hal-otp");
+    TCase *tc = tcase_create("rounding");
+
+    tcase_add_test(tc, test_rounds_partial_block_before_bounds_check);
+    tcase_add_test(tc, test_exact_multiple_keeps_exact_block_count);
+    suite_add_tcase(s, tc);
+
+    return s;
+}
+
+int main(void)
+{
+    int fails;
+    Suite *s = hal_otp_suite();
+    SRunner *sr = srunner_create(s);
+
+    srunner_run_all(sr, CK_NORMAL);
+    fails = srunner_ntests_failed(sr);
+    srunner_free(sr);
+
+    return fails;
+}