diff --git a/make/source.mk b/make/source.mk index 40dc3fea0e..7e51f757c7 100644 --- a/make/source.mk +++ b/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 \ diff --git a/src/main/common/huffman.c b/src/main/common/huffman.c index fb2da5329a..d9bed7968a 100644 --- a/src/main/common/huffman.c +++ b/src/main/common/huffman.c @@ -18,6 +18,10 @@ #include #include +#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) { + + 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 diff --git a/src/main/common/huffman.h b/src/main/common/huffman.h index d08044ebb0..05306e7379 100644 --- a/src/main/common/huffman.h +++ b/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); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 457b8f9987..1d62702e05 100644 --- a/src/main/fc/fc_msp.c +++ b/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,26 +303,73 @@ 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); - 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); + // legacy format does not support compression + const uint8_t compressionMethod = (!allowCompression || useLegacyFormat) ? NO_COMPRESSION : HUFFMAN; - if (useLegacyFormat) { - // pad the buffer with zeros - for (int i = bytesRead; i < size; i++) { - sbufWriteU8(dst, 0); + if (compressionMethod == NO_COMPRESSION) { + if (!useLegacyFormat) { + // new format supports variable read lengths + sbufWriteU16(dst, readLen); + sbufWriteU8(dst, 0); // placeholder for compression format } + + const int bytesRead = flashfsReadAbs(address, sbufPtr(dst), readLen); + + sbufAdvance(dst, bytesRead); + + if (useLegacyFormat) { + // pad the buffer with zeros + for (int i = bytesRead; i < size; i++) { + 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 @@ -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 diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 1e59d5af70..b0de8205f1 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/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_FLASH_M25P16 +#define USE_FLASHFS +#define USE_FLASH_M25P16 -//#define M25P16_CS_GPIO GPIOB -//#define M25P16_CS_PIN GPIO_Pin_12 -//#define M25P16_SPI_INSTANCE SPI2 +#define M25P16_CS_PIN PB12 +#define M25P16_SPI_INSTANCE SPI2 // SPI1 // PB5 SPI1_MOSI // PB4 SPI1_MISO diff --git a/src/main/target/STM32F3DISCOVERY/target.mk b/src/main/target/STM32F3DISCOVERY/target.mk index 88f8eac34b..6ef9e40192 100644 --- a/src/main/target/STM32F3DISCOVERY/target.mk +++ b/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 \ diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index 80fc5d42d2..d5104588f1 100644 --- a/src/main/target/common_fc_pre.h +++ b/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 diff --git a/src/test/Makefile b/src/test/Makefile index 4d4d94c4c1..55c466b20c 100644 --- a/src/test/Makefile +++ b/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. diff --git a/src/test/unit/huffman_unittest.cc b/src/test/unit/huffman_unittest.cc index c68806b98e..e599eef47c 100644 --- a/src/test/unit/huffman_unittest.cc +++ b/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;