lib: add variable length ringbuffer

This adds a reusable class for a FIFO ringbuffer that accepts variable
length packets. It is using the Ringbuffer class internally.

Signed-off-by: Julian Oes <[email protected]>

src/lib/CMakeLists.txt

@@ -72,6 +72,7 @@ add_subdirectory(terrain_estimation EXCLUDE_FROM_ALL)
 add_subdirectory(timesync EXCLUDE_FROM_ALL)
 add_subdirectory(tinybson EXCLUDE_FROM_ALL)
 add_subdirectory(tunes EXCLUDE_FROM_ALL)
+add_subdirectory(variable_length_ringbuffer EXCLUDE_FROM_ALL)
 add_subdirectory(version EXCLUDE_FROM_ALL)
 add_subdirectory(weather_vane EXCLUDE_FROM_ALL)
 add_subdirectory(wind_estimator EXCLUDE_FROM_ALL)

src/lib/variable_length_ringbuffer/CMakeLists.txt

@@ -0,0 +1,42 @@
+############################################################################
+#
+#   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(variable_length_ringbuffer
+	VariableLengthRingbuffer.cpp
+)
+
+target_link_libraries(variable_length_ringbuffer PRIVATE ringbuffer)
+
+target_include_directories(variable_length_ringbuffer PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+px4_add_unit_gtest(SRC VariableLengthRingbufferTest.cpp LINKLIBS variable_length_ringbuffer)

src/lib/variable_length_ringbuffer/VariableLengthRingbuffer.cpp

@@ -0,0 +1,106 @@
+/****************************************************************************
+ *
+ *   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 "VariableLengthRingbuffer.hpp"
+
+#include <assert.h>
+#include <string.h>
+
+
+VariableLengthRingbuffer::~VariableLengthRingbuffer()
+{
+	deallocate();
+}
+
+bool VariableLengthRingbuffer::allocate(size_t buffer_size)
+{
+	return _ringbuffer.allocate(buffer_size);
+}
+
+void VariableLengthRingbuffer::deallocate()
+{
+	_ringbuffer.deallocate();
+}
+
+bool VariableLengthRingbuffer::push_back(const uint8_t *packet, size_t packet_len)
+{
+	if (packet_len == 0 || packet == nullptr) {
+		// Nothing to add, we better don't try.
+		return false;
+	}
+
+	size_t space_required = packet_len + sizeof(Header);
+
+	if (space_required > _ringbuffer.space_available()) {
+		return false;
+	}
+
+	Header header{static_cast<uint32_t>(packet_len)};
+	bool result = _ringbuffer.push_back(reinterpret_cast<const uint8_t * >(&header), sizeof(header));
+	assert(result);
+
+	result = _ringbuffer.push_back(packet, packet_len);
+	assert(result);
+
+	// In case asserts are commented out to prevent unused warnings.
+	(void)result;
+
+	return true;
+}
+
+size_t VariableLengthRingbuffer::pop_front(uint8_t *buf, size_t buf_max_len)
+{
+	if (buf == nullptr) {
+		// User needs to supply a valid pointer.
+		return 0;
+	}
+
+	// Check next header
+	Header header;
+
+	if (_ringbuffer.pop_front(reinterpret_cast<uint8_t *>(&header), sizeof(header)) < sizeof(header)) {
+		return 0;
+	}
+
+	// We can't fit the packet into the user supplied buffer.
+	// This should never happen as the user has to supply a big // enough buffer.
+	assert(static_cast<uint32_t>(header.len) <= buf_max_len);
+
+	size_t bytes_read = _ringbuffer.pop_front(buf, header.len);
+	assert(bytes_read == header.len);
+
+	return bytes_read;
+}

src/lib/variable_length_ringbuffer/VariableLengthRingbuffer.hpp

@@ -0,0 +1,111 @@
+/****************************************************************************
+ *
+ *   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 <lib/ringbuffer/Ringbuffer.hpp>
+
+
+// FIFO ringbuffer implementation for packets of variable length.
+//
+// The variable length is implemented using a 4 byte header
+// containing a the length.
+//
+// The buffer is not thread-safe.
+
+class VariableLengthRingbuffer
+{
+public:
+	/* @brief Constructor
+	 *
+	 * @note Does not allocate automatically.
+	 */
+	VariableLengthRingbuffer() = default;
+
+	/*
+	 * @brief Destructor
+	 *
+	 * Automatically calls deallocate.
+	 */
+	~VariableLengthRingbuffer();
+
+	/* @brief Allocate ringbuffer
+	 *
+	 * @note The variable length requires 4 bytes
+	 * of overhead per packet.
+	 *
+	 * @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 Copy packet into ringbuffer
+	 *
+	 * @param packet Pointer to packet to copy from.
+	 * @param packet_len Length of packet.
+	 *
+	 * @returns true if packet could be copied into buffer.
+	 */
+	bool push_back(const uint8_t *packet, size_t packet_len);
+
+	/*
+	 * @brief Get packet from ringbuffer
+	 *
+	 * @note max_buf_len needs to be bigger equal to any pushed packet.
+	 *
+	 * @param buf Pointer to where next packet can be copied into.
+	 * @param max_buf_len Max size of buf
+	 *
+	 * @returns 0 if packet is bigger than max_len or buffer is empty.
+	 */
+	size_t pop_front(uint8_t *buf, size_t max_buf_len);
+
+private:
+	struct Header {
+		uint32_t len;
+	};
+
+	Ringbuffer _ringbuffer {};
+};