Passthrough, USB Improvements

lib/main/STM32_USB_Device_Library/Class/cdc/inc/usbd_cdc_core.h

@@ -125,6 +125,7 @@ extern USBD_Class_cb_TypeDef  USBD_CDC_cb;
 /** @defgroup USB_CORE_Exported_Functions
   * @{
   */
+void USBD_CDC_ReceivePacket(void *pdev);
 /**
   * @}
   */ 

lib/main/STM32_USB_Device_Library/Class/cdc/src/usbd_cdc_core.c

@@ -674,7 +674,10 @@ uint8_t  usbd_cdc_DataOut (void *pdev, uint8_t epnum)
 
   /* USB data will be immediately processed, this allow next USB traffic being 
   NAKed till the end of the application Xfer */
-  APP_FOPS.pIf_DataRx(USB_Rx_Buffer, USB_Rx_Cnt);
+  if (APP_FOPS.pIf_DataRx(USB_Rx_Buffer, USB_Rx_Cnt) != USBD_OK)
+  {
+      return USBD_OK;
+  }
 
   /* Prepare Out endpoint to receive next packet */
   DCD_EP_PrepareRx(pdev,
@@ -685,6 +688,14 @@ uint8_t  usbd_cdc_DataOut (void *pdev, uint8_t epnum)
   return USBD_OK;
 }
 
+void USBD_CDC_ReceivePacket(void *pdev)
+{
+    DCD_EP_PrepareRx(pdev,
+                     CDC_OUT_EP,
+                     (uint8_t*)(USB_Rx_Buffer),
+                     CDC_DATA_OUT_PACKET_SIZE);
+}
+
 /**
   * @brief  usbd_audio_SOF
   *         Start Of Frame event management
@@ -696,7 +707,7 @@ uint8_t  usbd_cdc_SOF (void *pdev)
 {      
   static uint32_t FrameCount = 0;
 
-  if (FrameCount++ == CDC_IN_FRAME_INTERVAL)
+  if (FrameCount++ >= CDC_IN_FRAME_INTERVAL)
   {
     /* Reset the frame counter */
     FrameCount = 0;

src/main/drivers/serial.c

@@ -71,6 +71,15 @@ uint8_t serialRead(serialPort_t *instance)
     return instance->vTable->serialRead(instance);
 }
 
+uint32_t serialReadBuf(serialPort_t *instance, uint8_t *data, uint32_t maxLen)
+{
+    uint32_t count = 0;
+    while (count < maxLen && serialRxBytesWaiting(instance)) {
+        data[count++] = instance->vTable->serialRead(instance);
+    }
+    return count;
+}
+
 void serialSetBaudRate(serialPort_t *instance, uint32_t baudRate)
 {
     instance->vTable->serialSetBaudRate(instance, baudRate);

src/main/drivers/serial.h

@@ -113,6 +113,7 @@ uint32_t serialRxBytesWaiting(const serialPort_t *instance);
 uint32_t serialTxBytesFree(const serialPort_t *instance);
 void serialWriteBuf(serialPort_t *instance, const uint8_t *data, int count);
 uint8_t serialRead(serialPort_t *instance);
+uint32_t serialReadBuf(serialPort_t *instance, uint8_t *data, uint32_t maxLen);
 void serialSetBaudRate(serialPort_t *instance, uint32_t baudRate);
 void serialSetMode(serialPort_t *instance, portMode_t mode);
 void serialSetOptions(serialPort_t *instance, portOptions_t options);

src/main/drivers/serial_usb_vcp.c

@@ -256,4 +256,21 @@ uint32_t usbVcpGetBaudRate(serialPort_t *instance)
     return CDC_BaudRate();
 }
 
+portOptions_t usbVcpGetLineCoding(void)
+{
+    portOptions_t options = SERIAL_NOT_INVERTED;
+
+    // stop bits: CDC format 0=1 stop, 2=2 stop (1.5 not supported)
+    if (CDC_StopBits() == 2) {
+        options |= SERIAL_STOPBITS_2;
+    }
+
+    // parity: CDC 0=none, 2=even (odd parity not supported in INAV)
+    if (CDC_Parity() == 2) {
+        options |= SERIAL_PARITY_EVEN;
+    }
+
+    return options;
+}
+
 #endif

