[circular buffer] store size as uint16_t, add drop function. (#3512)

Co-authored-by: Fabien-B <Fabien-B@github.com>

sw/airborne/utils/circular_buffer.c

@@ -24,59 +24,61 @@ int circular_buffer_get(struct circular_buffer *cb, uint8_t *buf, size_t len)
   // buffer empty
   if (cb->read_offset == cb->write_offset) { return CIR_ERROR_NO_MSG; }
   // LEN| MSG...| LEN | MSG...
-  uint8_t msg_len = cb->_buf[cb->read_offset];
+  uint16_t* msg_len_p = (uint16_t*)&cb->_buf[cb->read_offset];
   // output buffer too small
-  if (len < msg_len) { return CIR_ERROR_BUFFER_TOO_SMALL; }
+  if (len < *msg_len_p) { return CIR_ERROR_BUFFER_TOO_SMALL; }
 
-  size_t end_offset = cb->read_offset + msg_len + 1;
+  size_t end_offset = cb->read_offset + *msg_len_p + 2;
   if (end_offset >= cb->_buf_len) {
     end_offset -= cb->_buf_len;
   }
-  uint8_t *start = cb->_buf + cb->read_offset + 1;
+  uint8_t *start = cb->_buf + cb->read_offset + 2;
 
-  if (end_offset > cb->read_offset + 1) {
-    memcpy(buf, start, msg_len);
+  if (end_offset > cb->read_offset + 2) {
+    memcpy(buf, start, *msg_len_p);
   } else {
-    size_t len1 = cb->_buf_len - (cb->read_offset + 1);
+    size_t len1 = cb->_buf_len - (cb->read_offset + 2);
     size_t len2 = len - len1;
     memcpy(buf, start, len1);
     memcpy(buf + len1, cb->_buf, len2);
   }
 
-  int nb_bytes = msg_len;
+  int nb_bytes = *msg_len_p;
   cb->read_offset = end_offset;
   return nb_bytes;
 }
 
-int circular_buffer_put(struct circular_buffer *cb, uint8_t *buf, size_t len)
+int circular_buffer_put(struct circular_buffer *cb, const uint8_t *buf, size_t len)
 {
   int available = 0;
   if (cb->read_offset > cb->write_offset) {
-    available = cb->read_offset - cb->write_offset - 1;
+    available = cb->read_offset - cb->write_offset - 2;
   } else {
-    available = cb->_buf_len - (cb->write_offset - cb->read_offset) - 1;
+    available = cb->_buf_len - (cb->write_offset - cb->read_offset) - 2;
   }
 
   /**
    * len == available is invalid because it will cause
-   * write_offset to ne equal to read_offset, which is considered an empty buffer.
+   * write_offset to be equal to read_offset, which is considered an empty buffer.
   */
   if ((int)len >= available) {
     return CIR_ERROR_NO_SPACE_AVAILABLE;
   }
 
-  size_t end_offset = cb->write_offset + len + 1;
+  size_t end_offset = cb->write_offset + len + 2;
   if (end_offset >= cb->_buf_len) {
     end_offset -= cb->_buf_len;
   }
 
-  cb->_buf[cb->write_offset] = len;
+  
+  uint16_t* len_p = (uint16_t*)&cb->_buf[cb->write_offset];
+  *len_p = len;
   if (end_offset > cb->write_offset) {
-    memcpy(cb->_buf + cb->write_offset + 1, buf, len);
+    memcpy(cb->_buf + cb->write_offset + 2, buf, len);
   } else {
-    size_t len1 = cb->_buf_len - (cb->write_offset + 1);
+    size_t len1 = cb->_buf_len - (cb->write_offset + 2);
     size_t len2 = len - len1;
-    memcpy(cb->_buf + cb->write_offset + 1, buf, len1);
+    memcpy(cb->_buf + cb->write_offset + 2, buf, len1);
     memcpy(cb->_buf, buf + len1, len2);
   }
 
@@ -84,3 +86,22 @@ int circular_buffer_put(struct circular_buffer *cb, uint8_t *buf, size_t len)
   return 0;
 }
 
+int circular_buffer_drop(struct circular_buffer *cb) {
+  // buffer empty
+  if (cb->read_offset == cb->write_offset) { return CIR_ERROR_NO_MSG; }
+
+  size_t record_head_offset = cb->read_offset;
+  size_t end_offset = cb->read_offset;
+
+  while(end_offset != cb->write_offset) {
+    size_t record_head_offset = end_offset;
+    uint16_t* msg_len_p = (uint16_t*)&cb->_buf[record_head_offset];
+    size_t end_offset = record_head_offset + *msg_len_p + 2;
+    if (end_offset >= cb->_buf_len) {
+      end_offset -= cb->_buf_len;
+    }
+  }
+
+  cb->write_offset = record_head_offset;
+  return 0;
+}

sw/airborne/utils/circular_buffer.h

@@ -12,8 +12,8 @@
 #include <stddef.h>
 
 /**
- * This is a general purpose circular buffer for storing buffers in a FIFO order.
- * A current limitation: the size of the buffers is limited to 255, is size beeing stored on a uint8_t.
+ * This is a general purpose circular buffer for storing variable lenght buffers in a FIFO order.
+ * Buffers length are stored as uint16_t.
  * 
  * Declare a \ref circular_buffer and allocate a buffer that will outlive it.
  * Initialize the \ref circular_buffer using \ref circular_buffer_init.
@@ -31,6 +31,7 @@ enum cir_error {
   CIR_ERROR_NO_MSG = -1,                /**< circular buffer is empty */
   CIR_ERROR_BUFFER_TOO_SMALL = -2,      /**< destination buffer is too small */
   CIR_ERROR_NO_SPACE_AVAILABLE = -3,    /**< no space available in the circular buffer */
+  CIR_ERROR_LOCKED = -4,                /**< mutex locked */
 };
 
 /**
@@ -58,4 +59,10 @@ int circular_buffer_get(struct circular_buffer *cb, uint8_t *buf, size_t len);
  * @param len Size of \p buf
  * @return 0 on success, Error code if negative
 */
-int circular_buffer_put(struct circular_buffer *cb, uint8_t *buf, size_t len);
+int circular_buffer_put(struct circular_buffer *cb, const uint8_t *buf, size_t len);
+
+
+/**
+ * @brief Drop last inserted record
+ */
+int circular_buffer_drop(struct circular_buffer *cb);