From ae3970ad5e1b3495726b70d5653c2424fccfba07 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Sun, 11 Dec 2022 16:13:03 +0100 Subject: add serial parser from robotrun to project --- shared/bin.c | 93 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 93 insertions(+) create mode 100644 shared/bin.c (limited to 'shared/bin.c') diff --git a/shared/bin.c b/shared/bin.c new file mode 100644 index 0000000..fdceb30 --- /dev/null +++ b/shared/bin.c @@ -0,0 +1,93 @@ +#include +#include + +#include "bin.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#define CD_ENDIAN_LITTLE (1) +#define CD_ENDIAN_BIG (0) + +#define _SHIFT_0B (8 * 0) +#define _SHIFT_1B (8 * 1) +#define _SHIFT_2B (8 * 2) +#define _SHIFT_3B (8 * 3) +#define _BYTE_0 ((uint32_t)0xff << (_SHIFT_0B)) +#define _BYTE_1 ((uint32_t)0xff << (_SHIFT_1B)) +#define _BYTE_2 ((uint32_t)0xff << (_SHIFT_2B)) +#define _BYTE_3 ((uint32_t)0xff << (_SHIFT_3B)) + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wshift-count-overflow" +cd_s_bin *cd_bin_from_uint8_t(uint8_t data) { + size_t size = 1; + cd_s_bin *ret = malloc(sizeof(cd_s_bin) + sizeof(uint8_t) * size); + ret->bytes = size; + ret->data[0] = data; + return ret; +} + +cd_s_bin *cd_bin_from_uint16_t(uint16_t data) { + size_t size = 2; + cd_s_bin *ret = malloc(sizeof(cd_s_bin) + sizeof(uint8_t) * size); + data = cd_bin_hton16(data); + ret->bytes = size; + ret->data[0] = (data & _BYTE_1) >> _SHIFT_1B; + ret->data[1] = (data & _BYTE_0) >> _SHIFT_0B; + return ret; +} + +cd_s_bin *cd_bin_from_uint32_t(uint32_t data) { + size_t size = 4; + cd_s_bin *ret = malloc(sizeof(cd_s_bin) + sizeof(uint8_t) * size); + data = cd_bin_hton32(data); + ret->bytes = size; + ret->data[0] = (data & _BYTE_3) >> _SHIFT_3B; + ret->data[1] = (data & _BYTE_2) >> _SHIFT_2B; + ret->data[2] = (data & _BYTE_1) >> _SHIFT_1B; + ret->data[3] = (data & _BYTE_0) >> _SHIFT_0B; + return ret; +} + +uint32_t cd_bin_hton32(uint32_t h32) { + if (g_cd_endianness == CD_ENDIAN_BIG) return h32; + return ((h32 & _BYTE_0) << _SHIFT_3B) | ((h32 & _BYTE_1) << _SHIFT_1B) | + ((h32 & _BYTE_2) >> _SHIFT_1B) | ((h32 & _BYTE_3) >> _SHIFT_3B); +} +#pragma GCC diagnostic pop + +uint16_t cd_bin_hton16(uint16_t h16) { + if (g_cd_endianness == CD_ENDIAN_BIG) return h16; + return ((h16 & _BYTE_0) << _SHIFT_1B) | ((h16 & _BYTE_1) >> _SHIFT_1B); +} + +uint32_t cd_bin_ntoh32(uint32_t n32) { return cd_bin_hton32(n32); } +uint16_t cd_bin_ntoh16(uint16_t n16) { return cd_bin_hton16(n16); } + +cd_s_bin *cd_bin_s_alloc(uint16_t bytes, uint8_t *data) { + cd_s_bin *temp = malloc(sizeof(cd_s_bin) + sizeof(uint8_t) * bytes); + temp->bytes = bytes; + memcpy(&temp->data, data, bytes); + return temp; +} + +cd_s_bin *cd_bin_s_cat(cd_s_bin *a, cd_s_bin *b) { + uint8_t data[a->bytes + b->bytes]; + memcpy(data, a->data, a->bytes); + memcpy(data + a->bytes, b->data, b->bytes); + cd_s_bin *c = cd_bin_s_alloc(a->bytes + b->bytes, data); + free(a); + free(b); + return c; +} + +void cd_bin_repl_hton32(uint32_t *h32) { *h32 = cd_bin_hton32(*h32); } +void cd_bin_repl_hton16(uint16_t *h16) { *h16 = cd_bin_hton16(*h16); } +void cd_bin_repl_ntoh32(uint32_t *h32) { *h32 = cd_bin_ntoh32(*h32); } +void cd_bin_repl_ntoh16(uint16_t *h16) { *h16 = cd_bin_ntoh16(*h16); } + +#ifdef __cplusplus +} +#endif -- cgit v1.2.3 From 61dd8a5ee66734dfd53ee9be725c9e71fd8d5414 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Mon, 12 Dec 2022 13:57:33 +0100 Subject: implement DYN_MEMBER_SIZEOF macro correctly and implement cd_cmd_response_sizeof --- confui/mainwindow.cpp | 5 ++++- confui/serial.h | 2 +- shared/bin.c | 8 ++++++++ shared/bin.h | 16 ++++++++++++++++ shared/protocol-tests/ping-response.bin | Bin 0 -> 10 bytes shared/protocol.c | 32 +++++++++++++++++++++++++++----- 6 files changed, 56 insertions(+), 7 deletions(-) create mode 100644 shared/protocol-tests/ping-response.bin (limited to 'shared/bin.c') diff --git a/confui/mainwindow.cpp b/confui/mainwindow.cpp index 1fd34c4..b5bbdf0 100644 --- a/confui/mainwindow.cpp +++ b/confui/mainwindow.cpp @@ -13,7 +13,10 @@ CDMeshConnector *g_cd_mesh_connector = nullptr; CDSerialConnector *g_cd_serial = nullptr; QApplication* g_cd_app = nullptr; -CDMainWindow::~CDMainWindow() { delete this->mesh_connector; } +CDMainWindow::~CDMainWindow() { + delete g_cd_mesh_connector; + delete g_cd_serial; +} CDMainWindow::CDMainWindow(QWidget *parent) : QMainWindow(parent) { g_cd_mesh_connector = new CDMeshConnector(); diff --git a/confui/serial.h b/confui/serial.h index 22f2542..aa0508f 100644 --- a/confui/serial.h +++ b/confui/serial.h @@ -11,7 +11,7 @@ using std::vector; class CDSerialConnector { public: CDSerialConnector(); - ~CDSerialConnector(); + virtual ~CDSerialConnector(); /** @brief get list of available serial ports */ static vector get_ports(); diff --git a/shared/bin.c b/shared/bin.c index fdceb30..875d013 100644 --- a/shared/bin.c +++ b/shared/bin.c @@ -65,6 +65,14 @@ uint16_t cd_bin_hton16(uint16_t h16) { uint32_t cd_bin_ntoh32(uint32_t n32) { return cd_bin_hton32(n32); } uint16_t cd_bin_ntoh16(uint16_t n16) { return cd_bin_hton16(n16); } +uint32_t cd_bin_ntohd(uint8_t* n, size_t s) { return cd_bin_htond(n, s); } + +uint32_t cd_bin_htond(uint8_t* h, size_t s) { + if (s == sizeof(uint8_t)) return *h; + else if (s == sizeof(uint16_t)) return cd_bin_hton16(*(uint16_t*) h); + else if (s == sizeof(uint32_t)) return cd_bin_hton32(*(uint32_t*) h); + else return 0; +} cd_s_bin *cd_bin_s_alloc(uint16_t bytes, uint8_t *data) { cd_s_bin *temp = malloc(sizeof(cd_s_bin) + sizeof(uint8_t) * bytes); diff --git a/shared/bin.h b/shared/bin.h index 35d2bc4..0e16ec0 100644 --- a/shared/bin.h +++ b/shared/bin.h @@ -10,6 +10,7 @@ */ #include +#include #ifdef __cplusplus extern "C" { @@ -50,6 +51,21 @@ uint32_t cd_bin_ntoh32(uint32_t n32); /** @brief convert 16-bit value from network (big-endian) to host endian */ uint16_t cd_bin_ntoh16(uint16_t n16); +/** + * @brief convert (8*s)-bit value from network (big-endian) to host endian + * (dynamic size) + * + * @param n pointer to number + * @param s size of number in bytes + * + * @return 32-bit integer regardless of `s` + * + * this function is exclusively used by the CD_DYN_MEMBER_SIZEOF macro in + * shared/protocol.c + */ +uint32_t cd_bin_ntohd(uint8_t* n, size_t s); +uint32_t cd_bin_htond(uint8_t* h, size_t s); + /** @brief replace 32-bit value from host endian to network (big-endian) */ void cd_bin_repl_hton32(uint32_t *h32); /** @brief replace 16-bit value from host endian to network (big-endian) */ diff --git a/shared/protocol-tests/ping-response.bin b/shared/protocol-tests/ping-response.bin new file mode 100644 index 0000000..6067349 Binary files /dev/null and b/shared/protocol-tests/ping-response.bin differ diff --git a/shared/protocol.c b/shared/protocol.c index 474398a..1d66f17 100644 --- a/shared/protocol.c +++ b/shared/protocol.c @@ -1,3 +1,5 @@ +#include + #include "protocol.h" #ifdef __cplusplus @@ -16,13 +18,33 @@ size_t cd_cmd_sizeof(uint8_t data[CD_SERIAL_READ_BUFFER_SIZE], uint8_t data_leng return length; } -#define CD_DYN_MEMBER_SIZEOF(struct_t, length_byte, trailing_type) \ - sizeof(struct_t) + \ - (data->bytes > length_byte ? (sizeof(trailing_type) * data->data[length_byte]) : 0) +/** + * @brief macro to calculate size of message based on struct with member to + * indicate length of dynamic (last) field + * + * @param data cd_s_bin pointer to currently received data + * @param struct_t message struct + * @param length_field struct field with dynamic length + * + * @return size_t with calculated size + * + * equivalent c code: + * + * size_t size = sizeof(struct_t); + * size_t dyn_member_offset = offsetof(struct_t, length_field); + * size_t dyn_member_size = sizeof(((struct_t*)0)->length_field); + * if (data->bytes >= (dyn_member_offset + dyn_member_size)) + * size += cd_bin_ntohd(&data->data[dyn_member_offset], dyn_member_size); + * return size; + */ +#define CD_DYN_MEMBER_SIZEOF(data, struct_t, length_field) \ + sizeof(struct_t) + ( \ + (data->bytes >= (offsetof(struct_t, length_field) + sizeof(((struct_t*)0)->length_field))) ? \ + (cd_bin_ntohd(&data->data[offsetof(struct_t, length_field)], sizeof(((struct_t*)0)->length_field))) :\ + 0); size_t cd_cmd_response_sizeof(cd_s_bin* data) { - (void) data; // unused variable TODO: implement this - return 0; + return CD_DYN_MEMBER_SIZEOF(data, cd_s_cmd_response, response_size); } #ifdef __cplusplus -- cgit v1.2.3