Merge pull request #197 from chocol4te/betaflight

Ben Timby's Serial Passthough feature

src/main/io/gps.c

@@ -1058,6 +1058,18 @@ static bool gpsNewFrameUBLOX(uint8_t data)
     return parsed;
 }
 
+static void gpsHandlePassthrough(uint8_t data)
+ {
+     gpsNewData(data);
+ #ifdef DISPLAY
+     if (feature(FEATURE_DISPLAY)) {
+         updateDisplay();
+     }
+ #endif
+ 	
+ }
+ 
+
 void gpsEnablePassthrough(serialPort_t *gpsPassthroughPort)
 {
     waitForSerialPortToFinishTransmitting(gpsPort);
@@ -1066,34 +1078,13 @@ void gpsEnablePassthrough(serialPort_t *gpsPassthroughPort)
     if(!(gpsPort->mode & MODE_TX))
         serialSetMode(gpsPort, gpsPort->mode | MODE_TX);
 
-    LED0_OFF;
-    LED1_OFF;
-
 #ifdef DISPLAY
     if (feature(FEATURE_DISPLAY)) {
         displayShowFixedPage(PAGE_GPS);
     }
 #endif
-    char c;
+    
-    while(1) {
+    serialPassthrough(gpsPort, gpsPassthroughPort, &gpsHandlePassthrough, NULL);
-        if (serialRxBytesWaiting(gpsPort)) {
-            LED0_ON;
-            c = serialRead(gpsPort);
-            gpsNewData(c);
-            serialWrite(gpsPassthroughPort, c);
-            LED0_OFF;
-        }
-        if (serialRxBytesWaiting(gpsPassthroughPort)) {
-            LED1_ON;
-            serialWrite(gpsPort, serialRead(gpsPassthroughPort));
-            LED1_OFF;
-        }
-#ifdef DISPLAY
-        if (feature(FEATURE_DISPLAY)) {
-            updateDisplay();
-        }
-#endif
-    }
 }
 
 void updateGpsIndicator(uint32_t currentTime)

src/main/io/serial.c

@@ -36,6 +36,9 @@
 #include "drivers/serial_uart.h"
 #endif
 
+#include "drivers/gpio.h"
+#include "drivers/light_led.h"
+ 
 #if defined(USE_VCP)
 #include "drivers/serial_usb_vcp.h"
 #endif
@@ -92,7 +95,7 @@ baudRate_e lookupBaudRateIndex(uint32_t baudRate)
     return BAUD_AUTO;
 }
 
-static serialPortUsage_t *findSerialPortUsageByIdentifier(serialPortIdentifier_e identifier)
+serialPortUsage_t *findSerialPortUsageByIdentifier(serialPortIdentifier_e identifier)
 {
     uint8_t index;
     for (index = 0; index < SERIAL_PORT_COUNT; index++) {
@@ -421,3 +424,55 @@ void evaluateOtherData(serialPort_t *serialPort, uint8_t receivedChar)
         systemResetToBootloader();
     }
 }
+
+#if defined(GPS) || ! defined(SKIP_SERIAL_PASSTHROUGH)
+// Default data consumer for serialPassThrough.
+static void nopConsumer(uint8_t data)
+{
+    UNUSED(data);
+}
+
+/*
+ A high-level serial passthrough implementation. Used by cli to start an
+ arbitrary serial passthrough "proxy". Optional callbacks can be given to allow
+ for specialized data processing.
+ */
+void serialPassthrough(serialPort_t *left, serialPort_t *right, serialConsumer 
+                       *leftC, serialConsumer *rightC)
+{
+    waitForSerialPortToFinishTransmitting(left);
+    waitForSerialPortToFinishTransmitting(right);
+
+    if (!leftC)
+        leftC = &nopConsumer;
+    if (!rightC)
+        rightC = &nopConsumer;
+
+    LED0_OFF;
+    LED1_OFF;
+
+    // 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);
+            serialWrite(right, c);
+            leftC(c);
+            LED0_OFF;
+         }
+         if (serialRxBytesWaiting(right)) {
+             LED0_ON;
+             uint8_t c = serialRead(right);
+             serialWrite(left, c);
+             rightC(c);
+             LED0_OFF;
+         }
+     }
+ }
+ #endif

src/main/io/serial.h

