From 8434845050e84986a3b47381db58781bd6d64632 Mon Sep 17 00:00:00 2001
From: "Michael (XIAO Xufeng)" <xiaoxufeng@espressif.com>
Date: Thu, 26 Dec 2019 18:20:54 +0800
Subject: [PATCH] atomic: support fetch_and, fetch_and and fetch_xor

---
 components/xtensa/linker.lf   |  1 +
 components/xtensa/stdatomic.c | 77 +++++++++++++++++++++++++++++++----
 2 files changed, 71 insertions(+), 7 deletions(-)

diff --git a/components/xtensa/linker.lf b/components/xtensa/linker.lf
index c39c4ff52..a721ac41b 100644
--- a/components/xtensa/linker.lf
+++ b/components/xtensa/linker.lf
@@ -2,6 +2,7 @@
 archive: libxtensa.a
 entries:
     eri (noflash_text)
+    stdatomic (noflash)
 
 [mapping:hal]
 archive: libhal.a
diff --git a/components/xtensa/stdatomic.c b/components/xtensa/stdatomic.c
index bc5b7ba60..b052c2ba2 100644
--- a/components/xtensa/stdatomic.c
+++ b/components/xtensa/stdatomic.c
@@ -15,42 +15,90 @@
 //replacement for gcc built-in functions
 
 #include "sdkconfig.h"
-#include "freertos/FreeRTOS.h"
+#include <stdbool.h>
 #include "xtensa/config/core-isa.h"
+#include "xtensa/xtruntime.h"
 
+//reserved to measure atomic operation time
+#define atomic_benchmark_intr_disable()
+#define atomic_benchmark_intr_restore(STATE)
+
+// This allows nested interrupts disabling and restoring via local registers or stack.
+// They can be called from interrupts too.
+// WARNING: Only applies to current CPU.
+#define _ATOMIC_ENTER_CRITICAL(void) ({ \
+	unsigned state = XTOS_SET_INTLEVEL(XCHAL_EXCM_LEVEL); \
+	atomic_benchmark_intr_disable(); \
+	state; \
+})
+
+#define _ATOMIC_EXIT_CRITICAL(state)   do { \
+    atomic_benchmark_intr_restore(state); \
+    XTOS_RESTORE_JUST_INTLEVEL(state); \
+    } while (0)
 
 #define CMP_EXCHANGE(n, type) bool __atomic_compare_exchange_ ## n (type* mem, type* expect, type desired, int success, int failure) \
 { \
     bool ret = false; \
-    unsigned state = portENTER_CRITICAL_NESTED(); \
+    unsigned state = _ATOMIC_ENTER_CRITICAL(); \
     if (*mem == *expect) { \
         ret = true; \
         *mem = desired; \
     } else { \
         *expect = *mem; \
     } \
-    portEXIT_CRITICAL_NESTED(state); \
+    _ATOMIC_EXIT_CRITICAL(state); \
     return ret; \
 }
 
 #define FETCH_ADD(n, type) type __atomic_fetch_add_ ## n (type* ptr, type value, int memorder) \
 { \
-    unsigned state = portENTER_CRITICAL_NESTED(); \
+    unsigned state = _ATOMIC_ENTER_CRITICAL(); \
     type ret = *ptr; \
     *ptr = *ptr + value; \
-    portEXIT_CRITICAL_NESTED(state); \
+    _ATOMIC_EXIT_CRITICAL(state); \
     return ret; \
 }
 
 #define FETCH_SUB(n, type) type __atomic_fetch_sub_ ## n (type* ptr, type value, int memorder) \
 { \
-    unsigned state = portENTER_CRITICAL_NESTED(); \
+    unsigned state = _ATOMIC_ENTER_CRITICAL(); \
     type ret = *ptr; \
     *ptr = *ptr - value; \
-    portEXIT_CRITICAL_NESTED(state); \
+    _ATOMIC_EXIT_CRITICAL(state); \
     return ret; \
 }
 
+#define FETCH_AND(n, type) type __atomic_fetch_and_ ## n (type* ptr, type value, int memorder) \
+{ \
+    unsigned state = _ATOMIC_ENTER_CRITICAL(); \
+    type ret = *ptr; \
+    *ptr = *ptr & value; \
+    _ATOMIC_EXIT_CRITICAL(state); \
+    return ret; \
+}
+
+#define FETCH_OR(n, type) type __atomic_fetch_or_ ## n (type* ptr, type value, int memorder) \
+{ \
+    unsigned state = _ATOMIC_ENTER_CRITICAL(); \
+    type ret = *ptr; \
+    *ptr = *ptr | value; \
+    _ATOMIC_EXIT_CRITICAL(state); \
+    return ret; \
+}
+
+#define FETCH_XOR(n, type) type __atomic_fetch_xor_ ## n (type* ptr, type value, int memorder) \
+{ \
+    unsigned state = _ATOMIC_ENTER_CRITICAL(); \
+    type ret = *ptr; \
+    *ptr = *ptr ^ value; \
+    _ATOMIC_EXIT_CRITICAL(state); \
+    return ret; \
+}
+
+#ifndef XCHAL_HAVE_S32C1I
+#error "XCHAL_HAVE_S32C1I not defined, include correct header!"
+#endif
 
 //this piece of code should only be compiled if the cpu doesn't support atomic compare and swap (s32c1i)
 #if XCHAL_HAVE_S32C1I == 0
@@ -72,4 +120,19 @@ FETCH_SUB(2, uint16_t)
 FETCH_SUB(4, uint32_t)
 FETCH_SUB(8, uint64_t)
 
+FETCH_AND(1, uint8_t)
+FETCH_AND(2, uint16_t)
+FETCH_AND(4, uint32_t)
+FETCH_AND(8, uint64_t)
+
+FETCH_OR(1, uint8_t)
+FETCH_OR(2, uint16_t)
+FETCH_OR(4, uint32_t)
+FETCH_OR(8, uint64_t)
+
+FETCH_XOR(1, uint8_t)
+FETCH_XOR(2, uint16_t)
+FETCH_XOR(4, uint32_t)
+FETCH_XOR(8, uint64_t)
+
 #endif
\ No newline at end of file