Merge pull request #6365 from mikeller/add_msc_msp

Added MSP for reboot to MSC.

make/source.mk

@@ -52,6 +52,7 @@ COMMON_SRC = \
             io/statusindicator.c \
             io/transponder_ir.c \
             io/usb_cdc_hid.c \
+            io/usb_msc.c \
             msp/msp_serial.c \
             scheduler/scheduler.c \
             sensors/adcinternal.c \

src/main/drivers/usb_msc.h

@@ -31,3 +31,4 @@ bool mscCheckBoot(void);
 uint8_t mscStart(void);
 bool mscCheckButton(void);
 void mscWaitForButton(void);
+void systemResetToMsc(void);

src/main/drivers/usb_msc_f4xx.c

@@ -44,21 +44,13 @@
 #include "drivers/time.h"
 #include "drivers/usb_msc.h"
 
+#include "drivers/accgyro/accgyro_mpu.h"
+
 #include "pg/usb.h"
 
-#if defined(STM32F4)
 #include "usb_core.h"
 #include "usbd_cdc_vcp.h"
 #include "usb_io.h"
-#elif defined(STM32F7)
-#include "vcp_hal/usbd_cdc_interface.h"
-#include "usb_io.h"
-USBD_HandleTypeDef USBD_Device;
-#else
-#include "usb_core.h"
-#include "usb_init.h"
-#include "hw_config.h"
-#endif
 
 #include "msc/usbd_storage.h"
 
@@ -152,4 +144,16 @@ void mscWaitForButton(void)
         }
     }
 }
+
+void systemResetToMsc(void)
+{
+    if (mpuResetFn) {
+        mpuResetFn();
+    }
+
+    *((uint32_t *)0x2001FFF0) = MSC_MAGIC;
+
+    __disable_irq();
+    NVIC_SystemReset();
+}
 #endif

src/main/drivers/usb_msc_f7xx.c

@@ -42,6 +42,8 @@
 #include "drivers/time.h"
 #include "drivers/usb_msc.h"
 
+#include "drivers/accgyro/accgyro_mpu.h"
+
 #include "pg/usb.h"
 
 #include "vcp_hal/usbd_cdc_interface.h"
@@ -148,4 +150,16 @@ void mscWaitForButton(void)
         }
     }
 }
+
+void systemResetToMsc(void)
+{
+    if (mpuResetFn) {
+        mpuResetFn();
+    }
+
+    *((__IO uint32_t*) BKPSRAM_BASE + 16) = MSC_MAGIC;
+
+    __disable_irq();
+    NVIC_SystemReset();
+}
 #endif

src/main/interface/cli.c

@@ -62,28 +62,28 @@ extern uint8_t __config_end;
 #include "drivers/adc.h"
 #include "drivers/buf_writer.h"
 #include "drivers/bus_spi.h"
+#include "drivers/camera_control.h"
 #include "drivers/compass/compass.h"
 #include "drivers/display.h"
 #include "drivers/dma.h"
 #include "drivers/flash.h"
+#include "drivers/inverter.h"
 #include "drivers/io.h"
 #include "drivers/io_impl.h"
-#include "drivers/inverter.h"
+#include "drivers/light_led.h"
+#include "drivers/rangefinder/rangefinder_hcsr04.h"
 #include "drivers/sdcard.h"
 #include "drivers/sensor.h"
 #include "drivers/serial.h"
 #include "drivers/serial_escserial.h"
-#include "drivers/rangefinder/rangefinder_hcsr04.h"
 #include "drivers/sound_beeper.h"
 #include "drivers/stack_check.h"
 #include "drivers/system.h"
-#include "drivers/transponder_ir.h"
 #include "drivers/time.h"
 #include "drivers/timer.h"
-#include "drivers/light_led.h"
+#include "drivers/transponder_ir.h"
-#include "drivers/camera_control.h"
-#include "drivers/vtx_common.h"
 #include "drivers/usb_msc.h"
+#include "drivers/vtx_common.h"
 
 #include "fc/board_info.h"
 #include "fc/config.h"
@@ -116,6 +116,7 @@ extern uint8_t __config_end;
 #include "io/osd.h"
 #include "io/serial.h"
 #include "io/transponder_ir.h"
+#include "io/usb_msc.h"
 #include "io/vtx_control.h"
 #include "io/vtx.h"
 
@@ -4364,40 +4365,21 @@ static void cliDiff(char *cmdline)
     printConfig(cmdline, true);
 }
 