src/main/drivers/serial_usb_vcp.h

@@ -31,3 +31,4 @@ void usbVcpInitHardware(void);
 serialPort_t *usbVcpOpen(void);
 struct serialPort_s;
 uint32_t usbVcpGetBaudRate(struct serialPort_s *instance);
+portOptions_t usbVcpGetLineCoding(void);

src/main/drivers/serial_usb_vcp_at32f43x.c

@@ -495,7 +495,25 @@ uint32_t usbVcpGetBaudRate(serialPort_t *instance)
     UNUSED(instance);
     cdc_struct_type *pcdc = (cdc_struct_type *)otg_core_struct.dev.class_handler->pdata;
     return pcdc->linecoding.bitrate;
-   // return CDC_BaudRate();
+    // return CDC_BaudRate();
+}
+
+portOptions_t usbVcpGetLineCoding(void)
+{
+    portOptions_t options = 0;
+    cdc_struct_type *pcdc = (cdc_struct_type *)otg_core_struct.dev.class_handler->pdata;
+
+    // stop bits: CDC format 0=1 stop, 2=2 stop (1.5 not supported)
+    if (pcdc->linecoding.format == 2) {
+        options |= SERIAL_STOPBITS_2;
+    }
+
+    // parity: CDC 0=none, 2=even (odd parity not supported in INAV)
+    if (pcdc->linecoding.parity == 2) {
+        options |= SERIAL_PARITY_EVEN;
+    }
+
+    return options;
 }
 
 #endif

src/main/io/serial.c

@@ -509,6 +509,85 @@ void waitForSerialPortToFinishTransmitting(serialPort_t *serialPort)
     };
 }
 
+void escapeSequenceInit(escapeSequenceState_t *state)
+{
+    state->lastCharTime = 0;
+    state->lastPlusTime = 0;
+    state->count = 0;
+}
+
+void escapeSequenceProcessChar(escapeSequenceState_t *state, uint8_t c, uint32_t now)
+{
+    if (c == '+') {
+        if (state->count == 0) {
+            // First '+': check for leading guard interval
+            if (now - state->lastCharTime >= 1000) {
+                state->count = 1;
+                state->lastPlusTime = now;
+            }
+        } else if (state->count < 3) {
+            // Subsequent '+': must arrive within 1 second of previous '+'
+            if (now - state->lastPlusTime < 1000) {
+                state->count++;
+                state->lastPlusTime = now;
+            } else {
+                state->count = 0; // too much time between pluses
+            }
+        } else {
+            // More than 3 pluses - not a valid escape sequence
+            state->count = 0;
+        }
+    } else {
+        // Non-'+' character resets sequence
+        state->count = 0;
+    }
+    state->lastCharTime = now;
+}
+
+bool escapeSequenceCheckGuard(escapeSequenceState_t *state, uint32_t now)
+{
+    if (state->count == 3) {
+        if (now - state->lastPlusTime >= 1000) {
+            return true;
+        }
+    }
+    return false;
+}
+
+static bool serialPassthroughTransfer(serialPort_t *src, serialPort_t *dst, serialConsumer *consumer, escapeSequenceState_t *escapeState, uint32_t now)
+{
+    uint8_t buf[64];
+    uint32_t available = serialRxBytesWaiting(src);
+    uint32_t free = serialTxBytesFree(dst);
+    uint32_t count = (available < free) ? available : free;
+    if (count > sizeof(buf)) {
+        count = sizeof(buf);
+    }
+
+    if (count > 0) {
+        LED0_ON;
+        serialBeginWrite(dst);
+        serialReadBuf(src, buf, count);
+        serialWriteBuf(dst, buf, count);
+        for (uint32_t i = 0; i < count; i++) {
+            consumer(buf[i]);
+            // Hayes escape sequence detection: [1s silence]+++[1s silence]
+            // https://en.wikipedia.org/wiki/Escape_sequence#Modem_control
+            if (escapeState) {
+                escapeSequenceProcessChar(escapeState, buf[i], now);
+            }
+        }
+        serialEndWrite(dst);
+        LED0_OFF;
+    } else {
+        if (escapeState) {
+            return escapeSequenceCheckGuard(escapeState, now);
+        }
+    }
+
+    return false;
+}
+
 #if defined(USE_GPS) || defined(USE_SERIAL_PASSTHROUGH)
 // Default data consumer for serialPassThrough.
 static void nopConsumer(uint8_t data)
