From 9f96b42e0745544ba5cacc5ed925390ea76d55a5 Mon Sep 17 00:00:00 2001
From: timvisee <tim@visee.me>
Date: Thu, 12 Nov 2020 12:30:54 +0100
Subject: [PATCH] Implement atomic fetch_or and fetch_and for thumbv6

---
 core/src/bbbuffer.rs | 45 +++++++++++++++++++++++++++++++++++---------
 1 file changed, 36 insertions(+), 9 deletions(-)

diff --git a/core/src/bbbuffer.rs b/core/src/bbbuffer.rs
index 0c5fc9d..428d035 100644
--- a/core/src/bbbuffer.rs
+++ b/core/src/bbbuffer.rs
@@ -96,9 +96,8 @@ impl<'a, const N: usize> BBBuffer<N> {
     /// ```
     pub fn try_split(&'a self) -> Result<(Producer<'a, N>, Consumer<'a, N>)> {
         // Set producer/consumer taken bit, error and reset if one was already set
-        let prev = self
-            .split_prod_cons
-            .fetch_or(BIT_PRODUCER | BIT_CONSUMER, AcqRel);
+        let prev = atomic::fetch_or(&self.split_prod_cons, BIT_PRODUCER | BIT_CONSUMER, AcqRel);
+
         if prev > 0 {
             self.split_prod_cons.store(prev, Release);
             return Err(Error::AlreadySplit);
@@ -157,7 +156,7 @@ impl<'a, const N: usize> BBBuffer<N> {
     /// while splitting.
     pub fn try_take_producer(&'a self) -> Result<Producer<'a, N>> {
         // Set producer taken bit, error if already set
-        if self.split_prod_cons.fetch_or(BIT_PRODUCER, AcqRel) & BIT_PRODUCER > 0 {
+        if atomic::fetch_or(&self.split_prod_cons, BIT_PRODUCER, AcqRel) & BIT_PRODUCER > 0 {
             return Err(Error::AlreadySplit);
         }
 
@@ -191,7 +190,7 @@ impl<'a, const N: usize> BBBuffer<N> {
     /// while splitting.
     pub fn try_take_consumer(&'a self) -> Result<Consumer<'a, N>> {
         // Set producer taken bit, error if already set
-        if self.split_prod_cons.fetch_or(BIT_CONSUMER, AcqRel) & BIT_CONSUMER > 0 {
+        if atomic::fetch_or(&self.split_prod_cons, BIT_CONSUMER, AcqRel) & BIT_CONSUMER > 0 {
             return Err(Error::AlreadySplit);
         }
 
@@ -376,7 +375,7 @@ impl<'a, const N: usize> BBBuffer<N> {
         self.last.store(0, Release);
 
         // Mark the buffer as ready to retake producer
-        self.split_prod_cons.fetch_and(!BIT_PRODUCER, Release);
+        atomic::fetch_and(&self.split_prod_cons, !BIT_PRODUCER, Release);
 
         Ok(())
     }
@@ -447,7 +446,7 @@ impl<'a, const N: usize> BBBuffer<N> {
         self.last.store(0, Release);
 
         // Mark the buffer as ready to retake consumer
-        self.split_prod_cons.fetch_and(!BIT_CONSUMER, Release);
+        atomic::fetch_and(&self.split_prod_cons, !BIT_CONSUMER, Release);
 
         Ok(())
     }
@@ -1329,7 +1328,7 @@ impl<'a, const N: usize> DerefMut for GrantR<'a, N> {
 #[cfg(feature = "thumbv6")]
 mod atomic {
     use core::sync::atomic::{
-        AtomicBool, AtomicUsize,
+        AtomicBool, AtomicU8, AtomicUsize,
         Ordering::{self, Acquire, Release},
     };
     use cortex_m::interrupt::free;
@@ -1352,6 +1351,24 @@ mod atomic {
         })
     }
 
+    #[inline(always)]
+    pub fn fetch_and(atomic: &AtomicU8, val: u8, _order: Ordering) -> u8 {
+        free(|_| {
+            let prev = atomic.load(Acquire);
+            atomic.store(prev & val, Release);
+            prev
+        })
+    }
+
+    #[inline(always)]
+    pub fn fetch_or(atomic: &AtomicU8, val: u8, _order: Ordering) -> u8 {
+        free(|_| {
+            let prev = atomic.load(Acquire);
+            atomic.store(prev | val, Release);
+            prev
+        })
+    }
+
     #[inline(always)]
     pub fn swap(atomic: &AtomicBool, val: bool, _order: Ordering) -> bool {
         free(|_| {
@@ -1364,7 +1381,7 @@ mod atomic {
 
 #[cfg(not(feature = "thumbv6"))]
 mod atomic {
-    use core::sync::atomic::{AtomicBool, AtomicUsize, Ordering};
+    use core::sync::atomic::{AtomicBool, AtomicU8, AtomicUsize, Ordering};
 
     #[inline(always)]
     pub fn fetch_add(atomic: &AtomicUsize, val: usize, order: Ordering) -> usize {
@@ -1376,6 +1393,16 @@ mod atomic {
         atomic.fetch_sub(val, order)
     }
 
+    #[inline(always)]
+    pub fn fetch_and(atomic: &AtomicU8, val: u8, order: Ordering) -> u8 {
+        atomic.fetch_and(val, order)
+    }
+
+    #[inline(always)]
+    pub fn fetch_or(atomic: &AtomicU8, val: u8, order: Ordering) -> u8 {
+        atomic.fetch_or(val, order)
+    }
+
     #[inline(always)]
     pub fn swap(atomic: &AtomicBool, val: bool, order: Ordering) -> bool {
         atomic.swap(val, order)