AP_HAL_ChibiOS: add support for booting into DFU

Tools/ardupilotwaf/chibios.py

@@ -458,6 +458,8 @@ def load_env_vars(env):
         env.CHIBIOS_BUILD_FLAGS += ' ENABLE_MALLOC_GUARD=yes'
     if env.ENABLE_STATS:
         env.CHIBIOS_BUILD_FLAGS += ' ENABLE_STATS=yes'
+    if env.ENABLE_DFU_BOOT and env.BOOTLOADER:
+        env.CHIBIOS_BUILD_FLAGS += ' USE_ASXOPT=-DCRT0_ENTRY_HOOK=TRUE'
 
 
 def setup_optimization(env):

libraries/AP_HAL_ChibiOS/Util.cpp

@@ -748,6 +748,15 @@ void* Util::last_crash_dump_ptr() const
 }
 #endif // AP_CRASHDUMP_ENABLED
 
+#if HAL_ENABLE_DFU_BOOT && !defined(HAL_BOOTLOADER_BUILD)
+void Util::boot_to_dfu()
+{
+    hal.util->persistent_data.boot_to_dfu = true;
+    stm32_watchdog_save((uint32_t *)&hal.util->persistent_data, (sizeof(hal.util->persistent_data)+3)/4);
+    hal.scheduler->reboot(false);
+}
+#endif
+
 // set armed state
 void Util::set_soft_armed(const bool b)
 {

libraries/AP_HAL_ChibiOS/Util.h

@@ -20,6 +20,9 @@
 #include "AP_HAL_ChibiOS_Namespace.h"
 #include "AP_HAL_ChibiOS.h"
 #include <ch.h>
+#if !defined(HAL_BOOTLOADER_BUILD)
+#include <GCS_MAVLink/GCS.h>
+#endif
 
 class ExpandingString;
 
@@ -152,4 +155,7 @@ private:
     void* last_crash_dump_ptr() const override;
 #endif
 
+#if HAL_ENABLE_DFU_BOOT
+    void boot_to_dfu() override;
+#endif
 };

libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef-bl.inc

@@ -58,3 +58,6 @@ PD10 FRAM_CS CS SPEED_VERYLOW
 # disable peripheral and sensor power in the bootloader
 PE3 VDD_3V3_SENSORS_EN OUTPUT LOW
 PA8 nVDD_5V_PERIPH_EN OUTPUT LOW
+
+# uncomment to enable DFU reboot
+# ENABLE_DFU_BOOT 1

libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.inc

@@ -335,3 +335,6 @@ define HAL_SOLO_GIMBAL_ENABLED HAL_MOUNT_ENABLED
 
 # Enable Sagetech MXS ADSB transponder
 define HAL_ADSB_SAGETECH_MXS_ENABLED HAL_ADSB_ENABLED
+
+# uncomment to enable DFU reboot
+# ENABLE_DFU_BOOT 1

libraries/AP_HAL_ChibiOS/hwdef/Durandal/hwdef.dat

@@ -348,3 +348,6 @@ define HAL_BATT_CURR_SCALE 36.364
 
 # Enable Sagetech MXS ADSB transponder
 define HAL_ADSB_SAGETECH_MXS_ENABLED HAL_ADSB_ENABLED
+
+# uncomment to enable DFU boot
+# ENABLE_DFU_BOOT 1

libraries/AP_HAL_ChibiOS/hwdef/common/chibios_board.mk

@@ -18,6 +18,16 @@ ifeq ($(USE_CPPOPT),)
   USE_CPPOPT = -fno-rtti -std=gnu++11
 endif
 
+# Assembly specific options here (added to USE_OPT).
+ifeq ($(USE_ASOPT),)
+  USE_ASOPT = 
+endif
+
+# Assembly specific options here (added to USE_ASXOPT).
+ifeq ($(USE_ASXOPT),)
+  USE_ASXOPT =
+endif
+
 # Enable this if you want the linker to remove unused code and data
 ifeq ($(USE_LINK_GC),)
   USE_LINK_GC = yes

libraries/AP_HAL_ChibiOS/hwdef/common/chibios_common.mk

@@ -7,6 +7,8 @@
 OPT    := $(USE_OPT)
 COPT   := $(USE_COPT)
 CPPOPT := $(USE_CPPOPT)
+ASOPT := $(USE_ASOPT)
+ASXOPT := $(USE_ASXOPT)
 
 # Garbage collection
 ifeq ($(USE_LINK_GC),yes)
@@ -117,8 +119,8 @@ LIBS      := $(DLIBS) $(ULIBS)
 # Various settings
 MCFLAGS   := -mcpu=$(MCU)
 ODFLAGS	  = -x --syms
-ASFLAGS   = $(MCFLAGS) $(ADEFS)
+ASFLAGS   = $(MCFLAGS) $(ADEFS) $(ASOPT)
-ASXFLAGS  = $(MCFLAGS) $(ADEFS)
+ASXFLAGS  = $(MCFLAGS) $(ADEFS) $(ASXOPT)
 ifneq ($(USE_FPU),no)
   LIBCC_ASXFLAGS = $(ASXFLAGS) $(USE_FPU_OPT)
 else

