From 857f3c3c510ded552fe34b219003165d05776177 Mon Sep 17 00:00:00 2001
From: Francisco Molina <femolina@uc.cl>
Date: Mon, 22 Nov 2021 11:46:04 +0100
Subject: [PATCH] sys/tsrb: add peek functions

---
 sys/include/tsrb.h                      | 17 +++++++++++
 sys/tsrb/tsrb.c                         | 28 ++++++++++++++++++
 tests/unittests/tests-tsrb/tests-tsrb.c | 39 +++++++++++++++++++++++++
 3 files changed, 84 insertions(+)

diff --git a/sys/include/tsrb.h b/sys/include/tsrb.h
index 9d766242b8..04d8b26345 100644
--- a/sys/include/tsrb.h
+++ b/sys/include/tsrb.h
@@ -129,6 +129,14 @@ static inline unsigned int tsrb_free(const tsrb_t *rb)
  */
 int tsrb_get_one(tsrb_t *rb);
 
+/**
+ * @brief       Get a byte from ringbuffer, without removing it
+ * @param[in]   rb  Ringbuffer to operate on
+ * @return      >=0 byte that has been read
+ * @return      -1  if no byte available
+ */
+int tsrb_peek_one(tsrb_t *rb);
+
 /**
  * @brief       Get bytes from ringbuffer
  * @param[in]   rb  Ringbuffer to operate on
@@ -138,6 +146,15 @@ int tsrb_get_one(tsrb_t *rb);
  */
 int tsrb_get(tsrb_t *rb, uint8_t *dst, size_t n);
 
+/**
+ * @brief       Get bytes from ringbuffer, without removing them
+ * @param[in]   rb  Ringbuffer to operate on
+ * @param[out]  dst buffer to write to
+ * @param[in]   n   max number of bytes to write to @p dst
+ * @return      nr of bytes written to @p dst
+ */
+int tsrb_peek(tsrb_t *rb, uint8_t *dst, size_t n);
+
 /**
  * @brief       Drop bytes from ringbuffer
  * @param[in]   rb  Ringbuffer to operate on
diff --git a/sys/tsrb/tsrb.c b/sys/tsrb/tsrb.c
index 809b1df922..90bd72e2bc 100644
--- a/sys/tsrb/tsrb.c
+++ b/sys/tsrb/tsrb.c
@@ -30,6 +30,11 @@ static uint8_t _pop(tsrb_t *rb)
     return rb->buf[rb->reads++ & (rb->size - 1)];
 }
 
+static uint8_t _peek(tsrb_t *rb, unsigned int idx)
+{
+    return rb->buf[(rb->reads + idx) & (rb->size - 1)];
+}
+
 int tsrb_get_one(tsrb_t *rb)
 {
     int retval = -1;
@@ -41,6 +46,17 @@ int tsrb_get_one(tsrb_t *rb)
     return retval;
 }
 
+int tsrb_peek_one(tsrb_t *rb)
+{
+    int retval = -1;
+    unsigned irq_state = irq_disable();
+    if (!tsrb_empty(rb)) {
+        retval = _peek(rb, 0);
+    }
+    irq_restore(irq_state);
+    return retval;
+}
+
 int tsrb_get(tsrb_t *rb, uint8_t *dst, size_t n)
 {
     size_t tmp = n;
@@ -53,6 +69,18 @@ int tsrb_get(tsrb_t *rb, uint8_t *dst, size_t n)
     return (n - tmp);
 }
 
+int tsrb_peek(tsrb_t *rb, uint8_t *dst, size_t n)
+{
+    size_t idx = 0;
+    unsigned irq_state = irq_disable();
+    unsigned int avail = tsrb_avail(rb);
+    while (idx < n && idx != avail) {
+        *dst++ = _peek(rb, idx++);
+    }
+    irq_restore(irq_state);
+    return idx;
+}
+
 int tsrb_drop(tsrb_t *rb, size_t n)
 {
     size_t tmp = n;
diff --git a/tests/unittests/tests-tsrb/tests-tsrb.c b/tests/unittests/tests-tsrb/tests-tsrb.c
index 9cf7bd4b23..94d7a8745b 100644
--- a/tests/unittests/tests-tsrb/tests-tsrb.c
+++ b/tests/unittests/tests-tsrb/tests-tsrb.c
@@ -115,6 +115,43 @@ static void test_get(void)
     }
 }
 
+static void test_peek_one(void)
+{
+    int res;
+
+    TEST_ASSERT_EQUAL_INT(-1, tsrb_peek_one(&_tsrb));
+    TEST_ASSERT_EQUAL_INT(0, tsrb_add_one(&_tsrb, TEST_INPUT));
+    TEST_ASSERT_EQUAL_INT(TEST_INPUT, tsrb_peek_one(&_tsrb));
+    TEST_ASSERT_EQUAL_INT(TEST_INPUT, tsrb_peek_one(&_tsrb));
+    TEST_ASSERT_EQUAL_INT(TEST_INPUT, tsrb_get_one(&_tsrb));
+    TEST_ASSERT_EQUAL_INT(-1, tsrb_peek_one(&_tsrb));
+    TEST_ASSERT_EQUAL_INT(0, tsrb_add_one(&_tsrb, 0xff));
+    res = tsrb_peek_one(&_tsrb);
+    TEST_ASSERT_EQUAL_INT(0xff, res);
+    /* 0xff is -1 in signed int8_t */
+    TEST_ASSERT(-1 != res);
+}
+
+static void test_peek(void)
+{
+    TEST_ASSERT(BUFFER_SIZE < sizeof(_io_buffer));
+    TEST_ASSERT_EQUAL_INT(0, tsrb_peek(&_tsrb, _io_buffer,
+                                      sizeof(_io_buffer)));
+
+    for (int i = 0; i < BUFFER_SIZE; i++) {
+        TEST_ASSERT_EQUAL_INT(0, tsrb_add_one(&_tsrb, TEST_INPUT + i));
+    }
+    TEST_ASSERT_EQUAL_INT(BUFFER_SIZE, tsrb_peek(&_tsrb, _io_buffer,
+                                                sizeof(_io_buffer)));
+    for (int i = 0; i < BUFFER_SIZE; i++) {
+        TEST_ASSERT_EQUAL_INT((TEST_INPUT + i), _io_buffer[i]);
+    }
+    for (int i = BUFFER_SIZE; i < (int)sizeof(_io_buffer); i++) {
+        TEST_ASSERT_EQUAL_INT(IO_BUFFER_CANARY, _io_buffer[i]);
+    }
+    TEST_ASSERT_EQUAL_INT(BUFFER_SIZE, tsrb_avail(&_tsrb));
+}
+
 static void test_drop(void)
 {
     TEST_ASSERT(BUFFER_SIZE < sizeof(_io_buffer));
@@ -170,7 +207,9 @@ static Test *tests_tsrb_tests(void)
         new_TestFixture(test_full),
         new_TestFixture(test_free),
         new_TestFixture(test_get_one),
+        new_TestFixture(test_peek_one),
         new_TestFixture(test_get),
+        new_TestFixture(test_peek),
         new_TestFixture(test_drop),
         new_TestFixture(test_add_one),
         new_TestFixture(test_add),