Adding CLI command pass through for MSP (#13940)

src/main/cli/cli.c

@@ -175,6 +175,8 @@ bool cliMode = false;
 #include "cli.h"
 
 static serialPort_t *cliPort = NULL;
+static bool cliInteractive = false;
+static timeMs_t cliEntryTime = 0;
 
 // Space required to set array parameters
 #define CLI_IN_BUFFER_SIZE  256
@@ -315,6 +317,7 @@ typedef enum dumpFlags_e {
     BARE = (1 << 8),
 } dumpFlags_t;
 
+static void cliExit(const bool reboot);
 typedef bool printFn(dumpFlags_t dumpMask, bool equalsDefault, const char *format, ...);
 
 typedef enum {
@@ -330,6 +333,12 @@ typedef struct serialPassthroughPort_s {
     serialPort_t *port;
 } serialPassthroughPort_t;
 
+static void cliClearInputBuffer(void)
+{
+    memset(cliBuffer, 0, sizeof(cliBuffer));
+    bufferIndex = 0;
+}
+
 static void cliWriterFlushInternal(bufWriter_t *writer)
 {
     if (writer) {
@@ -3581,23 +3590,17 @@ static void cliBootloader(const char *cmdName, char *cmdline)
     cliRebootEx(rebootTarget);
 }
 
-static void cliExit(const char *cmdName, char *cmdline)
+static void cliExitCmd(const char *cmdName, char *cmdline)
 {
     UNUSED(cmdName);
-    UNUSED(cmdline);
 
+    const bool reboot = strcasecmp(cmdline, "noreboot") != 0;
+    if (reboot) {
         cliPrintHashLine("leaving CLI mode, unsaved changes lost");
-    cliWriterFlush();
+    } else {
-
+        cliPrintHashLine("leaving CLI mode, no reboot");
-    *cliBuffer = '\0';
-    bufferIndex = 0;
-    cliMode = false;
-    // incase a motor was left running during motortest, clear it here
-    mixerResetDisarmedMotors();
-    if (strcasecmp(cmdline, "noreboot") == 0) {
-        return;
     }
-    cliReboot();
+    cliExit(reboot);
 }
 
 #ifdef USE_GPS
@@ -6516,7 +6519,7 @@ const clicmd_t cmdTable[] = {
 #ifdef USE_ESCSERIAL
     CLI_COMMAND_DEF("escprog", "passthrough esc to serial", "<mode [sk/bl/ki/cc]> <index>", cliEscPassthrough),
 #endif
-    CLI_COMMAND_DEF("exit", "exit command line interface and reboot (default)", "[noreboot]", cliExit),
+    CLI_COMMAND_DEF("exit", "exit command line interface and reboot (default)", "[noreboot]", cliExitCmd),
     CLI_COMMAND_DEF("feature", "configure features",
         "list\r\n"
         "\t<->[name]", cliFeature),
@@ -6662,8 +6665,10 @@ static void cliHelp(const char *cmdName, char *cmdline)
 static void processCharacter(const char c)
 {
     if (bufferIndex && (c == '\n' || c == '\r')) {
-        // enter pressed
+        if (cliInteractive) {
+            // echo new line back to terminal
             cliPrintLinefeed();
+        }
 
         // Strip comment starting with # from line
         char *p = cliBuffer;
@@ -6690,27 +6695,39 @@ static void processCharacter(const char c)
             }
             if (cmd < cmdTable + ARRAYLEN(cmdTable)) {
                 cmd->cliCommand(cmd->name, options);
-            } else {
-                cliPrintError("input", "UNKNOWN COMMAND, TRY 'HELP'");
-            }
-            bufferIndex = 0;
-        }
-
-        memset(cliBuffer, 0, sizeof(cliBuffer));
-
-        // 'exit' will reset this flag, so we don't need to print prompt again
                 if (!cliMode) {
+                    // cli session ended
                     return;
                 }
+            } else {
+                if (cliInteractive) {
+                    cliPrintError("input", "UNKNOWN COMMAND, TRY 'HELP'");
+                } else {
+                    cliPrint("ERR_CMD_NA: ");
+                    cliPrintLine(cliBuffer);
+                }
+            }
+        }
 
+        cliClearInputBuffer();
+
+        // prompt if in interactive mode
+        if (cliInteractive) {
             cliPrompt();
+        }
+
     } else if (bufferIndex < sizeof(cliBuffer) && c >= 32 && c <= 126) {
-        if (!bufferIndex && c == ' ')
+        if (!bufferIndex && c == ' ') {
             return; // Ignore leading spaces
+        }
         cliBuffer[bufferIndex++] = c;
+
+        // echo the character if interactive
+        if (cliInteractive) {
             cliWrite(c);
         }
     }
+}
 
 static void processCharacterInteractive(const char c)
 {
@@ -6753,7 +6770,7 @@ static void processCharacterInteractive(const char c)
         for (; i < bufferIndex; i++)
             cliWrite(cliBuffer[i]);
     } else if (!bufferIndex && c == 4) {   // CTRL-D
-        cliExit("", cliBuffer);
+        cliExit(true);
         return;
     } else if (c == 12) {                  // NewPage / CTRL-L
         // clear screen
@@ -6770,42 +6787,76 @@ static void processCharacterInteractive(const char c)
     }
 }
 
