diff --git a/utilities/payload_builder.c b/utilities/payload_builder.c new file mode 100644 index 0000000..d228d80 --- /dev/null +++ b/utilities/payload_builder.c @@ -0,0 +1,100 @@ +#include +#include "payload_builder.h" + +#define pb_check_capacity(pb, needed) \ + if ((pb)->current + (needed) > (pb)->end) { \ + if ((pb)->full_handler == NULL || !(pb)->full_handler(pb, needed)) (pb)->ok = 0; \ + } + +/** Write from a buffer */ +bool pb_buf(PayloadBuilder *pb, const uint8_t *buf, uint32_t len) +{ + pb_check_capacity(pb, len); + if (!pb->ok) return false; + + memcpy(pb->current, buf, len); + pb->current += len; + return true; +} + +/** Write s zero terminated string */ +bool pb_string(PayloadBuilder *pb, const char *str) +{ + uint32_t len = (uint32_t) strlen(str); + pb_check_capacity(pb, len+1); + if (!pb->ok) return false; + + memcpy(pb->current, str, len+1); + pb->current += len+1; + return true; +} + +/** Write uint8_t to the buffer */ +bool pb_u8(PayloadBuilder *pb, uint8_t byte) +{ + pb_check_capacity(pb, 1); + if (!pb->ok) return false; + + *pb->current++ = byte; + return true; +} + +/** Write uint16_t to the buffer. */ +bool pb_u16(PayloadBuilder *pb, uint16_t word) +{ + pb_check_capacity(pb, 2); + if (!pb->ok) return false; + + if (pb->bigendian) { + *pb->current++ = (uint8_t) ((word >> 8) & 0xFF); + *pb->current++ = (uint8_t) (word & 0xFF); + } else { + *pb->current++ = (uint8_t) (word & 0xFF); + *pb->current++ = (uint8_t) ((word >> 8) & 0xFF); + } + return true; +} + +/** Write uint32_t to the buffer. */ +bool pb_u32(PayloadBuilder *pb, uint32_t word) +{ + pb_check_capacity(pb, 4); + if (!pb->ok) return false; + + if (pb->bigendian) { + *pb->current++ = (uint8_t) ((word >> 24) & 0xFF); + *pb->current++ = (uint8_t) ((word >> 16) & 0xFF); + *pb->current++ = (uint8_t) ((word >> 8) & 0xFF); + *pb->current++ = (uint8_t) (word & 0xFF); + } else { + *pb->current++ = (uint8_t) (word & 0xFF); + *pb->current++ = (uint8_t) ((word >> 8) & 0xFF); + *pb->current++ = (uint8_t) ((word >> 16) & 0xFF); + *pb->current++ = (uint8_t) ((word >> 24) & 0xFF); + } + return true; +} + +/** Write int8_t to the buffer. */ +bool pb_i8(PayloadBuilder *pb, int8_t byte) +{ + return pb_u8(pb, ((union conv8){.i8 = byte}).u8); +} + +/** Write int16_t to the buffer. */ +bool pb_i16(PayloadBuilder *pb, int16_t word) +{ + return pb_u16(pb, ((union conv16){.i16 = word}).u16); +} + +/** Write int32_t to the buffer. */ +bool pb_i32(PayloadBuilder *pb, int32_t word) +{ + return pb_u32(pb, ((union conv32){.i32 = word}).u32); +} + +/** Write 4-byte float to the buffer. */ +bool pb_float(PayloadBuilder *pb, float f) +{ + return pb_u32(pb, ((union conv32){.f32 = f}).u32); +} diff --git a/utilities/payload_builder.h b/utilities/payload_builder.h new file mode 100644 index 0000000..e4d6568 --- /dev/null +++ b/utilities/payload_builder.h @@ -0,0 +1,110 @@ +#ifndef PAYLOAD_BUILDER_H +#define PAYLOAD_BUILDER_H + +/** + * PayloadBuilder, part of the TinyFrame utilities collection + * + * (c) Ondřej Hruška, 2014-2017. MIT license. + * + * The builder supports big and little endian which is selected when + * initializing it or by accessing the bigendian struct field. + * + * This module helps you with building payloads (not only for TinyFrame) + * + * The builder performs bounds checking and calls the provided handler when + * the requested write wouldn't fit. Use the handler to realloc / flush the buffer + * or report an error. + */ + +#include +#include +#include +#include "type_coerce.h" + +typedef struct PayloadBuilder_ PayloadBuilder; + +/** + * Full buffer handler. + * + * 'needed' more bytes should be written but the end of the buffer was reached. + * + * Return true if the problem was solved (e.g. buffer was flushed and the + * 'current' pointer moved to the beginning). + * + * If false is returned, the 'ok' flag on the struct is set to false + * and all following writes are discarded. + */ +typedef bool (*pb_full_handler)(PayloadBuilder *pb, uint32_t needed); + +struct PayloadBuilder_ { + uint8_t *start; //!< Pointer to the beginning of the buffer + uint8_t *current; //!< Pointer to the next byte to be read + uint8_t *end; //!< Pointer to the end of the buffer (start + length) + pb_full_handler full_handler; //!< Callback for buffer overrun + bool bigendian; //!< Flag to use big-endian parsing + bool ok; //!< Indicates that all reads were successful +}; + +// --- initializer helper macros --- + +/** Start the builder. */ +#define pb_start_e(buf, capacity, bigendian, full_handler) ((PayloadBuilder){buf, buf, (buf)+(capacity), full_handler, bigendian, 1}) + +/** Start the builder in big-endian mode */ +#define pb_start_be(buf, capacity, full_handler) pb_start_e(buf, capacity, 1, full_handler) + +/** Start the builder in little-endian mode */ +#define pb_start_le(buf, capacity, full_handler) pb_start_e(buf, capacity, 0, full_handler) + +/** Start the parser in little-endian mode (default) */ +#define pb_start(buf, capacity, full_handler) pb_start_le(buf, capacity, full_handler) + +// --- utilities --- + +/** Get already used bytes count */ +#define pb_length(pb) ((pb)->current - (pb)->start) + +/** Reset the current pointer to start */ +#define pb_rewind(pb) do { pb->current = pb->start; } while (0) + + +/** Write from a buffer */ +bool pb_buf(PayloadBuilder *pb, const uint8_t *buf, uint32_t len); + +/** Write a zero terminated string */ +bool pb_string(PayloadBuilder *pb, const char *str); + +/** Write uint8_t to the buffer */ +bool pb_u8(PayloadBuilder *pb, uint8_t byte); + +/** Write boolean to the buffer. */ +static inline bool pb_bool(PayloadBuilder *pb, bool b) +{ + return pb_u8(pb, (uint8_t) b); +} + +/** Write uint16_t to the buffer. */ +bool pb_u16(PayloadBuilder *pb, uint16_t word); + +/** Write uint32_t to the buffer. */ +bool pb_u32(PayloadBuilder *pb, uint32_t word); + +/** Write int8_t to the buffer. */ +bool pb_i8(PayloadBuilder *pb, int8_t byte); + +/** Write char (int8_t) to the buffer. */ +static inline bool pb_char(PayloadBuilder *pb, char c) +{ + return pb_i8(pb, c); +} + +/** Write int16_t to the buffer. */ +bool pb_i16(PayloadBuilder *pb, int16_t word); + +/** Write int32_t to the buffer. */ +bool pb_i32(PayloadBuilder *pb, int32_t word); + +/** Write 4-byte float to the buffer. */ +bool pb_float(PayloadBuilder *pb, float f); + +#endif // PAYLOAD_BUILDER_H diff --git a/utilities/payload_parser.c b/utilities/payload_parser.c new file mode 100644 index 0000000..ca80605 --- /dev/null +++ b/utilities/payload_parser.c @@ -0,0 +1,121 @@ +#include "payload_parser.h" + +#define pp_check_capacity(pp, needed) \ + if ((pp)->current + (needed) > (pp)->end) { \ + if ((pp)->empty_handler == NULL || !(pp)->empty_handler(pp, needed)) {(pp)->ok = 0;} ; \ + } + +void pp_skip(PayloadParser *pp, uint32_t num) +{ + pp->current += num; +} + +uint8_t pp_u8(PayloadParser *pp) +{ + pp_check_capacity(pp, 1); + if (!pp->ok) return 0; + + return *pp->current++; +} + +uint16_t pp_u16(PayloadParser *pp) +{ + pp_check_capacity(pp, 2); + if (!pp->ok) return 0; + + uint16_t x = 0; + + if (pp->bigendian) { + x |= *pp->current++ << 8; + x |= *pp->current++; + } else { + x |= *pp->current++; + x |= *pp->current++ << 8; + } + return x; +} + +uint32_t pp_u32(PayloadParser *pp) +{ + pp_check_capacity(pp, 4); + if (!pp->ok) return 0; + + uint32_t x = 0; + + if (pp->bigendian) { + x |= (uint32_t) (*pp->current++ << 24); + x |= (uint32_t) (*pp->current++ << 16); + x |= (uint32_t) (*pp->current++ << 8); + x |= *pp->current++; + } else { + x |= *pp->current++; + x |= (uint32_t) (*pp->current++ << 8); + x |= (uint32_t) (*pp->current++ << 16); + x |= (uint32_t) (*pp->current++ << 24); + } + return x; +} + +const uint8_t *pp_tail(PayloadParser *pp, uint32_t *length) +{ + int32_t len = (int) (pp->end - pp->current); + if (!pp->ok || len <= 0) { + if (length != NULL) *length = 0; + return NULL; + } + + if (length != NULL) { + *length = (uint32_t) len; + } + + return pp->current; +} + +/** Read int8_t from the payload. */ +int8_t pp_i8(PayloadParser *pp) +{ + return ((union conv8) {.u8 = pp_u8(pp)}).i8; +} + +/** Read int16_t from the payload. */ +int16_t pp_i16(PayloadParser *pp) +{ + return ((union conv16) {.u16 = pp_u16(pp)}).i16; +} + +/** Read int32_t from the payload. */ +int32_t pp_i32(PayloadParser *pp) +{ + return ((union conv32) {.u32 = pp_u32(pp)}).i32; +} + +/** Read 4-byte float from the payload. */ +float pp_float(PayloadParser *pp) +{ + return ((union conv32) {.u32 = pp_u32(pp)}).f32; +} + +/** Read a zstring */ +uint32_t pp_string(PayloadParser *pp, char *buffer, uint32_t maxlen) +{ + pp_check_capacity(pp, 1); + uint32_t len = 0; + while (len < maxlen-1 && pp->current != pp->end) { + char c = *buffer++ = *pp->current++; + if (c == 0) break; + len++; + } + *buffer = 0; + return len; +} + +/** Read a buffer */ +uint32_t pp_buf(PayloadParser *pp, uint8_t *buffer, uint32_t maxlen) +{ + uint32_t len = 0; + while (len < maxlen && pp->current != pp->end) { + *buffer++ = *pp->current++; + len++; + } + return len; +} diff --git a/utilities/payload_parser.h b/utilities/payload_parser.h new file mode 100644 index 0000000..cddab1b --- /dev/null +++ b/utilities/payload_parser.h @@ -0,0 +1,143 @@ +#ifndef PAYLOAD_PARSER_H +#define PAYLOAD_PARSER_H + +/** + * PayloadParser, part of the TinyFrame utilities collection + * + * (c) Ondřej Hruška, 2016-2017. MIT license. + * + * This module helps you with parsing payloads (not only from TinyFrame). + * + * The parser supports big and little-endian which is selected when + * initializing it or by accessing the bigendian struct field. + * + * The parser performs bounds checking and calls the provided handler when + * the requested read doesn't have enough data. Use the callback to take + * appropriate action, e.g. report an error. + * + * If the handler function is not defined, the pb->ok flag is set to false + * (use this to check for success), and further reads won't have any effect + * and always result in 0 or empty array. + */ + +#include +#include +#include +#include "type_coerce.h" + +typedef struct PayloadParser_ PayloadParser; + +/** + * Empty buffer handler. + * + * 'needed' more bytes should be read but the end was reached. + * + * Return true if the problem was solved (e.g. new data loaded into + * the buffer and the 'current' pointer moved to the beginning). + * + * If false is returned, the 'ok' flag on the struct is set to false + * and all following reads will fail / return 0. + */ +typedef bool (*pp_empty_handler)(PayloadParser *pp, uint32_t needed); + +struct PayloadParser_ { + uint8_t *start; //!< Pointer to the beginning of the buffer + uint8_t *current; //!< Pointer to the next byte to be read + uint8_t *end; //!< Pointer to the end of the buffer (start + length) + pp_empty_handler empty_handler; //!< Callback for buffer underrun + bool bigendian; //!< Flag to use big-endian parsing + bool ok; //!< Indicates that all reads were successful +}; + +// --- initializer helper macros --- + +/** Start the parser. */ +#define pp_start_e(buf, length, bigendian, empty_handler) ((PayloadParser){buf, buf, (buf)+(length), empty_handler, bigendian, 1}) + +/** Start the parser in big-endian mode */ +#define pp_start_be(buf, length, empty_handler) pp_start_e(buf, length, 1, empty_handler) + +/** Start the parser in little-endian mode */ +#define pp_start_le(buf, length, empty_handler) pp_start_e(buf, length, 0, empty_handler) + +/** Start the parser in little-endian mode (default) */ +#define pp_start(buf, length, empty_handler) pp_start_le(buf, length, empty_handler) + +// --- utilities --- + +/** Get remaining length */ +#define pp_length(pp) ((pp)->end - (pp)->current) + +/** Reset the current pointer to start */ +#define pp_rewind(pp) do { pp->current = pp->start; } while (0) + + +/** + * @brief Get the remainder of the buffer. + * + * Returns NULL and sets 'length' to 0 if there are no bytes left. + * + * @param pp + * @param length : here the buffer length will be stored. NULL to do not store. + * @return the remaining portion of the input buffer + */ +const uint8_t *pp_tail(PayloadParser *pp, uint32_t *length); + +/** Read uint8_t from the payload. */ +uint8_t pp_u8(PayloadParser *pp); + +/** Read bool from the payload. */ +static inline int8_t pp_bool(PayloadParser *pp) +{ + return pp_u8(pp) != 0; +} + +/** Skip bytes */ +void pp_skip(PayloadParser *pp, uint32_t num); + +/** Read uint16_t from the payload. */ +uint16_t pp_u16(PayloadParser *pp); + +/** Read uint32_t from the payload. */ +uint32_t pp_u32(PayloadParser *pp); + +/** Read int8_t from the payload. */ +int8_t pp_i8(PayloadParser *pp); + +/** Read char (int8_t) from the payload. */ +static inline int8_t pp_char(PayloadParser *pp) +{ + return pp_i8(pp); +} + +/** Read int16_t from the payload. */ +int16_t pp_i16(PayloadParser *pp); + +/** Read int32_t from the payload. */ +int32_t pp_i32(PayloadParser *pp); + +/** Read 4-byte float from the payload. */ +float pp_float(PayloadParser *pp); + +/** + * Parse a zero-terminated string + * + * @param pp - parser + * @param buffer - target buffer + * @param maxlen - buffer size + * @return actual number of bytes, excluding terminator + */ +uint32_t pp_string(PayloadParser *pp, char *buffer, uint32_t maxlen); + +/** + * Parse a buffer + * + * @param pp - parser + * @param buffer - target buffer + * @param maxlen - buffer size + * @return actual number of bytes, excluding terminator + */ +uint32_t pp_buf(PayloadParser *pp, uint8_t *buffer, uint32_t maxlen); + + +#endif // PAYLOAD_PARSER_H diff --git a/utilities/type_coerce.h b/utilities/type_coerce.h new file mode 100644 index 0000000..2e4d0c6 --- /dev/null +++ b/utilities/type_coerce.h @@ -0,0 +1,32 @@ +#ifndef TYPE_COERCE_H +#define TYPE_COERCE_H + +/** + * Structs for conversion between types, + * part of the TinyFrame utilities collection + * + * (c) Ondřej Hruška, 2016-2017. MIT license. + * + * This is a support header file for PayloadParser and PayloadBuilder. + */ + +#include +#include + +union conv8 { + uint8_t u8; + int8_t i8; +}; + +union conv16 { + uint16_t u16; + int16_t i16; +}; + +union conv32 { + uint32_t u32; + int32_t i32; + float f32; +}; + +#endif // TYPE_COERCE_H