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

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.6.2.1 ! misho       6: * $Id: pack.c,v 1.6 2014/02/11 01:06:21 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: /*
1.6.2.1 ! misho     103:  * rpack_attach() - Attach dynamic allocating buffer at packet
        !           104:  *
        !           105:  * @rp = raw packet;
        !           106:  * @len = allocate bytes
        !           107:  * return: -1 error or 0 ok, should be detached with rpack_detach() 
        !           108:  *                             before call rpack_destroy() after use!
        !           109:  */
        !           110: int
        !           111: rpack_attach(rpack_t * __restrict rp, size_t len)
        !           112: {
        !           113:        if (!rp)
        !           114:                return -1;
        !           115: 
        !           116:        rp->r_buf = e_malloc(len);
        !           117:        if (!rp->r_buf) {
        !           118:                RPACK_FREE(rp);
        !           119:                return -1;
        !           120:        } else
        !           121:                rp->r_len = len;
        !           122:        rp->r_next = rp->r_buf;
        !           123: 
        !           124:        return 0;
        !           125: }
        !           126: 
        !           127: /*
        !           128:  * rpack_resize() - Resize dynamic allocated buffer at packet
        !           129:  *
        !           130:  * @rp = raw packet
        !           131:  * @newlen = resize buffer to bytes
        !           132:  * return: -1 error or 0 ok, should be detached with rpack_detach() 
        !           133:  *                             before call rpack_destroy() after use!
        !           134:  */
        !           135: int
        !           136: rpack_resize(rpack_t * __restrict rp, size_t newlen)
        !           137: {
        !           138:        void *buf = NULL;
        !           139: 
        !           140:        if (!rp)
        !           141:                return -1;
        !           142: 
        !           143:        buf = e_realloc(rp->r_buf, newlen);
        !           144:        if (!buf)
        !           145:                return -1;
        !           146:        else {
        !           147:                rp->r_buf = buf;
        !           148:                rp->r_len = newlen;
        !           149:        }
        !           150: 
        !           151:        if (rp->r_next > (rp->r_buf + rp->r_len))
        !           152:                rp->r_next = rp->r_buf;
        !           153: 
        !           154:        return 0;
        !           155: }
        !           156: 
        !           157: /*
        !           158:  * rpack_detach() - Detach and free dynamic allocated buffer from packet
        !           159:  *
        !           160:  * @rp = raw packet
        !           161:  * return: none
        !           162:  */
        !           163: void
        !           164: rpack_detach(rpack_t * __restrict rp)
        !           165: {
        !           166:        if (!rp)
        !           167:                return;
        !           168: 
        !           169:        e_free(rp->r_buf);
        !           170:        RPACK_FREE(rp);
        !           171: }
        !           172: 
        !           173: /*
1.2       misho     174:  * rpack_align_and_reserve() - Align & reserve space
                    175:  *
                    176:  * @rp = raw buffer
                    177:  * @siz = need size
                    178:  * return: NULL error or not enough space, !=NULL next position
                    179:  */
                    180: uint8_t *
                    181: rpack_align_and_reserve(rpack_t * __restrict rp, size_t siz)
                    182: {
                    183:        uint8_t *n;
                    184: 
                    185:        if (!RPACK_SANITY(rp))
                    186:                return NULL;
                    187: 
                    188:        n = rpack_next_boundary(rp->r_buf, rp->r_next, siz);
                    189:        /* too little space for siz */
                    190:        if (n - rp->r_buf + siz > rp->r_len)
                    191:                return NULL;
                    192: 
                    193:        return n;
                    194: }
                    195: 
                    196: 
                    197: /*
                    198:  * rpack_uint8() - Pack/Unpack 8bit value
                    199:  *
                    200:  * @rp = raw buffer
                    201:  * @n = set value if !=NULL
                    202:  * return: -1 error or get value
                    203:  */
                    204: uint8_t
                    205: rpack_uint8(rpack_t * __restrict rp, uint8_t * __restrict n)
                    206: {
                    207:        uint8_t u;
                    208: 
                    209:        if (!RPACK_SANITY(rp))
                    210:                return (uint8_t) -1;
                    211:        /* No space left */
                    212:        if ((size_t) (rp->r_next - rp->r_buf) >= rp->r_len)
                    213:                return (uint8_t) -1;
                    214: 
                    215:        u = *rp->r_next;
                    216:        if (n)
                    217:                *rp->r_next = *n;
                    218: 
                    219:        rp->r_next++;
                    220:        return u;
                    221: }
                    222: 
                    223: /*
                    224:  * rpack_uint16() - Pack/Unpack 16bit value
                    225:  *
                    226:  * @rp = raw buffer
                    227:  * @n = set value if !=NULL
1.4       misho     228:  * @be = extract in big-endian
1.2       misho     229:  * return: -1 error or get value
                    230:  */
                    231: uint16_t