-void cliProcess(void)
+bool cliProcess(void)
 {
     if (!cliWriter || !cliMode) {
-        return;
+        return false;
     }
 
-    // Flush the buffer to get rid of any MSP data polls sent by configurator after CLI was invoked
-    cliWriterFlush();
-
     while (serialRxBytesWaiting(cliPort)) {
         uint8_t c = serialRead(cliPort);
-
+        if (cliInteractive) {
             processCharacterInteractive(c);
+        } else {
+            // handle terminating flow control character
+            if (c == 0x3 || (cmp32(millis(), cliEntryTime) > 2000)) { // CTRL-C (ETX) or 2 seconds timeout
+                cliWrite(0x3); // send end of text, terminating flow control
+                cliExit(false);
+                return cliMode;
+            }
+            processCharacter(c);
+        }
+    }
+    cliWriterFlush();
+    return cliMode;
+}
+
+static void cliExit(const bool reboot)
+{
+    cliWriterFlush();
+    waitForSerialPortToFinishTransmitting(cliPort);
+    cliClearInputBuffer();
+    cliMode = false;
+    cliInteractive = false;
+    // incase a motor was left running during motortest, clear it here
+    mixerResetDisarmedMotors();
+
+    if (reboot) {
+        cliReboot();
     }
 }
 
-void cliEnter(serialPort_t *serialPort)
+void cliEnter(serialPort_t *serialPort, bool interactive)
 {
     cliMode = true;
+    cliInteractive = interactive;
     cliPort = serialPort;
+    cliEntryTime = millis();
+    cliClearInputBuffer();
+
+    if (interactive) {
         setPrintfSerialPort(cliPort);
+    }
+
     bufWriterInit(&cliWriterDesc, cliWriteBuffer, sizeof(cliWriteBuffer), (bufWrite_t)serialWriteBufShim, serialPort);
     cliErrorWriter = cliWriter = &cliWriterDesc;
 
+    if (interactive) {
 #ifndef MINIMAL_CLI
-    cliPrintLine("\r\nEntering CLI Mode, type 'exit' to return, or 'help'");
+        cliPrintLine("\r\nEntering CLI Mode, type 'exit' to reboot, or 'help'");
 #else
         cliPrintLine("\r\nCLI");
 #endif
+        // arming flag not released if exiting cli with no reboot for safety
         setArmingDisabled(ARMING_DISABLED_CLI);
-
         cliPrompt();
 
 #ifdef USE_CLI_BATCH
         resetCommandBatch();
 #endif
+    } else {
+        cliWrite(0x2); // send start of text, initiating flow control
+    }
 }
 
 #endif // USE_CLI

src/main/cli/cli.h

@@ -24,9 +24,9 @@
 
 extern bool cliMode;
 
-void cliProcess(void);
+bool cliProcess(void);
 struct serialPort_s;
-void cliEnter(struct serialPort_s *serialPort);
+void cliEnter(struct serialPort_s *serialPort, bool interactive);
 
 #ifdef USE_CLI_DEBUG_PRINT
 void cliPrint(const char *str);

src/main/drivers/serial.c

@@ -125,6 +125,7 @@ void serialWriteBuf(serialPort_t *instance, const uint8_t *data, int count)
     serialWriteBufNoFlush(instance, data, count);
     serialEndWrite(instance);
 }
