Implemented streaming Huffman compression in chunks of 256 bytes

make/source.mk

@@ -7,6 +7,8 @@ COMMON_SRC = \
             common/bitarray.c \
             common/encoding.c \
             common/filter.c \
+            common/huffman.c \
+            common/huffman_table.c \
             common/maths.c \
             common/printf.c \
             common/streambuf.c \

src/main/common/huffman.c

@@ -18,6 +18,10 @@
 #include <stdbool.h>
 #include <stdint.h>
 
+#include "platform.h"
+
+#ifdef USE_HUFFMAN
+
 #include "huffman.h"
 
 
@@ -28,15 +32,18 @@ int huffmanEncodeBuf(uint8_t *outBuf, int outBufLen, const uint8_t *inBuf, int i
     uint8_t *outByte = outBuf;
     *outByte = 0;
     uint8_t outBit = 0x80;
+    
     for (int ii = 0; ii < inLen; ++ii) {
         const int huffCodeLen = huffmanTable[*inBuf].codeLen;
         const uint16_t huffCode = huffmanTable[*inBuf].code;
         ++inBuf;
         uint16_t testBit = 0x8000;
+
         for (int jj = 0; jj < huffCodeLen; ++jj) {
             if (huffCode & testBit) {
                 *outByte |= outBit;
             }
+
             testBit >>= 1;
             outBit >>= 1;
             if (outBit == 0) {
@@ -45,7 +52,8 @@ int huffmanEncodeBuf(uint8_t *outBuf, int outBufLen, const uint8_t *inBuf, int i
                 *outByte = 0;
                 ++ret;
             }
-            if (ret >= outBufLen) {
+
+            if (ret >= outBufLen && ii < inLen - 1 && jj < huffCodeLen - 1) {
                 return -1;
             }
         }
@@ -57,3 +65,40 @@ int huffmanEncodeBuf(uint8_t *outBuf, int outBufLen, const uint8_t *inBuf, int i
     return ret;
 }
 
+int huffmanEncodeBufStreaming(huffmanState_t *state, const uint8_t *inBuf, int inLen, const huffmanTable_t *huffmanTable)
+{
+    uint8_t *savedOutBytePtr = state->outByte;
+    uint8_t savedOutByte = *savedOutBytePtr;
+
+    for (const uint8_t *pos = inBuf, *end = inBuf + inLen; pos < end; ++pos) {
+        const int huffCodeLen = huffmanTable[*pos].codeLen;
+        const uint16_t huffCode = huffmanTable[*pos].code;
+        uint16_t testBit = 0x8000;
+
+        for (int jj = 0; jj < huffCodeLen; ++jj) {
+            if (huffCode & testBit) {
+                *state->outByte |= state->outBit;
+            }
+
+            testBit >>= 1;
+            state->outBit >>= 1;
+            if (state->outBit == 0) {
+                state->outBit = 0x80;
+                ++state->outByte;
+                *state->outByte = 0;
+                ++state->bytesWritten;
+            }
+
+            // if buffer is filled and we haven't finished compressing
+            if (state->bytesWritten >= state->outBufLen && (pos < end - 1 || jj < huffCodeLen - 1)) {
+                // restore savedOutByte
+                *savedOutBytePtr = savedOutByte;
+                return -1;
+            }
+        }
+    }
+
+    return 0;
+}
+
+#endif

src/main/common/huffman.h

@@ -25,6 +25,20 @@ typedef struct huffmanTable_s {
     uint16_t    code;
 } huffmanTable_t;
 
+typedef struct huffmanState_s {
+    uint16_t    bytesWritten;
+    uint8_t     *outByte;
+    uint16_t    outBufLen;
+    uint8_t     outBit;
+} huffmanState_t;
+
 extern const huffmanTable_t huffmanTable[HUFFMAN_TABLE_SIZE];
 
+struct huffmanInfo_s {
+    uint16_t uncompressedByteCount;
+};
+
+#define HUFFMAN_INFO_SIZE sizeof(struct huffmanInfo_s)
+
 int huffmanEncodeBuf(uint8_t *outBuf, int outBufLen, const uint8_t *inBuf, int inLen, const huffmanTable_t *huffmanTable);
+int huffmanEncodeBufStreaming(huffmanState_t *state, const uint8_t *inBuf, int inLen, const huffmanTable_t *huffmanTable);

src/main/fc/fc_msp.c

@@ -33,6 +33,7 @@
 #include "common/color.h"
 #include "common/maths.h"
 #include "common/streambuf.h"
+#include "common/huffman.h"
 
 #include "config/config_eeprom.h"
 #include "config/feature.h"
@@ -287,7 +288,12 @@ static void serializeDataflashSummaryReply(sbuf_t *dst)
 }
 
 #ifdef USE_FLASHFS
-static void serializeDataflashReadReply(sbuf_t *dst, uint32_t address, const uint16_t size, bool useLegacyFormat)
+enum compressionType_e {
+    NO_COMPRESSION,
+    HUFFMAN
+};
+
+static void serializeDataflashReadReply(sbuf_t *dst, uint32_t address, const uint16_t size, bool useLegacyFormat, bool allowCompression)
 {
     BUILD_BUG_ON(MSP_PORT_DATAFLASH_INFO_SIZE < 16);
 
@@ -297,19 +303,25 @@ static void serializeDataflashReadReply(sbuf_t *dst, uint32_t address, const uin
         readLen = bytesRemainingInBuf;
     }
     // size will be lower than that requested if we reach end of volume
-    if (readLen > flashfsGetSize() - address) {
+    const uint32_t flashfsSize = flashfsGetSize();
+    if (readLen > flashfsSize - address) {
         // truncate the request
-        readLen = flashfsGetSize() - address;
+        readLen = flashfsSize - address;
     }
     sbufWriteU32(dst, address);
+
+    // legacy format does not support compression
+    const uint8_t compressionMethod = (!allowCompression || useLegacyFormat) ? NO_COMPRESSION : HUFFMAN;
+
+    if (compressionMethod == NO_COMPRESSION) {
         if (!useLegacyFormat) {
             // new format supports variable read lengths
             sbufWriteU16(dst, readLen);
             sbufWriteU8(dst, 0); // placeholder for compression format
         }
 
-    // bytesRead will equal readLen
         const int bytesRead = flashfsReadAbs(address, sbufPtr(dst), readLen);
+
         sbufAdvance(dst, bytesRead);
 
         if (useLegacyFormat) {
@@ -318,6 +330,47 @@ static void serializeDataflashReadReply(sbuf_t *dst, uint32_t address, const uin
                 sbufWriteU8(dst, 0);
             }
         }
+    } else {
+#ifdef USE_HUFFMAN
+        // compress in 256-byte chunks
+        const uint16_t READ_BUFFER_SIZE = 256;
+        uint8_t readBuffer[READ_BUFFER_SIZE];
+
+        huffmanState_t state = {
+            .bytesWritten = 0,
+            .outByte = sbufPtr(dst) + MSP_PORT_DATAFLASH_INFO_SIZE + HUFFMAN_INFO_SIZE,
+            .outBufLen = readLen - HUFFMAN_INFO_SIZE,
+            .outBit = 0x80,
+        };
+        *state.outByte = 0;
+
+        uint16_t bytesReadTotal = 0;
+        // read until output buffer overflows or flash is exhausted
+        while (state.bytesWritten < state.outBufLen && address + bytesReadTotal < flashfsSize) {
+            const int bytesRead = flashfsReadAbs(address + bytesReadTotal, readBuffer,
+                MIN(sizeof(readBuffer), flashfsSize - address - bytesReadTotal));
+
+            const int status = huffmanEncodeBufStreaming(&state, readBuffer, bytesRead, huffmanTable);
+            if (status == -1) {
+                // overflow
+                break;
+            }
+
+            bytesReadTotal += bytesRead;
+        }
+
+        if (state.outBit != 0x80) {
+            ++state.bytesWritten;
+        }
+
+        // header
+        sbufWriteU16(dst, sizeof(uint16_t) + state.bytesWritten);
+        sbufWriteU8(dst, compressionMethod);
+        // payload
+        sbufWriteU16(dst, bytesReadTotal);
+        sbufAdvance(dst, state.bytesWritten);
+#endif
+    }
 }
 #endif // USE_FLASHFS
 #endif // USE_OSD_SLAVE
@@ -1186,16 +1239,20 @@ static void mspFcDataFlashReadCommand(sbuf_t *dst, sbuf_t *src)
     const unsigned int dataSize = sbufBytesRemaining(src);
     const uint32_t readAddress = sbufReadU32(src);
     uint16_t readLength;
+    bool allowCompression = false;
     bool useLegacyFormat;
     if (dataSize >= sizeof(uint32_t) + sizeof(uint16_t)) {
         readLength = sbufReadU16(src);
+        if (sbufBytesRemaining(src)) {
+            allowCompression = sbufReadU8(src);
+        }
         useLegacyFormat = false;
     } else {
         readLength = 128;
         useLegacyFormat = true;
     }
 
-    serializeDataflashReadReply(dst, readAddress, readLength, useLegacyFormat);
+    serializeDataflashReadReply(dst, readAddress, readLength, useLegacyFormat, allowCompression);
 }
 #endif
 

src/main/target/STM32F3DISCOVERY/target.h

@@ -56,12 +56,11 @@
 //#define SD_CS_PIN               PB12
 //#define SD_SPI_INSTANCE         SPI2
 
-//#define USE_FLASHFS
+#define USE_FLASHFS
-//#define USE_FLASH_M25P16
+#define USE_FLASH_M25P16
 
-//#define M25P16_CS_GPIO          GPIOB
+#define M25P16_CS_PIN           PB12
-//#define M25P16_CS_PIN           GPIO_Pin_12
+#define M25P16_SPI_INSTANCE     SPI2
-//#define M25P16_SPI_INSTANCE     SPI2
 // SPI1
 // PB5  SPI1_MOSI
 // PB4  SPI1_MISO

src/main/target/STM32F3DISCOVERY/target.mk

@@ -1,5 +1,5 @@
 F3_TARGETS  += $(TARGET)
-FEATURES    = VCP SDCARD
+FEATURES    = VCP SDCARD ONBOARDFLASH
 
 TARGET_SRC = \
             drivers/accgyro/accgyro_adxl345.c \

src/main/target/common_fc_pre.h

@@ -125,6 +125,7 @@
 #define VTX_SMARTAUDIO
 #define VTX_TRAMP
 #define USE_CAMERA_CONTROL
+#define USE_HUFFMAN
 
 #ifdef USE_SERIALRX_SPEKTRUM
 #define USE_SPEKTRUM_BIND

src/test/Makefile

@@ -235,6 +235,13 @@ rcsplit_unitest_DEFINES := \
 		USE_UART3 \
 		USE_RCSPLIT \
 
+huffman_unittest_SRC := \
+		$(USER_DIR)/common/huffman.c \
+		$(USER_DIR)/common/huffman_table.c
+
+huffman_unittest_DEFINES := \
+		USE_HUFFMAN
+
 # Please tweak the following variable definitions as needed by your
 # project, except GTEST_HEADERS, which you can use in your own targets
 # but shouldn't modify.

src/test/unit/huffman_unittest.cc

@@ -417,6 +417,92 @@ TEST(HuffmanUnittest, TestHuffmanEncode)
     EXPECT_EQ(0xd8, (int)outBuf[4]);
 }
 
+TEST(HuffmanUnittest, TestHuffmanEncodeStreaming)
+{
+    #define INBUF_LEN1  3
+    #define INBUF_LEN1_CHUNK1 2
+    #define INBUF_LEN1_CHUNK2 (INBUF_LEN1 - INBUF_LEN1_CHUNK1)
+    const uint8_t inBuf1[INBUF_LEN1] = {0,1,1};
+    // 11 101 101
+    // 1110 1101
+    // e    d
+    huffmanState_t state1 = {
+        .bytesWritten = 0,
+        .outByte = outBuf,
+        .outBufLen = OUTBUF_LEN,
+        .outBit = 0x80,
+    };
+    *state1.outByte = 0;
+    int status = huffmanEncodeBufStreaming(&state1, inBuf1, INBUF_LEN1_CHUNK1, huffmanTable);
+    EXPECT_EQ(0, status);
+    status = huffmanEncodeBufStreaming(&state1, inBuf1 + INBUF_LEN1_CHUNK1, INBUF_LEN1_CHUNK2, huffmanTable);
+    EXPECT_EQ(0, status);
+    if (state1.outBit != 0x80) {
+        ++state1.bytesWritten;
+    }
+
+    EXPECT_EQ(1, state1.bytesWritten);
+    EXPECT_EQ(0xed, (int)outBuf[0]);
+
+    #define INBUF_LEN2 4
+    #define INBUF_LEN2_CHUNK1 1
+    #define INBUF_LEN2_CHUNK2 1
+    #define INBUF_LEN2_CHUNK3 2
+    const uint8_t inBuf2[INBUF_LEN2] = {0,1,2,3};
+    // 11 101 1001 10001
+    // 1110 1100 1100 01
+    // e    c    c    8
+    huffmanState_t state2 = {
+        .bytesWritten = 0,
+        .outByte = outBuf,
+        .outBufLen = OUTBUF_LEN,
+        .outBit = 0x80,
+    };
+    *state2.outByte = 0;
+    status = huffmanEncodeBufStreaming(&state2, inBuf2, INBUF_LEN2_CHUNK1, huffmanTable);
+    EXPECT_EQ(0, status);
+    status = huffmanEncodeBufStreaming(&state2, inBuf2 + INBUF_LEN2_CHUNK1, INBUF_LEN2_CHUNK2, huffmanTable);
+    EXPECT_EQ(0, status);
+    status = huffmanEncodeBufStreaming(&state2, inBuf2 + INBUF_LEN2_CHUNK1 + INBUF_LEN2_CHUNK2, INBUF_LEN2_CHUNK3, huffmanTable);
+    EXPECT_EQ(0, status);
+    if (state2.outBit != 0x80) {
+        ++state2.bytesWritten;
+    }
+
+    EXPECT_EQ(2, state2.bytesWritten);
+    EXPECT_EQ(0xec, (int)outBuf[0]);
+    EXPECT_EQ(0xc4, (int)outBuf[1]);
+
+    #define INBUF_LEN3 8
+    #define INBUF_LEN3_CHUNK1 4
+    #define INBUF_LEN3_CHUNK2 (INBUF_LEN3 - INBUF_LEN3_CHUNK1)
+    const uint8_t inBuf3[INBUF_LEN3] = {0,1,2,3,4,5,6,7};
+    // 11 101 1001 10001 10000 011101 011100 011011
+    // 1110 1100 1100 0110 0000 1110 1011 1000 1101 1
+    // e    c    c    6    0    e    b    8    d    8
+    huffmanState_t state3 = {
+        .bytesWritten = 0,
+        .outByte = outBuf,
+        .outBufLen = OUTBUF_LEN,
+        .outBit = 0x80,
+    };
+    *state3.outByte = 0;
+    status = huffmanEncodeBufStreaming(&state3, inBuf3, INBUF_LEN3_CHUNK1, huffmanTable);
+    EXPECT_EQ(0, status);
+    status = huffmanEncodeBufStreaming(&state3, inBuf3 + INBUF_LEN3_CHUNK1, INBUF_LEN3_CHUNK2, huffmanTable);
+    EXPECT_EQ(0, status);
+    if (state3.outBit != 0x80) {
+        ++state3.bytesWritten;
+    }
+
+    EXPECT_EQ(5, state3.bytesWritten);
+    EXPECT_EQ(0xec, (int)outBuf[0]);
+    EXPECT_EQ(0xc6, (int)outBuf[1]);
+    EXPECT_EQ(0x0e, (int)outBuf[2]);
+    EXPECT_EQ(0xb8, (int)outBuf[3]);
+    EXPECT_EQ(0xd8, (int)outBuf[4]);
+}
+
 TEST(HuffmanUnittest, TestHuffmanDecode)
 {
     int len;