-#ifdef USE_USB_MSC
+#if defined(USE_USB_MSC)
 static void cliMsc(char *cmdline)
 {
     UNUSED(cmdline);
 
-    if (false
+    if (mscCheckFilesystemReady()) {
-#ifdef USE_SDCARD
+        cliPrintHashLine("Restarting in mass storage mode");
-        || sdcard_isFunctional()
-#endif
-#ifdef USE_FLASHFS
-        || flashfsGetSize() > 0
-#endif
-    ) {
-        cliPrintHashLine("restarting in mass storage mode");
         cliPrint("\r\nRebooting");
         bufWriterFlush(cliWriter);
-        delay(1000);
         waitForSerialPortToFinishTransmitting(cliPort);
         stopPwmAllMotors();
-        if (mpuResetFn) {
-            mpuResetFn();
-        }
 
-#ifdef STM32F7
+        systemResetToMsc();
-        *((__IO uint32_t*) BKPSRAM_BASE + 16) = MSC_MAGIC;
-#elif defined(STM32F4)
-        *((uint32_t *)0x2001FFF0) = MSC_MAGIC;
-#endif
-
-        __disable_irq();
-        NVIC_SystemReset();
     } else {
-        cliPrint("\r\nStorage not present or failed to initialize!");
+        cliPrintHashLine("Storage not present or failed to initialize!");
-        bufWriterFlush(cliWriter);
     }
 }
 #endif

src/main/interface/msp.c

@@ -41,14 +41,10 @@
 
 #include "config/config_eeprom.h"
 #include "config/feature.h"
-#include "pg/pg.h"
-#include "pg/pg_ids.h"
-#include "pg/beeper.h"
-#include "pg/rx.h"
-#include "pg/rx_spi.h"
 
 #include "drivers/accgyro/accgyro.h"
 #include "drivers/bus_i2c.h"
+#include "drivers/camera_control.h"
 #include "drivers/compass/compass.h"
 #include "drivers/flash.h"
 #include "drivers/io.h"
@@ -58,9 +54,9 @@
 #include "drivers/serial.h"
 #include "drivers/serial_escserial.h"
 #include "drivers/system.h"
-#include "drivers/vtx_common.h"
 #include "drivers/transponder_ir.h"
-#include "drivers/camera_control.h"
+#include "drivers/usb_msc.h"
+#include "drivers/vtx_common.h"
 
 #include "fc/board_info.h"
 #include "fc/config.h"
@@ -96,13 +92,19 @@
 #include "io/serial_4way.h"
 #include "io/servos.h"
 #include "io/transponder_ir.h"
+#include "io/usb_msc.h"
 #include "io/vtx_control.h"
 #include "io/vtx.h"
 #include "io/vtx_string.h"
 
 #include "msp/msp_serial.h"
 
+#include "pg/beeper.h"
 #include "pg/board.h"
+#include "pg/pg.h"
+#include "pg/pg_ids.h"
+#include "pg/rx.h"
+#include "pg/rx_spi.h"
 #include "pg/vcd.h"
 
 #include "rx/rx.h"
@@ -129,6 +131,15 @@
 
 static const char * const flightControllerIdentifier = BETAFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
 
+enum {
+    MSP_REBOOT_FIRMWARE = 0,
+    MSP_REBOOT_BOOTLOADER,
+    MSP_REBOOT_MSC,
+    MSP_REBOOT_COUNT,
+};
+
+static uint8_t rebootMode;
+
 #ifndef USE_OSD_SLAVE
 
 static const char pidnames[] =
@@ -232,8 +243,27 @@ static void mspRebootFn(serialPort_t *serialPort)
 #ifndef USE_OSD_SLAVE
     stopPwmAllMotors();
 #endif
+
+    switch (rebootMode) {
+    case MSP_REBOOT_FIRMWARE:
         systemReset();
 
+        break;
+    case MSP_REBOOT_BOOTLOADER:
+        systemResetToBootloader();
+
+        break;
+#if defined(USE_USB_MSC)
+    case MSP_REBOOT_MSC:
+        systemResetToMsc();
+
+        break;
+#endif
+    default:
+    
+        break;
+    }
+
     // control should never return here.
     while (true) ;
 }
@@ -410,6 +440,8 @@ static void serializeDataflashReadReply(sbuf_t *dst, uint32_t address, const uin
  */
 static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn)
 {
+    UNUSED(mspPostProcessFn);
+
     switch (cmdMSP) {
     case MSP_API_VERSION:
         sbufWriteU8(dst, MSP_PROTOCOL_VERSION);
@@ -486,12 +518,6 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce
         sbufWriteData(dst, shortGitRevision, GIT_SHORT_REVISION_LENGTH);
         break;
 
-    case MSP_REBOOT:
-        if (mspPostProcessFn) {
-            *mspPostProcessFn = mspRebootFn;
-        }
-        break;
-
     case MSP_ANALOG:
         sbufWriteU8(dst, (uint8_t)constrain(getBatteryVoltage(), 0, 255));
         sbufWriteU16(dst, (uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF)); // milliamp hours drawn from battery
@@ -1331,28 +1357,68 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
     }
     return !unsupportedCommand;
 }
+#endif // USE_OSD_SLAVE
 
