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
new file mode 100644
index 0000000000..d9bed7968a
--- /dev/null
+++ b/src/main/common/huffman.c
@@ -0,0 +1,104 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ */
+
+#include
+#include
+
+#include "platform.h"
+
+#ifdef USE_HUFFMAN
+
+#include "huffman.h"
+
+
+int huffmanEncodeBuf(uint8_t *outBuf, int outBufLen, const uint8_t *inBuf, int inLen, const huffmanTable_t *huffmanTable)
+{
+ int ret = 0;
+
+ 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) {
+ outBit = 0x80;
+ ++outByte;
+ *outByte = 0;
+ ++ret;
+ }
+
+ if (ret >= outBufLen && ii < inLen - 1 && jj < huffCodeLen - 1) {
+ return -1;
+ }
+ }
+ }
+ if (outBit != 0x80) {
+ // ensure last character in output buffer is counted
+ ++ret;
+ }
+ 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
new file mode 100644
index 0000000000..05306e7379
--- /dev/null
+++ b/src/main/common/huffman.h
@@ -0,0 +1,44 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ */
+
+#pragma once
+
+#include
+
+#define HUFFMAN_TABLE_SIZE 257 // 256 characters plus EOF
+typedef struct huffmanTable_s {
+ uint8_t codeLen;
+ 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/common/huffman_table.c b/src/main/common/huffman_table.c
new file mode 100644
index 0000000000..c93111030f
--- /dev/null
+++ b/src/main/common/huffman_table.c
@@ -0,0 +1,285 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ */
+
+#include
+
+#include "huffman.h"
+
+/*
+ * Huffman Table, used to compress a bytestream
+ */
+const huffmanTable_t huffmanTable[HUFFMAN_TABLE_SIZE] = {
+// Len Code Char Bitcode
+ { 2, 0xC000 }, // 0x00 11
+ { 3, 0xA000 }, // 0x01 101
+ { 4, 0x9000 }, // 0x02 1001
+ { 5, 0x8800 }, // 0x03 10001
+ { 5, 0x8000 }, // 0x04 10000
+ { 6, 0x7400 }, // 0x05 011101
+ { 6, 0x7000 }, // 0x06 011100
+ { 6, 0x6C00 }, // 0x07 011011
+ { 6, 0x6800 }, // 0x08 011010
+ { 7, 0x6200 }, // 0x09 0110001
+ { 7, 0x6000 }, // 0x0A 0110000
+ { 7, 0x5E00 }, // 0x0B 0101111
+ { 7, 0x5C00 }, // 0x0C 0101110
+ { 7, 0x5A00 }, // 0x0D 0101101
+ { 7, 0x5800 }, // 0x0E 0101100
+ { 7, 0x5600 }, // 0x0F 0101011
+ { 6, 0x6400 }, // 0x10 011001
+ { 7, 0x5400 }, // 0x11 0101010
+ { 7, 0x5200 }, // 0x12 0101001
+ { 8, 0x5100 }, // 0x13 01010001
+ { 8, 0x5000 }, // 0x14 01010000
+ { 8, 0x4F00 }, // 0x15 01001111
+ { 8, 0x4E00 }, // 0x16 01001110
+ { 8, 0x4D00 }, // 0x17 01001101
+ { 8, 0x4C00 }, // 0x18 01001100
+ { 8, 0x4B00 }, // 0x19 01001011
+ { 8, 0x4A00 }, // 0x1A 01001010
+ { 8, 0x4900 }, // 0x1B 01001001
+ { 8, 0x4800 }, // 0x1C 01001000
+ { 8, 0x4700 }, // 0x1D 01000111
+ { 8, 0x4600 }, // 0x1E 01000110
+ { 8, 0x4500 }, // 0x1F 01000101
+ { 8, 0x4400 }, // 0x20 01000100
+ { 8, 0x4300 }, // 0x21 01000011
+ { 8, 0x4200 }, // 0x22 01000010
+ { 8, 0x4100 }, // 0x23 01000001
+ { 8, 0x4000 }, // 0x24 01000000
+ { 9, 0x3C80 }, // 0x25 001111001
+ { 9, 0x3C00 }, // 0x26 001111000
+ { 9, 0x3B80 }, // 0x27 001110111
+ { 9, 0x3B00 }, // 0x28 001110110
+ { 9, 0x3A80 }, // 0x29 001110101
+ { 9, 0x3A00 }, // 0x2A 001110100
+ { 9, 0x3980 }, // 0x2B 001110011
+ { 9, 0x3900 }, // 0x2C 001110010
+ { 9, 0x3880 }, // 0x2D 001110001
+ { 9, 0x3800 }, // 0x2E 001110000
+ { 9, 0x3780 }, // 0x2F 001101111
+ { 8, 0x3F00 }, // 0x30 00111111
+ { 9, 0x3700 }, // 0x31 001101110
+ { 9, 0x3680 }, // 0x32 001101101
+ { 9, 0x3600 }, // 0x33 001101100
+ { 9, 0x3580 }, // 0x34 001101011
+ { 9, 0x3500 }, // 0x35 001101010
+ { 9, 0x3480 }, // 0x36 001101001
+ { 9, 0x3400 }, // 0x37 001101000
+ { 9, 0x3380 }, // 0x38 001100111
+ { 9, 0x3300 }, // 0x39 001100110
+ { 9, 0x3280 }, // 0x3A 001100101
+ { 9, 0x3200 }, // 0x3B 001100100
+ { 9, 0x3180 }, // 0x3C 001100011
+ { 9, 0x3100 }, // 0x3D 001100010
+ { 9, 0x3080 }, // 0x3E 001100001
+ { 9, 0x3000 }, // 0x3F 001100000
+ { 8, 0x3E00 }, // 0x40 00111110
+ { 9, 0x2F80 }, // 0x41 001011111
+ { 9, 0x2F00 }, // 0x42 001011110
+ { 9, 0x2E80 }, // 0x43 001011101
+ { 9, 0x2E00 }, // 0x44 001011100
+ { 9, 0x2D80 }, // 0x45 001011011
+ { 9, 0x2D00 }, // 0x46 001011010
+ { 9, 0x2C80 }, // 0x47 001011001
+ { 9, 0x2C00 }, // 0x48 001011000
+ { 9, 0x2B80 }, // 0x49 001010111
+ { 10, 0x27C0 }, // 0x4A 0010011111
+ { 10, 0x2780 }, // 0x4B 0010011110
+ { 9, 0x2B00 }, // 0x4C 001010110
+ { 10, 0x2740 }, // 0x4D 0010011101
+ { 10, 0x2700 }, // 0x4E 0010011100
+ { 9, 0x2A80 }, // 0x4F 001010101
+ { 5, 0x7800 }, // 0x50 01111
+ { 9, 0x2A00 }, // 0x51 001010100
+ { 10, 0x26C0 }, // 0x52 0010011011
+ { 10, 0x2680 }, // 0x53 0010011010
+ { 10, 0x2640 }, // 0x54 0010011001
+ { 10, 0x2600 }, // 0x55 0010011000
+ { 10, 0x25C0 }, // 0x56 0010010111
+ { 10, 0x2580 }, // 0x57 0010010110
+ { 10, 0x2540 }, // 0x58 0010010101
+ { 10, 0x2500 }, // 0x59 0010010100
+ { 10, 0x24C0 }, // 0x5A 0010010011
+ { 10, 0x2480 }, // 0x5B 0010010010
+ { 10, 0x2440 }, // 0x5C 0010010001
+ { 10, 0x2400 }, // 0x5D 0010010000
+ { 10, 0x23C0 }, // 0x5E 0010001111
+ { 10, 0x2380 }, // 0x5F 0010001110
+ { 10, 0x2340 }, // 0x60 0010001101
+ { 10, 0x2300 }, // 0x61 0010001100
+ { 10, 0x22C0 }, // 0x62 0010001011
+ { 10, 0x2280 }, // 0x63 0010001010
+ { 10, 0x2240 }, // 0x64 0010001001
+ { 10, 0x2200 }, // 0x65 0010001000
+ { 10, 0x21C0 }, // 0x66 0010000111
+ { 10, 0x2180 }, // 0x67 0010000110
+ { 10, 0x2140 }, // 0x68 0010000101
+ { 10, 0x2100 }, // 0x69 0010000100
+ { 10, 0x20C0 }, // 0x6A 0010000011
+ { 10, 0x2080 }, // 0x6B 0010000010
+ { 10, 0x2040 }, // 0x6C 0010000001
+ { 10, 0x2000 }, // 0x6D 0010000000
+ { 10, 0x1FC0 }, // 0x6E 0001111111
+ { 10, 0x1F80 }, // 0x6F 0001111110
+ { 10, 0x1F40 }, // 0x70 0001111101
+ { 10, 0x1F00 }, // 0x71 0001111100
+ { 10, 0x1EC0 }, // 0x72 0001111011
+ { 10, 0x1E80 }, // 0x73 0001111010
+ { 10, 0x1E40 }, // 0x74 0001111001
+ { 10, 0x1E00 }, // 0x75 0001111000
+ { 10, 0x1DC0 }, // 0x76 0001110111
+ { 10, 0x1D80 }, // 0x77 0001110110
+ { 10, 0x1D40 }, // 0x78 0001110101
+ { 10, 0x1D00 }, // 0x79 0001110100
+ { 10, 0x1CC0 }, // 0x7A 0001110011
+ { 10, 0x1C80 }, // 0x7B 0001110010
+ { 10, 0x1C40 }, // 0x7C 0001110001
+ { 10, 0x1C00 }, // 0x7D 0001110000
+ { 10, 0x1BC0 }, // 0x7E 0001101111
+ { 10, 0x1B80 }, // 0x7F 0001101110
+ { 9, 0x2980 }, // 0x80 001010011
+ { 10, 0x1B40 }, // 0x81 0001101101
+ { 10, 0x1B00 }, // 0x82 0001101100
+ { 10, 0x1AC0 }, // 0x83 0001101011
+ { 10, 0x1A80 }, // 0x84 0001101010
+ { 10, 0x1A40 }, // 0x85 0001101001
+ { 10, 0x1A00 }, // 0x86 0001101000
+ { 10, 0x19C0 }, // 0x87 0001100111
+ { 10, 0x1980 }, // 0x88 0001100110
+ { 10, 0x1940 }, // 0x89 0001100101
+ { 10, 0x1900 }, // 0x8A 0001100100
+ { 10, 0x18C0 }, // 0x8B 0001100011
+ { 10, 0x1880 }, // 0x8C 0001100010
+ { 10, 0x1840 }, // 0x8D 0001100001
+ { 10, 0x1800 }, // 0x8E 0001100000
+ { 10, 0x17C0 }, // 0x8F 0001011111
+ { 10, 0x1780 }, // 0x90 0001011110
+ { 10, 0x1740 }, // 0x91 0001011101
+ { 10, 0x1700 }, // 0x92 0001011100
+ { 10, 0x16C0 }, // 0x93 0001011011
+ { 10, 0x1680 }, // 0x94 0001011010
+ { 10, 0x1640 }, // 0x95 0001011001
+ { 10, 0x1600 }, // 0x96 0001011000
+ { 10, 0x15C0 }, // 0x97 0001010111
+ { 10, 0x1580 }, // 0x98 0001010110
+ { 10, 0x1540 }, // 0x99 0001010101
+ { 10, 0x1500 }, // 0x9A 0001010100
+ { 10, 0x14C0 }, // 0x9B 0001010011
+ { 10, 0x1480 }, // 0x9C 0001010010
+ { 10, 0x1440 }, // 0x9D 0001010001
+ { 10, 0x1400 }, // 0x9E 0001010000
+ { 10, 0x13C0 }, // 0x9F 0001001111
+ { 10, 0x1380 }, // 0xA0 0001001110
+ { 10, 0x1340 }, // 0xA1 0001001101
+ { 10, 0x1300 }, // 0xA2 0001001100
+ { 10, 0x12C0 }, // 0xA3 0001001011
+ { 10, 0x1280 }, // 0xA4 0001001010
+ { 10, 0x1240 }, // 0xA5 0001001001
+ { 10, 0x1200 }, // 0xA6 0001001000
+ { 10, 0x11C0 }, // 0xA7 0001000111
+ { 10, 0x1180 }, // 0xA8 0001000110
+ { 10, 0x1140 }, // 0xA9 0001000101
+ { 10, 0x1100 }, // 0xAA 0001000100
+ { 10, 0x10C0 }, // 0xAB 0001000011
+ { 10, 0x1080 }, // 0xAC 0001000010
+ { 10, 0x1040 }, // 0xAD 0001000001
+ { 10, 0x1000 }, // 0xAE 0001000000
+ { 10, 0x0FC0 }, // 0xAF 0000111111
+ { 10, 0x0F80 }, // 0xB0 0000111110
+ { 10, 0x0F40 }, // 0xB1 0000111101
+ { 10, 0x0F00 }, // 0xB2 0000111100
+ { 10, 0x0EC0 }, // 0xB3 0000111011
+ { 10, 0x0E80 }, // 0xB4 0000111010
+ { 10, 0x0E40 }, // 0xB5 0000111001
+ { 10, 0x0E00 }, // 0xB6 0000111000
+ { 10, 0x0DC0 }, // 0xB7 0000110111
+ { 10, 0x0D80 }, // 0xB8 0000110110
+ { 10, 0x0D40 }, // 0xB9 0000110101
+ { 10, 0x0D00 }, // 0xBA 0000110100
+ { 10, 0x0CC0 }, // 0xBB 0000110011
+ { 10, 0x0C80 }, // 0xBC 0000110010
+ { 10, 0x0C40 }, // 0xBD 0000110001
+ { 10, 0x0C00 }, // 0xBE 0000110000
+ { 10, 0x0BC0 }, // 0xBF 0000101111
+ { 10, 0x0B80 }, // 0xC0 0000101110
+ { 10, 0x0B40 }, // 0xC1 0000101101
+ { 10, 0x0B00 }, // 0xC2 0000101100
+ { 10, 0x0AC0 }, // 0xC3 0000101011
+ { 10, 0x0A80 }, // 0xC4 0000101010
+ { 10, 0x0A40 }, // 0xC5 0000101001
+ { 10, 0x0A00 }, // 0xC6 0000101000
+ { 10, 0x09C0 }, // 0xC7 0000100111
+ { 10, 0x0980 }, // 0xC8 0000100110
+ { 10, 0x0940 }, // 0xC9 0000100101
+ { 10, 0x0900 }, // 0xCA 0000100100
+ { 10, 0x08C0 }, // 0xCB 0000100011
+ { 10, 0x0880 }, // 0xCC 0000100010
+ { 10, 0x0840 }, // 0xCD 0000100001
+ { 10, 0x0800 }, // 0xCE 0000100000
+ { 10, 0x07C0 }, // 0xCF 0000011111
+ { 10, 0x0780 }, // 0xD0 0000011110
+ { 10, 0x0740 }, // 0xD1 0000011101
+ { 10, 0x0700 }, // 0xD2 0000011100
+ { 10, 0x06C0 }, // 0xD3 0000011011
+ { 10, 0x0680 }, // 0xD4 0000011010
+ { 11, 0x0320 }, // 0xD5 00000011001
+ { 10, 0x0640 }, // 0xD6 0000011001
+ { 10, 0x0600 }, // 0xD7 0000011000
+ { 10, 0x05C0 }, // 0xD8 0000010111
+ { 10, 0x0580 }, // 0xD9 0000010110
+ { 10, 0x0540 }, // 0xDA 0000010101
+ { 10, 0x0500 }, // 0xDB 0000010100
+ { 10, 0x04C0 }, // 0xDC 0000010011
+ { 11, 0x0300 }, // 0xDD 00000011000
+ { 10, 0x0480 }, // 0xDE 0000010010
+ { 10, 0x0440 }, // 0xDF 0000010001
+ { 9, 0x2900 }, // 0xE0 001010010
+ { 10, 0x0400 }, // 0xE1 0000010000
+ { 10, 0x03C0 }, // 0xE2 0000001111
+ { 11, 0x02E0 }, // 0xE3 00000010111
+ { 10, 0x0380 }, // 0xE4 0000001110
+ { 11, 0x02C0 }, // 0xE5 00000010110
+ { 11, 0x02A0 }, // 0xE6 00000010101
+ { 11, 0x0280 }, // 0xE7 00000010100
+ { 11, 0x0260 }, // 0xE8 00000010011
+ { 11, 0x0240 }, // 0xE9 00000010010
+ { 11, 0x0220 }, // 0xEA 00000010001
+ { 11, 0x0200 }, // 0xEB 00000010000
+ { 11, 0x01E0 }, // 0xEC 00000001111
+ { 11, 0x01C0 }, // 0xED 00000001110
+ { 11, 0x01A0 }, // 0xEE 00000001101
+ { 10, 0x0340 }, // 0xEF 0000001101
+ { 8, 0x3D00 }, // 0xF0 00111101
+ { 9, 0x2880 }, // 0xF1 001010001
+ { 11, 0x0180 }, // 0xF2 00000001100
+ { 11, 0x0160 }, // 0xF3 00000001011
+ { 11, 0x0140 }, // 0xF4 00000001010
+ { 11, 0x0120 }, // 0xF5 00000001001
+ { 11, 0x0100 }, // 0xF6 00000001000
+ { 11, 0x00E0 }, // 0xF7 00000000111
+ { 11, 0x00C0 }, // 0xF8 00000000110
+ { 12, 0x0010 }, // 0xF9 000000000001
+ { 11, 0x00A0 }, // 0xFA 00000000101
+ { 11, 0x0080 }, // 0xFB 00000000100
+ { 11, 0x0060 }, // 0xFC 00000000011
+ { 11, 0x0040 }, // 0xFD 00000000010
+ { 11, 0x0020 }, // 0xFE 00000000001
+ { 9, 0x2800 }, // 0xFF 001010000
+ { 12, 0x0000 }, // EOF 000000000000
+};
+
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 1b08d58295..cadae46d38 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"
@@ -286,7 +287,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);
@@ -296,26 +302,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
@@ -1185,16 +1238,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
new file mode 100644
index 0000000000..e599eef47c
--- /dev/null
+++ b/src/test/unit/huffman_unittest.cc
@@ -0,0 +1,552 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ */
+
+#include
+
+extern "C" {
+ #include "common/huffman.h"
+}
+
+#include "unittest_macros.h"
+#include "gtest/gtest.h"
+
+#define OUTBUF_LEN 128
+static uint8_t outBuf[OUTBUF_LEN];
+
+/*
+ * Huffman Tree, used to decompress a bytestream.
+ *
+ * The leaf nodes of the Huffman tree are stored in an array.
+ */
+
+#define HUFFMAN_EOF (-1)
+
+#define HUFFMAN_TREE_SIZE 257 // 256 characters plus EOF
+
+typedef struct huffmanTree_s {
+ int16_t value;
+ uint16_t codeLen;
+ uint16_t code;
+} huffmanTree_t;
+
+static const huffmanTree_t huffmanTree[HUFFMAN_TREE_SIZE] = {
+// Char Len Code Bitcode
+ { 0x00, 2, 0x0003 }, // 11
+ { 0x01, 3, 0x0005 }, // 101
+ { 0x02, 4, 0x0009 }, // 1001
+ { 0x03, 5, 0x0011 }, // 10001
+ { 0x04, 5, 0x0010 }, // 10000
+ { 0x50, 5, 0x000F }, // 01111
+ { 0x05, 6, 0x001D }, // 011101
+ { 0x06, 6, 0x001C }, // 011100
+ { 0x07, 6, 0x001B }, // 011011
+ { 0x08, 6, 0x001A }, // 011010
+ { 0x10, 6, 0x0019 }, // 011001
+ { 0x09, 7, 0x0031 }, // 0110001
+ { 0x0A, 7, 0x0030 }, // 0110000
+ { 0x0B, 7, 0x002F }, // 0101111
+ { 0x0C, 7, 0x002E }, // 0101110
+ { 0x0D, 7, 0x002D }, // 0101101
+ { 0x0E, 7, 0x002C }, // 0101100
+ { 0x0F, 7, 0x002B }, // 0101011
+ { 0x11, 7, 0x002A }, // 0101010
+ { 0x12, 7, 0x0029 }, // 0101001
+ { 0x13, 8, 0x0051 }, // 01010001
+ { 0x14, 8, 0x0050 }, // 01010000
+ { 0x15, 8, 0x004F }, // 01001111
+ { 0x16, 8, 0x004E }, // 01001110
+ { 0x17, 8, 0x004D }, // 01001101
+ { 0x18, 8, 0x004C }, // 01001100
+ { 0x19, 8, 0x004B }, // 01001011
+ { 0x1A, 8, 0x004A }, // 01001010
+ { 0x1B, 8, 0x0049 }, // 01001001
+ { 0x1C, 8, 0x0048 }, // 01001000
+ { 0x1D, 8, 0x0047 }, // 01000111
+ { 0x1E, 8, 0x0046 }, // 01000110
+ { 0x1F, 8, 0x0045 }, // 01000101
+ { 0x20, 8, 0x0044 }, // 01000100
+ { 0x21, 8, 0x0043 }, // 01000011
+ { 0x22, 8, 0x0042 }, // 01000010
+ { 0x23, 8, 0x0041 }, // 01000001
+ { 0x24, 8, 0x0040 }, // 01000000
+ { 0x30, 8, 0x003F }, // 00111111
+ { 0x40, 8, 0x003E }, // 00111110
+ { 0xF0, 8, 0x003D }, // 00111101
+ { 0x25, 9, 0x0079 }, // 001111001
+ { 0x26, 9, 0x0078 }, // 001111000
+ { 0x27, 9, 0x0077 }, // 001110111
+ { 0x28, 9, 0x0076 }, // 001110110
+ { 0x29, 9, 0x0075 }, // 001110101
+ { 0x2A, 9, 0x0074 }, // 001110100
+ { 0x2B, 9, 0x0073 }, // 001110011
+ { 0x2C, 9, 0x0072 }, // 001110010
+ { 0x2D, 9, 0x0071 }, // 001110001
+ { 0x2E, 9, 0x0070 }, // 001110000
+ { 0x2F, 9, 0x006F }, // 001101111
+ { 0x31, 9, 0x006E }, // 001101110
+ { 0x32, 9, 0x006D }, // 001101101
+ { 0x33, 9, 0x006C }, // 001101100
+ { 0x34, 9, 0x006B }, // 001101011
+ { 0x35, 9, 0x006A }, // 001101010
+ { 0x36, 9, 0x0069 }, // 001101001
+ { 0x37, 9, 0x0068 }, // 001101000
+ { 0x38, 9, 0x0067 }, // 001100111
+ { 0x39, 9, 0x0066 }, // 001100110
+ { 0x3A, 9, 0x0065 }, // 001100101
+ { 0x3B, 9, 0x0064 }, // 001100100
+ { 0x3C, 9, 0x0063 }, // 001100011
+ { 0x3D, 9, 0x0062 }, // 001100010
+ { 0x3E, 9, 0x0061 }, // 001100001
+ { 0x3F, 9, 0x0060 }, // 001100000
+ { 0x41, 9, 0x005F }, // 001011111
+ { 0x42, 9, 0x005E }, // 001011110
+ { 0x43, 9, 0x005D }, // 001011101
+ { 0x44, 9, 0x005C }, // 001011100
+ { 0x45, 9, 0x005B }, // 001011011
+ { 0x46, 9, 0x005A }, // 001011010
+ { 0x47, 9, 0x0059 }, // 001011001
+ { 0x48, 9, 0x0058 }, // 001011000
+ { 0x49, 9, 0x0057 }, // 001010111
+ { 0x4C, 9, 0x0056 }, // 001010110
+ { 0x4F, 9, 0x0055 }, // 001010101
+ { 0x51, 9, 0x0054 }, // 001010100
+ { 0x80, 9, 0x0053 }, // 001010011
+ { 0xE0, 9, 0x0052 }, // 001010010
+ { 0xF1, 9, 0x0051 }, // 001010001
+ { 0xFF, 9, 0x0050 }, // 001010000
+ { 0x4A, 10, 0x009F }, // 0010011111
+ { 0x4B, 10, 0x009E }, // 0010011110
+ { 0x4D, 10, 0x009D }, // 0010011101
+ { 0x4E, 10, 0x009C }, // 0010011100
+ { 0x52, 10, 0x009B }, // 0010011011
+ { 0x53, 10, 0x009A }, // 0010011010
+ { 0x54, 10, 0x0099 }, // 0010011001
+ { 0x55, 10, 0x0098 }, // 0010011000
+ { 0x56, 10, 0x0097 }, // 0010010111
+ { 0x57, 10, 0x0096 }, // 0010010110
+ { 0x58, 10, 0x0095 }, // 0010010101
+ { 0x59, 10, 0x0094 }, // 0010010100
+ { 0x5A, 10, 0x0093 }, // 0010010011
+ { 0x5B, 10, 0x0092 }, // 0010010010
+ { 0x5C, 10, 0x0091 }, // 0010010001
+ { 0x5D, 10, 0x0090 }, // 0010010000
+ { 0x5E, 10, 0x008F }, // 0010001111
+ { 0x5F, 10, 0x008E }, // 0010001110
+ { 0x60, 10, 0x008D }, // 0010001101
+ { 0x61, 10, 0x008C }, // 0010001100
+ { 0x62, 10, 0x008B }, // 0010001011
+ { 0x63, 10, 0x008A }, // 0010001010
+ { 0x64, 10, 0x0089 }, // 0010001001
+ { 0x65, 10, 0x0088 }, // 0010001000
+ { 0x66, 10, 0x0087 }, // 0010000111
+ { 0x67, 10, 0x0086 }, // 0010000110
+ { 0x68, 10, 0x0085 }, // 0010000101
+ { 0x69, 10, 0x0084 }, // 0010000100
+ { 0x6A, 10, 0x0083 }, // 0010000011
+ { 0x6B, 10, 0x0082 }, // 0010000010
+ { 0x6C, 10, 0x0081 }, // 0010000001
+ { 0x6D, 10, 0x0080 }, // 0010000000
+ { 0x6E, 10, 0x007F }, // 0001111111
+ { 0x6F, 10, 0x007E }, // 0001111110
+ { 0x70, 10, 0x007D }, // 0001111101
+ { 0x71, 10, 0x007C }, // 0001111100
+ { 0x72, 10, 0x007B }, // 0001111011
+ { 0x73, 10, 0x007A }, // 0001111010
+ { 0x74, 10, 0x0079 }, // 0001111001
+ { 0x75, 10, 0x0078 }, // 0001111000
+ { 0x76, 10, 0x0077 }, // 0001110111
+ { 0x77, 10, 0x0076 }, // 0001110110
+ { 0x78, 10, 0x0075 }, // 0001110101
+ { 0x79, 10, 0x0074 }, // 0001110100
+ { 0x7A, 10, 0x0073 }, // 0001110011
+ { 0x7B, 10, 0x0072 }, // 0001110010
+ { 0x7C, 10, 0x0071 }, // 0001110001
+ { 0x7D, 10, 0x0070 }, // 0001110000
+ { 0x7E, 10, 0x006F }, // 0001101111
+ { 0x7F, 10, 0x006E }, // 0001101110
+ { 0x81, 10, 0x006D }, // 0001101101
+ { 0x82, 10, 0x006C }, // 0001101100
+ { 0x83, 10, 0x006B }, // 0001101011
+ { 0x84, 10, 0x006A }, // 0001101010
+ { 0x85, 10, 0x0069 }, // 0001101001
+ { 0x86, 10, 0x0068 }, // 0001101000
+ { 0x87, 10, 0x0067 }, // 0001100111
+ { 0x88, 10, 0x0066 }, // 0001100110
+ { 0x89, 10, 0x0065 }, // 0001100101
+ { 0x8A, 10, 0x0064 }, // 0001100100
+ { 0x8B, 10, 0x0063 }, // 0001100011
+ { 0x8C, 10, 0x0062 }, // 0001100010
+ { 0x8D, 10, 0x0061 }, // 0001100001
+ { 0x8E, 10, 0x0060 }, // 0001100000
+ { 0x8F, 10, 0x005F }, // 0001011111
+ { 0x90, 10, 0x005E }, // 0001011110
+ { 0x91, 10, 0x005D }, // 0001011101
+ { 0x92, 10, 0x005C }, // 0001011100
+ { 0x93, 10, 0x005B }, // 0001011011
+ { 0x94, 10, 0x005A }, // 0001011010
+ { 0x95, 10, 0x0059 }, // 0001011001
+ { 0x96, 10, 0x0058 }, // 0001011000
+ { 0x97, 10, 0x0057 }, // 0001010111
+ { 0x98, 10, 0x0056 }, // 0001010110
+ { 0x99, 10, 0x0055 }, // 0001010101
+ { 0x9A, 10, 0x0054 }, // 0001010100
+ { 0x9B, 10, 0x0053 }, // 0001010011
+ { 0x9C, 10, 0x0052 }, // 0001010010
+ { 0x9D, 10, 0x0051 }, // 0001010001
+ { 0x9E, 10, 0x0050 }, // 0001010000
+ { 0x9F, 10, 0x004F }, // 0001001111
+ { 0xA0, 10, 0x004E }, // 0001001110
+ { 0xA1, 10, 0x004D }, // 0001001101
+ { 0xA2, 10, 0x004C }, // 0001001100
+ { 0xA3, 10, 0x004B }, // 0001001011
+ { 0xA4, 10, 0x004A }, // 0001001010
+ { 0xA5, 10, 0x0049 }, // 0001001001
+ { 0xA6, 10, 0x0048 }, // 0001001000
+ { 0xA7, 10, 0x0047 }, // 0001000111
+ { 0xA8, 10, 0x0046 }, // 0001000110
+ { 0xA9, 10, 0x0045 }, // 0001000101
+ { 0xAA, 10, 0x0044 }, // 0001000100
+ { 0xAB, 10, 0x0043 }, // 0001000011
+ { 0xAC, 10, 0x0042 }, // 0001000010
+ { 0xAD, 10, 0x0041 }, // 0001000001
+ { 0xAE, 10, 0x0040 }, // 0001000000
+ { 0xAF, 10, 0x003F }, // 0000111111
+ { 0xB0, 10, 0x003E }, // 0000111110
+ { 0xB1, 10, 0x003D }, // 0000111101
+ { 0xB2, 10, 0x003C }, // 0000111100
+ { 0xB3, 10, 0x003B }, // 0000111011
+ { 0xB4, 10, 0x003A }, // 0000111010
+ { 0xB5, 10, 0x0039 }, // 0000111001
+ { 0xB6, 10, 0x0038 }, // 0000111000
+ { 0xB7, 10, 0x0037 }, // 0000110111
+ { 0xB8, 10, 0x0036 }, // 0000110110
+ { 0xB9, 10, 0x0035 }, // 0000110101
+ { 0xBA, 10, 0x0034 }, // 0000110100
+ { 0xBB, 10, 0x0033 }, // 0000110011
+ { 0xBC, 10, 0x0032 }, // 0000110010
+ { 0xBD, 10, 0x0031 }, // 0000110001
+ { 0xBE, 10, 0x0030 }, // 0000110000
+ { 0xBF, 10, 0x002F }, // 0000101111
+ { 0xC0, 10, 0x002E }, // 0000101110
+ { 0xC1, 10, 0x002D }, // 0000101101
+ { 0xC2, 10, 0x002C }, // 0000101100
+ { 0xC3, 10, 0x002B }, // 0000101011
+ { 0xC4, 10, 0x002A }, // 0000101010
+ { 0xC5, 10, 0x0029 }, // 0000101001
+ { 0xC6, 10, 0x0028 }, // 0000101000
+ { 0xC7, 10, 0x0027 }, // 0000100111
+ { 0xC8, 10, 0x0026 }, // 0000100110
+ { 0xC9, 10, 0x0025 }, // 0000100101
+ { 0xCA, 10, 0x0024 }, // 0000100100
+ { 0xCB, 10, 0x0023 }, // 0000100011
+ { 0xCC, 10, 0x0022 }, // 0000100010
+ { 0xCD, 10, 0x0021 }, // 0000100001
+ { 0xCE, 10, 0x0020 }, // 0000100000
+ { 0xCF, 10, 0x001F }, // 0000011111
+ { 0xD0, 10, 0x001E }, // 0000011110
+ { 0xD1, 10, 0x001D }, // 0000011101
+ { 0xD2, 10, 0x001C }, // 0000011100
+ { 0xD3, 10, 0x001B }, // 0000011011
+ { 0xD4, 10, 0x001A }, // 0000011010
+ { 0xD6, 10, 0x0019 }, // 0000011001
+ { 0xD7, 10, 0x0018 }, // 0000011000
+ { 0xD8, 10, 0x0017 }, // 0000010111
+ { 0xD9, 10, 0x0016 }, // 0000010110
+ { 0xDA, 10, 0x0015 }, // 0000010101
+ { 0xDB, 10, 0x0014 }, // 0000010100
+ { 0xDC, 10, 0x0013 }, // 0000010011
+ { 0xDE, 10, 0x0012 }, // 0000010010
+ { 0xDF, 10, 0x0011 }, // 0000010001
+ { 0xE1, 10, 0x0010 }, // 0000010000
+ { 0xE2, 10, 0x000F }, // 0000001111
+ { 0xE4, 10, 0x000E }, // 0000001110
+ { 0xEF, 10, 0x000D }, // 0000001101
+ { 0xD5, 11, 0x0019 }, // 00000011001
+ { 0xDD, 11, 0x0018 }, // 00000011000
+ { 0xE3, 11, 0x0017 }, // 00000010111
+ { 0xE5, 11, 0x0016 }, // 00000010110
+ { 0xE6, 11, 0x0015 }, // 00000010101
+ { 0xE7, 11, 0x0014 }, // 00000010100
+ { 0xE8, 11, 0x0013 }, // 00000010011
+ { 0xE9, 11, 0x0012 }, // 00000010010
+ { 0xEA, 11, 0x0011 }, // 00000010001
+ { 0xEB, 11, 0x0010 }, // 00000010000
+ { 0xEC, 11, 0x000F }, // 00000001111
+ { 0xED, 11, 0x000E }, // 00000001110
+ { 0xEE, 11, 0x000D }, // 00000001101
+ { 0xF2, 11, 0x000C }, // 00000001100
+ { 0xF3, 11, 0x000B }, // 00000001011
+ { 0xF4, 11, 0x000A }, // 00000001010
+ { 0xF5, 11, 0x0009 }, // 00000001001
+ { 0xF6, 11, 0x0008 }, // 00000001000
+ { 0xF7, 11, 0x0007 }, // 00000000111
+ { 0xF8, 11, 0x0006 }, // 00000000110
+ { 0xFA, 11, 0x0005 }, // 00000000101
+ { 0xFB, 11, 0x0004 }, // 00000000100
+ { 0xFC, 11, 0x0003 }, // 00000000011
+ { 0xFD, 11, 0x0002 }, // 00000000010
+ { 0xFE, 11, 0x0001 }, // 00000000001
+ { 0xF9, 12, 0x0001 }, // 000000000001
+ { HUFFMAN_EOF, 12, 0x0000 }, // 000000000000
+};
+
+int huffManLenIndex[HUFFMAN_TREE_SIZE];
+
+void huffmanInitDecodeLenIndex(void)
+{
+ // create an index of first code at each possible length
+ for (int ii = 0; ii < HUFFMAN_TREE_SIZE; ++ii) {
+ huffManLenIndex[ii] = -1;
+ }
+ for (int ii = 0; ii < HUFFMAN_TREE_SIZE; ++ii) {
+ if (huffManLenIndex[huffmanTree[ii].codeLen] == -1) {
+ huffManLenIndex[huffmanTree[ii].codeLen] = ii;
+ }
+ }
+}
+
+int huffmanDecodeBuf(uint8_t *outBuf, int outBufLen, const uint8_t *inBuf, int inBufLen, int inBufCharacterCount, const huffmanTree_t *huffmanTree)
+{
+ static bool initialized = false;
+ if (!initialized) {
+ huffmanInitDecodeLenIndex();
+ initialized = true;
+ }
+
+ if (inBufCharacterCount > outBufLen) {
+ return -1;
+ }
+ uint16_t code = 0;
+ int codeLen = 0;
+ int outCount = 0;
+ int inCount = 0;
+ uint8_t testBit = 0x80;
+ bool eof = false;
+ while (!eof && outCount < outBufLen && inCount < inBufLen) {
+ if (outCount == inBufCharacterCount) {
+ // we've exhausted the input stream, discard any odd bits on the end
+ return outCount;
+ }
+ if (inCount >= inBufLen) {
+ return -1;
+ }
+ // get the next bit from the input buffer
+ code <<= 1;
+ ++codeLen;
+ if (*inBuf & testBit) {
+ code |= 0x01;
+ }
+ testBit >>= 1;
+ if (testBit == 0) {
+ testBit = 0x80;
+ ++inBuf;
+ ++inCount;
+ }
+ // check if the code is a leaf node or an interior node
+ if (huffManLenIndex[codeLen] != -1) {
+ // look for the code in the tree, only leaf nodes are stored in the tree
+ for(int ii = huffManLenIndex[codeLen]; (ii < HUFFMAN_TREE_SIZE) && (huffmanTree[ii].codeLen == codeLen); ++ii) {
+ if (huffmanTree[ii].code == code) {
+ // we've found the code, so it is a leaf node
+ const int16_t value = huffmanTree[ii].value;
+ if (value == HUFFMAN_EOF) {
+ eof = true;
+ } else {
+ // output the value
+ *outBuf = (uint8_t)value;
+ ++outBuf;
+ ++outCount;
+ }
+ // reset the code to continue decompressing the input buffer
+ code = 0;
+ codeLen = 0;
+ break;
+ }
+ }
+ }
+ }
+ return outCount;
+}
+
+TEST(HuffmanUnittest, TestHuffmanEncode)
+{
+ #define INBUF_LEN1 3
+ const uint8_t inBuf1[INBUF_LEN1] = {0,1,1};
+ // 11 101 101
+ // 1110 1101
+ // e d
+ int len = huffmanEncodeBuf(outBuf, OUTBUF_LEN, inBuf1, INBUF_LEN1, huffmanTable);
+ EXPECT_EQ(1, len);
+ EXPECT_EQ(0xed, (int)outBuf[0]);
+
+ #define INBUF_LEN2 4
+ const uint8_t inBuf2[INBUF_LEN2] = {0,1,2,3};
+ // 11 101 1001 10001
+ // 1110 1100 1100 01
+ // e c c 8
+ len = huffmanEncodeBuf(outBuf, OUTBUF_LEN, inBuf2, INBUF_LEN2, huffmanTable);
+ EXPECT_EQ(2, len);
+ EXPECT_EQ(0xec, (int)outBuf[0]);
+ EXPECT_EQ(0xc4, (int)outBuf[1]);
+
+ #define INBUF_LEN3 8
+ 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
+ len = huffmanEncodeBuf(outBuf, OUTBUF_LEN, inBuf3, INBUF_LEN3, huffmanTable);
+ EXPECT_EQ(5, len);
+ 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, 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;
+
+ #define HUFF_BUF_LEN1 1
+ #define HUFF_BUF_COUNT1 1
+ const uint8_t inBuf1[HUFF_BUF_LEN1] = {0xc0}; // 11
+ len = huffmanDecodeBuf(outBuf, OUTBUF_LEN, inBuf1, HUFF_BUF_LEN1, HUFF_BUF_COUNT1, huffmanTree);
+ EXPECT_EQ(1, len);
+ EXPECT_EQ(0x00, (int)outBuf[0]);
+ EXPECT_EQ(-1, huffManLenIndex[0]);
+ EXPECT_EQ(-1, huffManLenIndex[1]);
+ EXPECT_EQ(0, huffManLenIndex[2]);
+ EXPECT_EQ(1, huffManLenIndex[3]);
+ EXPECT_EQ(2, huffManLenIndex[4]);
+ EXPECT_EQ(3, huffManLenIndex[5]);
+ EXPECT_EQ(6, huffManLenIndex[6]);
+ EXPECT_EQ(11, huffManLenIndex[7]);
+
+ #define HUFF_BUF_LEN2 1
+ #define HUFF_BUF_COUNT2 3
+ const uint8_t inBuf2[HUFF_BUF_LEN2] = {0xed}; // 11 101 101
+ len = huffmanDecodeBuf(outBuf, OUTBUF_LEN, inBuf2, HUFF_BUF_LEN2, HUFF_BUF_COUNT2, huffmanTree);
+ EXPECT_EQ(3, len);
+ EXPECT_EQ(0x00, (int)outBuf[0]);
+ EXPECT_EQ(0x01, (int)outBuf[1]);
+ EXPECT_EQ(0x01, (int)outBuf[2]);
+
+ #define HUFF_BUF_LEN3 5
+ #define HUFF_BUF_COUNT3 8
+ const uint8_t inBuf3[HUFF_BUF_LEN3] = {0xec, 0xc6, 0x0e, 0xb8, 0xd8};
+ len = huffmanDecodeBuf(outBuf, OUTBUF_LEN, inBuf3, HUFF_BUF_LEN3, HUFF_BUF_COUNT3, huffmanTree);
+ EXPECT_EQ(8, len);
+ EXPECT_EQ(0x00, (int)outBuf[0]);
+ EXPECT_EQ(0x01, (int)outBuf[1]);
+ EXPECT_EQ(0x02, (int)outBuf[2]);
+ EXPECT_EQ(0x03, (int)outBuf[3]);
+ EXPECT_EQ(0x04, (int)outBuf[4]);
+ EXPECT_EQ(0x05, (int)outBuf[5]);
+ EXPECT_EQ(0x06, (int)outBuf[6]);
+ EXPECT_EQ(0x07, (int)outBuf[7]);
+}
+
+// STUBS
+
+extern "C" {
+}