diff --git a/src/main/io/rcdevice_cam.c b/src/main/io/rcdevice_cam.c
index 74759d4e21..4e02610c6c 100644
--- a/src/main/io/rcdevice_cam.c
+++ b/src/main/io/rcdevice_cam.c
@@ -72,7 +72,7 @@ static bool rcdeviceIsCameraControlEnabled(void)
 
 bool rcdeviceIsEnabled(void)
 {
-    return findSerialPortConfig(FUNCTION_RCDEVICE) != NULL;
+    return camDevice->serialPort != NULL;
 }
 
 static bool rcdeviceIs5KeyEnabled(void)
diff --git a/src/main/io/serial.c b/src/main/io/serial.c
index 1b3d81af6c..726548ed7d 100644
--- a/src/main/io/serial.c
+++ b/src/main/io/serial.c
@@ -221,10 +221,6 @@ serialPortConfig_t *findNextSerialPortConfig(serialPortFunction_e function)
     return NULL;
 }
 
-typedef struct findSharedSerialPortState_s {
-    uint8_t lastIndex;
-} findSharedSerialPortState_t;
-
 portSharing_e determinePortSharing(const serialPortConfig_t *portConfig, serialPortFunction_e function)
 {
     if (!portConfig || (portConfig->functionMask & function) == 0) {
@@ -238,19 +234,10 @@ bool isSerialPortShared(const serialPortConfig_t *portConfig, uint16_t functionM
     return (portConfig) && (portConfig->functionMask & sharedWithFunction) && (portConfig->functionMask & functionMask);
 }
 
-static findSharedSerialPortState_t findSharedSerialPortState;
-
 serialPort_t *findSharedSerialPort(uint16_t functionMask, serialPortFunction_e sharedWithFunction)
 {
-    memset(&findSharedSerialPortState, 0, sizeof(findSharedSerialPortState));
-
-    return findNextSharedSerialPort(functionMask, sharedWithFunction);
-}
-
-serialPort_t *findNextSharedSerialPort(uint16_t functionMask, serialPortFunction_e sharedWithFunction)
-{
-    while (findSharedSerialPortState.lastIndex < SERIAL_PORT_COUNT) {
-        const serialPortConfig_t *candidate = &serialConfig()->portConfigs[findSharedSerialPortState.lastIndex++];
+    for (unsigned i = 0; i < SERIAL_PORT_COUNT; i++) {
+        const serialPortConfig_t *candidate = &serialConfig()->portConfigs[i];
 
         if (isSerialPortShared(candidate, functionMask, sharedWithFunction)) {
             const serialPortUsage_t *serialPortUsage = findSerialPortUsageByIdentifier(candidate->identifier);
diff --git a/src/main/io/serial.h b/src/main/io/serial.h
index 8aaabdc71d..eabd033171 100644
--- a/src/main/io/serial.h
+++ b/src/main/io/serial.h
@@ -107,7 +107,6 @@ typedef struct serialPortUsage_s {
 } serialPortUsage_t;
 
 serialPort_t *findSharedSerialPort(uint16_t functionMask, serialPortFunction_e sharedWithFunction);
-serialPort_t *findNextSharedSerialPort(uint16_t functionMask, serialPortFunction_e sharedWithFunction);
 
 //
 // configuration
diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c
index bab7005f48..598245f447 100644
--- a/src/main/msp/msp_serial.c
+++ b/src/main/msp/msp_serial.c
@@ -41,11 +41,12 @@
 
 static mspPort_t mspPorts[MAX_MSP_PORT_COUNT];
 
-static void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort)
+static void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort, bool sharedWithTelemetry)
 {
     memset(mspPortToReset, 0, sizeof(mspPort_t));
 
     mspPortToReset->port = serialPort;
+    mspPortToReset->sharedWithTelemetry = sharedWithTelemetry;
 }
 
 void mspSerialAllocatePorts(void)
@@ -61,7 +62,8 @@ void mspSerialAllocatePorts(void)
 
         serialPort_t *serialPort = openSerialPort(portConfig->identifier, FUNCTION_MSP, NULL, NULL, baudRates[portConfig->msp_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED);
         if (serialPort) {
-            resetMspPort(mspPort, serialPort);
+            bool sharedWithTelemetry = isSerialPortShared(portConfig, FUNCTION_MSP, TELEMETRY_PORT_FUNCTIONS_MASK);
+            resetMspPort(mspPort, serialPort, sharedWithTelemetry);
             portIndex++;
         }
 
@@ -82,10 +84,12 @@ 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);
+    for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
+        mspPort_t *candidateMspPort = &mspPorts[portIndex];
+        if (candidateMspPort->sharedWithTelemetry) {
+            closeSerialPort(candidateMspPort->port);
+            memset(candidateMspPort, 0, sizeof(mspPort_t));
+        }
     }
 }
 #endif
diff --git a/src/main/msp/msp_serial.h b/src/main/msp/msp_serial.h
index 1ae0aa7138..276a49b165 100644
--- a/src/main/msp/msp_serial.h
+++ b/src/main/msp/msp_serial.h
@@ -96,6 +96,7 @@ typedef struct __attribute__((packed)) {
 struct serialPort_s;
 typedef struct mspPort_s {
     struct serialPort_s *port; // null when port unused.
+    bool sharedWithTelemetry;
     timeMs_t lastActivityMs;
     mspPendingSystemRequest_e pendingRequest;
     mspState_e c_state;