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 2014/02/11 01:06:21 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_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
157: * @be = extract in big-endian
158: * return: -1 error or get value
159: */
160: uint16_t
161: rpack_uint16(rpack_t * __restrict rp, uint16_t * __restrict n, int be)
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:
172: u = be ? EXTRACT_BE_16(next) : EXTRACT_LE_16(next);
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
185: * @be = extract in big-endian
186: * return: -1 error or get value
187: */
188: uint32_t
189: rpack_uint24(rpack_t * __restrict rp, uint32_t * __restrict n, int be)
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:
200: u = be ? EXTRACT_BE_24(next) : EXTRACT_LE_24(next);
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
213: * @be = extract in big-endian
214: * return: -1 error or get value
215: */
216: uint32_t
217: rpack_uint32(rpack_t * __restrict rp, uint32_t * __restrict n, int be)
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:
228: u = be ? EXTRACT_BE_32(next) : EXTRACT_LE_32(next);
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
241: * @be = extract in big-endian
242: * return: -1 error or get value
243: */
244: uint64_t
245: rpack_uint64(rpack_t * __restrict rp, uint64_t * __restrict n, int be)
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:
256: u = be ? EXTRACT_BE_64(next) : EXTRACT_LE_64(next);
257: if (n)
258: RPACK_SET_64(next, n);
259:
260: rp->r_next = next + sizeof(uint64_t);
261: return u;
262: }
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_rdata() - 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_rdata(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: }
328:
329: /*
330: * rpack_ruint16() - Pack/Unpack raw 16bit value
331: *
332: * @rp = raw buffer
333: * @n = set value if !=NULL
334: * @be = extract in big-endian
335: * return: -1 error or get value
336: */
337: uint16_t
338: rpack_ruint16(rpack_t * __restrict rp, uint16_t * __restrict n, int be)
339: {
340: uint16_t u;
341:
342: if (!RPACK_SANITY(rp))
343: return (uint16_t) -1;
344: /* No space left */
345: if (sizeof(uint16_t) + rp->r_next - rp->r_buf > rp->r_len)
346: return (uint16_t) -1;
347:
348: u = be ? EXTRACT_BE_16(rp->r_next) : EXTRACT_LE_16(rp->r_next);
349: if (n)
350: RPACK_SET_16(rp->r_next, n);
351:
352: rp->r_next += sizeof(uint16_t);
353: return u;
354: }
355:
356: /*
357: * rpack_ruint24() - Pack/Unpack raw 24bit value
358: *
359: * @rp = raw buffer
360: * @n = set value if !=NULL
361: * @be = extract in big-endian
362: * return: -1 error or get value
363: */
364: uint32_t
365: rpack_ruint24(rpack_t * __restrict rp, uint32_t * __restrict n, int be)
366: {
367: uint32_t u;
368:
369: if (!RPACK_SANITY(rp))
370: return (uint32_t) -1;
371: /* No space left */
372: if (sizeof(uint32_t) + rp->r_next - rp->r_buf > rp->r_len)
373: return (uint32_t) -1;
374:
375: u = be ? EXTRACT_BE_24(rp->r_next) : EXTRACT_LE_24(rp->r_next);
376: if (n)
377: RPACK_SET_24(rp->r_next, n);
378:
379: rp->r_next += sizeof(uint32_t);
380: return u;
381: }
382:
383: /*
384: * rpack_ruint32() - Pack/Unpack raw 32bit value
385: *
386: * @rp = raw buffer
387: * @n = set value if !=NULL
388: * @be = extract in big-endian
389: * return: -1 error or get value
390: */
391: uint32_t
392: rpack_ruint32(rpack_t * __restrict rp, uint32_t * __restrict n, int be)
393: {
394: uint32_t u;
395:
396: if (!RPACK_SANITY(rp))
397: return (uint32_t) -1;
398: /* No space left */
399: if (sizeof(uint32_t) + rp->r_next - rp->r_buf > rp->r_len)
400: return (uint32_t) -1;
401:
402: u = be ? EXTRACT_BE_32(rp->r_next) : EXTRACT_LE_32(rp->r_next);
403: if (n)
404: RPACK_SET_32(rp->r_next, n);
405:
406: rp->r_next += sizeof(uint32_t);
407: return u;
408: }
409:
410: /*
411: * rpack_ruint64() - Pack/Unpack raw 64bit value
412: *
413: * @rp = raw buffer
414: * @n = set value if !=NULL
415: * @be = extract in big-endian
416: * return: -1 error or get value
417: */
418: uint64_t
419: rpack_ruint64(rpack_t * __restrict rp, uint64_t * __restrict n, int be)
420: {
421: uint64_t u;
422:
423: if (!RPACK_SANITY(rp))
424: return (uint64_t) -1;
425: /* No space left */
426: if (sizeof(uint64_t) + rp->r_next - rp->r_buf > rp->r_len)
427: return (uint64_t) -1;
428:
429: u = be ? EXTRACT_BE_64(rp->r_next) : EXTRACT_LE_64(rp->r_next);
430: if (n)
431: RPACK_SET_64(rp->r_next, n);
432:
433: rp->r_next += sizeof(uint64_t);
434: return u;
435: }
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