diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c
index 5212bb93b2..257caa8d48 100644
--- a/src/main/rx/crsf.c
+++ b/src/main/rx/crsf.c
@@ -44,7 +44,6 @@
 #include "rx/crsf.h"
 
 #include "telemetry/crsf.h"
-#include "telemetry/msp_shared.h"
 
 #define CRSF_TIME_NEEDED_PER_FRAME_US   1000
 #define CRSF_TIME_BETWEEN_FRAMES_US     4000 // a frame is sent by the transmitter every 4 milliseconds
@@ -112,6 +111,15 @@ struct crsfPayloadRcChannelsPacked_s {
 
 typedef struct crsfPayloadRcChannelsPacked_s crsfPayloadRcChannelsPacked_t;
 
+STATIC_UNIT_TESTED uint8_t crsfFrameCRC(void)
+{
+    // CRC includes type and payload
+    uint8_t crc = crc8_dvb_s2(0, crsfFrame.frame.type);
+    for (int ii = 0; ii < crsfFrame.frame.frameLength - CRSF_FRAME_LENGTH_TYPE_CRC; ++ii) {
+        crc = crc8_dvb_s2(crc, crsfFrame.frame.payload[ii]);
+    }
+    return crc;
+}
 
 // Receive ISR callback, called back from serial port
 STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c)
@@ -141,20 +149,21 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c)
         crsfFrameDone = crsfFramePosition < fullFrameLength ? false : true;
         if (crsfFrameDone) {
             crsfFramePosition = 0;
+#if defined(USE_MSP_OVER_TELEMETRY)
+            if (crsfFrame.frame.type == CRSF_FRAMETYPE_MSP_REQ || crsfFrame.frame.type == CRSF_FRAMETYPE_MSP_WRITE) {
+                const uint8_t crc = crsfFrameCRC();
+                if (crc == crsfFrame.bytes[fullFrameLength - 1]) {
+                    uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE;
+                    if (bufferCrsfMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE)) {
+                        crsfScheduleMspResponse();
+                    }
+                }
+            }
+#endif
         }
     }
 }
 
-STATIC_UNIT_TESTED uint8_t crsfFrameCRC(void)
-{
-    // CRC includes type and payload
-    uint8_t crc = crc8_dvb_s2(0, crsfFrame.frame.type);
-    for (int ii = 0; ii < crsfFrame.frame.frameLength - CRSF_FRAME_LENGTH_TYPE_CRC; ++ii) {
-        crc = crc8_dvb_s2(crc, crsfFrame.frame.payload[ii]);
-    }
-    return crc;
-}
-
 STATIC_UNIT_TESTED uint8_t crsfFrameStatus(void)
 {
     if (crsfFrameDone) {
@@ -185,22 +194,9 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(void)
             crsfChannelData[14] = rcChannels->chan14;
             crsfChannelData[15] = rcChannels->chan15;
             return RX_FRAME_COMPLETE;
-        } else {
-            if (crsfFrame.frame.type == CRSF_FRAMETYPE_DEVICE_PING) {
-                // TODO: CRC CHECK
-                crsfScheduleDeviceInfoResponse();
-                return RX_FRAME_COMPLETE;
-#if defined(USE_MSP_OVER_TELEMETRY)
-            } else if (crsfFrame.frame.type == CRSF_FRAMETYPE_MSP_REQ || crsfFrame.frame.type == CRSF_FRAMETYPE_MSP_WRITE) {
-                // TODO: CRC CHECK
-                uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2;
-                uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + 2 + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE;
-                if (handleMspFrame(frameStart, frameEnd)) {
-                    crsfScheduleMspResponse();
-                }
-                return RX_FRAME_COMPLETE;
-#endif
-            }
+        } else if (crsfFrame.frame.type == CRSF_FRAMETYPE_DEVICE_PING) {
+            crsfScheduleDeviceInfoResponse();
+            return RX_FRAME_COMPLETE;
         }
     }
     return RX_FRAME_PENDING;
