diff --git a/py/ringbuf.c b/py/ringbuf.c index 83887b300..10dca6208 100644 --- a/py/ringbuf.c +++ b/py/ringbuf.c @@ -23,6 +23,9 @@ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ + +#include + #include "ringbuf.h" int ringbuf_get16(ringbuf_t *r) { @@ -71,3 +74,47 @@ int ringbuf_put16(ringbuf_t *r, uint16_t v) { r->iput = iput_b; return 0; } + +// Returns: +// 0: Success +// -1: Not enough data available to complete read (try again later) +// -2: Requested read is larger than buffer - will never succeed +int ringbuf_get_bytes(ringbuf_t *r, uint8_t *data, size_t data_len) { + if (ringbuf_avail(r) < data_len) { + return (r->size <= data_len) ? -2 : -1; + } + uint32_t iget = r->iget; + uint32_t iget_a = (iget + data_len) % r->size; + uint8_t *datap = data; + if (iget_a < iget) { + // Copy part of the data from the space left at the end of the buffer + memcpy(datap, r->buf + iget, r->size - iget); + datap += (r->size - iget); + iget = 0; + } + memcpy(datap, r->buf + iget, iget_a - iget); + r->iget = iget_a; + return 0; +} + +// Returns: +// 0: Success +// -1: Not enough free space available to complete write (try again later) +// -2: Requested write is larger than buffer - will never succeed +int ringbuf_put_bytes(ringbuf_t *r, const uint8_t *data, size_t data_len) { + if (ringbuf_free(r) < data_len) { + return (r->size <= data_len) ? -2 : -1; + } + uint32_t iput = r->iput; + uint32_t iput_a = (iput + data_len) % r->size; + const uint8_t *datap = data; + if (iput_a < iput) { + // Copy part of the data to the end of the buffer + memcpy(r->buf + iput, datap, r->size - iput); + datap += (r->size - iput); + iput = 0; + } + memcpy(r->buf + iput, datap, iput_a - iput); + r->iput = iput_a; + return 0; +} diff --git a/py/ringbuf.h b/py/ringbuf.h index 468584896..c8508c07e 100644 --- a/py/ringbuf.h +++ b/py/ringbuf.h @@ -96,4 +96,7 @@ int ringbuf_get16(ringbuf_t *r); int ringbuf_peek16(ringbuf_t *r); int ringbuf_put16(ringbuf_t *r, uint16_t v); +int ringbuf_get_bytes(ringbuf_t *r, uint8_t *data, size_t data_len); +int ringbuf_put_bytes(ringbuf_t *r, const uint8_t *data, size_t data_len); + #endif // MICROPY_INCLUDED_PY_RINGBUF_H