From 3c39057090fe5dcc7ddc06543d3f47db912077e8 Mon Sep 17 00:00:00 2001
From: bjoernQ <bjoern.quentin@mobile-j.de>
Date: Wed, 23 Oct 2024 16:53:57 +0200
Subject: [PATCH] Move locking into RawQueue

---
 esp-wifi/src/ble/btdm.rs      | 29 +++++++---------
 esp-wifi/src/ble/npl.rs       |  2 +-
 esp-wifi/src/compat/common.rs | 65 +++++++++++++++++++----------------
 3 files changed, 49 insertions(+), 47 deletions(-)

diff --git a/esp-wifi/src/ble/btdm.rs b/esp-wifi/src/ble/btdm.rs
index d89ad840afb..f730696de32 100644
--- a/esp-wifi/src/ble/btdm.rs
+++ b/esp-wifi/src/ble/btdm.rs
@@ -191,10 +191,9 @@ unsafe extern "C" fn queue_send(queue: *const (), item: *const (), _block_time_m
         item as u32,
         _block_time_ms
     );
-    critical_section::with(|_| {
-        let queue = queue as *mut RawQueue;
-        (*queue).enqueue(item as *mut _)
-    })
+
+    let queue = queue as *mut RawQueue;
+    (*queue).enqueue(item as *mut _)
 }
 
 #[ram]
@@ -224,19 +223,15 @@ unsafe extern "C" fn queue_recv(queue: *const (), item: *const (), block_time_ms
     let item = item as *mut _;
 
     loop {
-        let res = critical_section::with(|_| {
-            memory_fence();
-
-            critical_section::with(|_cs| {
-                let queue = queue as *mut RawQueue;
-                if (*queue).try_dequeue(item) {
-                    trace!("received from queue");
-                    1
-                } else {
-                    0
-                }
-            })
-        });
+        let res = {
+            let queue = queue as *mut RawQueue;
+            if (*queue).try_dequeue(item) {
+                trace!("received from queue");
+                1
+            } else {
+                0
+            }
+        };
 
         if res == 1 {
             trace!("queue_recv returns");
diff --git a/esp-wifi/src/ble/npl.rs b/esp-wifi/src/ble/npl.rs
index 910cf400a32..3511bf9b37b 100644
--- a/esp-wifi/src/ble/npl.rs
+++ b/esp-wifi/src/ble/npl.rs
@@ -947,7 +947,7 @@ unsafe extern "C" fn ble_npl_eventq_deinit(queue: *const ble_npl_eventq) {
         panic!("Trying to deinitialize an uninitialized queue");
     } else {
         let raw_queue = (*queue).dummy as *mut RawQueue;
-        (*raw_queue).free_storage();
+        (*raw_queue).release_storage();
         crate::compat::malloc::free(raw_queue.cast());
         (*queue).dummy = 0;
     }
diff --git a/esp-wifi/src/compat/common.rs b/esp-wifi/src/compat/common.rs
index d587f7acb6e..12d10ddb236 100644
--- a/esp-wifi/src/compat/common.rs
+++ b/esp-wifi/src/compat/common.rs
@@ -47,7 +47,7 @@ impl RawQueue {
         }
     }
 
-    pub(crate) fn free_storage(&mut self) {
+    pub(crate) fn release_storage(&mut self) {
         unsafe {
             free(self.storage);
         }
@@ -55,38 +55,45 @@ impl RawQueue {
     }
 
     pub(crate) fn enqueue(&mut self, item: *mut c_void) -> i32 {
-        if self.count() < self.count {
-            unsafe {
-                let p = self.storage.byte_add(self.item_size * self.current_write);
-                p.copy_from(item as *mut u8, self.item_size);
-                self.current_write = (self.current_write + 1) % self.count;
-            }
+        critical_section::with(|_| {
+            if self.count() < self.count {
+                unsafe {
+                    let p = self.storage.byte_add(self.item_size * self.current_write);
+                    p.copy_from(item as *mut u8, self.item_size);
+                    self.current_write = (self.current_write + 1) % self.count;
+                }
 
-            1
-        } else {
-            0
-        }
+                1
+            } else {
+                0
+            }
+        })
     }
 
     pub(crate) fn try_dequeue(&mut self, item: *mut c_void) -> bool {
-        if self.count() > 0 {
-            unsafe {
-                let p = self.storage.byte_add(self.item_size * self.current_read) as *const c_void;
-                item.copy_from(p, self.item_size);
-                self.current_read = (self.current_read + 1) % self.count;
+        critical_section::with(|_| {
+            if self.count() > 0 {
+                unsafe {
+                    let p =
+                        self.storage.byte_add(self.item_size * self.current_read) as *const c_void;
+                    item.copy_from(p, self.item_size);
+                    self.current_read = (self.current_read + 1) % self.count;
+                }
+                true
+            } else {
+                false
             }
-            true
-        } else {
-            false
-        }
+        })
     }
 
     pub(crate) fn count(&self) -> usize {
-        if self.current_write >= self.current_read {
-            self.current_write - self.current_read
-        } else {
-            self.count - self.current_read + self.current_write
-        }
+        critical_section::with(|_| {
+            if self.current_write >= self.current_read {
+                self.current_write - self.current_read
+            } else {
+                self.count - self.current_read + self.current_write
+            }
+        })
     }
 }
 
@@ -267,7 +274,7 @@ pub(crate) fn delete_queue(queue: *mut c_void) {
 
     let queue: *mut RawQueue = queue.cast();
     unsafe {
-        (*queue).free_storage();
+        (*queue).release_storage();
         free(queue.cast());
     }
 }
@@ -281,7 +288,7 @@ pub(crate) fn send_queued(queue: *mut c_void, item: *mut c_void, block_time_tick
     );
 
     let queue: *mut RawQueue = queue.cast();
-    critical_section::with(|_| unsafe { (*queue).enqueue(item) })
+    unsafe { (*queue).enqueue(item) }
 }
 
 pub(crate) fn receive_queued(queue: *mut c_void, item: *mut c_void, block_time_tick: u32) -> i32 {
@@ -299,7 +306,7 @@ pub(crate) fn receive_queued(queue: *mut c_void, item: *mut c_void, block_time_t
     let queue: *mut RawQueue = queue.cast();
 
     loop {
-        if critical_section::with(|_| unsafe { (*queue).try_dequeue(item) }) {
+        if unsafe { (*queue).try_dequeue(item) } {
             trace!("received");
             return 1;
         }
@@ -317,7 +324,7 @@ pub(crate) fn number_of_messages_in_queue(queue: *const c_void) -> u32 {
     trace!("queue_msg_waiting {:?}", queue);
 
     let queue: *const RawQueue = queue.cast();
-    critical_section::with(|_| unsafe { (*queue).count() as u32 })
+    unsafe { (*queue).count() as u32 }
 }
 
 /// Implementation of sleep() from newlib in esp-idf.