From 9d5fa7311ece6ed4d9b543ea770608d00ee0db15 Mon Sep 17 00:00:00 2001 From: Chris Date: Fri, 6 Apr 2018 02:53:26 +0200 Subject: [PATCH] Add MSC for F7 (#5629) * Add MSC for F7 * Fix compilation error. --- make/mcu/STM32F7.mk | 12 ++- src/main/drivers/usb_msc.h | 5 ++ src/main/drivers/usb_msc_f4xx.c | 10 ++- src/main/drivers/usb_msc_f7xx.c | 129 +++++++++++++++++++++++++++++ src/main/fc/fc_init.c | 2 +- src/main/interface/cli.c | 6 +- src/main/msc/usbd_storage.h | 27 ++++++ src/main/msc/usbd_storage_sd_spi.c | 37 ++++++++- src/main/msc/usbd_storage_sdio.c | 40 ++++++++- src/main/target/common_fc_pre.h | 1 + src/main/vcp_hal/usbd_conf.c | 9 ++ src/main/vcp_hal/usbd_desc.c | 37 +++++++++ 12 files changed, 306 insertions(+), 9 deletions(-) create mode 100644 src/main/drivers/usb_msc_f7xx.c create mode 100644 src/main/msc/usbd_storage.h diff --git a/make/mcu/STM32F7.mk b/make/mcu/STM32F7.mk index acdd7a6860..9e576cdde4 100644 --- a/make/mcu/STM32F7.mk +++ b/make/mcu/STM32F7.mk @@ -180,9 +180,19 @@ MCU_EXCLUDES = \ drivers/timer.c \ drivers/serial_uart.c +MSC_SRC = \ + drivers/usb_msc_f7xx.c + ifneq ($(filter SDIO,$(FEATURES)),) MCU_COMMON_SRC += \ - drivers/sdio_f7xx.c + drivers/sdio_f7xx.c +MSC_SRC += \ + msc/usbd_storage_sdio.c +endif + +ifneq ($(filter SDCARD,$(FEATURES)),) +MSC_SRC += \ + msc/usbd_storage_sd_spi.c endif DSP_LIB := $(ROOT)/lib/main/CMSIS/DSP diff --git a/src/main/drivers/usb_msc.h b/src/main/drivers/usb_msc.h index 4f92c5831a..58c3f9d109 100644 --- a/src/main/drivers/usb_msc.h +++ b/src/main/drivers/usb_msc.h @@ -18,7 +18,12 @@ * */ +#pragma once + +#define MSC_MAGIC 0xDDDD1010 + void mscInit(void); +bool mscCheckBoot(void); uint8_t mscStart(void); bool mscCheckButton(void); void mscWaitForButton(void); diff --git a/src/main/drivers/usb_msc_f4xx.c b/src/main/drivers/usb_msc_f4xx.c index 66683d8ff2..d6bb66fd4d 100644 --- a/src/main/drivers/usb_msc_f4xx.c +++ b/src/main/drivers/usb_msc_f4xx.c @@ -91,6 +91,14 @@ uint8_t mscStart(void) return 0; } +bool mscCheckBoot(void) +{ + if (*((uint32_t *)0x2001FFF0) == MSC_MAGIC) { + return true; + } + return false; +} + bool mscCheckButton(void) { bool result = false; @@ -114,7 +122,7 @@ void mscWaitForButton(void) while (true) { asm("NOP"); if (mscCheckButton()) { - *((uint32_t *)0x2001FFF0) = 0xDDDD1011; + *((uint32_t *)0x2001FFF0) = 0xFFFFFFFF; delay(1); NVIC_SystemReset(); } diff --git a/src/main/drivers/usb_msc_f7xx.c b/src/main/drivers/usb_msc_f7xx.c new file mode 100644 index 0000000000..22ad5da42a --- /dev/null +++ b/src/main/drivers/usb_msc_f7xx.c @@ -0,0 +1,129 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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. + * + * Cleanflight 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. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + * + * Author: Chris Hockuba (https://github.com/conkerkh) + * + */ + +#include +#include +#include + +#include "platform.h" + +#if defined(USE_USB_MSC) + +#include "build/build_config.h" + +#include "common/utils.h" + +#include "drivers/io.h" +#include "drivers/light_led.h" +#include "drivers/nvic.h" +#include "drivers/time.h" +#include "drivers/usb_msc.h" + +#include "pg/usb.h" + +#include "vcp_hal/usbd_cdc_interface.h" +#include "usb_io.h" +#include "usbd_msc.h" +#include "msc/usbd_storage.h" +USBD_HandleTypeDef USBD_Device; + +#define DEBOUNCE_TIME_MS 20 + +static IO_t mscButton; + +void mscInit(void) +{ + if (usbDevConfig()->mscButtonPin) { + mscButton = IOGetByTag(usbDevConfig()->mscButtonPin); + IOInit(mscButton, OWNER_USB_MSC_PIN, 0); + if (usbDevConfig()->mscButtonUsePullup) { + IOConfigGPIO(mscButton, IOCFG_IPU); + } else { + IOConfigGPIO(mscButton, IOCFG_IPD); + } + } +} + +uint8_t mscStart(void) +{ + ledInit(statusLedConfig()); + + //Start USB + usbGenerateDisconnectPulse(); + + IOInit(IOGetByTag(IO_TAG(PA11)), OWNER_USB, 0); + IOInit(IOGetByTag(IO_TAG(PA12)), OWNER_USB, 0); + + USBD_Init(&USBD_Device, &VCP_Desc, 0); + + /** Regsiter class */ + USBD_RegisterClass(&USBD_Device, USBD_MSC_CLASS); + + /** Register interface callbacks */ + USBD_MSC_RegisterStorage(&USBD_Device, &USBD_MSC_Template_fops); + + USBD_Start(&USBD_Device); + + // NVIC configuration for SYSTick + NVIC_DisableIRQ(SysTick_IRQn); + NVIC_SetPriority(SysTick_IRQn, NVIC_BUILD_PRIORITY(0, 0)); + NVIC_EnableIRQ(SysTick_IRQn); + + return 0; +} + +bool mscCheckBoot(void) +{ + if (*((__IO uint32_t *)BKPSRAM_BASE + 16) == MSC_MAGIC) { + return true; + } + return false; +} + +bool mscCheckButton(void) +{ + bool result = false; + if (mscButton) { + uint8_t state = IORead(mscButton); + if (usbDevConfig()->mscButtonUsePullup) { + result = state == 0; + } else { + result = state == 1; + } + } + + return result; +} + +void mscWaitForButton(void) +{ + // In order to exit MSC mode simply disconnect the board, or push the button again. + while (mscCheckButton()); + delay(DEBOUNCE_TIME_MS); + while (true) { + asm("NOP"); + if (mscCheckButton()) { + *((uint32_t *)0x2001FFF0) = 0xFFFFFFFF; + delay(1); + NVIC_SystemReset(); + } + } +} +#endif diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 9e204e68db..3f60f923ff 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -458,7 +458,7 @@ void init(void) /* MSC mode will start after init, but will not allow scheduler to run, * so there is no bottleneck in reading and writing data */ mscInit(); - if (*((uint32_t *)0x2001FFF0) == 0xDDDD1010 || mscCheckButton()) { + if (mscCheckBoot() || mscCheckButton()) { if (mscStart() == 0) { mscWaitForButton(); } else { diff --git a/src/main/interface/cli.c b/src/main/interface/cli.c index c70a6b1a34..8623830eb2 100644 --- a/src/main/interface/cli.c +++ b/src/main/interface/cli.c @@ -3757,7 +3757,11 @@ static void cliMsc(char *cmdline) mpuResetFn(); } - *((uint32_t *)0x2001FFF0) = 0xDDDD1010; // Same location as bootloader magic but different value +#ifdef STM32F7 + *((__IO uint32_t*) BKPSRAM_BASE + 16) = MSC_MAGIC; +#elif defined(STM32F4) + *((uint32_t *)0x2001FFF0) = MSC_MAGIC; +#endif __disable_irq(); NVIC_SystemReset(); diff --git a/src/main/msc/usbd_storage.h b/src/main/msc/usbd_storage.h new file mode 100644 index 0000000000..5b6f7957eb --- /dev/null +++ b/src/main/msc/usbd_storage.h @@ -0,0 +1,27 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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. + * + * Cleanflight 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. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + * + * Author: Chris Hockuba (https://github.com/conkerkh) + * + */ + +#pragma once + +#include "platform.h" + +#ifdef USE_HAL_DRIVER +extern USBD_StorageTypeDef USBD_MSC_Template_fops; +#endif diff --git a/src/main/msc/usbd_storage_sd_spi.c b/src/main/msc/usbd_storage_sd_spi.c index f58c771047..3f427bb9a2 100644 --- a/src/main/msc/usbd_storage_sd_spi.c +++ b/src/main/msc/usbd_storage_sd_spi.c @@ -31,10 +31,14 @@ #include #include "platform.h" +#include "common/utils.h" +#ifdef USE_HAL_DRIVER +#include "usbd_msc.h" +#else #include "usbd_msc_mem.h" #include "usbd_msc_core.h" -#include "common/utils.h" +#endif #include "drivers/sdcard.h" #include "drivers/light_led.h" @@ -57,12 +61,20 @@ */ #define STORAGE_LUN_NBR 1 +#define STORAGE_BLK_NBR 0x10000 +#define STORAGE_BLK_SIZ 0x200 int8_t STORAGE_Init (uint8_t lun); +#ifdef USE_HAL_DRIVER +int8_t STORAGE_GetCapacity (uint8_t lun, + uint32_t *block_num, + uint16_t *block_size); +#else int8_t STORAGE_GetCapacity (uint8_t lun, uint32_t *block_num, uint32_t *block_size); +#endif int8_t STORAGE_IsReady (uint8_t lun); @@ -88,7 +100,11 @@ uint8_t STORAGE_Inquirydata[] = {//36 0x80, 0x02, 0x02, +#ifdef USE_HAL_DRIVER + (STANDARD_INQUIRY_DATA_LEN - 5), +#else (USBD_STD_INQUIRY_LENGTH - 5), +#endif 0x00, 0x00, 0x00, @@ -98,6 +114,19 @@ uint8_t STORAGE_Inquirydata[] = {//36 '0', '.', '0' ,'1', /* Version : 4 Bytes */ }; +#ifdef USE_HAL_DRIVER +USBD_StorageTypeDef USBD_MSC_Template_fops = +{ + STORAGE_Init, + STORAGE_GetCapacity, + STORAGE_IsReady, + STORAGE_IsWriteProtected, + STORAGE_Read, + STORAGE_Write, + STORAGE_GetMaxLun, + (int8_t*)STORAGE_Inquirydata, +}; +#else USBD_STORAGE_cb_TypeDef USBD_MICRO_SDIO_fops = { STORAGE_Init, @@ -108,10 +137,10 @@ USBD_STORAGE_cb_TypeDef USBD_MICRO_SDIO_fops = STORAGE_Write, STORAGE_GetMaxLun, (int8_t*)STORAGE_Inquirydata, - }; USBD_STORAGE_cb_TypeDef *USBD_STORAGE_fops = &USBD_MICRO_SDIO_fops; +#endif /******************************************************************************* * Function Name : Read_Memory * Description : Handle the Read operation from the microSD card. @@ -136,7 +165,11 @@ int8_t STORAGE_Init (uint8_t lun) * Output : None. * Return : None. *******************************************************************************/ +#ifdef USE_HAL_DRIVER +int8_t STORAGE_GetCapacity (uint8_t lun, uint32_t *block_num, uint16_t *block_size) +#else int8_t STORAGE_GetCapacity (uint8_t lun, uint32_t *block_num, uint32_t *block_size) +#endif { UNUSED(lun); *block_num = sdcard_getMetadata()->numBlocks; diff --git a/src/main/msc/usbd_storage_sdio.c b/src/main/msc/usbd_storage_sdio.c index 1ea0968dcc..3d5e335a0b 100644 --- a/src/main/msc/usbd_storage_sdio.c +++ b/src/main/msc/usbd_storage_sdio.c @@ -32,13 +32,18 @@ #include "platform.h" -#include "usbd_msc_mem.h" -#include "usbd_msc_core.h" #include "drivers/sdmmc_sdio.h" #include "drivers/light_led.h" #include "drivers/io.h" #include "common/utils.h" +#ifdef USE_HAL_DRIVER +#include "usbd_msc.h" +#else +#include "usbd_msc_mem.h" +#include "usbd_msc_core.h" +#endif + /* Private typedef -----------------------------------------------------------*/ /* Private define ------------------------------------------------------------*/ /* Private macro -------------------------------------------------------------*/ @@ -52,12 +57,20 @@ */ #define STORAGE_LUN_NBR 1 +#define STORAGE_BLK_NBR 0x10000 +#define STORAGE_BLK_SIZ 0x200 int8_t STORAGE_Init (uint8_t lun); +#ifdef USE_HAL_DRIVER +int8_t STORAGE_GetCapacity (uint8_t lun, + uint32_t *block_num, + uint16_t *block_size); +#else int8_t STORAGE_GetCapacity (uint8_t lun, uint32_t *block_num, uint32_t *block_size); +#endif int8_t STORAGE_IsReady (uint8_t lun); @@ -83,7 +96,11 @@ uint8_t STORAGE_Inquirydata[] = {//36 0x80, 0x02, 0x02, +#ifdef USE_HAL_DRIVER + (STANDARD_INQUIRY_DATA_LEN - 5), +#else (USBD_STD_INQUIRY_LENGTH - 5), +#endif 0x00, 0x00, 0x00, @@ -93,6 +110,19 @@ uint8_t STORAGE_Inquirydata[] = {//36 '0', '.', '0' ,'1', /* Version : 4 Bytes */ }; +#ifdef USE_HAL_DRIVER +USBD_StorageTypeDef USBD_MSC_Template_fops = +{ + STORAGE_Init, + STORAGE_GetCapacity, + STORAGE_IsReady, + STORAGE_IsWriteProtected, + STORAGE_Read, + STORAGE_Write, + STORAGE_GetMaxLun, + (int8_t*)STORAGE_Inquirydata, +}; +#else USBD_STORAGE_cb_TypeDef USBD_MICRO_SDIO_fops = { STORAGE_Init, @@ -103,10 +133,10 @@ USBD_STORAGE_cb_TypeDef USBD_MICRO_SDIO_fops = STORAGE_Write, STORAGE_GetMaxLun, (int8_t*)STORAGE_Inquirydata, - }; USBD_STORAGE_cb_TypeDef *USBD_STORAGE_fops = &USBD_MICRO_SDIO_fops; +#endif /******************************************************************************* * Function Name : Read_Memory * Description : Handle the Read operation from the microSD card. @@ -138,7 +168,11 @@ int8_t STORAGE_Init (uint8_t lun) * Output : None. * Return : None. *******************************************************************************/ +#ifdef USE_HAL_DRIVER +int8_t STORAGE_GetCapacity (uint8_t lun, uint32_t *block_num, uint16_t *block_size) +#else int8_t STORAGE_GetCapacity (uint8_t lun, uint32_t *block_num, uint32_t *block_size) +#endif { UNUSED(lun); if (SD_IsDetected() == 0) { diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index 9b96ad572b..5e2eab48a4 100644 --- a/src/main/target/common_fc_pre.h +++ b/src/main/target/common_fc_pre.h @@ -70,6 +70,7 @@ #define USE_OVERCLOCK #define USE_ADC_INTERNAL #define USE_USB_CDC_HID +#define USE_USB_MSC #endif #if defined(STM32F4) || defined(STM32F7) diff --git a/src/main/vcp_hal/usbd_conf.c b/src/main/vcp_hal/usbd_conf.c index 7e960288cb..2258b13418 100644 --- a/src/main/vcp_hal/usbd_conf.c +++ b/src/main/vcp_hal/usbd_conf.c @@ -57,6 +57,10 @@ #include "pg/pg.h" #include "pg/usb.h" +#ifdef USE_USB_MSC +#include "drivers/usb_msc.h" +#endif + /* Private typedef -----------------------------------------------------------*/ /* Private define ------------------------------------------------------------*/ /* Private macro -------------------------------------------------------------*/ @@ -406,8 +410,13 @@ USBD_StatusTypeDef USBD_LL_Init(USBD_HandleTypeDef *pdev) /* Initialize LL Driver */ HAL_PCD_Init(&hpcd); + #ifdef USE_USB_CDC_HID +#ifdef USE_USB_MSC + if (usbDevConfig()->type == COMPOSITE && !mscCheckBoot()) { +#else if (usbDevConfig()->type == COMPOSITE) { +#endif HAL_PCDEx_SetRxFiFo(&hpcd, 0x80); HAL_PCDEx_SetTxFiFo(&hpcd, 0, 0x20); HAL_PCDEx_SetTxFiFo(&hpcd, 1, 0x40); diff --git a/src/main/vcp_hal/usbd_desc.c b/src/main/vcp_hal/usbd_desc.c index 701d25c1f4..bd0a94ba3a 100644 --- a/src/main/vcp_hal/usbd_desc.c +++ b/src/main/vcp_hal/usbd_desc.c @@ -54,6 +54,10 @@ #include "pg/pg.h" #include "pg/usb.h" +#ifdef USE_USB_MSC +#include "drivers/usb_msc.h" +#endif + /* Private typedef -----------------------------------------------------------*/ /* Private define ------------------------------------------------------------*/ #define USBD_VID 0x0483 @@ -120,6 +124,33 @@ __ALIGN_BEGIN uint8_t USBD_DeviceDesc[USB_LEN_DEV_DESC] __ALIGN_END = { extern uint8_t USBD_HID_CDC_DeviceDescriptor[USB_LEN_DEV_DESC]; #endif +#ifdef USE_USB_MSC + +#define USBD_PID_MSC 22314 + +__ALIGN_BEGIN uint8_t USBD_MSC_DeviceDesc[USB_LEN_DEV_DESC] __ALIGN_END = +{ + 0x12, /*bLength */ + USB_DESC_TYPE_DEVICE, /*bDescriptorType*/ + 0x00, /*bcdUSB */ + 0x02, + 0x00, /*bDeviceClass*/ + 0x00, /*bDeviceSubClass*/ + 0x00, /*bDeviceProtocol*/ + USB_MAX_EP0_SIZE, /*bMaxPacketSize*/ + LOBYTE(USBD_VID), /*idVendor*/ + HIBYTE(USBD_VID), /*idVendor*/ + LOBYTE(USBD_PID_MSC), /*idProduct*/ + HIBYTE(USBD_PID_MSC), /*idProduct*/ + 0x00, /*bcdDevice rel. 2.00*/ + 0x02, + USBD_IDX_MFC_STR, /*Index of manufacturer string*/ + USBD_IDX_PRODUCT_STR, /*Index of product string*/ + USBD_IDX_SERIAL_STR, /*Index of serial number string*/ + USBD_MAX_NUM_CONFIGURATION /*bNumConfigurations*/ +}; +#endif + /* USB Standard Device Descriptor */ #if defined ( __ICCARM__ ) /*!< IAR Compiler */ #pragma data_alignment=4 @@ -155,6 +186,12 @@ static void Get_SerialNum(void); uint8_t *USBD_VCP_DeviceDescriptor(USBD_SpeedTypeDef speed, uint16_t *length) { (void)speed; +#ifdef USE_USB_MSC + if (mscCheckBoot()) { + *length = sizeof(USBD_MSC_DeviceDesc); + return USBD_MSC_DeviceDesc; + } +#endif #ifdef USE_USB_CDC_HID if (usbDevConfig()->type == COMPOSITE) { *length = sizeof(USBD_HID_CDC_DeviceDescriptor);