src/main/common/huffman.c

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