1.4       misho     232: rpack_uint16(rpack_t * __restrict rp, uint16_t * __restrict n, int be)
1.2       misho     233: {
                    234:        uint16_t u;
                    235:        uint8_t *next;
                    236: 
                    237:        if (!RPACK_SANITY(rp))
                    238:                return (uint16_t) -1;
                    239:        /* No space left */
                    240:        if (!(next = rpack_align_and_reserve(rp, sizeof(uint16_t))))
                    241:                return (uint16_t) -1;
                    242: 
1.4       misho     243:        u = be ? EXTRACT_BE_16(next) : EXTRACT_LE_16(next);
1.2       misho     244:        if (n)
                    245:                RPACK_SET_16(next, n);
                    246: 
                    247:        rp->r_next = next + sizeof(uint16_t);
                    248:        return u;
                    249: }
                    250: 
                    251: /*
                    252:  * rpack_uint24() - Pack/Unpack 24bit value
                    253:  *
                    254:  * @rp = raw buffer
                    255:  * @n = set value if !=NULL
1.4       misho     256:  * @be = extract in big-endian
1.2       misho     257:  * return: -1 error or get value
                    258:  */
                    259: uint32_t
1.4       misho     260: rpack_uint24(rpack_t * __restrict rp, uint32_t * __restrict n, int be)
1.2       misho     261: {
                    262:        uint32_t u;
                    263:        uint8_t *next;
                    264: 
                    265:        if (!RPACK_SANITY(rp))
                    266:                return (uint32_t) -1;
                    267:        /* No space left */
                    268:        if (!(next = rpack_align_and_reserve(rp, sizeof(uint32_t))))
                    269:                return (uint32_t) -1;
                    270: 
1.4       misho     271:        u = be ? EXTRACT_BE_24(next) : EXTRACT_LE_24(next);
1.2       misho     272:        if (n)
                    273:                RPACK_SET_24(next, n);
                    274: 
                    275:        rp->r_next = next + sizeof(uint32_t);
                    276:        return u;
                    277: }
                    278: 
                    279: /*
                    280:  * rpack_uint32() - Pack/Unpack 32bit value
                    281:  *
                    282:  * @rp = raw buffer
                    283:  * @n = set value if !=NULL
1.4       misho     284:  * @be = extract in big-endian
1.2       misho     285:  * return: -1 error or get value
                    286:  */
                    287: uint32_t
1.4       misho     288: rpack_uint32(rpack_t * __restrict rp, uint32_t * __restrict n, int be)
1.2       misho     289: {
                    290:        uint32_t u;
                    291:        uint8_t *next;
                    292: 
                    293:        if (!RPACK_SANITY(rp))
                    294:                return (uint32_t) -1;
                    295:        /* No space left */
                    296:        if (!(next = rpack_align_and_reserve(rp, sizeof(uint32_t))))
                    297:                return (uint32_t) -1;
                    298: 
1.4       misho     299:        u = be ? EXTRACT_BE_32(next) : EXTRACT_LE_32(next);
1.2       misho     300:        if (n)
                    301:                RPACK_SET_32(next, n);
                    302: 
                    303:        rp->r_next = next + sizeof(uint32_t);
                    304:        return u;
                    305: }
                    306: 
                    307: /*
                    308:  * rpack_uint64() - Pack/Unpack 64bit value
                    309:  *
                    310:  * @rp = raw buffer
                    311:  * @n = set value if !=NULL
1.4       misho     312:  * @be = extract in big-endian
1.2       misho     313:  * return: -1 error or get value
                    314:  */
                    315: uint64_t
