[utils] Add circular buffer. (#3038)

sw/airborne/utils/circular_buffer.c

@@ -0,0 +1,86 @@
+/*
+ * General purpose circular buffer
+ *
+ * Copyright (C) 2021 Fabien-B <fabien-b@github.com> 
+ *
+ * This file is part of paparazzi. See LICENCE file.
+ */
+
+#include "circular_buffer.h"
+#include <string.h>
+
+
+void circular_buffer_init(struct circular_buffer *cb, uint8_t *buffer, size_t len)
+{
+  cb->_buf = buffer;
+  cb->_buf_len = len;
+  cb->read_offset = 0;
+  cb->write_offset = 0;
+}
+
+
+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];
+  // output buffer too small
+  if (len < msg_len) { return CIR_ERROR_BUFFER_TOO_SMALL; }
+
+  size_t end_offset = cb->read_offset + msg_len + 1;
+  if (end_offset >= cb->_buf_len) {
+    end_offset -= cb->_buf_len;
+  }
+  uint8_t *start = cb->_buf + cb->read_offset + 1;
+
+  if (end_offset > cb->read_offset + 1) {
+    memcpy(buf, start, msg_len);
+  } else {
+    size_t len1 = cb->_buf_len - (cb->read_offset + 1);
+    size_t len2 = len - len1;
+    memcpy(buf, start, len1);
+    memcpy(buf + len1, cb->_buf, len2);
+  }
+
+  int nb_bytes = msg_len;
+  cb->read_offset = end_offset;
+  return nb_bytes;
+}
+
+int circular_buffer_put(struct circular_buffer *cb, uint8_t *buf, size_t len)
+{
+  int available = 0;
+  if (cb->read_offset > cb->write_offset) {
+    available = cb->read_offset - cb->write_offset - 1;
+  } else {
+    available = cb->_buf_len - (cb->write_offset - cb->read_offset) - 1;
+  }
+
+  /**
+   * len == available is invalid because it will cause
+   * write_offset to ne 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;
+  if (end_offset >= cb->_buf_len) {
+    end_offset -= cb->_buf_len;
+  }
+
+  cb->_buf[cb->write_offset] = len;
+  if (end_offset > cb->write_offset) {
+    memcpy(cb->_buf + cb->write_offset + 1, buf, len);
+  } else {
+    size_t len1 = cb->_buf_len - (cb->write_offset + 1);
+    size_t len2 = len - len1;
+    memcpy(cb->_buf + cb->write_offset + 1, buf, len1);
+    memcpy(cb->_buf, buf + len1, len2);
+  }
+
+  cb->write_offset = end_offset;
+  return 0;
+}
+

sw/airborne/utils/circular_buffer.h

@@ -0,0 +1,61 @@
+/*
+ * General purpose circular buffer
+ *
+ * Copyright (C) 2021 Fabien-B <fabien-b@github.com> 
+ *
+ * This file is part of paparazzi. See LICENCE file.
+ */
+
+#pragma once
+
+#include <stdint.h>
+#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.
+ * 
+ * Declare a \ref circular_buffer and allocate a buffer that will outlive it.
+ * Initialize the \ref circular_buffer using \ref circular_buffer_init.
+ * 
+*/
+
+struct circular_buffer {
+  size_t read_offset;
+  size_t write_offset;
+  size_t _buf_len;
+  uint8_t *_buf;
+};
+
+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 */
+};
+
+/**
+ * @brief initialize a circular buffer.
+ * @param cb circular_buffer structure
+ * @param buffer buffer used internally by the circular buffer
+ * @param len size of \p buffer
+*/
+void circular_buffer_init(struct circular_buffer *cb, uint8_t *buffer, size_t len);
+
+
+/**
+ * @brief copy the next buffer available in \p cb to \p buf.
+ * @param cb The circular buffer
+ * @param buf destination buffer
+ * @param len size of \p buf
+ * @return Size of the data copied to \p buf, or an error code if negative.
+*/
+int circular_buffer_get(struct circular_buffer *cb, uint8_t *buf, size_t len);
+
+/**
+ * @brief Copy \p buf in the circular buffer
+ * @param cb The circular buffer
+ * @param buf source buffer
+ * @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);

tests/Makefile

@@ -27,6 +27,7 @@ endif
 
 test:
 	$(Q)make -C math test
+	$(Q)make -C utils test
 	$(Q)$(PERLENV) $(PERL) "-e" "$(RUNTESTS)"
 
 test_modules:

tests/math/Makefile

@@ -61,7 +61,7 @@ test_state_interface.run: $(PAPARAZZI_SRC)/sw/airborne/state.c
 
 %.run: %.c | math_shlib
 	@echo BUILD $@
-	$(Q)$(CC) -L$(MATHLIB_PATH) -I$(PAPARAZZI_SRC)/sw/airborne -I$(PAPARAZZI_SRC)/sw/include $(USER_CFLAGS) tap.c $^ -lpprzmath -lm -o $@
+	$(Q)$(CC) -L$(MATHLIB_PATH) -I$(PAPARAZZI_SRC)/sw/airborne -I$(PAPARAZZI_SRC)/sw/include -I$(PAPARAZZI_SRC)/tests/common $(USER_CFLAGS) $(PAPARAZZI_SRC)/tests/common/tap.c $^ -lpprzmath -lm -o $@
 
 clean:
 	$(Q)rm -f $(MATHLIB_PATH)/*.o $(MATHLIB_PATH)/libpprzmath.so

tests/utils/Makefile

@@ -0,0 +1,63 @@
+# Copyright (C) 2014 Piotr Esden-Tempski
+#
+# This file is part of paparazzi.
+#
+# paparazzi is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2, or (at your option)
+# any later version.
+#
+# paparazzi is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with paparazzi; see the file COPYING.  If not, see
+# <http://www.gnu.org/licenses/>.
+
+# The default is to produce a quiet echo of compilation commands
+# Launch with "make Q=''" to get full echo
+
+# Make sure all our environment is set properly in case we run make not from toplevel director.
+Q ?= @
+
+PAPARAZZI_SRC ?= $(shell pwd)/../..
+ifeq ($(PAPARAZZI_HOME),)
+PAPARAZZI_HOME=$(PAPARAZZI_SRC)
+endif
+
+# export the PAPARAZZI environment to sub-make
+export PAPARAZZI_SRC
+export PAPARAZZI_HOME
+
+#####################################################
+# If you add more test files you add their names here
+TESTS = test_circular_buffer.run
+
+###################################################
+# You should not need to touch the rest of the file
+
+TEST_VERBOSE ?= 0
+ifneq ($(TEST_VERBOSE), 0)
+VERBOSE = --verbose
+endif
+
+all: test
+
+build_tests: $(TESTS)
+
+test: build_tests
+	prove $(VERBOSE) --exec '' ./*.run
+
+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 $@
+
+clean:
+	$(Q)rm -f $(TESTS)
+
+
+.PHONY: build_tests test clean all

tests/utils/test_circular_buffer.c

@@ -0,0 +1,87 @@
+/*
+ * Copyright (C) 2021 Fabien-B <fabien-b@github.com> 
+ *
+ * This file is part of paparazzi. See LICENCE file.
+ */
+
+#include "stdio.h"
+#include "stdlib.h"
+#include "circular_buffer.h"
+#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];
+struct circular_buffer cbuf;
+
+int main(int argc __attribute_maybe_unused__, char **argv __attribute_maybe_unused__)
+{
+
+  note("running circular buffer tests");
+  plan(15);
+
+  circular_buffer_init(&cbuf, bubu, sizeof(bubu));
+
+  uint8_t bout[18];
+
+  // get on an empty buffer
+  int ret = circular_buffer_get(&cbuf, bout, sizeof(bout));
+  ok(ret == CIR_ERROR_NO_MSG, "expected CIR_ERROR_NO_MSG, got %d", ret);
+
+
+  // put plop
+  ret = circular_buffer_put(&cbuf, plop, sizeof(plop));
+  ok(ret == 0, "expected 0, got %d\n", ret);
+
+  // put toto
+  ret = circular_buffer_put(&cbuf, toto, sizeof(toto));
+  ok(ret == 0, "expected 0, got %d\n", ret);
+
+
+  // put azert : should fail
+  ret = circular_buffer_put(&cbuf, azert, sizeof(azert));
+  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));
+  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");
+
+
+  // put toto
+  ret = circular_buffer_put(&cbuf, toto, sizeof(toto));
+  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 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 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);
+
+  return 0;
+}