lib: add FIFO ringbuffer class

This adds a reusable class for a simple FIFO ringbuffer.

Signed-off-by: Julian Oes <julian@oes.ch>

src/lib/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2017 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2017-2023 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -62,6 +62,7 @@ add_subdirectory(pid EXCLUDE_FROM_ALL)
 add_subdirectory(pid_design EXCLUDE_FROM_ALL)
 add_subdirectory(rate_control EXCLUDE_FROM_ALL)
 add_subdirectory(rc EXCLUDE_FROM_ALL)
+add_subdirectory(ringbuffer EXCLUDE_FROM_ALL)
 add_subdirectory(sensor_calibration EXCLUDE_FROM_ALL)
 add_subdirectory(slew_rate EXCLUDE_FROM_ALL)
 add_subdirectory(systemlib EXCLUDE_FROM_ALL)

src/lib/ringbuffer/CMakeLists.txt

@@ -0,0 +1,40 @@
+############################################################################
+#
+#   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(ringbuffer
+	Ringbuffer.cpp
+)
+
+target_include_directories(ringbuffer PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+px4_add_unit_gtest(SRC RingbufferTest.cpp LINKLIBS ringbuffer)

src/lib/ringbuffer/Ringbuffer.cpp

@@ -0,0 +1,180 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+
+
+
+#include "Ringbuffer.hpp"
+
+#include <mathlib/mathlib.h>
+#include <assert.h>
+#include <string.h>
+
+
+Ringbuffer::~Ringbuffer()
+{
+	deallocate();
+}
+
+bool Ringbuffer::allocate(size_t buffer_size)
+{
+	assert(_ringbuffer == nullptr);
+
+	_size = buffer_size;
+	_ringbuffer = new uint8_t[_size];
+	return _ringbuffer != nullptr;
+}
+
+void Ringbuffer::deallocate()
+{
+	delete[] _ringbuffer;
+	_ringbuffer = nullptr;
+	_size = 0;
+}
+
+size_t Ringbuffer::space_available() const
+{
+	if (_start > _end) {
+		return _start - _end - 1;
+
+	} else {
+		return _start - _end - 1 + _size;
+	}
+}
+
+size_t Ringbuffer::space_used() const
+{
+	if (_start <= _end) {
+		return _end - _start;
+
+	} else {
+		// Potential wrap around.
+		return _end - _start + _size;
+	}
+}
+
+
+bool Ringbuffer::push_back(const uint8_t *buf, size_t buf_len)
+{
+	if (buf_len == 0 || buf == nullptr) {
+		// Nothing to add, we better don't try.
+		return false;
+	}
+
+	if (_start > _end) {
+		// Add after end up to start, no wrap around.
+
+		// Leave one byte free so that start don't end up the same
+		// which signals empty.
+		const size_t available = _start - _end - 1;
+
+		if (available < buf_len) {
+			return false;
+		}
+
+		memcpy(&_ringbuffer[_end], buf, buf_len);
+		_end += buf_len;
+
+	} else {
+		// Add after end, maybe wrap around.
+		const size_t available = _start - _end - 1 + _size;
+
+		if (available < buf_len) {
+			return false;
+		}
+
+		const size_t remaining_packet_len = _size - _end;
+
+		if (buf_len > remaining_packet_len) {
+			memcpy(&_ringbuffer[_end], buf, remaining_packet_len);
+			_end = 0;
+
+			memcpy(&_ringbuffer[_end], buf + remaining_packet_len, buf_len - remaining_packet_len);
+			_end += buf_len - remaining_packet_len;
+
+		} else {
+			memcpy(&_ringbuffer[_end], buf, buf_len);
+			_end += buf_len;
+		}
+	}
+
+	return true;
+}
+
+size_t Ringbuffer::pop_front(uint8_t *buf, size_t buf_max_len)
+{
+	if (buf == nullptr) {
+		// User needs to supply a valid pointer.
+		return 0;
+	}
+
+	if (_start == _end) {
+		// Empty
+		return 0;
+	}
+
+	if (_start < _end) {
+
+		// No wrap around.
+		size_t to_copy_len = math::min(_end - _start, buf_max_len);
+
+		memcpy(buf, &_ringbuffer[_start], to_copy_len);
+		_start += to_copy_len;
+
+		return to_copy_len;
+
+	} else {
+		// Potential wrap around.
+		size_t to_copy_len = _end - _start + _size;
+
+		if (to_copy_len > buf_max_len) {
+			to_copy_len = buf_max_len;
+		}
+
+		const size_t remaining_buf_len = _size - _start;
+
+		if (to_copy_len > remaining_buf_len) {
+
+			memcpy(buf, &_ringbuffer[_start], remaining_buf_len);
+			_start = 0;
+			memcpy(buf + remaining_buf_len, &_ringbuffer[_start], to_copy_len - remaining_buf_len);
+			_start += to_copy_len - remaining_buf_len;
+
+		} else {
+			memcpy(buf, &_ringbuffer[_start], to_copy_len);
+			_start += to_copy_len;
+		}
+
+		return to_copy_len;
+	}
+}

src/lib/ringbuffer/Ringbuffer.hpp

@@ -0,0 +1,120 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+
+
+#pragma once
+
+#include <stdint.h>
+#include <pthread.h>
+
+
+// FIFO ringbuffer implementation.
+//
+// The ringbuffer can store up 1 byte less than allocated as
+// start and end marker need to be one byte apart when the buffer
+// is full, otherwise it would suddenly be empty.
+//
+// The buffer is not thread-safe.
+
+class Ringbuffer
+{
+public:
+	/* @brief Constructor
+	 *
+	 * @note Does not allocate automatically.
+	 */
+	Ringbuffer() = default;
+
+	/*
+	 * @brief Destructor
+	 *
+	 * Automatically calls deallocate.
+	 */
+	~Ringbuffer();
+
+	/* @brief Allocate ringbuffer
+	 *
+	 * @param buffer_size Number of bytes to allocate on heap.
+	 *
+	 * @returns false if allocation fails.
+	 */
+	bool allocate(size_t buffer_size);
+
+	/*
+	 * @brief Deallocate ringbuffer
+	 *
+	 * @note only required to deallocate and reallocate again.
+	 */
+	void deallocate();
+
+	/*
+	 * @brief Space available to copy bytes into
+	 *
+	 * @returns number of free bytes.
+	 */
+	size_t space_available() const;
+
+	/*
+	 * @brief Space used to copy data from
+	 *
+	 * @returns number of used bytes.
+	 */
+	size_t space_used() const;
+
+	/*
+	 * @brief Copy data into ringbuffer
+	 *
+	 * @param buf Pointer to buffer to copy from.
+	 * @param buf_len Number of bytes to copy.
+	 *
+	 * @returns true if packet could be copied into buffer.
+	 */
+	bool push_back(const uint8_t *buf, size_t buf_len);
+
+	/*
+	 * @brief Get data from ringbuffer
+	 *
+	 * @param buf Pointer to buffer where data can be copied into.
+	 * @param max_buf_len Max number of bytes to copy.
+	 *
+	 * @returns 0 if buffer is empty.
+	 */
+	size_t pop_front(uint8_t *buf, size_t max_buf_len);
+
+private:
+	size_t _size {0};
+	uint8_t *_ringbuffer {nullptr};
+	size_t _start{0};
+	size_t _end{0};
+};

src/lib/ringbuffer/RingbufferTest.cpp

@@ -0,0 +1,247 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <gtest/gtest.h>
+#include <stdint.h>
+#include <string.h>
+
+
+#include "Ringbuffer.hpp"
+
+class TempData
+{
+public:
+	TempData(size_t len)
+	{
+		_size = len;
+		_buf = new uint8_t[_size];
+	}
+
+	~TempData()
+	{
+		delete[] _buf;
+		_buf = nullptr;
+	}
+
+	uint8_t *buf() const
+	{
+		return _buf;
+	}
+
+	size_t size() const
+	{
+		return _size;
+	}
+
+	void paint(unsigned offset = 0)
+	{
+		for (size_t i = 0; i < _size; ++i) {
+			_buf[i] = (uint8_t)((i + offset) % UINT8_MAX);
+		}
+	}
+
+private:
+	uint8_t *_buf {nullptr};
+	size_t _size{0};
+
+};
+
+bool operator==(const TempData &lhs, const TempData &rhs)
+{
+	if (lhs.size() != rhs.size()) {
+		return false;
+	}
+
+	return memcmp(lhs.buf(), rhs.buf(), lhs.size()) == 0;
+}
+
+
+TEST(Ringbuffer, AllocateAndDeallocate)
+{
+	Ringbuffer buf;
+	ASSERT_TRUE(buf.allocate(100));
+	buf.deallocate();
+
+	ASSERT_TRUE(buf.allocate(1000));
+	// The second time we forget to clean up, but we expect no leak.
+}
+
+TEST(Ringbuffer, PushATooBigMessage)
+{
+	Ringbuffer buf;
+	ASSERT_TRUE(buf.allocate(100));
+
+	TempData data{200};
+
+	// A message that doesn't fit should get rejected.
+	EXPECT_FALSE(buf.push_back(data.buf(), data.size()));
+}
+
+TEST(Ringbuffer, PushAndPopOne)
+{
+	Ringbuffer buf;
+	ASSERT_TRUE(buf.allocate(100));
+
+	TempData data{20};
+	data.paint();
+
+	EXPECT_TRUE(buf.push_back(data.buf(), data.size()));
+
+	EXPECT_EQ(buf.space_used(), 20);
+	EXPECT_EQ(buf.space_available(), 79);
+
+	// Get everything
+	TempData out{20};
+	EXPECT_EQ(buf.pop_front(out.buf(), out.size()), 20);
+	EXPECT_EQ(data, out);
+
+	// Nothing remaining
+	EXPECT_EQ(buf.pop_front(out.buf(), out.size()), 0);
+}
+
+TEST(Ringbuffer, PushAndPopSeveral)
+{
+	Ringbuffer buf;
+	ASSERT_TRUE(buf.allocate(100));
+
+	TempData data{90};
+	data.paint();
+
+	// 9 little chunks in
+	for (unsigned i = 0; i < 9; ++i) {
+		EXPECT_TRUE(buf.push_back(data.buf() + i * 10, 10));
+	}
+
+	// 10 won't because of overhead inside the buffer
+	EXPECT_FALSE(buf.push_back(data.buf(), 10));
+
+	TempData out{90};
+	// Take it back out in 2 big steps
+	EXPECT_EQ(buf.pop_front(out.buf(), 50), 50);
+	EXPECT_EQ(buf.pop_front(out.buf() + 50, 40), 40);
+	EXPECT_EQ(data, out);
+}
+
+TEST(Ringbuffer, PushAndTryToPopMore)
+{
+	Ringbuffer buf;
+	ASSERT_TRUE(buf.allocate(100));
+
+	TempData data1{50};
+	data1.paint();
+	EXPECT_TRUE(buf.push_back(data1.buf(), data1.size()));
+
+	TempData out1{80};
+	EXPECT_EQ(buf.pop_front(out1.buf(), out1.size()), data1.size());
+}
+
+TEST(Ringbuffer, PushAndPopSeveralInterleaved)
+{
+	Ringbuffer buf;
+	ASSERT_TRUE(buf.allocate(100));
+
+	TempData data1{50};
+	data1.paint();
+	EXPECT_TRUE(buf.push_back(data1.buf(), data1.size()));
+
+	TempData data2{30};
+	data2.paint(50);
+	EXPECT_TRUE(buf.push_back(data2.buf(), data2.size()));
+
+	TempData out12{80};
+	EXPECT_EQ(buf.pop_front(out12.buf(), out12.size()), out12.size());
+
+	TempData out12_ref{80};
+	out12_ref.paint();
+	EXPECT_EQ(out12_ref, out12);
+
+	TempData data3{50};
+	data3.paint(33);
+	EXPECT_TRUE(buf.push_back(data3.buf(), data3.size()));
+
+	TempData out3{50};
+	EXPECT_EQ(buf.pop_front(out3.buf(), out3.size()), data3.size());
+	EXPECT_EQ(data3, out3);
+}
+
+TEST(Ringbuffer, PushEmpty)
+{
+	Ringbuffer buf;
+	ASSERT_TRUE(buf.allocate(100));
+
+	EXPECT_FALSE(buf.push_back(nullptr, 0));
+}
+
+TEST(Ringbuffer, PopWithoutBuffer)
+{
+	Ringbuffer buf;
+	ASSERT_TRUE(buf.allocate(100));
+
+	EXPECT_FALSE(buf.push_back(nullptr, 0));
+
+	TempData data{50};
+	data.paint();
+
+	EXPECT_TRUE(buf.push_back(data.buf(), data.size()));
+
+
+	EXPECT_EQ(buf.pop_front(nullptr, 50), 0);
+}
+
+TEST(Ringbuffer, EmptyAndNoSpaceForHeader)
+{
+	// Addressing a corner case where start and end are at the end
+	// and the same.
+
+	Ringbuffer buf;
+	// Allocate 1 bytes more than the packet, 1 for the start/end logic.
+	ASSERT_TRUE(buf.allocate(21));
+
+	{
+		TempData data{20};
+		data.paint();
+		EXPECT_TRUE(buf.push_back(data.buf(), data.size()));
+		TempData out{20};
+		EXPECT_EQ(buf.pop_front(out.buf(), out.size()), out.size());
+		EXPECT_EQ(data, out);
+	}
+
+	{
+		TempData data{10};
+		data.paint();
+		EXPECT_TRUE(buf.push_back(data.buf(), data.size()));
+		TempData out{10};
+		EXPECT_EQ(buf.pop_front(out.buf(), out.size()), out.size());
+		EXPECT_EQ(data, out);
+	}
+}