From c987f268b79de1f22fc3dcb9e6ea3103af2163ef Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Tue, 15 Nov 2016 12:32:58 +0000
Subject: [PATCH] Fixed up CRSF CRC checking. Fixed telemetry port mask

---
 src/main/rx/crsf.c                | 11 ++++++-----
 src/main/rx/crsf.h                |  2 ++
 src/main/telemetry/telemetry.h    |  3 +--
 src/test/unit/rx_crsf_unittest.cc | 27 +++++++++++++++++++++++++--
 4 files changed, 34 insertions(+), 9 deletions(-)

diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c
index ccab8a7b58..4eb92c8a41 100644
--- a/src/main/rx/crsf.c
+++ b/src/main/rx/crsf.c
@@ -90,7 +90,7 @@ typedef struct crsfPayloadRcChannelsPacked_s crsfPayloadRcChannelsPacked_t;
 
 
 // Receive ISR callback, called back from serial port
-static void crsfDataReceive(uint16_t c)
+STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c)
 {
     static uint8_t crsfFramePosition = 0;
     static uint32_t crsfFrameStartAt = 0;
@@ -98,7 +98,7 @@ static void crsfDataReceive(uint16_t c)
 
     const int32_t crsfFrameTime = now - crsfFrameStartAt;
 #ifdef DEBUG_CRSF_PACKETS
-        debug[2] = crsfFrameTime;
+    debug[2] = crsfFrameTime;
 #endif
 
     if (crsfFrameTime > (long)(CRSF_TIME_NEEDED_PER_FRAME_US + 500)) {
@@ -109,11 +109,12 @@ static void crsfDataReceive(uint16_t c)
         crsfFrameStartAt = now;
     }
     // assume frame is 5 bytes long until we have received the frame length
-    const int frameLength = crsfFramePosition < 3 ? 5 : crsfFrame.frame.frameLength;
+    // full frame length includes the length of the address and framelength fields
+    const int fullFrameLength = crsfFramePosition < 3 ? 5 : crsfFrame.frame.frameLength + CRSF_FRAME_LENGTH_ADDRESS + CRSF_FRAME_LENGTH_FRAMELENGTH;
 
-    if (crsfFramePosition < frameLength) {
+    if (crsfFramePosition < fullFrameLength) {
         crsfFrame.bytes[crsfFramePosition++] = (uint8_t)c;
-        crsfFrameDone = crsfFramePosition < frameLength ? false : true;
+        crsfFrameDone = crsfFramePosition < fullFrameLength ? false : true;
     }
 }
 
diff --git a/src/main/rx/crsf.h b/src/main/rx/crsf.h
index ea87d45b2b..096ba185dc 100644
--- a/src/main/rx/crsf.h
+++ b/src/main/rx/crsf.h
@@ -37,6 +37,8 @@ enum {
     CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE = 10,
     CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE = 22, // 11 bits per channel * 16 channels = 22 bytes.
     CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE = 6,
+    CRSF_FRAME_LENGTH_ADDRESS = 1, // length of ADDRESS field
+    CRSF_FRAME_LENGTH_FRAMELENGTH = 1, // length of FRAMELENGTH field
     CRSF_FRAME_LENGTH_TYPE = 1, // length of TYPE field
     CRSF_FRAME_LENGTH_CRC = 1, // length of CRC field
     CRSF_FRAME_LENGTH_TYPE_CRC = 2 // length of TYPE and CRC fields combined
diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h
index c730ae0dec..7a96631c3a 100644
--- a/src/main/telemetry/telemetry.h
+++ b/src/main/telemetry/telemetry.h
@@ -59,5 +59,4 @@ bool telemetryDetermineEnabledState(portSharing_e portSharing);
 
 void telemetryUseConfig(telemetryConfig_t *telemetryConfig);
 
-#define TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_LTM)
-
+#define TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_CRSF)
diff --git a/src/test/unit/rx_crsf_unittest.cc b/src/test/unit/rx_crsf_unittest.cc
index 3df35cad7f..781637d66a 100644
--- a/src/test/unit/rx_crsf_unittest.cc
+++ b/src/test/unit/rx_crsf_unittest.cc
@@ -34,6 +34,7 @@ extern "C" {
     #include "rx/rx.h"
     #include "rx/crsf.h"
 
+    void crsfDataReceive(uint16_t c);
     uint8_t crsfFrameCRC(void);
     uint8_t crsfFrameStatus(void);
     uint16_t crsfReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan);
@@ -41,6 +42,8 @@ extern "C" {
     extern bool crsfFrameDone;
     extern crsfFrame_t crsfFrame;
     extern uint32_t crsfChannelData[CRSF_MAX_CHANNEL];
+
+    uint32_t dummyTimeUs;
 }
 
 #include "unittest_macros.h"
@@ -209,7 +212,7 @@ TEST(CrossFireTest, TestCapturedData)
     const crsfRcChannelsFrame_t *framePtr = (const crsfRcChannelsFrame_t*)capturedData;
     crsfFrame = *(const crsfFrame_t*)framePtr;
     crsfFrameDone = true;
-     uint8_t status = crsfFrameStatus();
+    uint8_t status = crsfFrameStatus();
     EXPECT_EQ(RX_FRAME_COMPLETE, status);
     EXPECT_EQ(false, crsfFrameDone);
     EXPECT_EQ(RX_FRAME_COMPLETE, status);
@@ -246,12 +249,32 @@ TEST(CrossFireTest, TestCapturedData)
     crc = crsfFrameCRC();
     EXPECT_EQ(crc, crsfFrame.frame.payload[CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE]);
 }
+
+
+TEST(CrossFireTest, TestcrsfDataReceive)
+{
+    crsfFrameDone = false;
+    const uint8_t *pData = capturedData;
+    for (unsigned int ii = 0; ii < sizeof(crsfRcChannelsFrame_t); ++ii) {
+        crsfDataReceive(*pData++);
+    }
+    EXPECT_EQ(true, crsfFrameDone);
+    EXPECT_EQ(CRSF_ADDRESS_BROADCAST, crsfFrame.frame.deviceAddress);
+    EXPECT_EQ(CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC, crsfFrame.frame.frameLength);
+    EXPECT_EQ(CRSF_FRAMETYPE_RC_CHANNELS_PACKED, crsfFrame.frame.type);
+    uint8_t crc = crsfFrameCRC();
+    for (int ii = 0; ii < CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE; ++ii) {
+        EXPECT_EQ(capturedData[ii + 3], crsfFrame.frame.payload[ii]);
+    }
+    EXPECT_EQ(crc, crsfFrame.frame.payload[CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE]);
+}
+
 // STUBS
 
 extern "C" {
 
 int16_t debug[DEBUG16_VALUE_COUNT];
-uint32_t micros(void) {return 0;}
+uint32_t micros(void) {return dummyTimeUs;}
 serialPort_t *openSerialPort(serialPortIdentifier_e, serialPortFunction_e, serialReceiveCallbackPtr, uint32_t, portMode_t, portOptions_t)
 {
     return NULL;