[BACKPORT v1.4] Fix MAVLink forwarding

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
@@ -61,6 +61,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)
@@ -70,6 +71,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/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};
+};