diff --git a/src/main/rx/crsf.h b/src/main/rx/crsf.h
index 8342a29917..2c25f68701 100644
--- a/src/main/rx/crsf.h
+++ b/src/main/rx/crsf.h
@@ -45,8 +45,9 @@ enum {
     CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE = 10,
     CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE = 22, // 11 bits per channel * 16 channels = 22 bytes.
     CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE = 6,
-    CRSF_FRAME_TX_MSP_PAYLOAD_SIZE = 58,
-    CRSF_FRAME_RX_MSP_PAYLOAD_SIZE = 8,
+    CRSF_FRAME_TX_MSP_FRAME_SIZE = 58,
+    CRSF_FRAME_RX_MSP_FRAME_SIZE = 8,
+    CRSF_FRAME_ORIGIN_DEST_SIZE = 2,
     CRSF_FRAME_LENGTH_ADDRESS = 1, // length of ADDRESS field
     CRSF_FRAME_LENGTH_FRAMELENGTH = 1, // length of FRAMELENGTH field
     CRSF_FRAME_LENGTH_TYPE = 1, // length of TYPE field
diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h
index cec8f5e4a2..af46d5d173 100644
--- a/src/main/target/STM32F3DISCOVERY/target.h
+++ b/src/main/target/STM32F3DISCOVERY/target.h
@@ -33,7 +33,6 @@
 
 #undef TELEMETRY_JETIEXBUS // ROM SAVING
 
-
 #define CURRENT_TARGET_CPU_VOLTAGE 3.0
 
 #define LED0_PIN                PE8  // Blue LEDs - PE8/PE12
diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c
index 8e9aa5d6c7..e3ca8dd641 100644
--- a/src/main/telemetry/crsf.c
+++ b/src/main/telemetry/crsf.c
@@ -24,6 +24,7 @@
 #ifdef TELEMETRY
 
 #include "config/feature.h"
+#include "build/atomic.h"
 #include "build/build_config.h"
 #include "build/version.h"
 
@@ -36,6 +37,8 @@
 #include "common/streambuf.h"
 #include "common/utils.h"
 
+#include "drivers/nvic.h"
+
 #include "sensors/battery.h"
 
 #include "io/gps.h"
@@ -60,12 +63,65 @@
 
 #define CRSF_CYCLETIME_US                   100000 // 100ms, 10 Hz
 #define CRSF_DEVICEINFO_VERSION             0x01
-#define CRSF_DEVICEINFO_PARAMETER_COUNT     255
+#define CRSF_DEVICEINFO_PARAMETER_COUNT     0
+
+#define CRSF_MSP_BUFFER_SIZE 96
+#define CRSF_MSP_LENGTH_OFFSET 1
 
 static bool crsfTelemetryEnabled;
 static bool deviceInfoReplyPending;
 static uint8_t crsfFrame[CRSF_FRAME_SIZE_MAX];
 
+#if defined(USE_MSP_OVER_TELEMETRY)
+typedef struct mspBuffer_s {
+    uint8_t bytes[CRSF_MSP_BUFFER_SIZE];
+    int len;
+} mspBuffer_t;
+
+static mspBuffer_t mspRxBuffer;
+
+void initCrsfMspBuffer(void)
+{
+    mspRxBuffer.len = 0;
+}
+
+bool bufferCrsfMspFrame(uint8_t *frameStart, int frameLength)
+{
+    if (mspRxBuffer.len + CRSF_MSP_LENGTH_OFFSET + frameLength > CRSF_MSP_BUFFER_SIZE) {
+        return false;
+    } else {
+        uint8_t *p = mspRxBuffer.bytes + mspRxBuffer.len;
+        *p++ = frameLength;
+        memcpy(p, frameStart, frameLength);
+        mspRxBuffer.len += CRSF_MSP_LENGTH_OFFSET + frameLength;
+        return true;
+    }
+}
+
+bool handleCrsfMspFrameBuffer(uint8_t payloadSize, mspResponseFnPtr responseFn)
+{
+    bool requestHandled = false;
+    if (!mspRxBuffer.len) {
+        return false;
+    }
+    int pos = 0;
+    while (true) {
+        const int mspFrameLength = mspRxBuffer.bytes[pos];
+        if (handleMspFrame(&mspRxBuffer.bytes[CRSF_MSP_LENGTH_OFFSET + pos], mspFrameLength)) {
+            requestHandled |= sendMspReply(payloadSize, responseFn);
+        }
+        pos += CRSF_MSP_LENGTH_OFFSET + mspFrameLength;
+        ATOMIC_BLOCK(NVIC_PRIO_SERIALUART1) {
+            if (pos >= mspRxBuffer.len) {
+                mspRxBuffer.len = 0;
+                return requestHandled;
+            }
+        }
+    }
+    return requestHandled;
+}
+#endif
+
 static void crsfInitializeFrame(sbuf_t *dst)
 {
     dst->ptr = crsfFrame;
@@ -291,11 +347,11 @@ void crsfSendMspResponse(uint8_t *payload)
     sbuf_t *dst = &crsfPayloadBuf;
 
     crsfInitializeFrame(dst);
-    sbufWriteU8(dst, CRSF_FRAME_TX_MSP_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_EXT_TYPE_CRC);
+    sbufWriteU8(dst, CRSF_FRAME_TX_MSP_FRAME_SIZE + CRSF_FRAME_LENGTH_EXT_TYPE_CRC);
     sbufWriteU8(dst, CRSF_FRAMETYPE_MSP_RESP);
     sbufWriteU8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER);
     sbufWriteU8(dst, CRSF_ADDRESS_BETAFLIGHT);
-    sbufWriteData(dst, payload, CRSF_FRAME_TX_MSP_PAYLOAD_SIZE);
+    sbufWriteData(dst, payload, CRSF_FRAME_TX_MSP_FRAME_SIZE);
     crsfFinalize(dst);
 }
 #endif