+
 void serialWriteBufShim(void *instance, const uint8_t *data, int count)
 {
     serialWriteBuf((serialPort_t *)instance, data, count);

src/main/fc/tasks.c

@@ -138,13 +138,6 @@ static void taskHandleSerial(timeUs_t currentTimeUs)
     DEBUG_SET(DEBUG_USB, 1, usbVcpIsConnected());
 #endif
 
-#ifdef USE_CLI
-    // in cli mode, all serial stuff goes to here. enter cli mode by sending #
-    if (cliMode) {
-        cliProcess();
-        return;
-    }
-#endif
     bool evaluateMspData = ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA;
     mspSerialProcess(evaluateMspData, mspFcProcessCommand, mspFcProcessReply);
 }

src/main/msp/msp_serial.c

@@ -120,39 +120,32 @@ void mspSerialReleaseSharedTelemetryPorts(void)
 }
 #endif
 
-static bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c)
+static void mspSerialProcessReceivedPacketData(mspPort_t *mspPort, uint8_t c)
 {
-    switch (mspPort->c_state) {
+    switch (mspPort->packetState) {
         default:
-        case MSP_IDLE:      // Waiting for '$' character
+        case MSP_IDLE:
-            if (c == '$') {
-                mspPort->c_state = MSP_HEADER_START;
-            } else {
-                return false;
-            }
-            break;
-
         case MSP_HEADER_START:  // Waiting for 'M' (MSPv1 / MSPv2_over_v1) or 'X' (MSPv2 native)
             mspPort->offset = 0;
             mspPort->checksum1 = 0;
             mspPort->checksum2 = 0;
             switch (c) {
                 case 'M':
-                    mspPort->c_state = MSP_HEADER_M;
+                    mspPort->packetState = MSP_HEADER_M;
                     mspPort->mspVersion = MSP_V1;
                     break;
                 case 'X':
-                    mspPort->c_state = MSP_HEADER_X;
+                    mspPort->packetState = MSP_HEADER_X;
                     mspPort->mspVersion = MSP_V2_NATIVE;
                     break;
                 default:
-                    mspPort->c_state = MSP_IDLE;
+                    mspPort->packetState = MSP_IDLE;
                     break;
             }
             break;
 
         case MSP_HEADER_M:      // Waiting for '<' / '>'
-            mspPort->c_state = MSP_HEADER_V1;
+            mspPort->packetState = MSP_HEADER_V1;
             switch (c) {
                 case '<':
                     mspPort->packetType = MSP_PACKET_COMMAND;
@@ -161,13 +154,13 @@ static bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c)
                     mspPort->packetType = MSP_PACKET_REPLY;
                     break;
                 default:
-                    mspPort->c_state = MSP_IDLE;
+                    mspPort->packetState = MSP_IDLE;
                     break;
             }
             break;
 
         case MSP_HEADER_X:
-            mspPort->c_state = MSP_HEADER_V2_NATIVE;
+            mspPort->packetState = MSP_HEADER_V2_NATIVE;
             switch (c) {
                 case '<':
                     mspPort->packetType = MSP_PACKET_COMMAND;
@@ -176,7 +169,7 @@ static bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c)
                     mspPort->packetType = MSP_PACKET_REPLY;
                     break;
                 default:
-                    mspPort->c_state = MSP_IDLE;
+                    mspPort->packetState = MSP_IDLE;
                     break;
             }
             break;
@@ -188,22 +181,22 @@ static bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c)
                 mspHeaderV1_t * hdr = (mspHeaderV1_t *)&mspPort->inBuf[0];
                 // Check incoming buffer size limit
                 if (hdr->size > MSP_PORT_INBUF_SIZE) {
-                    mspPort->c_state = MSP_IDLE;
+                    mspPort->packetState = MSP_IDLE;
                 }
                 else if (hdr->cmd == MSP_V2_FRAME_ID) {
                     // MSPv1 payload must be big enough to hold V2 header + extra checksum
                     if (hdr->size >= sizeof(mspHeaderV2_t) + 1) {
                         mspPort->mspVersion = MSP_V2_OVER_V1;
-                        mspPort->c_state = MSP_HEADER_V2_OVER_V1;
+                        mspPort->packetState = MSP_HEADER_V2_OVER_V1;
                     } else {
-                        mspPort->c_state = MSP_IDLE;
+                        mspPort->packetState = MSP_IDLE;
                     }
                 } else {
                     mspPort->dataSize = hdr->size;
                     mspPort->cmdMSP = hdr->cmd;
                     mspPort->cmdFlags = 0;
                     mspPort->offset = 0;                // re-use buffer
-                    mspPort->c_state = mspPort->dataSize > 0 ? MSP_PAYLOAD_V1 : MSP_CHECKSUM_V1;    // If no payload - jump to checksum byte
+                    mspPort->packetState = mspPort->dataSize > 0 ? MSP_PAYLOAD_V1 : MSP_CHECKSUM_V1;    // If no payload - jump to checksum byte
                 }
             }
             break;
