#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;
}