diff --git a/APP/CDH_App/src/main.c b/APP/CDH_App/src/main.c index 55d9128..17d6cc2 100644 --- a/APP/CDH_App/src/main.c +++ b/APP/CDH_App/src/main.c @@ -14,97 +14,19 @@ #define SYSID_COMMS 3 #define ENDPOINT_COMMS_TEST 154 - - -void simple_example(void) { - while (1) { - const uint8_t payload[10] = "Meow meow"; - uint8_t buf[LFP_HEADER_SIZE + lfp_encoded_body_length(payload, sizeof(payload))]; - - if (lfp_encode_to_buf(SYSID_CDH, SYSID_COMMS, ENDPOINT_COMMS_TEST, LFP_DIRECTION_REQUEST, - payload,sizeof(payload), - buf,sizeof(buf) - ) < 0) { - ULOG_ERROR("Failed to encode LFP packet!"); - } - - ULOG_INFO("LFP encoding successful: {}", ULOG_SLICE(buf)); - - lfp_header_t header; - if (lfp_parse_header(buf, &header) != LFP_EOK) { - ULOG_ERROR("Failed to decode LFP header!"); - } - - ULOG_INFO( - "LFP header decoded! \n\tInitiator: {} \n\tTarget: {} \n\tEndpoint: {} \n\tDirection: {} \n\tLen: {}", - header.initiator, header.target, header.endpoint, (uint8_t)header.direction, header.encoded_length, - ); - - uint8_t * p_body_buf = buf + LFP_HEADER_SIZE; - if (lfp_body_decoder(&header, p_body_buf, p_body_buf, sizeof(buf) - LFP_HEADER_SIZE) == LFP_FAIL) { - ULOG_ERROR("Failed to decode buffer"); - } - - ULOG_INFO("Packet decoded successfully: {}", (char *)p_body_buf); - - k_msleep(2000); - } +void asn1_example_bms_system_status_req(const BMSSystemStatusRequest * p_payload, const asn1_lfp_decode_data_t * p_data) { + ULOG_INFO("Received system status request"); } - -void on_msg(const lfp_header_t * p_header, const uint8_t * p_body, uint16_t body_length, void * p_ctx) { - ARG_UNUSED(p_ctx); - ARG_UNUSED(body_length); - - ULOG_INFO("Decoded packet on endpoint {}: {}", p_header->endpoint, (char *)p_body); +void asn1_example_bms_system_status_res(const BMSSystemStatusResponse * p_payload, const asn1_lfp_decode_data_t * p_data) { + ULOG_INFO("Received system status response: version {}, uptime {}", p_payload->version, p_payload->uptime); } -void on_error(const lfp_code_t reason, void * p_ctx) { +void asn1_example_stream_error_handler(lfp_code_t reason, void * p_ctx) { ARG_UNUSED(p_ctx); ULOG_ERROR("Stream error: {}", reason); } -void stream_example(void) { - const uint8_t payload[10] = "Woof woof"; - uint8_t buf[LFP_HEADER_SIZE + lfp_encoded_body_length(payload, sizeof(payload))]; - - if (lfp_encode_to_buf(SYSID_CDH, SYSID_COMMS, ENDPOINT_COMMS_TEST, LFP_DIRECTION_REQUEST, - payload,sizeof(payload), - buf,sizeof(buf) - ) < 0) { - ULOG_ERROR("Failed to encode LFP packet!"); - } - - ULOG_INFO("LFP encoding successful: {}", ULOG_SLICE(buf)); - - lfp_stream_ctx_t ctx; - lfp_stream_init(&ctx, (uint8_t[1024]){}, 1024, NULL, on_msg, on_error, NULL); - lfp_stream_update_buf(&ctx, buf, sizeof(buf)); - - ULOG_INFO("LFP Packet ingested"); -} - -//static const struct device * dev_demo_uart = DEVICE_DT_GET(DT_NODELABEL(demo_serial)); -void asn1_example_cb(uint8_t chr, void * p_ctx) { - //uart_poll_out(dev_demo_uart, chr); -} - -void asn1_example(void) { - if (!ASN1_LFP_SERIALIZE(cdhSystemId, bmsSystemId, BMSSystemStatusResponse, asn1_example_cb, NULL, { - .uptime = 0xDEADBEEF, - .version = 12 - })) { - ULOG_ERROR("LFP encoding failed! {} {}", ASN1_LFP_ERROR_CODE.asn1, ASN1_LFP_ERROR_CODE.lfp); - } -} - -void asn1_example_bms_system_status_req(const BMSSystemStatusRequest * p_payload, const asn1_lfp_decode_data_t * p_data) { - ULOG_INFO("Received system status request"); -} -void asn1_example_bms_system_status_res(const BMSSystemStatusResponse * p_payload, const asn1_lfp_decode_data_t * p_data) { - ULOG_INFO("Received system status response: version {}, uptime {}", p_payload->version, p_payload->uptime); -} - void asn1_example_error_handler(const int asn1_error, const asn1_lfp_decode_data_t * p_data) { ULOG_ERROR("Failed to decode message {} (error {})", lfp_composite_id(p_data->p_header), asn1_error); } @@ -113,7 +35,7 @@ void asn1_example_on_msg(const lfp_header_t * p_header, const uint8_t * p_body, ARG_UNUSED(p_ctx); ARG_UNUSED(body_length); - ULOG_INFO("Decoded packet on endpoint {}: {}", p_header->endpoint, (char *)p_body); + ULOG_INFO("Decoded packet on endpoint {}: {}", p_header->endpoint, ((struct ulog_slice){.data=(void *)p_body,.size=body_length})); const asn1_lfp_decode_data_t data = { .p_header = p_header, @@ -133,7 +55,26 @@ void asn1_example_on_msg(const lfp_header_t * p_header, const uint8_t * p_body, ULOG_WARN("Received unknown message {}", ULOG_SLICE_PTR((lfp_header_t *)p_header)); } +void asn1_example(void) { + uint8_t payload[ASN1_LFP_SEND_BUF_SIZE(BMSSystemStatusResponse)]; + + const uint32_t size = ASN1_LFP_SERIALIZE(cdhSystemId, bmsSystemId, BMSSystemStatusResponse, payload, sizeof(payload), { + .uptime = 0xDEADBEEF, + .version = 12 + }); + if (!size) { + ULOG_ERROR("LFP encoding failed! {} {}", ASN1_LFP_ERROR_CODE.asn1, ASN1_LFP_ERROR_CODE.lfp); + } + + ULOG_INFO("Sending: {}", ((struct ulog_slice) {.data = payload, .size = size})); + + uint8_t payload2[ASN1_LFP_RECV_BUF_SIZE(BMSSystemStatusResponse)]; + lfp_stream_ctx_t stream; + lfp_stream_init(&stream, payload2, sizeof(payload2), NULL, asn1_example_on_msg, asn1_example_stream_error_handler, NULL); + lfp_stream_update_buf(&stream, payload, size); +} + int main(void) { - stream_example(); + asn1_example(); } \ No newline at end of file diff --git a/MIDDLEWARE/ASN1SCC/asn1_lfp.h b/MIDDLEWARE/ASN1SCC/asn1_lfp.h index 1992160..47e8acf 100644 --- a/MIDDLEWARE/ASN1SCC/asn1_lfp.h +++ b/MIDDLEWARE/ASN1SCC/asn1_lfp.h @@ -20,95 +20,65 @@ static struct { /// Bool value for success or failure of the last ASN1_LFP operation #define ASN1_LFP_SUCCESS() (ASN1_LFP_ERROR_CODE.lfp == LFP_EOK && ASN1_LFP_ERROR_CODE.asn1 == 0) -/** - * Macro to serialize a struct to an LFP target. Use ASN1_LFP_SUCCESS() to check for success - * @param INITIATOR System ID of the transaction initiator - * @param TARGET System ID of the transaction target - * @param TYPE type name of the asn1 generated struct, pass directly, do not quote. - * @param P_WRITE_CB callback passed to lfp_encode() - * @param P_CTX user context pointer passed to lfp_encode() - * @param ... the struct value to serialize. Passed as the last argument - * @return true on success, false on error (see ASN1_LFP_ERROR_CODE) - */ -#define ASN1_LFP_SERIALIZE(INITIATOR, TARGET, TYPE, P_WRITE_CB, P_CTX, ...) \ - ({ \ - /*Reset last error*/ \ - asn1_lfp_last_error.lfp = 0; \ - asn1_lfp_last_error.asn1 = 0; \ - \ - /*Setup serialized but unencoded buffer*/ \ - const TYPE payload = __VA_ARGS__; \ - unsigned char payload_buffer[TYPE ## _REQUIRED_BYTES_FOR_ENCODING]; \ - /*Prepare encoder*/ \ - BitStream encoder; \ - BitStream_Init(&encoder, payload_buffer, sizeof(payload_buffer)); \ - \ - const uint8_t endpoint = (lfpId ## TYPE); \ - \ - /*Do encoding*/ \ - if ((TYPE ## _Encode)(&payload, &encoder, &asn1_lfp_last_error.asn1, true)) { \ - asn1_lfp_last_error.lfp = lfp_encode( \ - INITIATOR, \ - TARGET, \ - endpoint & 0xFE, \ - endpoint & 0x01, \ - payload_buffer, \ - BitStream_GetLength(&encoder), \ - P_WRITE_CB, \ - P_CTX \ - ); \ - } \ - ASN1_LFP_SUCCESS(); \ - }) +/// Size of the buffer required by ASN1_LFP_SERIALIZE() for a given message type +#define ASN1_LFP_SEND_BUF_SIZE(TYPE) (LFP_HEADER_SIZE + LFP_ENCODED_BODY_LENGTH_APPROX(TYPE ## _REQUIRED_BYTES_FOR_ENCODING)) +/// Size of the buffer required by lfp_stream for a given message type +// We dont need to hold the crc in the recv buffer, the stream parser consumes it +#define ASN1_LFP_RECV_BUF_SIZE(TYPE) (LFP_ENCODED_BODY_LENGTH_APPROX(TYPE ## _REQUIRED_BYTES_FOR_ENCODING) - LFP_DATA_CRC32_SIZE) /** * Macro to serialize a struct to an LFP target in one shot. Use ASN1_LFP_SUCCESS() to check for success * @param INITIATOR System ID of the transaction initiator * @param TARGET System ID of the transaction target * @param TYPE type name of the asn1 generated struct, pass directly, do not quote. + * @param P_BUFFER buffer that will contain the encoded message, use ASN1_LFP_BUF_SIZE() to determine the maximal size for a given message + * @param BUFFER_LEN length P_BUFFER * @param P_WRITE_CB callback called with the encoded buffer, the length and P_CTX * @param P_CTX user context pointer passed to P_WRITE_CB * @param ... the struct value to serialize. Passed as the last argument - * @return true on success, false on error (see ASN1_LFP_ERROR_CODE) + * @return encoded length on success, 0 on error (see ASN1_LFP_ERROR_CODE) */ -#define ASN1_LFP_SERIALIZE_BUF(INITIATOR, TARGET, TYPE, P_WRITE_CB, P_CTX, ...) \ +#define ASN1_LFP_SERIALIZE(INITIATOR, TARGET, TYPE, P_BUFFER, BUFFER_LEN, ...) \ ({ \ + uint32_t asn1_lfp__ret = 0; \ /*Reset last error*/ \ asn1_lfp_last_error.lfp = 0; \ asn1_lfp_last_error.asn1 = 0; \ \ - /*Setup serialized but unencoded buffer*/ \ - const TYPE payload = __VA_ARGS__; \ - const uint16_t asn1_size = TYPE ## _REQUIRED_BYTES_FOR_ENCODING; \ - unsigned char payload_buffer[ \ - LFP_HEADER_SIZE + LFP_ENCODED_BODY_LENGTH_APPROX(asn1_size) \ - ]; \ - unsigned char * p_asn1_payload = payload_buffer + sizeof(payload_buffer) - asn1_size; \ - /*Prepare encoder*/ \ - BitStream encoder; \ - BitStream_Init(&encoder, p_asn1_payload, asn1_size); \ + /*Validate that the buffer is big enough, otherwise abort*/ \ + if ((BUFFER_LEN) < ASN1_LFP_RECV_BUF_SIZE(TYPE)) { \ + asn1_lfp_last_error.lfp = LFP_EOVERFLOW; \ + } else { \ + /*Setup serialized but unencoded buffer*/ \ + const TYPE asn1_lfp__payload = __VA_ARGS__; \ + const uint16_t asn1_lfp__asn1_size = TYPE ## _REQUIRED_BYTES_FOR_ENCODING; \ + unsigned char * asn1_lfp__p_asn1_payload = (P_BUFFER) + (BUFFER_LEN) - asn1_lfp__asn1_size; \ + /*Prepare encoder*/ \ + BitStream asn1_lfp__encoder; \ + BitStream_Init(&asn1_lfp__encoder, asn1_lfp__p_asn1_payload, asn1_lfp__asn1_size); \ \ - const uint8_t endpoint = (lfpId ## TYPE); \ + const uint8_t asn1_lfp__endpoint = (lfpId ## TYPE); \ \ - /*Do encoding*/ \ - if ((TYPE ## _Encode)(&payload, &encoder, &asn1_lfp_last_error.asn1, true)) { \ - lfp_size_or_code_t ret = lfp_encode_to_buf( \ - INITIATOR, \ - TARGET, \ - endpoint & 0xFE, \ - endpoint & 0x01, \ - p_asn1_payload, \ - BitStream_GetLength(&encoder), \ - payload_buffer, \ - sizeof(payload_buffer) \ - ); \ - if(ret < 0) { \ - asn1_lfp_last_error.lfp = ret; \ - } else { \ - P_WRITE_CB(payload_buffer, ret, P_CTX); \ + /*Do encoding*/ \ + if ((TYPE ## _Encode)(&asn1_lfp__payload, &asn1_lfp__encoder, &asn1_lfp_last_error.asn1, true)) { \ + const lfp_size_or_code_t asn1_lfp__lfp_ret = lfp_encode( \ + (INITIATOR), \ + (TARGET), \ + asn1_lfp__endpoint & 0xFE, \ + asn1_lfp__endpoint & 0x01, \ + asn1_lfp__p_asn1_payload, \ + BitStream_GetLength(&asn1_lfp__encoder), \ + (P_BUFFER), \ + (BUFFER_LEN) \ + ); \ + if(asn1_lfp__lfp_ret < 0) { \ + asn1_lfp_last_error.lfp = (lfp_code_t)asn1_lfp__lfp_ret; \ + } else { \ + asn1_lfp__ret = asn1_lfp__lfp_ret; \ + } \ } \ } \ - ASN1_LFP_SUCCESS(); \ + asn1_lfp__ret; \ }) // Forward decl for the struct to break the cycle @@ -149,7 +119,7 @@ typedef struct asn1_lfp_decode_data_t asn1_lfp_decode_data_t; bool condition = lfp_composite_id(DATA.p_header) == ((TARGET_SYSTEMID << 8) | (lfpId ## TYPE)); \ if (condition) { \ BitStream decoder; \ - BitStream_Init(&decoder, (void *)DATA.p_body, DATA.body_length); \ + BitStream_AttachBuffer(&decoder, (void *)DATA.p_body, DATA.body_length); \ int errCode; \ TYPE payload; \ \ diff --git a/MIDDLEWARE/ASN1SCC/example.c b/MIDDLEWARE/ASN1SCC/example.c index c30d93e..f14ee98 100644 --- a/MIDDLEWARE/ASN1SCC/example.c +++ b/MIDDLEWARE/ASN1SCC/example.c @@ -1,16 +1,6 @@ -//static const struct device * dev_demo_uart = DEVICE_DT_GET(DT_NODELABEL(demo_serial)); -void asn1_example_cb(uint8_t chr, void * p_ctx) { - //uart_poll_out(dev_demo_uart, chr); -} - -void asn1_example(void) { - if (!ASN1_LFP_SERIALIZE(cdhSystemId, bmsSystemId, BMSSystemStatusResponse, asn1_example_cb, NULL, { - .uptime = 0xDEADBEEF, - .version = 12 - })) { - ULOG_ERROR("LFP encoding failed! {} {}", ASN1_LFP_ERROR_CODE.asn1, ASN1_LFP_ERROR_CODE.lfp); - } -} +#define SYSID_CDH 1 +#define SYSID_COMMS 3 +#define ENDPOINT_COMMS_TEST 154 void asn1_example_bms_system_status_req(const BMSSystemStatusRequest * p_payload, const asn1_lfp_decode_data_t * p_data) { ULOG_INFO("Received system status request"); @@ -19,6 +9,12 @@ void asn1_example_bms_system_status_res(const BMSSystemStatusResponse * p_payloa ULOG_INFO("Received system status response: version {}, uptime {}", p_payload->version, p_payload->uptime); } +void asn1_example_stream_error_handler(lfp_code_t reason, void * p_ctx) { + ARG_UNUSED(p_ctx); + + ULOG_ERROR("Stream error: {}", reason); +} + void asn1_example_error_handler(const int asn1_error, const asn1_lfp_decode_data_t * p_data) { ULOG_ERROR("Failed to decode message {} (error {})", lfp_composite_id(p_data->p_header), asn1_error); } @@ -27,7 +23,7 @@ void asn1_example_on_msg(const lfp_header_t * p_header, const uint8_t * p_body, ARG_UNUSED(p_ctx); ARG_UNUSED(body_length); - ULOG_INFO("Decoded packet on endpoint {}: {}", p_header->endpoint, (char *)p_body); + ULOG_INFO("Decoded packet on endpoint {}: {}", p_header->endpoint, ((struct ulog_slice){.data=(void *)p_body,.size=body_length})); const asn1_lfp_decode_data_t data = { .p_header = p_header, @@ -45,4 +41,23 @@ void asn1_example_on_msg(const lfp_header_t * p_header, const uint8_t * p_body, } ULOG_WARN("Received unknown message {}", ULOG_SLICE_PTR((lfp_header_t *)p_header)); +} + +void asn1_example(void) { + uint8_t payload[ASN1_LFP_SEND_BUF_SIZE(BMSSystemStatusResponse)]; + + const uint32_t size = ASN1_LFP_SERIALIZE(cdhSystemId, bmsSystemId, BMSSystemStatusResponse, payload, sizeof(payload), { + .uptime = 0xDEADBEEF, + .version = 12 + }); + if (!size) { + ULOG_ERROR("LFP encoding failed! {} {}", ASN1_LFP_ERROR_CODE.asn1, ASN1_LFP_ERROR_CODE.lfp); + } + + ULOG_INFO("Sending: {}", ((struct ulog_slice) {.data = payload, .size = size})); + + uint8_t payload2[ASN1_LFP_RECV_BUF_SIZE(BMSSystemStatusResponse)]; + lfp_stream_ctx_t stream; + lfp_stream_init(&stream, payload2, sizeof(payload2), NULL, asn1_example_on_msg, asn1_example_stream_error_handler, NULL); + lfp_stream_update_buf(&stream, payload, size); } \ No newline at end of file diff --git a/MIDDLEWARE/LFP/body.c b/MIDDLEWARE/LFP/body.c index 5f6a039..43e5c6a 100644 --- a/MIDDLEWARE/LFP/body.c +++ b/MIDDLEWARE/LFP/body.c @@ -110,9 +110,6 @@ lfp_size_or_fail_t lfp_body_decoder( return output_size; } -uint16_t lfp_encoded_body_length( - const uint8_t *p_data, - const uint16_t length -) { - return lfp_cobs_encode(p_data, length, NULL, NULL) + LFP_DATA_CRC32_SIZE; +uint16_t lfp_encoded_body_length(const uint8_t *p_data, const uint16_t length) { + return lfp_cobs_encode_size(p_data, length) + LFP_DATA_CRC32_SIZE; } \ No newline at end of file diff --git a/MIDDLEWARE/LFP/cobs.c b/MIDDLEWARE/LFP/cobs.c index 580df3e..72a2737 100644 --- a/MIDDLEWARE/LFP/cobs.c +++ b/MIDDLEWARE/LFP/cobs.c @@ -1,14 +1,10 @@ -#include "cobs_private.h" - -#include + #include "cobs_private.h" #include "lfp.h" -uint16_t lfp_cobs_encode( +uint16_t lfp_cobs_encode_size( const uint8_t * p_buf, - const uint16_t length, - const lfp_cobs_write_cb_t p_write_cb, - void * p_ctx + const uint16_t length ) { uint16_t encoded_size = 0; @@ -18,7 +14,7 @@ uint16_t lfp_cobs_encode( // Find the size of the current block while (block_length < 0xFF) { - // We reached the end of the buffer, we assume there's an extra 0 byte so we increment the block length + // We reached the end of the buffer, we assume there's an extra LFP_PREAMBLE byte, so we increment the block length if (p_block + block_length >= p_buf + length) { // Consume the virtual byte block_length++; @@ -32,23 +28,12 @@ uint16_t lfp_cobs_encode( } // Write out the block size - if (p_write_cb != NULL) { - p_write_cb(block_length + LFP_PREAMBLE, p_ctx); - } - encoded_size++; - - // Write out all bytes in block except the last one - if (p_write_cb != NULL) { - for (const uint8_t * p_byte = p_block; p_byte < p_block + block_length - 1; p_byte++) { - p_write_cb(*p_byte, p_ctx); - } - } - encoded_size += block_length - 1; + encoded_size += block_length; // Move to the next block, skipping the last byte p_block = p_block + block_length; - // If we have a full block, unskip the last byte so that it can go in the next buffer, unless its EOF, because + // If we have a full block, un-skip the last byte so that it can go in the next buffer, unless it's EOF, because // we don't care about encoding the virtual byte if (block_length == 0xFF && p_block <= p_buf + length) { p_block--; @@ -58,49 +43,85 @@ uint16_t lfp_cobs_encode( return encoded_size; } -// the decoded size cant be 0xFFFF because of the overhead bytes, so LFP_FAIL is safe to return -lfp_size_or_fail_t lfp_cobs_decode( - const uint8_t * p_buf, - const uint16_t length, - const lfp_cobs_write_cb_t p_write_cb, - void * p_ctx -) { - const uint8_t * p_byte = p_buf; - // Final size after decoding - uint16_t decoded_size = 0; - - // While we are inside the encoded buffer - while (p_byte < p_buf + length) { - // Get the length of the current block, but make sure it's not going to go out of bounds - const uint8_t block_length = *(p_byte++) - 1 - LFP_PREAMBLE; - if (p_byte + block_length > p_buf + length) { - return LFP_FAIL; +void lfp_cobs_encode_init(lfp_cobs_encode_ctx_t * p_ctx, const uint8_t * p_buf, const uint16_t length) +{ + *p_ctx = (lfp_cobs_encode_ctx_t) { + .p_buf = p_buf, + .length = length, + .p_byte = p_buf, + .block_length = 0, + .full_block = false + }; +} + +static uint8_t lfp_cobs_encode_block_length(const lfp_cobs_encode_ctx_t* p_ctx) +{ + uint8_t block_length = 0; + + // Find the size of the current block + while (block_length < 0xFF) { + // We reached the end of the buffer, we assume there's an extra LFP_PREAMBLE byte, so we increment the block length + if (p_ctx->p_byte + block_length >= p_ctx->p_buf + p_ctx->length) { + // Consume the virtual byte + block_length++; + break; } - // Fast path: if we don't have a callback, we only care about the length, so don't iterate every single byte - if (p_write_cb == NULL) { - p_byte += block_length; - } else { - for (uint8_t i = 0; i < block_length; i++) { - p_write_cb(*(p_byte++), p_ctx); - } + // Consume non-null bytes + if (p_ctx->p_byte[block_length++] == LFP_PREAMBLE) { + break; } + } - // Add the block we just read to the size - decoded_size += block_length; + return block_length; +} - // Add the delimiter byte we removed back in: - // - If the block size isn't 0xFE - if (block_length != 0xFE && p_byte < p_buf + length) { - if (p_write_cb != NULL) { - p_write_cb(LFP_PREAMBLE, p_ctx); - } - decoded_size++; +uint8_t lfp_cobs_encode_next_byte(lfp_cobs_encode_ctx_t* p_ctx) +{ + // Allow a single virtual delim byte at the end of the buffer + if (p_ctx->p_byte > p_ctx->p_buf + p_ctx->length) { + return LFP_PREAMBLE; + } + + uint8_t byte; + + // We didn't compute a block length for this block yet. It's impossible to have a 0-length block. + if (p_ctx->block_length == 0) { + p_ctx->block_length = lfp_cobs_encode_block_length(p_ctx); + p_ctx->full_block = p_ctx->block_length == 0xFF; + byte = (uint8_t)(p_ctx->block_length + LFP_PREAMBLE); + } else { + // Write out all bytes in the block except the last one + byte = *p_ctx->p_byte++; + p_ctx->block_length--; + } + + // Special handling for the second-to-last byte, as the last byte is not encoded in the block + if (p_ctx->block_length <= 1) + { + // If the block isn't a full block, or if we are hitting the virtual byte, ignore the last byte of the block. + // If it's not a full block, the block length already encodes it, and if it's the virtual byte in a full block, + // we don't want to transcribe that virtual byte + if (!p_ctx->full_block || p_ctx->p_byte >= (p_ctx->p_buf + p_ctx->length)) + { + p_ctx->p_byte++; } + + // Reset the block length to move to the next one + p_ctx->block_length = 0; } - return decoded_size; + + return byte; } +lfp_code_t lfp_cobs_encode_finish(const lfp_cobs_encode_ctx_t* p_ctx) +{ + if (p_ctx->p_byte <= p_ctx->p_buf + p_ctx->length) { + return LFP_FAIL; + } + + return LFP_EOK; +} void lfp_cobs_decode_init(lfp_cobs_decode_ctx_t * p_ctx) { *p_ctx = (lfp_cobs_decode_ctx_t) { @@ -118,7 +139,7 @@ lfp_byte_or_code_t lfp_cobs_decode_update(lfp_cobs_decode_ctx_t * p_ctx, const u const bool emit_delim = p_ctx->_emit_delim; - // Setup next block + // Set up the next block const uint8_t block_length = chr - 1 - LFP_PREAMBLE; p_ctx->_block_length = block_length; p_ctx->_emit_delim = block_length != 0xFE; diff --git a/MIDDLEWARE/LFP/cobs_private.h b/MIDDLEWARE/LFP/cobs_private.h index 8f5cce6..48c4b6a 100644 --- a/MIDDLEWARE/LFP/cobs_private.h +++ b/MIDDLEWARE/LFP/cobs_private.h @@ -7,41 +7,34 @@ /** - * Callback when a COBS byte is available - * @param chr Newly available COBS byte - * @param p_ctx Pointer to the user context provided via lfp_cobs_encode() or lfp_cobs_decode() - */ -typedef void (*lfp_cobs_write_cb_t)(uint8_t chr, void * p_ctx); - -/** - * COBS encodes a buffer of data + * Gets the encoded size of the data if it were to be COBS encoded * @param p_buf Pointer to the buffer containing the data to encode * @param length Length of p_buf - * @param p_write_cb Callback for when a new encoded byte is available. Can be NULL to only compute size - * @param p_ctx User context, provided to p_write_cb - * @return The size of the encoded data, this value is undefined if the encoded data exceeds a u16 + * @return The size of the encoded data, this value is undefined if the encoded data exceeds an u16 */ -uint16_t lfp_cobs_encode( - const uint8_t * p_buf, - uint16_t length, - lfp_cobs_write_cb_t p_write_cb, - void * p_ctx -); +uint16_t lfp_cobs_encode_size(const uint8_t * p_buf, uint16_t length); /** - * COBS decodes a buffer of data - * @param p_buf Pointer to the buffer containing the data to decode - * @param length Length of p_buf - * @param p_write_cb Callback for when a new decoded byte is available. Can be NULL to only compute size - * @param p_ctx User context, provided to p_write_cb - * @return The size of the decoded data + * Starts the COBS encoding process for an unencoded buffer + * @param p_ctx Pointer to the opaque context + * @param p_buf Pointer to the unencoded data buffer + * @param length Length of the unencoded data buffer */ -lfp_size_or_fail_t lfp_cobs_decode( - const uint8_t * p_buf, - uint16_t length, - lfp_cobs_write_cb_t p_write_cb, - void * p_ctx -); +void lfp_cobs_encode_init(lfp_cobs_encode_ctx_t * p_ctx, const uint8_t * p_buf, uint16_t length); +/** + * Gets the next encoded byte + * @param p_ctx Pointer to the opaque context + * @return The next encoded byte, or LFP_PREAMBLE if complete + */ +uint8_t lfp_cobs_encode_next_byte(lfp_cobs_encode_ctx_t * p_ctx); +/** + * Checks if the encoder is in a valid final state + * @param p_ctx Pointer to the opaque context + * @return LFP_EOK if the encoding process was successful, or an error code otherwise + */ +lfp_code_t lfp_cobs_encode_finish(const lfp_cobs_encode_ctx_t * p_ctx); + + /** * Starts the decoding process for a COBS encoded buffer diff --git a/MIDDLEWARE/LFP/include/lfp.h b/MIDDLEWARE/LFP/include/lfp.h index b9bee5a..b8332d9 100644 --- a/MIDDLEWARE/LFP/include/lfp.h +++ b/MIDDLEWARE/LFP/include/lfp.h @@ -41,6 +41,7 @@ #define LFP_HEADER_CRC_MASK 0xFEFE // Can technically be higher but this simplifies things +// Computed to be: M + ceil(M / 254) + 5 = 0xFFFF - 1 /// Maximum amount of data safe to hold in an LFP message #define LFP_MAX_BODY_SIZE 0xFEF8 @@ -64,13 +65,6 @@ /// Generic failure #define LFP_FAIL (0xFFFF) -/** - * Callback called when a new encoded data byte is available - * @param chr Encoded data byte - * @param p_ctx Pointer to the user context provided to lfp_encode() - */ -typedef void (*lfp_write_cb_t)(uint8_t chr, void * p_ctx); - /// Direction that a message is intended towards /// /// TARGET == REQUEST @@ -86,7 +80,7 @@ typedef enum { /// 0-65534 on success, LFP_FAIL on unspecified failure typedef uint16_t lfp_size_or_fail_t; -/// 0-65535 on success, negative values are error codes +/// 0-MAX on success, negative values are error codes typedef int32_t lfp_size_or_code_t; /// 0-254 on success, negative values are error codes typedef int16_t lfp_byte_or_code_t; @@ -102,34 +96,11 @@ typedef int16_t lfp_code_t; * @param direction Direction of the LFP message * @param p_data Pointer to the data buffer * @param length Length of p_data - * @param p_write_cb Callback to call when a new encoded data byte is available - * @param p_ctx User context to provide to p_write_cb - * @return LFP_EOK if the message was successfully encoded, an error code otherwise - */ -lfp_code_t lfp_encode( - uint8_t initiator, - uint8_t target, - uint8_t endpoint, - lfp_direction_t direction, - const uint8_t *p_data, - uint16_t length, - lfp_write_cb_t p_write_cb, - void * p_ctx -); - -/** - * Helper to encode an LFP message directly to a buffer instead of using the callback system - * @param initiator ID of the system who initiated the request - * @param target ID of the system the request was/is intended for. Sends the response - * @param endpoint Endpoint to use - * @param direction Direction of the LFP message - * @param p_data Pointer to the data buffer - * @param length Length of p_data * @param p_dst Pointer to the buffer that will contain the encoded message * @param dst_len Length of p_dst * @return An error code if negative, the size of the encoded message otherwise */ -lfp_size_or_code_t lfp_encode_to_buf( +lfp_size_or_code_t lfp_encode( uint8_t initiator, uint8_t target, uint8_t endpoint, diff --git a/MIDDLEWARE/LFP/include/lfp/body.h b/MIDDLEWARE/LFP/include/lfp/body.h index aa65deb..9232efe 100644 --- a/MIDDLEWARE/LFP/include/lfp/body.h +++ b/MIDDLEWARE/LFP/include/lfp/body.h @@ -70,11 +70,11 @@ uint16_t lfp_encoded_body_length( * @param encoded_length Size of the encoded body buffer * @return Upper bound size required to hold the buffer after decoding */ -#define LFP_DECODED_BODY_LENGTH_APPROX(encoded_length) (encoded_length - (encoded_length / 254) - LFP_DATA_CRC32_SIZE) +#define LFP_DECODED_BODY_LENGTH_APPROX(encoded_length) ((encoded_length) - LFP_DATA_CRC32_SIZE - (((encoded_length) - LFP_DATA_CRC32_SIZE) / 254)) /** * Computes an upper bound for the number of bytes required to hold a body after encoding * @param encoded_length Size of the body buffer * @return Upper bound size required to hold the buffer after encoding */ -#define LFP_ENCODED_BODY_LENGTH_APPROX(length) (LFP_DATA_CRC32_SIZE + length + (((LFP_DATA_CRC32_SIZE + length) / 254) + 1)) \ No newline at end of file +#define LFP_ENCODED_BODY_LENGTH_APPROX(length) (LFP_DATA_CRC32_SIZE + (length) + (((length) / 254) + 1)) \ No newline at end of file diff --git a/MIDDLEWARE/LFP/include/lfp/cobs.h b/MIDDLEWARE/LFP/include/lfp/cobs.h index f093baa..4d17bb0 100644 --- a/MIDDLEWARE/LFP/include/lfp/cobs.h +++ b/MIDDLEWARE/LFP/include/lfp/cobs.h @@ -3,6 +3,21 @@ #include #include +/// Opaque context struct for the lfp_cobs_encode_* functions. All members are private +typedef struct { + /// Pointer to the unencoded data buffer + const uint8_t * p_buf; + /// Length of the buffer pointed by p_buf + uint16_t length; + /// Pointer to the current byte + const uint8_t * p_byte; + /// Length of the current COBS block being processed + uint8_t block_length; + /// If the current block is a full block or a block that ends in an LFP_PREAMBLE byte + bool full_block; +} lfp_cobs_encode_ctx_t; + + /// Opaque context struct for the lfp_cobs_decode_* functions. All members are private typedef struct { /// Number of encoded bytes left in the current block diff --git a/MIDDLEWARE/LFP/lfp.c b/MIDDLEWARE/LFP/lfp.c index f1f8d02..a53d0b0 100644 --- a/MIDDLEWARE/LFP/lfp.c +++ b/MIDDLEWARE/LFP/lfp.c @@ -4,16 +4,18 @@ #include "crc.h" #include "lfp/body.h" -lfp_code_t lfp_encode( +lfp_size_or_code_t lfp_encode( const uint8_t initiator, const uint8_t target, const uint8_t endpoint, const lfp_direction_t direction, const uint8_t *p_data, const uint16_t length, - const lfp_write_cb_t p_write_cb, - void * p_ctx + uint8_t *p_dst, + uint32_t dst_len ) { + int32_t encoded_size = 0; + if (initiator > LFP_LRI_INITIATOR_MAX) { return LFP_EINVAL; } @@ -42,98 +44,68 @@ lfp_code_t lfp_encode( uint16_t crc16 = LFP_CRC16_INIT; +#define WRITE_CHR(chr) if (encoded_size == dst_len) { return LFP_EOVERFLOW; } else { p_dst[encoded_size++] = chr; } + // Preamble - p_write_cb(LFP_PREAMBLE, p_ctx); + WRITE_CHR(LFP_PREAMBLE); // LRI uint8_t byte = LFP_LRI_INITIATOR_SET(0, initiator) | LFP_LRI_TARGET_SET(0, target); - p_write_cb(byte, p_ctx); + WRITE_CHR(byte); crc16 = lfp_crc16_update(crc16, byte); // Endpoint byte = endpoint | direction; - p_write_cb(byte, p_ctx); + WRITE_CHR(byte); crc16 = lfp_crc16_update(crc16, byte); // Length (High) byte = (encoded_length & 0xFF00) >> 8; - p_write_cb(byte, p_ctx); + WRITE_CHR(byte); crc16 = lfp_crc16_update(crc16, byte); // Length (Low) byte = (encoded_length & 0x00FF) >> 0; - p_write_cb(byte, p_ctx); + WRITE_CHR(byte); crc16 = lfp_crc16_update(crc16, byte); // Reserved 0 byte = 0; - p_write_cb(byte, p_ctx); + WRITE_CHR(byte); crc16 = lfp_crc16_update(crc16, byte); // Reserved 1 byte = 0; - p_write_cb(byte, p_ctx); + WRITE_CHR(byte); crc16 = lfp_crc16_update(crc16, byte); // Header checksum crc16 &= LFP_HEADER_CRC_MASK; - p_write_cb((crc16 & 0xFF00) >> 8, p_ctx); - p_write_cb((crc16 & 0x00FF) >> 0, p_ctx); + WRITE_CHR((crc16 & 0xFF00) >> 8); + WRITE_CHR((crc16 & 0x00FF) >> 0); // Data segment - lfp_cobs_encode(p_data, length, p_write_cb, p_ctx); + lfp_cobs_encode_ctx_t encode_ctx; + lfp_cobs_encode_init(&encode_ctx, p_data, length); + while (true) { + const uint8_t chr = lfp_cobs_encode_next_byte(&encode_ctx); + if (chr == 0xFF) break; + WRITE_CHR(chr); + } + const lfp_code_t ret = lfp_cobs_encode_finish(&encode_ctx); + if (ret != LFP_OK) { + return ret; + } // Data checksum const uint32_t crc32 = lfp_crc32(p_data, length); - p_write_cb(crc32 >> 25 & 0b01111111, p_ctx); - p_write_cb(crc32 >> 18 & 0b01111111, p_ctx); - p_write_cb(crc32 >> 11 & 0b01111111, p_ctx); - p_write_cb(crc32 >> 4 & 0b01111111, p_ctx); - p_write_cb(crc32 >> 0 & 0b00001111, p_ctx); + WRITE_CHR(crc32 >> 25 & 0b01111111); + WRITE_CHR(crc32 >> 18 & 0b01111111); + WRITE_CHR(crc32 >> 11 & 0b01111111); + WRITE_CHR(crc32 >> 4 & 0b01111111); + WRITE_CHR(crc32 >> 0 & 0b00001111); - return LFP_EOK; -} +#undef WRITE_CHR -// If size > capacity, we ran out of space -typedef struct lfp_encode_ctx { - int32_t size; - uint32_t capacity; - uint8_t * data; -} lfp_encode_ctx_t; - -static void lfp_encode_cb(const uint8_t chr, void * _p_ctx) { - lfp_encode_ctx_t * p_ctx = _p_ctx; - if (p_ctx->size >= p_ctx->capacity) { - // Go 1 above the limit, to flag the overflow - if (p_ctx->size == p_ctx->capacity) { - p_ctx->size++; - } - // Stop here, do not actually write into the buffer - return; - } - p_ctx->data[p_ctx->size++] = chr; + return encoded_size; } -lfp_size_or_code_t lfp_encode_to_buf( - const uint8_t initiator, - const uint8_t target, - const uint8_t endpoint, - const lfp_direction_t direction, - const uint8_t *p_data, - const uint16_t length, - uint8_t *p_dst, - const uint32_t dst_len -) { - lfp_encode_ctx_t ctx = { - .data = p_dst, - .size = 0, - .capacity = dst_len, - }; - const lfp_code_t res = lfp_encode(initiator, target, endpoint, direction, p_data, length, lfp_encode_cb, &ctx); - if (res != LFP_EOK) { - return res; - } - if (ctx.size > ctx.capacity) { - return LFP_EOVERFLOW; - } - return ctx.size; -} \ No newline at end of file diff --git a/Tools/remote_dev/open_console.sh b/Tools/remote_dev/open_console.sh index 426f52d..7748696 100755 --- a/Tools/remote_dev/open_console.sh +++ b/Tools/remote_dev/open_console.sh @@ -19,7 +19,7 @@ samv71() { # shellcheck disable=SC2087 ssh -tt -p "$ssh_port" "$ssh_user@$ssh_host" <<- EOF cd remote_files - FORCE_COLOR=1 TERM=xterm ulog-decoder -s /dev/serial/by-id/usb-Atmel_Corp._EDBG_CMSIS-DAP_* -b 115200 "${binary}.elf" 2>&1 | sed -u -r 's/WEST_TOPDIR\/[^\/]+\///' | ts '[%H:%M:%S]' + echo "${binary}.elf" | FORCE_COLOR=1 TERM=xterm entr -r -n ulog-decoder -s /dev/serial/by-id/usb-Atmel_Corp._EDBG_CMSIS-DAP_* -b 115200 "${binary}.elf" 2>&1 | sed -u -r 's/WEST_TOPDIR\/[^\/]+\///' | ts '[%H:%M:%S]' exit EOF echo "====Console(END)===="