281 lines
8.3 KiB
C
281 lines
8.3 KiB
C
#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;
|
|
}
|