Merge pull request #1330 from martinbudden/bf_msp_streambuf

Converted MSP to use streambuf

src/main/msp/msp.h

@@ -17,6 +17,8 @@
 
 #pragma once
 
+#include "common/streambuf.h"
+
 // return positive for ACK, negative on error, zero for no reply
 typedef enum {
     MSP_RESULT_ACK = 1,
@@ -24,8 +26,12 @@ typedef enum {
     MSP_RESULT_NO_REPLY = 0
 } mspResult_e;
 
+typedef struct mspPacket_s {
+    sbuf_t buf;
+    int16_t cmd;
+    int16_t result;
+} mspPacket_t;
 
 struct serialPort_s;
 typedef void (*mspPostProcessFnPtr)(struct serialPort_s *port); // msp post process function, used for gracefully handling reboots, etc.
-struct mspPort_s;
+typedef mspResult_e (*mspProcessCommandFnPtr)(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn);
-typedef mspResult_e (*mspProcessCommandFnPtr)(struct mspPort_s *mspPort, mspPostProcessFnPtr *mspPostProcessFn);

src/main/msp/msp_serial.c

@@ -21,22 +21,18 @@
 
 #include "platform.h"
 
+#include "common/streambuf.h"
 #include "common/utils.h"
 
-#include "drivers/buf_writer.h"
 #include "drivers/serial.h"
 
-#include "fc/runtime_config.h"
-
 #include "io/serial.h"
 
 #include "msp/msp.h"
 #include "msp/msp_serial.h"
 
-
 static mspProcessCommandFnPtr mspProcessCommandFn;
 static mspPort_t mspPorts[MAX_MSP_PORT_COUNT];
-bufWriter_t *writer;
 
 
 static void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort)
@@ -78,7 +74,7 @@ void mspSerialReleasePortIfAllocated(serialPort_t *serialPort)
     }
 }
 
-bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c)
+static bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c)
 {
     if (mspPort->c_state == MSP_IDLE) {
         if (c == '$') {
@@ -93,12 +89,10 @@ bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c)
     } else if (mspPort->c_state == MSP_HEADER_ARROW) {
         if (c > MSP_PORT_INBUF_SIZE) {
             mspPort->c_state = MSP_IDLE;
-
         } else {
             mspPort->dataSize = c;
             mspPort->offset = 0;
             mspPort->checksum = 0;
-            mspPort->indRX = 0;
             mspPort->checksum ^= c;
             mspPort->c_state = MSP_HEADER_SIZE;
         }
@@ -119,12 +113,64 @@ bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c)
     return true;
 }
 
-static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *mspPort)
+static uint8_t mspSerialChecksumBuf(uint8_t checksum, const uint8_t *data, int len)
 {
-    mspPostProcessFnPtr mspPostProcessFn = NULL;
+    while (len-- > 0) {
-    mspProcessCommandFn(mspPort, &mspPostProcessFn);
+        checksum ^= *data++;
+    }
+    return checksum;
+}
 
