add msp passthrough support

src/io/msp.c

@@ -3,21 +3,24 @@
 #include <stdbool.h>
 #include <string.h>
 
+#include "drv_serial.h"
 #include "drv_serial_4way.h"
 #include "flight/control.h"
+#include "io/usb_configurator.h"
 #include "quic.h"
 #include "util/crc.h"
 #include "util/util.h"
+#include "vtx.h"
 
-static void msp_send_reply(msp_t *msp, msp_magic_t magic, uint8_t code, uint8_t *data, uint32_t len) {
+static void msp_send_reply(msp_t *msp, msp_magic_t magic, uint16_t cmd, uint8_t *data, uint32_t len) {
   if (msp->send) {
-    msp->send(magic, '>', code, data, len);
+    msp->send(magic, '>', cmd, data, len);
   }
 }
 
-static void msp_send_error(msp_t *msp, msp_magic_t magic, uint8_t code) {
+static void msp_send_error(msp_t *msp, msp_magic_t magic, uint16_t cmd) {
   if (msp->send) {
-    msp->send(magic, '!', code, NULL, 0);
+    msp->send(magic, '!', cmd, NULL, 0);
   }
 }
 
@@ -139,19 +142,48 @@ static void msp_process_serial_cmd(msp_t *msp, msp_magic_t magic, uint16_t cmd,
     msp_send_reply(msp, magic, cmd, NULL, 0);
     break;
   }
+  case MSP_SET_PASSTHROUGH: {
+    msp_passthrough_mode_t mode = MSP_PASSTHROUGH_ESC_4WAY;
+    uint8_t arg = 0;
+
+    if (size != 0) {
+      mode = payload[0];
+      arg = payload[1];
+    }
+
+    switch (mode) {
+    case MSP_PASSTHROUGH_SERIAL_ID: {
+      uint8_t data[1] = {1};
+      msp_send_reply(msp, magic, cmd, data, 1);
+
+      if (arg == serial_smart_audio_port) {
+        if (vtx_settings.protocol == VTX_PROTOCOL_TRAMP) {
+          usb_serial_passthrough(arg, 9600, 1, true);
+        } else {
+          usb_serial_passthrough(arg, 4800, 2, true);
+        }
+      }
+
+      break;
+    }
+
+    default:
+    case MSP_PASSTHROUGH_ESC_4WAY: {
 #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
-  case MSP_SET_4WAY_IF: {
-    uint8_t data[1] = {4};
-    msp_send_reply(msp, magic, cmd, data, 1);
+      uint8_t data[1] = {4};
+      msp_send_reply(msp, magic, cmd, data, 1);
 
-    motor_test.active = 0;
+      motor_test.active = 0;
 
-    serial_4way_init();
-    serial_4way_process();
+      serial_4way_init();
+      serial_4way_process();
+#endif
+      break;
+    }
+    }
 
     break;
   }
-#endif
 
   case MSP_RESERVE_1: {
     quic_t quic = {
@@ -162,6 +194,41 @@ static void msp_process_serial_cmd(msp_t *msp, msp_magic_t magic, uint16_t cmd,
     break;
   }
 
+  case MSP2_COMMON_SERIAL_CONFIG: {
+    const uint8_t uart_count = USART_PORTS_MAX - 1;
+    uint8_t data[1 + uart_count * 5];
+
+    data[0] = uart_count;
+
+    for (uint32_t i = 0; i < uart_count; i++) {
+      const usart_ports_t port = i + 1;
+      data[1 + i * 5] = port;
+
+      uint32_t function = 0;
+      if (port == serial_rx_port) {
+        function = MSP_SERIAL_FUNCTION_RX;
+      }
+      if (port == serial_smart_audio_port) {
+        if (vtx_settings.protocol == VTX_PROTOCOL_TRAMP) {
+          function = MSP_SERIAL_FUNCTION_TRAMP;
+        } else {
+          function = MSP_SERIAL_FUNCTION_SA;
+        }
+      }
+      if (port == serial_hdzero_port) {
+        function = MSP_SERIAL_FUNCTION_HDZERO;
+      }
+
+      data[1 + i * 5 + 1] = (function >> 0) & 0xFF;
+      data[1 + i * 5 + 2] = (function >> 8) & 0xFF;
+      data[1 + i * 5 + 3] = (function >> 16) & 0xFF;
+      data[1 + i * 5 + 4] = (function >> 24) & 0xFF;
+    }
+
+    msp_send_reply(msp, magic, cmd, data, 1 + uart_count * 5);
+    break;
+  }
+
   default:
     msp_send_error(msp, magic, cmd);
     break;
@@ -216,7 +283,7 @@ msp_status_t msp_process_serial(msp_t *msp, uint8_t *data, uint32_t len) {
       return MSP_ERROR;
     }
 
-    msp_process_serial_cmd(msp, MSP2_MAGIC, cmd, data + MSP_HEADER_LEN, size);
+    msp_process_serial_cmd(msp, MSP2_MAGIC, cmd, data + MSP2_HEADER_LEN, size);
     return MSP_SUCCESS;
   }
 

src/io/msp.h

@@ -19,11 +19,13 @@
 
 #define MSP_MOTOR_CONFIG 131 // out message         Motor configuration (min/max throttle, etc)
 
-#define MSP_SET_MOTOR 214   // in message          PropBalance function
-#define MSP_SET_4WAY_IF 245 // in message          Sets 4way interface
+#define MSP_SET_MOTOR 214       // in message          PropBalance function
+#define MSP_SET_PASSTHROUGH 245 // in message          serial passthrough
 
 #define MSP_RESERVE_1 251 // reserved for system usage
 
+#define MSP2_COMMON_SERIAL_CONFIG 0x1009
+
 #define MSP_HEADER_LEN 5
 #define MSP_TLM_HEADER_LEN 3
 
@@ -48,6 +50,20 @@ typedef enum {
   MSP_SUCCESS,
 } msp_status_t;
 
+typedef enum {
+  MSP_SERIAL_FUNCTION_RX = (1 << 0),
+  MSP_SERIAL_FUNCTION_SA = (1 << 1),
+  MSP_SERIAL_FUNCTION_TRAMP = (1 << 2),
+  MSP_SERIAL_FUNCTION_HDZERO = (1 << 3),
+} msp_serial_function_t;
+
+typedef enum {
+  MSP_PASSTHROUGH_SERIAL_ID = 0xFD,
+  MSP_PASSTHROUGH_SERIAL_FUNCTION_ID = 0xFE,
+
+  MSP_PASSTHROUGH_ESC_4WAY = 0xFF,
+} msp_passthrough_mode_t;
+
 typedef void (*msp_send_fn_t)(msp_magic_t magic, uint8_t direction, uint16_t code, uint8_t *data, uint16_t len);
 
 typedef struct {

src/io/quic.c

@@ -13,6 +13,7 @@
 #include "flight/control.h"
 #include "flight/sixaxis.h"
 #include "io/data_flash.h"
+#include "io/usb_configurator.h"
 #include "io/vtx.h"
 #include "osd/osd_render.h"
 #include "profile.h"
@@ -514,42 +515,7 @@ static void process_serial(quic_t *quic, cbor_value_t *dec) {
 
     quic_send(quic, QUIC_CMD_SERIAL, QUIC_FLAG_NONE, encode_buffer, cbor_encoder_len(&enc));
 
-    uint8_t tx_data[512];
-    circular_buffer_t tx_buffer = {
-        .buffer = tx_data,
-        .head = 0,
-        .tail = 0,
-        .size = 512,
-    };
-
-    uint8_t rx_data[512];
-    circular_buffer_t rx_buffer = {
-        .buffer = rx_data,
-        .head = 0,
-        .tail = 0,
-        .size = 512,
-    };
-
-    serial_port_t serial = {
-        .rx_buffer = &rx_buffer,
-        .tx_buffer = &tx_buffer,
-    };
-
-    serial_enable_rcc(port);
-    serial_init(&serial, port, baudrate, stop_bits, half_duplex);
-
-    uint8_t data[512];
-    while (1) {
-      {
-        const uint32_t size = usb_serial_read(data, 512);
-        serial_write_bytes(&serial, data, size);
-      }
-      {
-        const uint32_t size = serial_read_bytes(&serial, data, 512);
-        usb_serial_write(data, size);
-      }
-    }
-
+    usb_serial_passthrough(port, baudrate, stop_bits, half_duplex);
     break;
   }
 

src/io/usb_configurator.c

@@ -5,13 +5,15 @@
 #include <string.h>
 
 #include "debug.h"
+#include "drv_serial.h"
 #include "drv_usb.h"
 #include "flight/control.h"
 #include "io/msp.h"
 #include "io/quic.h"
 #include "profile.h"
 #include "project.h"
 #include "reset.h"
+#include "util/circular_buffer.h"
 #include "util/crc.h"
 #include "util/util.h"
 
@@ -78,6 +80,44 @@ void usb_quic_logf(const char *fmt, ...) {
   quic_send_str(&quic, QUIC_CMD_LOG, QUIC_FLAG_NONE, str);
 }
 
+void usb_serial_passthrough(usart_ports_t port, uint32_t baudrate, uint8_t stop_bits, bool half_duplex) {
+  uint8_t tx_data[512];
+  circular_buffer_t tx_buffer = {
+      .buffer = tx_data,
+      .head = 0,
+      .tail = 0,
+      .size = 512,
+  };
+
+  uint8_t rx_data[512];
+  circular_buffer_t rx_buffer = {
+      .buffer = rx_data,
+      .head = 0,
+      .tail = 0,
+      .size = 512,
+  };
+
+  serial_port_t serial = {
+      .rx_buffer = &rx_buffer,
+      .tx_buffer = &tx_buffer,
+  };
+
+  serial_enable_rcc(port);
+  serial_init(&serial, port, baudrate, stop_bits, half_duplex);
+
+  uint8_t data[512];
+  while (1) {
+    {
+      const uint32_t size = usb_serial_read(data, 512);
+      serial_write_bytes(&serial, data, size);
+    }
+    {
+      const uint32_t size = serial_read_bytes(&serial, data, 512);
+      usb_serial_write(data, size);
+    }
+  }
+}
+
 // double promition in the following is intended
 #pragma GCC diagnostic ignored "-Wdouble-promotion"
 // This function will be where all usb send/receive coms live

src/io/usb_configurator.h

@@ -1,6 +1,8 @@
 #pragma once
 
-#include "io/quic.h"
+#include <stdbool.h>
+
+#include "project.h"
 
 #define USB_BUFFER_SIZE (16 * 1024)
 
@@ -11,6 +13,7 @@ typedef enum {
   USB_MAGIC_QUIC = '#',
 } usb_magics;
 
+void usb_serial_passthrough(usart_ports_t port, uint32_t baudrate, uint8_t stop_bits, bool half_duplex);
 void usb_process_msp();
 void usb_process_quic();
 void usb_quic_logf(const char *fmt, ...);

src/io/vtx.c

@@ -9,6 +9,7 @@
 #include "drv_time.h"
 #include "flight/control.h"
 #include "io/usb_configurator.h"
+#include "profile.h"
 #include "project.h"
 #include "rx.h"
 #include "util/cbor_helper.h"