diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c
index 914f89ff8c..3fc7cbc55b 100644
--- a/src/main/io/serial_msp.c
+++ b/src/main/io/serial_msp.c
@@ -25,12 +25,9 @@
 
 #include "drivers/buf_writer.h"
 #include "drivers/serial.h"
-#include "drivers/system.h"
 
 #include "fc/runtime_config.h"
 
-#include "flight/mixer.h"
-
 #include "io/serial.h"
 #include "io/serial_msp.h"
 
@@ -38,8 +35,6 @@
 
 
 static mspPort_t mspPorts[MAX_MSP_PORT_COUNT];
-serialPort_t *mspSerialPort;
-mspPort_t *currentPort;
 bufWriter_t *writer;
 
 
@@ -52,12 +47,8 @@ static void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort)
 
 void mspSerialAllocatePorts(void)
 {
-    serialPort_t *serialPort;
-
     uint8_t portIndex = 0;
-
     serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_MSP);
-
     while (portConfig && portIndex < MAX_MSP_PORT_COUNT) {
         mspPort_t *mspPort = &mspPorts[portIndex];
         if (mspPort->port) {
@@ -65,7 +56,7 @@ void mspSerialAllocatePorts(void)
             continue;
         }
 
-        serialPort = openSerialPort(portConfig->identifier, FUNCTION_MSP, NULL, baudRates[portConfig->msp_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED);
+        serialPort_t *serialPort = openSerialPort(portConfig->identifier, FUNCTION_MSP, NULL, baudRates[portConfig->msp_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED);
         if (serialPort) {
             resetMspPort(mspPort, serialPort);
             portIndex++;
@@ -77,8 +68,7 @@ void mspSerialAllocatePorts(void)
 
 void mspSerialReleasePortIfAllocated(serialPort_t *serialPort)
 {
-    uint8_t portIndex;
-    for (portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
+    for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
         mspPort_t *candidateMspPort = &mspPorts[portIndex];
         if (candidateMspPort->port == serialPort) {
             closeSerialPort(serialPort);
@@ -94,12 +84,6 @@ void mspSerialInit(void)
     mspSerialAllocatePorts();
 }
 
-static void setCurrentPort(mspPort_t *port)
-{
-    currentPort = port;
-    mspSerialPort = currentPort->port;
-}
-
 /*
  * Process MSP commands from serial ports configured as MSP ports.
  *
@@ -107,46 +91,37 @@ static void setCurrentPort(mspPort_t *port)
  */
 void mspSerialProcess(void)
 {
-    uint8_t portIndex;
-    mspPort_t *candidatePort;
-
-    for (portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
-        candidatePort = &mspPorts[portIndex];
-        if (!candidatePort->port) {
+    for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
+        mspPort_t * const mspPort = &mspPorts[portIndex];
+        if (!mspPort->port) {
             continue;
         }
 
-        setCurrentPort(candidatePort);
         // Big enough to fit a MSP_STATUS in one write.
         uint8_t buf[sizeof(bufWriter_t) + 20];
-        writer = bufWriterInit(buf, sizeof(buf),
-                               (bufWrite_t)serialWriteBufShim, currentPort->port);
+        writer = bufWriterInit(buf, sizeof(buf), (bufWrite_t)serialWriteBufShim, mspPort->port);
 
-        while (serialRxBytesWaiting(mspSerialPort)) {
+        mspPostProcessFuncPtr mspPostProcessFn = NULL;
+        while (serialRxBytesWaiting(mspPort->port)) {
 
-            uint8_t c = serialRead(mspSerialPort);
-            bool consumed = mspProcessReceivedData(c);
+            const uint8_t c = serialRead(mspPort->port);
+            const bool consumed = mspProcessReceivedData(mspPort, c);
 
             if (!consumed && !ARMING_FLAG(ARMED)) {
-                evaluateOtherData(mspSerialPort, c);
+                evaluateOtherData(mspPort->port, c);
             }
 
-            if (currentPort->c_state == COMMAND_RECEIVED) {
-                mspProcessReceivedCommand();
+            if (mspPort->c_state == COMMAND_RECEIVED) {
+                mspPostProcessFn = mspProcessReceivedCommand(mspPort);
                 break; // process one command at a time so as not to block.
             }
         }
 
         bufWriterFlush(writer);
 
-        if (isRebootScheduled) {
-            waitForSerialPortToFinishTransmitting(candidatePort->port);
-            stopPwmAllMotors();
-            // On real flight controllers, systemReset() will do a soft reset of the device,
-            // reloading the program.  But to support offline testing this flag needs to be
-            // cleared so that the software doesn't continuously attempt to reboot itself.
-            isRebootScheduled = false;
-            systemReset();
+        if (mspPostProcessFn) {
+            waitForSerialPortToFinishTransmitting(mspPort->port);
+            mspPostProcessFn(mspPort);
         }
     }
 }
diff --git a/src/main/io/serial_msp.h b/src/main/io/serial_msp.h
index 2d9d89afd8..dd552da664 100644
--- a/src/main/io/serial_msp.h
+++ b/src/main/io/serial_msp.h
@@ -45,15 +45,9 @@ typedef struct mspPort_s {
 } mspPort_t;
 
 
-extern struct serialPort_s *mspSerialPort;
-extern mspPort_t *currentPort;
-
 struct bufWriter_s;
 extern struct bufWriter_s *writer;
 
-extern bool isRebootScheduled;
-
-
 void mspSerialInit(void);
 void mspSerialProcess(void);
 void mspSerialAllocatePorts(void);
diff --git a/src/main/msp/msp.h b/src/main/msp/msp.h
index c1058326c8..e49dd6bb77 100644
--- a/src/main/msp/msp.h
+++ b/src/main/msp/msp.h
@@ -17,7 +17,9 @@
 
 #pragma once
 
+struct mspPort_s;
+typedef void (*mspPostProcessFuncPtr)(struct mspPort_s *); // msp post process function, used for gracefully handling reboots, etc.
 
 void mspInit(void);
-bool mspProcessReceivedData(uint8_t c);
-void mspProcessReceivedCommand(void);
+bool mspProcessReceivedData(struct mspPort_s *mspPort, uint8_t c);
+mspPostProcessFuncPtr mspProcessReceivedCommand(struct mspPort_s *mspPort);
diff --git a/src/main/msp/msp_server_fc.c b/src/main/msp/msp_server_fc.c
index e1f3263d7d..f61344bf8d 100755
--- a/src/main/msp/msp_server_fc.c
+++ b/src/main/msp/msp_server_fc.c
@@ -69,6 +69,7 @@
 #include "io/vtx.h"
 
 #include "msp/msp_protocol.h"
+#include "msp/msp.h"
 
 #include "rx/rx.h"
 #include "rx/msp.h"
@@ -110,9 +111,11 @@
 extern uint16_t cycleTime; // FIXME dependency on mw.c
 extern void resetProfile(profile_t *profile);
 
-void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, motorConfig_t *motorConfigToUse, pidProfile_t *pidProfileToUse);
+// cause reboot after MSP processing complete
+static mspPostProcessFuncPtr mspPostProcessFn = NULL;
+static mspPort_t *currentPort;
 
-const char * const flightControllerIdentifier = BETAFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
+static const char * const flightControllerIdentifier = BETAFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
 static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
 
 typedef struct box_e {
@@ -161,12 +164,6 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
 static uint8_t activeBoxIds[CHECKBOX_ITEM_COUNT];
 // this is the number of filled indexes in above array
 static uint8_t activeBoxIdCount = 0;
-// from mixer.c
-extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
-
-// cause reboot after MSP processing complete
-bool isRebootScheduled = false;
-
 
 static const char pidnames[] =
     "ROLL;"
@@ -195,6 +192,29 @@ typedef enum {
 
 #define DATAFLASH_BUFFER_SIZE 4096
 
+#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
+void msp4WayIfFn(mspPort_t *mspPort)
+{
+    // 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(mspPort->port);
+    // former used MSP uart is still active
+    // proceed as usual with MSP commands
+}
+#endif
+
+static void mspRebootFn(mspPort_t *mspPort)
+{
+    UNUSED(mspPort);
+
+    stopPwmAllMotors();
+    systemReset();
+
+    // control should never return here.
+    while(1) ;
+}
+
 static void serialize8(uint8_t a)
 {
     bufWriterAppend(writer, a);
@@ -234,7 +254,7 @@ static uint32_t read32(void)
 
 static void headSerialResponse(uint8_t err, uint16_t responseBodySize)
 {
-    serialBeginWrite(mspSerialPort);
+    serialBeginWrite(currentPort->port);
 
     serialize8('$');
     serialize8('M');
@@ -264,7 +284,7 @@ static void headSerialError(uint8_t responseBodySize)
 static void tailSerialReply(void)
 {
     serialize8(currentPort->checksum);
-    serialEndWrite(mspSerialPort);
+    serialEndWrite(currentPort->port);
 }
 
 static void s_struct(uint8_t *cb, uint8_t siz)
@@ -591,7 +611,7 @@ static uint32_t packFlightModeFlags(void)
 static bool processOutCommand(uint8_t cmdMSP)
 {
     uint32_t i;
-    uint8_t len;
+    const unsigned int dataSize = currentPort->dataSize;
 #ifdef GPS
     uint8_t wp_no;
     int32_t lat = 0, lon = 0;
@@ -685,10 +705,12 @@ static bool processOutCommand(uint8_t cmdMSP)
         break;
 
     case MSP_NAME:
-        len = strlen(masterConfig.name);
-        headSerialReply(len);
-        for (uint8_t i=0; i<len; i++) {
-            serialize8(masterConfig.name[i]);
+        {
+            const unsigned int nameLen = strlen(masterConfig.name);
+            headSerialReply(nameLen);
+            for (i = 0; i < nameLen; i++) {
+                serialize8(masterConfig.name[i]);
+            }
         }
         break;
 
@@ -1136,7 +1158,7 @@ static bool processOutCommand(uint8_t cmdMSP)
             uint32_t readAddress = read32();
             uint16_t readLength;
             bool useLegacyFormat;
-            if (currentPort->dataSize >= sizeof(uint32_t) + sizeof(uint16_t)) {
+            if (dataSize >= sizeof(uint32_t) + sizeof(uint16_t)) {
                 readLength = read16();
                 useLegacyFormat = false;
             } else {
@@ -1280,17 +1302,34 @@ static bool processOutCommand(uint8_t cmdMSP)
         serialize8(masterConfig.mag_hardware);
         break;
 
+    case MSP_REBOOT:
+        headSerialReply(0);
+        mspPostProcessFn = mspRebootFn;
+        break;
+
+#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
+    case MSP_SET_4WAY_IF:
+        headSerialReply(1);
+        // get channel number
+        // switch all motor lines HI
+        // reply with the count of ESC found
+        serialize8(esc4wayInit());
+        mspPostProcessFn = msp4WayIfFn;
+        break;
+#endif
+
     default:
         return false;
     }
     return true;
 }
 
-static bool processInCommand(void)
+static bool processInCommand(uint8_t cmdMSP)
 {
     uint32_t i;
     uint16_t tmp;
     uint8_t value;
+    const unsigned int dataSize = currentPort->dataSize;
 #ifdef GPS
     uint8_t wp_no;
     int32_t lat = 0, lon = 0, alt = 0;
@@ -1298,7 +1337,7 @@ static bool processInCommand(void)
 #ifdef OSD
     uint8_t addr, font_data[64];
 #endif
-    switch (currentPort->cmdMSP) {
+    switch (cmdMSP) {
     case MSP_SELECT_SETTING:
         value = read8();
         if ((value & RATEPROFILE_MASK) == 0) {
@@ -1324,7 +1363,7 @@ static bool processInCommand(void)
     case MSP_SET_RAW_RC:
 #ifndef SKIP_RX_MSP
         {
-            uint8_t channelCount = currentPort->dataSize / sizeof(uint16_t);
+            uint8_t channelCount = dataSize / sizeof(uint16_t);
             if (channelCount > MAX_SUPPORTED_RC_CHANNEL_COUNT) {
                 headSerialError(0);
             } else {
@@ -1404,7 +1443,7 @@ static bool processInCommand(void)
         break;
 
     case MSP_SET_RC_TUNING:
-        if (currentPort->dataSize >= 10) {
+        if (dataSize >= 10) {
             currentControlRateProfile->rcRate8 = read8();
             currentControlRateProfile->rcExpo8 = read8();
             for (i = 0; i < 3; i++) {
@@ -1416,10 +1455,10 @@ static bool processInCommand(void)
             currentControlRateProfile->thrMid8 = read8();
             currentControlRateProfile->thrExpo8 = read8();
             currentControlRateProfile->tpa_breakpoint = read16();
-            if (currentPort->dataSize >= 11) {
+            if (dataSize >= 11) {
                 currentControlRateProfile->rcYawExpo8 = read8();
             }
-            if (currentPort->dataSize >= 12) {
+            if (dataSize >= 12) {
                 currentControlRateProfile->rcYawRate8 = read8();
             }
         } else {
@@ -1463,7 +1502,7 @@ static bool processInCommand(void)
         break;
     case MSP_SET_SERVO_CONFIGURATION:
 #ifdef USE_SERVOS
-        if (currentPort->dataSize != 1 + sizeof(servoParam_t)) {
+        if (dataSize != 1 + sizeof(servoParam_t)) {
             headSerialError(0);
             break;
         }
@@ -1560,7 +1599,7 @@ static bool processInCommand(void)
 
 #ifdef TRANSPONDER
     case MSP_SET_TRANSPONDER_CONFIG:
-        if (currentPort->dataSize != sizeof(masterConfig.transponderData)) {
+        if (dataSize != sizeof(masterConfig.transponderData)) {
             headSerialError(0);
             break;
         }
@@ -1692,16 +1731,16 @@ static bool processInCommand(void)
         masterConfig.rxConfig.midrc = read16();
         masterConfig.rxConfig.mincheck = read16();
         masterConfig.rxConfig.spektrum_sat_bind = read8();
-        if (currentPort->dataSize > 8) {
+        if (dataSize > 8) {
             masterConfig.rxConfig.rx_min_usec = read16();
             masterConfig.rxConfig.rx_max_usec = read16();
         }
-        if (currentPort->dataSize > 12) {
+        if (dataSize > 12) {
             masterConfig.rxConfig.rcInterpolation = read8();
             masterConfig.rxConfig.rcInterpolationInterval = read8();
             masterConfig.rxConfig.airModeActivateThreshold = read16();
         }
-        if (currentPort->dataSize > 16) {
+        if (dataSize > 16) {
             masterConfig.rxConfig.rx_spi_protocol = read8();
             masterConfig.rxConfig.rx_spi_id = read32();
             masterConfig.rxConfig.rx_spi_rf_channel_count = read8();
@@ -1761,12 +1800,12 @@ static bool processInCommand(void)
         {
             uint8_t portConfigSize = sizeof(uint8_t) + sizeof(uint16_t) + (sizeof(uint8_t) * 4);
 
-            if (currentPort->dataSize % portConfigSize != 0) {
+            if (dataSize % portConfigSize != 0) {
                 headSerialError(0);
                 break;
             }
 
-            uint8_t remainingPortsInPacket = currentPort->dataSize / portConfigSize;
+            uint8_t remainingPortsInPacket = dataSize / portConfigSize;
 
             while (remainingPortsInPacket--) {
                 uint8_t identifier = read8();
@@ -1800,7 +1839,7 @@ static bool processInCommand(void)
     case MSP_SET_LED_STRIP_CONFIG:
         {
             i = read8();
-            if (i >= LED_MAX_STRIP_LENGTH || currentPort->dataSize != (1 + 4)) {
+            if (i >= LED_MAX_STRIP_LENGTH || dataSize != (1 + 4)) {
                 headSerialError(0);
                 break;
             }
@@ -1821,32 +1860,6 @@ static bool processInCommand(void)
         }
         break;
 #endif
-    case MSP_REBOOT:
-        isRebootScheduled = true;
-        break;
-
-#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
-    case MSP_SET_4WAY_IF:
-        // get channel number
-        // switch all motor lines HI
-        // reply the count of ESC found
-        headSerialReply(1);
-        serialize8(esc4wayInit());
-        // because we do not come back after calling Process4WayInterface
-        // proceed with a success reply first
-        tailSerialReply();
-        // flush the transmit buffer
-        bufWriterFlush(writer);
-        // wait for all data to send
-        waitForSerialPortToFinishTransmitting(currentPort->port);
-        // 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(currentPort->port);
-        // former used MSP uart is still active
-        // proceed as usual with MSP commands
-        break;
-#endif
 
     case MSP_SET_ADVANCED_CONFIG :
         masterConfig.gyro_sync_denom = read8();
@@ -1859,13 +1872,13 @@ static bool processInCommand(void)
         masterConfig.gyro_soft_lpf_hz = read8();
         currentProfile->pidProfile.dterm_lpf_hz = read16();
         currentProfile->pidProfile.yaw_lpf_hz = read16();
-        if (currentPort->dataSize > 5) {
+        if (dataSize > 5) {
             masterConfig.gyro_soft_notch_hz_1 = read16();
             masterConfig.gyro_soft_notch_cutoff_1 = read16();
             currentProfile->pidProfile.dterm_notch_hz = read16();
             currentProfile->pidProfile.dterm_notch_cutoff = read16();
         }
-        if (currentPort->dataSize > 13) {
+        if (dataSize > 13) {
             serialize16(masterConfig.gyro_soft_notch_hz_2);
             serialize16(masterConfig.gyro_soft_notch_cutoff_2);
         }
@@ -1892,7 +1905,7 @@ static bool processInCommand(void)
 
     case MSP_SET_NAME:
         memset(masterConfig.name, 0, ARRAYLEN(masterConfig.name));
-        for (i = 0; i < MIN(MAX_NAME_LENGTH, currentPort->dataSize); i++) {
+        for (i = 0; i < MIN(MAX_NAME_LENGTH, dataSize); i++) {
             masterConfig.name[i] = read8();
         }
         break;
@@ -1904,51 +1917,54 @@ static bool processInCommand(void)
     return true;
 }
 
-void mspProcessReceivedCommand(void)
+mspPostProcessFuncPtr mspProcessReceivedCommand(mspPort_t *mspPort)
 {
-    if (!(processOutCommand(currentPort->cmdMSP) || processInCommand())) {
+    currentPort = mspPort;
+    mspPostProcessFn = NULL;
+    if (!(processOutCommand(mspPort->cmdMSP) || processInCommand(mspPort->cmdMSP))) {
         headSerialError(0);
     }
     tailSerialReply();
-    currentPort->c_state = IDLE;
+    mspPort->c_state = IDLE;
+    return mspPostProcessFn;
 }
 
-bool mspProcessReceivedData(uint8_t c)
+bool mspProcessReceivedData(mspPort_t *mspPort, uint8_t c)
 {
-    if (currentPort->c_state == IDLE) {
+    if (mspPort->c_state == IDLE) {
         if (c == '$') {
-            currentPort->c_state = HEADER_START;
+            mspPort->c_state = HEADER_START;
         } else {
             return false;
         }
-    } else if (currentPort->c_state == HEADER_START) {
-        currentPort->c_state = (c == 'M') ? HEADER_M : IDLE;
-    } else if (currentPort->c_state == HEADER_M) {
-        currentPort->c_state = (c == '<') ? HEADER_ARROW : IDLE;
-    } else if (currentPort->c_state == HEADER_ARROW) {
+    } else if (mspPort->c_state == HEADER_START) {
+        mspPort->c_state = (c == 'M') ? HEADER_M : IDLE;
+    } else if (mspPort->c_state == HEADER_M) {
+        mspPort->c_state = (c == '<') ? HEADER_ARROW : IDLE;
+    } else if (mspPort->c_state == HEADER_ARROW) {
         if (c > MSP_PORT_INBUF_SIZE) {
-            currentPort->c_state = IDLE;
+            mspPort->c_state = IDLE;
 
         } else {
-            currentPort->dataSize = c;
-            currentPort->offset = 0;
-            currentPort->checksum = 0;
-            currentPort->indRX = 0;
-            currentPort->checksum ^= c;
-            currentPort->c_state = HEADER_SIZE;
+            mspPort->dataSize = c;
+            mspPort->offset = 0;
+            mspPort->checksum = 0;
+            mspPort->indRX = 0;
+            mspPort->checksum ^= c;
+            mspPort->c_state = HEADER_SIZE;
         }
-    } else if (currentPort->c_state == HEADER_SIZE) {
-        currentPort->cmdMSP = c;
-        currentPort->checksum ^= c;
-        currentPort->c_state = HEADER_CMD;
-    } else if (currentPort->c_state == HEADER_CMD && currentPort->offset < currentPort->dataSize) {
-        currentPort->checksum ^= c;
-        currentPort->inBuf[currentPort->offset++] = c;
-    } else if (currentPort->c_state == HEADER_CMD && currentPort->offset >= currentPort->dataSize) {
-        if (currentPort->checksum == c) {
-            currentPort->c_state = COMMAND_RECEIVED;
+    } else if (mspPort->c_state == HEADER_SIZE) {
+        mspPort->cmdMSP = c;
+        mspPort->checksum ^= c;
+        mspPort->c_state = HEADER_CMD;
+    } else if (mspPort->c_state == HEADER_CMD && mspPort->offset < mspPort->dataSize) {
+        mspPort->checksum ^= c;
+        mspPort->inBuf[mspPort->offset++] = c;
+    } else if (mspPort->c_state == HEADER_CMD && mspPort->offset >= mspPort->dataSize) {
+        if (mspPort->checksum == c) {
+            mspPort->c_state = COMMAND_RECEIVED;
         } else {
-            currentPort->c_state = IDLE;
+            mspPort->c_state = IDLE;
         }
     }
     return true;