1.4       misho     316: rpack_uint64(rpack_t * __restrict rp, uint64_t * __restrict n, int be)
1.2       misho     317: {
                    318:        uint64_t u;
                    319:        uint8_t *next;
                    320: 
                    321:        if (!RPACK_SANITY(rp))
                    322:                return (uint64_t) -1;
                    323:        /* No space left */
                    324:        if (!(next = rpack_align_and_reserve(rp, sizeof(uint64_t))))
                    325:                return (uint64_t) -1;
                    326: 
1.4       misho     327:        u = be ? EXTRACT_BE_64(next) : EXTRACT_LE_64(next);
1.2       misho     328:        if (n)
                    329:                RPACK_SET_64(next, n);
                    330: 
                    331:        rp->r_next = next + sizeof(uint64_t);
                    332:        return u;
                    333: }
1.5       misho     334: 
                    335: /*
                    336:  * rpack_data() - Pack/Unpack align data
                    337:  *
                    338:  * @rp = raw buffer
                    339:  * @dat = data
                    340:  * @datlen = data length
                    341:  * return: NULL error or != NULL get data, must be e_free() after use!
                    342:  */
                    343: void *
                    344: rpack_data(rpack_t * __restrict rp, void * __restrict dat, size_t datlen)
                    345: {
                    346:        void *buf = NULL;
                    347:        uint8_t *next;
                    348: 
                    349:        if (!datlen || !RPACK_SANITY(rp))
                    350:                return NULL;
                    351:        buf = e_malloc(datlen);
                    352:        if (!buf)
                    353:                return NULL;
                    354:        /* No space left */
                    355:        if (!(next = rpack_align_and_reserve(rp, datlen))) {
                    356:                e_free(buf);
                    357:                return NULL;
                    358:        }
                    359: 
                    360:        memcpy(buf, next, datlen);
                    361:        if (dat)
                    362:                memcpy(next, dat, datlen);
                    363: 
                    364:        rp->r_next = next + datlen;
                    365:        return buf;
                    366: }
                    367: 
                    368: /*
1.6       misho     369:  * rpack_rdata() - Pack/Unpack raw data
1.5       misho     370:  *
                    371:  * @rp = raw buffer
                    372:  * @dat = data
                    373:  * @datlen = data length
                    374:  * return: NULL error or != NULL get data, must be e_free() after use!
                    375:  */
                    376: void *
1.6       misho     377: rpack_rdata(rpack_t * __restrict rp, void * __restrict dat, size_t datlen)
1.5       misho     378: {
                    379:        void *buf = NULL;
                    380: 
                    381:        if (!datlen || !RPACK_SANITY(rp))
                    382:                return NULL;
                    383:        buf = e_malloc(datlen);
                    384:        if (!buf)
                    385:                return NULL;
                    386:        /* No space left */
                    387:        if (datlen + rp->r_next - rp->r_buf > rp->r_len) {
                    388:                e_free(buf);
                    389:                return NULL;
                    390:        }
                    391: 
                    392:        memcpy(buf, rp->r_next, datlen);
                    393:        if (dat)
                    394:                memcpy(rp->r_next, dat, datlen);
                    395: 
                    396:        rp->r_next += datlen;
                    397:        return buf;
                    398: }
