diff --git a/Tools/AP_Bootloader/bl_protocol.cpp b/Tools/AP_Bootloader/bl_protocol.cpp index 9b505664d6..a270abb81e 100644 --- a/Tools/AP_Bootloader/bl_protocol.cpp +++ b/Tools/AP_Bootloader/bl_protocol.cpp @@ -505,8 +505,16 @@ bootloader(unsigned timeout) led_off(LED_ACTIVITY); do { - /* if we have a timeout and the timer has expired, return now */ - if (timeout && !timer[TIMER_BL_WAIT]) { + /* if we have a timeout and the timer has expired and serial forward is not busy, return now */ +#if defined(BOOTLOADER_FORWARD_OTG2_SERIAL) + bool ser_forward_active = update_otg2_serial_forward(); +#endif + if (timeout && !timer[TIMER_BL_WAIT] +#if defined(BOOTLOADER_FORWARD_OTG2_SERIAL) + // do serial forward only when idle + && !ser_forward_active +#endif + ) { return; } diff --git a/Tools/AP_Bootloader/support.cpp b/Tools/AP_Bootloader/support.cpp index 86d51d35f2..9a02124b1a 100644 --- a/Tools/AP_Bootloader/support.cpp +++ b/Tools/AP_Bootloader/support.cpp @@ -445,7 +445,11 @@ void init_uarts(void) #if HAL_USE_SERIAL_USB == TRUE sduObjectInit(&SDU1); sduStart(&SDU1, &serusbcfg1); - +#if HAL_HAVE_DUAL_USB_CDC + sduObjectInit(&SDU2); + sduStart(&SDU2, &serusbcfg2); +#endif + usbDisconnectBus(serusbcfg1.usbp); thread_sleep_ms(1000); usbStart(serusbcfg1.usbp, &usbcfg); @@ -457,7 +461,11 @@ void init_uarts(void) for (const auto &uart : uarts) { #if HAL_USE_SERIAL_USB == TRUE - if (uart == (BaseChannel *)&SDU1) { + if (uart == (BaseChannel *)&SDU1 +#if HAL_HAVE_DUAL_USB_CDC + || uart == (BaseChannel *)&SDU2 +#endif + ) { continue; } #endif @@ -467,13 +475,52 @@ void init_uarts(void) } +#if defined(BOOTLOADER_FORWARD_OTG2_SERIAL) +/* forward serial to OTG2 +Used for devices containing multiple devices in one +*/ +static SerialConfig forward_sercfg; +static uint32_t otg2_serial_deadline_ms; +bool update_otg2_serial_forward() +{ + // get baudrate set on SDU2 and set it on HAL_FORWARD_OTG2_SERIAL if changed + if (forward_sercfg.speed != BOOTLOADER_FORWARD_OTG2_SERIAL_BAUDRATE) { + forward_sercfg.speed = BOOTLOADER_FORWARD_OTG2_SERIAL_BAUDRATE; +#if defined(BOOTLOADER_FORWARD_OTG2_SERIAL_SWAP) && BOOTLOADER_FORWARD_OTG2_SERIAL_SWAP + forward_sercfg.cr2 = USART_CR2_SWAP; +#endif + sdStart(&BOOTLOADER_FORWARD_OTG2_SERIAL, &forward_sercfg); + } + // check how many bytes are available to read from HAL_FORWARD_OTG2_SERIAL + uint8_t data[SERIAL_BUFFERS_SIZE]; // read upto SERIAL_BUFFERS_SIZE at a time + int n = chnReadTimeout(&SDU2, data, SERIAL_BUFFERS_SIZE, TIME_IMMEDIATE); + if (n > 0) { + // do a blocking write to HAL_FORWARD_OTG2_SERIAL + chnWriteTimeout(&BOOTLOADER_FORWARD_OTG2_SERIAL, data, n, TIME_IMMEDIATE); + otg2_serial_deadline_ms = AP_HAL::millis() + 1000; + } + + n = chnReadTimeout(&BOOTLOADER_FORWARD_OTG2_SERIAL, data, SERIAL_BUFFERS_SIZE, TIME_IMMEDIATE); + if (n > 0) { + // do a blocking write to SDU2 + chnWriteTimeout(&SDU2, data, n, TIME_IMMEDIATE); + } + + return (AP_HAL::millis() > otg2_serial_deadline_ms); +} +#endif + /* set baudrate on the current port */ void port_setbaud(uint32_t baudrate) { #if HAL_USE_SERIAL_USB == TRUE - if (uarts[last_uart] == (BaseChannel *)&SDU1) { + if (uarts[last_uart] == (BaseChannel *)&SDU1 +#if HAL_HAVE_DUAL_USB_CDC + || uarts[last_uart] == (BaseChannel *)&SDU2 +#endif + ) { // can't set baudrate on USB return; } diff --git a/Tools/AP_Bootloader/support.h b/Tools/AP_Bootloader/support.h index 1eebcd22fe..5dda7a38b2 100644 --- a/Tools/AP_Bootloader/support.h +++ b/Tools/AP_Bootloader/support.h @@ -18,7 +18,9 @@ int16_t cin(unsigned timeout_ms); int cin_word(uint32_t *wp, unsigned timeout_ms); void cout(uint8_t *data, uint32_t len); void port_setbaud(uint32_t baudrate); - +#if defined(BOOTLOADER_FORWARD_OTG2_SERIAL) +bool update_otg2_serial_forward(void); +#endif void flash_init(); uint32_t flash_func_read_word(uint32_t offset);