-    mspPort->c_state = MSP_IDLE;
+#define JUMBO_FRAME_SIZE_LIMIT 255
+
+static void mspSerialEncode(mspPort_t *msp, mspPacket_t *packet)
+{
+    serialBeginWrite(msp->port);
+    const int len = sbufBytesRemaining(&packet->buf);
+    const int mspLen = len < JUMBO_FRAME_SIZE_LIMIT ? len : JUMBO_FRAME_SIZE_LIMIT;
+    const uint8_t hdr[5] = {'$', 'M', packet->result == MSP_RESULT_ERROR ? '!' : '>', mspLen, packet->cmd};
+    serialWriteBuf(msp->port, hdr, sizeof(hdr));
+    uint8_t checksum = mspSerialChecksumBuf(0, hdr + 3, 2); // checksum starts from len field
+    if (len >= JUMBO_FRAME_SIZE_LIMIT) {
+        serialWrite(msp->port, len & 0xff);
+        checksum ^= len & 0xff;
+        serialWrite(msp->port, (len >> 8) & 0xff);
+        checksum ^= (len >> 8) & 0xff;
+    }
+    if (len > 0) {
+        serialWriteBuf(msp->port, sbufPtr(&packet->buf), len);
+        checksum = mspSerialChecksumBuf(checksum, sbufPtr(&packet->buf), len);
+    }
+    serialWrite(msp->port, checksum);
+    serialEndWrite(msp->port);
+}
+
+static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp)
+{
+    static uint8_t outBuf[MSP_PORT_OUTBUF_SIZE];
+
+    mspPacket_t reply = {
+        .buf = { .ptr = outBuf, .end = ARRAYEND(outBuf), },
+        .cmd = -1,
+        .result = 0,
+    };
+    uint8_t *outBufHead = reply.buf.ptr;
+
+    mspPacket_t command = {
+        .buf = { .ptr = msp->inBuf, .end = msp->inBuf + msp->dataSize, },
+        .cmd = msp->cmdMSP,
+        .result = 0,
+    };
+
+    mspPostProcessFnPtr mspPostProcessFn = NULL;
+    const mspResult_e status = mspProcessCommandFn(&command, &reply, &mspPostProcessFn);
+
+    if (status != MSP_RESULT_NO_REPLY) {
+        sbufSwitchToReader(&reply.buf, outBufHead); // change streambuf direction
+        mspSerialEncode(msp, &reply);
+    }
+
+    msp->c_state = MSP_IDLE;
     return mspPostProcessFn;
 }
 
@@ -140,11 +186,6 @@ void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData)
         if (!mspPort->port) {
             continue;
         }
-
-        // Big enough to fit a MSP_STATUS in one write.
-        uint8_t buf[sizeof(bufWriter_t) + 20];
-        writer = bufWriterInit(buf, sizeof(buf), (bufWrite_t)serialWriteBufShim, mspPort->port);
-
         mspPostProcessFnPtr mspPostProcessFn = NULL;
         while (serialRxBytesWaiting(mspPort->port)) {
 
@@ -160,9 +201,6 @@ void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData)
                 break; // process one command at a time so as not to block.
             }
         }
-
-        bufWriterFlush(writer);
-
         if (mspPostProcessFn) {
             waitForSerialPortToFinishTransmitting(mspPort->port);
             mspPostProcessFn(mspPort->port);
@@ -176,3 +214,4 @@ void mspSerialInit(mspProcessCommandFnPtr mspProcessCommandFnToUse)
     memset(mspPorts, 0, sizeof(mspPorts));
     mspSerialAllocatePorts();
 }
+

src/main/msp/msp_serial.h

@@ -17,7 +17,6 @@
 
 #pragma once
 
-
 #include "msp/msp.h"
 
 // Each MSP port requires state and a receive buffer, revisit this default if someone needs more than 2 MSP ports.
@@ -38,7 +37,14 @@ typedef enum {
     MSP_SKIP_NON_MSP_DATA
 } mspEvaluateNonMspData_e;
 
-#define MSP_PORT_INBUF_SIZE 64
+#define MSP_PORT_INBUF_SIZE 192
+#ifdef USE_FLASHFS
+#define MSP_PORT_DATAFLASH_BUFFER_SIZE 4096
+#define MSP_PORT_DATAFLASH_INFO_SIZE 16
+#define MSP_PORT_OUTBUF_SIZE (MSP_PORT_DATAFLASH_BUFFER_SIZE + MSP_PORT_DATAFLASH_INFO_SIZE)
+#else
+#define MSP_PORT_OUTBUF_SIZE 256
+#endif
 
 struct serialPort_s;
 typedef struct mspPort_s {
@@ -46,16 +52,12 @@ typedef struct mspPort_s {
     uint8_t offset;
     uint8_t dataSize;
     uint8_t checksum;
-    uint8_t indRX;
-    uint8_t inBuf[MSP_PORT_INBUF_SIZE];
-    mspState_e c_state;
     uint8_t cmdMSP;
+    mspState_e c_state;
+    uint8_t inBuf[MSP_PORT_INBUF_SIZE];
 } mspPort_t;
 
 
-struct bufWriter_s;
-extern struct bufWriter_s *writer;
-
 void mspSerialInit(mspProcessCommandFnPtr mspProcessCommandFn);
 void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData);
 void mspSerialAllocatePorts(void);