libraries/AP_HAL_ChibiOS/hwdef/common/common.ld

@@ -72,6 +72,7 @@ SECTIONS
     {
         /* we want app_descriptor near the start of flash so a false
            positive isn't found by the bootloader (eg. ROMFS) */
+        KEEP(*(.ecc_raw));
         KEEP(*(.app_descriptor));
         *(.text)
         *(.text.*)
@@ -201,4 +202,5 @@ SECTIONS
         . = ORIGIN(flash) + LENGTH(flash);
         __crash_log_end__ = .;
     } > flash
+
 }

libraries/AP_HAL_ChibiOS/hwdef/common/common_extf.ld

@@ -220,6 +220,7 @@ SECTIONS
            positive isn't found by the bootloader (eg. ROMFS) */
         KEEP(*libch.a:vectors.o);
         KEEP(*libch.a:crt0_v7m.o);
+        KEEP(*(.ecc_raw));
         KEEP(*(.app_descriptor));
         *(.text)
         *(.text.*)

libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py

@@ -19,6 +19,8 @@ parser.add_argument(
     '-D', '--outdir', type=str, default=None, help='Output directory')
 parser.add_argument(
     '--bootloader', action='store_true', default=False, help='configure for bootloader')
+parser.add_argument(
+    '--signed-fw', action='store_true', default=False, help='configure for signed FW')
 parser.add_argument(
     'hwdef', type=str, nargs='+', default=None, help='hardware definition file')
 parser.add_argument(
@@ -1134,7 +1136,7 @@ def write_mcu_config(f):
 #define DISABLE_SERIAL_ESC_COMM TRUE
 #define CH_CFG_USE_DYNAMIC FALSE
 ''')
-        if not env_vars['EXT_FLASH_SIZE_MB']:
+        if not env_vars['EXT_FLASH_SIZE_MB'] and not args.signed_fw:
             f.write('''
 #define CH_CFG_USE_MEMCORE FALSE
 #define CH_CFG_USE_SEMAPHORES FALSE
@@ -1308,7 +1310,12 @@ def write_USB_config(f):
     default_product = "%BOARD%"
     if args.bootloader:
         default_product += "-BL"
-    f.write('#define HAL_USB_STRING_PRODUCT %s\n' % get_config("USB_STRING_PRODUCT", default="\"%s\""%default_product))
+    product_string = get_config("USB_STRING_PRODUCT", default="\"%s\""%default_product)
+    if args.bootloader and args.signed_fw:
+        product_string = product_string.replace("-BL", "-Secure-BL-v10")
+        print(product_string)
+    f.write('#define HAL_USB_STRING_PRODUCT %s\n' % product_string)
+    
     f.write('#define HAL_USB_STRING_SERIAL %s\n' % get_config("USB_STRING_SERIAL", default="\"%SERIAL%\""))
 
     f.write('\n\n')
@@ -2303,6 +2310,27 @@ def write_hwdef_header(outfilename):
 #define MHZ (1000U*1000U)
 #define KHZ (1000U)
 
+''')
+
+    if args.signed_fw:
+        f.write('''
+#define AP_SIGNED_FIRMWARE 1
+''')
+    else:
+        f.write('''
+#define AP_SIGNED_FIRMWARE 0
+''')
+
+    enable_dfu_boot = get_config('ENABLE_DFU_BOOT', default=0)
+    if enable_dfu_boot:
+        env_vars['ENABLE_DFU_BOOT'] = 1
+        f.write('''
+#define HAL_ENABLE_DFU_BOOT TRUE
+''')
+    else:
+        env_vars['ENABLE_DFU_BOOT'] = 0
+        f.write('''
+#define HAL_ENABLE_DFU_BOOT FALSE
 ''')
 
     dma_noshare.extend(get_config('DMA_NOSHARE', default='', aslist=True))

libraries/AP_HAL_ChibiOS/system.cpp

@@ -240,6 +240,28 @@ void __cxa_pure_virtual() { while (1); } //TODO: Handle properly, maybe generate
 void NMI_Handler(void);
 void NMI_Handler(void) { while (1); }
 
+#if defined(HAL_BOOTLOADER_BUILD) && HAL_ENABLE_DFU_BOOT
+void __entry_hook(void);
+void __entry_hook()
+{
+    // read the persistent data
+    AP_HAL::Util::PersistentData pd;
+    stm32_watchdog_load((uint32_t *)&pd, (sizeof(pd)+3)/4);
+    if (pd.boot_to_dfu) {
+        pd.boot_to_dfu = false;
+        stm32_watchdog_save((uint32_t *)&pd, (sizeof(pd)+3)/4);
+#if defined(STM32H7)
+        const uint32_t *app_base = (const uint32_t *)(0x1FF09800); 
+#else
+        const uint32_t *app_base = (const uint32_t *)(0x1FFF0000);
+#endif
+        __set_MSP(*app_base);
+        ((void (*)())*(&app_base[1]))();
+        while(true);
+    }
+}
+#endif
+
 }
 namespace AP_HAL {