diff --git a/Makefile b/Makefile
index afe5aaf9c..3852953ff 100644
--- a/Makefile
+++ b/Makefile
@@ -439,8 +439,8 @@ COMMON_SRC = \
             io/serial_4way_avrootloader.c \
             io/serial_4way_stk500v2.c \
             io/serial_cli.c \
-            io/serial_msp.c \
             io/statusindicator.c \
+            msp/msp_server_fc.c \
             rx/ibus.c \
             rx/jetiexbus.c \
             rx/msp.c \
diff --git a/src/main/io/serial_msp.h b/src/main/io/serial_msp.h
index 8046aa856..bd94bff6f 100644
--- a/src/main/io/serial_msp.h
+++ b/src/main/io/serial_msp.h
@@ -48,5 +48,4 @@ struct serialConfig_s;
 void mspSerialInit(struct serialConfig_s *serialConfig);
 void mspSerialProcess(void);
 void mspSerialAllocatePorts(struct serialConfig_s *serialConfig);
-struct serialPort_s;
 void mspSerialReleasePortIfAllocated(struct serialPort_s *serialPort);
diff --git a/src/main/io/msp_protocol.h b/src/main/msp/msp_protocol.h
similarity index 100%
rename from src/main/io/msp_protocol.h
rename to src/main/msp/msp_protocol.h
diff --git a/src/main/io/serial_msp.c b/src/main/msp/msp_server_fc.c
similarity index 99%
rename from src/main/io/serial_msp.c
rename to src/main/msp/msp_server_fc.c
index 5ea8e0e9a..919f30247 100755
--- a/src/main/io/serial_msp.c
+++ b/src/main/msp/msp_server_fc.c
@@ -67,7 +67,8 @@
 #include "io/serial_4way.h"
 #include "io/serial_msp.h"
 #include "io/vtx.h"
-#include "io/msp_protocol.h"
+
+#include "msp/msp_protocol.h"
 
 #include "rx/rx.h"
 #include "rx/msp.h"
@@ -102,6 +103,10 @@
 #include "hardware_revision.h"
 #endif
 
+#include "io/serial_msp.h"
+
+#include "io/serial_4way.h"
+
 static serialPort_t *mspSerialPort;
 
 extern uint16_t cycleTime; // FIXME dependency on mw.c
@@ -164,6 +169,10 @@ extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
 
 // cause reboot after MSP processing complete
 static bool isRebootScheduled = false;
+STATIC_UNIT_TESTED mspPort_t mspPorts[MAX_MSP_PORT_COUNT];
+STATIC_UNIT_TESTED mspPort_t *currentPort;
+STATIC_UNIT_TESTED bufWriter_t *writer;
+
 
 static const char pidnames[] =
     "ROLL;"
@@ -186,11 +195,6 @@ typedef enum {
 } mspSDCardState_e;
 
 
-STATIC_UNIT_TESTED mspPort_t mspPorts[MAX_MSP_PORT_COUNT];
-
-STATIC_UNIT_TESTED mspPort_t *currentPort;
-STATIC_UNIT_TESTED bufWriter_t *writer;
-
 #define RATEPROFILE_MASK (1 << 7)
 
 #define JUMBO_FRAME_SIZE_LIMIT 255
@@ -490,7 +494,7 @@ void mspSerialReleasePortIfAllocated(serialPort_t *serialPort)
     }
 }
 
-void mspSerialInit(serialConfig_t *serialConfig)
+static void mspInit(void)
 {
     // calculate used boxes based on features and fill availableBoxes[] array
     memset(activeBoxIds, 0xFF, sizeof(activeBoxIds));
@@ -587,7 +591,11 @@ void mspSerialInit(serialConfig_t *serialConfig)
         activeBoxIds[activeBoxIdCount++] = BOXSERVO3;
     }
 #endif
+}
 
+void mspSerialInit(serialConfig_t *serialConfig)
+{
+    mspInit();
     memset(mspPorts, 0x00, sizeof(mspPorts));
     mspSerialAllocatePorts(serialConfig);
 }