Marlin Binary Protocol Mark II (#14817)
This commit is contained in:
parent
5bc2fb022c
commit
f499cecf0d
9 changed files with 1026 additions and 256 deletions
36
Marlin/src/feature/binary_protocol.cpp
Normal file
36
Marlin/src/feature/binary_protocol.cpp
Normal file
|
@ -0,0 +1,36 @@
|
|||
/**
|
||||
* Marlin 3D Printer Firmware
|
||||
* Copyright (c) 2019 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 <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
*/
|
||||
|
||||
#include "../inc/MarlinConfigPre.h"
|
||||
|
||||
#if ENABLED(BINARY_FILE_TRANSFER)
|
||||
|
||||
#include "../sd/cardreader.h"
|
||||
#include "binary_protocol.h"
|
||||
|
||||
char* SDFileTransferProtocol::Packet::Open::data = nullptr;
|
||||
size_t SDFileTransferProtocol::data_waiting, SDFileTransferProtocol::transfer_timeout, SDFileTransferProtocol::idle_timeout;
|
||||
bool SDFileTransferProtocol::transfer_active, SDFileTransferProtocol::dummy_transfer, SDFileTransferProtocol::compression;
|
||||
|
||||
BinaryStream binaryStream[NUM_SERIAL];
|
||||
|
||||
#endif // BINARY_FILE_TRANSFER
|
471
Marlin/src/feature/binary_protocol.h
Normal file
471
Marlin/src/feature/binary_protocol.h
Normal file
|
@ -0,0 +1,471 @@
|
|||
/**
|
||||
* Marlin 3D Printer Firmware
|
||||
* Copyright (c) 2019 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 <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include "../inc/MarlinConfig.h"
|
||||
|
||||
#define BINARY_STREAM_COMPRESSION
|
||||
|
||||
#if ENABLED(BINARY_STREAM_COMPRESSION)
|
||||
#include "../libs/heatshrink/heatshrink_decoder.h"
|
||||
#endif
|
||||
|
||||
inline bool bs_serial_data_available(const uint8_t index) {
|
||||
switch (index) {
|
||||
case 0: return MYSERIAL0.available();
|
||||
#if NUM_SERIAL > 1
|
||||
case 1: return MYSERIAL1.available();
|
||||
#endif
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
inline int bs_read_serial(const uint8_t index) {
|
||||
switch (index) {
|
||||
case 0: return MYSERIAL0.read();
|
||||
#if NUM_SERIAL > 1
|
||||
case 1: return MYSERIAL1.read();
|
||||
#endif
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
#if ENABLED(BINARY_STREAM_COMPRESSION)
|
||||
static heatshrink_decoder hsd;
|
||||
static uint8_t decode_buffer[512] = {};
|
||||
#endif
|
||||
|
||||
class SDFileTransferProtocol {
|
||||
private:
|
||||
struct Packet {
|
||||
struct [[gnu::packed]] Open {
|
||||
static bool validate(char* buffer, size_t length) {
|
||||
return (length > sizeof(Open) && buffer[length - 1] == '\0');
|
||||
}
|
||||
static Open& decode(char* buffer) {
|
||||
data = &buffer[2];
|
||||
return *reinterpret_cast<Open*>(buffer);
|
||||
}
|
||||
bool compression_enabled() { return compression & 0x1; }
|
||||
bool dummy_transfer() { return dummy & 0x1; }
|
||||
static char* filename() { return data; }
|
||||
private:
|
||||
uint8_t dummy, compression;
|
||||
static char* data; // variable length strings complicate things
|
||||
};
|
||||
};
|
||||
|
||||
static bool file_open(char* filename) {
|
||||
if (!dummy_transfer) {
|
||||
card.initsd();
|
||||
card.openFile(filename, false);
|
||||
if (!card.isFileOpen()) return false;
|
||||
}
|
||||
transfer_active = true;
|
||||
data_waiting = 0;
|
||||
#if ENABLED(BINARY_STREAM_COMPRESSION)
|
||||
heatshrink_decoder_reset(&hsd);
|
||||
#endif
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool file_write(char* buffer, const size_t length) {
|
||||
#if ENABLED(BINARY_STREAM_COMPRESSION)
|
||||
if (compression) {
|
||||
size_t total_processed = 0, processed_count = 0;
|
||||
HSD_poll_res presult;
|
||||
|
||||
while (total_processed < length) {
|
||||
heatshrink_decoder_sink(&hsd, reinterpret_cast<uint8_t*>(&buffer[total_processed]), length - total_processed, &processed_count);
|
||||
total_processed += processed_count;
|
||||
do {
|
||||
presult = heatshrink_decoder_poll(&hsd, &decode_buffer[data_waiting], sizeof(decode_buffer) - data_waiting, &processed_count);
|
||||
data_waiting += processed_count;
|
||||
if (data_waiting == sizeof(decode_buffer)) {
|
||||
if (!dummy_transfer)
|
||||
if (card.write(decode_buffer, data_waiting) < 0) {
|
||||
return false;
|
||||
}
|
||||
data_waiting = 0;
|
||||
}
|
||||
} while (presult == HSDR_POLL_MORE);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
#endif
|
||||
return (dummy_transfer || card.write(buffer, length) >= 0);
|
||||
}
|
||||
|
||||
static bool file_close() {
|
||||
if (!dummy_transfer) {
|
||||
#if ENABLED(BINARY_STREAM_COMPRESSION)
|
||||
// flush any buffered data
|
||||
if (data_waiting) {
|
||||
if (card.write(decode_buffer, data_waiting) < 0) return false;
|
||||
data_waiting = 0;
|
||||
}
|
||||
#endif
|
||||
card.closefile();
|
||||
card.release();
|
||||
}
|
||||
#if ENABLED(BINARY_STREAM_COMPRESSION)
|
||||
heatshrink_decoder_finish(&hsd);
|
||||
#endif
|
||||
transfer_active = false;
|
||||
return true;
|
||||
}
|
||||
|
||||
static void transfer_abort() {
|
||||
if (!dummy_transfer) {
|
||||
card.closefile();
|
||||
card.removeFile(card.filename);
|
||||
card.release();
|
||||
#if ENABLED(BINARY_STREAM_COMPRESSION)
|
||||
heatshrink_decoder_finish(&hsd);
|
||||
#endif
|
||||
}
|
||||
transfer_active = false;
|
||||
return;
|
||||
}
|
||||
|
||||
enum class FileTransfer : uint8_t { QUERY, OPEN, CLOSE, WRITE, ABORT };
|
||||
|
||||
static size_t data_waiting, transfer_timeout, idle_timeout;
|
||||
static bool transfer_active, dummy_transfer, compression;
|
||||
|
||||
public:
|
||||
|
||||
static void idle() {
|
||||
// If a transfer is interrupted and a file is left open, abort it after TIMEOUT ms
|
||||
const millis_t ms = millis();
|
||||
if (transfer_active && ELAPSED(ms, idle_timeout)) {
|
||||
idle_timeout = ms + IDLE_PERIOD;
|
||||
if (ELAPSED(ms, transfer_timeout)) transfer_abort();
|
||||
}
|
||||
}
|
||||
|
||||
static void process(uint8_t packet_type, char* buffer, const uint16_t length) {
|
||||
transfer_timeout = millis() + TIMEOUT;
|
||||
switch (static_cast<FileTransfer>(packet_type)) {
|
||||
case FileTransfer::QUERY:
|
||||
SERIAL_ECHOPAIR("PFT:version:", VERSION_MAJOR, ".", VERSION_MINOR, ".", VERSION_PATCH);
|
||||
#if ENABLED(BINARY_STREAM_COMPRESSION)
|
||||
SERIAL_ECHOLNPAIR(":compresion:heatshrink,", HEATSHRINK_STATIC_WINDOW_BITS, ",", HEATSHRINK_STATIC_LOOKAHEAD_BITS);
|
||||
#else
|
||||
SERIAL_ECHOLNPGM(":compresion:none");
|
||||
#endif
|
||||
break;
|
||||
case FileTransfer::OPEN:
|
||||
if (transfer_active)
|
||||
SERIAL_ECHOLNPGM("PFT:busy");
|
||||
else {
|
||||
if (Packet::Open::validate(buffer, length)) {
|
||||
auto packet = Packet::Open::decode(buffer);
|
||||
compression = packet.compression_enabled();
|
||||
dummy_transfer = packet.dummy_transfer();
|
||||
if (file_open(packet.filename())) {
|
||||
SERIAL_ECHOLNPGM("PFT:success");
|
||||
break;
|
||||
}
|
||||
}
|
||||
SERIAL_ECHOLNPGM("PFT:fail");
|
||||
}
|
||||
break;
|
||||
case FileTransfer::CLOSE:
|
||||
if (transfer_active) {
|
||||
if (file_close())
|
||||
SERIAL_ECHOLNPGM("PFT:success");
|
||||
else
|
||||
SERIAL_ECHOLNPGM("PFT:ioerror");
|
||||
}
|
||||
else SERIAL_ECHOLNPGM("PFT:invalid");
|
||||
break;
|
||||
case FileTransfer::WRITE:
|
||||
if (!transfer_active)
|
||||
SERIAL_ECHOLNPGM("PFT:invalid");
|
||||
else if (!file_write(buffer, length))
|
||||
SERIAL_ECHOLNPGM("PFT:ioerror");
|
||||
break;
|
||||
case FileTransfer::ABORT:
|
||||
transfer_abort();
|
||||
SERIAL_ECHOLNPGM("PFT:success");
|
||||
break;
|
||||
default:
|
||||
SERIAL_ECHOLNPGM("PTF:invalid");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
static const uint16_t VERSION_MAJOR = 0, VERSION_MINOR = 1, VERSION_PATCH = 0, TIMEOUT = 10000, IDLE_PERIOD = 1000;
|
||||
};
|
||||
|
||||
class BinaryStream {
|
||||
public:
|
||||
enum class Protocol : uint8_t { CONTROL, FILE_TRANSFER };
|
||||
|
||||
enum class ProtocolControl : uint8_t { SYNC = 1, CLOSE };
|
||||
|
||||
enum class StreamState : uint8_t { PACKET_RESET, PACKET_WAIT, PACKET_HEADER, PACKET_DATA, PACKET_FOOTER,
|
||||
PACKET_PROCESS, PACKET_RESEND, PACKET_TIMEOUT, PACKET_ERROR };
|
||||
|
||||
struct Packet { // 10 byte protocol overhead, ascii with checksum and line number has a minimum of 7 increasing with line
|
||||
struct [[gnu::packed]] Header {
|
||||
static constexpr uint16_t HEADER_TOKEN = 0xB5AD;
|
||||
uint16_t token; // packet start token
|
||||
uint8_t sync; // stream sync, resend id and packet loss detection
|
||||
uint8_t meta; // 4 bit protocol,
|
||||
// 4 bit packet type
|
||||
uint16_t size; // data length
|
||||
uint16_t checksum; // header checksum
|
||||
|
||||
uint8_t protocol() { return (meta >> 4) & 0xF; }
|
||||
uint8_t type() { return meta & 0xF; }
|
||||
void reset() { token = 0; sync = 0; meta = 0; size = 0; checksum = 0; }
|
||||
};
|
||||
|
||||
struct [[gnu::packed]] Footer {
|
||||
uint16_t checksum; // full packet checksum
|
||||
void reset() { checksum = 0; }
|
||||
};
|
||||
|
||||
uint8_t header_data[sizeof(Header)],
|
||||
footer_data[sizeof(Footer)];
|
||||
uint32_t bytes_received;
|
||||
uint16_t checksum, header_checksum;
|
||||
millis_t timeout;
|
||||
char* buffer;
|
||||
|
||||
Header& header() { return *reinterpret_cast<Header*>(header_data); }
|
||||
Footer& footer() { return *reinterpret_cast<Footer*>(footer_data); }
|
||||
void reset() {
|
||||
header().reset();
|
||||
footer().reset();
|
||||
bytes_received = 0;
|
||||
checksum = 0;
|
||||
header_checksum = 0;
|
||||
timeout = millis() + PACKET_MAX_WAIT;
|
||||
buffer = nullptr;
|
||||
}
|
||||
} packet{};
|
||||
|
||||
void reset() {
|
||||
sync = 0;
|
||||
packet_retries = 0;
|
||||
buffer_next_index = 0;
|
||||
}
|
||||
|
||||
// fletchers 16 checksum
|
||||
uint32_t checksum(uint32_t cs, uint8_t value) {
|
||||
uint16_t cs_low = (((cs & 0xFF) + value) % 255);
|
||||
return ((((cs >> 8) + cs_low) % 255) << 8) | cs_low;
|
||||
}
|
||||
|
||||
// read the next byte from the data stream keeping track of
|
||||
// whether the stream times out from data starvation
|
||||
// takes the data variable by reference in order to return status
|
||||
bool stream_read(uint8_t& data) {
|
||||
if (stream_state != StreamState::PACKET_WAIT && ELAPSED(millis(), packet.timeout)) {
|
||||
stream_state = StreamState::PACKET_TIMEOUT;
|
||||
return false;
|
||||
}
|
||||
if (!bs_serial_data_available(card.transfer_port_index)) return false;
|
||||
data = bs_read_serial(card.transfer_port_index);
|
||||
packet.timeout = millis() + PACKET_MAX_WAIT;
|
||||
return true;
|
||||
}
|
||||
|
||||
template<const size_t buffer_size>
|
||||
void receive(char (&buffer)[buffer_size]) {
|
||||
uint8_t data = 0;
|
||||
millis_t transfer_window = millis() + RX_TIMESLICE;
|
||||
|
||||
#if ENABLED(SDSUPPORT)
|
||||
PORT_REDIRECT(card.transfer_port_index);
|
||||
#endif
|
||||
|
||||
while (PENDING(millis(), transfer_window)) {
|
||||
switch (stream_state) {
|
||||
/**
|
||||
* Data stream packet handling
|
||||
*/
|
||||
case StreamState::PACKET_RESET:
|
||||
packet.reset();
|
||||
stream_state = StreamState::PACKET_WAIT;
|
||||
case StreamState::PACKET_WAIT:
|
||||
if (!stream_read(data)) { idle(); return; } // no active packet so don't wait
|
||||
packet.header_data[1] = data;
|
||||
if (packet.header().token == Packet::Header::HEADER_TOKEN) {
|
||||
packet.bytes_received = 2;
|
||||
stream_state = StreamState::PACKET_HEADER;
|
||||
}
|
||||
else {
|
||||
// stream corruption drop data
|
||||
packet.header_data[0] = data;
|
||||
}
|
||||
break;
|
||||
case StreamState::PACKET_HEADER:
|
||||
if (!stream_read(data)) break;
|
||||
|
||||
packet.header_data[packet.bytes_received++] = data;
|
||||
packet.checksum = checksum(packet.checksum, data);
|
||||
|
||||
// header checksum calculation can't contain the checksum
|
||||
if (packet.bytes_received == sizeof(Packet::Header) - 2)
|
||||
packet.header_checksum = packet.checksum;
|
||||
|
||||
if (packet.bytes_received == sizeof(Packet::Header)) {
|
||||
if (packet.header().checksum == packet.header_checksum) {
|
||||
// The SYNC control packet is a special case in that it doesn't require the stream sync to be correct
|
||||
if (static_cast<Protocol>(packet.header().protocol()) == Protocol::CONTROL && static_cast<ProtocolControl>(packet.header().type()) == ProtocolControl::SYNC) {
|
||||
SERIAL_ECHOLNPAIR("ss", sync, ",", buffer_size, ",", VERSION_MAJOR, ".", VERSION_MINOR, ".", VERSION_PATCH);
|
||||
stream_state = StreamState::PACKET_RESET;
|
||||
break;
|
||||
}
|
||||
if (packet.header().sync == sync) {
|
||||
buffer_next_index = 0;
|
||||
packet.bytes_received = 0;
|
||||
if (packet.header().size) {
|
||||
stream_state = StreamState::PACKET_DATA;
|
||||
packet.buffer = static_cast<char *>(&buffer[0]); // multipacket buffering not implemented, always allocate whole buffer to packet
|
||||
}
|
||||
else
|
||||
stream_state = StreamState::PACKET_PROCESS;
|
||||
}
|
||||
else if (packet.header().sync == sync - 1) { // ok response must have been lost
|
||||
SERIAL_ECHOLNPAIR("ok", packet.header().sync); // transmit valid packet received and drop the payload
|
||||
stream_state = StreamState::PACKET_RESET;
|
||||
}
|
||||
else if (packet_retries) {
|
||||
stream_state = StreamState::PACKET_RESET; // could be packets already buffered on flow controlled connections, drop them without ack
|
||||
}
|
||||
else {
|
||||
SERIAL_ECHO_MSG("Datastream packet out of order");
|
||||
stream_state = StreamState::PACKET_RESEND;
|
||||
}
|
||||
}
|
||||
else {
|
||||
SERIAL_ECHO_START();
|
||||
SERIAL_ECHOLNPAIR("Packet Header(", packet.header().sync, "?) Corrupt");
|
||||
stream_state = StreamState::PACKET_RESEND;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case StreamState::PACKET_DATA:
|
||||
if (!stream_read(data)) break;
|
||||
|
||||
if (buffer_next_index < buffer_size)
|
||||
packet.buffer[buffer_next_index] = data;
|
||||
else {
|
||||
SERIAL_ECHO_MSG("Datastream packet data buffer overrun");
|
||||
stream_state = StreamState::PACKET_ERROR;
|
||||
break;
|
||||
}
|
||||
|
||||
packet.checksum = checksum(packet.checksum, data);
|
||||
packet.bytes_received++;
|
||||
buffer_next_index++;
|
||||
|
||||
if (packet.bytes_received == packet.header().size) {
|
||||
stream_state = StreamState::PACKET_FOOTER;
|
||||
packet.bytes_received = 0;
|
||||
}
|
||||
break;
|
||||
case StreamState::PACKET_FOOTER:
|
||||
if (!stream_read(data)) break;
|
||||
|
||||
packet.footer_data[packet.bytes_received++] = data;
|
||||
if (packet.bytes_received == sizeof(Packet::Footer)) {
|
||||
if (packet.footer().checksum == packet.checksum) {
|
||||
stream_state = StreamState::PACKET_PROCESS;
|
||||
}
|
||||
else {
|
||||
SERIAL_ECHO_START();
|
||||
SERIAL_ECHOLNPAIR("Packet(", packet.header().sync, ") Payload Corrupt");
|
||||
stream_state = StreamState::PACKET_RESEND;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case StreamState::PACKET_PROCESS:
|
||||
sync++;
|
||||
packet_retries = 0;
|
||||
bytes_received += packet.header().size;
|
||||
|
||||
SERIAL_ECHOLNPAIR("ok", packet.header().sync); // transmit valid packet received
|
||||
dispatch();
|
||||
stream_state = StreamState::PACKET_RESET;
|
||||
break;
|
||||
case StreamState::PACKET_RESEND:
|
||||
if (packet_retries < MAX_RETRIES || MAX_RETRIES == 0) {
|
||||
packet_retries++;
|
||||
stream_state = StreamState::PACKET_RESET;
|
||||
SERIAL_ECHO_START();
|
||||
SERIAL_ECHOLNPAIR("Resend request ", int(packet_retries));
|
||||
SERIAL_ECHOLNPAIR("rs", sync);
|
||||
}
|
||||
else
|
||||
stream_state = StreamState::PACKET_ERROR;
|
||||
break;
|
||||
case StreamState::PACKET_TIMEOUT:
|
||||
SERIAL_ECHO_MSG("Datastream timeout");
|
||||
stream_state = StreamState::PACKET_RESEND;
|
||||
break;
|
||||
case StreamState::PACKET_ERROR:
|
||||
SERIAL_ECHOLNPAIR("fe", packet.header().sync);
|
||||
reset(); // reset everything, resync required
|
||||
stream_state = StreamState::PACKET_RESET;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void dispatch() {
|
||||
switch(static_cast<Protocol>(packet.header().protocol())) {
|
||||
case Protocol::CONTROL:
|
||||
switch(static_cast<ProtocolControl>(packet.header().type())) {
|
||||
case ProtocolControl::CLOSE: // revert back to ASCII mode
|
||||
card.flag.binary_mode = false;
|
||||
break;
|
||||
default:
|
||||
SERIAL_ECHO_MSG("Unknown BinaryProtocolControl Packet");
|
||||
}
|
||||
break;
|
||||
case Protocol::FILE_TRANSFER:
|
||||
SDFileTransferProtocol::process(packet.header().type(), packet.buffer, packet.header().size); // send user data to be processed
|
||||
break;
|
||||
default:
|
||||
SERIAL_ECHO_MSG("Unsupported Binary Protocol");
|
||||
}
|
||||
}
|
||||
|
||||
void idle() {
|
||||
// Some Protocols may need periodic updates without new data
|
||||
SDFileTransferProtocol::idle();
|
||||
}
|
||||
|
||||
static const uint16_t PACKET_MAX_WAIT = 500, RX_TIMESLICE = 20, MAX_RETRIES = 0, VERSION_MAJOR = 0, VERSION_MINOR = 1, VERSION_PATCH = 0;
|
||||
uint8_t packet_retries, sync;
|
||||
uint16_t buffer_next_index;
|
||||
uint32_t bytes_received;
|
||||
StreamState stream_state = StreamState::PACKET_RESET;
|
||||
};
|
||||
|
||||
extern BinaryStream binaryStream[NUM_SERIAL];
|
|
@ -39,6 +39,11 @@ GCodeQueue queue;
|
|||
#include "../feature/leds/printer_event_leds.h"
|
||||
#endif
|
||||
|
||||
#if ENABLED(BINARY_FILE_TRANSFER)
|
||||
#include "../feature/binary_protocol.h"
|
||||
#endif
|
||||
|
||||
|
||||
/**
|
||||
* GCode line number handling. Hosts may opt to include line numbers when
|
||||
* sending commands to Marlin, and lines will be checked for sequentiality.
|
||||
|
@ -285,256 +290,6 @@ inline int read_serial(const uint8_t index) {
|
|||
}
|
||||
}
|
||||
|
||||
#if ENABLED(BINARY_FILE_TRANSFER)
|
||||
|
||||
inline bool serial_data_available(const uint8_t index) {
|
||||
switch (index) {
|
||||
case 0: return MYSERIAL0.available();
|
||||
#if NUM_SERIAL > 1
|
||||
case 1: return MYSERIAL1.available();
|
||||
#endif
|
||||
default: return false;
|
||||
}
|
||||
}
|
||||
|
||||
class BinaryStream {
|
||||
public:
|
||||
enum class StreamState : uint8_t {
|
||||
STREAM_RESET,
|
||||
PACKET_RESET,
|
||||
STREAM_HEADER,
|
||||
PACKET_HEADER,
|
||||
PACKET_DATA,
|
||||
PACKET_VALIDATE,
|
||||
PACKET_RESEND,
|
||||
PACKET_FLUSHRX,
|
||||
PACKET_TIMEOUT,
|
||||
STREAM_COMPLETE,
|
||||
STREAM_FAILED,
|
||||
};
|
||||
|
||||
#pragma pack(push, 1)
|
||||
|
||||
struct StreamHeader {
|
||||
uint16_t token;
|
||||
uint32_t filesize;
|
||||
};
|
||||
union {
|
||||
uint8_t stream_header_bytes[sizeof(StreamHeader)];
|
||||
StreamHeader stream_header;
|
||||
};
|
||||
|
||||
struct Packet {
|
||||
struct Header {
|
||||
uint32_t id;
|
||||
uint16_t size, checksum;
|
||||
};
|
||||
union {
|
||||
uint8_t header_bytes[sizeof(Header)];
|
||||
Header header;
|
||||
};
|
||||
uint32_t bytes_received;
|
||||
uint16_t checksum;
|
||||
millis_t timeout;
|
||||
} packet{};
|
||||
|
||||
#pragma pack(pop)
|
||||
|
||||
void packet_reset() {
|
||||
packet.header.id = 0;
|
||||
packet.header.size = 0;
|
||||
packet.header.checksum = 0;
|
||||
packet.bytes_received = 0;
|
||||
packet.checksum = 0x53A2;
|
||||
packet.timeout = millis() + STREAM_MAX_WAIT;
|
||||
}
|
||||
|
||||
void stream_reset() {
|
||||
packets_received = 0;
|
||||
bytes_received = 0;
|
||||
packet_retries = 0;
|
||||
buffer_next_index = 0;
|
||||
stream_header.token = 0;
|
||||
stream_header.filesize = 0;
|
||||
}
|
||||
|
||||
uint32_t checksum(uint32_t seed, uint8_t value) {
|
||||
return ((seed ^ value) ^ (seed << 8)) & 0xFFFF;
|
||||
}
|
||||
|
||||
// read the next byte from the data stream keeping track of
|
||||
// whether the stream times out from data starvation
|
||||
// takes the data variable by reference in order to return status
|
||||
bool stream_read(uint8_t& data) {
|
||||
if (ELAPSED(millis(), packet.timeout)) {
|
||||
stream_state = StreamState::PACKET_TIMEOUT;
|
||||
return false;
|
||||
}
|
||||
if (!serial_data_available(card.transfer_port_index)) return false;
|
||||
data = read_serial(card.transfer_port_index);
|
||||
packet.timeout = millis() + STREAM_MAX_WAIT;
|
||||
return true;
|
||||
}
|
||||
|
||||
template<const size_t buffer_size>
|
||||
void receive(char (&buffer)[buffer_size]) {
|
||||
uint8_t data = 0;
|
||||
millis_t transfer_timeout = millis() + RX_TIMESLICE;
|
||||
|
||||
#if ENABLED(SDSUPPORT)
|
||||
PORT_REDIRECT(card.transfer_port_index);
|
||||
#endif
|
||||
|
||||
while (PENDING(millis(), transfer_timeout)) {
|
||||
switch (stream_state) {
|
||||
case StreamState::STREAM_RESET:
|
||||
stream_reset();
|
||||
case StreamState::PACKET_RESET:
|
||||
packet_reset();
|
||||
stream_state = StreamState::PACKET_HEADER;
|
||||
break;
|
||||
case StreamState::STREAM_HEADER: // The filename could also be in this packet, rather than handling it in the gcode
|
||||
for (size_t i = 0; i < sizeof(stream_header); ++i)
|
||||
stream_header_bytes[i] = buffer[i];
|
||||
|
||||
if (stream_header.token == 0x1234) {
|
||||
stream_state = StreamState::PACKET_RESET;
|
||||
bytes_received = 0;
|
||||
time_stream_start = millis();
|
||||
// confirm active stream and the maximum block size supported
|
||||
SERIAL_ECHO_START();
|
||||
SERIAL_ECHOLNPAIR("Datastream initialized (", stream_header.filesize, " bytes expected)");
|
||||
SERIAL_ECHOLNPAIR("so", buffer_size);
|
||||
}
|
||||
else {
|
||||
SERIAL_ECHO_MSG("Datastream init error (invalid token)");
|
||||
stream_state = StreamState::STREAM_FAILED;
|
||||
}
|
||||
buffer_next_index = 0;
|
||||
break;
|
||||
case StreamState::PACKET_HEADER:
|
||||
if (!stream_read(data)) break;
|
||||
|
||||
packet.header_bytes[packet.bytes_received++] = data;
|
||||
if (packet.bytes_received == sizeof(Packet::Header)) {
|
||||
if (packet.header.id == packets_received) {
|
||||
buffer_next_index = 0;
|
||||
packet.bytes_received = 0;
|
||||
stream_state = StreamState::PACKET_DATA;
|
||||
}
|
||||
else {
|
||||
SERIAL_ECHO_MSG("Datastream packet out of order");
|
||||
stream_state = StreamState::PACKET_FLUSHRX;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case StreamState::PACKET_DATA:
|
||||
if (!stream_read(data)) break;
|
||||
|
||||
if (buffer_next_index < buffer_size)
|
||||
buffer[buffer_next_index] = data;
|
||||
else {
|
||||
SERIAL_ECHO_MSG("Datastream packet data buffer overrun");
|
||||
stream_state = StreamState::STREAM_FAILED;
|
||||
break;
|
||||
}
|
||||
|
||||
packet.checksum = checksum(packet.checksum, data);
|
||||
packet.bytes_received++;
|
||||
buffer_next_index++;
|
||||
|
||||
if (packet.bytes_received == packet.header.size)
|
||||
stream_state = StreamState::PACKET_VALIDATE;
|
||||
|
||||
break;
|
||||
case StreamState::PACKET_VALIDATE:
|
||||
if (packet.header.checksum == packet.checksum) {
|
||||
packet_retries = 0;
|
||||
packets_received++;
|
||||
bytes_received += packet.header.size;
|
||||
|
||||
if (packet.header.id == 0) // id 0 is always the stream descriptor
|
||||
stream_state = StreamState::STREAM_HEADER; // defer packet confirmation to STREAM_HEADER state
|
||||
else {
|
||||
if (bytes_received < stream_header.filesize) {
|
||||
stream_state = StreamState::PACKET_RESET; // reset and receive next packet
|
||||
SERIAL_ECHOLNPAIR("ok", packet.header.id); // transmit confirm packet received and valid token
|
||||
}
|
||||
else
|
||||
stream_state = StreamState::STREAM_COMPLETE; // no more data required
|
||||
|
||||
if (card.write(buffer, buffer_next_index) < 0) {
|
||||
stream_state = StreamState::STREAM_FAILED;
|
||||
SERIAL_ECHO_MSG("SDCard IO Error");
|
||||
break;
|
||||
};
|
||||
}
|
||||
}
|
||||
else {
|
||||
SERIAL_ECHO_START();
|
||||
SERIAL_ECHOLNPAIR("Block(", packet.header.id, ") Corrupt");
|
||||
stream_state = StreamState::PACKET_FLUSHRX;
|
||||
}
|
||||
break;
|
||||
case StreamState::PACKET_RESEND:
|
||||
if (packet_retries < MAX_RETRIES) {
|
||||
packet_retries++;
|
||||
stream_state = StreamState::PACKET_RESET;
|
||||
SERIAL_ECHO_START();
|
||||
SERIAL_ECHOLNPAIR("Resend request ", int(packet_retries));
|
||||
SERIAL_ECHOLNPAIR("rs", packet.header.id); // transmit resend packet token
|
||||
}
|
||||
else {
|
||||
stream_state = StreamState::STREAM_FAILED;
|
||||
}
|
||||
break;
|
||||
case StreamState::PACKET_FLUSHRX:
|
||||
if (ELAPSED(millis(), packet.timeout)) {
|
||||
stream_state = StreamState::PACKET_RESEND;
|
||||
break;
|
||||
}
|
||||
if (!serial_data_available(card.transfer_port_index)) break;
|
||||
read_serial(card.transfer_port_index); // throw away data
|
||||
packet.timeout = millis() + STREAM_MAX_WAIT;
|
||||
break;
|
||||
case StreamState::PACKET_TIMEOUT:
|
||||
SERIAL_ECHO_START();
|
||||
SERIAL_ECHOLNPGM("Datastream timeout");
|
||||
stream_state = StreamState::PACKET_RESEND;
|
||||
break;
|
||||
case StreamState::STREAM_COMPLETE:
|
||||
stream_state = StreamState::STREAM_RESET;
|
||||
card.flag.binary_mode = false;
|
||||
SERIAL_ECHO_START();
|
||||
SERIAL_ECHO(card.filename);
|
||||
SERIAL_ECHOLNPAIR(" transfer completed @ ", ((bytes_received / (millis() - time_stream_start) * 1000) / 1024), "KiB/s");
|
||||
SERIAL_ECHOLNPGM("sc"); // transmit stream complete token
|
||||
card.closefile();
|
||||
return;
|
||||
case StreamState::STREAM_FAILED:
|
||||
stream_state = StreamState::STREAM_RESET;
|
||||
card.flag.binary_mode = false;
|
||||
card.closefile();
|
||||
card.removeFile(card.filename);
|
||||
SERIAL_ECHO_START();
|
||||
SERIAL_ECHOLNPGM("File transfer failed");
|
||||
SERIAL_ECHOLNPGM("sf"); // transmit stream failed token
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static const uint16_t STREAM_MAX_WAIT = 500, RX_TIMESLICE = 20, MAX_RETRIES = 3;
|
||||
uint8_t packet_retries;
|
||||
uint16_t buffer_next_index;
|
||||
uint32_t packets_received, bytes_received;
|
||||
millis_t time_stream_start;
|
||||
StreamState stream_state = StreamState::STREAM_RESET;
|
||||
|
||||
} binaryStream{};
|
||||
|
||||
#endif // BINARY_FILE_TRANSFER
|
||||
|
||||
void GCodeQueue::gcode_line_error(PGM_P const err, const int8_t port) {
|
||||
PORT_REDIRECT(port);
|
||||
SERIAL_ERROR_START();
|
||||
|
@ -564,13 +319,13 @@ void GCodeQueue::get_serial_commands() {
|
|||
;
|
||||
|
||||
#if ENABLED(BINARY_FILE_TRANSFER)
|
||||
if (card.flag.saving && card.flag.binary_mode) {
|
||||
if (card.flag.binary_mode) {
|
||||
/**
|
||||
* For binary stream file transfer, use serial_line_buffer as the working
|
||||
* receive buffer (which limits the packet size to MAX_CMD_SIZE).
|
||||
* The receive buffer also limits the packet size for reliable transmission.
|
||||
*/
|
||||
binaryStream.receive(serial_line_buffer[card.transfer_port_index]);
|
||||
binaryStream[card.transfer_port_index].receive(serial_line_buffer[card.transfer_port_index]);
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
|
|
|
@ -48,10 +48,7 @@ void GcodeSuite::M28() {
|
|||
|
||||
// Binary transfer mode
|
||||
if ((card.flag.binary_mode = binary_mode)) {
|
||||
SERIAL_ECHO_START();
|
||||
SERIAL_ECHO(" preparing to receive: ");
|
||||
SERIAL_ECHOLN(p);
|
||||
card.openFile(p, false);
|
||||
SERIAL_ECHO_MSG("Switching to Binary Protocol");
|
||||
#if NUM_SERIAL > 1
|
||||
card.transfer_port_index = queue.port[queue.index_r];
|
||||
#endif
|
||||
|
|
14
Marlin/src/libs/heatshrink/LICENSE
Normal file
14
Marlin/src/libs/heatshrink/LICENSE
Normal file
|
@ -0,0 +1,14 @@
|
|||
Copyright (c) 2013-2015, Scott Vokes <vokes.s@gmail.com>
|
||||
All rights reserved.
|
||||
|
||||
Permission to use, copy, modify, and/or distribute this software for any
|
||||
purpose with or without fee is hereby granted, provided that the above
|
||||
copyright notice and this permission notice appear in all copies.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
|
||||
WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
|
||||
MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
|
||||
ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
|
||||
WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
|
||||
ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
|
||||
OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
|
20
Marlin/src/libs/heatshrink/heatshrink_common.h
Normal file
20
Marlin/src/libs/heatshrink/heatshrink_common.h
Normal file
|
@ -0,0 +1,20 @@
|
|||
/**
|
||||
* libs/heatshrink/heatshrink_common.h
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#define HEATSHRINK_AUTHOR "Scott Vokes <vokes.s@gmail.com>"
|
||||
#define HEATSHRINK_URL "https://github.com/atomicobject/heatshrink"
|
||||
|
||||
/* Version 0.4.1 */
|
||||
#define HEATSHRINK_VERSION_MAJOR 0
|
||||
#define HEATSHRINK_VERSION_MINOR 4
|
||||
#define HEATSHRINK_VERSION_PATCH 1
|
||||
|
||||
#define HEATSHRINK_MIN_WINDOW_BITS 4
|
||||
#define HEATSHRINK_MAX_WINDOW_BITS 15
|
||||
|
||||
#define HEATSHRINK_MIN_LOOKAHEAD_BITS 3
|
||||
|
||||
#define HEATSHRINK_LITERAL_MARKER 0x01
|
||||
#define HEATSHRINK_BACKREF_MARKER 0x00
|
26
Marlin/src/libs/heatshrink/heatshrink_config.h
Normal file
26
Marlin/src/libs/heatshrink/heatshrink_config.h
Normal file
|
@ -0,0 +1,26 @@
|
|||
/**
|
||||
* libs/heatshrink/heatshrink_config.h
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
// Should functionality assuming dynamic allocation be used?
|
||||
#ifndef HEATSHRINK_DYNAMIC_ALLOC
|
||||
//#define HEATSHRINK_DYNAMIC_ALLOC 1
|
||||
#endif
|
||||
|
||||
#if HEATSHRINK_DYNAMIC_ALLOC
|
||||
// Optional replacement of malloc/free
|
||||
#define HEATSHRINK_MALLOC(SZ) malloc(SZ)
|
||||
#define HEATSHRINK_FREE(P, SZ) free(P)
|
||||
#else
|
||||
// Required parameters for static configuration
|
||||
#define HEATSHRINK_STATIC_INPUT_BUFFER_SIZE 32
|
||||
#define HEATSHRINK_STATIC_WINDOW_BITS 8
|
||||
#define HEATSHRINK_STATIC_LOOKAHEAD_BITS 4
|
||||
#endif
|
||||
|
||||
// Turn on logging for debugging
|
||||
#define HEATSHRINK_DEBUGGING_LOGS 0
|
||||
|
||||
// Use indexing for faster compression. (This requires additional space.)
|
||||
#define HEATSHRINK_USE_INDEX 1
|
355
Marlin/src/libs/heatshrink/heatshrink_decoder.cpp
Normal file
355
Marlin/src/libs/heatshrink/heatshrink_decoder.cpp
Normal file
|
@ -0,0 +1,355 @@
|
|||
/**
|
||||
* libs/heatshrink/heatshrink_decoder.cpp
|
||||
*/
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include "heatshrink_decoder.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 == nullptr) 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 == nullptr || in_buf == nullptr || input_size == nullptr)
|
||||
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 == nullptr || out_buf == nullptr || output_size == nullptr)
|
||||
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 == nullptr) { 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;
|
||||
}
|
96
Marlin/src/libs/heatshrink/heatshrink_decoder.h
Normal file
96
Marlin/src/libs/heatshrink/heatshrink_decoder.h
Normal file
|
@ -0,0 +1,96 @@
|
|||
/**
|
||||
* libs/heatshrink/heatshrink_decoder.h
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stddef.h>
|
||||
#include "heatshrink_common.h"
|
||||
#include "heatshrink_config.h"
|
||||
|
||||
typedef enum {
|
||||
HSDR_SINK_OK, /* data sunk, ready to poll */
|
||||
HSDR_SINK_FULL, /* out of space in internal buffer */
|
||||
HSDR_SINK_ERROR_NULL=-1, /* NULL argument */
|
||||
} HSD_sink_res;
|
||||
|
||||
typedef enum {
|
||||
HSDR_POLL_EMPTY, /* input exhausted */
|
||||
HSDR_POLL_MORE, /* more data remaining, call again w/ fresh output buffer */
|
||||
HSDR_POLL_ERROR_NULL=-1, /* NULL arguments */
|
||||
HSDR_POLL_ERROR_UNKNOWN=-2,
|
||||
} HSD_poll_res;
|
||||
|
||||
typedef enum {
|
||||
HSDR_FINISH_DONE, /* output is done */
|
||||
HSDR_FINISH_MORE, /* more output remains */
|
||||
HSDR_FINISH_ERROR_NULL=-1, /* NULL arguments */
|
||||
} HSD_finish_res;
|
||||
|
||||
#if HEATSHRINK_DYNAMIC_ALLOC
|
||||
#define HEATSHRINK_DECODER_INPUT_BUFFER_SIZE(BUF) \
|
||||
((BUF)->input_buffer_size)
|
||||
#define HEATSHRINK_DECODER_WINDOW_BITS(BUF) \
|
||||
((BUF)->window_sz2)
|
||||
#define HEATSHRINK_DECODER_LOOKAHEAD_BITS(BUF) \
|
||||
((BUF)->lookahead_sz2)
|
||||
#else
|
||||
#define HEATSHRINK_DECODER_INPUT_BUFFER_SIZE(_) \
|
||||
HEATSHRINK_STATIC_INPUT_BUFFER_SIZE
|
||||
#define HEATSHRINK_DECODER_WINDOW_BITS(_) \
|
||||
(HEATSHRINK_STATIC_WINDOW_BITS)
|
||||
#define HEATSHRINK_DECODER_LOOKAHEAD_BITS(BUF) \
|
||||
(HEATSHRINK_STATIC_LOOKAHEAD_BITS)
|
||||
#endif
|
||||
|
||||
typedef struct {
|
||||
uint16_t input_size; /* bytes in input buffer */
|
||||
uint16_t input_index; /* offset to next unprocessed input byte */
|
||||
uint16_t output_count; /* how many bytes to output */
|
||||
uint16_t output_index; /* index for bytes to output */
|
||||
uint16_t head_index; /* head of window buffer */
|
||||
uint8_t state; /* current state machine node */
|
||||
uint8_t current_byte; /* current byte of input */
|
||||
uint8_t bit_index; /* current bit index */
|
||||
|
||||
#if HEATSHRINK_DYNAMIC_ALLOC
|
||||
/* Fields that are only used if dynamically allocated. */
|
||||
uint8_t window_sz2; /* window buffer bits */
|
||||
uint8_t lookahead_sz2; /* lookahead bits */
|
||||
uint16_t input_buffer_size; /* input buffer size */
|
||||
|
||||
/* Input buffer, then expansion window buffer */
|
||||
uint8_t buffers[];
|
||||
#else
|
||||
/* Input buffer, then expansion window buffer */
|
||||
uint8_t buffers[(1 << HEATSHRINK_DECODER_WINDOW_BITS(_)) + HEATSHRINK_DECODER_INPUT_BUFFER_SIZE(_)];
|
||||
#endif
|
||||
} heatshrink_decoder;
|
||||
|
||||
#if HEATSHRINK_DYNAMIC_ALLOC
|
||||
/* Allocate a decoder with an input buffer of INPUT_BUFFER_SIZE bytes,
|
||||
* an expansion buffer size of 2^WINDOW_SZ2, and a lookahead
|
||||
* size of 2^lookahead_sz2. (The window buffer and lookahead sizes
|
||||
* must match the settings used when the data was compressed.)
|
||||
* Returns NULL on error. */
|
||||
heatshrink_decoder *heatshrink_decoder_alloc(uint16_t input_buffer_size, uint8_t expansion_buffer_sz2, uint8_t lookahead_sz2);
|
||||
|
||||
/* Free a decoder. */
|
||||
void heatshrink_decoder_free(heatshrink_decoder *hsd);
|
||||
#endif
|
||||
|
||||
/* Reset a decoder. */
|
||||
void heatshrink_decoder_reset(heatshrink_decoder *hsd);
|
||||
|
||||
/* Sink at most SIZE bytes from IN_BUF into the decoder. *INPUT_SIZE is set to
|
||||
* indicate how many bytes were actually sunk (in case a buffer was filled). */
|
||||
HSD_sink_res heatshrink_decoder_sink(heatshrink_decoder *hsd, uint8_t *in_buf, size_t size, size_t *input_size);
|
||||
|
||||
/* Poll for output from the decoder, copying at most OUT_BUF_SIZE bytes into
|
||||
* OUT_BUF (setting *OUTPUT_SIZE to the actual amount copied). */
|
||||
HSD_poll_res heatshrink_decoder_poll(heatshrink_decoder *hsd, uint8_t *out_buf, size_t out_buf_size, size_t *output_size);
|
||||
|
||||
/* Notify the dencoder that the input stream is finished.
|
||||
* If the return value is HSDR_FINISH_MORE, there is still more output, so
|
||||
* call heatshrink_decoder_poll and repeat. */
|
||||
HSD_finish_res heatshrink_decoder_finish(heatshrink_decoder *hsd);
|
Reference in a new issue