From 8af1bdac4e1cfdd7aa64a189624228e98d00a993 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Thu, 6 Feb 2014 14:32:31 +0100 Subject: [PATCH] [usb] add usb-storage for apogee SD logger [usb] fix usb-storage when mounting before starting log --- conf/Makefile.chibios-libopencm3 | 3 +- conf/chibios/chibi_lib_for_pprz.mk | 20 +- .../apogee/chibios-libopencm3/halconf.h | 2 +- .../apogee/chibios-libopencm3/mcuconf.h | 8 +- .../chibios-libopencm3/chibios_init.c | 17 +- .../chibios-libopencm3/chibios_sdlog.c | 11 +- .../chibios-libopencm3/usbStorage.c | 333 +++++++ .../chibios-libopencm3/usbStorage.h | 30 + .../subsystems/chibios-libopencm3/usb_msd.c | 860 ++++++++++++++++++ .../subsystems/chibios-libopencm3/usb_msd.h | 209 +++++ 10 files changed, 1470 insertions(+), 23 deletions(-) create mode 100644 sw/airborne/subsystems/chibios-libopencm3/usbStorage.c create mode 100644 sw/airborne/subsystems/chibios-libopencm3/usbStorage.h create mode 100644 sw/airborne/subsystems/chibios-libopencm3/usb_msd.c create mode 100644 sw/airborne/subsystems/chibios-libopencm3/usb_msd.h diff --git a/conf/Makefile.chibios-libopencm3 b/conf/Makefile.chibios-libopencm3 index 51086aa17a..0c3f39198a 100644 --- a/conf/Makefile.chibios-libopencm3 +++ b/conf/Makefile.chibios-libopencm3 @@ -42,6 +42,7 @@ CHIBIOS_BOARD_DIR = $(PAPARAZZI_SRC)/sw/airborne/boards/$(BOARD)/chibios-libopen CHIBIOS_LIB_DIR = $(PAPARAZZI_SRC)/sw/airborne/subsystems/chibios-libopencm3 CHIBIOS_EXT = $(PAPARAZZI_SRC)/sw/ext/chibios OPENCM3_EXT = $(PAPARAZZI_SRC)/sw/ext/libopencm3 +PPRZ_GENERATED = $(PAPARAZZI_SRC)/var/$(AIRCRAFT)/generated # Launch with "make Q=''" to get full command display Q=@ @@ -216,7 +217,7 @@ INCDIR = $(PORTINC) $(KERNINC) $(TESTINC) \ $(HALINC) $(PLATFORMINC) $(BOARDINC) \ $(CHIBIOS)/os/various $(OPENCM3_INC) \ $(CHIBIOS_BOARD_DIR) $(CHIBIOS_LIB_DIR) \ - $(FATFSINC) + $(PPRZ_GENERATED) $(FATFSINC) BUILDDIR := $(OBJDIR) # diff --git a/conf/chibios/chibi_lib_for_pprz.mk b/conf/chibios/chibi_lib_for_pprz.mk index 08e1ea6b0b..dbfb5818d2 100644 --- a/conf/chibios/chibi_lib_for_pprz.mk +++ b/conf/chibios/chibi_lib_for_pprz.mk @@ -1,10 +1,12 @@ # List of all the board related files. -CHIBIOSLIBSRC = ${CHIBIOS_LIB_DIR}/ringBuffer.c \ - ${CHIBIOS_LIB_DIR}/chibios_stub.c \ - ${CHIBIOS_LIB_DIR}/pprz_stub.c \ - ${CHIBIOS_LIB_DIR}/varLengthMsgQ.c \ - ${CHIBIOS_LIB_DIR}/rtcAccess.c \ - ${CHIBIOS_LIB_DIR}/sdLog.c \ - ${CHIBIOS_LIB_DIR}/chibios_sdlog.c \ - ${CHIBIOS_LIB_DIR}/sdio.c \ - ${CHIBIOS_LIB_DIR}/printf.c +CHIBIOSLIBSRC = ${CHIBIOS_LIB_DIR}/ringBuffer.c \ + ${CHIBIOS_LIB_DIR}/chibios_stub.c \ + ${CHIBIOS_LIB_DIR}/pprz_stub.c \ + ${CHIBIOS_LIB_DIR}/varLengthMsgQ.c \ + ${CHIBIOS_LIB_DIR}/rtcAccess.c \ + ${CHIBIOS_LIB_DIR}/sdLog.c \ + ${CHIBIOS_LIB_DIR}/chibios_sdlog.c \ + ${CHIBIOS_LIB_DIR}/sdio.c \ + ${CHIBIOS_LIB_DIR}/printf.c \ + ${CHIBIOS_LIB_DIR}/usb_msd.c \ + ${CHIBIOS_LIB_DIR}/usbStorage.c \ No newline at end of file diff --git a/sw/airborne/boards/apogee/chibios-libopencm3/halconf.h b/sw/airborne/boards/apogee/chibios-libopencm3/halconf.h index 70c9c478c3..29838b1f80 100644 --- a/sw/airborne/boards/apogee/chibios-libopencm3/halconf.h +++ b/sw/airborne/boards/apogee/chibios-libopencm3/halconf.h @@ -157,7 +157,7 @@ * @brief Enables the USB subsystem. */ #if !defined(HAL_USE_USB) || defined(__DOXYGEN__) -#define HAL_USE_USB FALSE +#define HAL_USE_USB TRUE #endif /*===========================================================================*/ diff --git a/sw/airborne/boards/apogee/chibios-libopencm3/mcuconf.h b/sw/airborne/boards/apogee/chibios-libopencm3/mcuconf.h index 90451d8c28..8805490d28 100644 --- a/sw/airborne/boards/apogee/chibios-libopencm3/mcuconf.h +++ b/sw/airborne/boards/apogee/chibios-libopencm3/mcuconf.h @@ -243,14 +243,14 @@ /* * USB driver system settings. */ -#define STM32_USB_USE_OTG1 FALSE // FS, DFU_BOOT +#define STM32_USB_USE_OTG1 TRUE // FS, DFU_BOOT #define STM32_USB_USE_OTG2 FALSE // HS #define STM32_USB_OTG1_IRQ_PRIORITY 14 #define STM32_USB_OTG2_IRQ_PRIORITY 14 #define STM32_USB_OTG1_RX_FIFO_SIZE 512 #define STM32_USB_OTG2_RX_FIFO_SIZE 512 #define STM32_USB_OTG_THREAD_PRIO LOWPRIO -#define STM32_USB_OTG_THREAD_STACK_SIZE 128 +#define STM32_USB_OTG_THREAD_STACK_SIZE 256 #define STM32_USB_OTGFIFO_FILL_BASEPRI 0 @@ -310,7 +310,7 @@ [NVIC_USART3_IRQ] = NVIC_USART3_IRQ_VEC_OPENCM3, \ [NVIC_EXTI15_10_IRQ] = NVIC_EXTI15_10_IRQ_VEC_OPENCM3, \ [NVIC_RTC_ALARM_IRQ] = NVIC_RTC_ALARM_IRQ_VEC_CHIBIOS, \ -[NVIC_USB_FS_WKUP_IRQ] = NVIC_USB_FS_WKUP_IRQ_VEC_OPENCM3, \ +[NVIC_USB_FS_WKUP_IRQ] = NVIC_USB_FS_WKUP_IRQ_VEC_CHIBIOS, \ [NVIC_TIM8_BRK_TIM12_IRQ] = NVIC_TIM8_BRK_TIM12_IRQ_VEC_OPENCM3, \ [NVIC_TIM8_UP_TIM13_IRQ] = NVIC_TIM8_UP_TIM13_IRQ_VEC_OPENCM3, \ [NVIC_TIM8_TRG_COM_TIM14_IRQ] = NVIC_TIM8_TRG_COM_TIM14_IRQ_VEC_OPENCM3, \ @@ -335,7 +335,7 @@ [NVIC_CAN2_RX0_IRQ] = NVIC_CAN2_RX0_IRQ_VEC_OPENCM3, \ [NVIC_CAN2_RX1_IRQ] = NVIC_CAN2_RX1_IRQ_VEC_OPENCM3, \ [NVIC_CAN2_SCE_IRQ] = NVIC_CAN2_SCE_IRQ_VEC_OPENCM3, \ -[NVIC_OTG_FS_IRQ] = NVIC_OTG_FS_IRQ_VEC_OPENCM3, \ +[NVIC_OTG_FS_IRQ] = NVIC_OTG_FS_IRQ_VEC_CHIBIOS, \ [NVIC_DMA2_STREAM5_IRQ] = NVIC_DMA2_STREAM5_IRQ_VEC_OPENCM3, \ [NVIC_DMA2_STREAM6_IRQ] = NVIC_DMA2_STREAM6_IRQ_VEC_OPENCM3, \ [NVIC_DMA2_STREAM7_IRQ] = NVIC_DMA2_STREAM7_IRQ_VEC_OPENCM3, \ diff --git a/sw/airborne/firmwares/fixedwing/chibios-libopencm3/chibios_init.c b/sw/airborne/firmwares/fixedwing/chibios-libopencm3/chibios_init.c index 8a26cc9cf8..680ca241af 100644 --- a/sw/airborne/firmwares/fixedwing/chibios-libopencm3/chibios_init.c +++ b/sw/airborne/firmwares/fixedwing/chibios-libopencm3/chibios_init.c @@ -28,8 +28,15 @@ #include #include "subsystems/chibios-libopencm3/chibios_sdlog.h" #include "sdLog.h" +#include "usbStorage.h" #include "pprz_stub.h" #include "rtcAccess.h" +#include "airframe.h" + +// Delay before starting SD log +#ifndef SDLOG_START_DELAY +#define SDLOG_START_DELAY 30 +#endif #ifndef SYS_TIME_FREQUENCY @@ -62,12 +69,11 @@ bool_t chibios_init(void) { PWR->CSR &= ~PWR_CSR_BRE; DBGMCU->APB1FZ |= DBGMCU_APB1_FZ_DBG_IWDG_STOP; - chThdSleepMilliseconds (100); - sdOk = chibios_logInit(true); - chThdCreateStatic(wa_thd_heartbeat, sizeof(wa_thd_heartbeat), NORMALPRIO, thd_heartbeat, NULL); - return sdOk; + + usbStorageStartPolling (); + return RDY_OK; } static WORKING_AREA(pprzThd, 4096); @@ -85,6 +91,9 @@ static __attribute__((noreturn)) msg_t thd_heartbeat(void *arg) (void) arg; chRegSetThreadName("pprz heartbeat"); + chThdSleepSeconds (SDLOG_START_DELAY); + sdOk = chibios_logInit(true); + while (TRUE) { palTogglePad (GPIOC, GPIOC_LED3); chThdSleepMilliseconds (sdOk == TRUE ? 1000 : 200); diff --git a/sw/airborne/subsystems/chibios-libopencm3/chibios_sdlog.c b/sw/airborne/subsystems/chibios-libopencm3/chibios_sdlog.c index 4dd8d6ac6e..d0ca6b723d 100644 --- a/sw/airborne/subsystems/chibios-libopencm3/chibios_sdlog.c +++ b/sw/airborne/subsystems/chibios-libopencm3/chibios_sdlog.c @@ -92,12 +92,15 @@ error: void chibios_logFinish(void) { - sdLogStopThread (); - sdLogCloseLog (&pprzLogFile); + if (pprzLogFile.fs != NULL) { + sdLogStopThread (); + sdLogCloseLog (&pprzLogFile); #if LOG_PROCESS_STATE - sdLogCloseLog (&processLogFile); + sdLogCloseLog (&processLogFile); #endif - sdLogFinish (); + sdLogFinish (); + pprzLogFile.fs = NULL; + } } diff --git a/sw/airborne/subsystems/chibios-libopencm3/usbStorage.c b/sw/airborne/subsystems/chibios-libopencm3/usbStorage.c new file mode 100644 index 0000000000..403d688750 --- /dev/null +++ b/sw/airborne/subsystems/chibios-libopencm3/usbStorage.c @@ -0,0 +1,333 @@ +/* + * Copyright (C) 2014 Gautier Hattenberger, Alexandre Bustico + * + * This file is part of paparazzi. + * + * paparazzi 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 2, or (at your option) + * any later version. + * + * paparazzi 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 paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/* + * @file subsystems/chibios-libopencm3/usbStorage.c + * + */ + +#include "ch.h" +#include "hal.h" +#include "usb_msd.h" +#include "usbStorage.h" +#include "chibios_sdlog.h" +#include +#include +#include + +static msg_t thdUsbStorage(void *arg); +static Thread* usbStorageThreadPtr=NULL; +/* USB mass storage driver */ +static USBMassStorageDriver UMSD1; + +/* endpoint index */ +#define USB_MS_DATA_EP 1 +// cortex_m4 specific +#define MCU_RESTART() {*((unsigned long *)0x0E000ED0C) = 0x05FA0004;} + +/* USB device descriptor */ +static const uint8_t deviceDescriptorData[] = +{ + USB_DESC_DEVICE + ( + 0x0200, /* supported USB version (2.0) */ + 0x00, /* device class (none, specified in interface) */ + 0x00, /* device sub-class (none, specified in interface) */ + 0x00, /* device protocol (none, specified in interface) */ + 64, /* max packet size of control end-point */ + 0x0483, /* vendor ID (STMicroelectronics!) */ + 0x5740, /* product ID (STM32F407) */ + 0x0100, /* device release number */ + 1, /* index of manufacturer string descriptor */ + 2, /* index of product string descriptor */ + 3, /* index of serial number string descriptor */ + 1 /* number of possible configurations */ + ) +}; +static const USBDescriptor deviceDescriptor = +{ + sizeof(deviceDescriptorData), + deviceDescriptorData +}; + +/* configuration descriptor */ +static const uint8_t configurationDescriptorData[] = +{ + /* configuration descriptor */ + USB_DESC_CONFIGURATION + ( + 32, /* total length */ + 1, /* number of interfaces */ + 1, /* value that selects this configuration */ + 0, /* index of string descriptor describing this configuration */ + 0xC0, /* attributes (self-powered) */ + 50 /* max power (100 mA) */ + ), + + /* interface descriptor */ + USB_DESC_INTERFACE + ( + 0, /* interface number */ + 0, /* value used to select alternative setting */ + 2, /* number of end-points used by this interface */ + 0x08, /* interface class (Mass Storage) */ + 0x06, /* interface sub-class (SCSI Transparent Storage) */ + 0x50, /* interface protocol (Bulk Only) */ + 0 /* index of string descriptor describing this interface */ + ), + + /* end-point descriptor */ + USB_DESC_ENDPOINT + ( + USB_MS_DATA_EP | 0x80, /* address (end point index | OUT direction) */ + USB_EP_MODE_TYPE_BULK, /* attributes (bulk) */ + 64, /* max packet size */ + 0x05 /* polling interval (ignored for bulk end-points) */ + ), + + /* end-point descriptor */ + USB_DESC_ENDPOINT + ( + USB_MS_DATA_EP | 0x00, /* address (end point index | IN direction) */ + USB_EP_MODE_TYPE_BULK, /* attributes (bulk) */ + 64, /* max packet size */ + 0x05 /* polling interval (ignored for bulk end-points) */ + ) +}; +static const USBDescriptor configurationDescriptor = +{ + sizeof(configurationDescriptorData), + configurationDescriptorData +}; + +/* Language descriptor */ +static const uint8_t languageDescriptorData[] = +{ + USB_DESC_BYTE(4), + USB_DESC_BYTE(USB_DESCRIPTOR_STRING), + USB_DESC_WORD(0x0409) /* U.S. english */ +}; +static const USBDescriptor languageDescriptor = +{ + sizeof(languageDescriptorData), + languageDescriptorData +}; + +/* Vendor descriptor */ +static const uint8_t vendorDescriptorData[] = +{ + USB_DESC_BYTE(20), + USB_DESC_BYTE(USB_DESCRIPTOR_STRING), + 'P', 0, 'a', 0, 'p', 0, 'a', 0, 'r', 0, 'a', 0, 'z', 0, 'z', 0, 'i', 0 +}; +static const USBDescriptor vendorDescriptor = +{ + sizeof(vendorDescriptorData), + vendorDescriptorData +}; + +/* Product descriptor */ +static const uint8_t productDescriptorData[] = +{ + USB_DESC_BYTE(24), + USB_DESC_BYTE(USB_DESCRIPTOR_STRING), + 'A', 0, 'p', 0, 'o', 0, 'g', 0, 'e', 0, 'e', 0, ' ', 0, 'V', 0, '1', 0, '0', 0, '0', 0 +}; +static const USBDescriptor productDescriptor = +{ + sizeof(productDescriptorData), + productDescriptorData +}; + +/* Serial number descriptor */ +static const uint8_t serialNumberDescriptorData[] = +{ + USB_DESC_BYTE(24), + USB_DESC_BYTE(USB_DESCRIPTOR_STRING), + '0', 0, '0', 0, '0', 0, '0', 0, '0', 0, '0', 0, '0', 0, '1', 0, '.', 0, '0', 0, '0', 0 +}; +static const USBDescriptor serialNumberDescriptor = +{ + sizeof(serialNumberDescriptorData), + serialNumberDescriptorData +}; + +/* Handles GET_DESCRIPTOR requests from the USB host */ +static const USBDescriptor* getDescriptor(USBDriver* usbp, uint8_t type, uint8_t index, uint16_t lang) +{ + (void)usbp; + (void)lang; + + switch (type) + { + case USB_DESCRIPTOR_DEVICE: + return &deviceDescriptor; + + case USB_DESCRIPTOR_CONFIGURATION: + return &configurationDescriptor; + + case USB_DESCRIPTOR_STRING: + switch (index) + { + case 0: return &languageDescriptor; + case 1: return &vendorDescriptor; + case 2: return &productDescriptor; + case 3: return &serialNumberDescriptor; + } + } + + return 0; +} + +/* Handles global events of the USB driver */ +static void usbEvent(USBDriver* usbp, usbevent_t event) +{ + (void) usbp; + + switch (event) + { + case USB_EVENT_CONFIGURED: + chSysLockFromIsr(); + msdConfigureHookI(&UMSD1); + chSysUnlockFromIsr(); + break; + + case USB_EVENT_RESET: + case USB_EVENT_ADDRESS: + case USB_EVENT_SUSPEND: + case USB_EVENT_WAKEUP: + case USB_EVENT_STALLED: + default: + break; + } +} + +/* Configuration of the USB driver */ +const USBConfig usbConfig = +{ + usbEvent, + getDescriptor, + msdRequestsHook, + 0 +}; + +/* Turns on a LED when there is I/O activity on the USB port */ +static void usbActivity(bool_t active) +{ + if (active) + palSetPad(GPIOC, GPIOC_LED4); + else + palClearPad(GPIOC, GPIOC_LED4); +} + +/* USB mass storage configuration */ +static USBMassStorageConfig msdConfig = +{ + &USBD1, + (BaseBlockDevice*)&SDCD1, + USB_MS_DATA_EP, + &usbActivity, + "DPprz_sd", + "DApogee", + "0.1" +}; + + +static WORKING_AREA(waThsUsbStorage, 1024); +void usbStorageStartPolling (void) +{ + usbStorageThreadPtr = chThdCreateStatic (waThsUsbStorage, sizeof(waThsUsbStorage), + NORMALPRIO, thdUsbStorage, NULL); + +} + + +void usbStorageWaitForDeconnexion (void) +{ + if (usbStorageThreadPtr != NULL) + chThdWait (usbStorageThreadPtr); + usbStorageThreadPtr = NULL; +} + +void usbStorageStop (void) +{ + if (usbStorageThreadPtr != NULL) { + chThdTerminate (usbStorageThreadPtr); + } +} + + + + +static msg_t thdUsbStorage(void *arg) +{ + (void) arg; // unused + chRegSetThreadName("UsbStorage:polling"); + uint antiBounce=5; + + // Should use EXTI interrupt instead of active polling, + // but in the chibios_opencm3 implementation, since EXTI is + // used via libopencm3, ISR are routed on pprz/opencm3 and cannot + // be used concurrently by chibios api + // Should be fixed when using chibios-rt branch + while (!chThdShouldTerminate() && antiBounce) { + const bool_t usbConnected = palReadPad (GPIOA, GPIOA_OTG_FS_VBUS); + if (usbConnected) + antiBounce--; + else + antiBounce=5; + + chThdSleepMilliseconds(20); + } + + chRegSetThreadName("UsbStorage:connected"); + + chibios_logFinish (); + + /* connect sdcard sdc interface sdio */ + if (sdioConnect () == false) + chThdExit (RDY_TIMEOUT); + + /* initialize the USB mass storage driver */ + msdInit(&UMSD1); + + /* start the USB mass storage service */ + msdStart(&UMSD1, &msdConfig); + + /* start the USB driver */ + usbDisconnectBus(&USBD1); + chThdSleepMilliseconds(1000); + usbStart(&USBD1, &usbConfig); + usbConnectBus(&USBD1); + + /* watch the mass storage events */ + while (!chThdShouldTerminate() && palReadPad (GPIOA, GPIOA_OTG_FS_VBUS)) { + chThdSleepMilliseconds(10); + } + + usbDisconnectBus(&USBD1); + chThdSleepMilliseconds(500); + msdStop(&UMSD1); + sdioDisconnect (); + + MCU_RESTART(); + return RDY_OK; +} diff --git a/sw/airborne/subsystems/chibios-libopencm3/usbStorage.h b/sw/airborne/subsystems/chibios-libopencm3/usbStorage.h new file mode 100644 index 0000000000..8993a9d72b --- /dev/null +++ b/sw/airborne/subsystems/chibios-libopencm3/usbStorage.h @@ -0,0 +1,30 @@ +/* + * Copyright (C) 2014 Gautier Hattenberger, Alexandre Bustico + * + * This file is part of paparazzi. + * + * paparazzi 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 2, or (at your option) + * any later version. + * + * paparazzi 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 paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/* + * @file subsystems/chibios-libopencm3/usbStorage.h + * + */ +#pragma once + +void usbStorageStartPolling (void); +void usbStorageStop (void); +void usbStorageWaitForDeconnexion (void); diff --git a/sw/airborne/subsystems/chibios-libopencm3/usb_msd.c b/sw/airborne/subsystems/chibios-libopencm3/usb_msd.c new file mode 100644 index 0000000000..dbb7c9ed2a --- /dev/null +++ b/sw/airborne/subsystems/chibios-libopencm3/usb_msd.c @@ -0,0 +1,860 @@ +/* + * Copyright (C) 2014 Gautier Hattenberger, Alexandre Bustico + * + * This file is part of paparazzi. + * + * paparazzi 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 2, or (at your option) + * any later version. + * + * paparazzi 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 paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/* + * @file subsystems/chibios-libopencm3/usb_msd.c + * + */ + +#include "usb_msd.h" + +/* Request types */ +#define MSD_REQ_RESET 0xFF +#define MSD_GET_MAX_LUN 0xFE + +/* CBW/CSW block signatures */ +#define MSD_CBW_SIGNATURE 0x43425355 +#define MSD_CSW_SIGNATURE 0x53425355 + +/* Setup packet access macros */ +#define MSD_SETUP_WORD(setup, index) (uint16_t)(((uint16_t)setup[index + 1] << 8) | (setup[index] & 0x00FF)) +#define MSD_SETUP_VALUE(setup) MSD_SETUP_WORD(setup, 2) +#define MSD_SETUP_INDEX(setup) MSD_SETUP_WORD(setup, 4) +#define MSD_SETUP_LENGTH(setup) MSD_SETUP_WORD(setup, 6) + +/* Command statuses */ +#define MSD_COMMAND_PASSED 0x00 +#define MSD_COMMAND_FAILED 0x01 +#define MSD_COMMAND_PHASE_ERROR 0x02 + +/* SCSI commands */ +#define SCSI_CMD_TEST_UNIT_READY 0x00 +#define SCSI_CMD_REQUEST_SENSE 0x03 +#define SCSI_CMD_FORMAT_UNIT 0x04 +#define SCSI_CMD_INQUIRY 0x12 +#define SCSI_CMD_MODE_SENSE_6 0x1A +#define SCSI_CMD_START_STOP_UNIT 0x1B +#define SCSI_CMD_SEND_DIAGNOSTIC 0x1D +#define SCSI_CMD_PREVENT_ALLOW_MEDIUM_REMOVAL 0x1E +#define SCSI_CMD_READ_FORMAT_CAPACITIES 0x23 +#define SCSI_CMD_READ_CAPACITY_10 0x25 +#define SCSI_CMD_READ_10 0x28 +#define SCSI_CMD_WRITE_10 0x2A +#define SCSI_CMD_VERIFY_10 0x2F + +/* SCSI sense keys */ +#define SCSI_SENSE_KEY_GOOD 0x00 +#define SCSI_SENSE_KEY_RECOVERED_ERROR 0x01 +#define SCSI_SENSE_KEY_NOT_READY 0x02 +#define SCSI_SENSE_KEY_MEDIUM_ERROR 0x03 +#define SCSI_SENSE_KEY_HARDWARE_ERROR 0x04 +#define SCSI_SENSE_KEY_ILLEGAL_REQUEST 0x05 +#define SCSI_SENSE_KEY_UNIT_ATTENTION 0x06 +#define SCSI_SENSE_KEY_DATA_PROTECT 0x07 +#define SCSI_SENSE_KEY_BLANK_CHECK 0x08 +#define SCSI_SENSE_KEY_VENDOR_SPECIFIC 0x09 +#define SCSI_SENSE_KEY_COPY_ABORTED 0x0A +#define SCSI_SENSE_KEY_ABORTED_COMMAND 0x0B +#define SCSI_SENSE_KEY_VOLUME_OVERFLOW 0x0D +#define SCSI_SENSE_KEY_MISCOMPARE 0x0E + +#define SCSI_ASENSE_NO_ADDITIONAL_INFORMATION 0x00 +#define SCSI_ASENSE_WRITE_FAULT 0x03 +#define SCSI_ASENSE_LOGICAL_UNIT_NOT_READY 0x04 +#define SCSI_ASENSE_READ_ERROR 0x11 +#define SCSI_ASENSE_INVALID_COMMAND 0x20 +#define SCSI_ASENSE_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE 0x21 +#define SCSI_ASENSE_INVALID_FIELD_IN_CDB 0x24 +#define SCSI_ASENSE_WRITE_PROTECTED 0x27 +#define SCSI_ASENSE_NOT_READY_TO_READY_CHANGE 0x28 +#define SCSI_ASENSE_FORMAT_ERROR 0x31 +#define SCSI_ASENSE_MEDIUM_NOT_PRESENT 0x3A + +#define SCSI_ASENSEQ_NO_QUALIFIER 0x00 +#define SCSI_ASENSEQ_FORMAT_COMMAND_FAILED 0x01 +#define SCSI_ASENSEQ_INITIALIZING_COMMAND_REQUIRED 0x02 +#define SCSI_ASENSEQ_OPERATION_IN_PROGRESS 0x07 + +/** + * @brief Response to a READ_CAPACITY_10 SCSI command + */ +PACK_STRUCT_BEGIN typedef struct { + uint32_t last_block_addr; + uint32_t block_size; +} PACK_STRUCT_STRUCT msd_scsi_read_capacity_10_response_t PACK_STRUCT_END; + +/** + * @brief Response to a READ_FORMAT_CAPACITIES SCSI command + */ +PACK_STRUCT_BEGIN typedef struct { + uint8_t reserved[3]; + uint8_t capacity_list_length; + uint32_t block_count; + uint32_t desc_and_block_length; +} PACK_STRUCT_STRUCT msd_scsi_read_format_capacities_response_t PACK_STRUCT_END; + +/** + * @brief Read-write buffers (TODO: need a way of specifying the size of this) + */ +static uint8_t rw_buf[2][512]; + +typedef uint32_t DWORD __attribute__((__may_alias__));; +typedef uint16_t WORD __attribute__((__may_alias__)); + + +/** + * @brief Byte-swap a 32 bits unsigned integer + */ +static inline DWORD swap_uint32(DWORD val ); + +static inline DWORD swap_uint32(DWORD val ) { + val = ((val << 8) & 0xFF00FF00 ) | ((val >> 8) & 0xFF00FF ); + return ((val << 16) & 0xFFFF0000) | ((val >> 16) & 0x0000FFFF); +} +/** + * @brief Byte-swap a 16 bits unsigned integer + */ +#define swap_uint16(x) (((((WORD)(x)) >> 8) & 0xff) | ((((WORD)(x)) & 0xff) << 8)) + +static void msd_handle_end_point_notification(USBDriver *usbp, usbep_t ep); + +/** + * @brief IN end-point 1 state + */ +static USBInEndpointState ep1_in_state; + +/** + * @brief OUT end-point 1 state + */ +static USBOutEndpointState ep1_out_state; + +/** + * @brief End-point 1 initialization structure + */ +static const USBEndpointConfig ep_data_config = { + USB_EP_MODE_TYPE_BULK, + NULL, + msd_handle_end_point_notification, + msd_handle_end_point_notification, + 64, + 64, + &ep1_in_state, + &ep1_out_state, + 1, + NULL +}; + +/** + * @brief USB device configured handler. + * + * @param[in] msdp pointer to the @p USBMassStorageDriver object + * + * @iclass + */ +void msdConfigureHookI(USBMassStorageDriver *msdp) +{ + usbInitEndpointI(msdp->config->usbp, msdp->config->bulk_ep, &ep_data_config); + chBSemSignalI(&msdp->bsem); + chEvtBroadcastI(&msdp->evt_connected); +} + +/** + * @brief Default requests hook. + * + * @param[in] usbp pointer to the @p USBDriver object + * @return The hook status. + * @retval TRUE Message handled internally. + * @retval FALSE Message not handled. + */ +bool_t msdRequestsHook(USBDriver *usbp) { + + /* check that the request is of type Class / Interface */ + if (((usbp->setup[0] & USB_RTYPE_TYPE_MASK) == USB_RTYPE_TYPE_CLASS) && + ((usbp->setup[0] & USB_RTYPE_RECIPIENT_MASK) == USB_RTYPE_RECIPIENT_INTERFACE)) { + + /* check that the request is for interface 0 */ + if (MSD_SETUP_INDEX(usbp->setup) != 0) + return FALSE; + + /* act depending on bRequest = setup[1] */ + switch (usbp->setup[1]) { + case MSD_REQ_RESET: + /* check that it is a HOST2DEV request */ + if (((usbp->setup[0] & USB_RTYPE_DIR_MASK) != USB_RTYPE_DIR_HOST2DEV) || + (MSD_SETUP_LENGTH(usbp->setup) != 0) || + (MSD_SETUP_VALUE(usbp->setup) != 0)) + { + return FALSE; + } + + /* reset all endpoints */ + /* TODO!*/ + /* The device shall NAK the status stage of the device request until + * the Bulk-Only Mass Storage Reset is complete. + */ + return TRUE; + case MSD_GET_MAX_LUN: + /* check that it is a DEV2HOST request */ + if (((usbp->setup[0] & USB_RTYPE_DIR_MASK) != USB_RTYPE_DIR_DEV2HOST) || + (MSD_SETUP_LENGTH(usbp->setup) != 1) || + (MSD_SETUP_VALUE(usbp->setup) != 0)) + { + return FALSE; + } + + static uint8_t len_buf[1] = {0}; + /* stall to indicate that we don't support LUN */ + usbSetupTransfer(usbp, len_buf, 1, NULL); + return TRUE; + default: + return FALSE; + break; + } + } + + return FALSE; +} + +/** + * @brief Wait until the end-point interrupt handler has been called + */ +static void msd_wait_for_isr(USBMassStorageDriver *msdp) { + + /* sleep until it completes */ + chSysLock(); + chBSemWaitS(&msdp->bsem); + chSysUnlock(); +} + +/** + * @brief Called when data can be read or written on the endpoint -- wakes the thread up + */ +static void msd_handle_end_point_notification(USBDriver *usbp, usbep_t ep) { + + (void)usbp; + (void)ep; + + chSysLockFromIsr(); + chBSemSignalI(&((USBMassStorageDriver *)usbp->in_params[ep])->bsem); + chSysUnlockFromIsr(); +} + +/** + * @brief Starts sending data + */ +static void msd_start_transmit(USBMassStorageDriver *msdp, const uint8_t* buffer, size_t size) { + + usbPrepareTransmit(msdp->config->usbp, msdp->config->bulk_ep, buffer, size); + chSysLock(); + usbStartTransmitI(msdp->config->usbp, msdp->config->bulk_ep); + chSysUnlock(); +} + +/** + * @brief Starts receiving data + */ +static void msd_start_receive(USBMassStorageDriver *msdp, uint8_t* buffer, size_t size) { + + usbPrepareReceive(msdp->config->usbp, msdp->config->bulk_ep, buffer, size); + chSysLock(); + usbStartReceiveI(msdp->config->usbp, msdp->config->bulk_ep); + chSysUnlock(); +} + +/** + * @brief Changes the SCSI sense information + */ +static inline void msd_scsi_set_sense(USBMassStorageDriver *msdp, uint8_t key, uint8_t acode, uint8_t aqual) { + msdp->sense.byte[2] = key; + msdp->sense.byte[12] = acode; + msdp->sense.byte[13] = aqual; +} + +/** + * @brief Processes an INQUIRY SCSI command + */ +bool_t msd_scsi_process_inquiry(USBMassStorageDriver *msdp) { + + msd_cbw_t *cbw = &(msdp->cbw); + + /* check the EVPD bit (Vital Product Data) */ + if (cbw->scsi_cmd_data[1] & 0x01) { + + /* check the Page Code byte to know the type of product data to reply */ + switch (cbw->scsi_cmd_data[2]) { + + /* unit serial number */ + case 0x80: { + uint8_t response[] = {'0'}; /* TODO */ + msd_start_transmit(msdp, response, sizeof(response)); + msdp->result = TRUE; + + /* wait for ISR */ + return TRUE; + } + + /* unhandled */ + default: + msd_scsi_set_sense(msdp, + SCSI_SENSE_KEY_ILLEGAL_REQUEST, + SCSI_ASENSE_INVALID_FIELD_IN_CDB, + SCSI_ASENSEQ_NO_QUALIFIER); + return FALSE; + } + } + else + { + msd_start_transmit(msdp, (const uint8_t *)&msdp->inquiry, sizeof(msdp->inquiry)); + msdp->result = TRUE; + + /* wait for ISR */ + return TRUE; + } +} + +/** + * @brief Processes a REQUEST_SENSE SCSI command + */ +bool_t msd_scsi_process_request_sense(USBMassStorageDriver *msdp) { + + msd_start_transmit(msdp, (const uint8_t *)&msdp->sense, sizeof(msdp->sense)); + msdp->result = TRUE; + + /* wait for ISR immediately, otherwise the caller may reset the sense bytes before they are sent to the host! */ + msd_wait_for_isr(msdp); + + /* ... don't wait for ISR, we just did it */ + return FALSE; +} + +/** + * @brief Processes a READ_CAPACITY_10 SCSI command + */ +bool_t msd_scsi_process_read_capacity_10(USBMassStorageDriver *msdp) { + + static msd_scsi_read_capacity_10_response_t response; + + response.block_size = swap_uint32(msdp->block_dev_info.blk_size); + response.last_block_addr = swap_uint32(msdp->block_dev_info.blk_num-1); + + msd_start_transmit(msdp, (const uint8_t *)&response, sizeof(response)); + msdp->result = TRUE; + + /* wait for ISR */ + return TRUE; +} + +/** + * @brief Processes a SEND_DIAGNOSTIC SCSI command + */ +bool_t msd_scsi_process_send_diagnostic(USBMassStorageDriver *msdp) { + + msd_cbw_t *cbw = &(msdp->cbw); + + if (!(cbw->scsi_cmd_data[1] & (1 << 2))) { + /* only self-test supported - update SENSE key and fail the command */ + msd_scsi_set_sense(msdp, + SCSI_SENSE_KEY_ILLEGAL_REQUEST, + SCSI_ASENSE_INVALID_FIELD_IN_CDB, + SCSI_ASENSEQ_NO_QUALIFIER); + msdp->result = FALSE; + return FALSE; + } + + /* TODO: actually perform the test */ + msdp->result = TRUE; + + /* don't wait for ISR */ + return FALSE; +} + +/** + * @brief Processes a READ_WRITE_10 SCSI command + */ +bool_t msd_scsi_process_start_read_write_10(USBMassStorageDriver *msdp) { + + msd_cbw_t *cbw = &(msdp->cbw); + + if ((cbw->scsi_cmd_data[0] == SCSI_CMD_WRITE_10) && blkIsWriteProtected(msdp->config->bbdp)) { + /* device is write protected and a write has been issued */ + /* block address is invalid, update SENSE key and return command fail */ + msd_scsi_set_sense(msdp, + SCSI_SENSE_KEY_DATA_PROTECT, + SCSI_ASENSE_WRITE_PROTECTED, + SCSI_ASENSEQ_NO_QUALIFIER); + msdp->result = FALSE; + + /* don't wait for ISR */ + return FALSE; + } + + uint32_t rw_block_address = swap_uint32(*(DWORD *)&cbw->scsi_cmd_data[2]); + uint16_t total = swap_uint16(*(WORD *)&cbw->scsi_cmd_data[7]); + uint16_t i = 0; + + if (rw_block_address >= msdp->block_dev_info.blk_num) { + /* block address is invalid, update SENSE key and return command fail */ + msd_scsi_set_sense(msdp, + SCSI_SENSE_KEY_ILLEGAL_REQUEST, + SCSI_ASENSE_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE, + SCSI_ASENSEQ_NO_QUALIFIER); + msdp->result = FALSE; + + /* don't wait for ISR */ + return FALSE; + } + + if (cbw->scsi_cmd_data[0] == SCSI_CMD_WRITE_10) { + /* process a write command */ + + /* get the first packet */ + msd_start_receive(msdp, rw_buf[i % 2], msdp->block_dev_info.blk_size); + msd_wait_for_isr(msdp); + + /* loop over each block */ + for (i = 0; i < total; i++) { + + if (i < (total - 1)) { + /* there is at least one block of data left to be read over USB */ + /* queue this read before issuing the blocking write */ + msd_start_receive(msdp, rw_buf[(i + 1) % 2], msdp->block_dev_info.blk_size); + } + + /* now write the block to the block device */ + if (blkWrite(msdp->config->bbdp, rw_block_address++, rw_buf[i % 2], 1) == CH_FAILED) { + /* write failed */ + msd_scsi_set_sense(msdp, + SCSI_SENSE_KEY_MEDIUM_ERROR, + SCSI_ASENSE_WRITE_FAULT, + SCSI_ASENSEQ_NO_QUALIFIER); + msdp->result = FALSE; + + /* don't wait for ISR */ + return FALSE; + } + + if (i < (total - 1)) { + /* now wait for the USB event to complete */ + msd_wait_for_isr(msdp); + } + } + } else { + /* process a read command */ + + i = 0; + + /* read the first block from block device */ + if (blkRead(msdp->config->bbdp, rw_block_address++, rw_buf[i % 2], 1) == CH_FAILED) { + /* read failed */ + msd_scsi_set_sense(msdp, + SCSI_SENSE_KEY_MEDIUM_ERROR, + SCSI_ASENSE_READ_ERROR, + SCSI_ASENSEQ_NO_QUALIFIER); + msdp->result = FALSE; + + /* don't wait for ISR */ + return FALSE; + } + + /* loop over each block */ + for (i = 0; i < total; i++) { + /* transmit the block */ + msd_start_transmit(msdp, rw_buf[i % 2], msdp->block_dev_info.blk_size); + + if (i < (total - 1)) { + /* there is at least one more block to be read from device */ + /* so read that whilst the USB transfer takes place */ + if (blkRead(msdp->config->bbdp, rw_block_address++, rw_buf[(i + 1) % 2], 1) == CH_FAILED) { + /* read failed */ + msd_scsi_set_sense(msdp, + SCSI_SENSE_KEY_MEDIUM_ERROR, + SCSI_ASENSE_READ_ERROR, + SCSI_ASENSEQ_NO_QUALIFIER); + msdp->result = FALSE; + + /* wait for ISR (the previous transmission is still running) */ + return TRUE; + } + } + + /* wait for the USB event to complete */ + msd_wait_for_isr(msdp); + } + } + + msdp->result = TRUE; + + /* don't wait for ISR */ + return FALSE; +} + +/** + * @brief Processes a START_STOP_UNIT SCSI command + */ +bool_t msd_scsi_process_start_stop_unit(USBMassStorageDriver *msdp) { + + if ((msdp->cbw.scsi_cmd_data[4] & 0x03) == 0x02) { + /* device has been ejected */ + chEvtBroadcast(&msdp->evt_ejected); + msdp->state = MSD_EJECTED; + } + + msdp->result = TRUE; + + /* don't wait for ISR */ + return FALSE; +} + +/** + * @brief Processes a MODE_SENSE_6 SCSI command + */ +bool_t msd_scsi_process_mode_sense_6(USBMassStorageDriver *msdp) { + + static uint8_t response[4] = { + 0x03, /* number of bytes that follow */ + 0x00, /* medium type is SBC */ + 0x00, /* not write protected (TODO handle it correctly) */ + 0x00 /* no block descriptor */ + }; + + msd_start_transmit(msdp, response, sizeof(response)); + msdp->result = TRUE; + + /* wait for ISR */ + return TRUE; +} + +/** + * @brief Processes a READ_FORMAT_CAPACITIES SCSI command + */ +bool_t msd_scsi_process_read_format_capacities(USBMassStorageDriver *msdp) { + + msd_scsi_read_format_capacities_response_t response; + response.capacity_list_length = 1; + response.block_count = swap_uint32(msdp->block_dev_info.blk_num); + response.desc_and_block_length = swap_uint32((0x02 << 24) | (msdp->block_dev_info.blk_size & 0x00FFFFFF)); + + msd_start_transmit(msdp, (const uint8_t*)&response, sizeof(response)); + msdp->result = TRUE; + + /* wait for ISR */ + return TRUE; +} + +/** + * @brief Processes a TEST_UNIT_READY SCSI command + */ +bool_t msd_scsi_process_test_unit_ready(USBMassStorageDriver *msdp) { + + if (blkIsInserted(msdp->config->bbdp)) { + /* device inserted and ready */ + msdp->result = TRUE; + } else { + /* device not present or not ready */ + msd_scsi_set_sense(msdp, + SCSI_SENSE_KEY_NOT_READY, + SCSI_ASENSE_MEDIUM_NOT_PRESENT, + SCSI_ASENSEQ_NO_QUALIFIER); + msdp->result = FALSE; + } + + /* don't wait for ISR */ + return FALSE; +} + +/** + * @brief Waits for a new command block + */ +bool_t msd_wait_for_command_block(USBMassStorageDriver *msdp) { + + msd_start_receive(msdp, (uint8_t *)&msdp->cbw, sizeof(msdp->cbw)); + msdp->state = MSD_READ_COMMAND_BLOCK; + + /* wait for ISR */ + return TRUE; +} + +/** + * @brief Reads a newly received command block + */ +bool_t msd_read_command_block(USBMassStorageDriver *msdp) { + + msd_cbw_t *cbw = &(msdp->cbw); + + /* by default transition back to the idle state */ + msdp->state = MSD_IDLE; + + /* check the command */ + if ((cbw->signature != MSD_CBW_SIGNATURE) || + (cbw->lun > 0) || + ((cbw->data_len > 0) && (cbw->flags & 0x1F)) || + (cbw->scsi_cmd_len == 0) || + (cbw->scsi_cmd_len > 16)) { + + /* stall both IN and OUT endpoints */ + chSysLock(); + usbStallReceiveI(msdp->config->usbp, msdp->config->bulk_ep); + usbStallTransmitI(msdp->config->usbp, msdp->config->bulk_ep); + chSysUnlock(); + + /* don't wait for ISR */ + return FALSE; + } + + bool_t sleep = FALSE; + + /* check the command */ + switch (cbw->scsi_cmd_data[0]) { + case SCSI_CMD_INQUIRY: + sleep = msd_scsi_process_inquiry(msdp); + break; + case SCSI_CMD_REQUEST_SENSE: + sleep = msd_scsi_process_request_sense(msdp); + break; + case SCSI_CMD_READ_CAPACITY_10: + sleep = msd_scsi_process_read_capacity_10(msdp); + break; + case SCSI_CMD_READ_10: + case SCSI_CMD_WRITE_10: + if (msdp->config->rw_activity_callback) + msdp->config->rw_activity_callback(TRUE); + sleep = msd_scsi_process_start_read_write_10(msdp); + if (msdp->config->rw_activity_callback) + msdp->config->rw_activity_callback(FALSE); + break; + case SCSI_CMD_SEND_DIAGNOSTIC: + sleep = msd_scsi_process_send_diagnostic(msdp); + break; + case SCSI_CMD_MODE_SENSE_6: + sleep = msd_scsi_process_mode_sense_6(msdp); + break; + case SCSI_CMD_START_STOP_UNIT: + sleep = msd_scsi_process_start_stop_unit(msdp); + break; + case SCSI_CMD_READ_FORMAT_CAPACITIES: + sleep = msd_scsi_process_read_format_capacities(msdp); + break; + case SCSI_CMD_TEST_UNIT_READY: + sleep = msd_scsi_process_test_unit_ready(msdp); + break; + case SCSI_CMD_FORMAT_UNIT: + /* don't handle */ + msdp->result = TRUE; + break; + case SCSI_CMD_PREVENT_ALLOW_MEDIUM_REMOVAL: + /* don't handle */ + msdp->result = TRUE; + break; + case SCSI_CMD_VERIFY_10: + /* don't handle */ + msdp->result = TRUE; + break; + default: + msd_scsi_set_sense(msdp, + SCSI_SENSE_KEY_ILLEGAL_REQUEST, + SCSI_ASENSE_INVALID_COMMAND, + SCSI_ASENSEQ_NO_QUALIFIER); + + /* stall IN endpoint */ + chSysLock(); + usbStallTransmitI(msdp->config->usbp, msdp->config->bulk_ep); + chSysUnlock(); + + return FALSE; + } + + if (msdp->result) { + /* update sense with success status */ + msd_scsi_set_sense(msdp, + SCSI_SENSE_KEY_GOOD, + SCSI_ASENSE_NO_ADDITIONAL_INFORMATION, + SCSI_ASENSEQ_NO_QUALIFIER); + + /* reset data length left */ + cbw->data_len = 0; + } + + /* wait for ISR if needed */ + if (sleep) + msd_wait_for_isr(msdp); + + msd_csw_t *csw = &(msdp->csw); + + if (!msdp->result && cbw->data_len) { + /* still bytes left to send, this is too early to send CSW? */ + chSysLock(); + usbStallReceiveI(msdp->config->usbp, msdp->config->bulk_ep); + usbStallTransmitI(msdp->config->usbp, msdp->config->bulk_ep); + chSysUnlock(); + + /*return FALSE;*/ + } + + /* update the command status wrapper and send it to the host */ + csw->status = (msdp->result) ? MSD_COMMAND_PASSED : MSD_COMMAND_FAILED; + csw->signature = MSD_CSW_SIGNATURE; + csw->data_residue = cbw->data_len; + csw->tag = cbw->tag; + + msd_start_transmit(msdp, (const uint8_t *)csw, sizeof(*csw)); + + /* wait for ISR */ + return TRUE; +} + +/** + * @brief Mass storage thread that processes commands + */ +static WORKING_AREA(mass_storage_thread_wa, 1536); +static msg_t mass_storage_thread(void *arg) { + + USBMassStorageDriver *msdp = (USBMassStorageDriver *)arg; + + chRegSetThreadName("USB-MSD"); + + bool_t wait_for_isr = FALSE; + + /* wait for the usb to be initialised */ + msd_wait_for_isr(msdp); + + while (!chThdShouldTerminate()) { + wait_for_isr = FALSE; + + /* wait on data depending on the current state */ + switch (msdp->state) { + case MSD_IDLE: + wait_for_isr = msd_wait_for_command_block(msdp); + break; + case MSD_READ_COMMAND_BLOCK: + wait_for_isr = msd_read_command_block(msdp); + break; + case MSD_EJECTED: + /* disconnect usb device */ + usbDisconnectBus(msdp->config->usbp); + usbStop(msdp->config->usbp); + chThdExit(0); + return 0; + } + + /* wait until the ISR wakes thread */ + if (wait_for_isr) + msd_wait_for_isr(msdp); + } + + return 0; +} + +/** + * @brief Initializse a USB mass storage driver + */ +void msdInit(USBMassStorageDriver *msdp) { + + chDbgCheck(msdp != NULL, "msdInit"); + + msdp->config = NULL; + msdp->thread = NULL; + msdp->state = MSD_IDLE; + + /* initialize the driver events */ + chEvtInit(&msdp->evt_connected); + chEvtInit(&msdp->evt_ejected); + + /* initialise the binary semaphore as taken */ + chBSemInit(&msdp->bsem, TRUE); + + /* initialise the sense data structure */ + size_t i; + for (i = 0; i < sizeof(msdp->sense.byte); i++) + msdp->sense.byte[i] = 0x00; + msdp->sense.byte[0] = 0x70; /* response code */ + msdp->sense.byte[7] = 0x0A; /* additional sense length */ + + /* initialize the inquiry data structure */ + msdp->inquiry.peripheral = 0x00; /* direct access block device */ + msdp->inquiry.removable = 0x80; /* removable */ + msdp->inquiry.version = 0x04; /* SPC-2 */ + msdp->inquiry.response_data_format = 0x02; /* response data format */ + msdp->inquiry.additional_length = 0x20; /* response has 0x20 + 4 bytes */ + msdp->inquiry.sccstp = 0x00; + msdp->inquiry.bqueetc = 0x00; + msdp->inquiry.cmdque = 0x00; +} + +/** + * @brief Starts a USB mass storage driver + */ +void msdStart(USBMassStorageDriver *msdp, const USBMassStorageConfig *config) { + + chDbgCheck(msdp != NULL, "msdStart"); + chDbgCheck(config != NULL, "msdStart"); + chDbgCheck(msdp->thread == NULL, "msdStart"); + + /* save the configuration */ + msdp->config = config; + + /* copy the config strings to the inquiry response structure */ + size_t i; + for (i = 0; i < sizeof(msdp->config->short_vendor_id); ++i) + msdp->inquiry.vendor_id[i] = config->short_vendor_id[i]; + for (i = 0; i < sizeof(msdp->config->short_product_id); ++i) + msdp->inquiry.product_id[i] = config->short_product_id[i]; + for (i = 0; i < sizeof(msdp->config->short_product_version); ++i) + msdp->inquiry.product_rev[i] = config->short_product_version[i]; + + /* set the initial state */ + msdp->state = MSD_IDLE; + + /* make sure block device is working */ + while (blkGetDriverState(config->bbdp) != BLK_READY) { + chThdSleepMilliseconds(50); + } + + /* get block device information */ + blkGetInfo(config->bbdp, &msdp->block_dev_info); + + /* store the pointer to the mass storage driver into the user param + of the USB driver, so that we can find it back in callbacks */ + config->usbp->in_params[config->bulk_ep] = (void *)msdp; + config->usbp->out_params[config->bulk_ep] = (void *)msdp; + + /* run the thread */ + msdp->thread = chThdCreateStatic(mass_storage_thread_wa, sizeof(mass_storage_thread_wa), NORMALPRIO, mass_storage_thread, msdp); +} + +/** + * @brief Stops a USB mass storage driver + */ +void msdStop(USBMassStorageDriver *msdp) { + + chDbgCheck(msdp->thread != NULL, "msdStop"); + + /* notify the thread that it's over */ + chThdTerminate(msdp->thread); + + /* wake the thread up and wait until it ends */ + chBSemSignal(&msdp->bsem); + chThdWait(msdp->thread); + msdp->thread = NULL; + + /* release the user params in the USB driver */ + msdp->config->usbp->in_params[msdp->config->bulk_ep] = NULL; + msdp->config->usbp->out_params[msdp->config->bulk_ep] = NULL; +} diff --git a/sw/airborne/subsystems/chibios-libopencm3/usb_msd.h b/sw/airborne/subsystems/chibios-libopencm3/usb_msd.h new file mode 100644 index 0000000000..c8ca2954d1 --- /dev/null +++ b/sw/airborne/subsystems/chibios-libopencm3/usb_msd.h @@ -0,0 +1,209 @@ +/* + * Copyright (C) 2014 Gautier Hattenberger, Alexandre Bustico + * + * This file is part of paparazzi. + * + * paparazzi 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 2, or (at your option) + * any later version. + * + * paparazzi 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 paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/* + * @file subsystems/chibios-libopencm3/usb_msd.h + * + */ + +#pragma once + +#include "ch.h" +#include "hal.h" + +/** + * @brief Command Block Wrapper structure + */ +PACK_STRUCT_BEGIN typedef struct { + uint32_t signature; + uint32_t tag; + uint32_t data_len; + uint8_t flags; + uint8_t lun; + uint8_t scsi_cmd_len; + uint8_t scsi_cmd_data[16]; +} PACK_STRUCT_STRUCT msd_cbw_t PACK_STRUCT_END; + +/** + * @brief Command Status Wrapper structure + */ +PACK_STRUCT_BEGIN typedef struct { + uint32_t signature; + uint32_t tag; + uint32_t data_residue; + uint8_t status; +} PACK_STRUCT_STRUCT msd_csw_t PACK_STRUCT_END; + +/** + * @brief Structure holding sense data (status/error information) + */ +PACK_STRUCT_BEGIN typedef struct { + uint8_t byte[18]; +} PACK_STRUCT_STRUCT msd_scsi_sense_response_t PACK_STRUCT_END; + +/** + * @brief structure holding the data to reply to an INQUIRY SCSI command + */ +PACK_STRUCT_BEGIN typedef struct +{ + uint8_t peripheral; + uint8_t removable; + uint8_t version; + uint8_t response_data_format; + uint8_t additional_length; + uint8_t sccstp; + uint8_t bqueetc; + uint8_t cmdque; + uint8_t vendor_id[8]; + uint8_t product_id[16]; + uint8_t product_rev[4]; +} PACK_STRUCT_STRUCT msd_scsi_inquiry_response_t PACK_STRUCT_END; + +/** + * @brief Possible states for the USB mass storage driver + */ +typedef enum { + MSD_IDLE, + MSD_READ_COMMAND_BLOCK, + MSD_EJECTED +} msd_state_t; + +/** + * @brief Driver configuration structure + */ +typedef struct { + /** + * @brief USB driver to use for communication + */ + USBDriver *usbp; + + /** + * @brief Block device to use for storage + */ + BaseBlockDevice *bbdp; + + /** + * @brief Index of the USB endpoint to use for transfers + */ + usbep_t bulk_ep; + + /** + * @brief Optional callback that will be called whenever there is + * read/write activity + * @note The callback is called with argument TRUE when activity starts, + * and FALSE when activity stops. + */ + void (*rw_activity_callback)(bool_t); + + /** + * @brief Short vendor identification + * @note ASCII characters only, maximum 8 characters (pad with zeroes). + */ + uint8_t short_vendor_id[8]; + + /** + * @brief Short product identification + * @note ASCII characters only, maximum 16 characters (pad with zeroes). + */ + uint8_t short_product_id[16]; + + /** + * @brief Short product revision + * @note ASCII characters only, maximum 4 characters (pad with zeroes). + */ + uint8_t short_product_version[4]; + +} USBMassStorageConfig; + +/** + * @brief USB mass storage driver structure. + * @details This structure holds all the states and members of a USB mass + * storage driver. + */ +typedef struct { + const USBMassStorageConfig* config; + BinarySemaphore bsem; + Thread* thread; + EventSource evt_connected, evt_ejected; + BlockDeviceInfo block_dev_info; + msd_state_t state; + msd_cbw_t cbw; + msd_csw_t csw; + msd_scsi_sense_response_t sense; + msd_scsi_inquiry_response_t inquiry; + bool_t result; +} USBMassStorageDriver; + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * @brief Initializes a USB mass storage driver. + */ +void msdInit(USBMassStorageDriver *msdp); + +/** + * @brief Starts a USB mass storage driver. + * @details This function is sufficient to have USB mass storage running, it internally + * runs a thread that handles USB requests and transfers. + * The block device must be connected but no file system must be mounted, + * everything is handled by the host system. + */ +void msdStart(USBMassStorageDriver *msdp, const USBMassStorageConfig *config); + +/** + * @brief Stops a USB mass storage driver. + * @details This function waits for current tasks to be finished, if any, and then + * stops the mass storage thread. + */ +void msdStop(USBMassStorageDriver *msdp); + +/** + * @brief USB device configured handler. + * + * @param[in] msdp pointer to the @p USBMassStorageDriver object + * + * @iclass + */ +void msdConfigureHookI(USBMassStorageDriver *msdp); + +/** + * @brief Default requests hook. + * @details Applications wanting to use the Mass Storage over USB driver can use + * this function as requests hook in the USB configuration. + * The following requests are emulated: + * - MSD_REQ_RESET. + * - MSD_GET_MAX_LUN. + * . + * + * @param[in] usbp pointer to the @p USBDriver object + * @return The hook status. + * @retval TRUE Message handled internally. + * @retval FALSE Message not handled. + */ +bool_t msdRequestsHook(USBDriver *usbp); + +#ifdef __cplusplus +} +#endif + +