Add support for serial passthrough via MSP

- Rename MSP_SET_4WAY_IF to MSP_SET_PASSTHROUGH
- Keep backwards compatibility with existing uses of the command
- Add 2 new values for the passthrough mode: one that sets up
a serial port given its ID and another one that sets it up from
a function ID.

src/main/msp/msp.c

@@ -172,6 +172,19 @@ typedef enum {
     MSP_FLASHFS_FLAG_SUPPORTED  = 2
 } mspFlashFsFlags_e;
 
+typedef enum {
+    MSP_PASSTHROUGH_ESC_SIMONK = PROTOCOL_SIMONK,
+    MSP_PASSTHROUGH_ESC_BLHELI = PROTOCOL_BLHELI,
+    MSP_PASSTHROUGH_ESC_KISS = PROTOCOL_KISS,
+    MSP_PASSTHROUGH_ESC_KISSALL = PROTOCOL_KISSALL,
+    MSP_PASSTHROUGH_ESC_CASTLE = PROTOCOL_CASTLE,
+
+    MSP_PASSTHROUGH_SERIAL_ID = 0xFD,
+    MSP_PASSTHROUGH_SERIAL_FUNCTION_ID = 0xFE,
+
+    MSP_PASSTHROUGH_ESC_4WAY = 0xFF,
+} mspPassthroughType_e;
+
 #define RATEPROFILE_MASK (1 << 7)
 
 #define RTC_NOT_SUPPORTED 0xff
@@ -209,33 +222,73 @@ static bool mspIsMspArmingEnabled(void)
     return mspArmingDisableFlags == 0;
 }
 
-#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
+#define MSP_PASSTHROUGH_ESC_4WAY 0xff
-#define ESC_4WAY 0xff
 
-uint8_t escMode;
+static uint8_t mspPassthroughMode;
-uint8_t escPortIndex;
+static uint8_t mspPassthroughArgument;
 
 #ifdef USE_ESCSERIAL
 static void mspEscPassthroughFn(serialPort_t *serialPort)
 {
-    escEnablePassthrough(serialPort, &motorConfig()->dev, escPortIndex, escMode);
+    escEnablePassthrough(serialPort, &motorConfig()->dev, mspPassthroughArgument, mspPassthroughMode);
 }
 #endif
 
