Corrected smartport regression issue with msp_shared

Corrected condition where an extra padded zero was added to outgoing msp buffers.
Whitespace tidy and removal of unnecessary condition.
Commented OSD and CMS from STM32F3DISCOVERY target.h
Various improvements to msp_shared (removed byte loops, point arithmetic)
Raised schedule priority of CRSF device and MSP calls
Reworked buffers in msp_shared to enable writes.
Moved msp buffers to msp_shared
Added new custom frames and reworking msp implementation
Implemented CRSF device info reply
Adding crsf ping/pong device info functionality
Changed Colibri FC address to Betaflight address
Implementing MSP in CRSF Telemetry
Decoupled msp functionality from smartport into msp_shared
Moved USE_SERVOS to FLASH>128 per mikeller

src/main/telemetry/msp_shared.c

@@ -0,0 +1,231 @@
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <math.h>
+#include <string.h>
+
+#include "platform.h"
+
+#ifdef TELEMETRY
+
+#include "build/build_config.h"
+
+#include "common/utils.h"
+
+#include "fc/fc_msp.h"
+
+#include "msp/msp.h"
+
+#include "rx/crsf.h"
+#include "rx/msp.h"
+
+#include "telemetry/msp_shared.h"
+#include "telemetry/smartport.h"
+
+#define TELEMETRY_MSP_VERSION    1
+#define TELEMETRY_MSP_VER_SHIFT  5
+#define TELEMETRY_MSP_VER_MASK   (0x7 << TELEMETRY_MSP_VER_SHIFT)
+#define TELEMETRY_MSP_ERROR_FLAG (1 << 5)
+#define TELEMETRY_MSP_START_FLAG (1 << 4)
+#define TELEMETRY_MSP_SEQ_MASK   0x0F
+#define TELEMETRY_MSP_RES_ERROR (-10)
+
+enum {
+    TELEMETRY_MSP_VER_MISMATCH=0,
+    TELEMETRY_MSP_CRC_ERROR=1,
+    TELEMETRY_MSP_ERROR=2
+};
+
+#define REQUEST_BUFFER_SIZE 64
+#define RESPONSE_BUFFER_SIZE 64
+
+STATIC_UNIT_TESTED uint8_t checksum = 0;
+STATIC_UNIT_TESTED mspPackage_t mspPackage;
+static mspRxBuffer_t mspRxBuffer;
+static mspTxBuffer_t mspTxBuffer;
+static mspPacket_t mspRxPacket;
+static mspPacket_t mspTxPacket;
+
+void initSharedMsp() {
+    mspPackage.requestBuffer = (uint8_t *)&mspRxBuffer;
+    mspPackage.requestPacket = &mspRxPacket;
+    mspPackage.requestPacket->buf.ptr = mspPackage.requestBuffer;
+    mspPackage.requestPacket->buf.end = mspPackage.requestBuffer;
+
+    mspPackage.responseBuffer = (uint8_t *)&mspTxBuffer;
+    mspPackage.responsePacket = &mspTxPacket;
+    mspPackage.responsePacket->buf.ptr = mspPackage.responseBuffer;
+    mspPackage.responsePacket->buf.end = mspPackage.responseBuffer;
+}
+
+static void processMspPacket()
+{
+    mspPackage.responsePacket->cmd = 0;
+    mspPackage.responsePacket->result = 0;
+    mspPackage.responsePacket->buf.end = mspPackage.responseBuffer;
+
+    mspPostProcessFnPtr mspPostProcessFn = NULL;
+    if (mspFcProcessCommand(mspPackage.requestPacket, mspPackage.responsePacket, &mspPostProcessFn) == MSP_RESULT_ERROR) {
+        sbufWriteU8(&mspPackage.responsePacket->buf, TELEMETRY_MSP_ERROR);
+    }
+    if (mspPostProcessFn) {
+        mspPostProcessFn(NULL);
+    }
+
+    sbufSwitchToReader(&mspPackage.responsePacket->buf, mspPackage.responseBuffer);
+}
+
+void sendMspErrorResponse(uint8_t error, int16_t cmd)
+{
+    mspPackage.responsePacket->cmd = cmd;
+    mspPackage.responsePacket->result = 0;
+    mspPackage.responsePacket->buf.end = mspPackage.responseBuffer;
+
+    sbufWriteU8(&mspPackage.responsePacket->buf, error);
+    mspPackage.responsePacket->result = TELEMETRY_MSP_RES_ERROR;
+    sbufSwitchToReader(&mspPackage.responsePacket->buf, mspPackage.responseBuffer);
+}
+
+bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd)
+{
+    static uint8_t mspStarted = 0;
+    static uint8_t lastSeq = 0;
+
+    if (sbufBytesRemaining(&mspPackage.responsePacket->buf) > 0) {
+        return false;
+    }
+
+    if (mspStarted == 0) {
+        initSharedMsp();
+    }
+
+    mspPacket_t *packet = mspPackage.requestPacket;
+    sbuf_t *frameBuf = sbufInit(&mspPackage.requestFrame, frameStart, frameEnd);
+    sbuf_t *rxBuf = &mspPackage.requestPacket->buf;
+    uint8_t header = sbufReadU8(frameBuf);
+    uint8_t seqNumber = header & TELEMETRY_MSP_SEQ_MASK;
+    uint8_t version = (header & TELEMETRY_MSP_VER_MASK) >> TELEMETRY_MSP_VER_SHIFT;
+
+    if (version != TELEMETRY_MSP_VERSION) {
+        sendMspErrorResponse(TELEMETRY_MSP_VER_MISMATCH, 0);
+        return true;
+    }
+
+    if (header & TELEMETRY_MSP_START_FLAG) {
+        // first packet in sequence
+        uint8_t mspPayloadSize = sbufReadU8(frameBuf);
+
+        packet->cmd = sbufReadU8(frameBuf);
+        packet->result = 0;
+        packet->buf.ptr = mspPackage.requestBuffer;
+        packet->buf.end = mspPackage.requestBuffer + mspPayloadSize;
+
+        checksum = mspPayloadSize ^ packet->cmd;
+        mspStarted = 1;
+    } else if (!mspStarted) {
+        // no start packet yet, throw this one away
+        return false;
+    } else if (((lastSeq + 1) & TELEMETRY_MSP_SEQ_MASK) != seqNumber) {
+        // packet loss detected!
+        mspStarted = 0;
+        return false;
+    }
+
+    uint8_t bufferBytesRemaining = sbufBytesRemaining(rxBuf);
+    uint8_t frameBytesRemaining = sbufBytesRemaining(frameBuf);
+    uint8_t payload[frameBytesRemaining];
+
+    if (bufferBytesRemaining >= frameBytesRemaining) {
+        sbufReadData(frameBuf, payload, frameBytesRemaining);
+        sbufAdvance(frameBuf, frameBytesRemaining);
+        sbufWriteData(rxBuf, payload, frameBytesRemaining);
+        lastSeq = seqNumber;
+        return false;
+    } else {
+        sbufReadData(frameBuf, payload, bufferBytesRemaining);
+        sbufAdvance(frameBuf, bufferBytesRemaining);
+        sbufWriteData(rxBuf, payload, bufferBytesRemaining);
+        sbufSwitchToReader(rxBuf, mspPackage.requestBuffer);
+        while (sbufBytesRemaining(rxBuf)) {
+            checksum ^= sbufReadU8(rxBuf);
+        }
+
+        if (checksum != *frameBuf->ptr) {
+            mspStarted = 0;
+            sendMspErrorResponse(TELEMETRY_MSP_CRC_ERROR, packet->cmd);
+            return true;
+        }
+    }
+
+    mspStarted = 0;
+    sbufSwitchToReader(rxBuf, mspPackage.requestBuffer);
+    processMspPacket();
+    return true;
+}
+
+bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn)
+{
+    static uint8_t checksum = 0;
+    static uint8_t seq = 0;
+
+    uint8_t payloadOut[payloadSize];
+    sbuf_t payload;
+    sbuf_t *payloadBuf = sbufInit(&payload, payloadOut, payloadOut + payloadSize);
+    sbuf_t *txBuf = &mspPackage.responsePacket->buf;
+
+    // detect first reply packet
+    if (txBuf->ptr == mspPackage.responseBuffer) {
+
+        // header
+        uint8_t head = TELEMETRY_MSP_START_FLAG | (seq++ & TELEMETRY_MSP_SEQ_MASK);
+        if (mspPackage.responsePacket->result < 0) {
+            head |= TELEMETRY_MSP_ERROR_FLAG;
+        }
+        sbufWriteU8(payloadBuf, head);
+
+        uint8_t size = sbufBytesRemaining(txBuf);
+        sbufWriteU8(payloadBuf, size);
+    }
+    else {
+        // header
+        sbufWriteU8(payloadBuf, (seq++ & TELEMETRY_MSP_SEQ_MASK));
+    }
+
+    uint8_t bufferBytesRemaining = sbufBytesRemaining(txBuf);
+    uint8_t payloadBytesRemaining = sbufBytesRemaining(payloadBuf);
+    uint8_t frame[payloadBytesRemaining];
+
+    if (bufferBytesRemaining >= payloadBytesRemaining) {
+
+        sbufReadData(txBuf, frame, payloadBytesRemaining);
+        sbufAdvance(txBuf, payloadBytesRemaining);
+        sbufWriteData(payloadBuf, frame, payloadBytesRemaining);
+        responseFn(payloadOut);
+
+        return true;
+
+    } else {
+
+        sbufReadData(txBuf, frame, bufferBytesRemaining);
+        sbufAdvance(txBuf, bufferBytesRemaining);
+        sbufWriteData(payloadBuf, frame, bufferBytesRemaining);
+        sbufSwitchToReader(txBuf, mspPackage.responseBuffer);
+
+        checksum = sbufBytesRemaining(txBuf) ^ mspPackage.responsePacket->cmd;
+
+        while (sbufBytesRemaining(txBuf)) {
+            checksum ^= sbufReadU8(txBuf);
+        }
+        sbufWriteU8(payloadBuf, checksum);
+
+        while (sbufBytesRemaining(payloadBuf)>1) {
+            sbufWriteU8(payloadBuf, 0);
+        }
+
+    }
+
+    responseFn(payloadOut);
+    return false;
+}
+
+#endif