463 lines
16 KiB
C++
463 lines
16 KiB
C++
#ifndef SIMPLESYNC_HPP
|
|
#define SIMPLESYNC_HPP
|
|
|
|
#ifdef ARDUINO
|
|
#include <Arduino.h>
|
|
#ifdef __AVR__
|
|
#include "include/nonstd_functional.hpp"
|
|
using nonstd::function;
|
|
#else /* ndef __AVR__ */
|
|
#define STDCLIB
|
|
#endif
|
|
#else /* ndef ARDUINO */
|
|
#define STDCLIB
|
|
#endif
|
|
#ifdef STDCLIB
|
|
#include <cstdint>
|
|
#include <cstdlib>
|
|
#include <cstring>
|
|
#include <functional>
|
|
#include <iostream>
|
|
#include <memory>
|
|
#include <string>
|
|
#include <vector>
|
|
using std::function;
|
|
using std::string;
|
|
#endif
|
|
|
|
namespace simplesync {
|
|
|
|
enum error_t {
|
|
BUFFER_OVERFLOW = (-1),
|
|
INVALID_COMMAND = (-2),
|
|
UNKNOWN_INTERFACE = (-3),
|
|
OUT_OF_MEMORY = (-4),
|
|
CRC_MISSMATCH = (-5),
|
|
|
|
};
|
|
|
|
inline int cobs_encode(uint8_t *buf_in, unsigned int buf_in_size, uint8_t *buf_out,
|
|
unsigned int buf_out_size) {
|
|
unsigned int i_out = 1;
|
|
unsigned int last0 = 0;
|
|
for (unsigned int i_in = 0; i_in < buf_in_size; i_in += 1, i_out += 1) {
|
|
if (i_out >= buf_out_size) return OUT_OF_MEMORY;
|
|
if ((buf_in[i_in] != 0x00) && ((i_out - last0) < 0xFF)) {
|
|
buf_out[i_out] = buf_in[i_in];
|
|
} else {
|
|
buf_out[last0] = i_out - last0;
|
|
last0 = i_out;
|
|
}
|
|
}
|
|
buf_out[last0] = i_out - last0;
|
|
buf_out[i_out] = 0x00;
|
|
return i_out + 1;
|
|
}
|
|
|
|
inline int cobs_decode(uint8_t *buf_in, unsigned int &buf_in_size_used, uint8_t *buf_out,
|
|
unsigned int &buf_out_size_used) {
|
|
unsigned int i_out = 0;
|
|
unsigned int next0 = 0;
|
|
bool skip_next0 = true;
|
|
unsigned int i_in;
|
|
for (i_in = 0; i_in < buf_in_size_used || buf_in[i_in] == 0x00; i_in += 1) {
|
|
if (i_out >= buf_out_size_used) {
|
|
buf_out_size_used = i_out;
|
|
buf_in_size_used = i_in;
|
|
return OUT_OF_MEMORY;
|
|
}
|
|
if (next0 == i_in) {
|
|
if (buf_in[i_in] == 0x00) break;
|
|
if (!skip_next0) buf_out[i_out++] = 0x00;
|
|
next0 = i_in + buf_in[i_in];
|
|
skip_next0 = (next0 - i_in) == 0xFF;
|
|
} else {
|
|
buf_out[i_out++] = buf_in[i_in];
|
|
}
|
|
}
|
|
buf_out_size_used = i_out;
|
|
buf_in_size_used = i_in;
|
|
return 0;
|
|
}
|
|
|
|
inline unsigned int encode_zigzag(int input) {
|
|
return (input << 1) ^ (input >> (sizeof(int) * 8 - 1));
|
|
}
|
|
|
|
inline unsigned int decode_zigzag(int input) { return (input >> 1) ^ (-(input & 1)); }
|
|
|
|
inline unsigned int encode_unsigned_varint(unsigned int input, uint8_t *out) {
|
|
int offset = 0;
|
|
while (input > 0x7F) {
|
|
out[offset++] = input & 0x7F;
|
|
input >>= 7;
|
|
}
|
|
out[offset++] = input | 0x80;
|
|
return offset;
|
|
}
|
|
|
|
inline unsigned int encode_varint(int input, uint8_t *out) {
|
|
return encode_unsigned_varint(encode_zigzag(input), out);
|
|
}
|
|
|
|
inline unsigned int decode_unsigned_varint(uint8_t *input, unsigned int &out) {
|
|
int offset = 0;
|
|
out = 0;
|
|
for (;; offset += 1) {
|
|
out |= ((unsigned int)(input[offset] & 0x7F)) << (7 * offset);
|
|
if (input[offset] & 0x80) break;
|
|
}
|
|
return offset + 1;
|
|
}
|
|
|
|
inline unsigned int decode_varint(uint8_t *input, int &out) {
|
|
unsigned int out_uint;
|
|
int offset = decode_unsigned_varint(input, out_uint);
|
|
out = decode_zigzag(out_uint);
|
|
return offset;
|
|
}
|
|
|
|
inline uint16_t calc_fletcher_checksum(uint8_t *buf, unsigned int buf_size) {
|
|
uint32_t c0, c1;
|
|
for (c0 = c1 = 0; buf_size > 0;) {
|
|
size_t blocklen = buf_size;
|
|
if (blocklen > 5802) blocklen = 5802;
|
|
buf_size -= blocklen;
|
|
do {
|
|
c0 = c0 + *buf++;
|
|
c1 = c1 + c0;
|
|
} while (--blocklen);
|
|
c0 = c0 % 255;
|
|
c1 = c1 % 255;
|
|
}
|
|
return (c1 << 8 | c0);
|
|
}
|
|
|
|
template <typename time_t, typename timedelta_t, unsigned int BUF_SIZE = 256,
|
|
bool DYNAMIC_INTERFACES = false>
|
|
class SimpleSync {
|
|
public:
|
|
class Interface {
|
|
public:
|
|
uint8_t type;
|
|
string key;
|
|
|
|
std::function<int(uint8_t *buf, unsigned int buf_size)> pack;
|
|
std::function<int(uint8_t *buf, unsigned int buf_size)> unpack;
|
|
|
|
bool send_requested = true;
|
|
time_t last_send;
|
|
|
|
timedelta_t update_interval;
|
|
bool update_periodically;
|
|
|
|
void *data;
|
|
bool data_owned;
|
|
|
|
Interface(uint8_t type, string key, void *data,
|
|
std::function<int(uint8_t *buf, unsigned int buf_size)> pack,
|
|
std::function<int(uint8_t *buf, unsigned int buf_size)> unpack)
|
|
: type(type), key(key), pack(pack), unpack(unpack), update_periodically(false),
|
|
data(data), data_owned(false) {}
|
|
~Interface() {
|
|
if (data != nullptr && data_owned) switch (type) {
|
|
case 0x01:
|
|
delete (int *)data;
|
|
break;
|
|
case 0x02:
|
|
delete (string *)data;
|
|
break;
|
|
default:
|
|
free(data);
|
|
break;
|
|
}
|
|
}
|
|
};
|
|
|
|
static Interface *NumberPtr(SimpleSync &sync, string key, int *value_ptr) {
|
|
sync.interfaces.push_back(std::unique_ptr<Interface>(new Interface(
|
|
0x01, key, (void *)value_ptr,
|
|
[=](uint8_t *buf, unsigned int buf_size) -> int { /* pack */
|
|
if (buf_size < sizeof(int) * 8 / 7)
|
|
return BUFFER_OVERFLOW;
|
|
return encode_varint(*value_ptr, buf);
|
|
},
|
|
[=](uint8_t *buf, unsigned int buf_size) -> int { /* unpack */
|
|
buf_size = 1 * buf_size;
|
|
return decode_varint(buf, *value_ptr);
|
|
})));
|
|
return sync.interfaces.back().get();
|
|
}
|
|
|
|
static Interface *NumberPtrPeriodic(SimpleSync &sync, string key, int *value_ptr,
|
|
timedelta_t update_interval) {
|
|
auto i = NumberPtr(sync, key, value_ptr);
|
|
if (i) {
|
|
i->update_interval = update_interval;
|
|
i->update_periodically = true;
|
|
}
|
|
return i;
|
|
}
|
|
|
|
static Interface *Number(SimpleSync &sync, string key) {
|
|
auto v_ptr = new int;
|
|
Interface *i = NumberPtr(sync, key, v_ptr);
|
|
if (i)
|
|
i->data_owned = true;
|
|
else
|
|
delete v_ptr;
|
|
return i;
|
|
}
|
|
|
|
static Interface *StringPtr(SimpleSync &sync, string key, string *value_ptr) {
|
|
sync.interfaces.push_back(std::make_unique<Interface>(
|
|
0x02, key, (void *)value_ptr,
|
|
[=](uint8_t *buf, unsigned int buf_size) -> int { /* pack */
|
|
auto value_len =
|
|
1 + strlen(value_ptr->c_str());
|
|
if (buf_size < value_len)
|
|
return BUFFER_OVERFLOW;
|
|
memcpy(buf, value_ptr->c_str(),
|
|
value_len);
|
|
return value_len;
|
|
},
|
|
[=](uint8_t *buf, unsigned int buf_size) -> int { /* unpack */
|
|
auto strl = strlen((char *)buf) + 1;
|
|
if (strl > buf_size)
|
|
return BUFFER_OVERFLOW;
|
|
value_ptr->assign((char *)buf);
|
|
return strl;
|
|
}));
|
|
return sync.interfaces.back().get();
|
|
}
|
|
|
|
static Interface *StringPtrPeriodic(SimpleSync &sync, string key, string *value_ptr,
|
|
timedelta_t update_interval) {
|
|
auto i = StringPtr(sync, key, value_ptr);
|
|
if (i) {
|
|
i->update_interval = update_interval;
|
|
i->update_periodically = true;
|
|
}
|
|
return i;
|
|
}
|
|
|
|
static Interface *String(SimpleSync &sync, string key) {
|
|
auto v_ptr = new string;
|
|
Interface *i = StringPtr(sync, key, v_ptr);
|
|
if (i)
|
|
i->data_owned = true;
|
|
else
|
|
delete v_ptr;
|
|
return i;
|
|
}
|
|
|
|
// int parse_number(uint8_t *buf, unsigned int buf_size, int* value){
|
|
// return decode_varint(&buf[parsed], *value);
|
|
// }
|
|
|
|
std::vector<std::unique_ptr<Interface>> interfaces;
|
|
|
|
function<int(uint8_t *, unsigned int)> write_pkg;
|
|
SimpleSync(function<int(uint8_t *, unsigned int)> write_pkg) : write_pkg(write_pkg) {}
|
|
|
|
Interface *get_interface(const uint8_t type, const char * const key) const {
|
|
for (auto &i : interfaces) {
|
|
if (i->type == type && strcmp(i->key.c_str(), key) == 0) {
|
|
return i.get();
|
|
}
|
|
}
|
|
return nullptr;
|
|
}
|
|
Interface *get_or_create_interface(uint8_t type, const char * const key) {
|
|
Interface *i_exists = get_interface(type, key);
|
|
if (i_exists) {
|
|
return i_exists;
|
|
}
|
|
if (DYNAMIC_INTERFACES) {
|
|
switch (type) {
|
|
case 0x01:
|
|
return Number(*this, key);
|
|
case 0x02:
|
|
return String(*this, key);
|
|
}
|
|
}
|
|
return nullptr;
|
|
}
|
|
|
|
int parse_pkg(uint8_t *buf, unsigned int buf_size) {
|
|
if (buf_size < 4) return CRC_MISSMATCH;
|
|
uint16_t crc = calc_fletcher_checksum(buf, buf_size - 2);
|
|
if (buf[--buf_size] != (crc & 0xFF) || (buf[--buf_size] != (crc >> 8)))
|
|
return CRC_MISSMATCH;
|
|
for (unsigned int parsed = 0; parsed < buf_size;) {
|
|
uint8_t type = buf[parsed];
|
|
switch (type) {
|
|
case 0x00: {
|
|
for (auto &ni : interfaces) ni->send_requested = true;
|
|
} break;
|
|
case 0x01:
|
|
case 0x02: {
|
|
char *key = (char *)&buf[parsed + 1];
|
|
parsed += 1 + strlen(key) + 1;
|
|
Interface *ni = get_or_create_interface(type, key);
|
|
if (ni == nullptr) return UNKNOWN_INTERFACE;
|
|
int unpacked_bytes = ni->unpack(buf + parsed, buf_size - parsed);
|
|
if (unpacked_bytes >= 0) {
|
|
parsed += unpacked_bytes;
|
|
} else { // ERROR -> return
|
|
return unpacked_bytes;
|
|
}
|
|
} break;
|
|
default:
|
|
// Error: Unknown datatype, ignore rest of the packet
|
|
return -1;
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
void request_all_interfaces() const {
|
|
uint8_t buf[] = {0x00, 0x00};
|
|
unsigned int buf_len_used = sizeof(buf);
|
|
uint8_t cobs_buf[10];
|
|
unsigned int cobs_buf_needed = cobs_encode(buf, buf_len_used, cobs_buf, BUF_SIZE);
|
|
write_pkg(cobs_buf, cobs_buf_needed);
|
|
}
|
|
|
|
private:
|
|
void encode_and_write_pkg(uint8_t *buf, unsigned int buf_size) {
|
|
if (buf_size <= 2) {
|
|
|
|
return;}
|
|
uint8_t cobs_buf[BUF_SIZE];
|
|
uint16_t crc = calc_fletcher_checksum(buf, buf_size);
|
|
buf[buf_size++] = crc >> 8;
|
|
buf[buf_size++] = crc & 0xFF;
|
|
unsigned int cobs_buf_needed = cobs_encode(buf, buf_size, cobs_buf, BUF_SIZE);
|
|
write_pkg(cobs_buf, cobs_buf_needed);
|
|
}
|
|
|
|
public:
|
|
void update(time_t time) {
|
|
uint8_t buf[BUF_SIZE];
|
|
unsigned int buf_len_used = 0;
|
|
for (auto &ni : interfaces) {
|
|
if (ni->send_requested ||
|
|
(ni->update_periodically &&
|
|
(ni->last_send > time || ni->last_send <= time - ni->update_interval))) {
|
|
unsigned int strl = strlen(ni->key.c_str()) + 1;
|
|
unsigned int buf_len_needed =
|
|
1 + strl + 32; // TODO get real pack size (not always 32)
|
|
buf_len_needed += buf_len_needed / 0xFF + 3 + 2; // COBS overhead
|
|
if (BUF_SIZE < buf_len_needed) continue; // Error: update to large, skip interface
|
|
if (BUF_SIZE < buf_len_used + buf_len_needed) {
|
|
encode_and_write_pkg(buf, buf_len_used);
|
|
buf_len_used = 0;
|
|
}
|
|
buf[buf_len_used++] = ni->type;
|
|
memcpy((char *)&buf[buf_len_used], ni->key.c_str(), strl);
|
|
buf_len_used += strl;
|
|
int pack_len = ni->pack(&buf[buf_len_used], BUF_SIZE - buf_len_used);
|
|
if (pack_len < 0) {
|
|
// ERROR -> return
|
|
buf_len_used = 0;
|
|
return;
|
|
}
|
|
buf_len_used += pack_len;
|
|
ni->last_send = time;
|
|
ni->send_requested = false;
|
|
}
|
|
}
|
|
encode_and_write_pkg(buf, buf_len_used);
|
|
buf_len_used = 0;
|
|
}
|
|
|
|
int parse_stream_buf(uint8_t *buf, unsigned int buf_size) {
|
|
int parsed = 0;
|
|
for (unsigned int i = 0; i < buf_size; i += 1) {
|
|
if (buf[i] == 0x00) {
|
|
unsigned int buf_used = buf_size - parsed;
|
|
uint8_t packet_buf[BUF_SIZE];
|
|
unsigned int packed_buf_used = sizeof(packet_buf);
|
|
if (cobs_decode(buf + parsed, buf_used, packet_buf, packed_buf_used) == 0) {
|
|
parse_pkg(packet_buf, packed_buf_used);
|
|
}
|
|
parsed = i + 1;
|
|
}
|
|
}
|
|
return parsed;
|
|
}
|
|
|
|
protected:
|
|
uint8_t stream_buf[BUF_SIZE];
|
|
unsigned int stream_buf_used = 0;
|
|
void handle_stream_buf(unsigned int buf_new_size) {
|
|
for (int i = stream_buf_used - buf_new_size; i < (int)stream_buf_used; i += 1) {
|
|
if (stream_buf[i] == 0x00) {
|
|
unsigned int stream_buf_pkg_delimiter = i;
|
|
uint8_t packet_buf[BUF_SIZE];
|
|
unsigned int packed_buf_used = sizeof(packet_buf);
|
|
if (cobs_decode(stream_buf, stream_buf_pkg_delimiter, packet_buf,
|
|
packed_buf_used) == 0) {
|
|
parse_pkg(packet_buf, packed_buf_used);
|
|
}
|
|
i += 1;
|
|
memmove(stream_buf, stream_buf + i, stream_buf_used - i);
|
|
stream_buf_used -= i;
|
|
i = -1;
|
|
}
|
|
}
|
|
}
|
|
|
|
public:
|
|
int handle_stream(const uint8_t *buf_new, unsigned int buf_new_size) {
|
|
if (buf_new_size + stream_buf_used >= BUF_SIZE) {
|
|
/* ERROR: buffer overflow -> discared data */
|
|
stream_buf_used = 0;
|
|
return BUFFER_OVERFLOW;
|
|
}
|
|
memcpy(stream_buf + stream_buf_used, buf_new, buf_new_size);
|
|
stream_buf_used += buf_new_size;
|
|
handle_stream_buf(buf_new_size);
|
|
return 0;
|
|
}
|
|
|
|
int *get_number(const char * const key) {
|
|
Interface *i = get_interface(0x01, key);
|
|
if (i) return (int *)i->data;
|
|
return nullptr;
|
|
}
|
|
|
|
string *get_string(const char * const key) {
|
|
Interface *i = get_interface(0x02, key);
|
|
if (i) return (string *)i->data;
|
|
return nullptr;
|
|
}
|
|
|
|
#ifdef ARDUINO
|
|
SimpleSync(Print *ostream)
|
|
: SimpleSync([ostream](uint8_t *buf, unsigned int buf_size) {
|
|
if (ostream) {
|
|
ostream->write(buf, buf_size);
|
|
return 0;
|
|
}
|
|
return -1;
|
|
}) {}
|
|
int handle_stream(Stream &in_stream) {
|
|
unsigned int buf_new_size = in_stream.available();
|
|
if (buf_new_size + stream_buf_used > BUF_SIZE) {
|
|
/* ERROR: buffer overflow -> discared data */
|
|
stream_buf_used = 0;
|
|
return BUFFER_OVERFLOW;
|
|
}
|
|
in_stream.readBytes(stream_buf + stream_buf_used, buf_new_size);
|
|
stream_buf_used += buf_new_size;
|
|
handle_stream_buf(buf_new_size);
|
|
return 0;
|
|
}
|
|
#endif
|
|
};
|
|
|
|
} // namespace simplesync
|
|
#endif // SIMPLESYNC_HPP
|