Andrey/templates
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: add unified SET protocol v2

Browse Source
This commit is contained in:
Andrey
2026-08-24 19:19:32 +03:00
parent 033c7ab9e8
commit 085eb3c8bd
15 changed files with 1953 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

185
c/set-protocol/src/set_can.c Normal file
View File

@@ -0,0 +1,185 @@
#include "set_can.h"
#include <string.h>
uint32_t setp_can_id_pack(const setp_can_id_t *value)
{
if (value == NULL) {
return 0U;
}
return ((uint32_t)SETP_CAN_ID_PREFIX << 24)
| ((uint32_t)value->destination << 16)
| ((uint32_t)value->source << 8)
| ((uint32_t)(value->priority & 1U) << 7)
| (uint32_t)(value->channel & 0x7FU);
}
bool setp_can_id_unpack(uint32_t raw, setp_can_id_t *out)
{
if ((out == NULL) || ((raw & ~SETP_CAN_ID_MASK) != 0U)
|| ((raw & SETP_CAN_ID_PREFIX_MASK)
!= ((uint32_t)SETP_CAN_ID_PREFIX << 24))) {
return false;
}
out->destination = (uint8_t)(raw >> 16);
out->source = (uint8_t)(raw >> 8);
out->priority = (uint8_t)((raw >> 7) & 1U);
out->channel = (uint8_t)(raw & 0x7FU);
return true;
}
bool setp_can_segment(const uint8_t *packet, uint16_t length, uint32_t can_id,
setp_can_send_fn send, void *user)
{
setp_can_frame_t frame;
uint16_t offset;
uint8_t sequence = 1U;
if ((packet == NULL) || (send == NULL)
|| (length < SETP_HEADER_SIZE + SETP_CRC_SIZE)
|| (length > SETP_FRAME_MAX)
|| ((can_id & ~SETP_CAN_ID_MASK) != 0U)) {
return false;
}
(void)memset(&frame, 0, sizeof(frame));
frame.id = can_id;
frame.length = 8U;
frame.data[0] = SETP_CAN_PCI_FIRST;
setp_put_u16(&frame.data[1], length);
(void)memcpy(&frame.data[3], packet, SETP_CAN_FIRST_DATA);
if (!send(&frame, user)) {
return false;
}
offset = SETP_CAN_FIRST_DATA;
while (offset < length) {
uint16_t remaining = (uint16_t)(length - offset);
uint8_t chunk = remaining > SETP_CAN_CONSECUTIVE_DATA
? SETP_CAN_CONSECUTIVE_DATA : (uint8_t)remaining;
(void)memset(frame.data, 0, sizeof(frame.data));
frame.data[0] = (uint8_t)(SETP_CAN_PCI_CONSECUTIVE
| (sequence & SETP_CAN_PCI_VALUE_MASK));
(void)memcpy(&frame.data[1], &packet[offset], chunk);
frame.length = (uint8_t)(1U + chunk);
if (!send(&frame, user)) {
return false;
}
offset = (uint16_t)(offset + chunk);
sequence = (uint8_t)((sequence + 1U) & SETP_CAN_PCI_VALUE_MASK);
}
return true;
}
void setp_can_rx_init(setp_can_rx_t *state)
{
if (state != NULL) {
(void)memset(state, 0, sizeof(*state));
}
}
static bool setp_can_expired(const setp_can_rx_t *state, uint32_t now_ms)
{
return state->active && ((int32_t)(now_ms - state->deadline_ms) >= 0);
}
setp_can_rx_result_t setp_can_rx_feed(setp_can_rx_t *state,
const setp_can_frame_t *frame,
uint32_t now_ms, setp_can_packet_t *out)
{
uint8_t pci;
if ((state == NULL) || (frame == NULL) || (out == NULL)
|| (frame->length == 0U) || (frame->length > 8U)) {
return SETP_CAN_RX_FORMAT_ERROR;
}
if (setp_can_expired(state, now_ms)) {
state->active = 0U;
return SETP_CAN_RX_TIMEOUT;
}
pci = (uint8_t)(frame->data[0] & SETP_CAN_PCI_TYPE_MASK);
if (pci == SETP_CAN_PCI_FIRST) {
uint16_t total;
if (frame->length != 8U) {
return SETP_CAN_RX_FORMAT_ERROR;
}
total = setp_get_u16(&frame->data[1]);
if ((total < SETP_HEADER_SIZE + SETP_CRC_SIZE) || (total > SETP_FRAME_MAX)) {
return SETP_CAN_RX_FORMAT_ERROR;
}
(void)memcpy(state->buffer, &frame->data[3], SETP_CAN_FIRST_DATA);
state->expected_length = total;
state->received_length = SETP_CAN_FIRST_DATA;
state->can_id = frame->id;
state->deadline_ms = now_ms + SETP_CAN_REASSEMBLY_TIMEOUT_MS;
state->next_sequence = 1U;
state->active = 1U;
return SETP_CAN_RX_NONE;
}
if (pci == SETP_CAN_PCI_CONSECUTIVE) {
uint8_t sequence = (uint8_t)(frame->data[0] & SETP_CAN_PCI_VALUE_MASK);
uint16_t remaining;
uint8_t chunk;
if (!state->active || (frame->id != state->can_id)
|| (sequence != state->next_sequence) || (frame->length < 2U)) {
state->active = 0U;
return SETP_CAN_RX_SEQUENCE_ERROR;
}
remaining = (uint16_t)(state->expected_length - state->received_length);
chunk = (uint8_t)(frame->length - 1U);
if (chunk > remaining) {
state->active = 0U;
return SETP_CAN_RX_FORMAT_ERROR;
}
(void)memcpy(&state->buffer[state->received_length], &frame->data[1], chunk);
state->received_length = (uint16_t)(state->received_length + chunk);
state->next_sequence = (uint8_t)((state->next_sequence + 1U)
& SETP_CAN_PCI_VALUE_MASK);
state->deadline_ms = now_ms + SETP_CAN_REASSEMBLY_TIMEOUT_MS;
if (state->received_length == state->expected_length) {
setp_can_id_t id;
uint16_t source;
uint16_t destination;
uint8_t priority;
if (!setp_can_id_unpack(state->can_id, &id)
|| (state->expected_length < SETP_HEADER_SIZE + SETP_CRC_SIZE)
|| (state->buffer[0] != SETP_SOF0)
|| (state->buffer[1] != SETP_SOF1)) {
state->active = 0U;
return SETP_CAN_RX_FORMAT_ERROR;
}
source = setp_get_u16(&state->buffer[6]);
destination = setp_get_u16(&state->buffer[8]);
priority = (state->buffer[3] & SETP_FLAG_PRIORITY) != 0U ? 1U : 0U;
if ((source != id.source) || (destination != id.destination)
|| (priority != id.priority)) {
state->active = 0U;
return SETP_CAN_RX_FORMAT_ERROR;
}
out->data = state->buffer;
out->length = state->expected_length;
out->can_id = state->can_id;
state->active = 0U;
return SETP_CAN_RX_COMPLETE;
}
return SETP_CAN_RX_NONE;
}
if (pci == SETP_CAN_PCI_FLOW_CONTROL) {
return SETP_CAN_RX_NONE;
}
return SETP_CAN_RX_FORMAT_ERROR;
}
bool setp_can_flow_control(setp_can_frame_t *out, uint32_t can_id,
uint8_t status, uint8_t block_size, uint8_t st_min_ms)
{
if ((out == NULL) || (status > SETP_CAN_FC_OVERFLOW)
|| (st_min_ms > 0x7FU) || ((can_id & ~SETP_CAN_ID_MASK) != 0U)) {
return false;
}
(void)memset(out, 0, sizeof(*out));
out->id = can_id;
out->length = 3U;
out->data[0] = (uint8_t)(SETP_CAN_PCI_FLOW_CONTROL | status);
out->data[1] = block_size;
out->data[2] = st_min_ms;
return true;
}

