diff --git a/src/main/drivers/serial_escserial.c b/src/main/drivers/serial_escserial.c
index 351b2c98c..76309c3bc 100644
--- a/src/main/drivers/serial_escserial.c
+++ b/src/main/drivers/serial_escserial.c
@@ -27,14 +27,6 @@ typedef enum {
     BAUDRATE_CASTLE = 18880
 } escBaudRate_e;
 
-typedef enum {
-    PROTOCOL_SIMONK = 0,
-    PROTOCOL_BLHELI = 1,
-    PROTOCOL_KISS = 2,
-    PROTOCOL_KISSALL = 3,
-    PROTOCOL_CASTLE = 4
-} escProtocol_e;
-
 #if defined(USE_ESCSERIAL)
 
 #include "build/build_config.h"
diff --git a/src/main/drivers/serial_escserial.h b/src/main/drivers/serial_escserial.h
index 113ce1c2a..41bbf0755 100644
--- a/src/main/drivers/serial_escserial.h
+++ b/src/main/drivers/serial_escserial.h
@@ -24,6 +24,15 @@ typedef enum {
     ESCSERIAL2
 } escSerialPortIndex_e;
 
+typedef enum {
+    PROTOCOL_SIMONK = 0,
+    PROTOCOL_BLHELI = 1,
+    PROTOCOL_KISS = 2,
+    PROTOCOL_KISSALL = 3,
+    PROTOCOL_CASTLE = 4,
+    PROTOCOL_COUNT
+} escProtocol_e;
+
 serialPort_t *openEscSerial(escSerialPortIndex_e portIndex, serialReceiveCallbackPtr callback, uint16_t output, uint32_t baud, portOptions_t options, uint8_t mode);
 
 // serialPort API
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index bbc065145..ebb4a16db 100755
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -46,6 +46,7 @@
 #include "drivers/max7456.h"
 #include "drivers/vtx_soft_spi_rtc6705.h"
 #include "drivers/pwm_output.h"
+#include "drivers/serial_escserial.h"
 
 #include "fc/config.h"
 #include "fc/mw.h"
@@ -182,14 +183,61 @@ typedef enum {
 #define RATEPROFILE_MASK (1 << 7)
 
 #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
-static void msp4WayIfFn(serialPort_t *serialPort)
+#define ESC_4WAY 0xff
+
+uint8_t escMode;
+uint8_t escPortIndex = 0;
+
+#ifdef USE_ESCSERIAL
+static void mspEscPassthroughFn(serialPort_t *serialPort)
 {
-    // rem: App: Wait at least appx. 500 ms for BLHeli to jump into
-    // bootloader mode before try to connect any ESC
-    // Start to activate here
-    esc4wayProcess(serialPort);
-    // former used MSP uart is still active
-    // proceed as usual with MSP commands
+    escEnablePassthrough(serialPort, escPortIndex, escMode);
+}
+#endif
+
+static void mspFc4waySerialCommand(sbuf_t *dst, sbuf_t *src, mspPostProcessFnPtr *mspPostProcessFn)
+{
+    const unsigned int dataSize = sbufBytesRemaining(src);
+    if (dataSize == 0) {
+        // Legacy format
+
+        escMode = ESC_4WAY;
+    } else {
+        escMode = sbufReadU8(src);
+        escPortIndex = sbufReadU8(src);
+    }
+
+    switch(escMode) {
+    case ESC_4WAY:
+        // get channel number
+        // switch all motor lines HI
+        // reply with the count of ESC found
+        sbufWriteU8(dst, esc4wayInit());
+
+        if (mspPostProcessFn) {
+            *mspPostProcessFn = esc4wayProcess;
+        }
+
+        break;
+#ifdef USE_ESCSERIAL
+    case PROTOCOL_SIMONK:
+    case PROTOCOL_BLHELI:
+    case PROTOCOL_KISS:
+    case PROTOCOL_KISSALL:
+    case PROTOCOL_CASTLE:
+        if (escPortIndex < USABLE_TIMER_CHANNEL_COUNT || (escMode == PROTOCOL_KISS && escPortIndex == 255)) {
+            sbufWriteU8(dst, 1);
+
+            if (mspPostProcessFn) {
+                *mspPostProcessFn = mspEscPassthroughFn;
+            }
+
+            break;
+        }
+#endif
+    default:
+        sbufWriteU8(dst, 0);
+    }
 }
 #endif
 
@@ -431,21 +479,24 @@ static void serializeSDCardSummaryReply(sbuf_t *dst)
         state = MSP_SDCARD_STATE_FATAL;
     } else {
         switch (afatfs_getFilesystemState()) {
-            case AFATFS_FILESYSTEM_STATE_READY:
-                state = MSP_SDCARD_STATE_READY;
-                break;
-            case AFATFS_FILESYSTEM_STATE_INITIALIZATION:
-                if (sdcard_isInitialized()) {
-                    state = MSP_SDCARD_STATE_FS_INIT;
-                } else {
-                    state = MSP_SDCARD_STATE_CARD_INIT;
-                }
-                break;
-            case AFATFS_FILESYSTEM_STATE_FATAL:
-            case AFATFS_FILESYSTEM_STATE_UNKNOWN:
-            default:
-                state = MSP_SDCARD_STATE_FATAL;
-                break;
+        case AFATFS_FILESYSTEM_STATE_READY:
+            state = MSP_SDCARD_STATE_READY;
+
+            break;
+        case AFATFS_FILESYSTEM_STATE_INITIALIZATION:
+            if (sdcard_isInitialized()) {
+                state = MSP_SDCARD_STATE_FS_INIT;
+            } else {
+                state = MSP_SDCARD_STATE_CARD_INIT;
+            }
+
+            break;
+        case AFATFS_FILESYSTEM_STATE_FATAL:
+        case AFATFS_FILESYSTEM_STATE_UNKNOWN:
+        default:
+            state = MSP_SDCARD_STATE_FATAL;
+
+            break;
         }
     }
 
@@ -1139,18 +1190,6 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
         }
         break;
 
-#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
-    case MSP_SET_4WAY_IF:
-        // get channel number
-        // switch all motor lines HI
-        // reply with the count of ESC found
-        sbufWriteU8(dst, esc4wayInit());
-        if (mspPostProcessFn) {
-            *mspPostProcessFn = msp4WayIfFn;
-        }
-        break;
-#endif
-
     default:
         return false;
     }
@@ -1842,6 +1881,11 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro
 
     if (mspFcProcessOutCommand(cmdMSP, dst, mspPostProcessFn)) {
         ret = MSP_RESULT_ACK;
+#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
+    } else if (cmdMSP == MSP_SET_4WAY_IF) {
+        mspFc4waySerialCommand(dst, src, mspPostProcessFn);
+        ret = MSP_RESULT_ACK;
+#endif
 #ifdef GPS
     } else if (cmdMSP == MSP_WP) {
         mspFcWpCommand(dst, src);