lockstep_scheduler: add generic lockstep component API

allows components to register and ensure the lockstep cycle waits for
all components to be updated.

platforms/posix/src/px4/common/drv_hrt.cpp

@@ -583,4 +583,24 @@ int px4_pthread_cond_timedwait(pthread_cond_t *cond,
 	const uint64_t scheduled = time_us + px4_timestart_monotonic;
 	return lockstep_scheduler->cond_timedwait(cond, mutex, scheduled);
 }
+
+int px4_lockstep_register_component()
+{
+	return lockstep_scheduler->components().register_component();
+}
+
+void px4_lockstep_unregister_component(int component)
+{
+	lockstep_scheduler->components().unregister_component(component);
+}
+
+void px4_lockstep_progress(int component)
+{
+	lockstep_scheduler->components().lockstep_progress(component);
+}
+
+void px4_lockstep_wait_for_components()
+{
+	lockstep_scheduler->components().wait_for_components();
+}
 #endif

platforms/posix/src/px4/common/lockstep_scheduler/CMakeLists.txt

@@ -1,8 +1,9 @@
 cmake_minimum_required(VERSION 2.8.12)
 
 # We want to test the lockstep schedule even if it is not used otherwise.
-add_library(lockstep_scheduler
+px4_add_library(lockstep_scheduler
 	src/lockstep_scheduler.cpp
+	src/lockstep_components.cpp
 )
 
 target_include_directories(lockstep_scheduler
@@ -10,4 +11,4 @@ target_include_directories(lockstep_scheduler
 	${CMAKE_CURRENT_SOURCE_DIR}/include
 )
 
-px4_add_unit_gtest(SRC test/src/lockstep_scheduler_test.cpp LINKLIBS lockstep_scheduler)
+px4_add_functional_gtest(SRC test/src/lockstep_scheduler_test.cpp LINKLIBS lockstep_scheduler)

platforms/posix/src/px4/common/lockstep_scheduler/include/lockstep_scheduler/lockstep_components.h

@@ -0,0 +1,78 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 <cstdint>
+#include <atomic>
+
+#include <px4_platform_common/sem.h>
+
+/**
+ * @class LockstepComponents
+ * Allows to register components (threads) that need to be updated or waited for in every lockstep cycle (barrier).
+ * Registered components need to ensure they poll on topics that is updated in every lockstep cycle.
+ */
+class LockstepComponents
+{
+public:
+	LockstepComponents();
+	~LockstepComponents();
+
+	/**
+	 * Register a component
+	 * @return a valid component ID > 0 or 0 on error (or unsupported)
+	 */
+	int register_component();
+	void unregister_component(int component);
+
+	/**
+	 * signal an update from a component
+	 * @param component component ID
+	 */
+	void lockstep_progress(int component);
+
+	/**
+	 * Wait for all registered components to call lockstep_progress()
+	 * Note: only 1 thread can call this
+	 */
+	void wait_for_components();
+
+private:
+
+	px4_sem_t _components_sem;
+
+	std::atomic_int _components_used_bitset{0};
+	std::atomic_int _components_progress_bitset{0};
+};
+

platforms/posix/src/px4/common/lockstep_scheduler/include/lockstep_scheduler/lockstep_scheduler.h

@@ -1,3 +1,36 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 <cstdint>
@@ -7,6 +40,8 @@
 #include <atomic>
 #include <pthread.h>
 
+#include "lockstep_components.h"
+
 class LockstepScheduler
 {
 public:
@@ -17,6 +52,8 @@ public:
 	int cond_timedwait(pthread_cond_t *cond, pthread_mutex_t *lock, uint64_t time_us);
 	int usleep_until(uint64_t timed_us);
 
+	LockstepComponents &components() { return _components; }
+
 private:
 	struct TimedWait {
 		~TimedWait()
@@ -54,6 +91,8 @@ private:
 		TimedWait *next{nullptr}; ///< linked list
 	};
 
+	LockstepComponents _components;
+
 	std::atomic<uint64_t> _time_us{0};
 
 	TimedWait *_timed_waits{nullptr}; ///< head of linked list

platforms/posix/src/px4/common/lockstep_scheduler/src/lockstep_components.cpp

@@ -0,0 +1,127 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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.
+ *
+ ****************************************************************************/
+
+#ifndef MODULE_NAME
+#define MODULE_NAME "lockstep"
+#endif
+
+#include <lockstep_scheduler/lockstep_components.h>
+
+#include <drivers/drv_hrt.h>
+#include <px4_platform_common/log.h>
+#include <px4_platform_common/tasks.h>
+#include <limits.h>
+
+LockstepComponents::LockstepComponents()
+{
+	px4_sem_init(&_components_sem, 0, 0);
+}
+
+LockstepComponents::~LockstepComponents()
+{
+	px4_sem_destroy(&_components_sem);
+}
+
+int LockstepComponents::register_component()
+{
+	for (int component = 0; component < (int)sizeof(int) * CHAR_BIT - 1; ++component) {
+		while (true) {
+			int expected = _components_used_bitset;
+
+			if ((expected & (1 << component))) { // already used
+				break;
+			}
+
+			if (_components_used_bitset.compare_exchange_weak(expected, expected | (1 << component))) {
+				PX4_DEBUG("%s: got lockstep component %i", px4_get_taskname(), component);
+				return 1 << component;
+			}
+		}
+	}
+
+	PX4_ERR("No more components left");
+	return 0;
+}
+
+void LockstepComponents::unregister_component(int component)
+{
+	if (component <= 0) {
+		return;
+	}
+
+	_components_progress_bitset.fetch_and(~component);
+	_components_used_bitset.fetch_and(~component);
+
+	int components_used_bitset = _components_used_bitset;
+
+	if (_components_progress_bitset == components_used_bitset && components_used_bitset != 0) {
+		_components_progress_bitset = 0;
+		px4_sem_post(&_components_sem);
+	}
+}
+
+void LockstepComponents::lockstep_progress(int component)
+{
+	if (component <= 0) {
+		return;
+	}
+
+	// Use a bitset to mark progress of each component. We could also use a simple counter,
+	// but this is more robust (e.g. if a component calls this multiple times per cycle).
+	int prev_value = _components_progress_bitset.fetch_or(component);
+
+	// proceed if this is the last component setting its bit
+	if ((prev_value | component) == _components_used_bitset) {
+		// Note: there's a minimal race condtion here during startup: if a thread is here, and another calls
+		// register_component and is fast enough it can land here as well, thus leading to 2 unlocks in a cycle.
+		// That is acceptable though.
+		_components_progress_bitset = 0;
+
+		// during startup it can happen that wait_for_components() is not called yet, so avoid increasing the
+		// semaphore counter more than necessary
+		int value;
+
+		if (px4_sem_getvalue(&_components_sem, &value) == 0 && value < 1) {
+			px4_sem_post(&_components_sem);
+		}
+	}
+}
+
+void LockstepComponents::wait_for_components()
+{
+	if (_components_used_bitset == 0) {
+		return;
+	}
+
+	while (px4_sem_wait(&_components_sem) != 0) {}
+}

platforms/posix/src/px4/common/lockstep_scheduler/src/lockstep_scheduler.cpp

@@ -1,3 +1,36 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 <lockstep_scheduler/lockstep_scheduler.h>
 
 LockstepScheduler::~LockstepScheduler()

src/drivers/drv_hrt.h

@@ -194,6 +194,19 @@ __EXPORT extern void	hrt_init(void);
 __EXPORT extern hrt_abstime hrt_absolute_time_offset(void);
 
 #endif
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+
+__EXPORT extern int px4_lockstep_register_component(void);
+__EXPORT extern void px4_lockstep_unregister_component(int component);
+__EXPORT extern void px4_lockstep_progress(int component);
+__EXPORT extern void px4_lockstep_wait_for_components(void);
+
+#else
+static inline int px4_lockstep_register_component(void) { return 0; }
+static inline void px4_lockstep_unregister_component(int component) { }
+static inline void px4_lockstep_progress(int component) { }
+static inline void px4_lockstep_wait_for_components(void) { }
+#endif /* defined(ENABLE_LOCKSTEP_SCHEDULER) */
 
 __END_DECLS