From e55011cde8d25e84853d75ea544946b70de89c68 Mon Sep 17 00:00:00 2001
From: Kevin O'Connor <kevin@koconnor.net>
Date: Sat, 18 Dec 2021 19:50:10 -0500
Subject: [PATCH] stm32: Reorganize usb bootloader code in stm32f4.c

Reorganize stm32f4.c into major code blocks.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
---
 src/stm32/stm32f4.c | 93 ++++++++++++++++++++++++++++++---------------
 1 file changed, 63 insertions(+), 30 deletions(-)

diff --git a/src/stm32/stm32f4.c b/src/stm32/stm32f4.c
index 267a5c74..a8320238 100644
--- a/src/stm32/stm32f4.c
+++ b/src/stm32/stm32f4.c
@@ -1,6 +1,6 @@
 // Code to setup clocks on stm32f2/stm32f4
 //
-// Copyright (C) 2019  Kevin O'Connor <kevin@koconnor.net>
+// Copyright (C) 2019-2021  Kevin O'Connor <kevin@koconnor.net>
 //
 // This file may be distributed under the terms of the GNU GPLv3 license.
 
@@ -12,6 +12,11 @@
 #include "internal.h" // enable_pclock
 #include "sched.h" // sched_main
 
+
+/****************************************************************
+ * Clock setup
+ ****************************************************************/
+
 #define FREQ_PERIPH_DIV (CONFIG_MACH_STM32F401 ? 2 : 4)
 #define FREQ_PERIPH (CONFIG_CLOCK_FREQ / FREQ_PERIPH_DIV)
 #define FREQ_USB 48000000
@@ -75,29 +80,6 @@ gpio_clock_enable(GPIO_TypeDef *regs)
     RCC->AHB1ENR;
 }
 
-#define USB_BOOT_FLAG_ADDR (CONFIG_RAM_START + CONFIG_RAM_SIZE - 4096)
-#define USB_BOOT_FLAG 0x55534220424f4f54 // "USB BOOT"
-
-// Handle USB reboot requests
-void
-usb_request_bootloader(void)
-{
-    irq_disable();
-    if (CONFIG_STM32_FLASH_START_4000) {
-        // HID Bootloader
-        RCC->APB1ENR |= RCC_APB1ENR_PWREN;
-        RCC->APB1ENR;
-        PWR->CR |= PWR_CR_DBP;
-        // HID Bootloader magic key
-        RTC->BKP4R = 0x424C;
-        PWR->CR &= ~PWR_CR_DBP;
-    } else {
-        // System DFU Bootloader
-        *(uint64_t*)USB_BOOT_FLAG_ADDR = USB_BOOT_FLAG;
-    }
-    NVIC_SystemReset();
-}
-
 #if !CONFIG_STM32_CLOCK_REF_INTERNAL
 DECL_CONSTANT_STR("RESERVE_PINS_crystal", "PH0,PH1");
 #endif
@@ -227,16 +209,67 @@ clock_setup(void)
         ;
 }
 
+
+/****************************************************************
+ * USB bootloader
+ ****************************************************************/
+
+// Reboot into USB "HID" bootloader
+static void
+usb_hid_bootloader(void)
+{
+    irq_disable();
+    RCC->APB1ENR |= RCC_APB1ENR_PWREN;
+    RCC->APB1ENR;
+    PWR->CR |= PWR_CR_DBP;
+    RTC->BKP4R = 0x424C; // HID Bootloader magic key
+    PWR->CR &= ~PWR_CR_DBP;
+    NVIC_SystemReset();
+}
+
+#define USB_BOOT_FLAG_ADDR (CONFIG_RAM_START + CONFIG_RAM_SIZE - 4096)
+#define USB_BOOT_FLAG 0x55534220424f4f54 // "USB BOOT"
+
+// Flag that bootloader is desired and reboot
+static void
+usb_reboot_for_dfu_bootloader(void)
+{
+    irq_disable();
+    *(uint64_t*)USB_BOOT_FLAG_ADDR = USB_BOOT_FLAG;
+    NVIC_SystemReset();
+}
+
+// Check if rebooting into system DFU Bootloader
+static void
+check_usb_dfu_bootloader(void)
+{
+    if (!CONFIG_USBSERIAL || *(uint64_t*)USB_BOOT_FLAG_ADDR != USB_BOOT_FLAG)
+        return;
+    *(uint64_t*)USB_BOOT_FLAG_ADDR = 0;
+    uint32_t *sysbase = (uint32_t*)0x1fff0000;
+    asm volatile("mov sp, %0\n bx %1"
+                 : : "r"(sysbase[0]), "r"(sysbase[1]));
+}
+
+// Handle USB reboot requests
+void
+usb_request_bootloader(void)
+{
+    if (CONFIG_STM32_FLASH_START_4000)
+        usb_hid_bootloader();
+    usb_reboot_for_dfu_bootloader();
+}
+
+
+/****************************************************************
+ * Startup
+ ****************************************************************/
+
 // Main entry point - called from armcm_boot.c:ResetHandler()
 void
 armcm_main(void)
 {
-    if (CONFIG_USBSERIAL && *(uint64_t*)USB_BOOT_FLAG_ADDR == USB_BOOT_FLAG) {
-        *(uint64_t*)USB_BOOT_FLAG_ADDR = 0;
-        uint32_t *sysbase = (uint32_t*)0x1fff0000;
-        asm volatile("mov sp, %0\n bx %1"
-                     : : "r"(sysbase[0]), "r"(sysbase[1]));
-    }
+    check_usb_dfu_bootloader();
 
     // Run SystemInit() and then restore VTOR
     SystemInit();