Annotation of libelwix/src/pack.c, revision 1.5

1.3       misho       1: /*************************************************************************
                      2: * (C) 2013 AITNET ltd - Sofia/Bulgaria - <misho@aitnet.org>
                      3: *  by Michael Pounov <misho@elwix.org>
                      4: *
                      5: * $Author: misho $
1.5     ! misho       6: * $Id: pack.c,v 1.4.2.2 2014/02/11 00:41:12 misho Exp $
1.3       misho       7: *
                      8: **************************************************************************
                      9: The ELWIX and AITNET software is distributed under the following
                     10: terms:
                     11: 
                     12: All of the documentation and software included in the ELWIX and AITNET
                     13: Releases is copyrighted by ELWIX - Sofia/Bulgaria <info@elwix.org>
                     14: 
                     15: Copyright 2004 - 2014
                     16:        by Michael Pounov <misho@elwix.org>.  All rights reserved.
                     17: 
                     18: Redistribution and use in source and binary forms, with or without
                     19: modification, are permitted provided that the following conditions
                     20: are met:
                     21: 1. Redistributions of source code must retain the above copyright
                     22:    notice, this list of conditions and the following disclaimer.
                     23: 2. Redistributions in binary form must reproduce the above copyright
                     24:    notice, this list of conditions and the following disclaimer in the
                     25:    documentation and/or other materials provided with the distribution.
                     26: 3. All advertising materials mentioning features or use of this software
                     27:    must display the following acknowledgement:
                     28: This product includes software developed by Michael Pounov <misho@elwix.org>
                     29: ELWIX - Embedded LightWeight unIX and its contributors.
                     30: 4. Neither the name of AITNET nor the names of its contributors
                     31:    may be used to endorse or promote products derived from this software
                     32:    without specific prior written permission.
                     33: 
                     34: THIS SOFTWARE IS PROVIDED BY AITNET AND CONTRIBUTORS ``AS IS'' AND
                     35: ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
                     36: IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
                     37: ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
                     38: FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
                     39: DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
                     40: OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
                     41: HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
                     42: LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
                     43: OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
                     44: SUCH DAMAGE.
                     45: */
1.2       misho      46: #include "global.h"
                     47: 
                     48: 
                     49: static inline uint8_t *
                     50: rpack_next_boundary(uint8_t * __restrict buf, uint8_t * __restrict p, size_t align)
                     51: {
                     52:        size_t misa = (size_t) (p - buf) % align;
                     53: 
                     54:        if (!misa)
                     55:                return p;
                     56: 
                     57:        return p + (align - misa);
                     58: }
                     59: 
                     60: 
                     61: /*
                     62:  * rpack_create() - Allocate & init raw packet structure
                     63:  *
                     64:  * @buf = buffer
                     65:  * @buflen = length of buffer
                     66:  * return: NULL error or !=NULL raw packet, should be freed by rpack_destroy()
                     67:  */
                     68: rpack_t *
                     69: rpack_create(void * __restrict buf, size_t buflen)
                     70: {
                     71:        rpack_t *rp = NULL;
                     72: 
                     73:        rp = e_malloc(sizeof(rpack_t));
                     74:        if (!rp) {
                     75:                LOGERR;
                     76:                return NULL;
                     77:        }
                     78: 
                     79:        RPACK_INIT(rp, buf, buflen);
                     80:        return rp;
                     81: }
                     82: 
                     83: /*
                     84:  * rpack_destroy() - Release & free raw packet structure
                     85:  *
                     86:  * @rp = raw packet
                     87:  * return: none
                     88:  */
                     89: void
                     90: rpack_destroy(rpack_t ** __restrict rp)
                     91: {
                     92:        if (!rp)
                     93:                return;
                     94: 
                     95:        if (*rp) {
                     96:                RPACK_FREE(*rp);
                     97:                e_free(*rp);
                     98:                *rp = NULL;
                     99:        }
                    100: }
                    101: 
                    102: /*
                    103:  * rpack_align_and_reserve() - Align & reserve space
                    104:  *
                    105:  * @rp = raw buffer
                    106:  * @siz = need size
                    107:  * return: NULL error or not enough space, !=NULL next position
                    108:  */
                    109: uint8_t *
                    110: rpack_align_and_reserve(rpack_t * __restrict rp, size_t siz)
                    111: {
                    112:        uint8_t *n;
                    113: 
                    114:        if (!RPACK_SANITY(rp))
                    115:                return NULL;
                    116: 
                    117:        n = rpack_next_boundary(rp->r_buf, rp->r_next, siz);
                    118:        /* too little space for siz */
                    119:        if (n - rp->r_buf + siz > rp->r_len)
                    120:                return NULL;
                    121: 
                    122:        return n;
                    123: }
                    124: 
                    125: 
                    126: /*
                    127:  * rpack_uint8() - Pack/Unpack 8bit value
                    128:  *
                    129:  * @rp = raw buffer
                    130:  * @n = set value if !=NULL
                    131:  * return: -1 error or get value
                    132:  */
                    133: uint8_t
                    134: rpack_uint8(rpack_t * __restrict rp, uint8_t * __restrict n)
                    135: {
                    136:        uint8_t u;
                    137: 
                    138:        if (!RPACK_SANITY(rp))
                    139:                return (uint8_t) -1;
                    140:        /* No space left */
                    141:        if ((size_t) (rp->r_next - rp->r_buf) >= rp->r_len)
                    142:                return (uint8_t) -1;
                    143: 
                    144:        u = *rp->r_next;
                    145:        if (n)
                    146:                *rp->r_next = *n;
                    147: 
                    148:        rp->r_next++;
                    149:        return u;
                    150: }
                    151: 
                    152: /*
                    153:  * rpack_uint16() - Pack/Unpack 16bit value
                    154:  *
                    155:  * @rp = raw buffer
                    156:  * @n = set value if !=NULL
1.4       misho     157:  * @be = extract in big-endian
1.2       misho     158:  * return: -1 error or get value
                    159:  */
                    160: uint16_t