@@ -535,32 +614,54 @@ void serialPassthrough(serialPort_t *left, serialPort_t *right, serialConsumer
     LED0_OFF;
     LED1_OFF;
 
+#ifdef USE_VCP
+    // Track current encoding applied to right port for VCP mirroring
+    portOptions_t currentOptions = right->options;
+    uint32_t currentBaudRate = right->baudRate;
+    bool leftIsVcp = (left->identifier == SERIAL_PORT_USB_VCP);
+    uint32_t lastMirrorTime = 0;
+#endif
+
+    static escapeSequenceState_t escapeSequenceState;
+    escapeSequenceInit(&escapeSequenceState);
+
     // Either port might be open in a mode other than MODE_RXTX. We rely on
     // serialRxBytesWaiting() to do the right thing for a TX only port. No
     // special handling is necessary OR performed.
     while (1) {
-        // TODO: maintain a timestamp of last data received. Use this to
-        // implement a guard interval and check for `+++` as an escape sequence
-        // to return to CLI command mode.
-        // https://en.wikipedia.org/wiki/Escape_sequence#Modem_control
-        if (serialRxBytesWaiting(left)) {
-            LED0_ON;
-            uint8_t c = serialRead(left);
-            // Make sure there is space in the tx buffer
-            while (!serialTxBytesFree(right));
-            serialWrite(right, c);
-            leftC(c);
-            LED0_OFF;
-         }
-         if (serialRxBytesWaiting(right)) {
-             LED0_ON;
-             uint8_t c = serialRead(right);
-             // Make sure there is space in the tx buffer
-             while (!serialTxBytesFree(left));
-             serialWrite(left, c);
-             rightC(c);
-             LED0_OFF;
-         }
+        uint32_t now = millis();
+#ifdef USE_VCP
+        // Mirror line coding from host PC to passthrough port (rate-limited to 15ms)
+        if (leftIsVcp) {
+            if (now - lastMirrorTime >= 15) {
+                lastMirrorTime = now;
+                uint32_t hostBaudRate = usbVcpGetBaudRate(left);
+                portOptions_t hostOptions = usbVcpGetLineCoding();
+
+                // apply baud rate change
+                if (hostBaudRate != currentBaudRate && hostBaudRate != 0) {
+                    serialSetBaudRate(right, hostBaudRate);
+                    currentBaudRate = hostBaudRate;
+                }
+
+                // apply encoding change (parity/stop bits)
+                if (hostOptions != currentOptions) {
+                    serialSetOptions(right, hostOptions);
+                    currentOptions = hostOptions;
+                }
+            }
+        }
+#endif
+
+        // Left (USB) to right (UART)
+        if (serialPassthroughTransfer(left, right, leftC, &escapeSequenceState, now)) {
+            return;
+        }
+
+        // Right (UART) to left (USB)
+        serialPassthroughTransfer(right, left, rightC, NULL, now);
      }
  }
  #endif
+
+

src/main/io/serial.h

@@ -139,6 +139,18 @@ PG_DECLARE(serialConfig_t, serialConfig);
 
 typedef void serialConsumer(uint8_t);
 
+// Hayes escape sequence detection state: [1s silence]+++[1s silence]
+// https://en.wikipedia.org/wiki/Escape_sequence#Modem_control
+typedef struct escapeSequenceState_s {
+    uint32_t lastCharTime;
+    uint32_t lastPlusTime;
+    uint8_t count;
+} escapeSequenceState_t;
+
+void escapeSequenceInit(escapeSequenceState_t *state);
+void escapeSequenceProcessChar(escapeSequenceState_t *state, uint8_t c, uint32_t now);
+bool escapeSequenceCheckGuard(escapeSequenceState_t *state, uint32_t now);
+
 //
 // configuration
 //