py/ringbuf: Implement put_bytes/get_bytes functions.

2 files changed, 50 insertions, 0 deletions

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 <string.h>
+
 #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