diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
index c793fa9ea..5a5880189 100644
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@@ -537,6 +537,7 @@ bool processRx(timeUs_t currentTimeUs)
 {
     static bool armedBeeperOn = false;
     static bool airmodeIsActivated;
+    static bool sharedPortTelemetryEnabled = false;
 
     if (!calculateRxChannelsAndUpdateFailsafe(currentTimeUs)) {
         return false;
@@ -788,14 +789,18 @@ bool processRx(timeUs_t currentTimeUs)
 
 #ifdef USE_TELEMETRY
     if (feature(FEATURE_TELEMETRY)) {
-        if ((!isModeActivationConditionPresent(BOXTELEMETRY) && ARMING_FLAG(ARMED)) ||
-                (isModeActivationConditionPresent(BOXTELEMETRY) && IS_RC_MODE_ACTIVE(BOXTELEMETRY))) {
-
+        bool enableSharedPortTelemetry = (!isModeActivationConditionPresent(BOXTELEMETRY) && ARMING_FLAG(ARMED)) || (isModeActivationConditionPresent(BOXTELEMETRY) && IS_RC_MODE_ACTIVE(BOXTELEMETRY));
+        if (enableSharedPortTelemetry && !sharedPortTelemetryEnabled) {
             mspSerialReleaseSharedTelemetryPorts();
-        } else {
+            telemetryCheckState();
+
+            sharedPortTelemetryEnabled = true;
+        } else if (!enableSharedPortTelemetry && sharedPortTelemetryEnabled) {
             // the telemetry state must be checked immediately so that shared serial ports are released.
             telemetryCheckState();
             mspSerialAllocatePorts();
+
+            sharedPortTelemetryEnabled = false;
         }
     }
 #endif
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index 0419fe355..cfd6b5803 100644
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -202,8 +202,6 @@ static void taskCalculateAltitude(timeUs_t currentTimeUs)
 #ifdef USE_TELEMETRY
 static void taskTelemetry(timeUs_t currentTimeUs)
 {
-    telemetryCheckState();
-
     if (!cliMode && feature(FEATURE_TELEMETRY)) {
         telemetryProcess(currentTimeUs);
     }
diff --git a/src/main/io/rcdevice_cam.c b/src/main/io/rcdevice_cam.c
index 74759d4e2..4e02610c6 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 cb77163c1..8aba7aa9d 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 8aaabdc71..eabd03317 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 bab7005f4..598245f44 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 1ae0aa713..37db1782b 100644
--- a/src/main/msp/msp_serial.h
+++ b/src/main/msp/msp_serial.h
@@ -108,6 +108,7 @@ typedef struct mspPort_s {
     uint_fast16_t dataSize;
     uint8_t checksum1;
     uint8_t checksum2;
+    bool sharedWithTelemetry;
 } mspPort_t;
 
 void mspSerialInit(void);