Merge pull request #1161 from mikeller/msp_dataflash_jumbo_frames

Implemented support for MSP jumbo frames, switched dataflash reading to be using jumbo frames.

src/main/config/config.c

@@ -308,7 +308,7 @@ void resetSerialConfig(serialConfig_t *serialConfig)
 
     for (index = 0; index < SERIAL_PORT_COUNT; index++) {
         serialConfig->portConfigs[index].identifier = serialPortIdentifiers[index];
-        serialConfig->portConfigs[index].msp_baudrateIndex = BAUD_115200;
+        serialConfig->portConfigs[index].msp_baudrateIndex = BAUD_500000;
         serialConfig->portConfigs[index].gps_baudrateIndex = BAUD_57600;
         serialConfig->portConfigs[index].telemetry_baudrateIndex = BAUD_AUTO;
         serialConfig->portConfigs[index].blackbox_baudrateIndex = BAUD_115200;

src/main/io/serial.c

@@ -85,7 +85,7 @@ const serialPortIdentifier_e serialPortIdentifiers[SERIAL_PORT_COUNT] = {
 
 static uint8_t serialPortCount;
 
-const uint32_t baudRates[] = {0, 9600, 19200, 38400, 57600, 115200, 230400, 250000}; // see baudRate_e
+const uint32_t baudRates[] = {0, 9600, 19200, 38400, 57600, 115200, 230400, 250000, 500000, 1000000}; // see baudRate_e
 
 #define BAUD_RATE_COUNT (sizeof(baudRates) / sizeof(baudRates[0]))
 

src/main/io/serial.h

@@ -46,6 +46,8 @@ typedef enum {
     BAUD_115200,
     BAUD_230400,
     BAUD_250000,
+    BAUD_500000,
+    BAUD_1000000,
 } baudRate_e;
 
 extern const uint32_t baudRates[];

src/main/io/serial_msp.c

@@ -195,6 +195,10 @@ STATIC_UNIT_TESTED bufWriter_t *writer;
 
 #define RATEPROFILE_MASK (1 << 7)
 
+#define JUMBO_FRAME_SIZE_LIMIT 255
+
+#define DATAFLASH_BUFFER_SIZE 4096
+
 static void serialize8(uint8_t a)
 {
     bufWriterAppend(writer, a);
@@ -232,7 +236,7 @@ static uint32_t read32(void)
     return t;
 }
 
-static void headSerialResponse(uint8_t err, uint8_t responseBodySize)
+static void headSerialResponse(uint8_t err, uint16_t responseBodySize)
 {
     serialBeginWrite(mspSerialPort);
 
@@ -240,11 +244,18 @@ static void headSerialResponse(uint8_t err, uint8_t responseBodySize)
     serialize8('M');
     serialize8(err ? '!' : '>');
     currentPort->checksum = 0;               // start calculating a new checksum
-    serialize8(responseBodySize);
+    if (responseBodySize < JUMBO_FRAME_SIZE_LIMIT) {
+        serialize8(responseBodySize);
+    } else {
+        serialize8(JUMBO_FRAME_SIZE_LIMIT);
+    }
     serialize8(currentPort->cmdMSP);
+    if (responseBodySize >= JUMBO_FRAME_SIZE_LIMIT) {
+        serialize16(responseBodySize);
+    }
 }
 
-static void headSerialReply(uint8_t responseBodySize)
+static void headSerialReply(uint16_t responseBodySize)
 {
     headSerialResponse(0, responseBodySize);
 }
@@ -400,25 +411,38 @@ static void serializeDataflashSummaryReply(void)
 }
 
 #ifdef USE_FLASHFS
-static void serializeDataflashReadReply(uint32_t address, uint8_t size)
+static void serializeDataflashReadReply(uint32_t address, uint16_t size, bool useLegacyFormat)
 {
-    uint8_t buffer[128];
+    static uint8_t buffer[DATAFLASH_BUFFER_SIZE];
-    int bytesRead;
 
     if (size > sizeof(buffer)) {
         size = sizeof(buffer);
     }
 
-    headSerialReply(4 + size);
-
-    serialize32(address);
-
     // bytesRead will be lower than that requested if we reach end of volume
-    bytesRead = flashfsReadAbs(address, buffer, size);
+    int bytesRead = flashfsReadAbs(address, buffer, size);
 
-    for (int i = 0; i < bytesRead; i++) {
+    if (useLegacyFormat) {
+        headSerialReply(sizeof(uint32_t) + size);
+
+        serialize32(address);
+    } else {
+        headSerialReply(sizeof(uint32_t) + sizeof(uint16_t) + bytesRead);
+
+        serialize32(address);
+        serialize16(bytesRead);
+    }
+
+    int i;
+    for (i = 0; i < bytesRead; i++) {
         serialize8(buffer[i]);
     }
+
+    if (useLegacyFormat) {
+        for (; i < size; i++) {
+            serialize8(0);
+        }
+    }
 }
 #endif
 
@@ -1162,8 +1186,17 @@ static bool processOutCommand(uint8_t cmdMSP)
     case MSP_DATAFLASH_READ:
         {
             uint32_t readAddress = read32();
+            uint16_t readLength;
+            bool useLegacyFormat;
+            if (currentPort->dataSize >= sizeof(uint32_t) + sizeof(uint16_t)) {
+                readLength = read16();
+                useLegacyFormat = false;
+            } else {
+                readLength = 128; 
+                useLegacyFormat = true;
+            }
 
-            serializeDataflashReadReply(readAddress, 128);
+            serializeDataflashReadReply(readAddress, readLength, useLegacyFormat);
         }
         break;
 #endif