-static void mspFc4waySerialCommand(sbuf_t *dst, sbuf_t *src, mspPostProcessFnPtr *mspPostProcessFn)
+static serialPort_t *mspFindPassthroughSerialPort(void)
+{
+    serialPortUsage_t *portUsage = NULL;
+
+    switch (mspPassthroughMode) {
+    case MSP_PASSTHROUGH_SERIAL_ID:
+    {
+        portUsage = findSerialPortUsageByIdentifier(mspPassthroughArgument);
+        break;
+    }
+    case MSP_PASSTHROUGH_SERIAL_FUNCTION_ID:
+    {
+        const serialPortConfig_t *portConfig = findSerialPortConfig(1 << mspPassthroughArgument);
+        if (portConfig) {
+            portUsage = findSerialPortUsageByIdentifier(portConfig->identifier);
+        }
+        break;
+    }
+    }
+    return portUsage ? portUsage->serialPort : NULL;
+}
+
+static void mspSerialPassthroughFn(serialPort_t *serialPort)
+{
+    serialPort_t *passthroughPort = mspFindPassthroughSerialPort();
+    if (passthroughPort && serialPort) {
+        serialPassthrough(passthroughPort, serialPort, NULL, NULL);
+    }
+}
+
+static void mspFcSetPassthroughCommand(sbuf_t *dst, sbuf_t *src, mspPostProcessFnPtr *mspPostProcessFn)
 {
     const unsigned int dataSize = sbufBytesRemaining(src);
     if (dataSize == 0) {
         // Legacy format
-
+        mspPassthroughMode = MSP_PASSTHROUGH_ESC_4WAY;
-        escMode = ESC_4WAY;
     } else {
-        escMode = sbufReadU8(src);
+        mspPassthroughMode = sbufReadU8(src);
-        escPortIndex = sbufReadU8(src);
+        mspPassthroughArgument = sbufReadU8(src);
     }
 
-    switch (escMode) {
+    switch (mspPassthroughMode) {
-    case ESC_4WAY:
+    case MSP_PASSTHROUGH_SERIAL_ID:
+    case MSP_PASSTHROUGH_SERIAL_FUNCTION_ID:
+        if (mspFindPassthroughSerialPort()) {
+            if (mspPostProcessFn) {
+                *mspPostProcessFn = mspSerialPassthroughFn;
+            }
+            sbufWriteU8(dst, 1);
+        } else {
+            sbufWriteU8(dst, 0);
+        }
+        break;
+#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
+    case MSP_PASSTHROUGH_ESC_4WAY:
         // get channel number
         // switch all motor lines HI
         // reply with the count of ESC found
@@ -247,12 +300,12 @@ static void mspFc4waySerialCommand(sbuf_t *dst, sbuf_t *src, mspPostProcessFnPtr
         break;
 
 #ifdef USE_ESCSERIAL
-    case PROTOCOL_SIMONK:
+    case MSP_PASSTHROUGH_ESC_SIMONK:
-    case PROTOCOL_BLHELI:
+    case MSP_PASSTHROUGH_ESC_BLHELI:
-    case PROTOCOL_KISS:
+    case MSP_PASSTHROUGH_ESC_KISS:
-    case PROTOCOL_KISSALL:
+    case MSP_PASSTHROUGH_ESC_KISSALL:
-    case PROTOCOL_CASTLE:
+    case MSP_PASSTHROUGH_ESC_CASTLE:
-        if (escPortIndex < getMotorCount() || (escMode == PROTOCOL_KISS && escPortIndex == ALL_MOTORS)) {
+        if (mspPassthroughArgument < getMotorCount() || (mspPassthroughMode == MSP_PASSTHROUGH_ESC_KISS && mspPassthroughArgument == ALL_MOTORS)) {
             sbufWriteU8(dst, 1);
 
             if (mspPostProcessFn) {
@@ -262,12 +315,12 @@ static void mspFc4waySerialCommand(sbuf_t *dst, sbuf_t *src, mspPostProcessFnPtr
             break;
         }
         FALLTHROUGH;
-#endif
+#endif // USE_ESCSERIAL
+#endif //USE_SERIAL_4WAY_BLHELI_INTERFACE
     default:
         sbufWriteU8(dst, 0);
     }
 }
-#endif //USE_SERIAL_4WAY_BLHELI_INTERFACE
 
 // TODO: Remove the pragma once this is called from unconditional code
 #pragma GCC diagnostic ignored "-Wunused-function"
@@ -3369,11 +3422,9 @@ mspResult_e mspFcProcessCommand(mspDescriptor_t srcDesc, mspPacket_t *cmd, mspPa
         ret = MSP_RESULT_ACK;
     } else if ((ret = mspFcProcessOutCommandWithArg(srcDesc, cmdMSP, src, dst, mspPostProcessFn)) != MSP_RESULT_CMD_UNKNOWN) {
         /* ret */;
-#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
+    } else if (cmdMSP == MSP_SET_PASSTHROUGH) {
-    } else if (cmdMSP == MSP_SET_4WAY_IF) {
+        mspFcSetPassthroughCommand(dst, src, mspPostProcessFn);
-        mspFc4waySerialCommand(dst, src, mspPostProcessFn);
         ret = MSP_RESULT_ACK;
-#endif
 #ifdef USE_FLASHFS
     } else if (cmdMSP == MSP_DATAFLASH_READ) {
         mspFcDataFlashReadCommand(dst, src);

src/main/msp/msp_protocol.h

@@ -336,7 +336,7 @@
 #define MSP_SET_ACC_TRIM         239    //in message          set acc angle trim values
 #define MSP_SERVO_MIX_RULES      241    //out message         Returns servo mixer configuration
 #define MSP_SET_SERVO_MIX_RULE   242    //in message          Sets servo mixer configuration
-#define MSP_SET_4WAY_IF          245    //in message          Sets 4way interface
+#define MSP_SET_PASSTHROUGH      245    //in message          Sets up passthrough to different peripherals (4way interface, uart, etc...)
 #define MSP_SET_RTC              246    //in message          Sets the RTC clock
 #define MSP_RTC                  247    //out message         Gets the RTC clock
 #define MSP_SET_BOARD_INFO       248    //in message          Sets the board information for this board