1.4       misho     161: rpack_uint16(rpack_t * __restrict rp, uint16_t * __restrict n, int be)
1.2       misho     162: {
                    163:        uint16_t u;
                    164:        uint8_t *next;
                    165: 
                    166:        if (!RPACK_SANITY(rp))
                    167:                return (uint16_t) -1;
                    168:        /* No space left */
                    169:        if (!(next = rpack_align_and_reserve(rp, sizeof(uint16_t))))
                    170:                return (uint16_t) -1;
                    171: 
1.4       misho     172:        u = be ? EXTRACT_BE_16(next) : EXTRACT_LE_16(next);
1.2       misho     173:        if (n)
                    174:                RPACK_SET_16(next, n);
                    175: 
                    176:        rp->r_next = next + sizeof(uint16_t);
                    177:        return u;
                    178: }
                    179: 
                    180: /*
                    181:  * rpack_uint24() - Pack/Unpack 24bit value
                    182:  *
                    183:  * @rp = raw buffer
                    184:  * @n = set value if !=NULL
1.4       misho     185:  * @be = extract in big-endian
1.2       misho     186:  * return: -1 error or get value
                    187:  */
                    188: uint32_t
1.4       misho     189: rpack_uint24(rpack_t * __restrict rp, uint32_t * __restrict n, int be)
1.2       misho     190: {
                    191:        uint32_t u;
                    192:        uint8_t *next;
                    193: 
                    194:        if (!RPACK_SANITY(rp))
                    195:                return (uint32_t) -1;
                    196:        /* No space left */
                    197:        if (!(next = rpack_align_and_reserve(rp, sizeof(uint32_t))))
                    198:                return (uint32_t) -1;
                    199: 
1.4       misho     200:        u = be ? EXTRACT_BE_24(next) : EXTRACT_LE_24(next);
1.2       misho     201:        if (n)
                    202:                RPACK_SET_24(next, n);
                    203: 
                    204:        rp->r_next = next + sizeof(uint32_t);
                    205:        return u;
                    206: }
                    207: 
                    208: /*
                    209:  * rpack_uint32() - Pack/Unpack 32bit value
                    210:  *
                    211:  * @rp = raw buffer
                    212:  * @n = set value if !=NULL
1.4       misho     213:  * @be = extract in big-endian
1.2       misho     214:  * return: -1 error or get value
                    215:  */
                    216: uint32_t
