From 6a8dfa75c2d3f2b6c68dba7d40e549e177bbbff5 Mon Sep 17 00:00:00 2001
From: jflyper <shinoda@jaist.ac.jp>
Date: Fri, 18 Oct 2019 12:24:07 +0900
Subject: [PATCH] Restrict MSP DisplayPort output to the MSP port that is
 marked

---
 src/main/cli/settings.c    |  2 ++
 src/main/drivers/display.h |  1 +
 src/main/io/serial.h       |  3 ++-
 src/main/msp/msp_serial.c  | 15 ++++++++++-----
 src/main/msp/msp_serial.h  |  1 +
 5 files changed, 16 insertions(+), 6 deletions(-)

diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c
index f820e60e96..c6152fd569 100644
--- a/src/main/cli/settings.c
+++ b/src/main/cli/settings.c
@@ -63,6 +63,7 @@
 #include "io/gimbal.h"
 #include "io/gps.h"
 #include "io/ledstrip.h"
+#include "io/serial.h"
 #include "io/vtx.h"
 #include "io/vtx_control.h"
 #include "io/vtx_rtc6705.h"
@@ -1420,6 +1421,7 @@ const clivalue_t valueTable[] = {
 #ifdef USE_MSP_DISPLAYPORT
     { "displayport_msp_col_adjust", VAR_INT8    | MASTER_VALUE, .config.minmax = { -6, 0 }, PG_DISPLAY_PORT_MSP_CONFIG, offsetof(displayPortProfile_t, colAdjust) },
     { "displayport_msp_row_adjust", VAR_INT8    | MASTER_VALUE, .config.minmax = { -3, 0 }, PG_DISPLAY_PORT_MSP_CONFIG, offsetof(displayPortProfile_t, rowAdjust) },
+    { "displayport_msp_serial",     VAR_INT8    | MASTER_VALUE, .config.minmax = { SERIAL_PORT_NONE, SERIAL_PORT_IDENTIFIER_MAX }, PG_DISPLAY_PORT_MSP_CONFIG, offsetof(displayPortProfile_t, displayPortSerial) },
 #endif
 
 // PG_DISPLAY_PORT_MSP_CONFIG
diff --git a/src/main/drivers/display.h b/src/main/drivers/display.h
index 79d428fe3b..86812578a5 100644
--- a/src/main/drivers/display.h
+++ b/src/main/drivers/display.h
@@ -60,6 +60,7 @@ typedef struct displayPortProfile_s {
     bool invert;
     uint8_t blackBrightness;
     uint8_t whiteBrightness;
+    int8_t displayPortSerial;  // serialPortIdentifier_e
 } displayPortProfile_t;
 
 // Note: displayPortProfile_t used as a parameter group for CMS over CRSF (io/displayport_crsf)
diff --git a/src/main/io/serial.h b/src/main/io/serial.h
index eabd033171..dff90a1970 100644
--- a/src/main/io/serial.h
+++ b/src/main/io/serial.h
@@ -88,7 +88,8 @@ typedef enum {
     SERIAL_PORT_USART8,
     SERIAL_PORT_USB_VCP = 20,
     SERIAL_PORT_SOFTSERIAL1 = 30,
-    SERIAL_PORT_SOFTSERIAL2
+    SERIAL_PORT_SOFTSERIAL2,
+    SERIAL_PORT_IDENTIFIER_MAX = SERIAL_PORT_SOFTSERIAL2
 } serialPortIdentifier_e;
 
 extern const serialPortIdentifier_e serialPortIdentifiers[SERIAL_PORT_COUNT];
diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c
index ae38ea9177..5134d1fd57 100644
--- a/src/main/msp/msp_serial.c
+++ b/src/main/msp/msp_serial.c
@@ -35,6 +35,7 @@
 #include "drivers/system.h"
 
 #include "io/serial.h"
+#include "io/displayport_msp.h"
 
 #include "msp/msp.h"
 
@@ -57,6 +58,7 @@ void mspSerialAllocatePorts(void)
     serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_MSP);
     while (portConfig && portIndex < MAX_MSP_PORT_COUNT) {
         mspPort_t *mspPort = &mspPorts[portIndex];
+
         if (mspPort->port) {
             portIndex++;
             continue;
@@ -66,6 +68,13 @@ void mspSerialAllocatePorts(void)
         if (serialPort) {
             bool sharedWithTelemetry = isSerialPortShared(portConfig, FUNCTION_MSP, TELEMETRY_PORT_FUNCTIONS_MASK);
             resetMspPort(mspPort, serialPort, sharedWithTelemetry);
+
+#ifdef USE_MSP_DISPLAYPORT
+            if (serialPort->identifier == displayPortProfileMsp()->displayPortSerial) {
+                mspPort->isDisplayPort = true;
+            }
+#endif
+
             portIndex++;
         }
 
@@ -562,12 +571,8 @@ int mspSerialPush(uint8_t cmd, uint8_t *data, int datalen, mspDirection_e direct
 
     for (int portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
         mspPort_t * const mspPort = &mspPorts[portIndex];
-        if (!mspPort->port) {
-            continue;
-        }
 
-        // XXX Kludge!!! Avoid zombie VCP port (avoid VCP entirely for now)
-        if (mspPort->port->identifier == SERIAL_PORT_USB_VCP) {
+        if (!mspPort->port || !mspPort->isDisplayPort) {
             continue;
         }
 
diff --git a/src/main/msp/msp_serial.h b/src/main/msp/msp_serial.h
index 010f489451..f1d27efbe8 100644
--- a/src/main/msp/msp_serial.h
+++ b/src/main/msp/msp_serial.h
@@ -111,6 +111,7 @@ typedef struct mspPort_s {
     uint8_t checksum2;
     bool sharedWithTelemetry;
     mspDescriptor_t descriptor;
+    bool isDisplayPort;
 } mspPort_t;
 
 void mspSerialInit(void);