From 694adaae8b3497373031b6b04bc996b3e07f1534 Mon Sep 17 00:00:00 2001
From: Kevin Bracey <kevin.bracey@arm.com>
Date: Mon, 4 Feb 2019 13:14:07 +0200
Subject: [PATCH] Add atomic exchange

---
 UNITTESTS/stubs/mbed_critical_stub.c | 26 ++++++++++
 platform/mbed_critical.c             | 77 ++++++++++++++++++++++++++++
 platform/mbed_critical.h             | 51 ++++++++++++++++++
 3 files changed, 154 insertions(+)

diff --git a/UNITTESTS/stubs/mbed_critical_stub.c b/UNITTESTS/stubs/mbed_critical_stub.c
index 0c6774301c..b13bf61bff 100644
--- a/UNITTESTS/stubs/mbed_critical_stub.c
+++ b/UNITTESTS/stubs/mbed_critical_stub.c
@@ -71,6 +71,22 @@ bool core_util_atomic_cas_u32(volatile uint32_t *ptr, uint32_t *expectedCurrentV
 }
 
 
+uint8_t core_util_atomic_exchange_u8(volatile uint8_t *ptr, uint8_t desiredValue)
+{
+    return 0;
+}
+
+uint16_t core_util_atomic_exchange_u16(volatile uint16_t *ptr, uint16_t desiredValue)
+{
+    return 0;
+}
+
+uint32_t core_util_atomic_exchange_u32(volatile uint32_t *ptr, uint32_t desiredValue)
+{
+    return 0;
+}
+
+
 uint8_t core_util_atomic_incr_u8(volatile uint8_t *valuePtr, uint8_t delta)
 {
     return 0;
@@ -112,6 +128,11 @@ void core_util_atomic_store_u64(volatile uint64_t *valuePtr, uint64_t desiredVal
 {
 }
 
+uint64_t core_util_atomic_exchange_u64(volatile uint64_t *valuePtr, uint64_t desiredValue)
+{
+    return 0;
+}
+
 bool core_util_atomic_cas_u64(volatile uint64_t *ptr, uint64_t *expectedCurrentValue, uint64_t desiredValue)
 {
     return false;
@@ -133,6 +154,11 @@ bool core_util_atomic_cas_ptr(void *volatile *ptr, void **expectedCurrentValue,
     return false;
 }
 
+void *core_util_atomic_exchange_ptr(void *volatile *valuePtr, void *desiredValue)
+{
+    return NULL;
+}
+
 void *core_util_atomic_incr_ptr(void *volatile *valuePtr, ptrdiff_t delta)
 {
     return NULL;
diff --git a/platform/mbed_critical.c b/platform/mbed_critical.c
index aebebe3280..8b847730c6 100644
--- a/platform/mbed_critical.c
+++ b/platform/mbed_critical.c
@@ -167,6 +167,39 @@ bool core_util_atomic_cas_u32(volatile uint32_t *ptr, uint32_t *expectedCurrentV
     return true;
 }
 
+uint8_t core_util_atomic_exchange_u8(volatile uint8_t *valuePtr, uint8_t desiredValue)
+{
+    MBED_BARRIER();
+    uint8_t currentValue;
+    do {
+        currentValue = __LDREXB(valuePtr);
+    } while (__STREXB(desiredValue, valuePtr));
+    MBED_BARRIER();
+    return currentValue;
+}
+
+uint16_t core_util_atomic_exchange_u16(volatile uint16_t *valuePtr, uint16_t desiredValue)
+{
+    MBED_BARRIER();
+    uint16_t currentValue;
+    do {
+        currentValue = __LDREXH(valuePtr);
+    } while (__STREXH(desiredValue, valuePtr));
+    MBED_BARRIER();
+    return currentValue;
+}
+
+uint32_t core_util_atomic_exchange_u32(volatile uint32_t *valuePtr, uint32_t desiredValue)
+{
+    MBED_BARRIER();
+    uint32_t currentValue;
+    do {
+        currentValue = __LDREXW(valuePtr);
+    } while (__STREXW(desiredValue, valuePtr));
+    MBED_BARRIER();
+    return currentValue;
+}
+
 uint8_t core_util_atomic_incr_u8(volatile uint8_t *valuePtr, uint8_t delta)
 {
     MBED_BARRIER();
@@ -298,6 +331,34 @@ bool core_util_atomic_cas_u32(volatile uint32_t *ptr, uint32_t *expectedCurrentV
 }
 
 
+uint8_t core_util_atomic_exchange_u8(volatile uint8_t *ptr, uint8_t desiredValue)
+{
+    core_util_critical_section_enter();
+    uint8_t currentValue = *ptr;
+    *ptr = desiredValue;
+    core_util_critical_section_exit();
+    return currentValue;
+}
+
+uint16_t core_util_atomic_exchange_u16(volatile uint16_t *ptr, uint16_t desiredValue)
+{
+    core_util_critical_section_enter();
+    uint16_t currentValue = *ptr;
+    *ptr = desiredValue;
+    core_util_critical_section_exit();
+    return currentValue;
+}
+
+uint32_t core_util_atomic_exchange_u32(volatile uint32_t *ptr, uint32_t desiredValue)
+{
+    core_util_critical_section_enter();
+    uint32_t currentValue = *ptr;
+    *ptr = desiredValue;
+    core_util_critical_section_exit();
+    return currentValue;
+}
+
+
 uint8_t core_util_atomic_incr_u8(volatile uint8_t *valuePtr, uint8_t delta)
 {
     uint8_t newValue;
@@ -377,6 +438,15 @@ void core_util_atomic_store_u64(volatile uint64_t *valuePtr, uint64_t desiredVal
     core_util_critical_section_exit();
 }
 
+uint64_t core_util_atomic_exchange_u64(volatile uint64_t *valuePtr, uint64_t desiredValue)
+{
+    core_util_critical_section_enter();
+    uint64_t currentValue = *valuePtr;
+    *valuePtr = desiredValue;
+    core_util_critical_section_exit();
+    return currentValue;
+}
+
 bool core_util_atomic_cas_u64(volatile uint64_t *ptr, uint64_t *expectedCurrentValue, uint64_t desiredValue)
 {
     bool success;
@@ -414,6 +484,8 @@ uint64_t core_util_atomic_decr_u64(volatile uint64_t *valuePtr, uint64_t delta)
     return newValue;
 }
 
+MBED_STATIC_ASSERT(sizeof(void *) == sizeof(uint32_t), "Alas, pointers must be 32-bit");
+
 bool core_util_atomic_cas_ptr(void *volatile *ptr, void **expectedCurrentValue, void *desiredValue)
 {
     return core_util_atomic_cas_u32(
@@ -422,6 +494,11 @@ bool core_util_atomic_cas_ptr(void *volatile *ptr, void **expectedCurrentValue,
                (uint32_t)desiredValue);
 }
 
+void *core_util_atomic_exchange_ptr(void *volatile *valuePtr, void *desiredValue)
+{
+    return (void *)core_util_atomic_exchange_u32((volatile uint32_t *)valuePtr, (uint32_t)desiredValue);
+}
+
 void *core_util_atomic_incr_ptr(void *volatile *valuePtr, ptrdiff_t delta)
 {
     return (void *)core_util_atomic_incr_u32((volatile uint32_t *)valuePtr, (uint32_t)delta);
diff --git a/platform/mbed_critical.h b/platform/mbed_critical.h
index c80b73d057..715bd13e56 100644
--- a/platform/mbed_critical.h
+++ b/platform/mbed_critical.h
@@ -354,6 +354,57 @@ MBED_FORCEINLINE void core_util_atomic_store_ptr(void *volatile *valuePtr, void
     MBED_BARRIER();
 }
 
+/**
+ * Atomic exchange.
+ * @param  valuePtr     Target memory location.
+ * @param  desiredValue The value to store.
+ * @return              The previous value.
+ */
+uint8_t core_util_atomic_exchange_u8(volatile uint8_t *valuePtr, uint8_t desiredValue);
+
+/**
+ * Atomic exchange.
+ * @param  valuePtr     Target memory location.
+ * @param  desiredValue The value to store.
+ * @return              The previous value.
+ */
+uint16_t core_util_atomic_exchange_u16(volatile uint16_t *valuePtr, uint16_t desiredValue);
+
+/**
+ * Atomic exchange.
+ * @param  valuePtr     Target memory location.
+ * @param  desiredValue The value to store.
+ * @return              The previous value.
+ */
+uint32_t core_util_atomic_exchange_u32(volatile uint32_t *valuePtr, uint32_t desiredValue);
+
+/**
+ * Atomic exchange.
+ * @param  valuePtr     Target memory location.
+ * @param  desiredValue The value to store.
+ * @return              The previous value.
+ */
+uint64_t core_util_atomic_exchange_u64(volatile uint64_t *valuePtr, uint64_t desiredValue);
+
+/**
+ * Atomic exchange.
+ * @param  valuePtr     Target memory location.
+ * @param  desiredValue The value to store.
+ * @return              The previous value.
+ */
+MBED_FORCEINLINE bool core_util_atomic_exchange_bool(volatile bool *valuePtr, bool desiredValue)
+{
+    return (bool)core_util_atomic_exchange_u8((volatile uint8_t *)valuePtr, desiredValue);
+}
+
+/**
+ * Atomic exchange.
+ * @param  valuePtr     Target memory location.
+ * @param  desiredValue The value to store.
+ * @return              The previous value.
+ */
+void *core_util_atomic_exchange_ptr(void *volatile *valuePtr, void *desiredValue);
+
 /**
  * Atomic increment.
  * @param  valuePtr Target memory location being incremented.