AP_HAL_ChibiOS: add reboot to USB mass storage mode

Tools/ardupilotwaf/chibios.py

@@ -633,6 +633,17 @@ def bldpath(path):
         env.DEFINES += [ 'CANARD_MULTI_IFACE=1' ]
     setup_optimization(cfg.env)
 
+def get_build_option_value(env, name):
+    '''return the value of a build option, or None if it was not specified'''
+    enable_option = 'enable_' + name
+    disable_option = 'disable_' + name
+    if env.OPTIONS.get(enable_option, False) or env.OPTIONS.get(enable_option.lower(), False):
+        return 1
+    if env.OPTIONS.get(disable_option, False) or env.OPTIONS.get(disable_option.lower(), False):
+        return 0
+    return None
+
+
 def generate_hwdef_h(env):
     '''run chibios_hwdef.py'''
     if env.BOOTLOADER:
@@ -660,6 +671,7 @@ def generate_hwdef_h(env):
         outdir=hwdef_out,
         bootloader=bootloader_flag,
         signed_fw=bool(env.AP_SIGNED_FIRMWARE),
+        mass_storage_option=get_build_option_value(env, 'MASS_STORAGE'),
         hwdef=hwdef,
         # stringify like old subprocess based invocation. note that no error is
         # generated if this path is missing!

Tools/scripts/build_options.py

@@ -420,6 +420,7 @@ def config_option(self):
     Feature('Other', 'GyroFFT', 'HAL_GYROFFT_ENABLED', 'Enable In-Flight gyro FFT calculations', 0, None),
     Feature('Other', 'NMEA_OUTPUT', 'HAL_NMEA_OUTPUT_ENABLED', 'Enable NMEA output', 0, None),
     Feature('Other', 'SDCARD_FORMATTING', 'AP_FILESYSTEM_FORMAT_ENABLED', 'Enable Formatting of microSD cards', 0, None),
+    Feature('Other', 'MASS_STORAGE', 'AP_REBOOT_MASS_STORAGE_ENABLED', 'Enable reboot to USB mass storage', 0, None),
     Feature('Other', 'BOOTLOADER_FLASHING', 'AP_BOOTLOADER_FLASHING_ENABLED', 'Enable Bootloader flashing', 0, "FILESYSTEM_ROMFS"),  # noqa
     Feature('Other', 'SERIALDEVICE_REGISTER', 'AP_SERIALMANAGER_REGISTER_ENABLED', 'Enable Serial device registration', 0, None), # noqa
 

Tools/scripts/extract_features.py

