From a332f7a87249a8c430e7e5afcf7937abb246ddf9 Mon Sep 17 00:00:00 2001
From: Damjan Adamic <projectkk2glider@gmail.com>
Date: Wed, 2 Nov 2016 12:56:43 +0100
Subject: [PATCH] =?UTF-8?q?Fixes=20#3964:=20Added=20free=20space=20check?=
 =?UTF-8?q?=20before=20putting=20new=20data=20into=20Lua=20=E2=80=A6=20(#3?=
 =?UTF-8?q?968)?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

* 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                    | 13 ++++++++++---
 radio/src/telemetry/crossfire.cpp   |  2 +-
 radio/src/telemetry/frsky_sport.cpp |  8 ++++----
 3 files changed, 15 insertions(+), 8 deletions(-)

diff --git a/radio/src/fifo.h b/radio/src/fifo.h
index 727294f50..59eb1566e 100644
--- a/radio/src/fifo.h
+++ b/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;
diff --git a/radio/src/telemetry/crossfire.cpp b/radio/src/telemetry/crossfire.cpp
index 1d9d7546d..6bb02b79f 100644
--- a/radio/src/telemetry/crossfire.cpp
+++ b/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]);
diff --git a/radio/src/telemetry/frsky_sport.cpp b/radio/src/telemetry/frsky_sport.cpp
index be3df1bc3..6dacb1a76 100644
--- a/radio/src/telemetry/frsky_sport.cpp
+++ b/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]);
       }
     }