From 04844bd5a1f46d877feac07331df8aea8d75e7ab Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Sun, 1 Dec 2019 10:22:59 -0500 Subject: [PATCH] Refactor common USB MSC code and improve activity LED Eliminated the duplicated MSC functions in the architecture specific files and moved to a shared common. Improved the activity indicating LED and made it consistent between onboard flash and sd card mass storage mode. --- make/mcu/STM32F4.mk | 1 + make/mcu/STM32F7.mk | 1 + make/mcu/STM32H7.mk | 1 + src/main/drivers/usb_msc.h | 2 + src/main/drivers/usb_msc_common.c | 148 ++++++++++++++++++++++++ src/main/drivers/usb_msc_f4xx.c | 82 +------------- src/main/drivers/usb_msc_f7xx.c | 82 +------------- src/main/fc/init.c | 2 +- src/main/msc/emfat_file.c | 39 +++---- src/main/msc/usbd_storage_emfat.c | 4 +- src/main/msc/usbd_storage_sd_spi.c | 15 ++- src/main/msc/usbd_storage_sdio.c | 174 ++++++++++++++--------------- 12 files changed, 272 insertions(+), 279 deletions(-) create mode 100644 src/main/drivers/usb_msc_common.c diff --git a/make/mcu/STM32F4.mk b/make/mcu/STM32F4.mk index 6a756f7de..234d58bd8 100644 --- a/make/mcu/STM32F4.mk +++ b/make/mcu/STM32F4.mk @@ -208,6 +208,7 @@ VCP_SRC = \ endif MSC_SRC = \ + drivers/usb_msc_common.c \ drivers/usb_msc_f4xx.c \ msc/usbd_msc_desc.c \ msc/usbd_storage.c diff --git a/make/mcu/STM32F7.mk b/make/mcu/STM32F7.mk index ba62d7aad..5c232f84c 100644 --- a/make/mcu/STM32F7.mk +++ b/make/mcu/STM32F7.mk @@ -192,6 +192,7 @@ MCU_EXCLUDES = \ drivers/timer.c MSC_SRC = \ + drivers/usb_msc_common.c \ drivers/usb_msc_f7xx.c \ msc/usbd_storage.c diff --git a/make/mcu/STM32H7.mk b/make/mcu/STM32H7.mk index 971b673fb..e8be80be4 100644 --- a/make/mcu/STM32H7.mk +++ b/make/mcu/STM32H7.mk @@ -256,6 +256,7 @@ MCU_EXCLUDES = \ drivers/timer.c #MSC_SRC = \ +# drivers/usb_msc_common.c \ # drivers/usb_msc_h7xx.c \ # msc/usbd_storage.c diff --git a/src/main/drivers/usb_msc.h b/src/main/drivers/usb_msc.h index 72ef17530..4b5303dc2 100644 --- a/src/main/drivers/usb_msc.h +++ b/src/main/drivers/usb_msc.h @@ -31,3 +31,5 @@ bool mscCheckButton(void); void mscWaitForButton(void); void systemResetToMsc(int timezoneOffsetMinutes); void systemResetFromMsc(void); +void mscSetActive(void); +void mscActivityLed(void); diff --git a/src/main/drivers/usb_msc_common.c b/src/main/drivers/usb_msc_common.c new file mode 100644 index 000000000..ada2bdfb6 --- /dev/null +++ b/src/main/drivers/usb_msc_common.c @@ -0,0 +1,148 @@ +/* + * 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 . + */ + +/* + * 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/persistent.h" +#include "drivers/system.h" +#include "drivers/time.h" +#include "drivers/usb_msc.h" + +#include "msc/usbd_storage.h" + +#include "pg/usb.h" + +#define DEBOUNCE_TIME_MS 20 +#define ACTIVITY_LED_PERIOD_MS 50 + +static IO_t mscButton; +static timeMs_t lastActiveTimeMs = 0; + +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); + } + } +} + +bool mscCheckBoot(void) +{ + const uint32_t bootModeRequest = persistentObjectRead(PERSISTENT_OBJECT_RESET_REASON); + return bootModeRequest == RESET_MSC_REQUEST; + // Note that we can't clear the persisent object after checking here. This is because + // this function is called multiple times during initialization. So we clear on a reset + // out of MSC mode. +} + +void mscSetActive(void) +{ + lastActiveTimeMs = millis(); +} + +void mscActivityLed(void) +{ + static timeMs_t nextToggleMs = 0; + const timeMs_t nowMs = millis(); + + if (nowMs - lastActiveTimeMs > ACTIVITY_LED_PERIOD_MS) { + LED0_OFF; + nextToggleMs = 0; + } else if (nowMs > nextToggleMs) { + LED0_TOGGLE; + nextToggleMs = nowMs + ACTIVITY_LED_PERIOD_MS; + } +} + +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()) { + systemResetFromMsc(); + } + mscActivityLed(); + } +} + +void systemResetToMsc(int timezoneOffsetMinutes) +{ + persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_MSC_REQUEST); + + __disable_irq(); + + // Persist the RTC across the reboot to use as the file timestamp +#ifdef USE_PERSISTENT_MSC_RTC + rtcPersistWrite(timezoneOffsetMinutes); +#else + UNUSED(timezoneOffsetMinutes); +#endif + NVIC_SystemReset(); +} + +void systemResetFromMsc(void) +{ + persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_NONE); + __disable_irq(); + NVIC_SystemReset(); +} + +#endif diff --git a/src/main/drivers/usb_msc_f4xx.c b/src/main/drivers/usb_msc_f4xx.c index b7608b87a..66b0fcf43 100644 --- a/src/main/drivers/usb_msc_f4xx.c +++ b/src/main/drivers/usb_msc_f4xx.c @@ -38,15 +38,13 @@ #include "blackbox/blackbox.h" #include "drivers/io.h" -#include "drivers/light_led.h" #include "drivers/nvic.h" -#include "drivers/persistent.h" #include "drivers/sdmmc_sdio.h" #include "drivers/system.h" #include "drivers/time.h" #include "drivers/usb_msc.h" -#include "drivers/accgyro/accgyro_mpu.h" +#include "msc/usbd_storage.h" #include "pg/sdcard.h" #include "pg/usb.h" @@ -55,25 +53,6 @@ #include "usbd_cdc_vcp.h" #include "usb_io.h" -#include "msc/usbd_storage.h" - -#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) { //Start USB @@ -121,63 +100,4 @@ uint8_t mscStart(void) return 0; } -bool mscCheckBoot(void) -{ - const uint32_t bootModeRequest = persistentObjectRead(PERSISTENT_OBJECT_RESET_REASON); - return bootModeRequest == RESET_MSC_REQUEST; - // Note that we can't clear the persisent object after checking here. This is because - // this function is called multiple times during initialization. So we clear on a reset - // out of MSC mode. -} - -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()) { - systemResetFromMsc(); - } - } -} - -void systemResetToMsc(int timezoneOffsetMinutes) -{ - persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_MSC_REQUEST); - - __disable_irq(); - - // Persist the RTC across the reboot to use as the file timestamp -#ifdef USE_PERSISTENT_MSC_RTC - rtcPersistWrite(timezoneOffsetMinutes); -#else - UNUSED(timezoneOffsetMinutes); -#endif - NVIC_SystemReset(); -} - -void systemResetFromMsc(void) -{ - persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_NONE); - __disable_irq(); - NVIC_SystemReset(); -} - #endif diff --git a/src/main/drivers/usb_msc_f7xx.c b/src/main/drivers/usb_msc_f7xx.c index d6b4c1887..9fbf62c8e 100644 --- a/src/main/drivers/usb_msc_f7xx.c +++ b/src/main/drivers/usb_msc_f7xx.c @@ -37,40 +37,21 @@ #include "blackbox/blackbox.h" #include "drivers/io.h" -#include "drivers/light_led.h" #include "drivers/nvic.h" -#include "drivers/persistent.h" #include "drivers/serial_usb_vcp.h" #include "drivers/system.h" #include "drivers/time.h" #include "drivers/usb_msc.h" -#include "drivers/accgyro/accgyro_mpu.h" +#include "msc/usbd_storage.h" #include "pg/sdcard.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" - -#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) { @@ -126,63 +107,4 @@ uint8_t mscStart(void) return 0; } -bool mscCheckBoot(void) -{ - const uint32_t bootModeRequest = persistentObjectRead(PERSISTENT_OBJECT_RESET_REASON); - return bootModeRequest == RESET_MSC_REQUEST; - // Note that we can't clear the persisent object after checking here. This is because - // this function is called multiple times during initialization. So we clear on a reset - // out of MSC mode. -} - -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()) { - systemResetFromMsc(); - } - } -} - -void systemResetToMsc(int timezoneOffsetMinutes) -{ - persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_MSC_REQUEST); - - __disable_irq(); - - // Persist the RTC across the reboot to use as the file timestamp -#ifdef USE_PERSISTENT_MSC_RTC - rtcPersistWrite(timezoneOffsetMinutes); -#else - UNUSED(timezoneOffsetMinutes); -#endif - NVIC_SystemReset(); -} - -void systemResetFromMsc(void) -{ - persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_NONE); - __disable_irq(); - NVIC_SystemReset(); -} - #endif diff --git a/src/main/fc/init.c b/src/main/fc/init.c index 5ff9b09f7..350358e80 100644 --- a/src/main/fc/init.c +++ b/src/main/fc/init.c @@ -614,7 +614,7 @@ void init(void) if (mscCheckBoot() || mscCheckButton()) { ledInit(statusLedConfig()); -#if defined(USE_FLASHFS) && defined(USE_FLASH_CHIP) +#if defined(USE_FLASHFS) // If the blackbox device is onboard flash, then initialize and scan // it to identify the log files *before* starting the USB device to // prevent timeouts of the mass storage device. diff --git a/src/main/msc/emfat_file.c b/src/main/msc/emfat_file.c index 242fdf44b..19fdf77d1 100644 --- a/src/main/msc/emfat_file.c +++ b/src/main/msc/emfat_file.c @@ -34,6 +34,7 @@ #include "drivers/flash.h" #include "drivers/light_led.h" #include "drivers/time.h" +#include "drivers/usb_msc.h" #include "io/flashfs.h" @@ -278,7 +279,6 @@ static const emfat_entry_t entriesPredefined[] = #define EMFAT_MAX_ENTRY (PREDEFINED_ENTRY_COUNT + EMFAT_MAX_LOG_ENTRY + APPENDED_ENTRY_COUNT) static emfat_entry_t entries[EMFAT_MAX_ENTRY]; -static char logNames[EMFAT_MAX_LOG_ENTRY][8+1+3]; emfat_t emfat; static uint32_t cmaTime = CMA_TIME; @@ -292,8 +292,11 @@ static void emfat_set_entry_cma(emfat_entry_t *entry) entry->cma_time[2] = cmaTime; } +#ifdef USE_FLASHFS static void emfat_add_log(emfat_entry_t *entry, int number, uint32_t offset, uint32_t size) { + static char logNames[EMFAT_MAX_LOG_ENTRY][8+1+3]; + tfp_sprintf(logNames[number], FC_FIRMWARE_IDENTIFIER "_%03d.BBL", number + 1); entry->name = logNames[number]; entry->level = 1; @@ -306,16 +309,6 @@ static void emfat_add_log(emfat_entry_t *entry, int number, uint32_t offset, uin entry->cma_time[2] = entry->cma_time[0]; } -static void blinkStatusLed(void) -{ - static timeMs_t nextToggleMs = 0; - const timeMs_t nowMs = millis(); - if (nowMs > nextToggleMs) { - LED0_TOGGLE; - nextToggleMs = nowMs + 50; // toggle the status LED every 50ms - } -} - static int emfat_find_log(emfat_entry_t *entry, int maxCount, int flashfsUsedSpace) { int lastOffset = 0; @@ -333,7 +326,9 @@ static int emfat_find_log(emfat_entry_t *entry, int maxCount, int flashfsUsedSpa for ( ; currOffset < flashfsUsedSpace ; currOffset += 2048) { // XXX 2048 = FREE_BLOCK_SIZE in io/flashfs.c - blinkStatusLed(); + mscSetActive(); + mscActivityLed(); + flashfsReadAbs(currOffset, buffer, HDR_BUF_SIZE); if (strncmp((char *)buffer, logHeader, lenLogHeader)) { @@ -413,9 +408,12 @@ static int emfat_find_log(emfat_entry_t *entry, int maxCount, int flashfsUsedSpa return logCount; } +#endif // USE_FLASHFS void emfat_init_files(void) { + int flashfsUsedSpace = 0; + int entryIndex = PREDEFINED_ENTRY_COUNT; emfat_entry_t *entry; memset(entries, 0, sizeof(entries)); @@ -427,22 +425,24 @@ void emfat_init_files(void) } #endif - flashInit(flashConfig()); - flashfsInit(); - LED0_OFF; - - const int flashfsUsedSpace = flashfsIdentifyStartOfFreeSpace(); - + // create the predefined entries for (size_t i = 0 ; i < PREDEFINED_ENTRY_COUNT ; i++) { entries[i] = entriesPredefined[i]; // These entries have timestamps corresponding to when the filesystem is mounted emfat_set_entry_cma(&entries[i]); } +#ifdef USE_FLASHFS + flashInit(flashConfig()); + flashfsInit(); + LED0_OFF; + + flashfsUsedSpace = flashfsIdentifyStartOfFreeSpace(); + // Detect and create entries for each individual log const int logCount = emfat_find_log(&entries[PREDEFINED_ENTRY_COUNT], EMFAT_MAX_LOG_ENTRY, flashfsUsedSpace); - int entryIndex = PREDEFINED_ENTRY_COUNT + logCount; + entryIndex += logCount; if (logCount > 0) { // Create the all logs entry that represents all used flash space to @@ -455,6 +455,7 @@ void emfat_init_files(void) emfat_set_entry_cma(entry); ++entryIndex; } +#endif // USE_FLASHFS // Padding file to fill out the filesystem size to FILESYSTEM_SIZE_MB if (flashfsUsedSpace * 2 < FILESYSTEM_SIZE_MB * 1024 * 1024) { diff --git a/src/main/msc/usbd_storage_emfat.c b/src/main/msc/usbd_storage_emfat.c index 66656c0f1..40d94fa06 100644 --- a/src/main/msc/usbd_storage_emfat.c +++ b/src/main/msc/usbd_storage_emfat.c @@ -33,6 +33,7 @@ #include "common/utils.h" #include "drivers/light_led.h" +#include "drivers/usb_msc.h" #include "msc/usbd_storage.h" #include "msc/usbd_storage_emfat.h" @@ -95,9 +96,8 @@ static int8_t STORAGE_Read( uint16_t blk_len) // nmber of blocks to be read { UNUSED(lun); - LED0_ON; + mscSetActive(); emfat_read(&emfat, buf, blk_addr, blk_len); - LED0_OFF; return 0; } diff --git a/src/main/msc/usbd_storage_sd_spi.c b/src/main/msc/usbd_storage_sd_spi.c index c514edfe8..4addcb581 100644 --- a/src/main/msc/usbd_storage_sd_spi.c +++ b/src/main/msc/usbd_storage_sd_spi.c @@ -35,10 +35,11 @@ #include "usbd_storage.h" -#include "drivers/sdcard.h" -#include "drivers/light_led.h" -#include "drivers/io.h" #include "drivers/bus_spi.h" +#include "drivers/io.h" +#include "drivers/light_led.h" +#include "drivers/sdcard.h" +#include "drivers/usb_msc.h" #include "pg/sdcard.h" #include "pg/bus_spi.h" @@ -148,7 +149,7 @@ static int8_t STORAGE_Init (uint8_t lun) LED0_OFF; sdcard_init(sdcardConfig()); while (sdcard_poll() == 0); - LED0_ON; + mscSetActive(); return 0; } @@ -214,12 +215,11 @@ static int8_t STORAGE_Read (uint8_t lun, uint16_t blk_len) { UNUSED(lun); - LED1_ON; for (int i = 0; i < blk_len; i++) { while (sdcard_readBlock(blk_addr + i, buf + (512 * i), NULL, NULL) == 0); while (sdcard_poll() == 0); } - LED1_OFF; + mscSetActive(); return 0; } /******************************************************************************* @@ -235,14 +235,13 @@ static int8_t STORAGE_Write (uint8_t lun, uint16_t blk_len) { UNUSED(lun); - LED1_ON; for (int i = 0; i < blk_len; i++) { while (sdcard_writeBlock(blk_addr + i, buf + (i * 512), NULL, NULL) != SDCARD_OPERATION_IN_PROGRESS) { sdcard_poll(); } while (sdcard_poll() == 0); } - LED1_OFF; + mscSetActive(); return 0; } /******************************************************************************* diff --git a/src/main/msc/usbd_storage_sdio.c b/src/main/msc/usbd_storage_sdio.c index 9cd26642e..53c1487a2 100644 --- a/src/main/msc/usbd_storage_sdio.c +++ b/src/main/msc/usbd_storage_sdio.c @@ -35,12 +35,14 @@ #ifdef USE_SDCARD #include "common/utils.h" + #include "drivers/dma.h" #include "drivers/dma_reqmap.h" -#include "drivers/sdmmc_sdio.h" -#include "drivers/light_led.h" #include "drivers/io.h" +#include "drivers/light_led.h" #include "drivers/sdcard.h" +#include "drivers/sdmmc_sdio.h" +#include "drivers/usb_msc.h" #include "pg/pg.h" #include "pg/sdcard.h" @@ -103,47 +105,47 @@ static int8_t STORAGE_GetMaxLun (void); static uint8_t STORAGE_Inquirydata[] = {//36 /* LUN 0 */ - 0x00, - 0x80, - 0x02, - 0x02, + 0x00, + 0x80, + 0x02, + 0x02, #ifdef USE_HAL_DRIVER - (STANDARD_INQUIRY_DATA_LEN - 5), + (STANDARD_INQUIRY_DATA_LEN - 5), #else - (USBD_STD_INQUIRY_LENGTH - 5), + (USBD_STD_INQUIRY_LENGTH - 5), #endif - 0x00, - 0x00, - 0x00, - 'S', 'T', 'M', ' ', ' ', ' ', ' ', ' ', /* Manufacturer : 8 bytes */ - 'P', 'r', 'o', 'd', 'u', 't', ' ', ' ', /* Product : 16 Bytes */ - ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', - '0', '.', '0' ,'1', /* Version : 4 Bytes */ + 0x00, + 0x00, + 0x00, + 'S', 'T', 'M', ' ', ' ', ' ', ' ', ' ', /* Manufacturer : 8 bytes */ + 'P', 'r', 'o', 'd', 'u', 't', ' ', ' ', /* Product : 16 Bytes */ + ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', + '0', '.', '0' ,'1', /* Version : 4 Bytes */ }; #ifdef USE_HAL_DRIVER USBD_StorageTypeDef USBD_MSC_MICRO_SDIO_fops = { - STORAGE_Init, - STORAGE_GetCapacity, - STORAGE_IsReady, - STORAGE_IsWriteProtected, - STORAGE_Read, - STORAGE_Write, - STORAGE_GetMaxLun, - (int8_t*)STORAGE_Inquirydata, + STORAGE_Init, + STORAGE_GetCapacity, + STORAGE_IsReady, + STORAGE_IsWriteProtected, + STORAGE_Read, + STORAGE_Write, + STORAGE_GetMaxLun, + (int8_t*)STORAGE_Inquirydata, }; #else USBD_STORAGE_cb_TypeDef USBD_MSC_MICRO_SDIO_fops = { - STORAGE_Init, - STORAGE_GetCapacity, - STORAGE_IsReady, - STORAGE_IsWriteProtected, - STORAGE_Read, - STORAGE_Write, - STORAGE_GetMaxLun, - (int8_t*)STORAGE_Inquirydata, + STORAGE_Init, + STORAGE_GetCapacity, + STORAGE_IsReady, + STORAGE_IsWriteProtected, + STORAGE_Read, + STORAGE_Write, + STORAGE_GetMaxLun, + (int8_t*)STORAGE_Inquirydata, }; #endif @@ -156,24 +158,24 @@ USBD_STORAGE_cb_TypeDef USBD_MSC_MICRO_SDIO_fops = *******************************************************************************/ static int8_t STORAGE_Init (uint8_t lun) { - //Initialize SD_DET - const IO_t sd_det = IOGetByTag(sdcardConfig()->cardDetectTag); - IOInit(sd_det, OWNER_SDCARD_DETECT, 0); - IOConfigGPIO(sd_det, IOCFG_IPU); + //Initialize SD_DET + const IO_t sd_det = IOGetByTag(sdcardConfig()->cardDetectTag); + IOInit(sd_det, OWNER_SDCARD_DETECT, 0); + IOConfigGPIO(sd_det, IOCFG_IPU); - UNUSED(lun); - LED0_OFF; + UNUSED(lun); + LED0_OFF; #ifdef USE_DMA_SPEC const dmaChannelSpec_t *dmaChannelSpec = dmaGetChannelSpecByPeripheral(DMA_PERIPH_SDIO, 0, sdioConfig()->dmaopt); - if (!dmaChannelSpec) { - return 1; - } + if (!dmaChannelSpec) { + return 1; + } - SD_Initialize_LL((DMA_ARCH_TYPE *)dmaChannelSpec->ref); + SD_Initialize_LL((DMA_ARCH_TYPE *)dmaChannelSpec->ref); #else - SD_Initialize_LL(SDCARD_SDIO_DMA_OPT); + SD_Initialize_LL(SDCARD_SDIO_DMA_OPT); #endif if (!sdcard_isInserted()) { @@ -183,9 +185,9 @@ static int8_t STORAGE_Init (uint8_t lun) return 1; } - LED0_ON; + mscSetActive(); - return 0; + return 0; } /******************************************************************************* @@ -201,15 +203,15 @@ static int8_t STORAGE_GetCapacity (uint8_t lun, uint32_t *block_num, uint16_t *b static int8_t STORAGE_GetCapacity (uint8_t lun, uint32_t *block_num, uint32_t *block_size) #endif { - UNUSED(lun); - if (!sdcard_isInserted()) { - return -1; - } - SD_GetCardInfo(); + UNUSED(lun); + if (!sdcard_isInserted()) { + return -1; + } + SD_GetCardInfo(); - *block_num = SD_CardInfo.CardCapacity; - *block_size = 512; - return (0); + *block_num = SD_CardInfo.CardCapacity; + *block_size = 512; + return (0); } /******************************************************************************* @@ -221,12 +223,12 @@ static int8_t STORAGE_GetCapacity (uint8_t lun, uint32_t *block_num, uint32_t *b *******************************************************************************/ static int8_t STORAGE_IsReady (uint8_t lun) { - UNUSED(lun); - int8_t ret = -1; - if (SD_GetState() == true && sdcard_isInserted()) { + UNUSED(lun); + int8_t ret = -1; + if (SD_GetState() == true && sdcard_isInserted()) { ret = 0; - } - return ret; + } + return ret; } /******************************************************************************* @@ -238,8 +240,8 @@ static int8_t STORAGE_IsReady (uint8_t lun) *******************************************************************************/ static int8_t STORAGE_IsWriteProtected (uint8_t lun) { - UNUSED(lun); - return 0; + UNUSED(lun); + return 0; } /******************************************************************************* @@ -254,20 +256,18 @@ static int8_t STORAGE_Read (uint8_t lun, uint32_t blk_addr, uint16_t blk_len) { - UNUSED(lun); - if (!sdcard_isInserted()) { - return -1; - } - LED1_ON; - //buf should be 32bit aligned, but usually is so we don't do byte alignment - if (SD_ReadBlocks_DMA(blk_addr, (uint32_t*) buf, 512, blk_len) == 0) { - while (SD_CheckRead()); - while(SD_GetState() == false); - LED1_OFF; - return 0; - } - LED1_OFF; - return -1; + UNUSED(lun); + if (!sdcard_isInserted()) { + return -1; + } + //buf should be 32bit aligned, but usually is so we don't do byte alignment + if (SD_ReadBlocks_DMA(blk_addr, (uint32_t*) buf, 512, blk_len) == 0) { + while (SD_CheckRead()); + while(SD_GetState() == false); + mscSetActive(); + return 0; + } + return -1; } /******************************************************************************* * Function Name : Write_Memory @@ -281,20 +281,18 @@ static int8_t STORAGE_Write (uint8_t lun, uint32_t blk_addr, uint16_t blk_len) { - UNUSED(lun); - if (!sdcard_isInserted()) { - return -1; - } - LED1_ON; - //buf should be 32bit aligned, but usually is so we don't do byte alignment - if (SD_WriteBlocks_DMA(blk_addr, (uint32_t*) buf, 512, blk_len) == 0) { - while (SD_CheckWrite()); - while(SD_GetState() == false); - LED1_OFF; - return 0; - } - LED1_OFF; - return -1; + UNUSED(lun); + if (!sdcard_isInserted()) { + return -1; + } + //buf should be 32bit aligned, but usually is so we don't do byte alignment + if (SD_WriteBlocks_DMA(blk_addr, (uint32_t*) buf, 512, blk_len) == 0) { + while (SD_CheckWrite()); + while(SD_GetState() == false); + mscSetActive(); + return 0; + } + return -1; } /******************************************************************************* * Function Name : Write_Memory @@ -305,7 +303,7 @@ static int8_t STORAGE_Write (uint8_t lun, *******************************************************************************/ static int8_t STORAGE_GetMaxLun (void) { - return (STORAGE_LUN_NBR - 1); + return (STORAGE_LUN_NBR - 1); } /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/