From 834693a1151aca36e4674465fe5be9ec47b2ff4f Mon Sep 17 00:00:00 2001
From: tychovrahe <brunam@seznam.cz>
Date: Thu, 23 Nov 2023 12:50:02 +0100
Subject: [PATCH] fix(core): fix OTP programming on U5

[no changelog]
---
 core/embed/firmware/main.c               |  4 ++--
 core/embed/trezorhal/stm32u5/flash_otp.c |  6 +++---
 core/embed/trezorhal/stm32u5/trustzone.c | 19 +++++++++++++++++++
 3 files changed, 24 insertions(+), 5 deletions(-)

diff --git a/core/embed/firmware/main.c b/core/embed/firmware/main.c
index d11a9e2510..71cafccec5 100644
--- a/core/embed/firmware/main.c
+++ b/core/embed/firmware/main.c
@@ -113,8 +113,6 @@ int main(void) {
   HAL_Init();
 #endif
 
-  collect_hw_entropy();
-
 #ifdef SYSTEM_VIEW
   enable_systemview();
 #endif
@@ -139,6 +137,8 @@ int main(void) {
 
   mpu_config_firmware_initial();
 
+  collect_hw_entropy();
+
 #if PRODUCTION || BOOTLOADER_QA
   check_and_replace_bootloader();
 #endif
diff --git a/core/embed/trezorhal/stm32u5/flash_otp.c b/core/embed/trezorhal/stm32u5/flash_otp.c
index d76044c9cc..c5c6d7aa61 100644
--- a/core/embed/trezorhal/stm32u5/flash_otp.c
+++ b/core/embed/trezorhal/stm32u5/flash_otp.c
@@ -50,11 +50,11 @@ secbool flash_otp_write(uint8_t block, uint8_t offset, const uint8_t *data,
     return secfalse;
   }
   ensure(flash_unlock_write(), NULL);
-  for (uint8_t i = 0; i < datalen; i++) {
+  for (uint8_t i = 0; i < datalen; i += 16) {
     uint32_t address =
         FLASH_OTP_BASE + block * FLASH_OTP_BLOCK_SIZE + offset + i;
-    ensure(sectrue * (HAL_OK == HAL_FLASH_Program(FLASH_TYPEPROGRAM_QUADWORD,
-                                                  address, (uint32_t)data)),
+    ensure(sectrue * (HAL_OK == HAL_FLASH_Program(FLASH_TYPEPROGRAM_QUADWORD_NS,
+                                                  address, (uint32_t)&data[i])),
            NULL);
   }
   ensure(flash_lock_write(), NULL);
diff --git a/core/embed/trezorhal/stm32u5/trustzone.c b/core/embed/trezorhal/stm32u5/trustzone.c
index 99342b3c84..e760217cc2 100644
--- a/core/embed/trezorhal/stm32u5/trustzone.c
+++ b/core/embed/trezorhal/stm32u5/trustzone.c
@@ -23,6 +23,22 @@
 
 #ifdef BOARDLOADER
 
+#define SAU_INIT_CTRL_ENABLE 1
+#define SAU_INIT_CTRL_ALLNS 0
+#define SAU_INIT_REGION(n, start, end, sec) \
+  SAU->RNR = ((n)&SAU_RNR_REGION_Msk);      \
+  SAU->RBAR = ((start)&SAU_RBAR_BADDR_Msk); \
+  SAU->RLAR = ((end)&SAU_RLAR_LADDR_Msk) |  \
+              (((sec) << SAU_RLAR_NSC_Pos) & SAU_RLAR_NSC_Msk) | 1U
+
+static void trustzone_configure_sau(void) {
+  SAU_INIT_REGION(0, 0x0BF90000, 0x0BFA8FFF, 0);  // OTP etc
+
+  SAU->CTRL =
+      ((SAU_INIT_CTRL_ENABLE << SAU_CTRL_ENABLE_Pos) & SAU_CTRL_ENABLE_Msk) |
+      ((SAU_INIT_CTRL_ALLNS << SAU_CTRL_ALLNS_Pos) & SAU_CTRL_ALLNS_Msk);
+}
+
 // Configure ARMCortex-M33 SCB and FPU security
 static void trustzone_configure_arm(void) {
   // Enable FPU in both secure and non-secure modes
@@ -100,6 +116,9 @@ void trustzone_init_boardloader(void) {
   // Configure ARM SCB/FBU security
   trustzone_configure_arm();
 
+  // Configure SAU security attributes
+  trustzone_configure_sau();
+
   // Enable GTZC (Global Trust-Zone Controller) peripheral clock
   __HAL_RCC_GTZC1_CLK_ENABLE();
   __HAL_RCC_GTZC2_CLK_ENABLE();