From 63c0fd0eb9831be0a2a64195ee67cba63c916ecf Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Tue, 7 Feb 2017 16:44:51 +0000
Subject: [PATCH] Fix to jumbo frame handling, taken from betaflight

---
 src/main/msp/msp_serial.c | 17 +++++++++--------
 src/main/msp/msp_serial.h |  2 +-
 2 files changed, 10 insertions(+), 9 deletions(-)

diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c
index 0031458880..19f41f2ac6 100644
--- a/src/main/msp/msp_serial.c
+++ b/src/main/msp/msp_serial.c
@@ -127,20 +127,21 @@ static int 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
+    uint8_t hdr[8] = {'$', 'M', packet->result == MSP_RESULT_ERROR ? '!' : '>', mspLen, packet->cmd};
+    int hdrLen = 5;
+#define CHECKSUM_STARTPOS 3  // checksum starts from mspLen 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;
+        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);
     if (len > 0) {
         serialWriteBuf(msp->port, sbufPtr(&packet->buf), len);
         checksum = mspSerialChecksumBuf(checksum, sbufPtr(&packet->buf), len);
     }
-    serialWrite(msp->port, checksum);
+    serialWriteBuf(msp->port, &checksum, 1);
     serialEndWrite(msp->port);
     return sizeof(hdr) + len + 1; // header, data, and checksum
 }
diff --git a/src/main/msp/msp_serial.h b/src/main/msp/msp_serial.h
index c0304b5b8c..c6faf448df 100644
--- a/src/main/msp/msp_serial.h
+++ b/src/main/msp/msp_serial.h
@@ -19,7 +19,7 @@
 
 #include "msp/msp.h"
 
-// Each MSP port requires state and a receive buffer, revisit this default if someone needs more than 2 MSP ports.
+// Each MSP port requires state and a receive buffer, revisit this default if someone needs more than 3 MSP ports.
 #define MAX_MSP_PORT_COUNT 3
 
 typedef enum {