@@ -212,15 +205,15 @@ static bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c)
             mspPort->inBuf[mspPort->offset++] = c;
             mspPort->checksum1 ^= c;
             if (mspPort->offset == mspPort->dataSize) {
-                mspPort->c_state = MSP_CHECKSUM_V1;
+                mspPort->packetState = MSP_CHECKSUM_V1;
             }
             break;
 
         case MSP_CHECKSUM_V1:
             if (mspPort->checksum1 == c) {
-                mspPort->c_state = MSP_COMMAND_RECEIVED;
+                mspPort->packetState = MSP_COMMAND_RECEIVED;
             } else {
-                mspPort->c_state = MSP_IDLE;
+                mspPort->packetState = MSP_IDLE;
             }
             break;
 
@@ -231,13 +224,13 @@ static bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c)
             if (mspPort->offset == (sizeof(mspHeaderV2_t) + sizeof(mspHeaderV1_t))) {
                 mspHeaderV2_t * hdrv2 = (mspHeaderV2_t *)&mspPort->inBuf[sizeof(mspHeaderV1_t)];
                 if (hdrv2->size > MSP_PORT_INBUF_SIZE) {
-                    mspPort->c_state = MSP_IDLE;
+                    mspPort->packetState = MSP_IDLE;
                 } else {
                     mspPort->dataSize = hdrv2->size;
                     mspPort->cmdMSP = hdrv2->cmd;
                     mspPort->cmdFlags = hdrv2->flags;
                     mspPort->offset = 0;                // re-use buffer
-                    mspPort->c_state = mspPort->dataSize > 0 ? MSP_PAYLOAD_V2_OVER_V1 : MSP_CHECKSUM_V2_OVER_V1;
+                    mspPort->packetState = mspPort->dataSize > 0 ? MSP_PAYLOAD_V2_OVER_V1 : MSP_CHECKSUM_V2_OVER_V1;
                 }
             }
             break;
@@ -248,16 +241,16 @@ static bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c)
             mspPort->inBuf[mspPort->offset++] = c;
 
             if (mspPort->offset == mspPort->dataSize) {
-                mspPort->c_state = MSP_CHECKSUM_V2_OVER_V1;
+                mspPort->packetState = MSP_CHECKSUM_V2_OVER_V1;
             }
             break;
 
         case MSP_CHECKSUM_V2_OVER_V1:
             mspPort->checksum1 ^= c;
             if (mspPort->checksum2 == c) {
-                mspPort->c_state = MSP_CHECKSUM_V1; // Checksum 2 correct - verify v1 checksum
+                mspPort->packetState = MSP_CHECKSUM_V1; // Checksum 2 correct - verify v1 checksum
             } else {
-                mspPort->c_state = MSP_IDLE;
+                mspPort->packetState = MSP_IDLE;
             }
             break;
 
@@ -270,7 +263,7 @@ static bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c)
                 mspPort->cmdMSP = hdrv2->cmd;
                 mspPort->cmdFlags = hdrv2->flags;
                 mspPort->offset = 0;                // re-use buffer
-                mspPort->c_state = mspPort->dataSize > 0 ? MSP_PAYLOAD_V2_NATIVE : MSP_CHECKSUM_V2_NATIVE;
+                mspPort->packetState = mspPort->dataSize > 0 ? MSP_PAYLOAD_V2_NATIVE : MSP_CHECKSUM_V2_NATIVE;
             }
             break;
 
@@ -279,20 +272,18 @@ static bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c)
             mspPort->inBuf[mspPort->offset++] = c;
 
             if (mspPort->offset == mspPort->dataSize) {
-                mspPort->c_state = MSP_CHECKSUM_V2_NATIVE;
+                mspPort->packetState = MSP_CHECKSUM_V2_NATIVE;
             }
             break;
 
         case MSP_CHECKSUM_V2_NATIVE:
             if (mspPort->checksum2 == c) {
-                mspPort->c_state = MSP_COMMAND_RECEIVED;
+                mspPort->packetState = MSP_COMMAND_RECEIVED;
             } else {
-                mspPort->c_state = MSP_IDLE;
+                mspPort->packetState = MSP_IDLE;
             }
             break;
     }
