From 0cd8c625680b87c502c2994a8b03ac5fed4b4b99 Mon Sep 17 00:00:00 2001
From: mikeller <github@ike.ch>
Date: Mon, 7 May 2018 23:33:23 +1200
Subject: [PATCH] Cleaned up implementation of MSP port releasing when shared.

---
 src/main/fc/fc_core.c          |  2 +-
 src/main/io/serial.h           |  3 +++
 src/main/msp/msp_serial.c      | 10 ++++++++++
 src/main/msp/msp_serial.h      |  1 +
 src/main/telemetry/telemetry.c |  8 --------
 src/main/telemetry/telemetry.h |  5 -----
 6 files changed, 15 insertions(+), 14 deletions(-)

diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
index 9324ca5d4..e68d03f76 100644
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@@ -778,7 +778,7 @@ bool processRx(timeUs_t currentTimeUs)
         if ((!telemetryConfig()->telemetry_switch && ARMING_FLAG(ARMED)) ||
                 (telemetryConfig()->telemetry_switch && IS_RC_MODE_ACTIVE(BOXTELEMETRY))) {
 
-            releaseSharedTelemetryPorts();
+            mspSerialReleaseSharedTelemetryPorts();
         } else {
             // the telemetry state must be checked immediately so that shared serial ports are released.
             telemetryCheckState();
diff --git a/src/main/io/serial.h b/src/main/io/serial.h
index a87bae74c..8aaabdc71 100644
--- a/src/main/io/serial.h
+++ b/src/main/io/serial.h
@@ -51,6 +51,9 @@ typedef enum {
     FUNCTION_LIDAR_TF            = (1 << 15), // 32768
 } serialPortFunction_e;
 
+#define TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY_HUB | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK)
+#define TELEMETRY_PORT_FUNCTIONS_MASK (TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT)
+
 typedef enum {
     BAUD_AUTO = 0,
     BAUD_9600,
diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c
index 76e9d4aa6..bab7005f4 100644
--- a/src/main/msp/msp_serial.c
+++ b/src/main/msp/msp_serial.c
@@ -80,6 +80,16 @@ void mspSerialReleasePortIfAllocated(serialPort_t *serialPort)
     }
 }
 
+#if defined(USE_TELEMETRY)
+void mspSerialReleaseSharedTelemetryPorts(void) {
+    serialPort_t *sharedPort = findSharedSerialPort(TELEMETRY_PORT_FUNCTIONS_MASK, FUNCTION_MSP);
+    while (sharedPort) {
+        mspSerialReleasePortIfAllocated(sharedPort);
+        sharedPort = findNextSharedSerialPort(TELEMETRY_PORT_FUNCTIONS_MASK, FUNCTION_MSP);
+    }
+}
+#endif
+
 static bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c)
 {
     switch (mspPort->c_state) {
diff --git a/src/main/msp/msp_serial.h b/src/main/msp/msp_serial.h
index a30618ab4..1ae0aa713 100644
--- a/src/main/msp/msp_serial.h
+++ b/src/main/msp/msp_serial.h
@@ -115,5 +115,6 @@ bool mspSerialWaiting(void);
 void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn, mspProcessReplyFnPtr mspProcessReplyFn);
 void mspSerialAllocatePorts(void);
 void mspSerialReleasePortIfAllocated(struct serialPort_s *serialPort);
+void mspSerialReleaseSharedTelemetryPorts(void);
 int mspSerialPush(uint8_t cmd, uint8_t *data, int datalen, mspDirection_e direction);
 uint32_t mspSerialTxBytesFree(void);
diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c
index 5d6fed925..d139f5353 100644
--- a/src/main/telemetry/telemetry.c
+++ b/src/main/telemetry/telemetry.c
@@ -221,12 +221,4 @@ void telemetryProcess(uint32_t currentTime)
     handleIbusTelemetry();
 #endif
 }
-
-void releaseSharedTelemetryPorts(void) {
-    serialPort_t *sharedPort = findSharedSerialPort(TELEMETRY_PORT_FUNCTIONS_MASK, FUNCTION_MSP);
-    while (sharedPort) {
-        mspSerialReleasePortIfAllocated(sharedPort);
-        sharedPort = findNextSharedSerialPort(TELEMETRY_PORT_FUNCTIONS_MASK, FUNCTION_MSP);
-    }
-}
 #endif
diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h
index ec01ff814..83a5f6508 100644
--- a/src/main/telemetry/telemetry.h
+++ b/src/main/telemetry/telemetry.h
@@ -58,9 +58,6 @@ typedef struct telemetryConfig_s {
 
 PG_DECLARE(telemetryConfig_t, telemetryConfig);
 
-#define TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY_HUB | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK)
-#define TELEMETRY_PORT_FUNCTIONS_MASK (TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT)
-
 extern serialPort_t *telemetrySharedPort;
 
 void telemetryInit(void);
@@ -70,5 +67,3 @@ void telemetryCheckState(void);
 void telemetryProcess(uint32_t currentTime);
 
 bool telemetryDetermineEnabledState(portSharing_e portSharing);
-
-void releaseSharedTelemetryPorts(void);
-- 
2.11.4.GIT