[utils] Fix circular buffer and uavcan (#3544)

* [utils] Fix circular buffer length wrapping

* [uavcan] Fix payload length to buffer length

* [utils] Fix circular buffer drop

* [utils] Fix circular buffer drop function, and add some tests

---------

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

sw/airborne/arch/chibios/modules/uavcan/uavcan.c

@@ -119,27 +119,27 @@ static void uavcan_tx(void* p)
   while (true) {
     pprz_bsem_wait(&iface->bsem);
 
-    pprz_mtx_lock(&iface->mutex);
-
-
     // read the Tx FIFO to canard
     pprz_mtx_lock(&iface->tx_fifo_mutex);
     while(true) {
       struct uavcan_msg_header_t header;
-      int ret = circular_buffer_get(&iface->_tx_fifo, (uint8_t*)&header, sizeof(header));
+      int ret = circular_buffer_get(&iface->_tx_fifo, (uint8_t*)&header, sizeof(struct uavcan_msg_header_t));
       if(ret < 0) {break;}
-      if(header.payload_len >= UAVCAN_MSG_MAX_SIZE) {
+      if(((header.payload_len+7)/8) >= UAVCAN_MSG_MAX_SIZE) {
         chSysHalt("UAVCAN_MSG_MAX_SIZE too small");
       }
       ret = circular_buffer_get(&iface->_tx_fifo, msg_payload, UAVCAN_MSG_MAX_SIZE);
       if(ret < 0) {break;}
+      pprz_mtx_lock(&iface->mutex);
       canardBroadcast(&iface->canard,
                     header.data_type_signature,
                     header.data_type_id, &iface->transfer_id,
                     header.priority, msg_payload, header.payload_len);
+      pprz_mtx_unlock(&iface->mutex);
     }
     pprz_mtx_unlock(&iface->tx_fifo_mutex);
 
+    pprz_mtx_lock(&iface->mutex);
     for (const CanardCANFrame *txf = NULL; (txf = canardPeekTxQueue(&iface->canard)) != NULL;) {
       struct pprzcan_frame tx_msg;
       memcpy(tx_msg.data, txf->data, 8);
@@ -273,16 +273,16 @@ void uavcan_broadcast(struct uavcan_iface_t *iface, uint64_t data_type_signature
     .data_type_signature = data_type_signature,
     .data_type_id = data_type_id,
     .priority = priority,
-    .payload_len = payload_len
+    .payload_len = payload_len // in bits
   };
 
-  if(circular_buffer_put(&iface->_tx_fifo, (uint8_t*)&header, sizeof(header)) < 0) {
+  if(circular_buffer_put(&iface->_tx_fifo, (uint8_t*)&header, sizeof(struct uavcan_msg_header_t)) < 0) {
     // fail to post header
     pprz_mtx_unlock(&iface->tx_fifo_mutex);
     return;
   }
 
-  if(circular_buffer_put(&iface->_tx_fifo, payload, payload_len) < 0) {
+  if(circular_buffer_put(&iface->_tx_fifo, payload, (payload_len+7)/8) < 0) {
     // fail to post payload. Remove the header from the fifo
     circular_buffer_drop(&iface->_tx_fifo);
     pprz_mtx_unlock(&iface->tx_fifo_mutex);

sw/airborne/utils/circular_buffer.c

@@ -24,18 +24,19 @@ 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...
-  uint16_t* msg_len_p = (uint16_t*)&cb->_buf[cb->read_offset];
+  uint16_t msg_len_p = cb->_buf[cb->read_offset] | (cb->_buf[(cb->read_offset + 1) % cb->_buf_len] << 8);
   // output buffer too small
-  if (len < *msg_len_p) { 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_p + 2;
+  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 + 2;
+  uint8_t *start = cb->_buf + ((cb->read_offset + 2) % cb->_buf_len);
 
-  if (end_offset >= cb->read_offset + 2) {
-    memcpy(buf, start, *msg_len_p);
+  // No wrapping can be read in one go
+  if (end_offset >= ((cb->read_offset + 2) % cb->_buf_len)) {
+    memcpy(buf, start, msg_len_p);
   } else {
     size_t len1 = cb->_buf_len - (cb->read_offset + 2);
     size_t len2 = len - len1;
@@ -43,25 +44,16 @@ int circular_buffer_get(struct circular_buffer *cb, uint8_t *buf, size_t len)
     memcpy(buf + len1, cb->_buf, len2);
   }
 
-  int nb_bytes = *msg_len_p;
+  int nb_bytes = msg_len_p;
   cb->read_offset = end_offset;
   return nb_bytes;
 }
 
 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 - 2;
-  } else {
-    available = cb->_buf_len - (cb->write_offset - cb->read_offset) - 2;
-  }
+  size_t available = circular_buffer_available(cb);
 
-  /**
-   * len == available is invalid because it will cause
-   * write_offset to be equal to read_offset, which is considered an empty buffer.
-  */
-  if ((int)len >= available) {
+  if (len > available) {
     return CIR_ERROR_NO_SPACE_AVAILABLE;
   }
 
@@ -70,17 +62,26 @@ int circular_buffer_put(struct circular_buffer *cb, const uint8_t *buf, size_t l
     end_offset -= cb->_buf_len;
   }
 
-  
-  uint16_t* len_p = (uint16_t*)&cb->_buf[cb->write_offset];
-  *len_p = len;
+
+  // Write the length and wrap around if needed
+  cb->_buf[cb->write_offset] = len & 0xFF;
+  cb->_buf[(cb->write_offset + 1) % cb->_buf_len] = (len >> 8) & 0xFF;
+
+  // Write the date no wrapping needed
   if (end_offset > cb->write_offset) {
     memcpy(cb->_buf + cb->write_offset + 2, buf, len);
-  } else {
+  } 
+  // Wrapping data partially
+  else if ((cb->write_offset + 2) < cb->_buf_len) {
     size_t len1 = cb->_buf_len - (cb->write_offset + 2);
     size_t len2 = len - len1;
     memcpy(cb->_buf + cb->write_offset + 2, buf, len1);
     memcpy(cb->_buf, buf + len1, len2);
   }
+  // Only data is wrapped (starting at the beginning or 1 byte in for wrapped length)
+  else {
+    memcpy(&cb->_buf[(cb->write_offset + 2) % cb->_buf_len], buf, len);
+  }
 
   cb->write_offset = end_offset;
   return 0;
@@ -94,9 +95,9 @@ int circular_buffer_drop(struct circular_buffer *cb) {
   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;
+    record_head_offset = end_offset;
+    uint16_t msg_len_p = cb->_buf[record_head_offset] | (cb->_buf[(record_head_offset + 1) % cb->_buf_len] << 8);
+    end_offset = record_head_offset + msg_len_p + 2;
     if (end_offset >= cb->_buf_len) {
       end_offset -= cb->_buf_len;
     }
@@ -105,3 +106,20 @@ int circular_buffer_drop(struct circular_buffer *cb) {
   cb->write_offset = record_head_offset;
   return 0;
 }
+
+size_t circular_buffer_available(struct circular_buffer *cb) {
+  // write_offset == read_offset is considered an empty buffer. => 3= 2 (lenght) + 1 (margin)
+  int available = 0;
+  if (cb->read_offset > cb->write_offset) {
+    available = (int)cb->read_offset - (int)cb->write_offset - 3;
+  } else {
+    available = cb->_buf_len - ((int)cb->write_offset - (int)cb->read_offset) - 3;
+  }
+
+  return available > 0 ? (size_t)available : 0;
+}
+
+void circular_buffer_clear(struct circular_buffer *cb) {
+  cb->read_offset = 0;
+  cb->write_offset = 0;
+}

sw/airborne/utils/circular_buffer.h

@@ -65,4 +65,14 @@ int circular_buffer_put(struct circular_buffer *cb, const uint8_t *buf, size_t l
 /**
  * @brief Drop last inserted record
  */
-int circular_buffer_drop(struct circular_buffer *cb);
+int circular_buffer_drop(struct circular_buffer *cb);
+
+/**
+ * @brief Get the available sapce for the next buffer
+ */
+size_t circular_buffer_available(struct circular_buffer *cb);
+
+/**
+ * @brief Clear buffer
+ */
+void circular_buffer_clear(struct circular_buffer *cb);

tests/utils/Makefile

@@ -43,6 +43,8 @@ ifneq ($(TEST_VERBOSE), 0)
 VERBOSE = --verbose
 endif
 
+FLAGS=-Wall -g -O0
+
 all: test
 
 build_tests: $(TESTS)
@@ -54,7 +56,7 @@ test_circular_buffer.run: $(PAPARAZZI_SRC)/sw/airborne/utils/circular_buffer.c
 
 %.run: %.c
 	@echo BUILD $@
-	$(Q)$(CC) -I$(PAPARAZZI_SRC)/sw/airborne/utils -I$(PAPARAZZI_SRC)/sw/include -I$(PAPARAZZI_SRC)/tests/common $(USER_CFLAGS) $(PAPARAZZI_SRC)/tests/common/tap.c $^ -o $@
+	$(Q)$(CC) $(FLAGS) -I$(PAPARAZZI_SRC)/sw/airborne/utils -I$(PAPARAZZI_SRC)/sw/include -I$(PAPARAZZI_SRC)/tests/common $(USER_CFLAGS) $(PAPARAZZI_SRC)/tests/common/tap.c $^ -o $@
 
 clean:
 	$(Q)rm -f $(TESTS)

tests/utils/test_circular_buffer.c

@@ -10,78 +10,102 @@
 #include <string.h>
 #include "tap.h"
 
-
-uint8_t plop[10] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10};
-uint8_t toto[5] = {105, 104, 103, 102, 101};
-uint8_t azert[12] = {201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212};
-
-uint8_t bubu[20];
+uint8_t buffer[20];
 struct circular_buffer cbuf;
 
+
 int main(int argc __attribute_maybe_unused__, char **argv __attribute_maybe_unused__)
 {
+  uint8_t src[20];
+  for(int i=0; i<20; i++) {
+    src[i] = 'A' + i;
+  }
+  uint8_t dst[20];
 
   note("running circular buffer tests");
-  plan(15);
+  plan(23);
 
-  circular_buffer_init(&cbuf, bubu, sizeof(bubu));
-
-  uint8_t bout[18];
+  circular_buffer_init(&cbuf, buffer, sizeof(buffer));
 
   // get on an empty buffer
-  int ret = circular_buffer_get(&cbuf, bout, sizeof(bout));
+  int ret = circular_buffer_get(&cbuf, dst, sizeof(dst));
   ok(ret == CIR_ERROR_NO_MSG, "expected CIR_ERROR_NO_MSG, got %d", ret);
 
 
-  // put plop
-  ret = circular_buffer_put(&cbuf, plop, sizeof(plop));
+  // put 10 bytes. 12 bytes will be occupied, 8 bytes free, 5 available for the next buffer
+  ret = circular_buffer_put(&cbuf, src, 10);
+  size_t available = circular_buffer_available(&cbuf);
   ok(ret == 0, "expected 0, got %d\n", ret);
+  ok(available == 5, "expected 5 bytes available, got %lu", available);
 
-  // put toto
-  ret = circular_buffer_put(&cbuf, toto, sizeof(toto));
+
+  // put 3 bytes. 12+5=17/20 bytes occupied, 3 left. 0 available for the next buffer
+  ret = circular_buffer_put(&cbuf, src, 3);
+  available = circular_buffer_available(&cbuf);
   ok(ret == 0, "expected 0, got %d\n", ret);
-
-
-  // put azert : should fail
-  ret = circular_buffer_put(&cbuf, azert, sizeof(azert));
+  ok(available == 0, "expected 0 bytes available, got %lu", available);
+  // try puting 1 byte : should fail
+  ret = circular_buffer_put(&cbuf, src, 1);
   ok(ret == CIR_ERROR_NO_SPACE_AVAILABLE, "expected CIR_ERROR_NO_SPACE_AVAILABLE, got %d\n", ret);
 
-  // get first buffer: plop
-  ret = circular_buffer_get(&cbuf, bout, sizeof(bout));
-  ok(ret == sizeof(plop), "expected %ld, got %d", sizeof(plop), ret);
-  ok(memcmp(bout, plop, ret) == 0, "buffer corrupted");
-
-
-  // put azert. data should wrap around the buffer (8+15>20)
-  ret = circular_buffer_put(&cbuf, azert, sizeof(azert));
+  // drop last buffer
+  circular_buffer_drop(&cbuf);
+  // put 4 bytes. 12+6=18/20 bytes occupied, 2 left. 0 available for the next buffer
+  ret = circular_buffer_put(&cbuf, src, 4);
+  available = circular_buffer_available(&cbuf);
   ok(ret == 0, "expected 0, got %d\n", ret);
-
-  // get next buffer: toto
-  ret = circular_buffer_get(&cbuf, bout, sizeof(bout));
-  ok(ret == sizeof(toto), "expected %ld, got %d", sizeof(toto), ret);
-  ok(memcmp(bout, toto, ret) == 0, "buffer corrupted");
+  ok(available == 0, "expected 0 bytes available, got %lu", available);
+  // try puting 1 byte : should fail
+  ret = circular_buffer_put(&cbuf, src, 1);
+  ok(ret == CIR_ERROR_NO_SPACE_AVAILABLE, "expected CIR_ERROR_NO_SPACE_AVAILABLE, got %d\n", ret);
 
 
-  // put toto
-  ret = circular_buffer_put(&cbuf, toto, sizeof(toto));
+    // drop last buffer
+  circular_buffer_drop(&cbuf);
+  // put 5 bytes. 12+7=19/20 bytes occupied, 0 left. 0 available for the next buffer
+  ret = circular_buffer_put(&cbuf, src, 5);
+  available = circular_buffer_available(&cbuf);
+  ok(ret == 0, "expected 0, got %d\n", ret);
+  ok(available == 0, "expected 0 bytes available, got %lu", available);
+  // try puting 1 byte : should fail
+  ret = circular_buffer_put(&cbuf, src, 1);
+  ok(ret == CIR_ERROR_NO_SPACE_AVAILABLE, "expected CIR_ERROR_NO_SPACE_AVAILABLE, got %d\n", ret);
+
+
+
+  // get first buffer: 10 bytes
+  ret = circular_buffer_get(&cbuf, dst, sizeof(dst));
+  ok(ret == 10, "expected %ld, got %d", 10, ret);
+  ok(memcmp(dst, src, ret) == 0, "buffer corrupted");
+
+  available = circular_buffer_available(&cbuf);
+  ok(available == 10, "expected 10 bytes available, got %lu", available);
+
+  // put 10 bytes. len should wrap around the buffer. len: 1 byte at the end, 1 at the start, 
+  ret = circular_buffer_put(&cbuf, src, 10);
   ok(ret == 0, "expected 0, got %d\n", ret);
 
 
-  // get next buffer: azert
-  ret = circular_buffer_get(&cbuf, bout, sizeof(bout));
-  ok(ret == sizeof(azert), "expected %ld, got %d", sizeof(azert), ret);
-  ok(memcmp(bout, azert, ret) == 0, "buffer corrupted");
 
+  // get 5 bytes buffer
+  ret = circular_buffer_get(&cbuf, dst, sizeof(dst));
+  ok(ret == 5, "expected %ld, got %d", 5, ret);
+  ok(memcmp(dst, src, ret) == 0, "buffer corrupted");
 
-  // get next buffer: toto
-  ret = circular_buffer_get(&cbuf, bout, sizeof(bout));
-  ok(ret == sizeof(toto), "expected %ld, got %d", sizeof(toto), ret);
-  ok(memcmp(bout, toto, ret) == 0, "buffer corrupted");
+  // get 10 bytes buffer
+  ret = circular_buffer_get(&cbuf, dst, sizeof(dst));
+  ok(ret == 10, "expected %ld, got %d", 10, ret);
+  ok(memcmp(dst, src, ret) == 0, "buffer corrupted");
 
+  // put 12 bytes buffer, data will wrap around
+  // write_offset at 11, 2lenght+7data + 5data wrapped
+  ret = circular_buffer_put(&cbuf, src, 12);
+  ok(ret == 0, "expected 0, got %d\n", ret);
 
-  // get on an empty buffer
-  ret = circular_buffer_get(&cbuf, bout, sizeof(bout));
-  ok(ret == CIR_ERROR_NO_MSG, "expected CIR_ERROR_NO_MSG, got %d", ret);
+  // get 12 bytes buffer
+  ret = circular_buffer_get(&cbuf, dst, sizeof(dst));
+  ok(ret == 12, "expected %ld, got %d", 12, ret);
+  ok(memcmp(dst, src, ret) == 0, "buffer corrupted");
 
   return 0;
 }