src/main/common/huffman.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include "platform.h"
  22
  23 #ifdef USE_HUFFMAN
  24
  25 #include "huffman.h"
  26
  27 int huffmanEncodeBuf(uint8_t *outBuf, int outBufLen, const uint8_t *inBuf, int inLen, const huffmanTable_t *huffmanTable)
  28 {
  29     int ret = 0;
  30
  31     uint8_t *outByte = outBuf;
  32     *outByte = 0;
  33     uint8_t outBit = 0x80;
  34
  35     for (int ii = 0; ii < inLen; ++ii) {
  36         const int huffCodeLen = huffmanTable[*inBuf].codeLen;
  37         const uint16_t huffCode = huffmanTable[*inBuf].code << 4;
  38         ++inBuf;
  39         uint16_t testBit = 0x8000;
  40
  41         for (int jj = 0; jj < huffCodeLen; ++jj) {
  42             if (huffCode & testBit) {
  43                 *outByte |= outBit;
  44             }
  45
  46             testBit >>= 1;
  47             outBit >>= 1;
  48             if (outBit == 0) {
  49                 outBit = 0x80;
  50                 ++outByte;
  51                 *outByte = 0;
  52                 ++ret;
  53             }
  54
  55             if (ret >= outBufLen && ii < inLen - 1 && jj < huffCodeLen - 1) {
  56                 return -1;
  57             }
  58         }
  59     }
  60     if (outBit != 0x80) {
  61         // ensure last character in output buffer is counted
  62         ++ret;
  63     }
  64     return ret;
  65 }
  66
  67 int huffmanEncodeBufStreaming(huffmanState_t *state, const uint8_t *inBuf, int inLen, const huffmanTable_t *huffmanTable)
  68 {
  69     uint8_t *savedOutBytePtr = state->outByte;
  70     uint8_t savedOutByte = *savedOutBytePtr;
  71
  72     for (const uint8_t *pos = inBuf, *end = inBuf + inLen; pos < end; ++pos) {
  73         const int huffCodeLen = huffmanTable[*pos].codeLen;
  74         const uint16_t huffCode = huffmanTable[*pos].code << 4;
  75         uint16_t testBit = 0x8000;
  76
  77         for (int jj = 0; jj < huffCodeLen; ++jj) {
  78             if (huffCode & testBit) {
  79                 *state->outByte |= state->outBit;
  80             }
  81
  82             testBit >>= 1;
  83             state->outBit >>= 1;
  84             if (state->outBit == 0) {
  85                 state->outBit = 0x80;
  86                 ++state->outByte;
  87                 *state->outByte = 0;
  88                 ++state->bytesWritten;
  89             }
  90
  91             // if buffer is filled and we haven't finished compressing
  92             if (state->bytesWritten >= state->outBufLen && (pos < end - 1 || jj < huffCodeLen - 1)) {
  93                 // restore savedOutByte
  94                 *savedOutBytePtr = savedOutByte;
  95                 return -1;
  96             }
  97         }
  98     }
  99
 100     return 0;
 101 }
 102
 103 #endif