MSPv2 implementation (#3977)

Some refactoring

Refactor CRC calculations; Unsigned type for size and offset

src/main/interface/msp.h

@@ -19,6 +19,17 @@
 
 #include "common/streambuf.h"
 
+#define MSP_V2_FRAME_ID         255
+
+typedef enum {
+    MSP_V1          = 0,
+    MSP_V2_OVER_V1  = 1,
+    MSP_V2_NATIVE   = 2,
+    MSP_VERSION_COUNT
+} mspVersion_e;
+
+#define MSP_VERSION_MAGIC_INITIALIZER { 'M', 'M', 'X' }
+
 // return positive for ACK, negative on error, zero for no reply
 typedef enum {
     MSP_RESULT_ACK = 1,
@@ -35,6 +46,7 @@ typedef enum {
 typedef struct mspPacket_s {
     sbuf_t buf;
     int16_t cmd;
+    uint8_t flags;
     int16_t result;
     uint8_t direction;
 } mspPacket_t;

src/main/interface/msp_protocol.h

@@ -312,7 +312,7 @@
 #define MSP_RESERVE_2            252    //reserved for system usage
 #define MSP_DEBUGMSG             253    //out message         debug string buffer
 #define MSP_DEBUG                254    //out message         debug1,debug2,debug3,debug4
-#define MSP_RESERVE_3            255    //reserved for system usage
+#define MSP_V2_FRAME             255    //MSPv2 payload indicator
 
 // Additional commands that are not compatible with MultiWii
 #define MSP_STATUS_EX            150    //out message         cycletime, errors_count, CPU load, sensor present etc

src/main/msp/msp_serial.c

@@ -25,6 +25,7 @@
 
 #include "common/streambuf.h"
 #include "common/utils.h"
+#include "common/crc.h"
 
 #include "drivers/system.h"
 
@@ -78,52 +79,165 @@ void mspSerialReleasePortIfAllocated(serialPort_t *serialPort)
 
 static bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c)
 {
-    if (mspPort->c_state == MSP_IDLE) {
+    switch (mspPort->c_state) {
-        if (c == '$') {
+        default:
-            mspPort->c_state = MSP_HEADER_START;
+        case MSP_IDLE:      // Waiting for '$' character
-        } else {
+            if (c == '$') {
-            return false;
+                mspPort->mspVersion = MSP_V1;
-        }
+                mspPort->c_state = MSP_HEADER_START;
-    } else if (mspPort->c_state == MSP_HEADER_START) {
+            }
-        mspPort->c_state = (c == 'M') ? MSP_HEADER_M : MSP_IDLE;
+            else {
-    } else if (mspPort->c_state == MSP_HEADER_M) {
+                return false;
-        mspPort->c_state = MSP_IDLE;
+            }
-        switch (c) {
+            break;
-            case '<': // COMMAND
+
-                mspPort->packetType = MSP_PACKET_COMMAND;
+        case MSP_HEADER_START:  // Waiting for 'M' (MSPv1 / MSPv2_over_v1) or 'X' (MSPv2 native)
-                mspPort->c_state = MSP_HEADER_ARROW;
+            switch (c) {
-                break;
+                case 'M':
-            case '>': // REPLY
+                    mspPort->c_state = MSP_HEADER_M;
-                mspPort->packetType = MSP_PACKET_REPLY;
+                    break;
-                mspPort->c_state = MSP_HEADER_ARROW;
+                case 'X':
-                break;
+                    mspPort->c_state = MSP_HEADER_X;
-            default:
+                    break;
-                break;
+                default:
-        }
+                    mspPort->c_state = MSP_IDLE;
-    } else if (mspPort->c_state == MSP_HEADER_ARROW) {
+                    break;
-        if (c > MSP_PORT_INBUF_SIZE) {
+            }
-            mspPort->c_state = MSP_IDLE;
+            break;
-        } else {
+
-            mspPort->dataSize = c;
+        case MSP_HEADER_M:      // Waiting for '<'
-            mspPort->offset = 0;
+            if (c == '<') {
-            mspPort->checksum = 0;
+                mspPort->offset = 0;
-            mspPort->checksum ^= c;
+                mspPort->checksum1 = 0;
-            mspPort->c_state = MSP_HEADER_SIZE;
+                mspPort->checksum2 = 0;
-        }
+                mspPort->c_state = MSP_HEADER_V1;
-    } else if (mspPort->c_state == MSP_HEADER_SIZE) {
+            }
-        mspPort->cmdMSP = c;
+            else {
-        mspPort->checksum ^= c;
+                mspPort->c_state = MSP_IDLE;
-        mspPort->c_state = MSP_HEADER_CMD;
+            }
-    } else if (mspPort->c_state == MSP_HEADER_CMD && mspPort->offset < mspPort->dataSize) {
+            break;
-        mspPort->checksum ^= c;
+
-        mspPort->inBuf[mspPort->offset++] = c;
+        case MSP_HEADER_X:
-    } else if (mspPort->c_state == MSP_HEADER_CMD && mspPort->offset >= mspPort->dataSize) {
+            if (c == '<') {
-        if (mspPort->checksum == c) {
+                mspPort->offset = 0;
-            mspPort->c_state = MSP_COMMAND_RECEIVED;
+                mspPort->checksum2 = 0;
-        } else {
+                mspPort->mspVersion = MSP_V2_NATIVE;
-            mspPort->c_state = MSP_IDLE;
+                mspPort->c_state = MSP_HEADER_V2_NATIVE;
-        }
+            }
+            else {
+                mspPort->c_state = MSP_IDLE;
+            }
+            break;
+
+        case MSP_HEADER_V1:     // Now receive v1 header (size/cmd), this is already checksummable
+            mspPort->inBuf[mspPort->offset++] = c;
+            mspPort->checksum1 ^= c;
+            if (mspPort->offset == sizeof(mspHeaderV1_t)) {
+                mspHeaderV1_t * hdr = (mspHeaderV1_t *)&mspPort->inBuf[0];
+                // Check incoming buffer size limit
+                if (hdr->size > MSP_PORT_INBUF_SIZE) {
+                    mspPort->c_state = MSP_IDLE;
+                }
+                else if (hdr->cmd == MSP_V2_FRAME_ID) {
+                    // MSPv1 payload must be big enough to hold V2 header + extra checksum
+                    if (hdr->size >= sizeof(mspHeaderV2_t) + 1) {
+                        mspPort->mspVersion = MSP_V2_OVER_V1;
+                        mspPort->c_state = MSP_HEADER_V2_OVER_V1;
+                    }
+                    else {
+                        mspPort->c_state = MSP_IDLE;
+                    }
+                }
+                else {
+                    mspPort->dataSize = hdr->size;
+                    mspPort->cmdMSP = hdr->cmd;
+                    mspPort->cmdFlags = 0;
+                    mspPort->offset = 0;                // re-use buffer
+                    mspPort->c_state = mspPort->dataSize > 0 ? MSP_PAYLOAD_V1 : MSP_CHECKSUM_V1;    // If no payload - jump to checksum byte
+                }
+            }
+            break;
+
+        case MSP_PAYLOAD_V1:
+            mspPort->inBuf[mspPort->offset++] = c;
+            mspPort->checksum1 ^= c;
+            if (mspPort->offset == mspPort->dataSize) {
+                mspPort->c_state = MSP_CHECKSUM_V1;
+            }
+            break;
+
+        case MSP_CHECKSUM_V1:
+            if (mspPort->checksum1 == c) {
+                mspPort->c_state = MSP_COMMAND_RECEIVED;
+            } else {
+                mspPort->c_state = MSP_IDLE;
+            }
+            break;
+
+        case MSP_HEADER_V2_OVER_V1:     // V2 header is part of V1 payload - we need to calculate both checksums now
+            mspPort->inBuf[mspPort->offset++] = c;
+            mspPort->checksum1 ^= c;
+            mspPort->checksum2 = crc8_dvb_s2(mspPort->checksum2, c);
+            if (mspPort->offset == (sizeof(mspHeaderV2_t) + sizeof(mspHeaderV1_t))) {
+                mspHeaderV2_t * hdrv2 = (mspHeaderV2_t *)&mspPort->inBuf[sizeof(mspHeaderV1_t)];
+                mspPort->dataSize = hdrv2->size;
+                mspPort->cmdMSP = hdrv2->cmd;
+                mspPort->cmdFlags = hdrv2->flags;
+                mspPort->offset = 0;                // re-use buffer
+                mspPort->c_state = mspPort->dataSize > 0 ? MSP_PAYLOAD_V2_OVER_V1 : MSP_CHECKSUM_V2_OVER_V1;
+            }
+            break;
+
+        case MSP_PAYLOAD_V2_OVER_V1:
+            mspPort->checksum2 = crc8_dvb_s2(mspPort->checksum2, c);
+            mspPort->checksum1 ^= c;
+            mspPort->inBuf[mspPort->offset++] = c;
+
+            if (mspPort->offset == mspPort->dataSize) {
+                mspPort->c_state = MSP_CHECKSUM_V2_OVER_V1;
+            }
+            break;
+
+        case MSP_CHECKSUM_V2_OVER_V1:
+            mspPort->checksum1 ^= c;
+            if (mspPort->checksum2 == c) {
+                mspPort->c_state = MSP_CHECKSUM_V1; // Checksum 2 correct - verify v1 checksum
+            } else {
+                mspPort->c_state = MSP_IDLE;
+            }
+            break;
+
+        case MSP_HEADER_V2_NATIVE:
+            mspPort->inBuf[mspPort->offset++] = c;
+            mspPort->checksum2 = crc8_dvb_s2(mspPort->checksum2, c);
+            if (mspPort->offset == sizeof(mspHeaderV2_t)) {
+                mspHeaderV2_t * hdrv2 = (mspHeaderV2_t *)&mspPort->inBuf[0];
+                mspPort->dataSize = hdrv2->size;
+                mspPort->cmdMSP = hdrv2->cmd;
+                mspPort->cmdFlags = hdrv2->flags;
+                mspPort->offset = 0;                // re-use buffer
+                mspPort->c_state = mspPort->dataSize > 0 ? MSP_PAYLOAD_V2_NATIVE : MSP_CHECKSUM_V2_NATIVE;
+            }
+            break;
+
+        case MSP_PAYLOAD_V2_NATIVE:
+            mspPort->checksum2 = crc8_dvb_s2(mspPort->checksum2, c);
+            mspPort->inBuf[mspPort->offset++] = c;
+
+            if (mspPort->offset == mspPort->dataSize) {
+                mspPort->c_state = MSP_CHECKSUM_V2_NATIVE;
+            }
+            break;
+
+        case MSP_CHECKSUM_V2_NATIVE:
+            if (mspPort->checksum2 == c) {
+                mspPort->c_state = MSP_COMMAND_RECEIVED;
+            } else {
+                mspPort->c_state = MSP_IDLE;
+            }
+            break;
     }
+
     return true;
 }
 
@@ -136,35 +250,117 @@ static uint8_t mspSerialChecksumBuf(uint8_t checksum, const uint8_t *data, int l
 }
 
 #define JUMBO_FRAME_SIZE_LIMIT 255
-
+static int mspSerialSendFrame(mspPort_t *msp, const uint8_t * hdr, int hdrLen, const uint8_t * data, int dataLen, const uint8_t * crc, int crcLen)
-static int mspSerialEncode(mspPort_t *msp, mspPacket_t *packet)
 {
+    // We are allowed to send out the response if
+    //  a) TX buffer is completely empty (we are talking to well-behaving party that follows request-response scheduling;
+    //     this allows us to transmit jumbo frames bigger than TX buffer (serialWriteBuf will block, but for jumbo frames we don't care)
+    //  b) Response fits into TX buffer
+    const int totalFrameLength = hdrLen + dataLen + crcLen;
+    if (!isSerialTransmitBufferEmpty(msp->port) && ((int)serialTxBytesFree(msp->port) < totalFrameLength))
+        return 0;
+
+    // Transmit frame
     serialBeginWrite(msp->port);
-    const int len = sbufBytesRemaining(&packet->buf);
-    const int mspLen = len < JUMBO_FRAME_SIZE_LIMIT ? len : JUMBO_FRAME_SIZE_LIMIT;
-    uint8_t hdr[8] = {
-        '$',
-        'M',
-        packet->result == MSP_RESULT_ERROR ? '!' : packet->direction == MSP_DIRECTION_REPLY ? '>' : '<',
-        mspLen,
-        packet->cmd
-    };
-    int hdrLen = 5;
-#define CHECKSUM_STARTPOS 3  // checksum starts from mspLen field
-    if (len >= JUMBO_FRAME_SIZE_LIMIT) {
-        hdrLen += 2;
-        hdr[5] = len & 0xff;
-        hdr[6] = (len >> 8) & 0xff;
-    }
     serialWriteBuf(msp->port, hdr, hdrLen);
-    uint8_t checksum = mspSerialChecksumBuf(0, hdr + CHECKSUM_STARTPOS, hdrLen - CHECKSUM_STARTPOS);
+    serialWriteBuf(msp->port, data, dataLen);
-    if (len > 0) {
+    serialWriteBuf(msp->port, crc, crcLen);
-        serialWriteBuf(msp->port, sbufPtr(&packet->buf), len);
-        checksum = mspSerialChecksumBuf(checksum, sbufPtr(&packet->buf), len);
-    }
-    serialWriteBuf(msp->port, &checksum, 1);
     serialEndWrite(msp->port);
-    return sizeof(hdr) + len + 1; // header, data, and checksum
+
+    return totalFrameLength;
+}
+
+static int mspSerialEncode(mspPort_t *msp, mspPacket_t *packet, mspVersion_e mspVersion)
+{
+    static const uint8_t mspMagic[MSP_VERSION_COUNT] = MSP_VERSION_MAGIC_INITIALIZER;
+    const int dataLen = sbufBytesRemaining(&packet->buf);
+    uint8_t hdrBuf[16] = { '$', mspMagic[mspVersion], packet->result == MSP_RESULT_ERROR ? '!' : '>'};
+    uint8_t crcBuf[2];
+    uint8_t checksum;
+    int hdrLen = 3;
+    int crcLen = 0;
+
+    #define V1_CHECKSUM_STARTPOS 3
+    if (mspVersion == MSP_V1) {
+        mspHeaderV1_t * hdrV1 = (mspHeaderV1_t *)&hdrBuf[hdrLen];
+        hdrLen += sizeof(mspHeaderV1_t);
+        hdrV1->cmd = packet->cmd;
+
+        // Add JUMBO-frame header if necessary
+        if (dataLen >= JUMBO_FRAME_SIZE_LIMIT) {
+            mspHeaderJUMBO_t * hdrJUMBO = (mspHeaderJUMBO_t *)&hdrBuf[hdrLen];
+            hdrLen += sizeof(mspHeaderJUMBO_t);
+
+            hdrV1->size = JUMBO_FRAME_SIZE_LIMIT;
+            hdrJUMBO->size = dataLen;
+        }
+        else {
+            hdrV1->size = dataLen;
+        }
+
+        // Pre-calculate CRC
+        checksum = mspSerialChecksumBuf(0, hdrBuf + V1_CHECKSUM_STARTPOS, hdrLen - V1_CHECKSUM_STARTPOS);
+        checksum = mspSerialChecksumBuf(checksum, sbufPtr(&packet->buf), dataLen);
+        crcBuf[crcLen++] = checksum;
+    }
+    else if (mspVersion == MSP_V2_OVER_V1) {
+        mspHeaderV1_t * hdrV1 = (mspHeaderV1_t *)&hdrBuf[hdrLen];
+
+        hdrLen += sizeof(mspHeaderV1_t);
+
+        mspHeaderV2_t * hdrV2 = (mspHeaderV2_t *)&hdrBuf[hdrLen];
+        hdrLen += sizeof(mspHeaderV2_t);
+
+        const int v1PayloadSize = sizeof(mspHeaderV2_t) + dataLen + 1;  // MSPv2 header + data payload + MSPv2 checksum
+        hdrV1->cmd = MSP_V2_FRAME_ID;
+
+        // Add JUMBO-frame header if necessary
+        if (v1PayloadSize >= JUMBO_FRAME_SIZE_LIMIT) {
+            mspHeaderJUMBO_t * hdrJUMBO = (mspHeaderJUMBO_t *)&hdrBuf[hdrLen];
+            hdrLen += sizeof(mspHeaderJUMBO_t);
+
+            hdrV1->size = JUMBO_FRAME_SIZE_LIMIT;
+            hdrJUMBO->size = v1PayloadSize;
+        }
+        else {
+            hdrV1->size = v1PayloadSize;
+        }
+
+        // Fill V2 header
+        hdrV2->flags = packet->flags;
+        hdrV2->cmd = packet->cmd;
+        hdrV2->size = dataLen;
+
+        // V2 CRC: only V2 header + data payload
+        checksum = crc8_dvb_s2_update(0, (uint8_t *)hdrV2, sizeof(mspHeaderV2_t));
+        checksum = crc8_dvb_s2_update(checksum, sbufPtr(&packet->buf), dataLen);
+        crcBuf[crcLen++] = checksum;
+
+        // V1 CRC: All headers + data payload + V2 CRC byte
+        checksum = mspSerialChecksumBuf(0, hdrBuf + V1_CHECKSUM_STARTPOS, hdrLen - V1_CHECKSUM_STARTPOS);
+        checksum = mspSerialChecksumBuf(checksum, sbufPtr(&packet->buf), dataLen);
+        checksum = mspSerialChecksumBuf(checksum, crcBuf, crcLen);
+        crcBuf[crcLen++] = checksum;
+    }
+    else if (mspVersion == MSP_V2_NATIVE) {
+        mspHeaderV2_t * hdrV2 = (mspHeaderV2_t *)&hdrBuf[hdrLen];
+        hdrLen += sizeof(mspHeaderV2_t);
+
+        hdrV2->flags = packet->flags;
+        hdrV2->cmd = packet->cmd;
+        hdrV2->size = dataLen;
+
+        checksum = crc8_dvb_s2_update(0, (uint8_t *)hdrV2, sizeof(mspHeaderV2_t));
+        checksum = crc8_dvb_s2_update(checksum, sbufPtr(&packet->buf), dataLen);
+        crcBuf[crcLen++] = checksum;
+    }
+    else {
+        // Shouldn't get here
+        return 0;
+    }
+
+    // Send the frame
+    return mspSerialSendFrame(msp, hdrBuf, hdrLen, sbufPtr(&packet->buf), dataLen, crcBuf, crcLen);
 }
 
 static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp, mspProcessCommandFnPtr mspProcessCommandFn)
@@ -174,6 +370,7 @@ static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp, mspPr
     mspPacket_t reply = {
         .buf = { .ptr = outBuf, .end = ARRAYEND(outBuf), },
         .cmd = -1,
+        .flags = 0,
         .result = 0,
         .direction = MSP_DIRECTION_REPLY,
     };
@@ -182,6 +379,7 @@ static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp, mspPr
     mspPacket_t command = {
         .buf = { .ptr = msp->inBuf, .end = msp->inBuf + msp->dataSize, },
         .cmd = msp->cmdMSP,
+        .flags = msp->cmdFlags,
         .result = 0,
         .direction = MSP_DIRECTION_REQUEST,
     };
@@ -191,7 +389,7 @@ static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp, mspPr
 
     if (status != MSP_RESULT_NO_REPLY) {
         sbufSwitchToReader(&reply.buf, outBufHead); // change streambuf direction
-        mspSerialEncode(msp, &reply);
+        mspSerialEncode(msp, &reply, msp->mspVersion);
     }
 
     return mspPostProcessFn;
@@ -345,7 +543,7 @@ int mspSerialPush(uint8_t cmd, uint8_t *data, int datalen, mspDirection_e direct
             .direction = direction,
         };
 
-        ret = mspSerialEncode(mspPort, &push);
+        ret = mspSerialEncode(mspPort, &push, MSP_V1);
     }
     return ret; // return the number of bytes written
 }

