--- libelwix/src/pack.c	2013/06/28 08:39:41	1.1
+++ libelwix/src/pack.c	2013/06/30 22:10:27	1.2
@@ -0,0 +1,213 @@
+#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;
+}