167
c/set-protocol/src/set_firmware.c Normal file
View File

@@ -0,0 +1,167 @@
#include "set_firmware.h"
#include <string.h>
size_t setp_fw_begin_encode(const setp_fw_begin_t *value, uint8_t *out, size_t capacity)
{
size_t total;
if ((value == NULL) || (out == NULL)
|| (value->block_size == 0U)
|| (value->block_size > SETP_MAX_PAYLOAD - SETP_FW_DATA_HEADER_SIZE)
|| ((value->signature_length != 0U) && (value->signature == NULL))) {
return 0U;
}
total = (size_t)SETP_FW_BEGIN_FIXED_SIZE + value->signature_length;
if ((total > SETP_MAX_PAYLOAD) || (capacity < total)
|| (((value->flags & SETP_FW_FLAG_SIGNED) != 0U)
&& (value->signature_length == 0U))) {
return 0U;
}
setp_put_u32(&out[0], value->image_size);
setp_put_u32(&out[4], value->image_crc32);
setp_put_u32(&out[8], value->image_version);
setp_put_u32(&out[12], value->base_address);
out[16] = value->slot;
out[17] = value->flags;
setp_put_u16(&out[18], value->block_size);
(void)memcpy(&out[20], value->sha256, SETP_SHA256_SIZE);
setp_put_u32(&out[52], value->signing_key_id);
setp_put_u16(&out[56], value->signature_length);
if (value->signature_length != 0U) {
(void)memcpy(&out[SETP_FW_BEGIN_FIXED_SIZE], value->signature,
value->signature_length);
}
return total;
}
bool setp_fw_begin_decode(const uint8_t *payload, uint16_t length, setp_fw_begin_t *out)
{
uint16_t signature_length;
if ((payload == NULL) || (out == NULL) || (length < SETP_FW_BEGIN_FIXED_SIZE)) {
return false;
}
out->image_size = setp_get_u32(&payload[0]);
out->image_crc32 = setp_get_u32(&payload[4]);
out->image_version = setp_get_u32(&payload[8]);
out->base_address = setp_get_u32(&payload[12]);
out->slot = payload[16];
out->flags = payload[17];
out->block_size = setp_get_u16(&payload[18]);
(void)memcpy(out->sha256, &payload[20], SETP_SHA256_SIZE);
out->signing_key_id = setp_get_u32(&payload[52]);
signature_length = setp_get_u16(&payload[56]);
if (length != SETP_FW_BEGIN_FIXED_SIZE + signature_length) {
return false;
}
out->signature_length = signature_length;
out->signature = signature_length != 0U ? &payload[SETP_FW_BEGIN_FIXED_SIZE] : NULL;
return (out->block_size != 0U)
&& (out->block_size <= SETP_MAX_PAYLOAD - SETP_FW_DATA_HEADER_SIZE)
&& (((out->flags & SETP_FW_FLAG_SIGNED) == 0U) || (signature_length != 0U));
}
size_t setp_fw_data_encode(uint32_t offset, uint16_t flags,
const uint8_t *data, uint16_t data_length,
uint8_t *out, size_t capacity)
{
size_t total = (size_t)SETP_FW_DATA_HEADER_SIZE + data_length;
if ((data == NULL) || (out == NULL) || (data_length == 0U)
|| (total > SETP_MAX_PAYLOAD) || (capacity < total)) {
return 0U;
}
setp_put_u32(&out[0], offset);
setp_put_u16(&out[4], data_length);
setp_put_u16(&out[6], flags);
setp_put_u32(&out[8], setp_crc32(data, data_length));
(void)memcpy(&out[SETP_FW_DATA_HEADER_SIZE], data, data_length);
return total;
}
bool setp_fw_data_decode(const uint8_t *payload, uint16_t length, setp_fw_data_t *out)
{
uint16_t data_length;
if ((payload == NULL) || (out == NULL) || (length < SETP_FW_DATA_HEADER_SIZE)) {
return false;
}
data_length = setp_get_u16(&payload[4]);
if ((data_length == 0U) || (length != SETP_FW_DATA_HEADER_SIZE + data_length)) {
return false;
}
out->offset = setp_get_u32(&payload[0]);
out->data_length = data_length;
out->flags = setp_get_u16(&payload[6]);
out->data_crc32 = setp_get_u32(&payload[8]);
out->data = &payload[SETP_FW_DATA_HEADER_SIZE];
return setp_crc32(out->data, out->data_length) == out->data_crc32;
}
size_t setp_fw_end_encode(const setp_fw_end_t *value, uint8_t *out, size_t capacity)
{
if ((value == NULL) || (out == NULL) || (capacity < SETP_FW_END_SIZE)) {
return 0U;
}
setp_put_u32(&out[0], value->image_size);
setp_put_u32(&out[4], value->image_crc32);
(void)memcpy(&out[8], value->sha256, SETP_SHA256_SIZE);
return SETP_FW_END_SIZE;
}
bool setp_fw_end_decode(const uint8_t *payload, uint16_t length, setp_fw_end_t *out)
{
if ((payload == NULL) || (out == NULL) || (length != SETP_FW_END_SIZE)) {
return false;
}
out->image_size = setp_get_u32(&payload[0]);
out->image_crc32 = setp_get_u32(&payload[4]);
(void)memcpy(out->sha256, &payload[8], SETP_SHA256_SIZE);
return true;
}
size_t setp_fw_status_encode(const setp_fw_status_t *value, uint8_t *out, size_t capacity)
{
if ((value == NULL) || (out == NULL) || (capacity < SETP_FW_STATUS_SIZE)) {
return 0U;
}
out[0] = value->state;
out[1] = value->active_slot;
setp_put_u16(&out[2], value->max_block_size);
setp_put_u32(&out[4], value->next_offset);
setp_put_u32(&out[8], value->image_size);
setp_put_u16(&out[12], value->last_error);
setp_put_u16(&out[14], value->flags);
return SETP_FW_STATUS_SIZE;
}
bool setp_fw_status_decode(const uint8_t *payload, uint16_t length, setp_fw_status_t *out)
{
if ((payload == NULL) || (out == NULL) || (length != SETP_FW_STATUS_SIZE)) {
return false;
}
out->state = payload[0];
out->active_slot = payload[1];
out->max_block_size = setp_get_u16(&payload[2]);
out->next_offset = setp_get_u32(&payload[4]);
out->image_size = setp_get_u32(&payload[8]);
out->last_error = setp_get_u16(&payload[12]);
out->flags = setp_get_u16(&payload[14]);
return true;
}
size_t setp_fw_manifest_encode(const setp_fw_begin_t *value,
uint8_t *out, size_t capacity)
{
static const uint8_t prefix[8] = {
'S', 'E', 'T', 'P', 'F', 'W', '2', 0U
};
if ((value == NULL) || (out == NULL) || (capacity < SETP_FW_MANIFEST_SIZE)) {
return 0U;
}
(void)memcpy(out, prefix, sizeof(prefix));
setp_put_u32(&out[8], value->image_size);
setp_put_u32(&out[12], value->image_crc32);
setp_put_u32(&out[16], value->image_version);
setp_put_u32(&out[20], value->base_address);
out[24] = value->slot;
(void)memcpy(&out[25], value->sha256, SETP_SHA256_SIZE);
return SETP_FW_MANIFEST_SIZE;
}

