303 lines
10 KiB
C
303 lines
10 KiB
C
#include "set_firmware.h"
|
|
#include "set_can.h"
|
|
#include "set_protocol.h"
|
|
#include "set_telemetry.h"
|
|
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
|
|
static int failures;
|
|
static int checks;
|
|
|
|
static void check_impl(int condition, const char *text, int line)
|
|
{
|
|
++checks;
|
|
if (!condition) {
|
|
++failures;
|
|
(void)printf("FAIL line %d: %s\n", line, text);
|
|
}
|
|
}
|
|
|
|
#define CHECK(condition) check_impl((condition) ? 1 : 0, #condition, __LINE__)
|
|
|
|
typedef struct {
|
|
setp_frame_t frame;
|
|
uint8_t payload[SETP_MAX_PAYLOAD];
|
|
int count;
|
|
} capture_t;
|
|
|
|
static void capture_frame(const setp_frame_t *frame, void *user)
|
|
{
|
|
capture_t *capture = (capture_t *)user;
|
|
capture->frame = *frame;
|
|
if (frame->payload_length != 0U) {
|
|
(void)memcpy(capture->payload, frame->payload, frame->payload_length);
|
|
}
|
|
capture->frame.payload = capture->payload;
|
|
++capture->count;
|
|
}
|
|
|
|
static void test_crc_and_reference_frame(void)
|
|
{
|
|
static const uint8_t expected[] = {
|
|
0xA5U, 0x5AU, 0x02U, 0x08U, 0x01U, 0x00U, 0x00U, 0x00U,
|
|
0x2AU, 0x00U, 0x34U, 0x12U, 0x00U, 0x00U, 0x33U, 0xECU,
|
|
0x33U, 0x04U
|
|
};
|
|
setp_frame_t frame;
|
|
uint8_t output[SETP_FRAME_MAX];
|
|
size_t length;
|
|
setp_frame_t decoded;
|
|
|
|
CHECK(setp_crc32((const uint8_t *)"123456789", 9U) == 0xCBF43926UL);
|
|
(void)memset(&frame, 0, sizeof(frame));
|
|
frame.flags = SETP_FLAG_ACK_REQUIRED;
|
|
frame.message_type = SETP_MSG_PING;
|
|
frame.destination = 0x002AU;
|
|
frame.sequence = 0x1234U;
|
|
length = setp_frame_encode(&frame, output, sizeof(output));
|
|
CHECK(length == sizeof(expected));
|
|
CHECK(memcmp(output, expected, sizeof(expected)) == 0);
|
|
CHECK(setp_frame_decode_datagram(output, length, &decoded));
|
|
CHECK(decoded.message_type == SETP_MSG_PING);
|
|
CHECK(decoded.destination == 0x002AU);
|
|
CHECK(!setp_frame_decode_datagram(output, length - 1U, &decoded));
|
|
CHECK(setp_frame_encode(&frame, output, 4U) == 0U);
|
|
}
|
|
|
|
static void test_parser(void)
|
|
{
|
|
setp_frame_t frame;
|
|
uint8_t raw[SETP_FRAME_MAX];
|
|
uint8_t payload[] = { 0x11U, 0x22U, 0x33U };
|
|
size_t length;
|
|
setp_parser_t parser;
|
|
capture_t capture;
|
|
|
|
(void)memset(&frame, 0, sizeof(frame));
|
|
frame.message_type = 0x3456U; /* Unknown extensions are valid frames. */
|
|
frame.source = 7U;
|
|
frame.destination = 9U;
|
|
frame.sequence = 11U;
|
|
frame.payload_length = sizeof(payload);
|
|
frame.payload = payload;
|
|
length = setp_frame_encode(&frame, raw, sizeof(raw));
|
|
|
|
(void)memset(&capture, 0, sizeof(capture));
|
|
setp_parser_init(&parser);
|
|
CHECK(setp_parser_feed(&parser, (const uint8_t *)"noise", 5U,
|
|
capture_frame, &capture) == 0U);
|
|
CHECK(setp_parser_feed(&parser, raw, 3U, capture_frame, &capture) == 0U);
|
|
CHECK(setp_parser_feed(&parser, &raw[3], length - 3U,
|
|
capture_frame, &capture) == 1U);
|
|
CHECK(capture.count == 1);
|
|
CHECK(capture.frame.message_type == 0x3456U);
|
|
CHECK(capture.frame.source == 7U);
|
|
CHECK(capture.frame.destination == 9U);
|
|
CHECK(capture.frame.sequence == 11U);
|
|
CHECK(capture.frame.payload_length == sizeof(payload));
|
|
CHECK(memcmp(capture.payload, payload, sizeof(payload)) == 0);
|
|
CHECK(parser.stats.stray_bytes == 5U);
|
|
|
|
raw[length - 1U] ^= 1U;
|
|
CHECK(setp_parser_feed(&parser, raw, length, capture_frame, &capture) == 0U);
|
|
CHECK(parser.stats.crc_errors == 1U);
|
|
}
|
|
|
|
static void test_status(void)
|
|
{
|
|
uint8_t payload[4] = { 0U };
|
|
setp_frame_t frame;
|
|
uint16_t status;
|
|
uint16_t body_length;
|
|
const uint8_t *body;
|
|
|
|
CHECK(setp_status_encode(SETP_STATUS_BUSY, payload, sizeof(payload)) == 2U);
|
|
payload[2] = 0xAAU;
|
|
payload[3] = 0x55U;
|
|
(void)memset(&frame, 0, sizeof(frame));
|
|
frame.flags = SETP_FLAG_RESPONSE | SETP_FLAG_ERROR;
|
|
frame.payload = payload;
|
|
frame.payload_length = sizeof(payload);
|
|
CHECK(setp_status_decode(&frame, &status, &body, &body_length));
|
|
CHECK(status == SETP_STATUS_BUSY);
|
|
CHECK(body_length == 2U);
|
|
CHECK(body[0] == 0xAAU && body[1] == 0x55U);
|
|
}
|
|
|
|
typedef struct {
|
|
setp_can_frame_t frames[96];
|
|
uint16_t count;
|
|
} can_capture_t;
|
|
|
|
static bool capture_can(const setp_can_frame_t *frame, void *user)
|
|
{
|
|
can_capture_t *capture = (can_capture_t *)user;
|
|
if (capture->count >= 96U) {
|
|
return false;
|
|
}
|
|
capture->frames[capture->count++] = *frame;
|
|
return true;
|
|
}
|
|
|
|
static void test_can_binding(void)
|
|
{
|
|
setp_can_id_t id = { 7U, 9U, 1U, 3U };
|
|
setp_can_id_t decoded_id;
|
|
uint32_t raw_id = setp_can_id_pack(&id);
|
|
setp_frame_t frame;
|
|
uint8_t payload[40];
|
|
uint8_t packet[SETP_FRAME_MAX];
|
|
size_t packet_length;
|
|
can_capture_t capture;
|
|
setp_can_rx_t rx;
|
|
setp_can_packet_t complete;
|
|
setp_can_rx_result_t result = SETP_CAN_RX_NONE;
|
|
uint16_t index;
|
|
|
|
CHECK(setp_can_id_unpack(raw_id, &decoded_id));
|
|
CHECK(decoded_id.source == 7U);
|
|
CHECK(decoded_id.destination == 9U);
|
|
CHECK(decoded_id.priority == 1U);
|
|
CHECK(decoded_id.channel == 3U);
|
|
for (index = 0U; index < sizeof(payload); ++index) {
|
|
payload[index] = (uint8_t)index;
|
|
}
|
|
(void)memset(&frame, 0, sizeof(frame));
|
|
frame.flags = SETP_FLAG_PRIORITY;
|
|
frame.message_type = SETP_MSG_PUBLISH;
|
|
frame.source = 7U;
|
|
frame.destination = 9U;
|
|
frame.sequence = 3U;
|
|
frame.payload = payload;
|
|
frame.payload_length = sizeof(payload);
|
|
packet_length = setp_frame_encode(&frame, packet, sizeof(packet));
|
|
(void)memset(&capture, 0, sizeof(capture));
|
|
(void)memset(&complete, 0, sizeof(complete));
|
|
CHECK(setp_can_segment(packet, (uint16_t)packet_length, raw_id,
|
|
capture_can, &capture));
|
|
CHECK(capture.count > 1U);
|
|
CHECK(capture.frames[0].data[0] == SETP_CAN_PCI_FIRST);
|
|
setp_can_rx_init(&rx);
|
|
for (index = 0U; index < capture.count; ++index) {
|
|
result = setp_can_rx_feed(&rx, &capture.frames[index], index, &complete);
|
|
}
|
|
CHECK(result == SETP_CAN_RX_COMPLETE);
|
|
CHECK(complete.length == packet_length);
|
|
CHECK(complete.can_id == raw_id);
|
|
CHECK(memcmp(complete.data, packet, packet_length) == 0);
|
|
}
|
|
|
|
static void test_firmware(void)
|
|
{
|
|
setp_fw_begin_t begin;
|
|
setp_fw_begin_t decoded_begin;
|
|
setp_fw_data_t decoded_data;
|
|
uint8_t signature[64];
|
|
uint8_t image[32];
|
|
uint8_t manifest[SETP_FW_MANIFEST_SIZE];
|
|
uint8_t payload[SETP_MAX_PAYLOAD];
|
|
size_t length;
|
|
uint16_t index;
|
|
|
|
(void)memset(&begin, 0, sizeof(begin));
|
|
(void)memset(signature, 0xA5, sizeof(signature));
|
|
for (index = 0U; index < sizeof(image); ++index) {
|
|
image[index] = (uint8_t)index;
|
|
}
|
|
begin.image_size = 4096U;
|
|
begin.image_crc32 = 0x12345678UL;
|
|
begin.image_version = 0x00010203UL;
|
|
begin.base_address = 0x08010000UL;
|
|
begin.slot = 1U;
|
|
begin.flags = SETP_FW_FLAG_RESUME | SETP_FW_FLAG_SIGNED;
|
|
begin.block_size = 480U;
|
|
(void)memset(begin.sha256, 0x5A, sizeof(begin.sha256));
|
|
begin.signing_key_id = 3U;
|
|
begin.signature_length = sizeof(signature);
|
|
begin.signature = signature;
|
|
length = setp_fw_begin_encode(&begin, payload, sizeof(payload));
|
|
CHECK(length == SETP_FW_BEGIN_FIXED_SIZE + sizeof(signature));
|
|
CHECK(setp_fw_begin_decode(payload, (uint16_t)length, &decoded_begin));
|
|
CHECK(decoded_begin.image_size == begin.image_size);
|
|
CHECK(decoded_begin.block_size == 480U);
|
|
CHECK(decoded_begin.signing_key_id == 3U);
|
|
CHECK(decoded_begin.signature_length == sizeof(signature));
|
|
CHECK(memcmp(decoded_begin.signature, signature, sizeof(signature)) == 0);
|
|
CHECK(setp_fw_manifest_encode(&decoded_begin, manifest, sizeof(manifest))
|
|
== SETP_FW_MANIFEST_SIZE);
|
|
CHECK(memcmp(manifest, "SETPFW2", 7U) == 0);
|
|
CHECK(setp_get_u32(&manifest[8]) == begin.image_size);
|
|
|
|
length = setp_fw_data_encode(1024U, 0U, image, sizeof(image),
|
|
payload, sizeof(payload));
|
|
CHECK(length == SETP_FW_DATA_HEADER_SIZE + sizeof(image));
|
|
CHECK(setp_fw_data_decode(payload, (uint16_t)length, &decoded_data));
|
|
CHECK(decoded_data.offset == 1024U);
|
|
CHECK(decoded_data.data_length == sizeof(image));
|
|
CHECK(memcmp(decoded_data.data, image, sizeof(image)) == 0);
|
|
payload[length - 1U] ^= 1U;
|
|
CHECK(!setp_fw_data_decode(payload, (uint16_t)length, &decoded_data));
|
|
}
|
|
|
|
static void test_telemetry(void)
|
|
{
|
|
uint32_t addresses[] = { 0x10U, 0x12345678UL };
|
|
uint8_t payload[SETP_MAX_PAYLOAD];
|
|
uint16_t subscription_id;
|
|
uint16_t count;
|
|
uint32_t period;
|
|
uint32_t address;
|
|
size_t length;
|
|
setp_publish_builder_t builder;
|
|
setp_publish_view_t view;
|
|
setp_publish_item_t item;
|
|
uint8_t value1[] = { 0x34U, 0x12U };
|
|
uint8_t value2[] = { 1U, 2U, 3U, 4U };
|
|
|
|
length = setp_subscribe_encode(7U, 50U, addresses, 2U, payload, sizeof(payload));
|
|
CHECK(length == 16U);
|
|
CHECK(setp_subscribe_decode_header(payload, (uint16_t)length,
|
|
&subscription_id, &period, &count));
|
|
CHECK(subscription_id == 7U);
|
|
CHECK(period == 50U);
|
|
CHECK(count == 2U);
|
|
CHECK(setp_subscribe_address(payload, (uint16_t)length, 1U, &address));
|
|
CHECK(address == 0x12345678UL);
|
|
|
|
CHECK(setp_publish_begin(&builder, 7U, 9U, 123456U,
|
|
payload, sizeof(payload)));
|
|
CHECK(setp_publish_add(&builder, 0x10U, SETP_VALUE_U16, 1U,
|
|
value1, sizeof(value1)));
|
|
CHECK(setp_publish_add(&builder, 0x20U, SETP_VALUE_BYTES, 4U,
|
|
value2, sizeof(value2)));
|
|
length = setp_publish_finish(&builder);
|
|
CHECK(length == SETP_PUBLISH_HEADER_SIZE
|
|
+ (2U * SETP_PUBLISH_ITEM_HEADER_SIZE) + sizeof(value1) + sizeof(value2));
|
|
CHECK(setp_publish_view_init(&view, payload, (uint16_t)length));
|
|
CHECK(view.subscription_id == 7U);
|
|
CHECK(view.sample_sequence == 9U);
|
|
CHECK(view.timestamp_ms == 123456U);
|
|
CHECK(setp_publish_next(&view, &item));
|
|
CHECK(item.address == 0x10U);
|
|
CHECK(item.encoding == SETP_VALUE_U16);
|
|
CHECK(item.data_length == sizeof(value1));
|
|
CHECK(setp_publish_next(&view, &item));
|
|
CHECK(item.address == 0x20U);
|
|
CHECK(item.element_count == 4U);
|
|
CHECK(view.remaining == 0U);
|
|
CHECK(!setp_publish_next(&view, &item));
|
|
}
|
|
|
|
int main(void)
|
|
{
|
|
test_crc_and_reference_frame();
|
|
test_parser();
|
|
test_status();
|
|
test_can_binding();
|
|
test_firmware();
|
|
test_telemetry();
|
|
(void)printf("checks: %d, failures: %d\n", checks, failures);
|
|
return failures == 0 ? 0 : 1;
|
|
}
|