feat: add unified SET protocol v2
This commit is contained in:
280
c/set-protocol/src/set_protocol.c
Normal file
280
c/set-protocol/src/set_protocol.c
Normal file
@@ -0,0 +1,280 @@
|
||||
#include "set_protocol.h"
|
||||
|
||||
#include <string.h>
|
||||
|
||||
uint16_t setp_get_u16(const uint8_t *data)
|
||||
{
|
||||
return (uint16_t)((uint16_t)data[0] | ((uint16_t)data[1] << 8));
|
||||
}
|
||||
|
||||
uint32_t setp_get_u32(const uint8_t *data)
|
||||
{
|
||||
return (uint32_t)data[0]
|
||||
| ((uint32_t)data[1] << 8)
|
||||
| ((uint32_t)data[2] << 16)
|
||||
| ((uint32_t)data[3] << 24);
|
||||
}
|
||||
|
||||
void setp_put_u16(uint8_t *data, uint16_t value)
|
||||
{
|
||||
data[0] = (uint8_t)(value & 0xFFU);
|
||||
data[1] = (uint8_t)(value >> 8);
|
||||
}
|
||||
|
||||
void setp_put_u32(uint8_t *data, uint32_t value)
|
||||
{
|
||||
data[0] = (uint8_t)(value & 0xFFU);
|
||||
data[1] = (uint8_t)((value >> 8) & 0xFFU);
|
||||
data[2] = (uint8_t)((value >> 16) & 0xFFU);
|
||||
data[3] = (uint8_t)(value >> 24);
|
||||
}
|
||||
|
||||
uint32_t setp_crc32(const uint8_t *data, size_t length)
|
||||
{
|
||||
uint32_t crc = 0xFFFFFFFFUL;
|
||||
size_t index;
|
||||
uint8_t bit;
|
||||
|
||||
if ((data == NULL) && (length != 0U)) {
|
||||
return 0U;
|
||||
}
|
||||
for (index = 0U; index < length; ++index) {
|
||||
crc ^= data[index];
|
||||
for (bit = 0U; bit < 8U; ++bit) {
|
||||
crc = (crc >> 1) ^ ((crc & 1U) ? 0xEDB88320UL : 0U);
|
||||
}
|
||||
}
|
||||
return crc ^ 0xFFFFFFFFUL;
|
||||
}
|
||||
|
||||
size_t setp_frame_encode(const setp_frame_t *frame, uint8_t *out, size_t capacity)
|
||||
{
|
||||
size_t total;
|
||||
uint32_t crc;
|
||||
|
||||
if ((frame == NULL) || (out == NULL)) {
|
||||
return 0U;
|
||||
}
|
||||
if ((frame->flags & SETP_FLAG_RESERVED_MASK) != 0U) {
|
||||
return 0U;
|
||||
}
|
||||
if ((frame->payload_length > SETP_MAX_PAYLOAD)
|
||||
|| ((frame->payload_length != 0U) && (frame->payload == NULL))) {
|
||||
return 0U;
|
||||
}
|
||||
total = (size_t)SETP_HEADER_SIZE + frame->payload_length + SETP_CRC_SIZE;
|
||||
if (capacity < total) {
|
||||
return 0U;
|
||||
}
|
||||
|
||||
out[0] = SETP_SOF0;
|
||||
out[1] = SETP_SOF1;
|
||||
out[2] = SETP_VERSION;
|
||||
out[3] = frame->flags;
|
||||
setp_put_u16(&out[4], frame->message_type);
|
||||
setp_put_u16(&out[6], frame->source);
|
||||
setp_put_u16(&out[8], frame->destination);
|
||||
setp_put_u16(&out[10], frame->sequence);
|
||||
setp_put_u16(&out[12], frame->payload_length);
|
||||
if (frame->payload_length != 0U) {
|
||||
(void)memcpy(&out[SETP_HEADER_SIZE], frame->payload, frame->payload_length);
|
||||
}
|
||||
crc = setp_crc32(&out[2], (size_t)(SETP_HEADER_SIZE - 2U) + frame->payload_length);
|
||||
setp_put_u32(&out[SETP_HEADER_SIZE + frame->payload_length], crc);
|
||||
return total;
|
||||
}
|
||||
|
||||
bool setp_frame_decode_datagram(const uint8_t *data, size_t length, setp_frame_t *out)
|
||||
{
|
||||
uint16_t payload_length;
|
||||
size_t expected_length;
|
||||
uint32_t expected_crc;
|
||||
if ((data == NULL) || (out == NULL) || (length < SETP_HEADER_SIZE + SETP_CRC_SIZE)
|
||||
|| (data[0] != SETP_SOF0) || (data[1] != SETP_SOF1)
|
||||
|| (data[2] != SETP_VERSION)
|
||||
|| ((data[3] & SETP_FLAG_RESERVED_MASK) != 0U)) {
|
||||
return false;
|
||||
}
|
||||
payload_length = setp_get_u16(&data[12]);
|
||||
expected_length = (size_t)SETP_HEADER_SIZE + payload_length + SETP_CRC_SIZE;
|
||||
if ((payload_length > SETP_MAX_PAYLOAD) || (length != expected_length)) {
|
||||
return false;
|
||||
}
|
||||
expected_crc = setp_get_u32(&data[SETP_HEADER_SIZE + payload_length]);
|
||||
if (setp_crc32(&data[2], (size_t)(SETP_HEADER_SIZE - 2U) + payload_length)
|
||||
!= expected_crc) {
|
||||
return false;
|
||||
}
|
||||
out->flags = data[3];
|
||||
out->message_type = setp_get_u16(&data[4]);
|
||||
out->source = setp_get_u16(&data[6]);
|
||||
out->destination = setp_get_u16(&data[8]);
|
||||
out->sequence = setp_get_u16(&data[10]);
|
||||
out->payload_length = payload_length;
|
||||
out->payload = &data[SETP_HEADER_SIZE];
|
||||
return true;
|
||||
}
|
||||
|
||||
void setp_parser_init(setp_parser_t *parser)
|
||||
{
|
||||
if (parser != NULL) {
|
||||
(void)memset(parser, 0, sizeof(*parser));
|
||||
}
|
||||
}
|
||||
|
||||
void setp_parser_reset(setp_parser_t *parser)
|
||||
{
|
||||
if (parser != NULL) {
|
||||
parser->length = 0U;
|
||||
}
|
||||
}
|
||||
|
||||
static void setp_drop(setp_parser_t *parser, uint16_t count)
|
||||
{
|
||||
if (count >= parser->length) {
|
||||
parser->length = 0U;
|
||||
return;
|
||||
}
|
||||
(void)memmove(parser->buffer, &parser->buffer[count], parser->length - count);
|
||||
parser->length = (uint16_t)(parser->length - count);
|
||||
}
|
||||
|
||||
static uint16_t setp_find_sof(const setp_parser_t *parser)
|
||||
{
|
||||
uint16_t index;
|
||||
for (index = 0U; index + 1U < parser->length; ++index) {
|
||||
if ((parser->buffer[index] == SETP_SOF0)
|
||||
&& (parser->buffer[index + 1U] == SETP_SOF1)) {
|
||||
return index;
|
||||
}
|
||||
}
|
||||
return UINT16_MAX;
|
||||
}
|
||||
|
||||
static size_t setp_parser_process(setp_parser_t *parser,
|
||||
setp_frame_fn callback, void *user)
|
||||
{
|
||||
size_t delivered = 0U;
|
||||
|
||||
for (;;) {
|
||||
uint16_t sof;
|
||||
uint16_t payload_length;
|
||||
uint16_t total;
|
||||
uint32_t expected_crc;
|
||||
uint32_t actual_crc;
|
||||
setp_frame_t frame;
|
||||
|
||||
if (parser->length < 2U) {
|
||||
break;
|
||||
}
|
||||
sof = setp_find_sof(parser);
|
||||
if (sof == UINT16_MAX) {
|
||||
if (parser->buffer[parser->length - 1U] == SETP_SOF0) {
|
||||
parser->stats.stray_bytes += parser->length - 1U;
|
||||
parser->buffer[0] = SETP_SOF0;
|
||||
parser->length = 1U;
|
||||
} else {
|
||||
parser->stats.stray_bytes += parser->length;
|
||||
parser->length = 0U;
|
||||
}
|
||||
break;
|
||||
}
|
||||
if (sof != 0U) {
|
||||
parser->stats.stray_bytes += sof;
|
||||
setp_drop(parser, sof);
|
||||
}
|
||||
if (parser->length < SETP_HEADER_SIZE) {
|
||||
break;
|
||||
}
|
||||
if (parser->buffer[2] != SETP_VERSION) {
|
||||
++parser->stats.version_errors;
|
||||
setp_drop(parser, 1U);
|
||||
continue;
|
||||
}
|
||||
if ((parser->buffer[3] & SETP_FLAG_RESERVED_MASK) != 0U) {
|
||||
++parser->stats.flag_errors;
|
||||
setp_drop(parser, 1U);
|
||||
continue;
|
||||
}
|
||||
payload_length = setp_get_u16(&parser->buffer[12]);
|
||||
if (payload_length > SETP_MAX_PAYLOAD) {
|
||||
++parser->stats.length_errors;
|
||||
setp_drop(parser, 1U);
|
||||
continue;
|
||||
}
|
||||
total = (uint16_t)(SETP_HEADER_SIZE + payload_length + SETP_CRC_SIZE);
|
||||
if (parser->length < total) {
|
||||
break;
|
||||
}
|
||||
expected_crc = setp_get_u32(&parser->buffer[SETP_HEADER_SIZE + payload_length]);
|
||||
actual_crc = setp_crc32(&parser->buffer[2],
|
||||
(size_t)(SETP_HEADER_SIZE - 2U) + payload_length);
|
||||
if (expected_crc != actual_crc) {
|
||||
++parser->stats.crc_errors;
|
||||
setp_drop(parser, 1U);
|
||||
continue;
|
||||
}
|
||||
|
||||
frame.flags = parser->buffer[3];
|
||||
frame.message_type = setp_get_u16(&parser->buffer[4]);
|
||||
frame.source = setp_get_u16(&parser->buffer[6]);
|
||||
frame.destination = setp_get_u16(&parser->buffer[8]);
|
||||
frame.sequence = setp_get_u16(&parser->buffer[10]);
|
||||
frame.payload_length = payload_length;
|
||||
frame.payload = &parser->buffer[SETP_HEADER_SIZE];
|
||||
if (callback != NULL) {
|
||||
callback(&frame, user);
|
||||
}
|
||||
++parser->stats.frames;
|
||||
++delivered;
|
||||
setp_drop(parser, total);
|
||||
}
|
||||
return delivered;
|
||||
}
|
||||
|
||||
size_t setp_parser_feed(setp_parser_t *parser, const uint8_t *data, size_t length,
|
||||
setp_frame_fn callback, void *user)
|
||||
{
|
||||
size_t index;
|
||||
size_t delivered = 0U;
|
||||
|
||||
if ((parser == NULL) || ((data == NULL) && (length != 0U))) {
|
||||
return 0U;
|
||||
}
|
||||
for (index = 0U; index < length; ++index) {
|
||||
if (parser->length >= SETP_FRAME_MAX) {
|
||||
++parser->stats.overflows;
|
||||
parser->length = 0U;
|
||||
}
|
||||
parser->buffer[parser->length++] = data[index];
|
||||
delivered += setp_parser_process(parser, callback, user);
|
||||
}
|
||||
return delivered;
|
||||
}
|
||||
|
||||
size_t setp_status_encode(uint16_t status, uint8_t *out, size_t capacity)
|
||||
{
|
||||
if ((out == NULL) || (capacity < 2U)) {
|
||||
return 0U;
|
||||
}
|
||||
setp_put_u16(out, status);
|
||||
return 2U;
|
||||
}
|
||||
|
||||
bool setp_status_decode(const setp_frame_t *frame, uint16_t *status,
|
||||
const uint8_t **body, uint16_t *body_length)
|
||||
{
|
||||
if ((frame == NULL) || (status == NULL)
|
||||
|| ((frame->flags & SETP_FLAG_RESPONSE) == 0U)
|
||||
|| (frame->payload_length < 2U) || (frame->payload == NULL)) {
|
||||
return false;
|
||||
}
|
||||
*status = setp_get_u16(frame->payload);
|
||||
if (body != NULL) {
|
||||
*body = &frame->payload[2];
|
||||
}
|
||||
if (body_length != NULL) {
|
||||
*body_length = (uint16_t)(frame->payload_length - 2U);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
Reference in New Issue
Block a user