280
c/set-protocol/src/set_protocol.c Normal file
View 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;
}

155
c/set-protocol/src/set_telemetry.c Normal file
View File

@@ -0,0 +1,155 @@
#include "set_telemetry.h"
#include <string.h>
size_t setp_subscribe_encode(uint16_t subscription_id, uint32_t period_ms,
const uint32_t *addresses, uint16_t address_count,
uint8_t *out, size_t capacity)
{
size_t total = (size_t)SETP_SUBSCRIBE_HEADER_SIZE + ((size_t)address_count * 4U);
uint16_t index;
if ((out == NULL) || ((address_count != 0U) && (addresses == NULL))
|| (total > SETP_MAX_PAYLOAD) || (capacity < total)) {
return 0U;
}
setp_put_u16(&out[0], subscription_id);
setp_put_u32(&out[2], period_ms);
setp_put_u16(&out[6], address_count);
for (index = 0U; index < address_count; ++index) {
setp_put_u32(&out[SETP_SUBSCRIBE_HEADER_SIZE + (index * 4U)], addresses[index]);
}
return total;
}
bool setp_subscribe_decode_header(const uint8_t *payload, uint16_t length,
uint16_t *subscription_id, uint32_t *period_ms,
uint16_t *address_count)
{
uint16_t count;
if ((payload == NULL) || (subscription_id == NULL)
|| (period_ms == NULL) || (address_count == NULL)
|| (length < SETP_SUBSCRIBE_HEADER_SIZE)) {
return false;
}
count = setp_get_u16(&payload[6]);
if (length != SETP_SUBSCRIBE_HEADER_SIZE + (count * 4U)) {
return false;
}
*subscription_id = setp_get_u16(&payload[0]);
*period_ms = setp_get_u32(&payload[2]);
*address_count = count;
return true;
}
bool setp_subscribe_address(const uint8_t *payload, uint16_t length,
uint16_t index, uint32_t *address)
{
uint16_t subscription_id;
uint32_t period_ms;
uint16_t count;
if ((address == NULL)
|| !setp_subscribe_decode_header(payload, length, &subscription_id,
&period_ms, &count)
|| (index >= count)) {
return false;
}
(void)subscription_id;
(void)period_ms;
*address = setp_get_u32(&payload[SETP_SUBSCRIBE_HEADER_SIZE + (index * 4U)]);
return true;
}
bool setp_publish_begin(setp_publish_builder_t *builder,
uint16_t subscription_id, uint16_t sample_sequence,
uint32_t timestamp_ms, uint8_t *out, uint16_t capacity)
{
if ((builder == NULL) || (out == NULL) || (capacity < SETP_PUBLISH_HEADER_SIZE)) {
return false;
}
builder->output = out;
builder->capacity = capacity;
builder->length = SETP_PUBLISH_HEADER_SIZE;
builder->item_count = 0U;
setp_put_u16(&out[0], subscription_id);
setp_put_u16(&out[2], sample_sequence);
setp_put_u32(&out[4], timestamp_ms);
setp_put_u16(&out[8], 0U);
return true;
}
bool setp_publish_add(setp_publish_builder_t *builder, uint32_t address,
uint8_t encoding, uint8_t element_count,
const uint8_t *data, uint16_t data_length)
{
uint16_t offset;
size_t next;
if ((builder == NULL) || (builder->output == NULL)
|| (element_count == 0U) || (data_length == 0U) || (data == NULL)
|| (encoding < SETP_VALUE_U16) || (encoding > SETP_VALUE_BYTES)) {
return false;
}
next = (size_t)builder->length + SETP_PUBLISH_ITEM_HEADER_SIZE + data_length;
if ((next > builder->capacity) || (next > SETP_MAX_PAYLOAD)) {
return false;
}
offset = builder->length;
setp_put_u32(&builder->output[offset], address);
builder->output[offset + 4U] = encoding;
builder->output[offset + 5U] = element_count;
setp_put_u16(&builder->output[offset + 6U], data_length);
(void)memcpy(&builder->output[offset + SETP_PUBLISH_ITEM_HEADER_SIZE], data, data_length);
builder->length = (uint16_t)next;
++builder->item_count;
return true;
}
size_t setp_publish_finish(setp_publish_builder_t *builder)
{
if ((builder == NULL) || (builder->output == NULL)) {
return 0U;
}
setp_put_u16(&builder->output[8], builder->item_count);
return builder->length;
}
bool setp_publish_view_init(setp_publish_view_t *view,
const uint8_t *payload, uint16_t length)
{
if ((view == NULL) || (payload == NULL) || (length < SETP_PUBLISH_HEADER_SIZE)) {
return false;
}
view->payload = payload;
view->length = length;
view->offset = SETP_PUBLISH_HEADER_SIZE;
view->subscription_id = setp_get_u16(&payload[0]);
view->sample_sequence = setp_get_u16(&payload[2]);
view->timestamp_ms = setp_get_u32(&payload[4]);
view->remaining = setp_get_u16(&payload[8]);
return true;
}
bool setp_publish_next(setp_publish_view_t *view, setp_publish_item_t *item)
{
uint16_t data_length;
size_t end;
if ((view == NULL) || (item == NULL) || (view->remaining == 0U)
|| (view->offset + SETP_PUBLISH_ITEM_HEADER_SIZE > view->length)) {
return false;
}
data_length = setp_get_u16(&view->payload[view->offset + 6U]);
end = (size_t)view->offset + SETP_PUBLISH_ITEM_HEADER_SIZE + data_length;
if ((data_length == 0U) || (end > view->length)) {
return false;
}
item->address = setp_get_u32(&view->payload[view->offset]);
item->encoding = view->payload[view->offset + 4U];
item->element_count = view->payload[view->offset + 5U];
item->data_length = data_length;
item->data = &view->payload[view->offset + SETP_PUBLISH_ITEM_HEADER_SIZE];
view->offset = (uint16_t)end;
--view->remaining;
if ((view->remaining == 0U) && (view->offset != view->length)) {
return false;
}
return true;
}