-
-    return true;
 }
 
 static uint8_t mspSerialChecksumBuf(uint8_t checksum, const uint8_t *data, int len)
@@ -445,39 +436,29 @@ static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp, mspPr
     return mspPostProcessFn;
 }
 
-static void mspEvaluateNonMspData(mspPort_t * mspPort, uint8_t receivedChar)
-{
-   if (receivedChar == serialConfig()->reboot_character) {
-        mspPort->pendingRequest = MSP_PENDING_BOOTLOADER_ROM;
-#ifdef USE_CLI
-   } else if (receivedChar == '#') {
-        mspPort->pendingRequest = MSP_PENDING_CLI;
-#endif
-    }
-}
-
 static void mspProcessPendingRequest(mspPort_t * mspPort)
 {
     // If no request is pending or 100ms guard time has not elapsed - do nothing
-    if ((mspPort->pendingRequest == MSP_PENDING_NONE) || (millis() - mspPort->lastActivityMs < 100)) {
+    if ((mspPort->pendingRequest == MSP_PENDING_NONE) || (cmp32(millis(), mspPort->lastActivityMs) < 100)) {
         return;
     }
 
     switch(mspPort->pendingRequest) {
     case MSP_PENDING_BOOTLOADER_ROM:
         systemResetToBootloader(BOOTLOADER_REQUEST_ROM);
-
         break;
+
 #if defined(USE_FLASH_BOOT_LOADER)
     case MSP_PENDING_BOOTLOADER_FLASH:
         systemResetToBootloader(BOOTLOADER_REQUEST_FLASH);
-
         break;
 #endif
+
 #ifdef USE_CLI
     case MSP_PENDING_CLI:
         mspPort->pendingRequest = MSP_PENDING_NONE;
-        cliEnter(mspPort->port);
+        mspPort->portState = PORT_CLI_ACTIVE;
+        cliEnter(mspPort->port, true);
         break;
 #endif
 
@@ -498,8 +479,38 @@ static void mspSerialProcessReceivedReply(mspPort_t *msp, mspProcessReplyFnPtr m
     };
 
     mspProcessReplyFn(&reply);
+}
 
-    msp->c_state = MSP_IDLE;
+void mspProcessPacket(mspPort_t *mspPort, mspProcessCommandFnPtr mspProcessCommandFn, mspProcessReplyFnPtr mspProcessReplyFn)
+{
+    mspPostProcessFnPtr mspPostProcessFn = NULL;
+
+    while (serialRxBytesWaiting(mspPort->port)) {
+        const uint8_t c = serialRead(mspPort->port);
+
+        mspSerialProcessReceivedPacketData(mspPort, c);
+
+        if (mspPort->packetState == MSP_COMMAND_RECEIVED) {
+            if (mspPort->packetType == MSP_PACKET_COMMAND) {
+                mspPostProcessFn = mspSerialProcessReceivedCommand(mspPort, mspProcessCommandFn);
+            } else if (mspPort->packetType == MSP_PACKET_REPLY) {
+                mspSerialProcessReceivedReply(mspPort, mspProcessReplyFn);
+            }
+
+            // process one command at a time so as not to block
+            mspPort->packetState = MSP_IDLE;
+        }
+
+        if (mspPort->packetState == MSP_IDLE) {
+            mspPort->portState = PORT_IDLE;
+            break;
+        }
+    }
+
+    if (mspPostProcessFn) {
+        waitForSerialPortToFinishTransmitting(mspPort->port);
+        mspPostProcessFn(mspPort->port);
+    }
 }
 
 /*
@@ -515,39 +526,50 @@ void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessComm
             continue;
         }
 
-        mspPostProcessFnPtr mspPostProcessFn = NULL;
+        // whilst port is idle, poll incoming until portState changes or no more bytes
+        while (mspPort->portState == PORT_IDLE && serialRxBytesWaiting(mspPort->port)) {
 
-        if (serialRxBytesWaiting(mspPort->port)) {
             // There are bytes incoming - abort pending request
             mspPort->lastActivityMs = millis();
             mspPort->pendingRequest = MSP_PENDING_NONE;
 
-            while (serialRxBytesWaiting(mspPort->port)) {
             const uint8_t c = serialRead(mspPort->port);
-                const bool consumed = mspSerialProcessReceivedData(mspPort, c);
+            if (c == '$') {
-
+                mspPort->portState = PORT_MSP_PACKET;
-                if (!consumed && evaluateNonMspData == MSP_EVALUATE_NON_MSP_DATA) {
+                mspPort->packetState = MSP_HEADER_START;
-                    mspEvaluateNonMspData(mspPort, c);
+            } else if (evaluateNonMspData == MSP_EVALUATE_NON_MSP_DATA) {
+                // evaluate the non-MSP data
+                if (c == serialConfig()->reboot_character) {
+                    mspPort->pendingRequest = MSP_PENDING_BOOTLOADER_ROM;
+#ifdef USE_CLI
+                } else if (c == '#') {
+                    mspPort->pendingRequest = MSP_PENDING_CLI;
+                } else if (c == 0x2) {
+                    mspPort->portState = PORT_CLI_CMD;
+                    cliEnter(mspPort->port, false);
+#endif
                 }
-
-                if (mspPort->c_state == MSP_COMMAND_RECEIVED) {
-                    if (mspPort->packetType == MSP_PACKET_COMMAND) {
-                        mspPostProcessFn = mspSerialProcessReceivedCommand(mspPort, mspProcessCommandFn);
-                    } else if (mspPort->packetType == MSP_PACKET_REPLY) {
-                        mspSerialProcessReceivedReply(mspPort, mspProcessReplyFn);
-                    }
-
-                    mspPort->c_state = MSP_IDLE;
-                    break; // process one command at a time so as not to block.
             }
         }
 
-            if (mspPostProcessFn) {
+        switch (mspPort->portState) {
-                waitForSerialPortToFinishTransmitting(mspPort->port);
+        case PORT_IDLE:
-                mspPostProcessFn(mspPort->port);
-            }
-        } else {
             mspProcessPendingRequest(mspPort);
+            break;
+        case PORT_MSP_PACKET:
+            mspProcessPacket(mspPort, mspProcessCommandFn, mspProcessReplyFn);
+            break;
+#ifdef USE_CLI
+        case PORT_CLI_ACTIVE:
+        case PORT_CLI_CMD:
+            if (!cliProcess()) {
+                mspPort->portState = PORT_IDLE;
+            }
+            break;
+#endif
+
+        default:
+            break;
         }
     }
 }
@@ -585,7 +607,8 @@ int mspSerialPush(serialPortIdentifier_e port, uint8_t cmd, uint8_t *data, int d
 #ifndef USE_MSP_PUSH_OVER_VCP
             || mspPort->port->identifier == SERIAL_PORT_USB_VCP
 #endif
-            || (port != SERIAL_PORT_ALL && mspPort->port->identifier != port)) {
+            || (port != SERIAL_PORT_ALL && mspPort->port->identifier != port)
+            || mspPort->portState == PORT_CLI_CMD || mspPort->portState == PORT_CLI_ACTIVE) {
             continue;
         }
 
@@ -601,7 +624,6 @@ int mspSerialPush(serialPortIdentifier_e port, uint8_t cmd, uint8_t *data, int d
     return ret; // return the number of bytes written
 }
 
-
 uint32_t mspSerialTxBytesFree(void)
 {
     uint32_t ret = UINT32_MAX;

src/main/msp/msp_serial.h

@@ -29,6 +29,13 @@
 // Each MSP port requires state and a receive buffer, revisit this default if someone needs more than 3 MSP ports.
 #define MAX_MSP_PORT_COUNT 3
 
+typedef enum {
+    PORT_IDLE,
+    PORT_MSP_PACKET,
+    PORT_CLI_ACTIVE,
+    PORT_CLI_CMD
+} mspPortState_e;
+
 typedef enum {
     MSP_IDLE,
     MSP_HEADER_START,
@@ -48,7 +55,7 @@ typedef enum {
     MSP_CHECKSUM_V2_NATIVE,
 
     MSP_COMMAND_RECEIVED
-} mspState_e;
+} mspPacketState_e;
 
 typedef enum {
     MSP_PACKET_COMMAND,
@@ -100,7 +107,8 @@ typedef struct mspPort_s {
     struct serialPort_s *port; // null when port unused.
     timeMs_t lastActivityMs;
     mspPendingSystemRequest_e pendingRequest;
-    mspState_e c_state;
+    mspPortState_e portState;
+    mspPacketState_e packetState;
     mspPacketType_e packetType;
     uint8_t inBuf[MSP_PORT_INBUF_SIZE];
     uint16_t cmdMSP;