@@ -33,6 +33,7 @@ typedef enum {
     FUNCTION_TELEMETRY_SMARTPORT = (1 << 5), // 32
     FUNCTION_RX_SERIAL           = (1 << 6), // 64
     FUNCTION_BLACKBOX            = (1 << 7), // 128
+    FUNCTION_PASSTHROUGH         = (1 << 8), // 256
 } serialPortFunction_e;
 
 typedef enum {
@@ -92,6 +93,7 @@ typedef struct serialConfig_s {
     serialPortConfig_t portConfigs[SERIAL_PORT_COUNT];
 } serialConfig_t;
 
+typedef void serialConsumer(uint8_t);
 
 //
 // configuration
@@ -108,7 +110,7 @@ serialPortConfig_t *findNextSerialPortConfig(serialPortFunction_e function);
 portSharing_e determinePortSharing(serialPortConfig_t *portConfig, serialPortFunction_e function);
 bool isSerialPortShared(serialPortConfig_t *portConfig, uint16_t functionMask, serialPortFunction_e sharedWithFunction);
 
-
+serialPortUsage_t *findSerialPortUsageByIdentifier(serialPortIdentifier_e identifier);
 
 //
 // runtime
@@ -133,3 +135,7 @@ baudRate_e lookupBaudRateIndex(uint32_t baudRate);
 //
 void evaluateOtherData(serialPort_t *serialPort, uint8_t receivedChar);
 void handleSerial(void);
+
+void evaluateOtherData(serialPort_t *serialPort, uint8_t receivedChar);
+void handleSerial(void);
+void serialPassthrough(serialPort_t *left, serialPort_t *right, serialConsumer *leftC, serialConsumer *rightC);

src/main/io/serial_cli.c

@@ -117,6 +117,9 @@ static void cliRateProfile(char *cmdline);
 static void cliReboot(void);
 static void cliSave(char *cmdline);
 static void cliSerial(char *cmdline);
+#ifndef SKIP_SERIAL_PASSTHROUGH
+static void cliSerialPassthrough(char *cmdline);
+#endif
 
 #ifdef USE_SERVOS
 static void cliServo(char *cmdline);
@@ -285,6 +288,11 @@ const clicmd_t cmdTable[] = {
     CLI_COMMAND_DEF("rxfail", "show/set rx failsafe settings", NULL, cliRxFail),
     CLI_COMMAND_DEF("save", "save and reboot", NULL, cliSave),
     CLI_COMMAND_DEF("serial", "configure serial ports", NULL, cliSerial),
+#ifndef SKIP_SERIAL_PASSTHROUGH
+    CLI_COMMAND_DEF("serialpassthrough", "passthrough serial data to port",
+                    "<id> [baud] [mode] : passthrough to serial",
+                    cliSerialPassthrough),
+#endif
 #ifdef USE_SERVOS
     CLI_COMMAND_DEF("servo", "configure servos", NULL, cliServo),
 #endif
@@ -1029,6 +1037,76 @@ static void cliSerial(char *cmdline)
 
 }
 
+#ifndef SKIP_SERIAL_PASSTHROUGH
+static void cliSerialPassthrough(char *cmdline)
+{
+    if (isEmpty(cmdline)) {
+        cliShowParseError();
+        return;
+    }
+
+    int id = -1;
+    uint32_t baud = 0;
+    unsigned mode = 0;
+    char* tok = strtok(cmdline, " ");
+    int index = 0;
+
+    while (tok != NULL) {
+        switch(index) {
+            case 0:
+                id = atoi(tok);
+                break;
+            case 1:
+                baud = atoi(tok);
+                break;
+            case 2:
+                if (strstr(tok, "rx") || strstr(tok, "RX"))
+                    mode |= MODE_RX;
+                if (strstr(tok, "tx") || strstr(tok, "TX"))
+                    mode |= MODE_TX;
+                break;
+        }
+        index++;
+        tok = strtok(NULL, " ");
+    }
+
+    serialPort_t *passThroughPort;
+    serialPortUsage_t *passThroughPortUsage = findSerialPortUsageByIdentifier(id);
+    if (!passThroughPortUsage || passThroughPortUsage->serialPort == NULL) {
+        if (!baud) {
+            printf("Port %d is not open, you must specify baud\r\n", id);
+            return;
+        }
+        if (!mode)
+            mode = MODE_RXTX;
+
+        passThroughPort = openSerialPort(id, FUNCTION_PASSTHROUGH, NULL,
+                                         baud, mode,
+                                         SERIAL_NOT_INVERTED);
+        if (!passThroughPort) {
+            printf("Port %d could not be opened\r\n", id);
+            return;
+        }
+        printf("Port %d opened, baud=%d\r\n", id, baud);
+    } else {
+        passThroughPort = passThroughPortUsage->serialPort;
+        // If the user supplied a mode, override the port's mode, otherwise
+        // leave the mode unchanged. serialPassthrough() handles one-way ports.
+        printf("Port %d already open\r\n", id);
+        if (mode && passThroughPort->mode != mode) {
+            printf("Adjusting mode from configured value %d to %d\r\n",
+                   passThroughPort->mode, mode);
+            serialSetMode(passThroughPort, mode);
+        }
+    }
+
+    printf("Relaying data to device on port %d, Reset your board to exit "
+           "serial passthrough mode.\r\n");
+
+    serialPassthrough(cliPort, passThroughPort, NULL, NULL);
+}
+#endif
+
 static void cliAdjustmentRange(char *cmdline)
 {
     int i, val = 0;