1 /**
2 * @file mbuf.c Memory buffers
3 *
4 * Copyright (C) 2010 Creytiv.com
5 */
6 #include <string.h>
7 #include <re_types.h>
8 #include <re_fmt.h>
9 #include <re_mem.h>
10 #include <re_mbuf.h>
11
12
13 #define DEBUG_MODULE "mbuf"
14 #define DEBUG_LEVEL 4
15 #include <re_dbg.h>
16
17
18 enum {DEFAULT_SIZE=512};
19
20
mbuf_destructor(void * data)21 static void mbuf_destructor(void *data)
22 {
23 struct mbuf *mb = data;
24
25 mem_deref(mb->buf);
26 }
27
28
29 /**
30 * Allocate a new memory buffer
31 *
32 * @param size Initial buffer size
33 *
34 * @return New memory buffer, NULL if no memory
35 */
mbuf_alloc(size_t size)36 struct mbuf *mbuf_alloc(size_t size)
37 {
38 struct mbuf *mb;
39
40 mb = mem_zalloc(sizeof(*mb), mbuf_destructor);
41 if (!mb)
42 return NULL;
43
44 if (mbuf_resize(mb, size ? size : DEFAULT_SIZE))
45 return mem_deref(mb);
46
47 return mb;
48 }
49
50
51 /**
52 * Allocate a new memory buffer with a reference to another mbuf
53 *
54 * @param mbr Memory buffer to reference
55 *
56 * @return New memory buffer, NULL if no memory
57 */
mbuf_alloc_ref(struct mbuf * mbr)58 struct mbuf *mbuf_alloc_ref(struct mbuf *mbr)
59 {
60 struct mbuf *mb;
61
62 if (!mbr)
63 return NULL;
64
65 mb = mem_zalloc(sizeof(*mb), mbuf_destructor);
66 if (!mb)
67 return NULL;
68
69 mb->buf = mem_ref(mbr->buf);
70 mb->size = mbr->size;
71 mb->pos = mbr->pos;
72 mb->end = mbr->end;
73
74 return mb;
75 }
76
77
78 /**
79 * Initialize a memory buffer
80 *
81 * @param mb Memory buffer to initialize
82 */
mbuf_init(struct mbuf * mb)83 void mbuf_init(struct mbuf *mb)
84 {
85 if (!mb)
86 return;
87
88 mb->buf = NULL;
89 mb->size = 0;
90 mb->pos = 0;
91 mb->end = 0;
92 }
93
94
95 /**
96 * Reset a memory buffer
97 *
98 * @param mb Memory buffer to reset
99 */
mbuf_reset(struct mbuf * mb)100 void mbuf_reset(struct mbuf *mb)
101 {
102 if (!mb)
103 return;
104
105 mb->buf = mem_deref(mb->buf);
106 mbuf_init(mb);
107 }
108
109
110 /**
111 * Resize a memory buffer
112 *
113 * @param mb Memory buffer to resize
114 * @param size New buffer size
115 *
116 * @return 0 if success, otherwise errorcode
117 */
mbuf_resize(struct mbuf * mb,size_t size)118 int mbuf_resize(struct mbuf *mb, size_t size)
119 {
120 uint8_t *buf;
121
122 if (!mb)
123 return EINVAL;
124
125 buf = mb->buf ? mem_realloc(mb->buf, size) : mem_alloc(size, NULL);
126 if (!buf)
127 return ENOMEM;
128
129 mb->buf = buf;
130 mb->size = size;
131
132 return 0;
133 }
134
135
136 /**
137 * Trim unused trailing bytes in a memory buffer, resize if necessary
138 *
139 * @param mb Memory buffer to trim
140 */
mbuf_trim(struct mbuf * mb)141 void mbuf_trim(struct mbuf *mb)
142 {
143 int err;
144
145 if (!mb || !mb->end || mb->end == mb->size)
146 return;
147
148 /* We shrink - this cannot fail */
149 err = mbuf_resize(mb, mb->end);
150 if (err) {
151 DEBUG_WARNING("trim: resize failed (%m)\n", err);
152 }
153 }
154
155
156 /**
157 * Shift mbuf content position
158 *
159 * @param mb Memory buffer to shift
160 * @param shift Shift offset count
161 *
162 * @return 0 if success, otherwise errorcode
163 */
mbuf_shift(struct mbuf * mb,ssize_t shift)164 int mbuf_shift(struct mbuf *mb, ssize_t shift)
165 {
166 size_t rsize;
167 uint8_t *p;
168
169 if (!mb)
170 return EINVAL;
171
172 if (((ssize_t)mb->pos + shift) < 0 ||
173 ((ssize_t)mb->end + shift) < 0)
174 return ERANGE;
175
176 rsize = mb->end + shift;
177
178 if (rsize > mb->size) {
179
180 int err;
181
182 err = mbuf_resize(mb, rsize);
183 if (err)
184 return err;
185 }
186
187 p = mbuf_buf(mb);
188
189 memmove(p + shift, p, mbuf_get_left(mb));
190
191 mb->pos += shift;
192 mb->end += shift;
193
194 return 0;
195 }
196
197
198 /**
199 * Write a block of memory to a memory buffer
200 *
201 * @param mb Memory buffer
202 * @param buf Memory block to write
203 * @param size Number of bytes to write
204 *
205 * @return 0 if success, otherwise errorcode
206 */
mbuf_write_mem(struct mbuf * mb,const uint8_t * buf,size_t size)207 int mbuf_write_mem(struct mbuf *mb, const uint8_t *buf, size_t size)
208 {
209 size_t rsize;
210
211 if (!mb || !buf)
212 return EINVAL;
213
214 rsize = mb->pos + size;
215
216 if (rsize > mb->size) {
217 const size_t dsize = mb->size ? (mb->size * 2)
218 : DEFAULT_SIZE;
219 int err;
220
221 err = mbuf_resize(mb, MAX(rsize, dsize));
222 if (err)
223 return err;
224 }
225
226 memcpy(mb->buf + mb->pos, buf, size);
227
228 mb->pos += size;
229 mb->end = MAX(mb->end, mb->pos);
230
231 return 0;
232 }
233
234
235 /**
236 * Write an 8-bit value to a memory buffer
237 *
238 * @param mb Memory buffer
239 * @param v 8-bit value to write
240 *
241 * @return 0 if success, otherwise errorcode
242 */
mbuf_write_u8(struct mbuf * mb,uint8_t v)243 int mbuf_write_u8(struct mbuf *mb, uint8_t v)
244 {
245 return mbuf_write_mem(mb, (uint8_t *)&v, sizeof(v));
246 }
247
248
249 /**
250 * Write a 16-bit value to a memory buffer
251 *
252 * @param mb Memory buffer
253 * @param v 16-bit value to write
254 *
255 * @return 0 if success, otherwise errorcode
256 */
mbuf_write_u16(struct mbuf * mb,uint16_t v)257 int mbuf_write_u16(struct mbuf *mb, uint16_t v)
258 {
259 return mbuf_write_mem(mb, (uint8_t *)&v, sizeof(v));
260 }
261
262
263 /**
264 * Write a 32-bit value to a memory buffer
265 *
266 * @param mb Memory buffer
267 * @param v 32-bit value to write
268 *
269 * @return 0 if success, otherwise errorcode
270 */
mbuf_write_u32(struct mbuf * mb,uint32_t v)271 int mbuf_write_u32(struct mbuf *mb, uint32_t v)
272 {
273 return mbuf_write_mem(mb, (uint8_t *)&v, sizeof(v));
274 }
275
276
277 /**
278 * Write a 64-bit value to a memory buffer
279 *
280 * @param mb Memory buffer
281 * @param v 64-bit value to write
282 *
283 * @return 0 if success, otherwise errorcode
284 */
mbuf_write_u64(struct mbuf * mb,uint64_t v)285 int mbuf_write_u64(struct mbuf *mb, uint64_t v)
286 {
287 return mbuf_write_mem(mb, (uint8_t *)&v, sizeof(v));
288 }
289
290
291 /**
292 * Write a null-terminated string to a memory buffer
293 *
294 * @param mb Memory buffer
295 * @param str Null terminated string to write
296 *
297 * @return 0 if success, otherwise errorcode
298 */
mbuf_write_str(struct mbuf * mb,const char * str)299 int mbuf_write_str(struct mbuf *mb, const char *str)
300 {
301 if (!str)
302 return EINVAL;
303
304 return mbuf_write_mem(mb, (const uint8_t *)str, strlen(str));
305 }
306
307
308 /**
309 * Write a pointer-length string to a memory buffer
310 *
311 * @param mb Memory buffer
312 * @param pl Pointer-length string
313 *
314 * @return 0 if success, otherwise errorcode
315 */
mbuf_write_pl(struct mbuf * mb,const struct pl * pl)316 int mbuf_write_pl(struct mbuf *mb, const struct pl *pl)
317 {
318 if (!pl)
319 return EINVAL;
320
321 return mbuf_write_mem(mb, (const uint8_t *)pl->p, pl->l);
322 }
323
324
325 /**
326 * Read a block of memory from a memory buffer
327 *
328 * @param mb Memory buffer
329 * @param buf Buffer to read data to
330 * @param size Size of buffer
331 *
332 * @return 0 if success, otherwise errorcode
333 */
mbuf_read_mem(struct mbuf * mb,uint8_t * buf,size_t size)334 int mbuf_read_mem(struct mbuf *mb, uint8_t *buf, size_t size)
335 {
336 if (!mb || !buf)
337 return EINVAL;
338
339 if (size > mbuf_get_left(mb)) {
340 DEBUG_WARNING("tried to read beyond mbuf end (%u > %u)\n",
341 size, mbuf_get_left(mb));
342 return EOVERFLOW;
343 }
344
345 memcpy(buf, mb->buf + mb->pos, size);
346
347 mb->pos += size;
348
349 return 0;
350 }
351
352
353 /**
354 * Read an 8-bit value from a memory buffer
355 *
356 * @param mb Memory buffer
357 *
358 * @return 8-bit value
359 */
mbuf_read_u8(struct mbuf * mb)360 uint8_t mbuf_read_u8(struct mbuf *mb)
361 {
362 uint8_t v;
363
364 return (0 == mbuf_read_mem(mb, &v, sizeof(v))) ? v : 0;
365 }
366
367
368 /**
369 * Read a 16-bit value from a memory buffer
370 *
371 * @param mb Memory buffer
372 *
373 * @return 16-bit value
374 */
mbuf_read_u16(struct mbuf * mb)375 uint16_t mbuf_read_u16(struct mbuf *mb)
376 {
377 uint16_t v;
378
379 return (0 == mbuf_read_mem(mb, (uint8_t *)&v, sizeof(v))) ? v : 0;
380 }
381
382
383 /**
384 * Read a 32-bit value from a memory buffer
385 *
386 * @param mb Memory buffer
387 *
388 * @return 32-bit value
389 */
mbuf_read_u32(struct mbuf * mb)390 uint32_t mbuf_read_u32(struct mbuf *mb)
391 {
392 uint32_t v;
393
394 return (0 == mbuf_read_mem(mb, (uint8_t *)&v, sizeof(v))) ? v : 0;
395 }
396
397
398 /**
399 * Read a 64-bit value from a memory buffer
400 *
401 * @param mb Memory buffer
402 *
403 * @return 64-bit value
404 */
mbuf_read_u64(struct mbuf * mb)405 uint64_t mbuf_read_u64(struct mbuf *mb)
406 {
407 uint64_t v;
408
409 return (0 == mbuf_read_mem(mb, (uint8_t *)&v, sizeof(v))) ? v : 0;
410 }
411
412
413 /**
414 * Read a string from a memory buffer
415 *
416 * @param mb Memory buffer
417 * @param str Buffer to read string to
418 * @param size Size of buffer
419 *
420 * @return 0 if success, otherwise errorcode
421 */
mbuf_read_str(struct mbuf * mb,char * str,size_t size)422 int mbuf_read_str(struct mbuf *mb, char *str, size_t size)
423 {
424 if (!mb || !str)
425 return EINVAL;
426
427 while (size--) {
428 const uint8_t c = mbuf_read_u8(mb);
429 *str++ = c;
430 if ('\0' == c)
431 break;
432 }
433
434 return 0;
435 }
436
437
438 /**
439 * Duplicate a null-terminated string from a memory buffer
440 *
441 * @param mb Memory buffer
442 * @param strp Pointer to destination string; allocated and set
443 * @param len Length of string
444 *
445 * @return 0 if success, otherwise errorcode
446 */
mbuf_strdup(struct mbuf * mb,char ** strp,size_t len)447 int mbuf_strdup(struct mbuf *mb, char **strp, size_t len)
448 {
449 char *str;
450 int err;
451
452 if (!mb || !strp)
453 return EINVAL;
454
455 str = mem_alloc(len + 1, NULL);
456 if (!str)
457 return ENOMEM;
458
459 err = mbuf_read_mem(mb, (uint8_t *)str, len);
460 if (err)
461 goto out;
462
463 str[len] = '\0';
464
465 out:
466 if (err)
467 mem_deref(str);
468 else
469 *strp = str;
470
471 return err;
472 }
473
474
vprintf_handler(const char * p,size_t size,void * arg)475 static int vprintf_handler(const char *p, size_t size, void *arg)
476 {
477 struct mbuf *mb = arg;
478
479 return mbuf_write_mem(mb, (const uint8_t *)p, size);
480 }
481
482
483 /**
484 * Print a formatted variable argument list to a memory buffer
485 *
486 * @param mb Memory buffer
487 * @param fmt Formatted string
488 * @param ap Variable argument list
489 *
490 * @return 0 if success, otherwise errorcode
491 */
mbuf_vprintf(struct mbuf * mb,const char * fmt,va_list ap)492 int mbuf_vprintf(struct mbuf *mb, const char *fmt, va_list ap)
493 {
494 return re_vhprintf(fmt, ap, vprintf_handler, mb);
495 }
496
497
498 /**
499 * Print a formatted string to a memory buffer
500 *
501 * @param mb Memory buffer
502 * @param fmt Formatted string
503 *
504 * @return 0 if success, otherwise errorcode
505 */
mbuf_printf(struct mbuf * mb,const char * fmt,...)506 int mbuf_printf(struct mbuf *mb, const char *fmt, ...)
507 {
508 int err = 0;
509 va_list ap;
510
511 va_start(ap, fmt);
512 err = re_vhprintf(fmt, ap, vprintf_handler, mb);
513 va_end(ap);
514
515 return err;
516 }
517
518
519 /**
520 * Write a pointer-length string to a memory buffer, excluding a section
521 *
522 * @param mb Memory buffer
523 * @param pl Pointer-length string
524 * @param skip Part of pl to exclude
525 *
526 * @return 0 if success, otherwise errorcode
527 *
528 * @todo: create substf variante
529 */
mbuf_write_pl_skip(struct mbuf * mb,const struct pl * pl,const struct pl * skip)530 int mbuf_write_pl_skip(struct mbuf *mb, const struct pl *pl,
531 const struct pl *skip)
532 {
533 struct pl r;
534 int err;
535
536 if (!pl || !skip)
537 return EINVAL;
538
539 if (pl->p > skip->p || (skip->p + skip->l) > (pl->p + pl->l))
540 return ERANGE;
541
542 r.p = pl->p;
543 r.l = skip->p - pl->p;
544
545 err = mbuf_write_mem(mb, (const uint8_t *)r.p, r.l);
546 if (err)
547 return err;
548
549 r.p = skip->p + skip->l;
550 r.l = pl->p + pl->l - r.p;
551
552 return mbuf_write_mem(mb, (const uint8_t *)r.p, r.l);
553 }
554
555
556 /**
557 * Write n bytes of value 'c' to a memory buffer
558 *
559 * @param mb Memory buffer
560 * @param c Value to write
561 * @param n Number of bytes to write
562 *
563 * @return 0 if success, otherwise errorcode
564 */
mbuf_fill(struct mbuf * mb,uint8_t c,size_t n)565 int mbuf_fill(struct mbuf *mb, uint8_t c, size_t n)
566 {
567 size_t rsize;
568
569 if (!mb || !n)
570 return EINVAL;
571
572 rsize = mb->pos + n;
573
574 if (rsize > mb->size) {
575 const size_t dsize = mb->size ? (mb->size * 2)
576 : DEFAULT_SIZE;
577 int err;
578
579 err = mbuf_resize(mb, MAX(rsize, dsize));
580 if (err)
581 return err;
582 }
583
584 memset(mb->buf + mb->pos, c, n);
585
586 mb->pos += n;
587 mb->end = MAX(mb->end, mb->pos);
588
589 return 0;
590 }
591
592
593 /**
594 * Debug the memory buffer
595 *
596 * @param pf Print handler
597 * @param mb Memory buffer
598 *
599 * @return 0 if success, otherwise errorcode
600 */
mbuf_debug(struct re_printf * pf,const struct mbuf * mb)601 int mbuf_debug(struct re_printf *pf, const struct mbuf *mb)
602 {
603 if (!mb)
604 return 0;
605
606 return re_hprintf(pf, "buf=%p pos=%zu end=%zu size=%zu",
607 mb->buf, mb->pos, mb->end, mb->size);
608 }
609