File:
[ELWIX - Embedded LightWeight unIX -] /
libelwix /
src /
pack.c
Revision
1.8:
download - view:
text,
annotated -
select for diffs -
revision graph
Thu Jun 25 17:53:50 2015 UTC (8 years, 10 months ago) by
misho
Branches:
MAIN
CVS tags:
elwix5_4,
elwix5_3,
elwix5_2,
elwix5_1,
elwix5_0,
elwix4_9,
elwix4_8,
elwix4_7,
elwix4_6,
elwix4_5,
elwix4_4,
elwix4_3,
elwix4_26,
elwix4_25,
elwix4_24,
elwix4_23,
elwix4_22,
elwix4_21,
elwix4_20,
elwix4_2,
elwix4_19,
elwix4_18,
elwix4_17,
elwix4_16,
elwix4_15,
elwix4_14,
elwix4_13,
elwix4_12,
elwix4_11,
elwix4_10,
elwix4_1,
elwix3_9,
elwix3_8,
HEAD,
ELWIX5_3,
ELWIX5_2,
ELWIX5_1,
ELWIX5_0,
ELWIX4_9,
ELWIX4_8,
ELWIX4_7,
ELWIX4_6,
ELWIX4_5,
ELWIX4_4,
ELWIX4_3,
ELWIX4_26,
ELWIX4_25,
ELWIX4_24,
ELWIX4_23,
ELWIX4_22,
ELWIX4_21,
ELWIX4_20,
ELWIX4_2,
ELWIX4_19,
ELWIX4_18,
ELWIX4_17,
ELWIX4_16,
ELWIX4_15,
ELWIX4_14,
ELWIX4_13,
ELWIX4_12,
ELWIX4_11,
ELWIX4_10,
ELWIX4_1,
ELWIX4_0,
ELWIX3_8,
ELWIX3_7
version 3.7
1: /*************************************************************************
2: * (C) 2013 AITNET ltd - Sofia/Bulgaria - <misho@aitnet.org>
3: * by Michael Pounov <misho@elwix.org>
4: *
5: * $Author: misho $
6: * $Id: pack.c,v 1.8 2015/06/25 17:53:50 misho Exp $
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 - 2015
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: */
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_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: /*
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
228: * @be = byte order [-1 little endian, 1 big endian and 0 host order]
229: * return: -1 error or get value
230: */
231: uint16_t
232: rpack_uint16(rpack_t * __restrict rp, uint16_t * __restrict n, int be)
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:
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
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
265: * @be = byte order [-1 little endian, 1 big endian and 0 host order]
266: * return: -1 error or get value
267: */
268: uint32_t
269: rpack_uint24(rpack_t * __restrict rp, uint32_t * __restrict n, int be)
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:
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
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
302: * @be = byte order [-1 little endian, 1 big endian and 0 host order]
303: * return: -1 error or get value
304: */
305: uint32_t
306: rpack_uint32(rpack_t * __restrict rp, uint32_t * __restrict n, int be)
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:
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
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
339: * @be = byte order [-1 little endian, 1 big endian and 0 host order]
340: * return: -1 error or get value
341: */
342: uint64_t
343: rpack_uint64(rpack_t * __restrict rp, uint64_t * __restrict n, int be)
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:
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
364: if (n)
365: RPACK_SET_64(next, n);
366:
367: rp->r_next = next + sizeof(uint64_t);
368: return u;
369: }
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: /*
405: * rpack_rdata() - Pack/Unpack raw data
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 *
413: rpack_rdata(rpack_t * __restrict rp, void * __restrict dat, size_t datlen)
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: }
435:
436: /*
437: * rpack_ruint16() - Pack/Unpack raw 16bit value
438: *
439: * @rp = raw buffer
440: * @n = set value if !=NULL
441: * @be = byte order [-1 little endian, 1 big endian and 0 host order]
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:
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
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
477: * @be = byte order [-1 little endian, 1 big endian and 0 host order]
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:
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
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
513: * @be = byte order [-1 little endian, 1 big endian and 0 host order]
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:
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
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
549: * @be = byte order [-1 little endian, 1 big endian and 0 host order]
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:
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
573: if (n)
574: RPACK_SET_64(rp->r_next, n);
575:
576: rp->r_next += sizeof(uint64_t);
577: return u;
578: }
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