src/main/msp/msp_serial.h

@@ -27,9 +27,20 @@ typedef enum {
     MSP_IDLE,
     MSP_HEADER_START,
     MSP_HEADER_M,
-    MSP_HEADER_ARROW,
+    MSP_HEADER_X,
-    MSP_HEADER_SIZE,
+
-    MSP_HEADER_CMD,
+    MSP_HEADER_V1,
+    MSP_PAYLOAD_V1,
+    MSP_CHECKSUM_V1,
+
+    MSP_HEADER_V2_OVER_V1,
+    MSP_PAYLOAD_V2_OVER_V1,
+    MSP_CHECKSUM_V2_OVER_V1,
+
+    MSP_HEADER_V2_NATIVE,
+    MSP_PAYLOAD_V2_NATIVE,
+    MSP_CHECKSUM_V2_NATIVE,
+
     MSP_COMMAND_RECEIVED
 } mspState_e;
 
@@ -62,18 +73,38 @@ typedef enum {
 #define MSP_PORT_OUTBUF_SIZE 256
 #endif
 
+typedef struct __attribute__((packed)) {
+    uint8_t size;
+    uint8_t cmd;
+} mspHeaderV1_t;
+
+typedef struct __attribute__((packed)) {
+    uint16_t size;
+} mspHeaderJUMBO_t;
+
+typedef struct __attribute__((packed)) {
+    uint8_t  flags;
+    uint16_t cmd;
+    uint16_t size;
+} mspHeaderV2_t;
+
+#define MSP_MAX_HEADER_SIZE     9
+
 struct serialPort_s;
 typedef struct mspPort_s {
     struct serialPort_s *port; // null when port unused.
     timeMs_t lastActivityMs;
     mspPendingSystemRequest_e pendingRequest;
-    uint8_t offset;
-    uint8_t dataSize;
-    uint8_t checksum;
-    uint8_t cmdMSP;
     mspState_e c_state;
     mspPacketType_e packetType;
     uint8_t inBuf[MSP_PORT_INBUF_SIZE];
+    uint16_t cmdMSP;
+    uint8_t cmdFlags;
+    mspVersion_e mspVersion;
+    uint_fast16_t offset;
+    uint_fast16_t dataSize;
+    uint8_t checksum1;
+    uint8_t checksum2;
 } mspPort_t;
 
 void mspSerialInit(void);