Rebased (streamBuf)

src/main/msp/msp_serial.c

@@ -21,23 +21,19 @@
 
 #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 mspPushCommandFnPtr mspPushCommandFn;
 static mspPort_t mspPorts[MAX_MSP_PORT_COUNT];
-bufWriter_t *writer;
 
 
 static void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort)
@@ -79,7 +75,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 == '$') {
@@ -94,12 +90,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;
         }
@@ -120,12 +114,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;
-    mspProcessCommandFn(mspPort, &mspPostProcessFn);
+    while (len-- > 0) {
+        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;
 }
 
@@ -141,11 +187,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)) {
 
@@ -161,9 +202,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);
@@ -178,22 +216,27 @@ void mspSerialInit(mspProcessCommandFnPtr mspProcessCommandFnToUse)
     mspSerialAllocatePorts();
 }
 
-void mspSerialPush(uint8_t cmd, uint8_t *data, int buflen)
+void mspSerialPush(uint8_t cmd, uint8_t *data, int datalen)
 {
+    static uint8_t pushBuf[30];
+
+    mspPacket_t push = {
+        .buf = { .ptr = pushBuf, .end = ARRAYEND(pushBuf), },
+        .cmd = cmd,
+        .result = 0,
+    };
+
     for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
         mspPort_t * const mspPort = &mspPorts[portIndex];
         if (!mspPort->port) {
             continue;
         }
 
-        // Big enough for a OSD line
-        uint8_t buf[sizeof(bufWriter_t) + 30];
+        mspPushCommandFn(&push, data, datalen);
 
-        writer = bufWriterInit(buf, sizeof(buf), (bufWrite_t)serialWriteBufShim, mspPort->port);
+        sbufSwitchToReader(&push.buf, pushBuf);
 
-        mspPushCommandFn(mspPort, cmd, data, buflen);
-
-        bufWriterFlush(writer);
+        mspSerialEncode(mspPort, &push);
     }
 }