--- libelwix/src/pack.c 2013/06/28 15:30:46 1.1.2.3 +++ libelwix/src/pack.c 2014/02/11 00:43:41 1.5 @@ -1,3 +1,48 @@ +/************************************************************************* +* (C) 2013 AITNET ltd - Sofia/Bulgaria - +* by Michael Pounov +* +* $Author: misho $ +* $Id: pack.c,v 1.5 2014/02/11 00:43:41 misho Exp $ +* +************************************************************************** +The ELWIX and AITNET software is distributed under the following +terms: + +All of the documentation and software included in the ELWIX and AITNET +Releases is copyrighted by ELWIX - Sofia/Bulgaria + +Copyright 2004 - 2014 + by Michael Pounov . All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions +are met: +1. Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. +3. All advertising materials mentioning features or use of this software + must display the following acknowledgement: +This product includes software developed by Michael Pounov +ELWIX - Embedded LightWeight unIX and its contributors. +4. Neither the name of AITNET nor the names of its contributors + may be used to endorse or promote products derived from this software + without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY AITNET AND CONTRIBUTORS ``AS IS'' AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS +OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY +OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF +SUCH DAMAGE. +*/ #include "global.h" @@ -109,10 +154,11 @@ rpack_uint8(rpack_t * __restrict rp, uint8_t * __restr * * @rp = raw buffer * @n = set value if !=NULL + * @be = extract in big-endian * 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; @@ -123,10 +169,159 @@ 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); + u = be ? EXTRACT_BE_16(next) : EXTRACT_LE_16(next); if (n) - EXTRACT_SET_16(next, *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 + * @be = extract in big-endian + * return: -1 error or get value + */ +uint32_t +rpack_uint24(rpack_t * __restrict rp, uint32_t * __restrict n, int be) +{ + 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 = be ? EXTRACT_BE_24(next) : 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 + * @be = extract in big-endian + * return: -1 error or get value + */ +uint32_t +rpack_uint32(rpack_t * __restrict rp, uint32_t * __restrict n, int be) +{ + 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 = be ? EXTRACT_BE_32(next) : 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 + * @be = extract in big-endian + * return: -1 error or get value + */ +uint64_t +rpack_uint64(rpack_t * __restrict rp, uint64_t * __restrict n, int be) +{ + 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 = be ? EXTRACT_BE_64(next) : EXTRACT_LE_64(next); + 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_raw() - 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_raw(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; }