Fixed lockups in FrSky SPI RX. (#8702)

Fixed lockups in FrSky SPI RX.

src/main/fc/core.c

@@ -1113,7 +1113,6 @@ static FAST_CODE_NOINLINE void subTaskRcCommand(timeUs_t currentTimeUs)
     }
 
     processRcCommand();
-
 }
 
 // Function for loop trigger

src/main/rx/cc2500_frsky_common.h

@@ -24,4 +24,5 @@
 
 bool frSkySpiInit(const rxSpiConfig_t *rxSpiConfig, rxRuntimeConfig_t *rxRuntimeConfig);
 rx_spi_received_e frSkySpiDataReceived(uint8_t *packet);
+rx_spi_received_e frSkySpiProcessFrame(uint8_t *packet);
 void frSkySpiSetRcData(uint16_t *rcData, const uint8_t *payload);

src/main/rx/cc2500_frsky_d.c

@@ -194,7 +194,7 @@ rx_spi_received_e frSkyDHandlePacket(uint8_t * const packet, uint8_t * const pro
     switch (*protocolState) {
     case STATE_STARTING:
         listLength = 47;
-        initialiseData(0);
+        initialiseData(false);
         *protocolState = STATE_UPDATE;
         nextChannel(1);
         cc2500Strobe(CC2500_SRX);
@@ -218,9 +218,11 @@ rx_spi_received_e frSkyDHandlePacket(uint8_t * const packet, uint8_t * const pro
     case STATE_DATA:
         if (cc2500getGdo()) {
             uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
+            bool packetOk = false;
             if (ccLen >= 20) {
                 cc2500ReadFifo(packet, 20);
                 if (packet[19] & 0x80) {
+                    packetOk = true;
                     missingPackets = 0;
                     timeoutUs = 1;
                     if (packet[0] == 0x11) {
@@ -246,6 +248,9 @@ rx_spi_received_e frSkyDHandlePacket(uint8_t * const packet, uint8_t * const pro
                     }
                 }
             }
+            if (!packetOk) {
+                cc2500Strobe(CC2500_SRX);
+            }
         }
 
         if (cmpTimeUs(currentPacketReceivedTime, lastPacketReceivedTime) > (timeoutUs * SYNC_DELAY_MAX)) {

src/main/rx/cc2500_frsky_shared.c

@@ -62,9 +62,11 @@ static uint8_t bindIdx;
 static int8_t bindOffset;
 
 typedef uint8_t handlePacketFn(uint8_t * const packet, uint8_t * const protocolState);
+typedef rx_spi_received_e processFrameFn(uint8_t * const packet);
 typedef void setRcDataFn(uint16_t *rcData, const uint8_t *payload);
 
 static handlePacketFn *handlePacket;
+static processFrameFn *processFrame;
 static setRcDataFn *setRcData;
 
 static void initialise() {
@@ -152,13 +154,16 @@ static void initialise() {
     }
 }
 
-void initialiseData(uint8_t adr)
+void initialiseData(bool inBindState)
 {
     cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)rxCc2500SpiConfig()->bindOffset);
     cc2500WriteReg(CC2500_18_MCSM0, 0x8);
-    cc2500WriteReg(CC2500_09_ADDR, adr ? 0x03 : rxCc2500SpiConfig()->bindTxId[0]);
+    cc2500WriteReg(CC2500_09_ADDR, inBindState ? 0x03 : rxCc2500SpiConfig()->bindTxId[0]);
     cc2500WriteReg(CC2500_07_PKTCTRL1, 0x0D);
     cc2500WriteReg(CC2500_19_FOCCFG, 0x16);
+    if (!inBindState) {
+        cc2500WriteReg(CC2500_03_FIFOTHR,  0x14);
+    }
     delay(10);
 }
 
@@ -334,7 +339,7 @@ rx_spi_received_e frSkySpiDataReceived(uint8_t *packet)
     case STATE_BIND_TUNING:
        if (tuneRx(packet)) {
             initGetBind();
-            initialiseData(1);
+            initialiseData(true);
 
             protocolState = STATE_BIND_BINDING1;
         }
@@ -378,6 +383,15 @@ rx_spi_received_e frSkySpiDataReceived(uint8_t *packet)
     return ret;
 }
 
+rx_spi_received_e frSkySpiProcessFrame(uint8_t *packet)
+{
+    if (processFrame) {
+        return processFrame(packet);
+    }
+
+    return RX_SPI_RECEIVED_NONE;
+}
+
 void frSkySpiSetRcData(uint16_t *rcData, const uint8_t *payload)
 {
     setRcData(rcData, payload);
@@ -425,6 +439,9 @@ bool frSkySpiInit(const rxSpiConfig_t *rxSpiConfig, rxRuntimeConfig_t *rxRuntime
         rxRuntimeConfig->channelCount = RC_CHANNEL_COUNT_FRSKY_X;
 
         handlePacket = frSkyXHandlePacket;
+#if defined(USE_RX_FRSKY_SPI_TELEMETRY) && defined(USE_TELEMETRY_SMARTPORT)
+        processFrame = frSkyXProcessFrame;
+#endif
         setRcData = frSkyXSetRcData;
         frSkyXInit(spiProtocol);
 

src/main/rx/cc2500_frsky_shared.h

@@ -51,6 +51,6 @@ extern uint8_t listLength;
 extern uint32_t missingPackets;
 extern timeDelta_t timeoutUs;
 
-void initialiseData(uint8_t adr);
+void initialiseData(bool inBindState);
 
 void nextChannel(uint8_t skip);

src/main/rx/cc2500_frsky_x.c

@@ -141,16 +141,21 @@ typedef struct telemetryPayload_s {
 static telemetryData_t telemetryTxBuffer[TELEMETRY_SEQUENCE_LENGTH];
 #endif
 
+static telemetryBuffer_t telemetryRxBuffer[TELEMETRY_SEQUENCE_LENGTH];
+
 static telemetrySequenceMarker_t responseToSend;
 
 #if defined(USE_RX_FRSKY_SPI_TELEMETRY)
 static uint8_t frame[20];
+
 #if defined(USE_TELEMETRY_SMARTPORT)
 static uint8_t telemetryOutWriter;
 
 static uint8_t telemetryOutBuffer[TELEMETRY_OUT_BUFFER_SIZE];
 
 static bool telemetryEnabled = false;
+
+static uint8_t remoteProcessedId = 0;
 #endif
 #endif // USE_RX_FRSKY_SPI_TELEMETRY
 
@@ -320,14 +325,10 @@ bool isValidPacket(const uint8_t *packet)
 rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const protocolState)
 {
     static unsigned receiveTelemetryRetryCount = 0;
-    static timeMs_t pollingTimeMs = 0;
     static bool skipChannels = true;
 
-    static uint8_t remoteProcessedId = 0;
     static uint8_t remoteAckId = 0;
 
-    static uint8_t remoteToProcessIndex = 0;
-
     static timeUs_t packetTimerUs;
 
     static bool frameReceived;
@@ -335,8 +336,6 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro
     static uint8_t channelsToSkip = 1;
     static uint32_t packetErrors = 0;
 
-    static telemetryBuffer_t telemetryRxBuffer[TELEMETRY_SEQUENCE_LENGTH];
-
 #if defined(USE_RX_FRSKY_SPI_TELEMETRY)
     static bool telemetryReceived = false;
 #endif
@@ -346,7 +345,7 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro
     switch (*protocolState) {
     case STATE_STARTING:
         listLength = 47;
-        initialiseData(0);
+        initialiseData(false);
         *protocolState = STATE_UPDATE;
         nextChannel(1);
         cc2500Strobe(CC2500_SRX);
@@ -369,10 +368,12 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro
         // here FS code could be
     case STATE_DATA:
         if (cc2500getGdo() && (frameReceived == false)){
+            bool packetOk = false;
             uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
             if (ccLen >= packetLength) {
                 cc2500ReadFifo(packet, packetLength);
                 if (isValidPacket(packet)) {
+                    packetOk = true;
                     missingPackets = 0;
                     timeoutUs = 1;
                     receiveDelayUs = 0;
@@ -430,7 +431,9 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro
                     }
 
                     if (receiveTelemetryRetryCount >= 5) {
+#if defined(USE_RX_FRSKY_SPI_TELEMETRY) && defined(USE_TELEMETRY_SMARTPORT)
                          remoteProcessedId = TELEMETRY_SEQUENCE_LENGTH - 1;
+#endif
                          remoteAckId = TELEMETRY_SEQUENCE_LENGTH - 1;
                          for (unsigned i = 0; i < TELEMETRY_SEQUENCE_LENGTH; i++) {
                              telemetryRxBuffer[i].needsProcessing = false;
@@ -447,6 +450,9 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro
                     DEBUG_SET(DEBUG_RX_FRSKY_SPI, DEBUG_DATA_BAD_FRAME, packetErrors);
                 }
             }
+            if (!packetOk) {
+                cc2500Strobe(CC2500_SRX);
+            }
         }
         if (telemetryReceived) {
             if (cmpTimeUs(micros(), packetTimerUs) > receiveDelayUs) { // if received or not received in this time sent telemetry data
@@ -478,35 +484,12 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro
 
 #if defined(USE_TELEMETRY_SMARTPORT)
             if (telemetryEnabled) {
-                bool clearToSend = false;
+                ret |= RX_SPI_ROCESSING_REQUIRED;
-                timeMs_t now = millis();
-                smartPortPayload_t *payload = NULL;
-                if ((now - pollingTimeMs) > 24) {
-                    pollingTimeMs = now;
-
-                    clearToSend = true;
-                } else {
-                    uint8_t remoteToProcessId = (remoteProcessedId + 1) % TELEMETRY_SEQUENCE_LENGTH;
-                    while (telemetryRxBuffer[remoteToProcessId].needsProcessing && !payload) {
-                        while (remoteToProcessIndex < telemetryRxBuffer[remoteToProcessId].data.dataLength && !payload) {
-                            payload = smartPortDataReceive(telemetryRxBuffer[remoteToProcessId].data.data[remoteToProcessIndex], &clearToSend, frSkyXCheckQueueEmpty, false);
-                            remoteToProcessIndex = remoteToProcessIndex + 1;
-                        }
-
-                        if (remoteToProcessIndex == telemetryRxBuffer[remoteToProcessId].data.dataLength) {
-                            remoteToProcessIndex = 0;
-                            telemetryRxBuffer[remoteToProcessId].needsProcessing = false;
-                            remoteProcessedId = remoteToProcessId;
-                            remoteToProcessId = (remoteProcessedId + 1) % TELEMETRY_SEQUENCE_LENGTH;
-                        }
-                    }
-                }
-                processSmartPortTelemetry(payload, &clearToSend, NULL);
             }
 #endif
             *protocolState = STATE_RESUME;
             if (frameReceived) {
-                ret = RX_SPI_RECEIVED_DATA;
+                ret |= RX_SPI_RECEIVED_DATA;
             }
         }
 
@@ -544,6 +527,44 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro
     return ret;
 }
 
+#if defined(USE_RX_FRSKY_SPI_TELEMETRY) && defined(USE_TELEMETRY_SMARTPORT)
+rx_spi_received_e frSkyXProcessFrame(uint8_t * const packet)
+{
+    static timeMs_t pollingTimeMs = 0;
+    static uint8_t remoteToProcessIndex = 0;
+
+    UNUSED(packet);
+
+    bool clearToSend = false;
+    timeMs_t now = millis();
+    smartPortPayload_t *payload = NULL;
+    if ((now - pollingTimeMs) > 24) {
+        pollingTimeMs = now;
+
+        clearToSend = true;
+    } else {
+        uint8_t remoteToProcessId = (remoteProcessedId + 1) % TELEMETRY_SEQUENCE_LENGTH;
+        while (telemetryRxBuffer[remoteToProcessId].needsProcessing && !payload) {
+            while (remoteToProcessIndex < telemetryRxBuffer[remoteToProcessId].data.dataLength && !payload) {
+                payload = smartPortDataReceive(telemetryRxBuffer[remoteToProcessId].data.data[remoteToProcessIndex], &clearToSend, frSkyXCheckQueueEmpty, false);
+                remoteToProcessIndex = remoteToProcessIndex + 1;
+            }
+
+            if (remoteToProcessIndex == telemetryRxBuffer[remoteToProcessId].data.dataLength) {
+                remoteToProcessIndex = 0;
+                telemetryRxBuffer[remoteToProcessId].needsProcessing = false;
+                remoteProcessedId = remoteToProcessId;
+                remoteToProcessId = (remoteProcessedId + 1) % TELEMETRY_SEQUENCE_LENGTH;
+            }
+        }
+    }
+
+    processSmartPortTelemetry(payload, &clearToSend, NULL);
+
+    return RX_SPI_RECEIVED_NONE;
+}
+#endif
+
 void frSkyXInit(const rx_spi_protocol_e spiProtocol)
 {
     switch(spiProtocol) {

src/main/rx/cc2500_frsky_x.h

@@ -29,3 +29,4 @@ void frSkyXSetRcData(uint16_t *rcData, const uint8_t *payload);
 
 void frSkyXInit(const rx_spi_protocol_e spiProtocol);
 rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const protocolState);
+rx_spi_received_e frSkyXProcessFrame(uint8_t * const packet);

src/main/rx/rx_spi.c

@@ -57,10 +57,12 @@ STATIC_UNIT_TESTED uint8_t rxSpiNewPacketAvailable; // set true when a new packe
 
 typedef bool (*protocolInitFnPtr)(const rxSpiConfig_t *rxSpiConfig, rxRuntimeConfig_t *rxRuntimeConfig);
 typedef rx_spi_received_e (*protocolDataReceivedFnPtr)(uint8_t *payload);
+typedef rx_spi_received_e (*protocolProcessFrameFnPtr)(uint8_t *payload);
 typedef void (*protocolSetRcDataFromPayloadFnPtr)(uint16_t *rcData, const uint8_t *payload);
 
 static protocolInitFnPtr protocolInit;
 static protocolDataReceivedFnPtr protocolDataReceived;
+static protocolProcessFrameFnPtr protocolProcessFrame;
 static protocolSetRcDataFromPayloadFnPtr protocolSetRcDataFromPayload;
 
 STATIC_UNIT_TESTED uint16_t rxSpiReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channel)
@@ -137,6 +139,7 @@ STATIC_UNIT_TESTED bool rxSpiSetProtocol(rx_spi_protocol_e protocol)
         protocolInit = frSkySpiInit;
         protocolDataReceived = frSkySpiDataReceived;
         protocolSetRcDataFromPayload = frSkySpiSetRcData;
+        protocolProcessFrame = frSkySpiProcessFrame;
 
         break;
 #endif // USE_RX_FRSKY_SPI
@@ -175,11 +178,39 @@ static uint8_t rxSpiFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
 {
     UNUSED(rxRuntimeConfig);
 
-    if (protocolDataReceived(rxSpiPayload) == RX_SPI_RECEIVED_DATA) {
+    uint8_t status = RX_FRAME_PENDING;
+
+    rx_spi_received_e result = protocolDataReceived(rxSpiPayload);
+
+    if (result & RX_SPI_RECEIVED_DATA) {
         rxSpiNewPacketAvailable = true;
-        return RX_FRAME_COMPLETE;
+        status = RX_FRAME_COMPLETE;
     }
-    return RX_FRAME_PENDING;
+
+    if (result & RX_SPI_ROCESSING_REQUIRED) {
+        status |= RX_FRAME_PROCESSING_REQUIRED;
+    }
+
+    return status;
+}
+
+static bool rxSpiProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig)
+{
+    UNUSED(rxRuntimeConfig);
+
+    if (protocolProcessFrame) {
+        rx_spi_received_e result = protocolProcessFrame(rxSpiPayload);
+
+        if (result & RX_SPI_RECEIVED_DATA) {
+            rxSpiNewPacketAvailable = true;
+        }
+
+        if (result & RX_SPI_ROCESSING_REQUIRED) {
+            return false;
+        }
+    }
+
+    return true;
 }
 
 /*
@@ -201,6 +232,7 @@ bool rxSpiInit(const rxSpiConfig_t *rxSpiConfig, rxRuntimeConfig_t *rxRuntimeCon
 
     rxRuntimeConfig->rcReadRawFn = rxSpiReadRawRC;
     rxRuntimeConfig->rcFrameStatusFn = rxSpiFrameStatus;
+    rxRuntimeConfig->rcProcessFrameFn = rxSpiProcessFrame;
 
     return ret;
 }

src/main/rx/rx_spi.h

@@ -47,8 +47,9 @@ typedef enum {
 
 typedef enum {
     RX_SPI_RECEIVED_NONE = 0,
-    RX_SPI_RECEIVED_BIND,
+    RX_SPI_RECEIVED_BIND = (1 << 0),
-    RX_SPI_RECEIVED_DATA
+    RX_SPI_RECEIVED_DATA = (1 << 1),
+    RX_SPI_ROCESSING_REQUIRED = (1 << 2),
 } rx_spi_received_e;
 
 // RC channels in AETR order