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 <stdbool.h>
  22 #include <stdint.h>
  23
  24 #include "platform.h"
  25
  26 #ifdef USE_HUFFMAN
  27
  28 #include "huffman.h"
  29
  30
  31 int huffmanEncodeBuf(uint8_t *outBuf, int outBufLen, const uint8_t *inBuf, int inLen, const huffmanTable_t *huffmanTable)
  32 {
  33     int ret = 0;
  34
  35     uint8_t *outByte = outBuf;
  36     *outByte = 0;
  37     uint8_t outBit = 0x80;
  38
  39     for (int ii = 0; ii < inLen; ++ii) {
  40         const int huffCodeLen = huffmanTable[*inBuf].codeLen;
  41         const uint16_t huffCode = huffmanTable[*inBuf].code;
  42         ++inBuf;
  43         uint16_t testBit = 0x8000;
  44
  45         for (int jj = 0; jj < huffCodeLen; ++jj) {
  46             if (huffCode & testBit) {
  47                 *outByte |= outBit;
  48             }
  49
  50             testBit >>= 1;
  51             outBit >>= 1;
  52             if (outBit == 0) {
  53                 outBit = 0x80;
  54                 ++outByte;
  55                 *outByte = 0;
  56                 ++ret;
  57             }
  58
  59             if (ret >= outBufLen && ii < inLen - 1 && jj < huffCodeLen - 1) {
  60                 return -1;
  61             }
  62         }
  63     }
  64     if (outBit != 0x80) {
  65         // ensure last character in output buffer is counted
  66         ++ret;
  67     }
  68     return ret;
  69 }
  70
  71 int huffmanEncodeBufStreaming(huffmanState_t *state, const uint8_t *inBuf, int inLen, const huffmanTable_t *huffmanTable)
  72 {
  73     uint8_t *savedOutBytePtr = state->outByte;
  74     uint8_t savedOutByte = *savedOutBytePtr;
  75
  76     for (const uint8_t *pos = inBuf, *end = inBuf + inLen; pos < end; ++pos) {
  77         const int huffCodeLen = huffmanTable[*pos].codeLen;
  78         const uint16_t huffCode = huffmanTable[*pos].code;
  79         uint16_t testBit = 0x8000;
  80
  81         for (int jj = 0; jj < huffCodeLen; ++jj) {
  82             if (huffCode & testBit) {
  83                 *state->outByte |= state->outBit;
  84             }
  85
  86             testBit >>= 1;
  87             state->outBit >>= 1;
  88             if (state->outBit == 0) {
  89                 state->outBit = 0x80;
  90                 ++state->outByte;
  91                 *state->outByte = 0;
  92                 ++state->bytesWritten;
  93             }
  94
  95             // if buffer is filled and we haven't finished compressing
  96             if (state->bytesWritten >= state->outBufLen && (pos < end - 1 || jj < huffCodeLen - 1)) {
  97                 // restore savedOutByte
  98                 *savedOutBytePtr = savedOutByte;
  99                 return -1;
 100             }
 101         }
 102     }
 103
 104     return 0;
 105 }
 106
 107 #endif