1.6       misho     399: 
                    400: /*
                    401:  * rpack_ruint16() - Pack/Unpack raw 16bit value
                    402:  *
                    403:  * @rp = raw buffer
                    404:  * @n = set value if !=NULL
                    405:  * @be = extract in big-endian
                    406:  * return: -1 error or get value
                    407:  */
                    408: uint16_t
                    409: rpack_ruint16(rpack_t * __restrict rp, uint16_t * __restrict n, int be)
                    410: {
                    411:        uint16_t u;
                    412: 
                    413:        if (!RPACK_SANITY(rp))
                    414:                return (uint16_t) -1;
                    415:        /* No space left */
                    416:        if (sizeof(uint16_t) + rp->r_next - rp->r_buf > rp->r_len)
                    417:                return (uint16_t) -1;
                    418: 
                    419:        u = be ? EXTRACT_BE_16(rp->r_next) : EXTRACT_LE_16(rp->r_next);
                    420:        if (n)
                    421:                RPACK_SET_16(rp->r_next, n);
                    422: 
                    423:        rp->r_next += sizeof(uint16_t);
                    424:        return u;
                    425: }
                    426: 
                    427: /*
                    428:  * rpack_ruint24() - Pack/Unpack raw 24bit value
                    429:  *
                    430:  * @rp = raw buffer
                    431:  * @n = set value if !=NULL
                    432:  * @be = extract in big-endian
                    433:  * return: -1 error or get value
                    434:  */
                    435: uint32_t
                    436: rpack_ruint24(rpack_t * __restrict rp, uint32_t * __restrict n, int be)
                    437: {
                    438:        uint32_t u;
                    439: 
                    440:        if (!RPACK_SANITY(rp))
                    441:                return (uint32_t) -1;
                    442:        /* No space left */
                    443:        if (sizeof(uint32_t) + rp->r_next - rp->r_buf > rp->r_len)
                    444:                return (uint32_t) -1;
                    445: 
                    446:        u = be ? EXTRACT_BE_24(rp->r_next) : EXTRACT_LE_24(rp->r_next);
                    447:        if (n)
                    448:                RPACK_SET_24(rp->r_next, n);
                    449: 
                    450:        rp->r_next += sizeof(uint32_t);
                    451:        return u;
                    452: }
                    453: 
                    454: /*
                    455:  * rpack_ruint32() - Pack/Unpack raw 32bit value
                    456:  *
                    457:  * @rp = raw buffer
                    458:  * @n = set value if !=NULL
                    459:  * @be = extract in big-endian
                    460:  * return: -1 error or get value
                    461:  */
                    462: uint32_t
                    463: rpack_ruint32(rpack_t * __restrict rp, uint32_t * __restrict n, int be)
                    464: {
                    465:        uint32_t u;
                    466: 
                    467:        if (!RPACK_SANITY(rp))
                    468:                return (uint32_t) -1;
                    469:        /* No space left */
                    470:        if (sizeof(uint32_t) + rp->r_next - rp->r_buf > rp->r_len)
                    471:                return (uint32_t) -1;
                    472: 
                    473:        u = be ? EXTRACT_BE_32(rp->r_next) : EXTRACT_LE_32(rp->r_next);
                    474:        if (n)
                    475:                RPACK_SET_32(rp->r_next, n);
                    476: 
                    477:        rp->r_next += sizeof(uint32_t);
                    478:        return u;
                    479: }
                    480: 
                    481: /*
                    482:  * rpack_ruint64() - Pack/Unpack raw 64bit value
                    483:  *
                    484:  * @rp = raw buffer
                    485:  * @n = set value if !=NULL
                    486:  * @be = extract in big-endian
                    487:  * return: -1 error or get value
                    488:  */
                    489: uint64_t
                    490: rpack_ruint64(rpack_t * __restrict rp, uint64_t * __restrict n, int be)
                    491: {
                    492:        uint64_t u;
                    493: 
                    494:        if (!RPACK_SANITY(rp))
                    495:                return (uint64_t) -1;
                    496:        /* No space left */
                    497:        if (sizeof(uint64_t) + rp->r_next - rp->r_buf > rp->r_len)
                    498:                return (uint64_t) -1;
                    499: 
                    500:        u = be ? EXTRACT_BE_64(rp->r_next) : EXTRACT_LE_64(rp->r_next);
                    501:        if (n)
                    502:                RPACK_SET_64(rp->r_next, n);
                    503: 
                    504:        rp->r_next += sizeof(uint64_t);
                    505:        return u;
                    506: }

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