123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384 |
- /**
- * Marlin 3D Printer Firmware
- * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
- *
- * Based on Sprinter and grbl.
- * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
- *
- * This program 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.
- *
- * This program 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 this program. If not, see <https://www.gnu.org/licenses/>.
- *
- */
-
- #include "../../inc/MarlinConfigPre.h"
-
- #if ENABLED(BINARY_FILE_TRANSFER)
-
- /**
- * libs/heatshrink/heatshrink_decoder.cpp
- */
- #include "heatshrink_decoder.h"
-
- #include <stdlib.h>
- #include <string.h>
-
- #pragma GCC optimize ("O3")
-
- /* States for the polling state machine. */
- typedef enum {
- HSDS_TAG_BIT, /* tag bit */
- HSDS_YIELD_LITERAL, /* ready to yield literal byte */
- HSDS_BACKREF_INDEX_MSB, /* most significant byte of index */
- HSDS_BACKREF_INDEX_LSB, /* least significant byte of index */
- HSDS_BACKREF_COUNT_MSB, /* most significant byte of count */
- HSDS_BACKREF_COUNT_LSB, /* least significant byte of count */
- HSDS_YIELD_BACKREF /* ready to yield back-reference */
- } HSD_state;
-
- #if HEATSHRINK_DEBUGGING_LOGS
- #include <stdio.h>
- #include <ctype.h>
- #include <assert.h>
- #define LOG(...) fprintf(stderr, __VA_ARGS__)
- #define ASSERT(X) assert(X)
- static const char *state_names[] = {
- "tag_bit",
- "yield_literal",
- "backref_index_msb",
- "backref_index_lsb",
- "backref_count_msb",
- "backref_count_lsb",
- "yield_backref"
- };
- #else
- #define LOG(...) /* no-op */
- #define ASSERT(X) /* no-op */
- #endif
-
- typedef struct {
- uint8_t *buf; /* output buffer */
- size_t buf_size; /* buffer size */
- size_t *output_size; /* bytes pushed to buffer, so far */
- } output_info;
-
- #define NO_BITS ((uint16_t)-1)
-
- /* Forward references. */
- static uint16_t get_bits(heatshrink_decoder *hsd, uint8_t count);
- static void push_byte(heatshrink_decoder *hsd, output_info *oi, uint8_t byte);
-
- #if HEATSHRINK_DYNAMIC_ALLOC
- heatshrink_decoder *heatshrink_decoder_alloc(uint16_t input_buffer_size, uint8_t window_sz2, uint8_t lookahead_sz2) {
- if ((window_sz2 < HEATSHRINK_MIN_WINDOW_BITS) ||
- (window_sz2 > HEATSHRINK_MAX_WINDOW_BITS) ||
- (input_buffer_size == 0) ||
- (lookahead_sz2 < HEATSHRINK_MIN_LOOKAHEAD_BITS) ||
- (lookahead_sz2 >= window_sz2)) {
- return nullptr;
- }
- size_t buffers_sz = (1 << window_sz2) + input_buffer_size;
- size_t sz = sizeof(heatshrink_decoder) + buffers_sz;
- heatshrink_decoder *hsd = HEATSHRINK_MALLOC(sz);
- if (!hsd) return nullptr;
- hsd->input_buffer_size = input_buffer_size;
- hsd->window_sz2 = window_sz2;
- hsd->lookahead_sz2 = lookahead_sz2;
- heatshrink_decoder_reset(hsd);
- LOG("-- allocated decoder with buffer size of %zu (%zu + %u + %u)\n",
- sz, sizeof(heatshrink_decoder), (1 << window_sz2), input_buffer_size);
- return hsd;
- }
-
- void heatshrink_decoder_free(heatshrink_decoder *hsd) {
- size_t buffers_sz = (1 << hsd->window_sz2) + hsd->input_buffer_size;
- size_t sz = sizeof(heatshrink_decoder) + buffers_sz;
- HEATSHRINK_FREE(hsd, sz);
- (void)sz; /* may not be used by free */
- }
- #endif
-
- void heatshrink_decoder_reset(heatshrink_decoder *hsd) {
- size_t buf_sz = 1 << HEATSHRINK_DECODER_WINDOW_BITS(hsd);
- size_t input_sz = HEATSHRINK_DECODER_INPUT_BUFFER_SIZE(hsd);
- memset(hsd->buffers, 0, buf_sz + input_sz);
- hsd->state = HSDS_TAG_BIT;
- hsd->input_size = 0;
- hsd->input_index = 0;
- hsd->bit_index = 0x00;
- hsd->current_byte = 0x00;
- hsd->output_count = 0;
- hsd->output_index = 0;
- hsd->head_index = 0;
- }
-
- /* Copy SIZE bytes into the decoder's input buffer, if it will fit. */
- HSD_sink_res heatshrink_decoder_sink(heatshrink_decoder *hsd,
- uint8_t *in_buf, size_t size, size_t *input_size) {
- if (!hsd || !in_buf || !input_size)
- return HSDR_SINK_ERROR_NULL;
-
- size_t rem = HEATSHRINK_DECODER_INPUT_BUFFER_SIZE(hsd) - hsd->input_size;
- if (rem == 0) {
- *input_size = 0;
- return HSDR_SINK_FULL;
- }
-
- size = rem < size ? rem : size;
- LOG("-- sinking %zd bytes\n", size);
- /* copy into input buffer (at head of buffers) */
- memcpy(&hsd->buffers[hsd->input_size], in_buf, size);
- hsd->input_size += size;
- *input_size = size;
- return HSDR_SINK_OK;
- }
-
-
- /*****************
- * Decompression *
- *****************/
-
- #define BACKREF_COUNT_BITS(HSD) (HEATSHRINK_DECODER_LOOKAHEAD_BITS(HSD))
- #define BACKREF_INDEX_BITS(HSD) (HEATSHRINK_DECODER_WINDOW_BITS(HSD))
-
- // States
- static HSD_state st_tag_bit(heatshrink_decoder *hsd);
- static HSD_state st_yield_literal(heatshrink_decoder *hsd, output_info *oi);
- static HSD_state st_backref_index_msb(heatshrink_decoder *hsd);
- static HSD_state st_backref_index_lsb(heatshrink_decoder *hsd);
- static HSD_state st_backref_count_msb(heatshrink_decoder *hsd);
- static HSD_state st_backref_count_lsb(heatshrink_decoder *hsd);
- static HSD_state st_yield_backref(heatshrink_decoder *hsd, output_info *oi);
-
- HSD_poll_res heatshrink_decoder_poll(heatshrink_decoder *hsd, uint8_t *out_buf, size_t out_buf_size, size_t *output_size) {
- if (!hsd || !out_buf || !output_size)
- return HSDR_POLL_ERROR_NULL;
-
- *output_size = 0;
-
- output_info oi;
- oi.buf = out_buf;
- oi.buf_size = out_buf_size;
- oi.output_size = output_size;
-
- while (1) {
- LOG("-- poll, state is %d (%s), input_size %d\n", hsd->state, state_names[hsd->state], hsd->input_size);
- uint8_t in_state = hsd->state;
- switch (in_state) {
- case HSDS_TAG_BIT:
- hsd->state = st_tag_bit(hsd);
- break;
- case HSDS_YIELD_LITERAL:
- hsd->state = st_yield_literal(hsd, &oi);
- break;
- case HSDS_BACKREF_INDEX_MSB:
- hsd->state = st_backref_index_msb(hsd);
- break;
- case HSDS_BACKREF_INDEX_LSB:
- hsd->state = st_backref_index_lsb(hsd);
- break;
- case HSDS_BACKREF_COUNT_MSB:
- hsd->state = st_backref_count_msb(hsd);
- break;
- case HSDS_BACKREF_COUNT_LSB:
- hsd->state = st_backref_count_lsb(hsd);
- break;
- case HSDS_YIELD_BACKREF:
- hsd->state = st_yield_backref(hsd, &oi);
- break;
- default:
- return HSDR_POLL_ERROR_UNKNOWN;
- }
-
- // If the current state cannot advance, check if input or output
- // buffer are exhausted.
- if (hsd->state == in_state)
- return (*output_size == out_buf_size) ? HSDR_POLL_MORE : HSDR_POLL_EMPTY;
- }
- }
-
- static HSD_state st_tag_bit(heatshrink_decoder *hsd) {
- uint32_t bits = get_bits(hsd, 1); // get tag bit
- if (bits == NO_BITS)
- return HSDS_TAG_BIT;
- else if (bits)
- return HSDS_YIELD_LITERAL;
- else if (HEATSHRINK_DECODER_WINDOW_BITS(hsd) > 8)
- return HSDS_BACKREF_INDEX_MSB;
- else {
- hsd->output_index = 0;
- return HSDS_BACKREF_INDEX_LSB;
- }
- }
-
- static HSD_state st_yield_literal(heatshrink_decoder *hsd, output_info *oi) {
- /* Emit a repeated section from the window buffer, and add it (again)
- * to the window buffer. (Note that the repetition can include
- * itself.)*/
- if (*oi->output_size < oi->buf_size) {
- uint16_t byte = get_bits(hsd, 8);
- if (byte == NO_BITS) { return HSDS_YIELD_LITERAL; } /* out of input */
- uint8_t *buf = &hsd->buffers[HEATSHRINK_DECODER_INPUT_BUFFER_SIZE(hsd)];
- uint16_t mask = (1 << HEATSHRINK_DECODER_WINDOW_BITS(hsd)) - 1;
- uint8_t c = byte & 0xFF;
- LOG("-- emitting literal byte 0x%02x ('%c')\n", c, isprint(c) ? c : '.');
- buf[hsd->head_index++ & mask] = c;
- push_byte(hsd, oi, c);
- return HSDS_TAG_BIT;
- }
- return HSDS_YIELD_LITERAL;
- }
-
- static HSD_state st_backref_index_msb(heatshrink_decoder *hsd) {
- uint8_t bit_ct = BACKREF_INDEX_BITS(hsd);
- ASSERT(bit_ct > 8);
- uint16_t bits = get_bits(hsd, bit_ct - 8);
- LOG("-- backref index (msb), got 0x%04x (+1)\n", bits);
- if (bits == NO_BITS) { return HSDS_BACKREF_INDEX_MSB; }
- hsd->output_index = bits << 8;
- return HSDS_BACKREF_INDEX_LSB;
- }
-
- static HSD_state st_backref_index_lsb(heatshrink_decoder *hsd) {
- uint8_t bit_ct = BACKREF_INDEX_BITS(hsd);
- uint16_t bits = get_bits(hsd, bit_ct < 8 ? bit_ct : 8);
- LOG("-- backref index (lsb), got 0x%04x (+1)\n", bits);
- if (bits == NO_BITS) { return HSDS_BACKREF_INDEX_LSB; }
- hsd->output_index |= bits;
- hsd->output_index++;
- uint8_t br_bit_ct = BACKREF_COUNT_BITS(hsd);
- hsd->output_count = 0;
- return (br_bit_ct > 8) ? HSDS_BACKREF_COUNT_MSB : HSDS_BACKREF_COUNT_LSB;
- }
-
- static HSD_state st_backref_count_msb(heatshrink_decoder *hsd) {
- uint8_t br_bit_ct = BACKREF_COUNT_BITS(hsd);
- ASSERT(br_bit_ct > 8);
- uint16_t bits = get_bits(hsd, br_bit_ct - 8);
- LOG("-- backref count (msb), got 0x%04x (+1)\n", bits);
- if (bits == NO_BITS) { return HSDS_BACKREF_COUNT_MSB; }
- hsd->output_count = bits << 8;
- return HSDS_BACKREF_COUNT_LSB;
- }
-
- static HSD_state st_backref_count_lsb(heatshrink_decoder *hsd) {
- uint8_t br_bit_ct = BACKREF_COUNT_BITS(hsd);
- uint16_t bits = get_bits(hsd, br_bit_ct < 8 ? br_bit_ct : 8);
- LOG("-- backref count (lsb), got 0x%04x (+1)\n", bits);
- if (bits == NO_BITS) { return HSDS_BACKREF_COUNT_LSB; }
- hsd->output_count |= bits;
- hsd->output_count++;
- return HSDS_YIELD_BACKREF;
- }
-
- static HSD_state st_yield_backref(heatshrink_decoder *hsd, output_info *oi) {
- size_t count = oi->buf_size - *oi->output_size;
- if (count > 0) {
- size_t i = 0;
- if (hsd->output_count < count) count = hsd->output_count;
- uint8_t *buf = &hsd->buffers[HEATSHRINK_DECODER_INPUT_BUFFER_SIZE(hsd)];
- uint16_t mask = (1 << HEATSHRINK_DECODER_WINDOW_BITS(hsd)) - 1;
- uint16_t neg_offset = hsd->output_index;
- LOG("-- emitting %zu bytes from -%u bytes back\n", count, neg_offset);
- ASSERT(neg_offset <= mask + 1);
- ASSERT(count <= (size_t)(1 << BACKREF_COUNT_BITS(hsd)));
-
- for (i = 0; i < count; i++) {
- uint8_t c = buf[(hsd->head_index - neg_offset) & mask];
- push_byte(hsd, oi, c);
- buf[hsd->head_index & mask] = c;
- hsd->head_index++;
- LOG(" -- ++ 0x%02x\n", c);
- }
- hsd->output_count -= count;
- if (hsd->output_count == 0) { return HSDS_TAG_BIT; }
- }
- return HSDS_YIELD_BACKREF;
- }
-
- /* Get the next COUNT bits from the input buffer, saving incremental progress.
- * Returns NO_BITS on end of input, or if more than 15 bits are requested. */
- static uint16_t get_bits(heatshrink_decoder *hsd, uint8_t count) {
- uint16_t accumulator = 0;
- int i = 0;
- if (count > 15) return NO_BITS;
- LOG("-- popping %u bit(s)\n", count);
-
- /* If we aren't able to get COUNT bits, suspend immediately, because we
- * don't track how many bits of COUNT we've accumulated before suspend. */
- if (hsd->input_size == 0 && hsd->bit_index < (1 << (count - 1))) return NO_BITS;
-
- for (i = 0; i < count; i++) {
- if (hsd->bit_index == 0x00) {
- if (hsd->input_size == 0) {
- LOG(" -- out of bits, suspending w/ accumulator of %u (0x%02x)\n", accumulator, accumulator);
- return NO_BITS;
- }
- hsd->current_byte = hsd->buffers[hsd->input_index++];
- LOG(" -- pulled byte 0x%02x\n", hsd->current_byte);
- if (hsd->input_index == hsd->input_size) {
- hsd->input_index = 0; /* input is exhausted */
- hsd->input_size = 0;
- }
- hsd->bit_index = 0x80;
- }
- accumulator <<= 1;
- if (hsd->current_byte & hsd->bit_index) {
- accumulator |= 0x01;
- if (0) {
- LOG(" -- got 1, accumulator 0x%04x, bit_index 0x%02x\n",
- accumulator, hsd->bit_index);
- }
- }
- else if (0) {
- LOG(" -- got 0, accumulator 0x%04x, bit_index 0x%02x\n",
- accumulator, hsd->bit_index);
- }
- hsd->bit_index >>= 1;
- }
-
- if (count > 1) LOG(" -- accumulated %08x\n", accumulator);
- return accumulator;
- }
-
- HSD_finish_res heatshrink_decoder_finish(heatshrink_decoder *hsd) {
- if (!hsd) return HSDR_FINISH_ERROR_NULL;
- switch (hsd->state) {
- case HSDS_TAG_BIT:
- return hsd->input_size == 0 ? HSDR_FINISH_DONE : HSDR_FINISH_MORE;
-
- /* If we want to finish with no input, but are in these states, it's
- * because the 0-bit padding to the last byte looks like a backref
- * marker bit followed by all 0s for index and count bits. */
- case HSDS_BACKREF_INDEX_LSB:
- case HSDS_BACKREF_INDEX_MSB:
- case HSDS_BACKREF_COUNT_LSB:
- case HSDS_BACKREF_COUNT_MSB:
- return hsd->input_size == 0 ? HSDR_FINISH_DONE : HSDR_FINISH_MORE;
-
- /* If the output stream is padded with 0xFFs (possibly due to being in
- * flash memory), also explicitly check the input size rather than
- * uselessly returning MORE but yielding 0 bytes when polling. */
- case HSDS_YIELD_LITERAL:
- return hsd->input_size == 0 ? HSDR_FINISH_DONE : HSDR_FINISH_MORE;
-
- default: return HSDR_FINISH_MORE;
- }
- }
-
- static void push_byte(heatshrink_decoder *hsd, output_info *oi, uint8_t byte) {
- LOG(" -- pushing byte: 0x%02x ('%c')\n", byte, isprint(byte) ? byte : '.');
- oi->buf[(*oi->output_size)++] = byte;
- (void)hsd;
- }
-
- #endif // BINARY_FILE_TRANSFER
|