@@ -34,6 +34,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"):
         self.features = [
             ('AP_ADVANCEDFAILSAFE_ENABLED', r'AP_AdvancedFailsafe::heartbeat\b',),
             ('AP_BOOTLOADER_FLASHING_ENABLED', 'ChibiOS::Util::flash_bootloader',),
+            ('AP_REBOOT_MASS_STORAGE_ENABLED', r'ChibiOS::usb_msd_run',),
             ('AP_AIRSPEED_ENABLED', 'AP_Airspeed::AP_Airspeed',),
             ('AP_AIRSPEED_{type}_ENABLED', r'AP_Airspeed_(?P<type>.*)::init',),
 

libraries/AP_HAL/AP_HAL_Boards.h

@@ -228,6 +228,10 @@
 #define HAL_OS_LITTLEFS_IO 0
 #endif
 
+#ifndef AP_REBOOT_MASS_STORAGE_ENABLED
+#define AP_REBOOT_MASS_STORAGE_ENABLED 0
+#endif
+
 #ifndef HAL_BARO_DEFAULT
 #define HAL_BARO_DEFAULT HAL_BARO_NONE
 #endif

libraries/AP_HAL/Util.h

@@ -24,6 +24,11 @@ class AP_HAL::Util {
     // return true if the reason for the reboot was a watchdog reset
     virtual bool was_watchdog_reset() const { return false; }
 
+#if AP_REBOOT_MASS_STORAGE_ENABLED
+    // support an early application mode which exports the SD card over USB
+    virtual bool request_usb_msd() { return false; }
+#endif
+
     // return true if safety was off and this was a watchdog reset
     bool was_watchdog_safety_off() const {
         return was_watchdog_reset() && persistent_data.safety_state == SAFETY_ARMED;
@@ -77,7 +82,8 @@ class AP_HAL::Util {
         uint8_t fault_thd_prio;
         char thread_name4[4];
         int8_t scheduler_task;
-        bool armed; // true if vehicle was armed
+        bool armed : 1; // true if vehicle was armed
+        bool boot_to_mass_storage : 1;
         enum safety_state safety_state;
         bool boot_to_dfu; // true if we should reboot to DFU on boot
     };

libraries/AP_HAL/board/chibios.h

@@ -30,6 +30,14 @@
 #define HAL_PROGRAM_SIZE_LIMIT_KB (BOARD_FLASH_SIZE+EXT_FLASH_SIZE_MB*1024)
 #endif
 
+#ifndef AP_REBOOT_MASS_STORAGE_ENABLED
+#if HAL_PROGRAM_SIZE_LIMIT_KB >= 2048
+#define AP_REBOOT_MASS_STORAGE_ENABLED 1
+#else
+#define AP_REBOOT_MASS_STORAGE_ENABLED 0
+#endif
+#endif
+
 #ifndef HAL_NUM_CAN_IFACES
 #define HAL_NUM_CAN_IFACES 0
 #endif

libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp

@@ -25,6 +25,7 @@
 #include <AP_HAL_ChibiOS/AP_HAL_ChibiOS_Private.h>
 #include "shared_dma.h"
 #include "sdcard.h"
+#include "USB_MSD.h"
 #include <sysperf.h>
 #include "hwdef/common/usbcfg.h"
 #include "hwdef/common/stm32_util.h"
@@ -367,6 +368,12 @@ void HAL_ChibiOS::run(int argc, char * const argv[], Callbacks* callbacks) const
     AP::sitl()->init();
 #endif  // AP_SIM_ENABLED
 
+#if AP_REBOOT_MASS_STORAGE_ENABLED && HAL_USB_MSD_BOOT_ENABLED
+    if (ChibiOS::usb_msd_boot_requested()) {
+        ChibiOS::usb_msd_run();
+    }
+#endif
+
 #if HAL_USE_SERIAL_USB == TRUE
     usb_initialise();
 #endif

libraries/AP_HAL_ChibiOS/USB_MSD.cpp

@@ -0,0 +1,225 @@
+/*
+ * This file 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.
+ *
+ * This file 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.
+ */
+
+#include <AP_HAL/AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
+
+#include "USB_MSD.h"
+
+#if AP_REBOOT_MASS_STORAGE_ENABLED && HAL_USB_MSD_BOOT_ENABLED
+
+#include <hal.h>
+#include <stdlib.h>
+
+#include "hwdef/common/stm32_util.h"
+#include "hwdef/common/watchdog.h"
+#include "sdcard.h"
+#include "shared_dma.h"
+
+#if !defined(STM32H7) && !defined(STM32F7) && !defined(STM32F4)
+#error HAL_USB_MSD_BOOT_ENABLED is only supported on STM32H7, STM32F7 and STM32F4
+#endif
+
+#if HAL_USE_USB_MSD != TRUE || (HAL_USE_SDC != TRUE && HAL_USE_MMC_SPI != TRUE) || HAL_USE_SERIAL_USB != TRUE
+#error HAL_USB_MSD_BOOT_ENABLED requires USB MSD, a microSD block device and USB support
+#endif
+
+extern const AP_HAL::HAL& hal;
+
+namespace ChibiOS {
+
+static constexpr size_t USB_MSD_IO_SIZE = 4096;
+
+static const uint8_t device_descriptor_data[18] = {
+    USB_DESC_DEVICE(
+        0x0110,
+        0x00,
+        0x00,
+        0x00,
+        0x40,
+        HAL_USB_VENDOR_ID,
+        HAL_USB_PRODUCT_ID,
+        0x0200,
+        0,
+        1,
+        0,
+        1)
+};
+
+static const USBDescriptor device_descriptor = {
+    sizeof(device_descriptor_data),
+    device_descriptor_data
+};
+
+static const uint8_t configuration_descriptor_data[32] = {
+    USB_DESC_CONFIGURATION(32, 1, 1, 0, 0xC0, 50),
+    USB_DESC_INTERFACE(0, 0, 2, 0x08, 0x06, 0x50, 0),
+    USB_DESC_ENDPOINT(0x01, 0x02, 0x0040, 0),
+    USB_DESC_ENDPOINT(0x81, 0x02, 0x0040, 0)
+};
+
+static const USBDescriptor configuration_descriptor = {
+    sizeof(configuration_descriptor_data),
+    configuration_descriptor_data
+};
+
+static const uint8_t string0[] = {
+    USB_DESC_BYTE(4),
+    USB_DESC_BYTE(USB_DESCRIPTOR_STRING),
+    USB_DESC_WORD(0x0409)
+};
+
+static const uint8_t product_string[] = {
+    USB_DESC_BYTE(36), USB_DESC_BYTE(USB_DESCRIPTOR_STRING),
+    'A', 0, 'r', 0, 'd', 0, 'u', 0, 'P', 0, 'i', 0, 'l', 0, 'o', 0, 't', 0,
+    ' ', 0, 'S', 0, 'D', 0, ' ', 0, 'C', 0, 'a', 0, 'r', 0, 'd', 0
+};
+
+static const USBDescriptor string_descriptors[] = {
+    { sizeof(string0), string0 },
+    { sizeof(product_string), product_string }
+};
+
+static const USBDescriptor *get_descriptor(USBDriver *usbp, uint8_t type,
+                                           uint8_t index, uint16_t language)
+{
+    (void)usbp;
+    (void)language;
+
+    switch (type) {
+    case USB_DESCRIPTOR_DEVICE:
+        return &device_descriptor;
+    case USB_DESCRIPTOR_CONFIGURATION:
+        return &configuration_descriptor;
+    case USB_DESCRIPTOR_STRING:
+        if (index < ARRAY_SIZE(string_descriptors)) {
+            return &string_descriptors[index];
+        }
+        break;
+    }
+    return nullptr;
+}
+
+static USBInEndpointState ep1_in_state;
+static USBOutEndpointState ep1_out_state;
+
+static const USBEndpointConfig ep1_config = {
+    USB_EP_MODE_TYPE_BULK,
+    nullptr,
+    nullptr,
+    nullptr,
+    0x0040,
+    0x0040,
+    &ep1_in_state,
+    &ep1_out_state,
+    2,
+    nullptr
+};
+
+static void usb_event(USBDriver *usbp, usbevent_t event)
+{
+    if (event != USB_EVENT_CONFIGURED) {
+        return;
+    }
+
+    chSysLockFromISR();
+    usbInitEndpointI(usbp, 1, &ep1_config);
+    chSysUnlockFromISR();
+}
+
+static const USBConfig usb_config = {
+    usb_event,
+    get_descriptor,
+    msd_request_hook,
+    nullptr
+};
+
+bool usb_msd_boot_requested()
+{
+    AP_HAL::Util::PersistentData persistent_data {};
+    stm32_watchdog_load(reinterpret_cast<uint32_t *>(&persistent_data),
+                        (sizeof(persistent_data) + 3) / 4);
+    if (!persistent_data.boot_to_mass_storage) {
+        return false;
+    }
+    persistent_data.boot_to_mass_storage = false;
+    stm32_watchdog_save(reinterpret_cast<uint32_t *>(&persistent_data),
+                        (sizeof(persistent_data) + 3) / 4);
+    return true;
+}
+
+void usb_msd_set_boot_request()
+{
+    hal.util->persistent_data.boot_to_mass_storage = true;
+    stm32_watchdog_save(reinterpret_cast<uint32_t *>(&hal.util->persistent_data),
+                        (sizeof(hal.util->persistent_data) + 3) / 4);
+}
+
+void usb_msd_run()
+{
+    peripheral_power_enable();
+
+#if AP_HAL_SHARED_DMA_ENABLED
+    Shared_DMA::init();
+#endif
+
+    // Mass-storage mode is dedicated to bulk transfers, so deliberately use
+    // no SD slowdown for maximum speed. Normal flight logging honours
+    // BRD_SD_SLOWDOWN for maximum robustness.
+    if (!sdcard_init_raw(0, 3)) {
+        return;
+    }
+
+    auto *msdp = static_cast<USBMassStorageDriver *>(calloc(1, sizeof(USBMassStorageDriver)));
+    auto *block_buffer = static_cast<uint8_t *>(
+        hal.util->malloc_type(USB_MSD_IO_SIZE * 2, AP_HAL::Util::MEM_FILESYSTEM));
+    if (msdp == nullptr || block_buffer == nullptr) {
+        free(msdp);
+        hal.util->free_type(block_buffer, USB_MSD_IO_SIZE * 2, AP_HAL::Util::MEM_FILESYSTEM);
+        return;
+    }
+
+#if STM32_OTG2_IS_OTG1
+    auto *usbp = &USBD2;
+#else
+    auto *usbp = &USBD1;
+#endif
+
+    usbDisconnectBus(usbp);
+    chThdSleep(chTimeUS2I(1500));
+    usbStart(usbp, &usb_config);
+
+    msdObjectInit(msdp);
+    msdStart(msdp, usbp, sdcard_get_block_device(),
+             block_buffer, block_buffer + USB_MSD_IO_SIZE, USB_MSD_IO_SIZE,
+             nullptr, nullptr, nullptr, nullptr);
+
+    usbConnectBus(usbp);
+
+#if !defined(DISABLE_WATCHDOG)
+    stm32_watchdog_init();
+#endif
+    // Mass-storage mode deliberately owns the SD card until power is removed;
+    // never return to the flight firmware while the USB host may retain access.
+    while (true) {
+#if !defined(DISABLE_WATCHDOG)
+        stm32_watchdog_pat();
+#endif
+        chThdSleepMilliseconds(50);
+    }
+}
+
+}
+
+#endif // AP_REBOOT_MASS_STORAGE_ENABLED && HAL_USB_MSD_BOOT_ENABLED
+#endif // HAL_BOARD_CHIBIOS

libraries/AP_HAL_ChibiOS/USB_MSD.h

@@ -0,0 +1,15 @@
+#pragma once
+
+#ifndef HAL_USB_MSD_BOOT_ENABLED
+#define HAL_USB_MSD_BOOT_ENABLED 0
+#endif
+
+namespace ChibiOS {
+
+#if AP_REBOOT_MASS_STORAGE_ENABLED && HAL_USB_MSD_BOOT_ENABLED
+bool usb_msd_boot_requested();
+void usb_msd_set_boot_request();
+void usb_msd_run();
+#endif
+
+}

libraries/AP_HAL_ChibiOS/Util.cpp

@@ -60,6 +60,15 @@ extern AP_IOMCU iomcu;
 extern const AP_HAL::HAL& hal;
 
 using namespace ChibiOS;
+
+#if AP_REBOOT_MASS_STORAGE_ENABLED && HAL_USB_MSD_BOOT_ENABLED
+bool Util::request_usb_msd()
+{
+    usb_msd_set_boot_request();
+    return true;
+}
+#endif
+
 #if CH_CFG_USE_HEAP == TRUE
 
 /**