diff --git a/inc/pbuf.h b/inc/pbuf.h index 1948131..423d1d9 100644 --- a/inc/pbuf.h +++ b/inc/pbuf.h @@ -4,6 +4,7 @@ #include #include +#include #include #include @@ -156,30 +157,37 @@ -static inline void pbuf_insert_uint8(pbuf_t* p, uint8_t value) +static inline bool pbuf_insert_uint8(pbuf_t* p, uint8_t value) { - assert((p->length + sizeof(value)) <= p->capacity); + if ((p->length + sizeof(value)) > p->capacity) + return false; *p->payload++ = value; p->length += sizeof(value); + + return true; } -static inline void pbuf_insert_uint16(pbuf_t* p, uint16_t value) +static inline bool pbuf_insert_uint16(pbuf_t* p, uint16_t value) { - assert((p->length + sizeof(value)) <= p->capacity); + if ((p->length + sizeof(value)) > p->capacity) + return false; *p->payload++ = value; *p->payload++ = value >> 8; p->length += sizeof(value); + + return true; } -static inline void pbuf_insert_uint32(pbuf_t* p, uint32_t value) +static inline bool pbuf_insert_uint32(pbuf_t* p, uint32_t value) { - assert((p->length + sizeof(value)) <= p->capacity); + if ((p->length + sizeof(value)) > p->capacity) + return false; *p->payload++ = value; *p->payload++ = value >> 8; @@ -187,12 +195,15 @@ *p->payload++ = value >> 24; p->length += sizeof(value); + + return true; } -static inline void pbuf_insert_uint64(pbuf_t* p, uint64_t value) +static inline bool pbuf_insert_uint64(pbuf_t* p, uint64_t value) { - assert((p->length + sizeof(value)) <= p->capacity); + if ((p->length + sizeof(value)) > p->capacity) + return false; *p->payload++ = value; *p->payload++ = value >> 8; @@ -205,111 +216,101 @@ *p->payload++ = value >> 56; p->length += sizeof(value); + + return true; } -static inline void pbuf_insert_float32(pbuf_t* p, float value) +static inline bool pbuf_insert_float(pbuf_t* p, float value) { - assert((p->length + sizeof(value)) <= p->capacity); - assert(sizeof(float) == sizeof(uint32_t)); + if ((p->length + sizeof(value)) > p->capacity) + return false; - const uint32_t* const value_ptr = (uint32_t*)&value; - - *p->payload++ = *value_ptr; - *p->payload++ = *value_ptr >> 8; - *p->payload++ = *value_ptr >> 16; - *p->payload++ = *value_ptr >> 24; - + memcpy(p->payload, &value, sizeof(value)); + p->payload += sizeof(value); p->length += sizeof(value); + + return true; } -static inline void pbuf_insert_data(pbuf_t* p, const void* data, size_t size) +static inline bool pbuf_insert_double(pbuf_t* p, double value) { - assert((p->length + size) <= p->capacity); + if ((p->length + sizeof(value)) > p->capacity) + return false; + + memcpy(p->payload, &value, sizeof(value)); + p->payload += sizeof(value); + p->length += sizeof(value); + + return true; +} + + +static inline bool pbuf_insert_data(pbuf_t* p, const void* data, size_t size) +{ + if ((p->length + size) > p->capacity) + return false; memcpy(p->payload, data, size); p->payload += size; p->length += size; + + return true; } -static inline void pbuf_insert_str(pbuf_t* p, const char* str) + +static inline bool pbuf_extract_uint8(pbuf_t* p, uint8_t* value) { - while(*str && (p->length < p->capacity)) - { - *p->payload++ = *str++; - p->length++; - } + if (p->length < sizeof(uint8_t)) + return false; + + *value = *p->payload++; + + p->length -= sizeof(uint8_t); + + return true; } -static inline void pbuf_insert_nstr(pbuf_t* p, const char* str, size_t count) +static inline bool pbuf_extract_uint16(pbuf_t* p, uint16_t* value) { - while(*str && (p->length < p->capacity) && count) - { - *p->payload++ = *str++; - p->length++; - count--; - } -} - - -static inline uint8_t pbuf_extract_uint8(pbuf_t* p) -{ - uint8_t value = 0; - - assert(p->length >= sizeof(value)); - - value = *p->payload++; - - p->length -= sizeof(value); - - return value; -} - - -static inline uint16_t pbuf_extract_uint16(pbuf_t* p) -{ - uint16_t value = 0; - - assert(p->length >= sizeof(value)); + if (p->length < sizeof(uint16_t)) + return false; - value = ((uint16_t)p->payload[1] << 8) + - p->payload[0]; + *value = ((uint16_t)p->payload[1] << 8) + p->payload[0]; - p->payload += sizeof(value); - p->length -= sizeof(value); + p->payload += sizeof(uint16_t); + p->length -= sizeof(uint16_t); - return value; + return true; } -static inline uint32_t pbuf_extract_uint32(pbuf_t* p) +static inline bool pbuf_extract_uint32(pbuf_t* p, uint32_t* value) { - uint32_t value = 0; + if (p->length < sizeof(uint32_t)) + return false; - assert(p->length >= sizeof(value)); - - value = ((uint32_t)p->payload[3] << 24) + + *value = ((uint32_t)p->payload[3] << 24) + ((uint32_t)p->payload[2] << 16) + ((uint32_t)p->payload[1] << 8) + p->payload[0]; - p->payload += sizeof(value); - p->length -= sizeof(value); + p->payload += sizeof(uint32_t); + p->length -= sizeof(uint32_t); - return value; + return true; } -static inline uint64_t pbuf_extract_uint64(pbuf_t* p) +static inline bool pbuf_extract_uint64(pbuf_t* p, uint64_t* value) { - uint64_t value = 0; + if (p->length < sizeof(uint64_t)) + return false; - assert(p->length >= sizeof(value)); - - value = ((uint64_t)p->payload[7] << 56) + + *value = ((uint64_t)p->payload[7] << 56) + ((uint64_t)p->payload[6] << 48) + ((uint64_t)p->payload[5] << 40) + ((uint64_t)p->payload[4] << 32) + @@ -318,62 +319,50 @@ ((uint32_t)p->payload[1] << 8) + p->payload[0]; - p->payload += sizeof(value); - p->length -= sizeof(value); + p->payload += sizeof(uint64_t); + p->length -= sizeof(uint64_t); - return value; + return true; } -static inline float pbuf_extract_float32(pbuf_t* p) +static inline bool pbuf_extract_float32(pbuf_t* p, float* value) { - assert(p->length >= sizeof(float)); - assert(sizeof(float) == sizeof(uint32_t)); + if (p->length < sizeof(float)) + return false; - const uint32_t value_u32 = ((uint32_t)p->payload[3] << 24) + ((uint32_t)p->payload[2] << 16) + ((uint32_t)p->payload[1] << 8) + p->payload[0]; - const float* const value_ptr = (float*)&value_u32; + memcpy(value, p->payload, sizeof(float)); p->payload += sizeof(float); p->length -= sizeof(float); - return *value_ptr; + return true; } -static inline void pbuf_extract_data(pbuf_t* p, void* data, size_t size) +static inline bool pbuf_extract_data(pbuf_t* p, void* data, size_t size) { - assert(p->length >= size); + if (p->length < size) + return false; memcpy(data, p->payload, size); p->payload += size; p->length -= size; + + return true; } - -static inline void pbuf_extract_str(pbuf_t* p, char* str, size_t max_count) +static inline bool pbuf_extract_pointer(pbuf_t* p, void** data, size_t size) { - while(*p->payload && p->length && max_count) - { - *str++ = *p->payload++; - p->length--; - max_count--; - } - - *str = 0; -} + if (p->length < size) + return false; + *data = p->payload; + p->payload += size; + p->length -= size; -static inline void pbuf_extract_nstr(pbuf_t* p, char* str, size_t count) -{ - while(*p->payload && p->length && count) - { - *str++ = *p->payload++; - p->length--; - count--; - } - - *str = 0; + return true; }