diff --git a/include/mgos_utils.h b/include/mgos_utils.h index dab06e986..196ee8e48 100644 --- a/include/mgos_utils.h +++ b/include/mgos_utils.h @@ -38,6 +38,9 @@ void mgos_system_restart_after(int delay_ms); /* Return random number in a given range. */ float mgos_rand_range(float from, float to); +/* Removes 'data_size' bytes from index of the buffer */ +size_t mbuf_remove_range(struct mbuf *, size_t index, size_t data_size); + #ifdef __cplusplus } #endif diff --git a/src/mgos_utils.c b/src/mgos_utils.c index 57cb7d2d7..4bb36ed20 100644 --- a/src/mgos_utils.c +++ b/src/mgos_utils.c @@ -43,6 +43,22 @@ float mgos_rand_range(float from, float to) { return from + (((float) (to - from)) / RAND_MAX * rand()); } + +size_t mbuf_remove_range(struct mbuf *mb, size_t i, size_t n) WEAK; +size_t mbuf_remove_range(struct mbuf *mb, size_t i, size_t n) { + size_t ret = 0; + if (n > 0 && n <= (mb->len - i)) { + memmove(&mb->buf[i], &mb->buf[i] + n, mb->len - n); + mb->len -= n; + ret = n; + } + else if (n > 0 && n >= (mb->len - i)) { + ret = mb->len - i; + mb->len -= ret; + } + return ret; +} + #if CS_ENABLE_STDIO /* * Intended for ffi