Fixes #3964: Added free space check before putting new data into Lua … (#3968)

* Fixes #3964: Added free space check before putting new data into Lua Telemetry Fifo

* Copy-paste error

* Compilation fix

* Another compilation fix

radio/src/fifo.h

@@ -24,6 +24,8 @@
 template <class T, int N>
 class Fifo
 {
+  static_assert((N > 1) & !(N & (N - 1)), "Fifo size must be a power of two!");
+
   public:
     Fifo():
       widx(0),
@@ -57,7 +59,7 @@ class Fifo
       }
     }
 
-    bool isEmpty()
+    bool isEmpty() const
     {
       return (ridx == widx);
     }
@@ -73,12 +75,17 @@ class Fifo
       while (!isEmpty()) {};
     }
 
-    uint32_t size()
+    uint32_t size() const
     {
       return (N + widx - ridx) & (N-1);
     }
 
-    bool probe(T & element)
+    uint32_t hasSpace(uint32_t n) const
+    {
+      return (N > (size() + n));
+    }
+
+    bool probe(T & element) const
     {
       if (isEmpty()) {
         return false;

radio/src/telemetry/crossfire.cpp

@@ -198,7 +198,7 @@ void processCrossfireTelemetryFrame()
 
 #if defined(LUA)
     default:
-      if (luaInputTelemetryFifo) {
+      if (luaInputTelemetryFifo && luaInputTelemetryFifo->hasSpace(telemetryRxBufferCount-2) ) {
         for (uint8_t i=1; i<telemetryRxBufferCount-1; i++) {
           // destination address and CRC are skipped
           luaInputTelemetryFifo->push(telemetryRxBuffer[i]);

radio/src/telemetry/frsky_sport.cpp

@@ -201,13 +201,13 @@ void sportProcessTelemetryPacket(uint8_t * packet)
         }
         else if (id >= DIY_FIRST_ID && id <= DIY_LAST_ID) {
 #if defined(LUA)
-          if (luaInputTelemetryFifo) {
+          if (luaInputTelemetryFifo && luaInputTelemetryFifo->hasSpace(sizeof(SportTelemetryPacket))) {
             SportTelemetryPacket luaPacket;
             luaPacket.physicalId = physicalId;
             luaPacket.primId = primId;
             luaPacket.dataId = id;
             luaPacket.value = data;
-            for (uint8_t i=0; i<sizeof(luaPacket); i++) {
+            for (uint8_t i=0; i<sizeof(SportTelemetryPacket); i++) {
               luaInputTelemetryFifo->push(luaPacket.raw[i]);
             }
           }
@@ -221,13 +221,13 @@ void sportProcessTelemetryPacket(uint8_t * packet)
   }
 #if defined(LUA)
   else if (primId == 0x32) {
-    if (luaInputTelemetryFifo) {
+    if (luaInputTelemetryFifo && luaInputTelemetryFifo->hasSpace(sizeof(SportTelemetryPacket))) {
       SportTelemetryPacket luaPacket;
       luaPacket.physicalId = physicalId;
       luaPacket.primId = primId;
       luaPacket.dataId = id;
       luaPacket.value = data;
-      for (uint8_t i=0; i<sizeof(luaPacket); i++) {
+      for (uint8_t i=0; i<sizeof(SportTelemetryPacket); i++) {
         luaInputTelemetryFifo->push(luaPacket.raw[i]);
       }
     }