@@ -383,6 +439,12 @@ void handleCrsfTelemetry(timeUs_t currentTimeUs)
     // in between the RX frames.
     crsfRxSendTelemetryData();
 
+#if defined(USE_MSP_OVER_TELEMETRY)
+    if (mspReplyPending) {
+        mspReplyPending = handleCrsfMspFrameBuffer(CRSF_FRAME_TX_MSP_FRAME_SIZE, &crsfSendMspResponse);
+    }
+#endif
+
     // Actual telemetry data only needs to be sent at a low frequency, ie 10Hz
     if (currentTimeUs >= crsfLastCycleTime + CRSF_CYCLETIME_US) {
         crsfLastCycleTime = currentTimeUs;
@@ -393,10 +455,6 @@ void handleCrsfTelemetry(timeUs_t currentTimeUs)
             crsfFrameDeviceInfo(dst);
             crsfFinalize(dst);
             deviceInfoReplyPending = false;
-#if defined(USE_MSP_OVER_TELEMETRY)
-        } else if (mspReplyPending) {
-            mspReplyPending = sendMspReply(CRSF_FRAME_TX_MSP_PAYLOAD_SIZE, &crsfSendMspResponse);
-#endif
         } else {
             processCrsf();
         }
diff --git a/src/main/telemetry/crsf.h b/src/main/telemetry/crsf.h
index 3ccfc76cac..e8fe4ae444 100644
--- a/src/main/telemetry/crsf.h
+++ b/src/main/telemetry/crsf.h
@@ -26,5 +26,8 @@ bool checkCrsfTelemetryState(void);
 void handleCrsfTelemetry(timeUs_t currentTimeUs);
 void crsfScheduleDeviceInfoResponse(void);
 void crsfScheduleMspResponse(void);
-
 int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType);
+#if defined(USE_MSP_OVER_TELEMETRY)
+void initCrsfMspBuffer(void);
+bool bufferCrsfMspFrame(uint8_t *frameStart, int frameLength);
+#endif
diff --git a/src/main/telemetry/msp_shared.c b/src/main/telemetry/msp_shared.c
index d80ec2a168..ebd732d168 100644
--- a/src/main/telemetry/msp_shared.c
+++ b/src/main/telemetry/msp_shared.c
@@ -36,9 +36,6 @@ enum {
     TELEMETRY_MSP_ERROR=2
 };
 
-#define REQUEST_BUFFER_SIZE 64
-#define RESPONSE_BUFFER_SIZE 64
-
 STATIC_UNIT_TESTED uint8_t checksum = 0;
 STATIC_UNIT_TESTED mspPackage_t mspPackage;
 static mspRxBuffer_t mspRxBuffer;
@@ -87,7 +84,7 @@ void sendMspErrorResponse(uint8_t error, int16_t cmd)
     sbufSwitchToReader(&mspPackage.responsePacket->buf, mspPackage.responseBuffer);
 }
 
-bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd)
+bool handleMspFrame(uint8_t *frameStart, int frameLength)
 {
     static uint8_t mspStarted = 0;
     static uint8_t lastSeq = 0;
@@ -101,7 +98,7 @@ bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd)
     }
 
     mspPacket_t *packet = mspPackage.requestPacket;
-    sbuf_t *frameBuf = sbufInit(&mspPackage.requestFrame, frameStart, frameEnd);
+    sbuf_t *frameBuf = sbufInit(&mspPackage.requestFrame, frameStart, frameStart + (uint8_t)frameLength);
     sbuf_t *rxBuf = &mspPackage.requestPacket->buf;
     const uint8_t header = sbufReadU8(frameBuf);
     const uint8_t seqNumber = header & TELEMETRY_MSP_SEQ_MASK;
