version 1.1.2.1, 2013/06/28 08:39:41
|
version 1.1.2.5, 2013/06/30 22:06:25
|
Line 1
|
Line 1
|
#include "global.h" |
#include "global.h" |
|
|
|
|
|
static inline uint8_t * |
|
rpack_next_boundary(uint8_t * __restrict buf, uint8_t * __restrict p, size_t align) |
|
{ |
|
size_t misa = (size_t) (p - buf) % align; |
|
|
|
if (!misa) |
|
return p; |
|
|
|
return p + (align - misa); |
|
} |
|
|
|
|
|
/* |
|
* rpack_create() - Allocate & init raw packet structure |
|
* |
|
* @buf = buffer |
|
* @buflen = length of buffer |
|
* return: NULL error or !=NULL raw packet, should be freed by rpack_destroy() |
|
*/ |
|
rpack_t * |
|
rpack_create(void * __restrict buf, size_t buflen) |
|
{ |
|
rpack_t *rp = NULL; |
|
|
|
rp = e_malloc(sizeof(rpack_t)); |
|
if (!rp) { |
|
LOGERR; |
|
return NULL; |
|
} |
|
|
|
RPACK_INIT(rp, buf, buflen); |
|
return rp; |
|
} |
|
|
|
/* |
|
* rpack_destroy() - Release & free raw packet structure |
|
* |
|
* @rp = raw packet |
|
* return: none |
|
*/ |
|
void |
|
rpack_destroy(rpack_t ** __restrict rp) |
|
{ |
|
if (!rp) |
|
return; |
|
|
|
if (*rp) { |
|
RPACK_FREE(*rp); |
|
e_free(*rp); |
|
*rp = NULL; |
|
} |
|
} |
|
|
|
/* |
|
* rpack_align_and_reserve() - Align & reserve space |
|
* |
|
* @rp = raw buffer |
|
* @siz = need size |
|
* return: NULL error or not enough space, !=NULL next position |
|
*/ |
|
uint8_t * |
|
rpack_align_and_reserve(rpack_t * __restrict rp, size_t siz) |
|
{ |
|
uint8_t *n; |
|
|
|
if (!RPACK_SANITY(rp)) |
|
return NULL; |
|
|
|
n = rpack_next_boundary(rp->r_buf, rp->r_next, siz); |
|
/* too little space for siz */ |
|
if (n - rp->r_buf + siz > rp->r_len) |
|
return NULL; |
|
|
|
return n; |
|
} |
|
|
|
|
|
/* |
|
* rpack_uint8() - Pack/Unpack 8bit value |
|
* |
|
* @rp = raw buffer |
|
* @n = set value if !=NULL |
|
* return: -1 error or get value |
|
*/ |
|
uint8_t |
|
rpack_uint8(rpack_t * __restrict rp, uint8_t * __restrict n) |
|
{ |
|
uint8_t u; |
|
|
|
if (!RPACK_SANITY(rp)) |
|
return (uint8_t) -1; |
|
/* No space left */ |
|
if ((size_t) (rp->r_next - rp->r_buf) >= rp->r_len) |
|
return (uint8_t) -1; |
|
|
|
u = *rp->r_next; |
|
if (n) |
|
*rp->r_next = *n; |
|
|
|
rp->r_next++; |
|
return u; |
|
} |
|
|
|
/* |
|
* rpack_uint16() - Pack/Unpack 16bit value |
|
* |
|
* @rp = raw buffer |
|
* @n = set value if !=NULL |
|
* return: -1 error or get value |
|
*/ |
|
uint16_t |
|
rpack_uint16(rpack_t * __restrict rp, uint16_t * __restrict n) |
|
{ |
|
uint16_t u; |
|
uint8_t *next; |
|
|
|
if (!RPACK_SANITY(rp)) |
|
return (uint16_t) -1; |
|
/* No space left */ |
|
if (!(next = rpack_align_and_reserve(rp, sizeof(uint16_t)))) |
|
return (uint16_t) -1; |
|
|
|
u = EXTRACT_LE_16(next); |
|
if (n) |
|
RPACK_SET_16(next, n); |
|
|
|
rp->r_next = next + sizeof(uint16_t); |
|
return u; |
|
} |
|
|
|
/* |
|
* rpack_uint24() - Pack/Unpack 24bit value |
|
* |
|
* @rp = raw buffer |
|
* @n = set value if !=NULL |
|
* return: -1 error or get value |
|
*/ |
|
uint32_t |
|
rpack_uint24(rpack_t * __restrict rp, uint32_t * __restrict n) |
|
{ |
|
uint32_t u; |
|
uint8_t *next; |
|
|
|
if (!RPACK_SANITY(rp)) |
|
return (uint32_t) -1; |
|
/* No space left */ |
|
if (!(next = rpack_align_and_reserve(rp, sizeof(uint32_t)))) |
|
return (uint32_t) -1; |
|
|
|
u = EXTRACT_LE_24(next); |
|
if (n) |
|
RPACK_SET_24(next, n); |
|
|
|
rp->r_next = next + sizeof(uint32_t); |
|
return u; |
|
} |
|
|
|
/* |
|
* rpack_uint32() - Pack/Unpack 32bit value |
|
* |
|
* @rp = raw buffer |
|
* @n = set value if !=NULL |
|
* return: -1 error or get value |
|
*/ |
|
uint32_t |
|
rpack_uint32(rpack_t * __restrict rp, uint32_t * __restrict n) |
|
{ |
|
uint32_t u; |
|
uint8_t *next; |
|
|
|
if (!RPACK_SANITY(rp)) |
|
return (uint32_t) -1; |
|
/* No space left */ |
|
if (!(next = rpack_align_and_reserve(rp, sizeof(uint32_t)))) |
|
return (uint32_t) -1; |
|
|
|
u = EXTRACT_LE_32(next); |
|
if (n) |
|
RPACK_SET_32(next, n); |
|
|
|
rp->r_next = next + sizeof(uint32_t); |
|
return u; |
|
} |
|
|
|
/* |
|
* rpack_uint64() - Pack/Unpack 64bit value |
|
* |
|
* @rp = raw buffer |
|
* @n = set value if !=NULL |
|
* return: -1 error or get value |
|
*/ |
|
uint64_t |
|
rpack_uint64(rpack_t * __restrict rp, uint64_t * __restrict n) |
|
{ |
|
uint64_t u; |
|
uint8_t *next; |
|
|
|
if (!RPACK_SANITY(rp)) |
|
return (uint64_t) -1; |
|
/* No space left */ |
|
if (!(next = rpack_align_and_reserve(rp, sizeof(uint64_t)))) |
|
return (uint64_t) -1; |
|
|
|
u = EXTRACT_LE_64(next); |
|
if (n) |
|
RPACK_SET_64(next, n); |
|
|
|
rp->r_next = next + sizeof(uint64_t); |
|
return u; |
|
} |