Annotation of libelwix/src/pack.c, revision 1.7
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.7 ! misho 6: * $Id: pack.c,v 1.6.2.4 2014/02/21 13:28:38 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.7 ! 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.7 ! misho 228: * @be = byte order [-1 little endian, 1 big endian and 0 host order]
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.7 ! misho 243: if (be < 0)
! 244: u = EXTRACT_LE_16(next);
! 245: else if (be > 0)
! 246: u = EXTRACT_BE_16(next);
! 247: else
! 248: #if BYTE_ORDER == BIG_ENDIAN
! 249: u = EXTRACT_BE_16(next);
! 250: #else
! 251: u = EXTRACT_LE_16(next);
! 252: #endif
1.2 misho 253: if (n)
254: RPACK_SET_16(next, n);
255:
256: rp->r_next = next + sizeof(uint16_t);
257: return u;
258: }
259:
260: /*
261: * rpack_uint24() - Pack/Unpack 24bit value
262: *
263: * @rp = raw buffer
264: * @n = set value if !=NULL
1.7 ! misho 265: * @be = byte order [-1 little endian, 1 big endian and 0 host order]
1.2 misho 266: * return: -1 error or get value
267: */
268: uint32_t
1.4 misho 269: rpack_uint24(rpack_t * __restrict rp, uint32_t * __restrict n, int be)
1.2 misho 270: {
271: uint32_t u;
272: uint8_t *next;
273:
274: if (!RPACK_SANITY(rp))
275: return (uint32_t) -1;
276: /* No space left */
277: if (!(next = rpack_align_and_reserve(rp, sizeof(uint32_t))))
278: return (uint32_t) -1;
279:
1.7 ! misho 280: if (be < 0)
! 281: u = EXTRACT_LE_24(next);
! 282: else if (be > 0)
! 283: u = EXTRACT_BE_24(next);
! 284: else
! 285: #if BYTE_ORDER == BIG_ENDIAN
! 286: u = EXTRACT_BE_24(next);
! 287: #else
! 288: u = EXTRACT_LE_24(next);
! 289: #endif
1.2 misho 290: if (n)
291: RPACK_SET_24(next, n);
292:
293: rp->r_next = next + sizeof(uint32_t);
294: return u;
295: }
296:
297: /*
298: * rpack_uint32() - Pack/Unpack 32bit value
299: *
300: * @rp = raw buffer
301: * @n = set value if !=NULL
1.7 ! misho 302: * @be = byte order [-1 little endian, 1 big endian and 0 host order]
1.2 misho 303: * return: -1 error or get value
304: */
305: uint32_t
1.4 misho 306: rpack_uint32(rpack_t * __restrict rp, uint32_t * __restrict n, int be)
1.2 misho 307: {
308: uint32_t u;
309: uint8_t *next;
310:
311: if (!RPACK_SANITY(rp))
312: return (uint32_t) -1;
313: /* No space left */
314: if (!(next = rpack_align_and_reserve(rp, sizeof(uint32_t))))
315: return (uint32_t) -1;
316:
1.7 ! misho 317: if (be < 0)
! 318: u = EXTRACT_LE_32(next);
! 319: else if (be > 0)
! 320: u = EXTRACT_BE_32(next);
! 321: else
! 322: #if BYTE_ORDER == BIG_ENDIAN
! 323: u = EXTRACT_BE_32(next);
! 324: #else
! 325: u = EXTRACT_LE_32(next);
! 326: #endif
1.2 misho 327: if (n)
328: RPACK_SET_32(next, n);
329:
330: rp->r_next = next + sizeof(uint32_t);
331: return u;
332: }
333:
334: /*
335: * rpack_uint64() - Pack/Unpack 64bit value
336: *
337: * @rp = raw buffer
338: * @n = set value if !=NULL
1.7 ! misho 339: * @be = byte order [-1 little endian, 1 big endian and 0 host order]
1.2 misho 340: * return: -1 error or get value
341: */
342: uint64_t
1.4 misho 343: rpack_uint64(rpack_t * __restrict rp, uint64_t * __restrict n, int be)
1.2 misho 344: {
345: uint64_t u;
346: uint8_t *next;
347:
348: if (!RPACK_SANITY(rp))
349: return (uint64_t) -1;
350: /* No space left */
351: if (!(next = rpack_align_and_reserve(rp, sizeof(uint64_t))))
352: return (uint64_t) -1;
353:
1.7 ! misho 354: if (be < 0)
! 355: u = EXTRACT_LE_64(next);
! 356: else if (be > 0)
! 357: u = EXTRACT_BE_64(next);
! 358: else
! 359: #if BYTE_ORDER == BIG_ENDIAN
! 360: u = EXTRACT_BE_64(next);
! 361: #else
! 362: u = EXTRACT_LE_64(next);
! 363: #endif
1.2 misho 364: if (n)
365: RPACK_SET_64(next, n);
366:
367: rp->r_next = next + sizeof(uint64_t);
368: return u;
369: }
1.5 misho 370:
371: /*
372: * rpack_data() - Pack/Unpack align data
373: *
374: * @rp = raw buffer
375: * @dat = data
376: * @datlen = data length
377: * return: NULL error or != NULL get data, must be e_free() after use!
378: */
379: void *
380: rpack_data(rpack_t * __restrict rp, void * __restrict dat, size_t datlen)
381: {
382: void *buf = NULL;
383: uint8_t *next;
384:
385: if (!datlen || !RPACK_SANITY(rp))
386: return NULL;
387: buf = e_malloc(datlen);
388: if (!buf)
389: return NULL;
390: /* No space left */
391: if (!(next = rpack_align_and_reserve(rp, datlen))) {
392: e_free(buf);
393: return NULL;
394: }
395:
396: memcpy(buf, next, datlen);
397: if (dat)
398: memcpy(next, dat, datlen);
399:
400: rp->r_next = next + datlen;
401: return buf;
402: }
403:
404: /*
1.6 misho 405: * rpack_rdata() - Pack/Unpack raw data
1.5 misho 406: *
407: * @rp = raw buffer
408: * @dat = data
409: * @datlen = data length
410: * return: NULL error or != NULL get data, must be e_free() after use!
411: */
412: void *
1.6 misho 413: rpack_rdata(rpack_t * __restrict rp, void * __restrict dat, size_t datlen)
1.5 misho 414: {
415: void *buf = NULL;
416:
417: if (!datlen || !RPACK_SANITY(rp))
418: return NULL;
419: buf = e_malloc(datlen);
420: if (!buf)
421: return NULL;
422: /* No space left */
423: if (datlen + rp->r_next - rp->r_buf > rp->r_len) {
424: e_free(buf);
425: return NULL;
426: }
427:
428: memcpy(buf, rp->r_next, datlen);
429: if (dat)
430: memcpy(rp->r_next, dat, datlen);
431:
432: rp->r_next += datlen;
433: return buf;
434: }
1.6 misho 435:
436: /*
437: * rpack_ruint16() - Pack/Unpack raw 16bit value
438: *
439: * @rp = raw buffer
440: * @n = set value if !=NULL
1.7 ! misho 441: * @be = byte order [-1 little endian, 1 big endian and 0 host order]
1.6 misho 442: * return: -1 error or get value
443: */
444: uint16_t
445: rpack_ruint16(rpack_t * __restrict rp, uint16_t * __restrict n, int be)
446: {
447: uint16_t u;
448:
449: if (!RPACK_SANITY(rp))
450: return (uint16_t) -1;
451: /* No space left */
452: if (sizeof(uint16_t) + rp->r_next - rp->r_buf > rp->r_len)
453: return (uint16_t) -1;
454:
1.7 ! misho 455: if (be < 0)
! 456: u = EXTRACT_LE_16(rp->r_next);
! 457: else if (be > 0)
! 458: u = EXTRACT_BE_16(rp->r_next);
! 459: else
! 460: #if BYTE_ORDER == BIG_ENDIAN
! 461: u = EXTRACT_BE_16(rp->r_next);
! 462: #else
! 463: u = EXTRACT_LE_16(rp->r_next);
! 464: #endif
1.6 misho 465: if (n)
466: RPACK_SET_16(rp->r_next, n);
467:
468: rp->r_next += sizeof(uint16_t);
469: return u;
470: }
471:
472: /*
473: * rpack_ruint24() - Pack/Unpack raw 24bit value
474: *
475: * @rp = raw buffer
476: * @n = set value if !=NULL
1.7 ! misho 477: * @be = byte order [-1 little endian, 1 big endian and 0 host order]
1.6 misho 478: * return: -1 error or get value
479: */
480: uint32_t
481: rpack_ruint24(rpack_t * __restrict rp, uint32_t * __restrict n, int be)
482: {
483: uint32_t u;
484:
485: if (!RPACK_SANITY(rp))
486: return (uint32_t) -1;
487: /* No space left */
488: if (sizeof(uint32_t) + rp->r_next - rp->r_buf > rp->r_len)
489: return (uint32_t) -1;
490:
1.7 ! misho 491: if (be < 0)
! 492: u = EXTRACT_LE_24(rp->r_next);
! 493: else if (be > 0)
! 494: u = EXTRACT_BE_24(rp->r_next);
! 495: else
! 496: #if BYTE_ORDER == BIG_ENDIAN
! 497: u = EXTRACT_BE_24(rp->r_next);
! 498: #else
! 499: u = EXTRACT_LE_24(rp->r_next);
! 500: #endif
1.6 misho 501: if (n)
502: RPACK_SET_24(rp->r_next, n);
503:
504: rp->r_next += sizeof(uint32_t);
505: return u;
506: }
507:
508: /*
509: * rpack_ruint32() - Pack/Unpack raw 32bit value
510: *
511: * @rp = raw buffer
512: * @n = set value if !=NULL
1.7 ! misho 513: * @be = byte order [-1 little endian, 1 big endian and 0 host order]
1.6 misho 514: * return: -1 error or get value
515: */
516: uint32_t
517: rpack_ruint32(rpack_t * __restrict rp, uint32_t * __restrict n, int be)
518: {
519: uint32_t u;
520:
521: if (!RPACK_SANITY(rp))
522: return (uint32_t) -1;
523: /* No space left */
524: if (sizeof(uint32_t) + rp->r_next - rp->r_buf > rp->r_len)
525: return (uint32_t) -1;
526:
1.7 ! misho 527: if (be < 0)
! 528: u = EXTRACT_LE_32(rp->r_next);
! 529: else if (be > 0)
! 530: u = EXTRACT_BE_32(rp->r_next);
! 531: else
! 532: #if BYTE_ORDER == BIG_ENDIAN
! 533: u = EXTRACT_BE_32(rp->r_next);
! 534: #else
! 535: u = EXTRACT_LE_32(rp->r_next);
! 536: #endif
1.6 misho 537: if (n)
538: RPACK_SET_32(rp->r_next, n);
539:
540: rp->r_next += sizeof(uint32_t);
541: return u;
542: }
543:
544: /*
545: * rpack_ruint64() - Pack/Unpack raw 64bit value
546: *
547: * @rp = raw buffer
548: * @n = set value if !=NULL
1.7 ! misho 549: * @be = byte order [-1 little endian, 1 big endian and 0 host order]
1.6 misho 550: * return: -1 error or get value
551: */
552: uint64_t
553: rpack_ruint64(rpack_t * __restrict rp, uint64_t * __restrict n, int be)
554: {
555: uint64_t u;
556:
557: if (!RPACK_SANITY(rp))
558: return (uint64_t) -1;
559: /* No space left */
560: if (sizeof(uint64_t) + rp->r_next - rp->r_buf > rp->r_len)
561: return (uint64_t) -1;
562:
1.7 ! misho 563: if (be < 0)
! 564: u = EXTRACT_LE_64(rp->r_next);
! 565: else if (be > 0)
! 566: u = EXTRACT_BE_64(rp->r_next);
! 567: else
! 568: #if BYTE_ORDER == BIG_ENDIAN
! 569: u = EXTRACT_BE_64(rp->r_next);
! 570: #else
! 571: u = EXTRACT_LE_64(rp->r_next);
! 572: #endif
1.6 misho 573: if (n)
574: RPACK_SET_64(rp->r_next, n);
575:
576: rp->r_next += sizeof(uint64_t);
577: return u;
578: }
1.7 ! misho 579:
! 580: /*
! 581: * rpack_next() - Get and set current position
! 582: *
! 583: * @rp = raw packet
! 584: * @after_len = move aligned current position after length
! 585: * return: NULL error or current position
! 586: */
! 587: uint8_t *
! 588: rpack_next(rpack_t * __restrict rp, size_t after_len)
! 589: {
! 590: uint8_t *cur = NULL, *next = NULL;
! 591:
! 592: if (!RPACK_SANITY(rp))
! 593: return NULL;
! 594: /* No space left */
! 595: if (!(next = rpack_align_and_reserve(rp, after_len)))
! 596: return NULL;
! 597:
! 598: cur = rp->r_next;
! 599:
! 600: rp->r_next = next + after_len;
! 601: return cur;
! 602: }
! 603:
! 604: /*
! 605: * rpack_rnext() - Get and set raw current position
! 606: *
! 607: * @rp = raw packet
! 608: * @after_len = !=0 move current position after length
! 609: * return: NULL error or raw current position
! 610: */
! 611: uint8_t *
! 612: rpack_rnext(rpack_t * __restrict rp, size_t after_len)
! 613: {
! 614: uint8_t *next = NULL;
! 615:
! 616: if (!RPACK_SANITY(rp))
! 617: return NULL;
! 618: /* No space left */
! 619: if (after_len + rp->r_next - rp->r_buf > rp->r_len)
! 620: return NULL;
! 621:
! 622: next = rp->r_next;
! 623:
! 624: rp->r_next += after_len;
! 625: return next;
! 626: }
FreeBSD-CVSweb <freebsd-cvsweb@FreeBSD.org>
![[BACK]](/icons/cvsweb/back.gif){kind=icon}
Return to pack.c CVS log ![[TXT]](/icons/cvsweb/text.gif){kind=icon}
![[DIR]](/icons/cvsweb/dir.gif){kind=icon}
Up to [ELWIX - Embedded LightWeight unIX -] / libelwix / src