77 lines
2.0 KiB
C
77 lines
2.0 KiB
C
#include "message_builder.h"
|
|
#include "message_parser.h"
|
|
#include <stdbool.h>
|
|
#include <stddef.h>
|
|
|
|
bool needs_stuffing_byte(uint8_t byte) {
|
|
return (byte == StartByte || byte == EscapeByte || byte == EndByte);
|
|
}
|
|
|
|
bool add_byte_with_length_check(uint8_t byte, uint8_t write_index,
|
|
uint8_t *data, uint8_t max_length) {
|
|
if (write_index >= max_length) {
|
|
return false;
|
|
}
|
|
data[write_index] = byte;
|
|
return true;
|
|
}
|
|
|
|
int build_message(uint8_t msgid, const uint8_t *payload, size_t payload_len,
|
|
uint8_t *msg_buffer, size_t msg_buffer_size) {
|
|
|
|
if (payload_len + 4 > msg_buffer_size) {
|
|
return PayloadBiggerThenBuffer;
|
|
}
|
|
|
|
uint8_t checksum = 0;
|
|
size_t write_index = 0;
|
|
msg_buffer[write_index++] = StartByte;
|
|
|
|
if (needs_stuffing_byte(msgid)) {
|
|
if (!add_byte_with_length_check(EscapeByte, write_index, msg_buffer,
|
|
msg_buffer_size)) {
|
|
return BufferOverFlow;
|
|
}
|
|
write_index++;
|
|
}
|
|
if (!add_byte_with_length_check(msgid, write_index, msg_buffer,
|
|
msg_buffer_size)) {
|
|
return BufferOverFlow;
|
|
}
|
|
write_index++;
|
|
checksum ^= msgid;
|
|
|
|
for (size_t i = 0; i < payload_len; i++) {
|
|
if (needs_stuffing_byte(payload[i])) {
|
|
if (!add_byte_with_length_check(EscapeByte, write_index, msg_buffer,
|
|
msg_buffer_size)) {
|
|
return BufferOverFlow;
|
|
}
|
|
write_index++;
|
|
}
|
|
if (!add_byte_with_length_check(payload[i], write_index, msg_buffer,
|
|
msg_buffer_size)) {
|
|
return BufferOverFlow;
|
|
}
|
|
|
|
write_index++;
|
|
checksum ^= payload[i];
|
|
}
|
|
|
|
if (needs_stuffing_byte(checksum)) {
|
|
if (!add_byte_with_length_check(EscapeByte, write_index, msg_buffer,
|
|
msg_buffer_size)) {
|
|
return BufferOverFlow;
|
|
}
|
|
write_index++;
|
|
}
|
|
|
|
if (write_index + 2 > msg_buffer_size) {
|
|
return BufferOverFlow;
|
|
}
|
|
msg_buffer[write_index++] = checksum;
|
|
msg_buffer[write_index++] = EndByte;
|
|
|
|
return write_index;
|
|
}
|