diff --git a/inc/pbuf.h b/inc/pbuf.h index a89ff4a..bb99620 100644 --- a/inc/pbuf.h +++ b/inc/pbuf.h @@ -142,6 +142,185 @@ } + +static inline void pbuf_insert_uint8(pbuf_t* p, uint8_t value) +{ + assert((p->length + sizeof(value)) <= p->capacity); + + *(p->payload + p->length) = value; + + p->length += sizeof(value); +} + + +static inline void pbuf_insert_uint16(pbuf_t* p, uint16_t value) +{ + assert((p->length + sizeof(value)) <= p->capacity); + + *(p->payload + p->length + 0) = value; + *(p->payload + p->length + 1) = value >> 8; + + p->length += sizeof(value); +} + + +static inline void pbuf_insert_uint32(pbuf_t* p, uint32_t value) +{ + assert((p->length + sizeof(value)) <= p->capacity); + + *(p->payload + p->length + 0) = value; + *(p->payload + p->length + 1) = (value >> 8); + *(p->payload + p->length + 2) = (value >> 16); + *(p->payload + p->length + 3) = (value >> 24); + + p->length += sizeof(value); +} + + +static inline void pbuf_insert_float32(pbuf_t* p, float value) +{ + assert((p->length + sizeof(value)) <= p->capacity); + assert(sizeof(float) == sizeof(uint32_t)); + + const uint32_t* const value_ptr = (uint32_t*)&value; + + *(p->payload + p->length + 0) = *value_ptr; + *(p->payload + p->length + 1) = (*value_ptr >> 8); + *(p->payload + p->length + 2) = (*value_ptr >> 16); + *(p->payload + p->length + 3) = (*value_ptr >> 24); + + p->length += sizeof(value); +} + + +static inline void pbuf_insert_data(pbuf_t* p, const void* data, size_t size) +{ + assert((p->length + size) <= p->capacity); + memcpy(p->payload + p->length, data, size); + p->length += size; +} + + +static inline void pbuf_insert_str(pbuf_t* p, const char* str) +{ + uint8_t* ptr = p->payload + p->length; + while(*str && (p->length < p->capacity)) + { + *ptr++ = *str++; + p->length++; + } +} + + +static inline void pbuf_insert_nstr(pbuf_t* p, const char* str, size_t count) +{ + uint8_t* ptr = p->payload + p->length; + while(*str && (p->length < p->capacity) && count) + { + *ptr++ = *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)); + + value = ((uint16_t)p->payload[1] << 8) + p->payload[0]; + + p->payload += sizeof(value); + p->length -= sizeof(value); + + return value; +} + + +static inline uint32_t pbuf_extract_uint32(pbuf_t* p) +{ + uint32_t value = 0; + + assert(p->length >= sizeof(value)); + + 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); + + return value; +} + + +static inline float pbuf_extract_float32(pbuf_t* p) +{ + assert(p->length >= sizeof(float)); + assert(sizeof(float) == sizeof(uint32_t)); + + 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; + + p->payload += sizeof(float); + p->length -= sizeof(float); + + return *value_ptr; +} + + +static inline void pbuf_extract_data(pbuf_t* p, void* data, size_t size) +{ + assert(p->length >= size); + + memcpy(data, p->payload, size); + p->payload += size; + p->length -= size; +} + + + +static inline void pbuf_extract_str(pbuf_t* p, char* str, size_t max_count) +{ + while(*p->payload && p->length && max_count) + { + *str++ = *p->payload++; + p->length--; + max_count--; + } + + *str = 0; +} + + +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; +} + + + #ifdef __cplusplus } #endif