--- libelwix/src/pack.c 2014/01/29 14:11:45 1.2.16.1 +++ libelwix/src/pack.c 2022/04/13 21:21:08 1.8.66.1 @@ -3,7 +3,7 @@ * by Michael Pounov * * $Author: misho $ -* $Id: pack.c,v 1.2.16.1 2014/01/29 14:11:45 misho Exp $ +* $Id: pack.c,v 1.8.66.1 2022/04/13 21:21:08 misho Exp $ * ************************************************************************** The ELWIX and AITNET software is distributed under the following @@ -12,7 +12,7 @@ terms: All of the documentation and software included in the ELWIX and AITNET Releases is copyrighted by ELWIX - Sofia/Bulgaria -Copyright 2004 - 2014 +Copyright 2004 - 2022 by Michael Pounov . All rights reserved. Redistribution and use in source and binary forms, with or without @@ -100,6 +100,77 @@ rpack_destroy(rpack_t ** __restrict rp) } /* + * rpack_attach() - Attach dynamic allocating buffer at packet + * + * @rp = raw packet; + * @len = allocate bytes + * return: -1 error or 0 ok, should be detached with rpack_detach() + * before call rpack_destroy() after use! + */ +int +rpack_attach(rpack_t * __restrict rp, size_t len) +{ + if (!rp) + return -1; + + rp->r_buf = e_malloc(len); + if (!rp->r_buf) { + RPACK_FREE(rp); + return -1; + } else + rp->r_len = len; + rp->r_next = rp->r_buf; + + return 0; +} + +/* + * rpack_resize() - Resize dynamic allocated buffer at packet + * + * @rp = raw packet + * @newlen = resize buffer to bytes + * return: -1 error or 0 ok, should be detached with rpack_detach() + * before call rpack_destroy() after use! + */ +int +rpack_resize(rpack_t * __restrict rp, size_t newlen) +{ + void *buf = NULL; + + if (!rp) + return -1; + + buf = e_realloc(rp->r_buf, newlen); + if (!buf) + return -1; + else { + rp->r_buf = buf; + rp->r_len = newlen; + } + + if (rp->r_next > (rp->r_buf + rp->r_len)) + rp->r_next = rp->r_buf; + + return 0; +} + +/* + * rpack_detach() - Detach and free dynamic allocated buffer from packet + * + * @rp = raw packet + * return: none + */ +void +rpack_detach(rpack_t * __restrict rp) +{ + if (!rp) + return; + + e_free(rp->r_buf); + RPACK_FREE(rp); +} + +/* * rpack_align_and_reserve() - Align & reserve space * * @rp = raw buffer @@ -154,10 +225,11 @@ rpack_uint8(rpack_t * __restrict rp, uint8_t * __restr * * @rp = raw buffer * @n = set value if !=NULL + * @be = byte order [-1 little endian, 1 big endian and 0 host order] * return: -1 error or get value */ uint16_t -rpack_uint16(rpack_t * __restrict rp, uint16_t * __restrict n) +rpack_uint16(rpack_t * __restrict rp, uint16_t * __restrict n, int be) { uint16_t u; uint8_t *next; @@ -168,7 +240,16 @@ rpack_uint16(rpack_t * __restrict rp, uint16_t * __res if (!(next = rpack_align_and_reserve(rp, sizeof(uint16_t)))) return (uint16_t) -1; - u = EXTRACT_LE_16(next); + if (be < 0) + u = EXTRACT_LE_16(next); + else if (be > 0) + u = EXTRACT_BE_16(next); + else +#if BYTE_ORDER == BIG_ENDIAN + u = EXTRACT_BE_16(next); +#else + u = EXTRACT_LE_16(next); +#endif if (n) RPACK_SET_16(next, n); @@ -181,10 +262,11 @@ rpack_uint16(rpack_t * __restrict rp, uint16_t * __res * * @rp = raw buffer * @n = set value if !=NULL + * @be = byte order [-1 little endian, 1 big endian and 0 host order] * return: -1 error or get value */ uint32_t -rpack_uint24(rpack_t * __restrict rp, uint32_t * __restrict n) +rpack_uint24(rpack_t * __restrict rp, uint32_t * __restrict n, int be) { uint32_t u; uint8_t *next; @@ -195,7 +277,16 @@ rpack_uint24(rpack_t * __restrict rp, uint32_t * __res if (!(next = rpack_align_and_reserve(rp, sizeof(uint32_t)))) return (uint32_t) -1; - u = EXTRACT_LE_24(next); + if (be < 0) + u = EXTRACT_LE_24(next); + else if (be > 0) + u = EXTRACT_BE_24(next); + else +#if BYTE_ORDER == BIG_ENDIAN + u = EXTRACT_BE_24(next); +#else + u = EXTRACT_LE_24(next); +#endif if (n) RPACK_SET_24(next, n); @@ -208,10 +299,11 @@ rpack_uint24(rpack_t * __restrict rp, uint32_t * __res * * @rp = raw buffer * @n = set value if !=NULL + * @be = byte order [-1 little endian, 1 big endian and 0 host order] * return: -1 error or get value */ uint32_t -rpack_uint32(rpack_t * __restrict rp, uint32_t * __restrict n) +rpack_uint32(rpack_t * __restrict rp, uint32_t * __restrict n, int be) { uint32_t u; uint8_t *next; @@ -222,7 +314,16 @@ rpack_uint32(rpack_t * __restrict rp, uint32_t * __res if (!(next = rpack_align_and_reserve(rp, sizeof(uint32_t)))) return (uint32_t) -1; - u = EXTRACT_LE_32(next); + if (be < 0) + u = EXTRACT_LE_32(next); + else if (be > 0) + u = EXTRACT_BE_32(next); + else +#if BYTE_ORDER == BIG_ENDIAN + u = EXTRACT_BE_32(next); +#else + u = EXTRACT_LE_32(next); +#endif if (n) RPACK_SET_32(next, n); @@ -235,10 +336,11 @@ rpack_uint32(rpack_t * __restrict rp, uint32_t * __res * * @rp = raw buffer * @n = set value if !=NULL + * @be = byte order [-1 little endian, 1 big endian and 0 host order] * return: -1 error or get value */ uint64_t -rpack_uint64(rpack_t * __restrict rp, uint64_t * __restrict n) +rpack_uint64(rpack_t * __restrict rp, uint64_t * __restrict n, int be) { uint64_t u; uint8_t *next; @@ -249,10 +351,274 @@ rpack_uint64(rpack_t * __restrict rp, uint64_t * __res if (!(next = rpack_align_and_reserve(rp, sizeof(uint64_t)))) return (uint64_t) -1; - u = EXTRACT_LE_64(next); + if (be < 0) + u = EXTRACT_LE_64(next); + else if (be > 0) + u = EXTRACT_BE_64(next); + else +#if BYTE_ORDER == BIG_ENDIAN + u = EXTRACT_BE_64(next); +#else + u = EXTRACT_LE_64(next); +#endif if (n) RPACK_SET_64(next, n); rp->r_next = next + sizeof(uint64_t); return u; +} + +/* + * rpack_data() - Pack/Unpack align data + * + * @rp = raw buffer + * @dat = data + * @datlen = data length + * return: NULL error or != NULL get data, must be e_free() after use! + */ +void * +rpack_data(rpack_t * __restrict rp, void * __restrict dat, size_t datlen) +{ + void *buf = NULL; + uint8_t *next; + + if (!datlen || !RPACK_SANITY(rp)) + return NULL; + buf = e_malloc(datlen); + if (!buf) + return NULL; + /* No space left */ + if (!(next = rpack_align_and_reserve(rp, datlen))) { + e_free(buf); + return NULL; + } + + memcpy(buf, next, datlen); + if (dat) + memcpy(next, dat, datlen); + + rp->r_next = next + datlen; + return buf; +} + +/* + * rpack_rdata() - Pack/Unpack raw data + * + * @rp = raw buffer + * @dat = data + * @datlen = data length + * return: NULL error or != NULL get data, must be e_free() after use! + */ +void * +rpack_rdata(rpack_t * __restrict rp, void * __restrict dat, size_t datlen) +{ + void *buf = NULL; + + if (!datlen || !RPACK_SANITY(rp)) + return NULL; + buf = e_malloc(datlen); + if (!buf) + return NULL; + /* No space left */ + if (datlen + rp->r_next - rp->r_buf > rp->r_len) { + e_free(buf); + return NULL; + } + + memcpy(buf, rp->r_next, datlen); + if (dat) + memcpy(rp->r_next, dat, datlen); + + rp->r_next += datlen; + return buf; +} + +/* + * rpack_ruint16() - Pack/Unpack raw 16bit value + * + * @rp = raw buffer + * @n = set value if !=NULL + * @be = byte order [-1 little endian, 1 big endian and 0 host order] + * return: -1 error or get value + */ +uint16_t +rpack_ruint16(rpack_t * __restrict rp, uint16_t * __restrict n, int be) +{ + uint16_t u; + + if (!RPACK_SANITY(rp)) + return (uint16_t) -1; + /* No space left */ + if (sizeof(uint16_t) + rp->r_next - rp->r_buf > rp->r_len) + return (uint16_t) -1; + + if (be < 0) + u = EXTRACT_LE_16(rp->r_next); + else if (be > 0) + u = EXTRACT_BE_16(rp->r_next); + else +#if BYTE_ORDER == BIG_ENDIAN + u = EXTRACT_BE_16(rp->r_next); +#else + u = EXTRACT_LE_16(rp->r_next); +#endif + if (n) + RPACK_SET_16(rp->r_next, n); + + rp->r_next += sizeof(uint16_t); + return u; +} + +/* + * rpack_ruint24() - Pack/Unpack raw 24bit value + * + * @rp = raw buffer + * @n = set value if !=NULL + * @be = byte order [-1 little endian, 1 big endian and 0 host order] + * return: -1 error or get value + */ +uint32_t +rpack_ruint24(rpack_t * __restrict rp, uint32_t * __restrict n, int be) +{ + uint32_t u; + + if (!RPACK_SANITY(rp)) + return (uint32_t) -1; + /* No space left */ + if (sizeof(uint32_t) + rp->r_next - rp->r_buf > rp->r_len) + return (uint32_t) -1; + + if (be < 0) + u = EXTRACT_LE_24(rp->r_next); + else if (be > 0) + u = EXTRACT_BE_24(rp->r_next); + else +#if BYTE_ORDER == BIG_ENDIAN + u = EXTRACT_BE_24(rp->r_next); +#else + u = EXTRACT_LE_24(rp->r_next); +#endif + if (n) + RPACK_SET_24(rp->r_next, n); + + rp->r_next += sizeof(uint32_t); + return u; +} + +/* + * rpack_ruint32() - Pack/Unpack raw 32bit value + * + * @rp = raw buffer + * @n = set value if !=NULL + * @be = byte order [-1 little endian, 1 big endian and 0 host order] + * return: -1 error or get value + */ +uint32_t +rpack_ruint32(rpack_t * __restrict rp, uint32_t * __restrict n, int be) +{ + uint32_t u; + + if (!RPACK_SANITY(rp)) + return (uint32_t) -1; + /* No space left */ + if (sizeof(uint32_t) + rp->r_next - rp->r_buf > rp->r_len) + return (uint32_t) -1; + + if (be < 0) + u = EXTRACT_LE_32(rp->r_next); + else if (be > 0) + u = EXTRACT_BE_32(rp->r_next); + else +#if BYTE_ORDER == BIG_ENDIAN + u = EXTRACT_BE_32(rp->r_next); +#else + u = EXTRACT_LE_32(rp->r_next); +#endif + if (n) + RPACK_SET_32(rp->r_next, n); + + rp->r_next += sizeof(uint32_t); + return u; +} + +/* + * rpack_ruint64() - Pack/Unpack raw 64bit value + * + * @rp = raw buffer + * @n = set value if !=NULL + * @be = byte order [-1 little endian, 1 big endian and 0 host order] + * return: -1 error or get value + */ +uint64_t +rpack_ruint64(rpack_t * __restrict rp, uint64_t * __restrict n, int be) +{ + uint64_t u; + + if (!RPACK_SANITY(rp)) + return (uint64_t) -1; + /* No space left */ + if (sizeof(uint64_t) + rp->r_next - rp->r_buf > rp->r_len) + return (uint64_t) -1; + + if (be < 0) + u = EXTRACT_LE_64(rp->r_next); + else if (be > 0) + u = EXTRACT_BE_64(rp->r_next); + else +#if BYTE_ORDER == BIG_ENDIAN + u = EXTRACT_BE_64(rp->r_next); +#else + u = EXTRACT_LE_64(rp->r_next); +#endif + if (n) + RPACK_SET_64(rp->r_next, n); + + rp->r_next += sizeof(uint64_t); + return u; +} + +/* + * rpack_next() - Get and set current position + * + * @rp = raw packet + * @after_len = move aligned current position after length + * return: NULL error or current position + */ +uint8_t * +rpack_next(rpack_t * __restrict rp, size_t after_len) +{ + uint8_t *next = NULL; + + if (!RPACK_SANITY(rp)) + return NULL; + /* No space left */ + if (!(next = rpack_align_and_reserve(rp, after_len))) + return NULL; + + rp->r_next = next + after_len; + return next; +} + +/* + * rpack_rnext() - Get and set raw current position + * + * @rp = raw packet + * @after_len = !=0 move current position after length + * return: NULL error or raw current position + */ +uint8_t * +rpack_rnext(rpack_t * __restrict rp, size_t after_len) +{ + uint8_t *next = NULL; + + if (!RPACK_SANITY(rp)) + return NULL; + /* No space left */ + if (after_len + rp->r_next - rp->r_buf > rp->r_len) + return NULL; + + next = rp->r_next; + + rp->r_next += after_len; + return next; }