diff --git a/src/main/telemetry/msp_shared.h b/src/main/telemetry/msp_shared.h
index 85a3e2c703..ba2fd7b7bd 100644
--- a/src/main/telemetry/msp_shared.h
+++ b/src/main/telemetry/msp_shared.h
@@ -25,5 +25,5 @@ typedef union mspTxBuffer_u {
 } mspTxBuffer_t;
 
 void initSharedMsp(void);
-bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd);
+bool handleMspFrame(uint8_t *frameStart, int frameLength);
 bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn);
diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c
index 0ecfaba1c3..06a38553ba 100644
--- a/src/main/telemetry/smartport.c
+++ b/src/main/telemetry/smartport.c
@@ -361,9 +361,7 @@ void handleSmartPortTelemetry(void)
         if (smartPortRxBuffer.frameId == FSSP_MSPC_FRAME) {
             // Pass only the payload: skip sensorId & frameId
             uint8_t *frameStart = (uint8_t *)&smartPortRxBuffer + SMARTPORT_PAYLOAD_OFFSET;
-            uint8_t *frameEnd = (uint8_t *)&smartPortRxBuffer + SMARTPORT_PAYLOAD_OFFSET + SMARTPORT_PAYLOAD_SIZE;
-
-            smartPortMspReplyPending = handleMspFrame(frameStart, frameEnd);
+            smartPortMspReplyPending = handleMspFrame(frameStart, SMARTPORT_PAYLOAD_SIZE);
         }
 #endif
     }
diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c
index 74cca18c89..e8835de233 100644
--- a/src/main/telemetry/telemetry.c
+++ b/src/main/telemetry/telemetry.c
@@ -103,6 +103,7 @@ void telemetryInit(void)
 #endif
 #if defined(USE_MSP_OVER_TELEMETRY)
     initSharedMsp();
+    initCrsfMspBuffer();
 #endif
 
     telemetryCheckState();
diff --git a/src/test/Makefile b/src/test/Makefile
index 4cdd938a3e..5d34daf69c 100644
--- a/src/test/Makefile
+++ b/src/test/Makefile
@@ -229,6 +229,7 @@ telemetry_crsf_unittest_DEFINES := \
 
 telemetry_crsf_msp_unittest_SRC := \
 		$(USER_DIR)/rx/crsf.c \
+		$(USER_DIR)/build/atomic.c \
 		$(USER_DIR)/common/crc.c \
 		$(USER_DIR)/common/streambuf.c \
 		$(USER_DIR)/common/printf.c \
@@ -282,9 +283,6 @@ huffman_unittest_SRC := \
 huffman_unittest_DEFINES := \
 		USE_HUFFMAN
 
-ringbuffer_unittest_SRC := \
-		$(USER_DIR)/common/ringbuffer.c
-
 # Please tweak the following variable definitions as needed by your
 # project, except GTEST_HEADERS, which you can use in your own targets
 # but shouldn't modify.
diff --git a/src/test/unit/platform.h b/src/test/unit/platform.h
index f48b34b5ce..29aa361bad 100644
--- a/src/test/unit/platform.h
+++ b/src/test/unit/platform.h
@@ -94,5 +94,6 @@ typedef struct
 
 #define WS2811_DMA_TC_FLAG (void *)1
 #define WS2811_DMA_HANDLER_IDENTIFER 0
+#define NVIC_PriorityGroup_2 0x500
 
 #include "target.h"
diff --git a/src/test/unit/rx_crsf_unittest.cc b/src/test/unit/rx_crsf_unittest.cc
index 1aa2e89642..005be54c09 100644
--- a/src/test/unit/rx_crsf_unittest.cc
+++ b/src/test/unit/rx_crsf_unittest.cc
@@ -288,5 +288,5 @@ bool telemetryCheckRxPortShared(const serialPortConfig_t *) {return false;}
 serialPort_t *telemetrySharedPort = NULL;
 void crsfScheduleDeviceInfoResponse(void) {};
 void crsfScheduleMspResponse(mspPackage_t *package) { UNUSED(package); };
-bool handleMspFrame(uint8_t *, uint8_t *) { return false; }
+bool bufferMspFrame(uint8_t *, int) {return true;}
 }
