diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp index 4d7d1a2cda..48a1f4b42e 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp @@ -569,6 +569,17 @@ void UARTDriver::rxbuff_full_irq(void* self, uint32_t flags) void UARTDriver::begin(uint32_t b) { + if (lock_write_key != 0) { + return; + } + begin(b, 0, 0); +} + +void UARTDriver::begin_locked(uint32_t b, uint32_t key) +{ + if (lock_write_key != 0 && key != lock_write_key) { + return; + } begin(b, 0, 0); } @@ -618,6 +629,20 @@ void UARTDriver::set_blocking_writes(bool blocking) bool UARTDriver::tx_pending() { return false; } + +/* + get the requested usb baudrate - 0 = none +*/ +uint32_t UARTDriver::get_usb_baud() const +{ +#if HAL_USE_SERIAL_USB + if (sdef.is_usb) { + return ::get_usb_baud(sdef.endpoint_id); + } +#endif + return 0; +} + /* Empty implementations of Stream virtual methods */ uint32_t UARTDriver::available() { if (!_rx_initialised || lock_read_key) { diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.h b/libraries/AP_HAL_ChibiOS/UARTDriver.h index 37e2924962..fa53ebaac2 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.h +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.h @@ -37,13 +37,14 @@ public: UARTDriver &operator=(const UARTDriver&) = delete; void begin(uint32_t b) override; + void begin_locked(uint32_t b, uint32_t write_key) override; void begin(uint32_t b, uint16_t rxS, uint16_t txS) override; void end() override; void flush() override; bool is_initialized() override; void set_blocking_writes(bool blocking) override; bool tx_pending() override; - + uint32_t get_usb_baud() const override; uint32_t available() override; uint32_t available_locked(uint32_t key) override; @@ -91,6 +92,7 @@ public: uint8_t rxinv_polarity; int8_t txinv_gpio; uint8_t txinv_polarity; + uint8_t endpoint_id; uint8_t get_index(void) const { return uint8_t(this - &_serial_tab[0]); } diff --git a/libraries/AP_HAL_ChibiOS/Util.cpp b/libraries/AP_HAL_ChibiOS/Util.cpp index 6d4acd6a4a..0588559a11 100644 --- a/libraries/AP_HAL_ChibiOS/Util.cpp +++ b/libraries/AP_HAL_ChibiOS/Util.cpp @@ -24,7 +24,6 @@ #include "hwdef/common/stm32_util.h" #include "hwdef/common/watchdog.h" #include "hwdef/common/flash.h" -#include "hwdef/common/usbcfg.h" #include #include #include "sdcard.h" @@ -226,17 +225,6 @@ uint64_t Util::get_hw_rtc() const return stm32_get_utc_usec(); } -/* - get the requested usb baudrate - 0 = none -*/ -uint32_t Util::get_usb_baud(uint16_t endpoint_id) -{ -#if defined(HAL_USB_PRODUCT_ID) && HAL_HAVE_DUAL_USB_CDC - return ::get_usb_baud(endpoint_id); -#endif - return 0; -} - #if !defined(HAL_NO_FLASH_SUPPORT) && !defined(HAL_NO_ROMFS_SUPPORT) #if defined(HAL_NO_GCS) || defined(HAL_BOOTLOADER_BUILD) diff --git a/libraries/AP_HAL_ChibiOS/Util.h b/libraries/AP_HAL_ChibiOS/Util.h index 0ae4c0a701..93cb575a04 100644 --- a/libraries/AP_HAL_ChibiOS/Util.h +++ b/libraries/AP_HAL_ChibiOS/Util.h @@ -110,12 +110,6 @@ private: get system clock in UTC microseconds */ uint64_t get_hw_rtc() const override; - - /* - get the requested usb baudrate - 0 = none - */ - uint32_t get_usb_baud(uint16_t endpoint_id) override; - #if !defined(HAL_NO_FLASH_SUPPORT) && !defined(HAL_NO_ROMFS_SUPPORT) FlashBootloader flash_bootloader() override; #endif diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.c b/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.c index c67022c3a7..fd04b9abab 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.c @@ -39,6 +39,11 @@ /* Virtual serial port over USB.*/ SerialUSBDriver SDU1; +cdc_linecoding_t linecoding = { + {0x00, 0x96, 0x00, 0x00}, /* 38400. */ + LC_STOP_1, LC_PARITY_NONE, 8 +}; + /* * Endpoints to be used for USBD1. */ @@ -233,6 +238,17 @@ static const USBDescriptor *get_descriptor(USBDriver *usbp, return NULL; } +/* + get the requested usb baudrate - 0 = none +*/ +#if HAL_USE_SERIAL_USB +uint32_t get_usb_baud(uint16_t endpoint_id) +{ + if(endpoint_id == 0) + return *((uint32_t*)linecoding.dwDTERate); + return 0; +} +#endif /** * @brief IN EP1 state. */ @@ -329,6 +345,33 @@ static void usb_event(USBDriver *usbp, usbevent_t event) { return; } +/* + * Handling messages not implemented in the default handler nor in the + * SerialUSB handler. + */ +static bool requests_hook(USBDriver *usbp) { + if (((usbp->setup[0] & USB_RTYPE_RECIPIENT_MASK) == USB_RTYPE_RECIPIENT_INTERFACE) && + (usbp->setup[1] == USB_REQ_SET_INTERFACE)) { + usbSetupTransfer(usbp, NULL, 0, NULL); + return true; + } + if ((usbp->setup[0] & USB_RTYPE_TYPE_MASK) == USB_RTYPE_TYPE_CLASS && usbp->setup[4] == 0x00 && usbp->setup[5] == 0x00) { + switch (usbp->setup[1]) { + case CDC_GET_LINE_CODING: + usbSetupTransfer(usbp, (uint8_t *)&linecoding, sizeof(linecoding), NULL); + return true; + case CDC_SET_LINE_CODING: + usbSetupTransfer(usbp, (uint8_t *)&linecoding, sizeof(linecoding), NULL); + return true; + case CDC_SET_CONTROL_LINE_STATE: + /* Nothing to do, there are no control lines.*/ + usbSetupTransfer(usbp, NULL, 0, NULL); + return true; + } + } + return sduRequestsHook(usbp); +} + /* * Handles the USB driver global events. */ @@ -347,7 +390,7 @@ static void sof_handler(USBDriver *usbp) { const USBConfig usbcfg = { usb_event, get_descriptor, - sduRequestsHook, + requests_hook, sof_handler }; diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.h b/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.h index e8d35ea9e3..2add6e6c7f 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.h @@ -44,8 +44,8 @@ extern SerialUSBDriver SDU1; #if HAL_HAVE_DUAL_USB_CDC extern SerialUSBDriver SDU2; extern const SerialUSBConfig serusbcfg2; -uint32_t get_usb_baud(uint16_t endpoint_id); #endif //HAL_HAVE_DUAL_USB_CDC +uint32_t get_usb_baud(uint16_t endpoint_id); #endif #define USB_DESC_MAX_STRLEN 100 void setup_usb_strings(void); diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg_dualcdc.c b/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg_dualcdc.c index 8dc342e167..c3c35e02d8 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg_dualcdc.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg_dualcdc.c @@ -300,17 +300,16 @@ static const USBDescriptor *get_descriptor(USBDriver *usbp, /* get the requested usb baudrate - 0 = none */ +#if HAL_USE_SERIAL_USB uint32_t get_usb_baud(uint16_t endpoint_id) { -#if defined(HAL_USB_PRODUCT_ID) && HAL_HAVE_DUAL_USB_CDC if(endpoint_id == 0) return *((uint32_t*)linecoding.dwDTERate); if(endpoint_id == 2) return *((uint32_t*)linecoding2.dwDTERate); -#endif return 0; } - +#endif /** * @brief IN EP1 state. */ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index a9c4bd5bf9..e7aef09576 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -1343,13 +1343,13 @@ def write_UART_config(f): if dev.startswith('OTG2'): f.write( - '#define HAL_%s_CONFIG {(BaseSequentialStream*) &SDU2, 2, true, false, 0, 0, false, 0, 0}\n' + '#define HAL_%s_CONFIG {(BaseSequentialStream*) &SDU2, 2, true, false, 0, 0, false, 0, 0, 2}\n' % dev) OTG2_index = uart_list.index(dev) dual_USB_enabled = True elif dev.startswith('OTG'): f.write( - '#define HAL_%s_CONFIG {(BaseSequentialStream*) &SDU1, 1, true, false, 0, 0, false, 0, 0}\n' + '#define HAL_%s_CONFIG {(BaseSequentialStream*) &SDU1, 1, true, false, 0, 0, false, 0, 0, 0}\n' % dev) else: need_uart_driver = True @@ -1366,7 +1366,7 @@ def write_UART_config(f): f.write("%d, " % get_gpio_bylabel(dev + "_RXINV")) f.write("%s, " % get_extra_bylabel(dev + "_RXINV", "POL", "0")) f.write("%d, " % get_gpio_bylabel(dev + "_TXINV")) - f.write("%s}\n" % get_extra_bylabel(dev + "_TXINV", "POL", "0")) + f.write("%s, 0}\n" % get_extra_bylabel(dev + "_TXINV", "POL", "0")) if have_rts_cts: f.write('#define AP_FEATURE_RTSCTS 1\n') if OTG2_index is not None: