447 lines
18 KiB
C++
447 lines
18 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),
|
|
ID_INVALID = (-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> 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;
|
|
|
|
Interface(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)
|
|
: type(type), key(key), pack(pack), unpack(unpack), update_periodically(false) {}
|
|
Interface(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,
|
|
timedelta_t update_interval)
|
|
: Interface(type, key, pack, unpack), update_interval(update_interval),
|
|
update_periodically(true) {}
|
|
};
|
|
|
|
template <typename int_t> static void Number(SimpleSync &sync, string key, int_t *value_ptr) {
|
|
sync.interfaces.push_back(std::make_unique<Interface>(
|
|
0x01, key,
|
|
[=](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);
|
|
}));
|
|
}
|
|
|
|
static void String(SimpleSync &sync, string key, string *value_ptr) {
|
|
sync.interfaces.push_back(std::make_unique<Interface>(
|
|
0x02, key,
|
|
[=](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;
|
|
}));
|
|
}
|
|
|
|
// 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;
|
|
function<void(const std::unique_ptr<Interface> &, char *, int)> interface_update;
|
|
SimpleSync(function<int(uint8_t *, unsigned int)> write_pkg,
|
|
function<void(const std::unique_ptr<Interface> &, char *, int)> interface_update)
|
|
: write_pkg(write_pkg), interface_update(interface_update) {}
|
|
|
|
Interface &get_or_create_interface(uint8_t type, char *key) {
|
|
for (auto &i : interfaces) {
|
|
if (i->type == type && strcmp(i->key.c_str(), key) == 0) {
|
|
return *i;
|
|
}
|
|
}
|
|
}
|
|
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);
|
|
unsigned 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);
|
|
}
|
|
|
|
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) {
|
|
uint8_t cobs_buf[BUF_SIZE];
|
|
uint16_t crc = calc_fletcher_checksum(buf, buf_len_used);
|
|
buf[buf_len_used++] = crc >> 8;
|
|
buf[buf_len_used++] = crc & 0xFF;
|
|
unsigned int cobs_buf_needed =
|
|
cobs_encode(buf, buf_len_used, cobs_buf, BUF_SIZE);
|
|
write_pkg(cobs_buf, cobs_buf_needed);
|
|
buf_len_used = 0;
|
|
}
|
|
buf[buf_len_used++] = ni->type;
|
|
memcpy((char *)&buf[buf_len_used], ni->key.c_str(), strl);
|
|
ni->pack(&buf[buf_len_used], BUF_SIZE - buf_len_used);
|
|
ni->last_send = time;
|
|
ni->send_requested = false;
|
|
|
|
}
|
|
}
|
|
if (buf_len_used == 0) return;
|
|
uint8_t cobs_buf[BUF_SIZE];
|
|
uint16_t crc = calc_fletcher_checksum(buf, buf_len_used);
|
|
buf[buf_len_used++] = crc >> 8;
|
|
buf[buf_len_used++] = crc & 0xFF;
|
|
unsigned int cobs_buf_needed = cobs_encode(buf, buf_len_used, cobs_buf, BUF_SIZE);
|
|
write_pkg(cobs_buf, cobs_buf_needed);
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
#ifdef ARDUINO
|
|
SimpleSync(Print *ostream, function<void(NumberInterface *&, char *, int)> number_update,
|
|
function<void(StringInterface *&, char *, char *)> string_update)
|
|
: write_pkg([ostream](uint8_t *buf, unsigned int buf_size) {
|
|
if (ostream) {
|
|
ostream->write(buf, buf_size);
|
|
return 0;
|
|
}
|
|
return -1;
|
|
}),
|
|
number_update(number_update), string_update(string_update) {}
|
|
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
|
|
};
|
|
|
|
template <typename time_t, typename timedelta_t, unsigned int buf_len = 256,
|
|
unsigned int str_len_max = 32>
|
|
class SimpleSyncStatic : public SimpleSync<time_t, timedelta_t, buf_len, str_len_max> {
|
|
public:
|
|
SimpleSyncStatic(function<int(uint8_t *, unsigned int)> write_pkg)
|
|
: SimpleSync<time_t, timedelta_t, buf_len, str_len_max>(write_pkg, nullptr, nullptr){};
|
|
#ifdef ARDUINO
|
|
SimpleSyncStatic(Print *ostream)
|
|
: SimpleSync<time_t, timedelta_t, buf_len, str_len_max>(ostream, nullptr, nullptr) {}
|
|
#endif
|
|
};
|
|
template <typename time_t, typename timedelta_t, unsigned int buf_len = 256,
|
|
unsigned int str_len_max = 32>
|
|
class SimpleSyncDynamic : public SimpleSync<time_t, timedelta_t, buf_len, str_len_max> {
|
|
std::vector<typename SimpleSync<time_t, timedelta_t, buf_len, str_len_max>::NumberInterface *>
|
|
dyn_number_interfaces;
|
|
std::vector<typename SimpleSync<time_t, timedelta_t, buf_len, str_len_max>::StringInterface *>
|
|
dyn_string_interfaces;
|
|
std::vector<char *> dyn_keys;
|
|
|
|
public:
|
|
SimpleSyncDynamic(
|
|
function<int(uint8_t *, unsigned int)> write_pkg,
|
|
function<
|
|
void(typename SimpleSync<time_t, timedelta_t, buf_len, str_len_max>::NumberInterface *&,
|
|
char *, int)>
|
|
dyn_number_update,
|
|
function<
|
|
void(typename SimpleSync<time_t, timedelta_t, buf_len, str_len_max>::StringInterface *&,
|
|
char *, char *)>
|
|
dyn_string_update,
|
|
timedelta_t dyn_update_feq)
|
|
: SimpleSync<time_t, timedelta_t, buf_len, str_len_max>(
|
|
write_pkg,
|
|
[this, dyn_number_update, dyn_update_feq](
|
|
typename SimpleSync<time_t, timedelta_t, buf_len, str_len_max>::NumberInterface *
|
|
&ni,
|
|
char *key, int value) {
|
|
if (ni == nullptr) {
|
|
auto allocated_key = (char *)malloc(strlen(key) + 1);
|
|
strcpy(allocated_key, key);
|
|
dyn_keys.push_back(allocated_key);
|
|
ni = new SimpleSync<time_t, timedelta_t, buf_len,
|
|
str_len_max>::NumberInterface(*this, allocated_key,
|
|
dyn_update_feq);
|
|
ni->send_requested = false;
|
|
dyn_number_interfaces.push_back(ni);
|
|
}
|
|
dyn_number_update(ni, key, value);
|
|
},
|
|
[this, dyn_string_update, dyn_update_feq](
|
|
typename SimpleSync<time_t, timedelta_t, buf_len, str_len_max>::StringInterface *
|
|
&ni,
|
|
char *key, char *value) {
|
|
if (ni == nullptr) {
|
|
auto allocated_key = (char *)malloc(strlen(key) + 1);
|
|
strcpy(allocated_key, key);
|
|
dyn_keys.push_back(allocated_key);
|
|
ni = new SimpleSync<time_t, timedelta_t, buf_len,
|
|
str_len_max>::StringInterface(*this, allocated_key,
|
|
dyn_update_feq);
|
|
ni->send_requested = false;
|
|
dyn_string_interfaces.push_back(ni);
|
|
}
|
|
dyn_string_update(ni, key, value);
|
|
}){};
|
|
~SimpleSyncDynamic() {
|
|
for (auto i : dyn_number_interfaces) delete i;
|
|
for (auto i : dyn_string_interfaces) delete i;
|
|
for (auto i : dyn_keys) free(i);
|
|
};
|
|
#ifdef ARDUINO
|
|
SimpleSyncDynamic(Print *ostream)
|
|
: SimpleSync<time_t, timedelta_t, buf_len, str_len_max>(ostream, nullptr, nullptr) {}
|
|
#endif
|
|
};
|
|
} // namespace simplesync
|
|
#endif // SIMPLESYNC_HPP
|