Revert "px4_platform_common/atomic.h: Use IRQ locks only when REALLY …

…needed" This reverts commit cfa78c8.
tiiuae · May 8, 2024 · dcc8729 · dcc8729
1 parent cb9433a
commit dcc8729
Show file tree

Hide file tree

Showing 20 changed files with 103 additions and 348 deletions.
diff --git a/platforms/common/include/px4_platform_common/atomic.h b/platforms/common/include/px4_platform_common/atomic.h
@@ -58,7 +58,9 @@
 #include <stdbool.h>
 #include <stdint.h>
 
-#include <px4_platform_common/sem.h>
+#if defined(__PX4_NUTTX)
+# include <nuttx/irq.h>
+#endif // __PX4_NUTTX
 
 namespace px4
 {
@@ -73,31 +75,26 @@ class atomic
 	// 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 // __PX4_POSIX
-	atomic()
-	{
-		if (!__atomic_always_lock_free(sizeof(T), 0)) {
-			px4_sem_init(&_lock, 0, 1);
-		}
-	}
-	explicit atomic(T value) : _value(value)
-	{
-		if (!__atomic_always_lock_free(sizeof(T), 0)) {
-			px4_sem_init(&_lock, 0, 1);
-		}
-	}
+
+	atomic() = default;
+	explicit atomic(T value) : _value(value) {}
 
 	/**
 	 * Atomically read the current value
 	 */
 	inline T load() const
 	{
+#if defined(__PX4_NUTTX)
+
 		if (!__atomic_always_lock_free(sizeof(T), 0)) {
-			take_lock();
+			irqstate_t flags = enter_critical_section();
 			T val = _value;
-			release_lock();
+			leave_critical_section(flags);
 			return val;
 
-		} else {
+		} else
+#endif // __PX4_NUTTX
+		{
 			return __atomic_load_n(&_value, __ATOMIC_SEQ_CST);
 		}
 	}
@@ -107,12 +104,16 @@ class atomic
 	 */
 	inline void store(T value)
 	{
+#if defined(__PX4_NUTTX)
+
 		if (!__atomic_always_lock_free(sizeof(T), 0)) {
-			take_lock();
+			irqstate_t flags = enter_critical_section();
 			_value = value;
-			release_lock();
+			leave_critical_section(flags);
 
-		} else {
+		} else
+#endif // __PX4_NUTTX
+		{
 			__atomic_store(&_value, &value, __ATOMIC_SEQ_CST);
 		}
 	}
@@ -123,14 +124,18 @@ class atomic
 	 */
 	inline T fetch_add(T num)
 	{
+#if defined(__PX4_NUTTX)
+
 		if (!__atomic_always_lock_free(sizeof(T), 0)) {
-			take_lock();
+			irqstate_t flags = enter_critical_section();
 			T ret = _value;
 			_value += num;
-			release_lock();
+			leave_critical_section(flags);
 			return ret;
 
-		} else {
+		} else
+#endif // __PX4_NUTTX
+		{
 			return __atomic_fetch_add(&_value, num, __ATOMIC_SEQ_CST);
 		}
 	}
@@ -141,14 +146,18 @@ class atomic
 	 */
 	inline T fetch_sub(T num)
 	{
+#if defined(__PX4_NUTTX)
+
 		if (!__atomic_always_lock_free(sizeof(T), 0)) {
-			take_lock();
+			irqstate_t flags = enter_critical_section();
 			T ret = _value;
 			_value -= num;
-			release_lock();
+			leave_critical_section(flags);
 			return ret;
 
-		} else {
+		} else
+#endif // __PX4_NUTTX
+		{
 			return __atomic_fetch_sub(&_value, num, __ATOMIC_SEQ_CST);
 		}
 	}
@@ -159,14 +168,18 @@ class atomic
 	 */
 	inline T fetch_and(T num)
 	{
+#if defined(__PX4_NUTTX)
+
 		if (!__atomic_always_lock_free(sizeof(T), 0)) {
-			take_lock();
+			irqstate_t flags = enter_critical_section();
 			T val = _value;
 			_value &= num;
-			release_lock();
+			leave_critical_section(flags);
 			return val;
 
-		} else {
+		} else
+#endif // __PX4_NUTTX
+		{
 			return __atomic_fetch_and(&_value, num, __ATOMIC_SEQ_CST);
 		}
 	}
@@ -177,14 +190,18 @@ class atomic
 	 */
 	inline T fetch_xor(T num)
 	{
+#if defined(__PX4_NUTTX)
+
 		if (!__atomic_always_lock_free(sizeof(T), 0)) {
-			take_lock();
+			irqstate_t flags = enter_critical_section();
 			T val = _value;
 			_value ^= num;
-			release_lock();
+			leave_critical_section(flags);
 			return val;
 
-		} else {
+		} else
+#endif // __PX4_NUTTX
+		{
 			return __atomic_fetch_xor(&_value, num, __ATOMIC_SEQ_CST);
 		}
 	}
@@ -195,14 +212,18 @@ class atomic
 	 */
 	inline T fetch_or(T num)
 	{
+#if defined(__PX4_NUTTX)
+
 		if (!__atomic_always_lock_free(sizeof(T), 0)) {
-			take_lock();
+			irqstate_t flags = enter_critical_section();
 			T val = _value;
 			_value |= num;
-			release_lock();
+			leave_critical_section(flags);
 			return val;
 
-		} else {
+		} else
+#endif // __PX4_NUTTX
+		{
 			return __atomic_fetch_or(&_value, num, __ATOMIC_SEQ_CST);
 		}
 	}
@@ -213,14 +234,18 @@ class atomic
 	 */
 	inline T fetch_nand(T num)
 	{
+#if defined(__PX4_NUTTX)
+
 		if (!__atomic_always_lock_free(sizeof(T), 0)) {
-			take_lock();
+			irqstate_t flags = enter_critical_section();
 			T ret = _value;
 			_value = ~(_value & num);
-			release_lock();
+			leave_critical_section(flags);
 			return ret;
 
-		} else {
+		} else
+#endif // __PX4_NUTTX
+		{
 			return __atomic_fetch_nand(&_value, num, __ATOMIC_SEQ_CST);
 		}
 	}
@@ -235,31 +260,31 @@ class atomic
 	 */
 	inline bool compare_exchange(T *expected, T desired)
 	{
+#if defined(__PX4_NUTTX)
+
 		if (!__atomic_always_lock_free(sizeof(T), 0)) {
-			take_lock();
+			irqstate_t flags = enter_critical_section();
 
 			if (_value == *expected) {
 				_value = desired;
-				release_lock();
+				leave_critical_section(flags);
 				return true;
 
 			} else {
 				*expected = _value;
-				release_lock();
+				leave_critical_section(flags);
 				return false;
 			}
 
-		} else {
+		} else
+#endif // __PX4_NUTTX
+		{
 			return __atomic_compare_exchange(&_value, expected, &desired, false, __ATOMIC_SEQ_CST, __ATOMIC_SEQ_CST);
 		}
 	}
 
 private:
 	T _value {};
-
-	inline void take_lock() const { do {} while (px4_sem_wait(&_lock) != 0); }
-	inline void release_lock() const { px4_sem_post(&_lock); }
-	mutable px4_sem_t _lock;
 };
 
 using atomic_int = atomic<int>;
@@ -279,4 +304,3 @@ using atomic_bool = atomic<int>;
 } /* namespace px4 */
 
 #endif /* __cplusplus */
-