diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/chibios_board.mk b/libraries/AP_HAL_ChibiOS/hwdef/common/chibios_board.mk index df4fcea197..e94639b688 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/chibios_board.mk +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/chibios_board.mk @@ -133,6 +133,7 @@ CSRC += $(HWDEF)/common/stubs.c \ $(HWDEF)/common/usbcfg.c \ $(HWDEF)/common/usbcfg_dualcdc.c \ $(HWDEF)/common/usbcfg_common.c \ + $(HWDEF)/common/usbcfg_cdc_msd.c \ $(HWDEF)/common/flash.c \ $(HWDEF)/common/malloc.c \ $(HWDEF)/common/hrt.c \ @@ -183,6 +184,7 @@ ASMSRC = $(ALLASMSRC) ASMXSRC = $(ALLXASMSRC) INCDIR = $(CHIBIOS)/os/license \ + $(CHIBIOS)/os/various \ $(ALLINC) $(HWDEF)/common ifneq ($(CRASHCATCHER),) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_mcuconf.h b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_mcuconf.h index 2d3e5bc179..439864a13f 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_mcuconf.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_mcuconf.h @@ -615,8 +615,8 @@ #endif #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 1024 +#define STM32_USB_OTG1_RX_FIFO_SIZE 2048 +#define STM32_USB_OTG2_RX_FIFO_SIZE 2048 #define STM32_USB_HOST_WAKEUP_DURATION 2 /* diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.c b/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.c index 5c02c1dfef..02e9467df1 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.c @@ -34,7 +34,7 @@ #include "usbcfg.h" // #pragma GCC optimize("O0") -#if defined(HAL_USB_PRODUCT_ID) && !HAL_HAVE_DUAL_USB_CDC +#if defined(HAL_USB_PRODUCT_ID) && !HAL_HAVE_DUAL_USB_CDC && !HAL_HAVE_USB_CDC_MSD /* Virtual serial port over USB.*/ SerialUSBDriver SDU1; diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.h b/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.h index 2add6e6c7f..7a31718a91 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.h @@ -33,6 +33,10 @@ #define HAL_HAVE_DUAL_USB_CDC 0 #endif +#ifndef HAL_HAVE_USB_CDC_MSD +#define HAL_HAVE_USB_CDC_MSD 0 +#endif + #if defined(__cplusplus) extern "C" { #endif diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg_cdc_msd.c b/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg_cdc_msd.c new file mode 100644 index 0000000000..e650a46188 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg_cdc_msd.c @@ -0,0 +1,494 @@ +/* + ChibiOS - Copyright (C) 2006..2016 Giovanni Di Sirio + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. +*/ + +#include "hal.h" +#include "hwdef.h" + +#include +#include + +#include "hal_usb_msd.h" +#include "usbcfg.h" + +#if HAL_HAVE_USB_CDC_MSD +/* + * must be 64 for full speed and 512 for high speed + */ +#define USB_MSD_EP_SIZE 512U +#define USB_DATA_REQUEST_EP 2 +#define USB_DATA_AVAILABLE_EP 2 +#define USB_INTERRUPT_REQUEST_EP 3 + +SerialUSBDriver SDU1; + +static cdc_linecoding_t linecoding = { + {0x00, 0x96, 0x00, 0x00}, /* 38400. */ + LC_STOP_1, LC_PARITY_NONE, 8 +}; + +/* + * USB Device Descriptor. + */ +static const uint8_t vcom_device_descriptor_data[18] = { + USB_DESC_DEVICE( + 0x0110, /* bcdUSB (1.1). */ + 0x02, /* bDeviceClass (CDC). */ + 0x00, /* bDeviceSubClass. */ + 0x00, /* bDeviceProtocol. */ + 0x40, /* bMaxPacketSize. */ + HAL_USB_VENDOR_ID, /* idVendor (ST). */ + HAL_USB_PRODUCT_ID, /* idProduct. */ + 0x0200, /* bcdDevice. */ + 1, /* iManufacturer. */ + 2, /* iProduct. */ + 3, /* iSerialNumber. */ + 1) /* bNumConfigurations. */ +}; + + +/* + * Device Descriptor wrapper. + */ +static const USBDescriptor vcom_device_descriptor = { + sizeof vcom_device_descriptor_data, + vcom_device_descriptor_data +}; + +/* Configuration Descriptor tree for a CDC.*/ +static const uint8_t vcom_configuration_descriptor_data[] = { + /* Configuration Descriptor.*/ + USB_DESC_CONFIGURATION(0x0020, /* wTotalLength. */ + 0x01, /* bNumInterfaces. */ + 0x01, /* bConfigurationValue. */ + 0, /* iConfiguration. */ + 0xC0, /* bmAttributes (self powered). */ + 0x32), /* bMaxPower (100mA). */ + /* Interface Descriptor.*/ + USB_DESC_INTERFACE (0x00, /* bInterfaceNumber. */ + 0x00, /* bAlternateSetting. */ + 0x02, /* bNumEndpoints. */ + 0x08, /* bInterfaceClass (Mass Storage) */ + 0x06, /* bInterfaceSubClass (SCSI + Transparent storage class) */ + 0x50, /* bInterfaceProtocol (Bulk Only) */ + 0), /* iInterface. (none) */ + /* Mass Storage Data In Endpoint Descriptor.*/ + USB_DESC_ENDPOINT (USB_MSD_DATA_EP | 0x80, + 0x02, /* bmAttributes (Bulk). */ + USB_MSD_EP_SIZE, /* wMaxPacketSize. */ + 0x00), /* bInterval. 1ms */ + /* Mass Storage Data Out Endpoint Descriptor.*/ + USB_DESC_ENDPOINT (USB_MSD_DATA_EP, + 0x02, /* bmAttributes (Bulk). */ + USB_MSD_EP_SIZE, /* wMaxPacketSize. */ + 0x00), /* bInterval. 1ms */ + // CDC + /* IAD Descriptor */ + USB_DESC_INTERFACE_ASSOCIATION(0x01, /* bFirstInterface. */ + 0x02, /* bInterfaceCount. */ + 0x02, /* bFunctionClass (CDC). */ + 0x00, /* bFunctionSubClass. (2) */ + 0x00, /* bFunctionProtocol (1) */ + 2), /* iInterface. */ + /* Interface Descriptor.*/ + USB_DESC_INTERFACE (0x01, /* bInterfaceNumber. */ + 0x00, /* bAlternateSetting. */ + 0x01, /* bNumEndpoints. */ + 0x02, /* bInterfaceClass (Communications + Interface Class, CDC section + 4.2). */ + 0x02, /* bInterfaceSubClass (Abstract + Control Model, CDC section 4.3). */ + 0x01, /* bInterfaceProtocol (AT commands, + CDC section 4.4). */ + 0), /* iInterface. */ + /* Header Functional Descriptor (CDC section 5.2.3).*/ + USB_DESC_BYTE (5), /* bLength. */ + USB_DESC_BYTE (0x24), /* bDescriptorType (CS_INTERFACE). */ + USB_DESC_BYTE (0x00), /* bDescriptorSubtype (Header + Functional Descriptor. */ + USB_DESC_BCD (0x0110), /* bcdCDC. */ + /* Call Management Functional Descriptor. */ + USB_DESC_BYTE (5), /* bFunctionLength. */ + USB_DESC_BYTE (0x24), /* bDescriptorType (CS_INTERFACE). */ + USB_DESC_BYTE (0x01), /* bDescriptorSubtype (Call Management + Functional Descriptor). */ + USB_DESC_BYTE (0x00), /* bmCapabilities (D0+D1). */ + USB_DESC_BYTE (0x02), /* bDataInterface. */ + /* ACM Functional Descriptor.*/ + USB_DESC_BYTE (4), /* bFunctionLength. */ + USB_DESC_BYTE (0x24), /* bDescriptorType (CS_INTERFACE). */ + USB_DESC_BYTE (0x02), /* bDescriptorSubtype (Abstract + Control Management Descriptor). */ + USB_DESC_BYTE (0x02), /* bmCapabilities. */ + /* Union Functional Descriptor.*/ + USB_DESC_BYTE (5), /* bFunctionLength. */ + USB_DESC_BYTE (0x24), /* bDescriptorType (CS_INTERFACE). */ + USB_DESC_BYTE (0x06), /* bDescriptorSubtype (Union + Functional Descriptor). */ + USB_DESC_BYTE (0x01), /* bMasterInterface (Communication + Class Interface). */ + USB_DESC_BYTE (0x02), /* bSlaveInterface0 (Data Class + Interface). */ + /* Endpoint 5 Descriptor.*/ + USB_DESC_ENDPOINT (USB_INTERRUPT_REQUEST_EP|0x80, + 0x03, /* bmAttributes (Interrupt). */ + 0x0008, /* wMaxPacketSize. */ + 0xFF), /* bInterval. */ + /* Interface Descriptor.*/ + USB_DESC_INTERFACE (0x02, /* bInterfaceNumber. */ + 0x00, /* bAlternateSetting. */ + 0x02, /* bNumEndpoints. */ + 0x0A, /* bInterfaceClass (Data Class + Interface, CDC section 4.5). */ + 0x00, /* bInterfaceSubClass (CDC section + 4.6). */ + 0x00, /* bInterfaceProtocol (CDC section + 4.7). */ + 0x00), /* iInterface. */ + /* Endpoint 4 Descriptor.*/ + USB_DESC_ENDPOINT (USB_DATA_AVAILABLE_EP, /* bEndpointAddress.*/ + 0x02, /* bmAttributes (Bulk). */ + 0x0040, /* wMaxPacketSize. */ + 0x00), /* bInterval. */ + /* Endpoint 4 Descriptor.*/ + USB_DESC_ENDPOINT (USB_DATA_REQUEST_EP|0x80, /* bEndpointAddress.*/ + 0x02, /* bmAttributes (Bulk). */ + 0x0040, /* wMaxPacketSize. */ + 0x00) /* bInterval. */ +}; + +/* + * Configuration Descriptor wrapper. + */ +static const USBDescriptor vcom_configuration_descriptor = { + sizeof vcom_configuration_descriptor_data, + vcom_configuration_descriptor_data +}; + +/* + * U.S. English language identifier. + */ +static const uint8_t vcom_string0[] = { + USB_DESC_BYTE(4), /* bLength. */ + USB_DESC_BYTE(USB_DESCRIPTOR_STRING), /* bDescriptorType. */ + USB_DESC_WORD(0x0409) /* wLANGID (U.S. English). */ +}; + +/* + * Strings wrappers array. The strings are created dynamically to + * allow them to be setup with apj_tool + */ +static USBDescriptor vcom_strings[] = { + {sizeof vcom_string0, vcom_string0}, + {0, NULL}, // manufacturer + {0, NULL}, // product + {0, NULL}, // version +}; + +static uint8_t vcom_buffers[3][2+2*USB_DESC_MAX_STRLEN]; + +/* + dynamically allocate a USB descriptor string + */ +static void setup_usb_string(USBDescriptor *desc, const char *str, uint8_t *b) +{ + char str2[USB_DESC_MAX_STRLEN]; + string_substitute(str, str2); + uint8_t len = strlen(str2); + desc->ud_size = 2+2*len; + desc->ud_string = (const uint8_t *)b; + b[0] = USB_DESC_BYTE(desc->ud_size); + b[1] = USB_DESC_BYTE(USB_DESCRIPTOR_STRING); + uint8_t i; + for (i=0; isetup[0] & USB_RTYPE_TYPE_MASK) == USB_RTYPE_TYPE_CLASS) && + ((usbp->setup[0] & USB_RTYPE_RECIPIENT_MASK) == + USB_RTYPE_RECIPIENT_INTERFACE)) { + + if (MSD_SETUP_INDEX(usbp->setup) == USB_MSD_INTERFACE) { + + 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; + } + chSysLockFromISR(); + usbStallReceiveI(usbp, 1); + usbStallTransmitI(usbp, 1); + chSysUnlockFromISR(); + + /* response to this request using EP0 */ + + usbSetupTransfer(usbp, 0, 0, NULL); + 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; + } + // send 0 packet to indicate that we don't do LUN + zbuf = 0; + usbSetupTransfer(usbp, &zbuf, 1, NULL); + return true; + default: + return false; + } + } + } + + return sduRequestsHook(usbp); +} + + +/* + * Handles the USB driver global events. + */ +static void sof_handler(USBDriver *usbp) { + + (void)usbp; + + osalSysLockFromISR(); + sduSOFHookI(&SDU1); + osalSysUnlockFromISR(); +} + +// USB Driver configuration. + +const USBConfig usbcfg = { + usb_event, + get_descriptor, + hybridRequestHook, + sof_handler +}; + + +// Serial over USB driver configuration. + +const SerialUSBConfig serusbcfg1 = { +#if STM32_USB_USE_OTG1 + &USBD1, +#else + &USBD2, +#endif + USB_DATA_REQUEST_EP, + USB_DATA_AVAILABLE_EP, + USB_INTERRUPT_REQUEST_EP +}; +#endif // HAL_HAVE_USB_CDC_MSD diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index dd16aa4d91..3770f7348f 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -937,6 +937,12 @@ class ChibiOSHWDef(object): f.write('#define HAL_USE_USB TRUE\n') f.write('#define HAL_USE_SERIAL_USB TRUE\n') f.write('#define BOARD_OTG2_USES_ULPI\n') + + if self.get_config('HAL_USE_USB_MSD', required=False): + f.write('#define HAL_USE_USB_MSD TRUE\n') + f.write('#define HAL_HAVE_USB_CDC_MSD 1\n') + self.build_flags.append('USE_USB_MSD=yes') + defines = self.get_mcu_config('DEFINES', False) if defines is not None: for d in defines.keys(): @@ -988,9 +994,6 @@ class ChibiOSHWDef(object): if result: self.intdefines[result.group(1)] = int(result.group(2)) - if self.intdefines.get('HAL_USE_USB_MSD',0) == 1: - self.build_flags.append('USE_USB_MSD=yes') - if self.have_type_prefix('CAN') and not using_chibios_can: self.enable_can(f) flash_size = self.get_config('FLASH_SIZE_KB', type=int) diff --git a/libraries/AP_HAL_ChibiOS/sdcard.cpp b/libraries/AP_HAL_ChibiOS/sdcard.cpp index c8cc8dcf7e..b8472ede77 100644 --- a/libraries/AP_HAL_ChibiOS/sdcard.cpp +++ b/libraries/AP_HAL_ChibiOS/sdcard.cpp @@ -23,6 +23,7 @@ #include #include "bouncebuffer.h" #include "stm32_util.h" +#include "hwdef/common/usbcfg.h" extern const AP_HAL::HAL& hal; @@ -47,6 +48,11 @@ static SPIConfig lowspeed; static SPIConfig highspeed; #endif +#if HAL_HAVE_USB_CDC_MSD +static uint8_t blkbuf[512]; +static uint8_t txbuf[512]; +#endif + /* initialise microSD card if avaialble. This is called during AP_BoardConfig initialisation. The parameter BRD_SD_SLOWDOWN @@ -88,13 +94,41 @@ bool sdcard_init() sdcStop(&sdcd); continue; } - if (f_mount(&SDC_FS, "/", 1) != FR_OK) { + FRESULT res = f_mount(&SDC_FS, "/", 1); +#if defined(HAL_SDMMC_TYPE_EMMC) + if (res == FR_NO_FILESYSTEM) { + //format eMMC + MKFS_PARM opt = {0}; + opt.fmt = FM_EXFAT; + res = f_mkfs("/", &opt, 0, 4096); + if (res == FR_OK) { + res = f_mount(&SDC_FS, "/", 1); + } + } +#endif + if (res != FR_OK) { sdcDisconnect(&sdcd); sdcStop(&sdcd); continue; } printf("Successfully mounted SDCard (slowdown=%u)\n", (unsigned)sd_slowdown); - +#if HAL_HAVE_USB_CDC_MSD + if (USBMSD1.state == USB_MSD_UNINIT) { + msdObjectInit(&USBMSD1); + msdStart(&USBMSD1, + #if STM32_USB_USE_OTG1 + &USBD1, + #else + &USBD2, + #endif + (BaseBlockDevice*)&SDCD1, blkbuf, txbuf, NULL, NULL); + usbDisconnectBus(serusbcfg1.usbp); + usbStop(serusbcfg1.usbp); + chThdSleep(chTimeUS2I(1500)); + usbStart(serusbcfg1.usbp, &usbcfg); + usbConnectBus(serusbcfg1.usbp); + } +#endif sdcard_running = true; return true; }