From e4983ab88ca262014e514441e687e76ef88e7816 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 8 Jul 2021 10:56:25 -0400 Subject: [PATCH] px4_platform_common: atomic support larger types with critical sections (on NuttX) --- .../include/px4_platform_common/atomic.h | 151 ++++++++++++++++-- .../tests/test_microbench_atomic.cpp | 29 ++++ 2 files changed, 169 insertions(+), 11 deletions(-) diff --git a/platforms/common/include/px4_platform_common/atomic.h b/platforms/common/include/px4_platform_common/atomic.h index 3f8c941b25..d99a197817 100644 --- a/platforms/common/include/px4_platform_common/atomic.h +++ b/platforms/common/include/px4_platform_common/atomic.h @@ -58,6 +58,10 @@ #include #include +#if defined(__PX4_NUTTX) +# include +#endif // __PX4_NUTTX + namespace px4 { @@ -66,11 +70,11 @@ class atomic { public: -#ifdef __PX4_NUTTX +#if defined(__PX4_POSIX) // Ensure that all operations are lock-free, so that 'atomic' can be used from // IRQ handlers. This might not be required everywhere though. static_assert(__atomic_always_lock_free(sizeof(T), 0), "atomic is not lock-free for the given type T"); -#endif +#endif // __PX4_POSIX atomic() = default; explicit atomic(T value) : _value(value) {} @@ -83,7 +87,21 @@ public: #ifdef __PX4_QURT return _value; #else - return __atomic_load_n(&_value, __ATOMIC_SEQ_CST); + +#if defined(__PX4_NUTTX) + + if (!__atomic_always_lock_free(sizeof(T), 0)) { + irqstate_t flags = enter_critical_section(); + T val = _value; + leave_critical_section(flags); + return val; + + } else +#endif // __PX4_NUTTX + { + return __atomic_load_n(&_value, __ATOMIC_SEQ_CST); + } + #endif } @@ -95,7 +113,20 @@ public: #ifdef __PX4_QURT _value = value; #else - __atomic_store(&_value, &value, __ATOMIC_SEQ_CST); + +#if defined(__PX4_NUTTX) + + if (!__atomic_always_lock_free(sizeof(T), 0)) { + irqstate_t flags = enter_critical_section(); + _value = value; + leave_critical_section(flags); + + } else +#endif // __PX4_NUTTX + { + __atomic_store(&_value, &value, __ATOMIC_SEQ_CST); + } + #endif } @@ -109,7 +140,21 @@ public: // TODO: fix return _value++; #else - return __atomic_fetch_add(&_value, num, __ATOMIC_SEQ_CST); + +#if defined(__PX4_NUTTX) + + if (!__atomic_always_lock_free(sizeof(T), 0)) { + irqstate_t flags = enter_critical_section(); + T ret = _value++; + leave_critical_section(flags); + return ret; + + } else +#endif // __PX4_NUTTX + { + return __atomic_fetch_add(&_value, num, __ATOMIC_SEQ_CST); + } + #endif } @@ -119,7 +164,19 @@ public: */ inline T fetch_sub(T num) { - return __atomic_fetch_sub(&_value, num, __ATOMIC_SEQ_CST); +#if defined(__PX4_NUTTX) + + if (!__atomic_always_lock_free(sizeof(T), 0)) { + irqstate_t flags = enter_critical_section(); + T ret = _value--; + leave_critical_section(flags); + return ret; + + } else +#endif // __PX4_NUTTX + { + return __atomic_fetch_sub(&_value, num, __ATOMIC_SEQ_CST); + } } /** @@ -128,7 +185,20 @@ public: */ inline T fetch_and(T num) { - return __atomic_fetch_and(&_value, num, __ATOMIC_SEQ_CST); +#if defined(__PX4_NUTTX) + + if (!__atomic_always_lock_free(sizeof(T), 0)) { + irqstate_t flags = enter_critical_section(); + T val = _value; + _value &= num; + leave_critical_section(flags); + return val; + + } else +#endif // __PX4_NUTTX + { + return __atomic_fetch_and(&_value, num, __ATOMIC_SEQ_CST); + } } /** @@ -137,7 +207,20 @@ public: */ inline T fetch_xor(T num) { - return __atomic_fetch_xor(&_value, num, __ATOMIC_SEQ_CST); +#if defined(__PX4_NUTTX) + + if (!__atomic_always_lock_free(sizeof(T), 0)) { + irqstate_t flags = enter_critical_section(); + T val = _value; + _value ^= num; + leave_critical_section(flags); + return val; + + } else +#endif // __PX4_NUTTX + { + return __atomic_fetch_xor(&_value, num, __ATOMIC_SEQ_CST); + } } /** @@ -146,7 +229,20 @@ public: */ inline T fetch_or(T num) { - return __atomic_fetch_or(&_value, num, __ATOMIC_SEQ_CST); +#if defined(__PX4_NUTTX) + + if (!__atomic_always_lock_free(sizeof(T), 0)) { + irqstate_t flags = enter_critical_section(); + T val = _value; + _value |= num; + leave_critical_section(flags); + return val; + + } else +#endif // __PX4_NUTTX + { + return __atomic_fetch_or(&_value, num, __ATOMIC_SEQ_CST); + } } /** @@ -155,7 +251,20 @@ public: */ inline T fetch_nand(T num) { - return __atomic_fetch_nand(&_value, num, __ATOMIC_SEQ_CST); +#if defined(__PX4_NUTTX) + + if (!__atomic_always_lock_free(sizeof(T), 0)) { + irqstate_t flags = enter_critical_section(); + T ret = _value; + _value = ~(_value & num); + leave_critical_section(flags); + return ret; + + } else +#endif // __PX4_NUTTX + { + return __atomic_fetch_nand(&_value, num, __ATOMIC_SEQ_CST); + } } /** @@ -168,7 +277,27 @@ public: */ inline bool compare_exchange(T *expected, T desired) { - return __atomic_compare_exchange(&_value, expected, &desired, false, __ATOMIC_SEQ_CST, __ATOMIC_SEQ_CST); +#if defined(__PX4_NUTTX) + + if (!__atomic_always_lock_free(sizeof(T), 0)) { + irqstate_t flags = enter_critical_section(); + + if (_value == *expected) { + _value = desired; + leave_critical_section(flags); + return true; + + } else { + *expected = _value; + leave_critical_section(flags); + return false; + } + + } else +#endif // __PX4_NUTTX + { + return __atomic_compare_exchange(&_value, expected, &desired, false, __ATOMIC_SEQ_CST, __ATOMIC_SEQ_CST); + } } private: diff --git a/src/systemcmds/tests/test_microbench_atomic.cpp b/src/systemcmds/tests/test_microbench_atomic.cpp index 66c0b5d4a0..68f5589a37 100644 --- a/src/systemcmds/tests/test_microbench_atomic.cpp +++ b/src/systemcmds/tests/test_microbench_atomic.cpp @@ -85,6 +85,7 @@ private: bool time_atomic_int32(); bool time_atomic_uint32(); bool time_atomic_float(); + bool time_atomic_hrt_abstime(); void reset(); @@ -129,6 +130,10 @@ private: px4::atomic _atomic_float{0}; px4::atomic _atomic_float_storage{0}; float _test_load_float{}; + + px4::atomic _atomic_hrt_abstime{0}; + px4::atomic _atomic_hrt_abstime_storage{0}; + hrt_abstime _test_load_hrt_abstime{}; }; bool MicroBenchAtomic::run_tests() @@ -139,6 +144,7 @@ bool MicroBenchAtomic::run_tests() ut_run_test(time_atomic_int32); ut_run_test(time_atomic_uint32); ut_run_test(time_atomic_float); + ut_run_test(time_atomic_hrt_abstime); return (_tests_failed == 0); } @@ -288,4 +294,27 @@ bool MicroBenchAtomic::time_atomic_float() return true; } +bool MicroBenchAtomic::time_atomic_hrt_abstime() +{ + ut_compare("atomic hrt_abstime load", _atomic_hrt_abstime.load(), 0); + PERF("atomic hrt_abstime load", volatile hrt_abstime test_load = _atomic_hrt_abstime.load(), 100); + + _test_load_hrt_abstime = 1; + PERF("atomic hrt_abstime store", _atomic_hrt_abstime.store(_test_load_hrt_abstime), 100); + ut_compare("atomic hrt_abstime load", _atomic_hrt_abstime.load(), 1); + + PERF("atomic hrt_abstime load and store", _atomic_hrt_abstime_storage.store(_atomic_hrt_abstime.load()), 100); + + hrt_abstime expected = 12345678; + PERF("atomic hrt_abstime compare exchange (same)", + volatile bool compare_exchange = _atomic_hrt_abstime.compare_exchange(&expected, 12345678), 100); + ut_compare("atomic hrt_abstime load", _atomic_hrt_abstime.compare_exchange(&expected, 12345678) == true, true); + + PERF("atomic hrt_abstime compare exchange (different)", + volatile bool compare_exchange = _atomic_hrt_abstime.compare_exchange(&expected, 0), 100); + ut_compare("atomic hrt_abstime load", _atomic_hrt_abstime.compare_exchange(&expected, 0) == false, false); + + return true; +} + } // namespace MicroBenchAtomic