diff --git a/inc/pbuf.h b/inc/pbuf.h index 082ffd7..1948131 100644 --- a/inc/pbuf.h +++ b/inc/pbuf.h @@ -190,6 +190,24 @@ } +static inline void pbuf_insert_uint64(pbuf_t* p, uint64_t value) +{ + assert((p->length + sizeof(value)) <= p->capacity); + + *p->payload++ = value; + *p->payload++ = value >> 8; + *p->payload++ = value >> 16; + *p->payload++ = value >> 24; + + *p->payload++ = value >> 32; + *p->payload++ = value >> 40; + *p->payload++ = value >> 48; + *p->payload++ = value >> 56; + + p->length += sizeof(value); +} + + static inline void pbuf_insert_float32(pbuf_t* p, float value) { assert((p->length + sizeof(value)) <= p->capacity); @@ -257,7 +275,8 @@ assert(p->length >= sizeof(value)); - 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); @@ -272,7 +291,32 @@ 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]; + 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 uint64_t pbuf_extract_uint64(pbuf_t* p) +{ + uint64_t value = 0; + + assert(p->length >= sizeof(value)); + + 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) + + ((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);