diff --git a/include/re_mbuf.h b/include/re_mbuf.h index 2d2ff1d..feac6fd 100644 --- a/include/re_mbuf.h +++ b/include/re_mbuf.h @@ -46,6 +46,7 @@ void mbuf_init(struct mbuf *mb); void mbuf_reset(struct mbuf *mb); int mbuf_resize(struct mbuf *mb, size_t size); void mbuf_trim(struct mbuf *mb); +int mbuf_shift(struct mbuf *mb, ssize_t shift); int mbuf_write_mem(struct mbuf *mb, const uint8_t *buf, size_t size); int mbuf_write_u8(struct mbuf *mb, uint8_t v); int mbuf_write_u16(struct mbuf *mb, uint16_t v); diff --git a/src/mbuf/mbuf.c b/src/mbuf/mbuf.c index e0ba1c7..6fe47bd 100644 --- a/src/mbuf/mbuf.c +++ b/src/mbuf/mbuf.c @@ -153,6 +153,48 @@ void mbuf_trim(struct mbuf *mb) } +/** + * Shift mbuf content position + * + * @param mb Memory buffer to shift + * @param shift Shift offset count + * + * @return 0 if success, otherwise errorcode + */ +int mbuf_shift(struct mbuf *mb, ssize_t shift) +{ + size_t rsize; + uint8_t *p; + + if (!mb) + return EINVAL; + + if (((ssize_t)mb->pos + shift) < 0 || + ((ssize_t)mb->end + shift) < 0) + return ERANGE; + + rsize = mb->end + shift; + + if (rsize > mb->size) { + + int err; + + err = mbuf_resize(mb, rsize); + if (err) + return err; + } + + p = mbuf_buf(mb); + + memmove(p + shift, p, mbuf_get_left(mb)); + + mb->pos += shift; + mb->end += shift; + + return 0; +} + + /** * Write a block of memory to a memory buffer *