AArch64: support hardware atomic

Support aarch64 rt_hw_atomic_* api.
Add atomic implemente by rt_atomic api:
    rt_atomic_dec_and_test
    rt_atomic_fetch_add_unless
    rt_atomic_add_unless
    rt_atomic_inc_not_zero

Signed-off-by: GuEe-GUI <[email protected]>

1 files changed, 108 insertions, 0 deletions

diff --git a/libcpu/aarch64/common/atomic_aarch64.c b/libcpu/aarch64/common/atomic_aarch64.c
new file mode 100644
index 0000000000..c55cd8a84f
--- /dev/null
+++ b/libcpu/aarch64/common/atomic_aarch64.c
@@ -0,0 +1,108 @@
+/*
+ * Copyright (c) 2006-2023, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2023-05-18     GuEe-GUI     first version
+ */
+
+#include <rtatomic.h>
+
+rt_atomic_t rt_hw_atomic_load(volatile rt_atomic_t *ptr)
+{
+    rt_atomic_t ret;
+
+    __asm__ volatile (
+        "   ldr     %w0, %1\n"
+        "   dmb     ish"
+        : "=r" (ret)
+        : "Q" (*ptr)
+        : "memory");
+
+    return ret;
+}
+
+void rt_hw_atomic_store(volatile rt_atomic_t *ptr, rt_atomic_t val)
+{
+    __asm__ volatile (
+        "   stlr    %w1, %0\n"
+        "   dmb     ish"
+        : "=Q" (*ptr)
+        : "r" (val)
+        : "memory");
+}
+
+#define AARCH64_ATOMIC_OP_RETURN(op, ins, constraint)               \
+rt_atomic_t rt_hw_atomic_##op(volatile rt_atomic_t *ptr, rt_atomic_t in_val)    \
+{                                                                   \
+    rt_atomic_t tmp, val, result;                                   \
+                                                                    \
+    __asm__ volatile (                                              \
+        "   prfm    pstl1strm, %3\n"                                \
+        "1: ldxr    %w0, %3\n"                                      \
+        "   "#ins " %w1, %w0, %w4\n"                                \
+        "   stlxr   %w2, %w1, %3\n"                                 \
+        "   cbnz    %w2, 1b\n"                                      \
+        "   dmb     ish"                                            \
+        : "=&r" (result), "=&r" (val), "=&r" (tmp), "+Q" (*ptr)     \
+        : __RT_STRINGIFY(constraint) "r" (in_val)                   \
+        : "memory");                                                \
+                                                                    \
+    return result;                                                  \
+}
+
+AARCH64_ATOMIC_OP_RETURN(add, add, I)
+AARCH64_ATOMIC_OP_RETURN(sub, sub, J)
+AARCH64_ATOMIC_OP_RETURN(and, and, K)
+AARCH64_ATOMIC_OP_RETURN(or, orr, K)
+AARCH64_ATOMIC_OP_RETURN(xor, eor, K)
+
+rt_atomic_t rt_hw_atomic_exchange(volatile rt_atomic_t *ptr, rt_atomic_t val)
+{
+    rt_atomic_t ret, tmp;
+
+    __asm__ volatile (
+        "   prfm    pstl1strm, %2\n"
+        "1: ldxr    %w0, %2\n"
+        "   stlxr   %w1, %w3, %2\n"
+        "   cbnz    %w1, 1b\n"
+        "   dmb     ish"
+        : "=&r" (ret), "=&r" (tmp), "+Q" (*ptr)
+        : "r" (val)
+        : "memory");
+
+    return ret;
+}
+
+void rt_hw_atomic_flag_clear(volatile rt_atomic_t *ptr)
+{
+    rt_hw_atomic_and(ptr, 0);
+}
+
+rt_atomic_t rt_hw_atomic_flag_test_and_set(volatile rt_atomic_t *ptr)
+{
+    return rt_hw_atomic_or(ptr, 1);
+}
+
+rt_atomic_t rt_hw_atomic_compare_exchange_strong(volatile rt_atomic_t *ptr, rt_atomic_t *old, rt_atomic_t new)
+{
+    rt_atomic_t tmp, oldval;
+
+    __asm__ volatile (
+        "   prfm    pstl1strm, %2\n"
+        "1: ldxr    %w0, %2\n"
+        "   eor     %w1, %w0, %w3\n"
+        "   cbnz    %w1, 2f\n"
+        "   stlxr   %w1, %w4, %2\n"
+        "   cbnz    %w1, 1b\n"
+        "   dmb     ish\n"
+        "2:"
+        : "=&r" (oldval), "=&r" (tmp), "+Q" (*ptr)
+        : "Kr" (*old), "r" (new)
+        : "memory");
+
+    return oldval;
+}
+