From df55fc99ec9b036fa618438cca5a2cc0f83cfb9a Mon Sep 17 00:00:00 2001 From: mikeller Date: Sun, 15 Jul 2018 18:48:12 +1200 Subject: [PATCH 1/2] Added MSP for reboot to MSC. --- make/source.mk | 1 + src/main/drivers/usb_msc.h | 1 + src/main/drivers/usb_msc_f4xx.c | 24 ++++---- src/main/drivers/usb_msc_f7xx.c | 14 +++++ src/main/interface/cli.c | 44 +++++---------- src/main/interface/msp.c | 97 ++++++++++++++++++++++++-------- src/main/io/usb_msc.c | 42 ++++++++++++++ src/main/io/usb_msc.h | 23 ++++++++ src/main/target/common_fc_post.h | 2 +- 9 files changed, 183 insertions(+), 65 deletions(-) create mode 100644 src/main/io/usb_msc.c create mode 100644 src/main/io/usb_msc.h diff --git a/make/source.mk b/make/source.mk index 142ab4857..949859dce 100644 --- a/make/source.mk +++ b/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 \ diff --git a/src/main/drivers/usb_msc.h b/src/main/drivers/usb_msc.h index 3995b3c17..d2467894a 100644 --- a/src/main/drivers/usb_msc.h +++ b/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); diff --git a/src/main/drivers/usb_msc_f4xx.c b/src/main/drivers/usb_msc_f4xx.c index 920be99fe..a7c456131 100644 --- a/src/main/drivers/usb_msc_f4xx.c +++ b/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 diff --git a/src/main/drivers/usb_msc_f7xx.c b/src/main/drivers/usb_msc_f7xx.c index 65af03f35..776e8a47d 100644 --- a/src/main/drivers/usb_msc_f7xx.c +++ b/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 diff --git a/src/main/interface/cli.c b/src/main/interface/cli.c index bb1ada1e8..7af05009e 100644 --- a/src/main/interface/cli.c +++ b/src/main/interface/cli.c @@ -11,7 +11,7 @@ * 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. * @@ -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/camera_control.h" -#include "drivers/vtx_common.h" +#include "drivers/transponder_ir.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 -#ifdef USE_SDCARD - || sdcard_isFunctional() -#endif -#ifdef USE_FLASHFS - || flashfsGetSize() > 0 -#endif - ) { - cliPrintHashLine("restarting in mass storage mode"); + if (mscCheckFilesystemReady()) { + cliPrintHashLine("Restarting in mass storage mode"); cliPrint("\r\nRebooting"); bufWriterFlush(cliWriter); - delay(1000); waitForSerialPortToFinishTransmitting(cliPort); stopPwmAllMotors(); - if (mpuResetFn) { - mpuResetFn(); - } -#ifdef STM32F7 - *((__IO uint32_t*) BKPSRAM_BASE + 16) = MSC_MAGIC; -#elif defined(STM32F4) - *((uint32_t *)0x2001FFF0) = MSC_MAGIC; -#endif - - __disable_irq(); - NVIC_SystemReset(); + systemResetToMsc(); } else { - cliPrint("\r\nStorage not present or failed to initialize!"); - bufWriterFlush(cliWriter); + cliPrintHashLine("Storage not present or failed to initialize!"); } } #endif diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c index 0cff0d30d..e7697e426 100644 --- a/src/main/interface/msp.c +++ b/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,7 +243,26 @@ static void mspRebootFn(serialPort_t *serialPort) #ifndef USE_OSD_SLAVE stopPwmAllMotors(); #endif - systemReset(); + + 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,54 @@ 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 || (rebootMode == MSP_REBOOT_MSC +#if defined(USE_USB_MSC) + && !mscCheckFilesystemReady() +#endif + )) { + return MSP_RESULT_ERROR; + } + } else { + rebootMode = MSP_REBOOT_FIRMWARE; + } + + 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 +2182,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 +2382,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)) != MSP_RESULT_CMD_UNKNOWN) { + } else if ((ret = mspFcProcessOutCommandWithArg(cmdMSP, src, dst, mspPostProcessFn)) != 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 +2395,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; diff --git a/src/main/io/usb_msc.c b/src/main/io/usb_msc.c new file mode 100644 index 000000000..adb67e174 --- /dev/null +++ b/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 . + */ + +#include + +#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 diff --git a/src/main/io/usb_msc.h b/src/main/io/usb_msc.h new file mode 100644 index 000000000..35164c7de --- /dev/null +++ b/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 . + */ + +#pragma once + +bool mscCheckFilesystemReady(void); diff --git a/src/main/target/common_fc_post.h b/src/main/target/common_fc_post.h index 09c037a42..d02ddc656 100644 --- a/src/main/target/common_fc_post.h +++ b/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 From aff565398187777360ba23adbce34d39a91fff15 Mon Sep 17 00:00:00 2001 From: mikeller Date: Sun, 15 Jul 2018 23:37:19 +1200 Subject: [PATCH 2/2] Changes for an improved UX. --- src/main/interface/msp.c | 22 ++++++++++++++++++---- 1 file changed, 18 insertions(+), 4 deletions(-) diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c index e7697e426..cef9069c7 100644 --- a/src/main/interface/msp.c +++ b/src/main/interface/msp.c @@ -1384,17 +1384,31 @@ static mspResult_e mspFcProcessOutCommandWithArg(uint8_t cmdMSP, sbuf_t *src, sb if (sbufBytesRemaining(src)) { rebootMode = sbufReadU8(src); - if (rebootMode >= MSP_REBOOT_COUNT || (rebootMode == MSP_REBOOT_MSC -#if defined(USE_USB_MSC) - && !mscCheckFilesystemReady() + 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; }