Skip to content

Instantly share code, notes, and snippets.

@rroohhh

rroohhh/remote_protocol.cpp

Created December 17, 2020 15:19
Show Gist options
  • Save rroohhh/4cb563653469b381f3136a0b5f7df1aa to your computer and use it in GitHub Desktop.
Save rroohhh/4cb563653469b381f3136a0b5f7df1aa to your computer and use it in GitHub Desktop.
Download ZIP
Raw
remote_protocol.cpp
#include <cinttypes>
#include <cstdlib>
#include <cstring>
#include <cstdio>
#include <functional>
const uint8_t READ_FAILED = 0;
const uint8_t READ_SUCCESS = 1;
const uint8_t MAX_FIELDS = 8;
const uint32_t FIELDS_BUFFER_SIZE = 512;
const uint8_t BUFFER_SIZE = 128;
const uint8_t MAX_OUTSTANDING_REQUESTS = 8;
const char * FIELD_SEPERATOR = "";
const char * EOT = "";
const char * SEPERATOR = "";
uint8_t crc_table[256] = {
0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31, 0x24, 0x23, 0x2a, 0x2d, 0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65, 0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d, 0xe0, 0xe7, 0xee, 0xe9, 0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd, 0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1, 0xb4, 0xb3, 0xba, 0xbd, 0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2, 0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea, 0xb7, 0xb0, 0xb9, 0xbe, 0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a, 0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0d, 0x0a, 0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42, 0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a, 0x89, 0x8e, 0x87, 0x80, 0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4, 0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8, 0xdd, 0xda, 0xd3, 0xd4, 0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c, 0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44, 0x19, 0x1e, 0x17, 0x10, 0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34, 0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f, 0x6a, 0x6d, 0x64, 0x63, 0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b, 0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13, 0xae, 0xa9, 0xa0, 0xa7, 0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83, 0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef, 0xfa, 0xfd, 0xf4, 0xf3
};
uint8_t crc8(uint8_t old, uint8_t input) {
return crc_table[input ^ old];
}
template<typename T>
auto parse_string(char * string) -> T;
template<>
auto parse_string(char * string) -> float {
return strtof(string, NULL);
}
template<>
auto parse_string(char * string) -> double {
return strtod(string, NULL);
}
template<>
auto parse_string(char * string) -> uint32_t {
return strtol(string, NULL, 10);
}
template<typename W, typename R>
class ControlDaemonInterface {
private:
uint16_t request_id_counter = 0;
class BufferedWriter {
private:
char buffer[BUFFER_SIZE] = { 0 };
W writer;
uint32_t data_amount = 0;
public:
uint8_t crc = 0;
// BufferedWriter(W writer) : writer(writer) {};
uint8_t write(const char * data, uint32_t len) {
do {
for(; (data_amount < BUFFER_SIZE) && (len > 0); data_amount++, len--) {
crc = crc8(crc, *data);
buffer[data_amount] = *data++;
}
if ((data_amount == BUFFER_SIZE) && (len > 0)) {
flush(/* reset_crc = */ false);
}
} while (len > 0);
return crc;
}
void flush(bool reset_crc = true) {
uint32_t written_total = 0;
while (written_total < data_amount) {
written_total += writer.write(&buffer[written_total], BUFFER_SIZE - written_total);
}
if (reset_crc) {
crc = 0;
}
data_amount = 0;
}
};
BufferedWriter writer;
template<typename C>
class BufferedReader {
private:
char buffer[BUFFER_SIZE] = { 0 };
R reader;
uint32_t pos = 0;
uint32_t data_amount = 0;
public:
// BufferedReader(R reader) : reader(reader) {};
auto read_until(char stop_char, C callback, bool nonblocking = true) -> void {
auto found = false;
while (!found) {
auto start = pos;
auto max = pos + data_amount;
for (; pos < max; pos++) {
if (buffer[pos] == stop_char) {
found = true;
pos++;
break;
}
}
callback(false, &buffer[start], pos - start);
if ((!found) && (pos < BUFFER_SIZE)) {
data_amount = reader.read(&buffer[pos], BUFFER_SIZE - pos);
if ((data_amount == 0) && nonblocking) {
return;
}
} else { // buffer full, or we found something
pos = 0;
data_amount = 0;
callback(true, nullptr, 0);
return;
}
}
}
};
BufferedReader<std::function<void(bool, char*, uint32_t)>> reader;
// template<typename T>
// using RegReadCallback = std::function<void(uint8_t, T)>;
// using WriteCallback = std::function<void(const char[BUFFER_SIZE])>;
// using ReadCallback = std::function<void(const char[BUFFER_SIZE])>;
using Callback = std::function<void(char**)>;
class RequestBuffer {
private:
class Request {
public:
char id[4];
Callback callback;
Request() {}
Request(char req_id[4], Callback callback) : callback(callback) {
id[0] = req_id[0];
id[1] = req_id[1];
id[2] = req_id[2];
id[3] = req_id[3];
}
};
Request outstanding_requests[MAX_OUTSTANDING_REQUESTS];
bool free_outstanding_requests[MAX_OUTSTANDING_REQUESTS];
public:
RequestBuffer() {
for (uint32_t i = 0; i < MAX_OUTSTANDING_REQUESTS; i++) {
free_outstanding_requests[i] = true;
}
}
auto push(char id[4], Callback callback) {
for (uint32_t i = 0; i < MAX_OUTSTANDING_REQUESTS; i++) {
if (free_outstanding_requests[i]) {
free_outstanding_requests[i] = false;
outstanding_requests[i] = Request(id, callback);
break;
}
}
}
auto notify_callback(char id[4], char ** data) {
for (uint32_t i = 0; i < MAX_OUTSTANDING_REQUESTS; i++) {
if (!free_outstanding_requests[i]) {
if (!strncmp(outstanding_requests[i].id, id, 4)) {
free_outstanding_requests[i] = true;
outstanding_requests[i].callback(data);
break;
}
}
}
}
};
RequestBuffer request_buffer;
void send_request(const char type, const char ** fields, Callback callback) {
char id_buf[5];
snprintf(id_buf, 5, "%04hx", request_id_counter++);
request_buffer.push(id_buf, callback);
writer.write(&type, 1);
writer.write(id_buf, 4);
do {
writer.write(FIELD_SEPERATOR, 1);
writer.write(*fields, strlen(*fields));
} while (*(++fields));
writer.write(EOT, 1);
snprintf(id_buf, 3, "%02hhx", writer.crc);
writer.write(id_buf, 2);
writer.write(SEPERATOR, 1);
writer.flush();
}
struct ParserState {
enum State {
READING_TYPE,
READING_ID0,
READING_ID1,
READING_ID2,
READING_ID3,
READING_FIELDS,
READING_CRC0,
READING_CRC1,
READING_SEPERATOR,
};
State state;
char type;
char id[4];
char transmitted_crc[3];
uint8_t crc;
uint8_t calculated_crc;
uint32_t field_num;
char* fields[MAX_FIELDS];
};
ParserState parser_state;
auto process_incoming_data(bool buffer_overrun, char * data, uint32_t bytes) {
using State = typename ParserState::State;
if (buffer_overrun) {
parser_state.crc = 0;
parser_state.state = State::READING_TYPE;
return;
}
for (uint32_t i = 0; i < bytes; i++) {
auto byte = data[i];
parser_state.crc = crc8(parser_state.crc, byte);
switch (parser_state.state) {
case State::READING_TYPE: {
parser_state.field_num = 0;
for (uint32_t i = 0; i < MAX_FIELDS; i++) {
parser_state.fields[i] = nullptr;
}
parser_state.type = byte;
parser_state.state = State::READING_ID0;
break;
}
case State::READING_ID0: {
parser_state.id[0] = byte;
parser_state.state = State::READING_ID1;
break;
}
case State::READING_ID1: {
parser_state.id[1] = byte;
parser_state.state = State::READING_ID2;
break;
}
case State::READING_ID2: {
parser_state.id[2] = byte;
parser_state.state = State::READING_ID3;
break;
}
case State::READING_ID3: {
parser_state.id[3] = byte;
parser_state.state = State::READING_FIELDS;
break;
}
case State::READING_FIELDS: {
if (byte == *FIELD_SEPERATOR) {
data[i] = '\0';
if (parser_state.field_num < MAX_FIELDS) {
parser_state.fields[parser_state.field_num++] = &data[i + 1];
} else {
// TODO(robin): what to do about fields overrun?
}
} else if (byte == *EOT) {
data[i] = '\0';
parser_state.calculated_crc = parser_state.crc;
parser_state.state = State::READING_CRC0;
}
break;
}
case State::READING_CRC0: {
parser_state.transmitted_crc[0] = byte;
parser_state.state = State::READING_CRC1;
break;
}
case State::READING_CRC1: {
parser_state.transmitted_crc[1] = byte;
uint8_t crc = strtol(parser_state.transmitted_crc, nullptr, 16);
if (crc != parser_state.calculated_crc) {
// TODO(robin): what to do about that?
}
parser_state.state = State::READING_SEPERATOR;
break;
}
case State::READING_SEPERATOR: {
if (byte == *SEPERATOR) {
// TODO(robin): handle other types
if (parser_state.type == 'R') {
request_buffer.notify_callback(parser_state.id, parser_state.fields);
}
parser_state.calculated_crc = 0;
parser_state.state = State::READING_TYPE;
} else {
// TODO(robin): error handling?
parser_state.crc = 0;
parser_state.state = State::READING_TYPE;
}
break;
}
}
}
}
public:
void handle_incoming() {
reader.read_until(SEPERATOR[0], [&](bool overrun, char * data, uint32_t bytes) {
process_incoming_data(overrun, data, bytes);
});
}
template<typename T, typename C>
void read(const char * name, C callback) {
const char * fields[2] = { name, nullptr };
send_request('G', fields,
[&](char ** fields) {
if (!strcmp(fields[0], "OK")) {
callback(READ_SUCCESS, parse_string<T>(fields[1]));
} else {
callback(READ_FAILED, 0);
}
});
}
// X|xxxx|FIELD_SEPERATOR|FIELD_SEPERATOR.join(["analog_gain", "5"])|EOT|CRC|\0
};
struct DummyWriter {
uint32_t write(const char * data, uint32_t size) {
for (uint32_t i = 0; i < size; i++) {
printf("%c", data[i]);
}
return size;
}
};
struct DummyReader {
const char values[32] = {
'R',
'0',
'0',
'0',
'1',
0x1e,
'O',
'K',
0x1e,
'1',
'.',
'2',
0x04,
'e',
'b',
0,
'R',
'0',
'0',
'0',
'0',
0x1e,
'O',
'K',
0x1e,
'1',
'.',
'4',
0x04,
'e',
'c',
0
};
const uint8_t len = 32;
uint8_t pos = 0;
uint32_t read(char * data, uint32_t amount) {
if (pos < len) {
data[0] = values[pos++];
return 1;
} else {
return 0;
}
}
};
int main() {
auto control_daemon_interface = ControlDaemonInterface<DummyWriter, DummyReader>();
control_daemon_interface.read<float>("analog_gain",
[](uint8_t status, float value) {
printf("[0] read analog_gain, status: %d, value: %f\n", status, value);
});
printf("\n");
control_daemon_interface.read<float>("analog_gain",
[](uint8_t status, float value) {
printf("[1] read analog_gain, status: %d, value: %f\n", status, value);
});
printf("\n");
control_daemon_interface.handle_incoming();
control_daemon_interface.handle_incoming();
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment