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.6.2.2 2014/02/13 14:58:55 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 - 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: */
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 = extract in big-endian
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: u = be ? EXTRACT_BE_16(next) : EXTRACT_LE_16(next);
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
256: * @be = extract in big-endian
257: * return: -1 error or get value
258: */
259: uint32_t
260: rpack_uint24(rpack_t * __restrict rp, uint32_t * __restrict n, int be)
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:
271: u = be ? EXTRACT_BE_24(next) : EXTRACT_LE_24(next);
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
284: * @be = extract in big-endian
285: * return: -1 error or get value
286: */
287: uint32_t
288: rpack_uint32(rpack_t * __restrict rp, uint32_t * __restrict n, int be)
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:
299: u = be ? EXTRACT_BE_32(next) : EXTRACT_LE_32(next);
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
312: * @be = extract in big-endian
313: * return: -1 error or get value
314: */
315: uint64_t
316: rpack_uint64(rpack_t * __restrict rp, uint64_t * __restrict n, int be)
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:
327: u = be ? EXTRACT_BE_64(next) : EXTRACT_LE_64(next);
328: if (n)
329: RPACK_SET_64(next, n);
330:
331: rp->r_next = next + sizeof(uint64_t);
332: return u;
333: }
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: /*
369: * rpack_rdata() - Pack/Unpack raw data
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 *
377: rpack_rdata(rpack_t * __restrict rp, void * __restrict dat, size_t datlen)
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: }
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: }
507:
508: /*
509: * rpack_next() - Get and set current position
510: *
511: * @rp = raw packet
512: * @after_len = !=0 move current position after length
513: * return: NULL error or current position
514: */
515: uint8_t *
516: rpack_next(rpack_t * __restrict rp, size_t after_len)
517: {
518: uint8_t *next = NULL;
519:
520: if (!RPACK_SANITY(rp))
521: return NULL;
522: /* No space left */
523: if (after_len + rp->r_next - rp->r_buf > rp->r_len)
524: return NULL;
525:
526: next = rp->r_next;
527:
528: rp->r_next += after_len;
529: return next;
530: }
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