1.4       misho     217: rpack_uint32(rpack_t * __restrict rp, uint32_t * __restrict n, int be)
1.2       misho     218: {
                    219:        uint32_t u;
                    220:        uint8_t *next;
                    221: 
                    222:        if (!RPACK_SANITY(rp))
                    223:                return (uint32_t) -1;
                    224:        /* No space left */
                    225:        if (!(next = rpack_align_and_reserve(rp, sizeof(uint32_t))))
                    226:                return (uint32_t) -1;
                    227: 
1.4       misho     228:        u = be ? EXTRACT_BE_32(next) : EXTRACT_LE_32(next);
1.2       misho     229:        if (n)
                    230:                RPACK_SET_32(next, n);
                    231: 
                    232:        rp->r_next = next + sizeof(uint32_t);
                    233:        return u;
                    234: }
                    235: 
                    236: /*
                    237:  * rpack_uint64() - Pack/Unpack 64bit value
                    238:  *
                    239:  * @rp = raw buffer
                    240:  * @n = set value if !=NULL
1.4       misho     241:  * @be = extract in big-endian
1.2       misho     242:  * return: -1 error or get value
                    243:  */
                    244: uint64_t
1.4       misho     245: rpack_uint64(rpack_t * __restrict rp, uint64_t * __restrict n, int be)
1.2       misho     246: {
                    247:        uint64_t u;
                    248:        uint8_t *next;
                    249: 
                    250:        if (!RPACK_SANITY(rp))
                    251:                return (uint64_t) -1;
                    252:        /* No space left */
                    253:        if (!(next = rpack_align_and_reserve(rp, sizeof(uint64_t))))
                    254:                return (uint64_t) -1;
                    255: 
1.4       misho     256:        u = be ? EXTRACT_BE_64(next) : EXTRACT_LE_64(next);
1.2       misho     257:        if (n)
                    258:                RPACK_SET_64(next, n);
                    259: 
                    260:        rp->r_next = next + sizeof(uint64_t);
                    261:        return u;
                    262: }
1.5     ! misho     263: 
        !           264: /*
        !           265:  * rpack_data() - Pack/Unpack align data
        !           266:  *
        !           267:  * @rp = raw buffer
        !           268:  * @dat = data
        !           269:  * @datlen = data length
        !           270:  * return: NULL error or != NULL get data, must be e_free() after use!
        !           271:  */
        !           272: void *
        !           273: rpack_data(rpack_t * __restrict rp, void * __restrict dat, size_t datlen)
        !           274: {
        !           275:        void *buf = NULL;
        !           276:        uint8_t *next;
        !           277: 
        !           278:        if (!datlen || !RPACK_SANITY(rp))
        !           279:                return NULL;
        !           280:        buf = e_malloc(datlen);
        !           281:        if (!buf)
        !           282:                return NULL;
        !           283:        /* No space left */
        !           284:        if (!(next = rpack_align_and_reserve(rp, datlen))) {
        !           285:                e_free(buf);
        !           286:                return NULL;
        !           287:        }
        !           288: 
        !           289:        memcpy(buf, next, datlen);
        !           290:        if (dat)
        !           291:                memcpy(next, dat, datlen);
        !           292: 
        !           293:        rp->r_next = next + datlen;
        !           294:        return buf;
        !           295: }
        !           296: 
        !           297: /*
        !           298:  * rpack_raw() - Pack/Unpack raw data
        !           299:  *
        !           300:  * @rp = raw buffer
        !           301:  * @dat = data
        !           302:  * @datlen = data length
        !           303:  * return: NULL error or != NULL get data, must be e_free() after use!
        !           304:  */
        !           305: void *
        !           306: rpack_raw(rpack_t * __restrict rp, void * __restrict dat, size_t datlen)
        !           307: {
        !           308:        void *buf = NULL;
        !           309: 
        !           310:        if (!datlen || !RPACK_SANITY(rp))
        !           311:                return NULL;
        !           312:        buf = e_malloc(datlen);
        !           313:        if (!buf)
        !           314:                return NULL;
        !           315:        /* No space left */
        !           316:        if (datlen + rp->r_next - rp->r_buf > rp->r_len) {
        !           317:                e_free(buf);
        !           318:                return NULL;
        !           319:        }
        !           320: 
        !           321:        memcpy(buf, rp->r_next, datlen);
        !           322:        if (dat)
        !           323:                memcpy(rp->r_next, dat, datlen);
        !           324: 
        !           325:        rp->r_next += datlen;
        !           326:        return buf;
        !           327: }

FreeBSD-CVSweb <freebsd-cvsweb@FreeBSD.org>