From 439da284135ab762be13e7f1378272ed4aefb860 Mon Sep 17 00:00:00 2001
From: Peter Johanson <peter@peterjohanson.com>
Date: Mon, 2 Dec 2024 00:20:43 -0700
Subject: [PATCH] soc: arm: stm32: Add ROM bootloader support

Add jump to ROM bootloader for STM32 when bootmode is set via
retention boot mode feature.

Signed-off-by: Peter Johanson <peter@peterjohanson.com>
---
 soc/arm/st_stm32/common/CMakeLists.txt     |  1 +
 soc/arm/st_stm32/common/Kconfig.soc        | 11 +++++
 soc/arm/st_stm32/common/stm32_bootloader.c | 55 ++++++++++++++++++++++
 3 files changed, 67 insertions(+)
 create mode 100644 soc/arm/st_stm32/common/stm32_bootloader.c

diff --git a/soc/arm/st_stm32/common/CMakeLists.txt b/soc/arm/st_stm32/common/CMakeLists.txt
index 70dd8c865c9789..592d508bd52835 100644
--- a/soc/arm/st_stm32/common/CMakeLists.txt
+++ b/soc/arm/st_stm32/common/CMakeLists.txt
@@ -5,6 +5,7 @@ zephyr_sources(stm32cube_hal.c)
 zephyr_linker_sources_ifdef(CONFIG_STM32_CCM SECTIONS ccm.ld)
 
 zephyr_sources_ifdef(CONFIG_STM32_BACKUP_SRAM stm32_backup_sram.c)
+zephyr_sources_ifdef(CONFIG_STM32_BOOTLOADER stm32_bootloader.c)
 zephyr_linker_sources_ifdef(CONFIG_STM32_BACKUP_SRAM SECTIONS stm32_backup_sram.ld)
 
 zephyr_sources(soc_config.c)
diff --git a/soc/arm/st_stm32/common/Kconfig.soc b/soc/arm/st_stm32/common/Kconfig.soc
index 07b2122cefd969..97d962e7f48d2e 100644
--- a/soc/arm/st_stm32/common/Kconfig.soc
+++ b/soc/arm/st_stm32/common/Kconfig.soc
@@ -15,6 +15,17 @@ config STM32_BACKUP_SRAM
 	help
 	  Enable support for STM32 backup SRAM.
 
+
+config HAS_STM32_BOOTLOADER
+	def_bool $(dt_compat_enabled,$(DT_COMPAT_ST_STM32_BOOTLOADER))
+
+config STM32_BOOTLOADER
+	bool "STM32 Bootloader Support"
+	default HAS_STM32_BOOTLOADER
+	depends on RETENTION_BOOT_MODE
+	help
+	  Enable support for jumping into the STM32 when the bootmode is set.
+
 config USE_STM32_ASSERT
 	depends on ASSERT
 	bool "STM32Cube HAL and LL drivers asserts"
diff --git a/soc/arm/st_stm32/common/stm32_bootloader.c b/soc/arm/st_stm32/common/stm32_bootloader.c
new file mode 100644
index 00000000000000..bcc5db558bf2b4
--- /dev/null
+++ b/soc/arm/st_stm32/common/stm32_bootloader.c
@@ -0,0 +1,55 @@
+/*
+ * Copyright (c) 2020 Google LLC.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include <soc.h>
+#include <zephyr/init.h>
+#include <zephyr/retention/retention.h>
+#include <zephyr/retention/bootmode.h>
+#include <stm32_ll_system.h>
+
+static const uint32_t bootloader = DT_REG_ADDR(DT_INST(0, st_stm32_bootloader));
+static FUNC_NORETURN void jump_to_bootloader(void)
+{
+	uint32_t i = 0;
+	void (*jmp)(void);
+
+	__disable_irq();
+
+	SysTick->CTRL = 0;
+	SysTick->LOAD = 0;
+	SysTick->VAL = 0;
+
+	for (i = 0; i < sizeof(NVIC->ICER) / sizeof(NVIC->ICER[0]); i++)
+	{
+		NVIC->ICER[i] = 0xFFFFFFFF;
+		NVIC->ICPR[i] = 0xFFFFFFFF;
+	}
+
+	LL_SYSCFG_SetRemapMemory(LL_SYSCFG_REMAP_SYSTEMFLASH);
+
+	jmp = (void (*)(void)) (void (*)(void)) (*((uint32_t *) ((bootloader + 4))));
+
+	__set_CONTROL(0);
+	__set_MSP(*(uint32_t *)bootloader);
+
+	__enable_irq();
+
+	jmp();
+
+	while (1) { }
+}
+
+static int bootloader_check_boot_init(void)
+{
+	if (bootmode_check(BOOT_MODE_TYPE_BOOTLOADER) > 0) {
+		bootmode_clear();
+		jump_to_bootloader();
+	}
+
+	return 0;
+}
+
+SYS_INIT(bootloader_check_boot_init, EARLY, 0);