px4_platform_common: atomic support larger types with critical sections (on NuttX)

platforms/common/include/px4_platform_common/atomic.h

@@ -58,6 +58,10 @@
 #include <stdbool.h>
 #include <stdint.h>
 
+#if defined(__PX4_NUTTX)
+# include <nuttx/irq.h>
+#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:

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<float> _atomic_float{0};
 	px4::atomic<float> _atomic_float_storage{0};
 	float _test_load_float{};
+
+	px4::atomic<hrt_abstime> _atomic_hrt_abstime{0};
+	px4::atomic<hrt_abstime> _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