-static mspResult_e mspFcProcessOutCommandWithArg(uint8_t cmdMSP, sbuf_t *arg, sbuf_t *dst)
+static mspResult_e mspFcProcessOutCommandWithArg(uint8_t cmdMSP, sbuf_t *src, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn)
 {
+#if defined(USE_OSD_SLAVE)
+        UNUSED(dst);
+#endif
+
     switch (cmdMSP) {
+#if !defined(USE_OSD_SLAVE)
     case MSP_BOXNAMES:
         {
-            const int page = sbufBytesRemaining(arg) ? sbufReadU8(arg) : 0;
+            const int page = sbufBytesRemaining(src) ? sbufReadU8(src) : 0;
             serializeBoxReply(dst, page, &serializeBoxNameFn);
         }
         break;
     case MSP_BOXIDS:
         {
-            const int page = sbufBytesRemaining(arg) ? sbufReadU8(arg) : 0;
+            const int page = sbufBytesRemaining(src) ? sbufReadU8(src) : 0;
             serializeBoxReply(dst, page, &serializeBoxPermanentIdFn);
         }
+        break;
+#endif
+    case MSP_REBOOT:
+        if (sbufBytesRemaining(src)) {
+            rebootMode = sbufReadU8(src);
+
+            if (rebootMode >= MSP_REBOOT_COUNT
+#if !defined(USE_USB_MSC)
+                || rebootMode == MSP_REBOOT_MSC
+#endif
+                ) {
+                return MSP_RESULT_ERROR;
+            }
+        } else {
+            rebootMode = MSP_REBOOT_FIRMWARE;
+        }
+
+        sbufWriteU8(dst, rebootMode);
+
+#if defined(USE_USB_MSC)
+        if (rebootMode == MSP_REBOOT_MSC) {
+            if (mscCheckFilesystemReady()) {
+                sbufWriteU8(dst, 1);
+            } else {
+                sbufWriteU8(dst, 0);
+
+                return MSP_RESULT_ACK;
+            }
+        }
+#endif
+
+        if (mspPostProcessFn) {
+            *mspPostProcessFn = mspRebootFn;
+        }
+
         break;
     default:
         return MSP_RESULT_CMD_UNKNOWN;
     }
     return MSP_RESULT_ACK;
 }
-#endif // USE_OSD_SLAVE
 
 #ifdef USE_FLASHFS
 static void mspFcDataFlashReadCommand(sbuf_t *dst, sbuf_t *src)
@@ -2130,8 +2196,9 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
 }
 #endif // USE_OSD_SLAVE
 
-static mspResult_e mspCommonProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
+static mspResult_e mspCommonProcessInCommand(uint8_t cmdMSP, sbuf_t *src, mspPostProcessFnPtr *mspPostProcessFn)
 {
+    UNUSED(mspPostProcessFn);
     const unsigned int dataSize = sbufBytesRemaining(src);
     UNUSED(dataSize); // maybe unused due to compiler options
 
@@ -2329,10 +2396,8 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro
         ret = MSP_RESULT_ACK;
     } else if (mspProcessOutCommand(cmdMSP, dst)) {
         ret = MSP_RESULT_ACK;
-#ifndef USE_OSD_SLAVE
+    } else if ((ret = mspFcProcessOutCommandWithArg(cmdMSP, src, dst, mspPostProcessFn)) != MSP_RESULT_CMD_UNKNOWN) {
-    } else if ((ret = mspFcProcessOutCommandWithArg(cmdMSP, src, dst)) != MSP_RESULT_CMD_UNKNOWN) {
         /* ret */;
-#endif
 #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
     } else if (cmdMSP == MSP_SET_4WAY_IF) {
         mspFc4waySerialCommand(dst, src, mspPostProcessFn);
@@ -2344,7 +2409,7 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro
         ret = MSP_RESULT_ACK;
 #endif
     } else {
-        ret = mspCommonProcessInCommand(cmdMSP, src);
+        ret = mspCommonProcessInCommand(cmdMSP, src, mspPostProcessFn);
     }
     reply->result = ret;
     return ret;

src/main/io/usb_msc.c

@@ -0,0 +1,42 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+
+#include "platform.h"
+
+#include "drivers/sdcard.h"
+
+#include "io/flashfs.h"
+
+#if defined(USE_USB_MSC)
+
+bool mscCheckFilesystemReady(void)
+{
+    return false
+#if defined(USE_SDCARD)
+        || sdcard_isFunctional()
+#endif
+#if defined(USE_FLASHFS)
+        || flashfsGetSize() > 0
+#endif
+        ;
+}
+#endif

src/main/io/usb_msc.h

@@ -0,0 +1,23 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+bool mscCheckFilesystemReady(void);

src/main/target/common_fc_post.h

@@ -136,7 +136,7 @@
 #undef USE_ADC_INTERNAL
 #endif
 
-#if !defined(USE_SDCARD) && !defined(USE_FLASHFS)
+#if (!defined(USE_SDCARD) && !defined(USE_FLASHFS)) || !(defined(STM32F4) || defined(STM32F7))
 #undef USE_USB_MSC
 #endif