diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c
index 4f4d7408d5..d84a1073a5 100644
--- a/src/main/telemetry/crsf.c
+++ b/src/main/telemetry/crsf.c
@@ -112,7 +112,7 @@ bool handleCrsfMspFrameBuffer(uint8_t payloadSize, mspResponseFnPtr responseFn)
     int pos = 0;
     while (true) {
         const int mspFrameLength = mspRxBuffer.bytes[pos];
-        if (handleMspFrame(&mspRxBuffer.bytes[CRSF_MSP_LENGTH_OFFSET + pos], mspFrameLength)) {
+        if (handleMspFrame(&mspRxBuffer.bytes[CRSF_MSP_LENGTH_OFFSET + pos], mspFrameLength, NULL)) {
             requestHandled |= sendMspReply(payloadSize, responseFn);
         }
         pos += CRSF_MSP_LENGTH_OFFSET + mspFrameLength;
diff --git a/src/main/telemetry/msp_shared.c b/src/main/telemetry/msp_shared.c
index 3177d38f80..888343924c 100644
--- a/src/main/telemetry/msp_shared.c
+++ b/src/main/telemetry/msp_shared.c
@@ -31,6 +31,7 @@
 #include "common/utils.h"
 
 #include "interface/msp.h"
+#include "interface/msp_protocol.h"
 
 #include "telemetry/crsf.h"
 #include "telemetry/msp_shared.h"
@@ -44,6 +45,8 @@
 #define TELEMETRY_MSP_SEQ_MASK   0x0F
 #define TELEMETRY_MSP_RES_ERROR (-10)
 
+#define TELEMETRY_REQUEST_SKIPS_AFTER_EEPROMWRITE 5
+
 enum {
     TELEMETRY_MSP_VER_MISMATCH=0,
     TELEMETRY_MSP_CRC_ERROR=1,
@@ -98,7 +101,7 @@ void sendMspErrorResponse(uint8_t error, int16_t cmd)
     sbufSwitchToReader(&mspPackage.responsePacket->buf, mspPackage.responseBuffer);
 }
 
-bool handleMspFrame(uint8_t *frameStart, int frameLength)
+bool handleMspFrame(uint8_t *frameStart, int frameLength, uint8_t *skipsBeforeResponse)
 {
     static uint8_t mspStarted = 0;
     static uint8_t lastSeq = 0;
@@ -169,7 +172,12 @@ bool handleMspFrame(uint8_t *frameStart, int frameLength)
             return true;
         }
     }
-
+    
+    // Skip a few telemetry requests if command is MSP_EEPROM_WRITE
+    if (packet->cmd == MSP_EEPROM_WRITE) {
+        *skipsBeforeResponse = TELEMETRY_REQUEST_SKIPS_AFTER_EEPROMWRITE;
+    }
+    
     mspStarted = 0;
     sbufSwitchToReader(rxBuf, mspPackage.requestBuffer);
     processMspPacket();
diff --git a/src/main/telemetry/msp_shared.h b/src/main/telemetry/msp_shared.h
index f216b04740..9bf9154eae 100644
--- a/src/main/telemetry/msp_shared.h
+++ b/src/main/telemetry/msp_shared.h
@@ -46,5 +46,5 @@ typedef union mspTxBuffer_u {
 } mspTxBuffer_t;
 
 void initSharedMsp(void);
-bool handleMspFrame(uint8_t *frameStart, int frameLength);
+bool handleMspFrame(uint8_t *frameStart, int frameLength, uint8_t *skipsBeforeResponse);
 bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn);
diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c
index 19f7850dae..3e93de0da9 100644
--- a/src/main/telemetry/smartport.c
+++ b/src/main/telemetry/smartport.c
@@ -59,7 +59,6 @@
 #include "flight/pid.h"
 
 #include "interface/msp.h"
-#include "interface/msp_protocol.h"
 
 #include "io/beeper.h"
 #include "io/motors.h"
@@ -82,7 +81,6 @@
 #include "telemetry/msp_shared.h"
 
 #define SMARTPORT_MIN_TELEMETRY_RESPONSE_DELAY_US 500
-#define SMARTPORT_REQUEST_SKIPS_AFTER_EEPROMWRITE 5
 
 // these data identifiers are obtained from https://github.com/opentx/opentx/blob/master/radio/src/telemetry/frsky_hub.h
 enum
@@ -289,11 +287,6 @@ bool smartPortPayloadContainsMSP(const smartPortPayload_t *payload)
     return payload->frameId == FSSP_MSPC_FRAME_SMARTPORT || payload->frameId == FSSP_MSPC_FRAME_FPORT;
 }
 
-bool cmdIsEepromWrite(const smartPortPayload_t *payload)
-{
-    return ((payload->valueId >> 8)&0xFF) == 0 && (payload->data&0xFF) == MSP_EEPROM_WRITE && ((payload->data >> 8)&0xFF) == MSP_EEPROM_WRITE;
-}
-
 void smartPortWriteFrameSerial(const smartPortPayload_t *payload, serialPort_t *port, uint16_t checksum)
 {
     uint8_t *data = (uint8_t *)payload;
@@ -512,14 +505,13 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
         // unless we start receiving other sensors' packets
         // Pass only the payload: skip frameId
         uint8_t *frameStart = (uint8_t *)&payload->valueId;
-        smartPortMspReplyPending = handleMspFrame(frameStart, SMARTPORT_MSP_PAYLOAD_SIZE);
+        smartPortMspReplyPending = handleMspFrame(frameStart, SMARTPORT_MSP_PAYLOAD_SIZE, &skipRequests);
          
         // Don't send MSP response if MSP command is MSP_EEPROM_WRITE
         // CPU just got out of suspended state after writeEEPROM()
         // We don't know if the receiver is listening again
         // Skip a few telemetry requests before sending response
-        if (cmdIsEepromWrite(payload) && smartPortMspReplyPending) {
-            skipRequests = SMARTPORT_REQUEST_SKIPS_AFTER_EEPROMWRITE;
+        if (skipRequests) {
             *clearToSend = false;
         }
     }