ELRS SPI Fix eeprom write and reboot for msp over telemetry

src/main/msp/msp.c

@@ -75,6 +75,7 @@
 #include "fc/board_info.h"
 #include "fc/controlrate_profile.h"
 #include "fc/core.h"
+#include "fc/dispatch.h"
 #include "fc/rc.h"
 #include "fc/rc_adjustments.h"
 #include "fc/rc_controls.h"
@@ -142,6 +143,7 @@
 #include "sensors/gyro_init.h"
 #include "sensors/rangefinder.h"
 
+#include "telemetry/msp_shared.h"
 #include "telemetry/telemetry.h"
 
 #ifdef USE_HARDWARE_REVISION_DETECTION
@@ -384,6 +386,48 @@ static void mspRebootFn(serialPort_t *serialPort)
     while (true) ;
 }
 
+#define MSP_DISPATCH_DELAY_US 1000000
+
+void mspReboot(dispatchEntry_t* self)
+{
+    UNUSED(self);
+
+    if (ARMING_FLAG(ARMED)) {
+        return;
+    }
+
+    mspRebootFn(NULL);
+}
+
+dispatchEntry_t mspRebootEntry =
+{
+    mspReboot, 0, NULL, false
+};
+
+void writeReadEeprom(dispatchEntry_t* self)
+{
+    UNUSED(self);
+
+    if (ARMING_FLAG(ARMED)) {
+        return;
+    }
+
+    writeEEPROM();
+    readEEPROM();
+
+#ifdef USE_VTX_TABLE
+    if (vtxTableNeedsInit) {
+        vtxTableNeedsInit = false;
+        vtxTableInit();  // Reinitialize and refresh the in-memory copies
+    }
+#endif
+}
+
+dispatchEntry_t writeReadEepromEntry =
+{
+    writeReadEeprom, 0, NULL, false
+};
+
 static void serializeSDCardSummaryReply(sbuf_t *dst)
 {
     uint8_t flags = 0;
@@ -2193,6 +2237,11 @@ static mspResult_e mspFcProcessOutCommandWithArg(mspDescriptor_t srcDesc, int16_
         }
 #endif
 
+#if defined(USE_MSP_OVER_TELEMETRY)
+        if (featureIsEnabled(FEATURE_RX_SPI) && srcDesc == getMspTelemetryDescriptor()) {
+            dispatchAdd(&mspRebootEntry, MSP_DISPATCH_DELAY_US);
+        } else
+#endif
         if (mspPostProcessFn) {
             *mspPostProcessFn = mspRebootFn;
         }
@@ -3076,15 +3125,14 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
         // ignore how long it takes to avoid confusing the scheduler
         schedulerIgnoreTaskStateTime();
 
-        writeEEPROM();
+#if defined(USE_MSP_OVER_TELEMETRY)
-        readEEPROM();
+        if (featureIsEnabled(FEATURE_RX_SPI) && srcDesc == getMspTelemetryDescriptor()) {
-
+            dispatchAdd(&writeReadEepromEntry, MSP_DISPATCH_DELAY_US);
-#ifdef USE_VTX_TABLE
+        } else
-        if (vtxTableNeedsInit) {
-            vtxTableNeedsInit = false;
-            vtxTableInit();  // Reinitialize and refresh the in-memory copies
-        }
 #endif
+        {
+            writeReadEeprom(NULL);
+        }
 
         break;
 

src/main/rx/expresslrs_telemetry.c

@@ -290,8 +290,8 @@ void processMspPacket(uint8_t *packet)
         break;
     case CRSF_FRAMETYPE_MSP_REQ:
         FALLTHROUGH;
-    case CRSF_FRAMETYPE_MSP_WRITE: //TODO: MSP_EEPROM_WRITE command is disabled.
+    case CRSF_FRAMETYPE_MSP_WRITE:
-        if (packet[ELRS_MSP_COMMAND_INDEX] != MSP_EEPROM_WRITE && bufferCrsfMspFrame(&packet[ELRS_MSP_PACKET_OFFSET], CRSF_FRAME_RX_MSP_FRAME_SIZE)) {
+        if (bufferCrsfMspFrame(&packet[ELRS_MSP_PACKET_OFFSET], CRSF_FRAME_RX_MSP_FRAME_SIZE)) {
             handleCrsfMspFrameBuffer(&bufferMspResponse);
             mspReplyPending = true;
         }

src/main/rx/rx_spi.c

@@ -29,13 +29,14 @@
 
 #include "common/utils.h"
 
+#include "config/config.h"
 #include "config/feature.h"
 
 #include "drivers/io.h"
 #include "drivers/rx/rx_spi.h"
 #include "drivers/rx/rx_nrf24l01.h"
 
-#include "config/config.h"
+#include "fc/dispatch.h"
 
 #include "pg/rx_spi.h"
 
@@ -274,6 +275,8 @@ bool rxSpiInit(const rxSpiConfig_t *rxSpiConfig, rxRuntimeState_t *rxRuntimeStat
     rxRuntimeState->rcFrameStatusFn = rxSpiFrameStatus;
     rxRuntimeState->rcProcessFrameFn = rxSpiProcessFrame;
 
+    dispatchEnable();
+
     return ret;
 }
 

src/main/telemetry/msp_shared.c

@@ -127,7 +127,7 @@ STATIC_UNIT_TESTED mspPacket_t requestPacket;
 STATIC_UNIT_TESTED mspPacket_t responsePacket;
 static uint8_t lastRequestVersion; // MSP version of last request. Temporary solution. It's better to keep it in requestPacket.
 
-static mspDescriptor_t mspSharedDescriptor;
+static mspDescriptor_t mspSharedDescriptor = -1;
 
 void initSharedMsp(void)
 {
@@ -137,6 +137,11 @@ void initSharedMsp(void)
     mspSharedDescriptor = mspDescriptorAlloc();
 }
 
+mspDescriptor_t getMspTelemetryDescriptor(void)
+{
+    return mspSharedDescriptor;
+}
+
 static void processMspPacket(void)
 {
     responsePacket.cmd = 0;

src/main/telemetry/msp_shared.h

@@ -20,6 +20,8 @@
 
 #pragma once
 
+#include "msp/msp.h"
+
 #define MSP_TLM_INBUF_SIZE MSP_PORT_INBUF_SIZE
 #define MSP_TLM_OUTBUF_SIZE MSP_PORT_OUTBUF_SIZE_MIN
 
@@ -28,6 +30,9 @@ typedef void (*mspResponseFnPtr)(uint8_t *payload, const uint8_t payloadSize);
 
 void initSharedMsp(void);
 
+// get descriptor for MSP over telemetry
+mspDescriptor_t getMspTelemetryDescriptor(void);
+
 // receives telemetry payload with msp and handles it.
 bool handleMspFrame(uint8_t *const payload, uint8_t const payloadLength, uint8_t *const skipsBeforeResponse);