diff --git a/platforms/posix/src/px4/common/drv_hrt.cpp b/platforms/posix/src/px4/common/drv_hrt.cpp index d41844668d..c9c457b00a 100644 --- a/platforms/posix/src/px4/common/drv_hrt.cpp +++ b/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 diff --git a/platforms/posix/src/px4/common/lockstep_scheduler/CMakeLists.txt b/platforms/posix/src/px4/common/lockstep_scheduler/CMakeLists.txt index da84323d9d..15c40a2e99 100644 --- a/platforms/posix/src/px4/common/lockstep_scheduler/CMakeLists.txt +++ b/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) diff --git a/platforms/posix/src/px4/common/lockstep_scheduler/include/lockstep_scheduler/lockstep_components.h b/platforms/posix/src/px4/common/lockstep_scheduler/include/lockstep_scheduler/lockstep_components.h new file mode 100644 index 0000000000..6767d1389d --- /dev/null +++ b/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 +#include + +#include + +/** + * @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}; +}; + diff --git a/platforms/posix/src/px4/common/lockstep_scheduler/include/lockstep_scheduler/lockstep_scheduler.h b/platforms/posix/src/px4/common/lockstep_scheduler/include/lockstep_scheduler/lockstep_scheduler.h index 76271345a8..2e5a1eaab5 100644 --- a/platforms/posix/src/px4/common/lockstep_scheduler/include/lockstep_scheduler/lockstep_scheduler.h +++ b/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 @@ -7,6 +40,8 @@ #include #include +#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 _time_us{0}; TimedWait *_timed_waits{nullptr}; ///< head of linked list diff --git a/platforms/posix/src/px4/common/lockstep_scheduler/src/lockstep_components.cpp b/platforms/posix/src/px4/common/lockstep_scheduler/src/lockstep_components.cpp new file mode 100644 index 0000000000..2875ae298f --- /dev/null +++ b/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 + +#include +#include +#include +#include + +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) {} +} diff --git a/platforms/posix/src/px4/common/lockstep_scheduler/src/lockstep_scheduler.cpp b/platforms/posix/src/px4/common/lockstep_scheduler/src/lockstep_scheduler.cpp index 87479e263b..f33dc7ad64 100644 --- a/platforms/posix/src/px4/common/lockstep_scheduler/src/lockstep_scheduler.cpp +++ b/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 LockstepScheduler::~LockstepScheduler() diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h index 2b625a4922..60fccff0e6 100644 --- a/src/drivers/drv_hrt.h +++ b/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