diff --git a/buf/Makefile b/buf/Makefile new file mode 100644 index 0000000..eb9eeb6 --- /dev/null +++ b/buf/Makefile @@ -0,0 +1,14 @@ +CC = gcc +CFLAGS = -Wall -O2 + +TARGET = main.elf +SRC = main.c buf.c + +#LDFLAGS = + +$(TARGET): $(SRC) + @echo CC $@ + @$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS) + +clean: + rm -f $(TARGET) diff --git a/buf/buf.c b/buf/buf.c new file mode 100644 index 0000000..d68f7e6 --- /dev/null +++ b/buf/buf.c @@ -0,0 +1,107 @@ +#include +#include + +/** + * @brief Write a 8 bit unsigned int to the buffer + * + * @param buf The buffer to write to + * @param value The 8 bit unsigned int to write + * @param index The index read/write pointer + */ +void buf_append_u8(uint8_t *buf, uint8_t value, size_t *index) { + buf[(*index)++] = value; +} + +/** + * @brief Write a 16 bit signed int to the buffer + * + * @param buf The buffer to write to + * @param value The 16 bit signed int to write + * @param index The index read/write pointer + */ +void buf_append_i16(uint8_t *buf, int16_t value, size_t *index) { + buf[(*index)++] = (value >> 8) & 0xFF; + buf[(*index)++] = value & 0xFF; +} + +/** + * @brief Write a 32 bit signed int to the buffer + * + * @param buf The buffer to write to + * @param value The 32 bit signed int to write + * @param index The index read/write pointer + */ +void buf_append_i32(uint8_t *buf, int32_t value, size_t *index) { + buf[(*index)++] = (value >> 24) & 0xFF; + buf[(*index)++] = (value >> 16) & 0xFF; + buf[(*index)++] = (value >> 8) & 0xFF; + buf[(*index)++] = value & 0xFF; +} + +/** + * @brief Write a 32 bit float to the buffer + * + * @param buf The buffer to write to + * @param value The 32 bit float to write + * @param index The index read/write pointer + */ +void buf_append_f32(uint8_t *buf, float value, size_t *index) { + uint32_t u; + memcpy(&u, &value, sizeof(float)); // Safe conversion + buf_append_i32(buf, (int32_t)u, index); +} + +/** + * @brief Read a 8 bit unsigned int from the buffer + * + * @param buf The buffer to read from + * @param index The index read/write pointer + * @return 8 bit unsigned int + */ +uint8_t buf_read_u8(const uint8_t *buf, size_t *index) { + return buf[(*index)++]; +} + +/** + * @brief Read a 16 bit int from the buffer + * + * @param buf The buffer to read from + * @param index The index read/write pointer + * @return 16 bit signed int + */ +uint8_t buf_read_i16(const uint8_t *buf, size_t *index) { + int16_t val = 0; + val |= ((int16_t)buf[(*index)++]) << 8; + val |= ((int16_t)buf[(*index)++]); + return val; +} + +/** + * @brief Read a 32 bit int from the buffer + * + * @param buf The buffer to read from + * @param index The index read/write pointer + * @return 32 bit signed int + */ +int32_t buf_read_i32(const uint8_t *buf, size_t *index) { + int32_t val = 0; + val |= ((int32_t)buf[(*index)++]) << 24; + val |= ((int32_t)buf[(*index)++]) << 16; + val |= ((int32_t)buf[(*index)++]) << 8; + val |= ((int32_t)buf[(*index)++]); + return val; +} + +/** + * @brief Read a 32 bit float from the buffer + * + * @param buf The buffer to read from + * @param index The index read/write pointer + * @return 32 bit float + */ +float buf_read_f32(const uint8_t *buf, size_t *index) { + uint32_t u = (uint32_t)buf_read_i32(buf, index); + float f; + memcpy(&f, &u, sizeof(float)); + return f; +} diff --git a/buf/buf.h b/buf/buf.h new file mode 100644 index 0000000..40c5735 --- /dev/null +++ b/buf/buf.h @@ -0,0 +1,12 @@ +#include +#include + +void buf_append_u8(uint8_t *buf, uint8_t value, size_t *index); +void buf_append_i16(uint8_t *buf, int16_t value, size_t *index); +void buf_append_i32(uint8_t *buf, int32_t value, size_t *index); +void buf_append_f32(uint8_t *buf, float value, size_t *index); + +uint8_t buf_read_u8(const uint8_t *buf, size_t *index); +uint8_t buf_read_i16(const uint8_t *buf, size_t *index); +int32_t buf_read_i32(const uint8_t *buf, size_t *index); +float buf_read_f32(const uint8_t *buf, size_t *index); diff --git a/buf/main.c b/buf/main.c new file mode 100644 index 0000000..adaa262 --- /dev/null +++ b/buf/main.c @@ -0,0 +1,87 @@ +#include "buf.h" +#include +#include +#include +#include + +/* + * Inspired by Vedders bldc code. + * + * I stumbled upon this beautiful solution when reading the motor control firmware from the vesc project. + * It effectively packs/serializes data into a buffer that could easily be sent over, say, uart. The original solution + * also does fp magic in a seemingly portable way. + * + * For the original GPLv3 licensed source, see: + * https://github.com/vedderb/bldc/blob/master/util/buffer.c + * + * For a more permissive MIT licensed source, see: + * https://github.com/waas-rent/vesc_can_sdk/blob/main/vesc_buffer.c + */ + +/* This buffer can live anywhere, make sure its long enough to hold your data! */ +uint8_t buf[100]; + +/* Float comparison with error tolerance */ +static inline bool float_eq(float a, float b, float epsilon) { + return fabsf(a - b) < epsilon; +} + +int main(void) { + /* This index acts as a read/write pointer */ + size_t index = 0; + + /* Declare and define data to be serialized */ + uint8_t uint8bit = 8; + int16_t int16bit = 16; + int32_t int32bit = 32; + float float32bit = 1337.42; + + /* When appending the buffer, the index is passed along and incremented accordingly */ + buf_append_u8(buf, uint8bit, &index); + assert(index == 1); + buf_append_i16(buf, int16bit, &index); + assert(index == 3); + buf_append_i32(buf, int32bit, &index); + assert(index == 7); + buf_append_f32(buf, float32bit, &index); + assert(index == 11); + + /* + * Note that after we're done appending, the index value will hold the + * length of the 'message', so, if you're piping this through uart or some + * other medium, you dont have to send the entire buffer length, just send + * up to the index. + * + * For bonus points for doing it vesc-style, prepend the message with the message type (a + * byte-length enum shared by both sides, for example), as well as a message + * length. You could also use the last two bytes of every message for a + * crc16 checksum for error resilience. + */ + + /* Zero the original data */ + uint8bit = 0; + int16bit = 0; + int32bit = 0; + float32bit = 0.f; + + /* Read from beginning */ + index = 0; + + /* Re-populate our original data */ + uint8bit = buf_read_u8(buf, &index); + assert(index == 1); + int16bit = buf_read_i16(buf, &index); + assert(index == 3); + int32bit = buf_read_i32(buf, &index); + assert(index == 7); + float32bit = buf_read_f32(buf, &index); + assert(index == 11); + + /* Assert that it all worked out */ + assert(uint8bit == 8); + assert(int16bit == 16); + assert(int32bit == 32); + assert(float_eq(float32bit, 1337.42, 0.01)); + + printf("All asserts passed!\n"); +}