diff --git a/src/test/unit/telemetry_crsf_msp_unittest.cc b/src/test/unit/telemetry_crsf_msp_unittest.cc
index bd44bb127c..4333b26d48 100644
--- a/src/test/unit/telemetry_crsf_msp_unittest.cc
+++ b/src/test/unit/telemetry_crsf_msp_unittest.cc
@@ -26,6 +26,7 @@ extern "C" {
     #include <platform.h>
 
     #include "build/debug.h"
+    #include "build/atomic.h"
 
     #include "common/crc.h"
     #include "common/utils.h"
@@ -37,6 +38,7 @@ extern "C" {
     #include "config/parameter_group.h"
     #include "config/parameter_group_ids.h"
 
+    #include "drivers/nvic.h"
     #include "drivers/serial.h"
     #include "drivers/system.h"
 
@@ -61,7 +63,7 @@ extern "C" {
     #include "telemetry/smartport.h"
     #include "sensors/acceleration.h"
 
-    bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd);
+    bool handleMspFrame(uint8_t *frameStart, int frameLength);
     bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn);
     uint8_t sbufReadU8(sbuf_t *src);
     int sbufBytesRemaining(sbuf_t *buf);
@@ -99,7 +101,7 @@ typedef struct crsfMspFrame_s {
     uint8_t type;
     uint8_t destination;
     uint8_t origin;
-    uint8_t payload[CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_CRC];
+    uint8_t payload[CRSF_FRAME_RX_MSP_FRAME_SIZE + CRSF_FRAME_LENGTH_CRC];
 } crsfMspFrame_t;
 
 const uint8_t crsfPidRequest[] = {
@@ -108,17 +110,19 @@ const uint8_t crsfPidRequest[] = {
 
 TEST(CrossFireMSPTest, RequestBufferTest)
 {
+    atomic_BASEPRI = 0;
+
     initSharedMsp();
     const crsfMspFrame_t *framePtr = (const crsfMspFrame_t*)crsfPidRequest;
     crsfFrame = *(const crsfFrame_t*)framePtr;
     crsfFrameDone = true;
     EXPECT_EQ(CRSF_ADDRESS_BROADCAST, crsfFrame.frame.deviceAddress);
-    EXPECT_EQ(CRSF_FRAME_LENGTH_ADDRESS + CRSF_FRAME_LENGTH_EXT_TYPE_CRC + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE, crsfFrame.frame.frameLength);
+    EXPECT_EQ(CRSF_FRAME_LENGTH_ADDRESS + CRSF_FRAME_LENGTH_EXT_TYPE_CRC + CRSF_FRAME_RX_MSP_FRAME_SIZE, crsfFrame.frame.frameLength);
     EXPECT_EQ(CRSF_FRAMETYPE_MSP_REQ, crsfFrame.frame.type);
     uint8_t *destination = (uint8_t *)&crsfFrame.frame.payload;
     uint8_t *origin = (uint8_t *)&crsfFrame.frame.payload + 1;
     uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2;
-    uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2;
+    uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_FRAME_SIZE + 2;
     EXPECT_EQ(0xC8, *destination);
     EXPECT_EQ(0xEA, *origin);
     EXPECT_EQ(0x30, *frameStart);
@@ -132,8 +136,7 @@ TEST(CrossFireMSPTest, ResponsePacketTest)
     crsfFrame = *(const crsfFrame_t*)framePtr;
     crsfFrameDone = true;
     uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2;
-    uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2;
-    handleMspFrame(frameStart, frameEnd);
+    handleMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE);
     for (unsigned int ii=1; ii<30; ii++) {
         EXPECT_EQ(ii, sbufReadU8(&mspPackage.responsePacket->buf));
     }
@@ -153,8 +156,7 @@ TEST(CrossFireMSPTest, WriteResponseTest)
     crsfFrame = *(const crsfFrame_t*)framePtr1;
     crsfFrameDone = true;
     uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2;
-    uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2;
-    bool pending1 = handleMspFrame(frameStart, frameEnd);
+    bool pending1 = handleMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE);
     EXPECT_FALSE(pending1); // not done yet*/
     EXPECT_EQ(0x29, mspPackage.requestBuffer[0]);
     EXPECT_EQ(0x28, mspPackage.requestBuffer[1]);
@@ -166,8 +168,7 @@ TEST(CrossFireMSPTest, WriteResponseTest)
     crsfFrame = *(const crsfFrame_t*)framePtr2;
     crsfFrameDone = true;
     uint8_t *frameStart2 = (uint8_t *)&crsfFrame.frame.payload + 2;
-    uint8_t *frameEnd2 = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2;
-    bool pending2 = handleMspFrame(frameStart2, frameEnd2);
+    bool pending2 = handleMspFrame(frameStart2, CRSF_FRAME_RX_MSP_FRAME_SIZE);
     EXPECT_FALSE(pending2); // not done yet
     EXPECT_EQ(0x23, mspPackage.requestBuffer[5]);
     EXPECT_EQ(0x46, mspPackage.requestBuffer[6]);
@@ -181,8 +182,7 @@ TEST(CrossFireMSPTest, WriteResponseTest)
     crsfFrame = *(const crsfFrame_t*)framePtr3;
     crsfFrameDone = true;
     uint8_t *frameStart3 = (uint8_t *)&crsfFrame.frame.payload + 2;
-    uint8_t *frameEnd3 = frameStart3 + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE;
-    bool pending3 = handleMspFrame(frameStart3, frameEnd3);
+    bool pending3 = handleMspFrame(frameStart3, CRSF_FRAME_RX_MSP_FRAME_SIZE);
     EXPECT_FALSE(pending3); // not done yet
     EXPECT_EQ(0x0F, mspPackage.requestBuffer[12]);
     EXPECT_EQ(0x00, mspPackage.requestBuffer[13]);
@@ -196,8 +196,7 @@ TEST(CrossFireMSPTest, WriteResponseTest)
     crsfFrame = *(const crsfFrame_t*)framePtr4;
     crsfFrameDone = true;
     uint8_t *frameStart4 = (uint8_t *)&crsfFrame.frame.payload + 2;
-    uint8_t *frameEnd4 = frameStart4 + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE;
-    bool pending4 = handleMspFrame(frameStart4, frameEnd4);
+    bool pending4 = handleMspFrame(frameStart4, CRSF_FRAME_RX_MSP_FRAME_SIZE);
     EXPECT_FALSE(pending4); // not done yet
     EXPECT_EQ(0x21, mspPackage.requestBuffer[19]);
     EXPECT_EQ(0x53, mspPackage.requestBuffer[20]);
@@ -212,8 +211,7 @@ TEST(CrossFireMSPTest, WriteResponseTest)
     crsfFrame = *(const crsfFrame_t*)framePtr5;
     crsfFrameDone = true;
     uint8_t *frameStart5 = (uint8_t *)&crsfFrame.frame.payload + 2;
-    uint8_t *frameEnd5 = frameStart2 + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE;
-    bool pending5 = handleMspFrame(frameStart5, frameEnd5);
+    bool pending5 = handleMspFrame(frameStart5, CRSF_FRAME_RX_MSP_FRAME_SIZE);
     EXPECT_TRUE(pending5); // not done yet
     EXPECT_EQ(0x00, mspPackage.requestBuffer[26]);
     EXPECT_EQ(0x37, mspPackage.requestBuffer[27]);
@@ -235,8 +233,7 @@ TEST(CrossFireMSPTest, SendMspReply) {
     crsfFrame = *(const crsfFrame_t*)framePtr;
     crsfFrameDone = true;
     uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2;
-    uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2;
-    bool handled = handleMspFrame(frameStart, frameEnd);
+    bool handled = handleMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE);
     EXPECT_TRUE(handled);
     bool replyPending = sendMspReply(64, &testSendMspResponse);
     EXPECT_FALSE(replyPending);
@@ -278,7 +275,6 @@ extern "C" {
         UNUSED(mspPostProcessFn);
 
         sbuf_t *dst = &reply->buf;
-        //sbuf_t *src = &cmd->buf;
         const uint8_t cmdMSP = cmd->cmd;
         reply->cmd = cmd->cmd;
 
@@ -298,5 +294,4 @@ extern "C" {
     int32_t getMAhDrawn(void) {
       return testmAhDrawn;
     }
-
 }
diff --git a/src/test/unit/telemetry_crsf_unittest.cc b/src/test/unit/telemetry_crsf_unittest.cc
index 7515d18691..7571f7916e 100644
--- a/src/test/unit/telemetry_crsf_unittest.cc
+++ b/src/test/unit/telemetry_crsf_unittest.cc
@@ -334,6 +334,7 @@ int32_t getMAhDrawn(void){
 }
 
 bool sendMspReply(uint8_t, mspResponseFnPtr) { return false; }
-bool handleMspFrame(uint8_t *, uint8_t *)  { return false; }
+bool handleMspFrame(uint8_t *, int)  { return false; }
+void crsfScheduleMspResponse(void) {};
 
 }