From 021510e4467699ab7e260c404b588fa061b915ae Mon Sep 17 00:00:00 2001
From: bugobliterator <siddharthbharatpurohit@gmail.com>
Date: Thu, 13 Jun 2024 16:49:50 +1000
Subject: [PATCH] AP_Bootloader: add support for secondary FC bootloader to
 appear on second USB endpoint

---
 Tools/AP_Bootloader/bl_protocol.cpp | 12 +++++--
 Tools/AP_Bootloader/support.cpp     | 53 +++++++++++++++++++++++++++--
 Tools/AP_Bootloader/support.h       |  4 ++-
 3 files changed, 63 insertions(+), 6 deletions(-)

diff --git a/Tools/AP_Bootloader/bl_protocol.cpp b/Tools/AP_Bootloader/bl_protocol.cpp
index 5bc0df7b28..f6801adb7e 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 0954f7b753..1302db2d01 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);