Added l2cap tx,rx

examples/ble_l2cap_client.rs

@@ -1,3 +1,5 @@
+use core::str;
+
 use bstr::ByteSlice;
 use esp32_nimble::{l2cap::L2capClient, BLEClient, BLEDevice};
 use esp_idf_hal::task::block_on;
@@ -24,7 +26,8 @@ fn main() {
 
       let mut l2cap = L2capClient::connect(&client, 0x1002, 512).await.unwrap();
       for i in 0..4 {
-        l2cap.send(format!("test{}", i).as_bytes()).unwrap();
+        l2cap.tx(format!("test{}", i).as_bytes()).unwrap();
+        ::log::info!("< {:?}", str::from_utf8(l2cap.rx().await.data()));
         esp_idf_hal::delay::FreeRtos::delay_ms(1000);
       }
       l2cap.disconnect().await.unwrap();

examples/ble_l2cap_server.rs

@@ -1,5 +1,9 @@
+#![feature(future_join)]
+
 use esp32_nimble::{l2cap::L2capServer, BLEAdvertisementData, BLEDevice};
+use esp_idf_hal::task::block_on;
 use esp_idf_sys as _;
+use std::future::join;
 
 fn main() {
   esp_idf_sys::link_patches();
@@ -16,16 +20,18 @@ fn main() {
     .unwrap();
   ble_advertising.lock().start().unwrap();
 
-  L2capServer::create(0x1001, 512, |data| {
-    ::log::info!("Data received(0x1001): {:X?}", data.sdu_rx());
-  })
-  .unwrap();
-  L2capServer::create(0x1002, 512, |data| {
-    ::log::info!("Data received(0x1002): {:X?}", data.sdu_rx());
-  })
-  .unwrap();
+  let l2cap1 = L2capServer::create(0x1001, 512).unwrap();
+  let l2cap2 = L2capServer::create(0x1002, 512).unwrap();
+
+  block_on(async {
+    join!(run_callback(l2cap1), run_callback(l2cap2)).await;
+  });
+}
 
+async fn run_callback(server: &mut L2capServer) {
   loop {
-    esp_idf_hal::delay::FreeRtos::delay_ms(1000);
+    let recv = server.rx().await;
+    ::log::info!("< {:?}", recv.data());
+    server.tx(recv.data()).unwrap();
   }
 }

src/l2cap/l2cap_client.rs

@@ -2,24 +2,15 @@ use alloc::boxed::Box;
 use core::borrow::BorrowMut;
 use esp_idf_hal::task::block_on;
 
-#[cfg(not(esp_idf_soc_esp_nimble_controller))]
-use esp_idf_sys::os_mbuf_free;
-#[cfg(esp_idf_soc_esp_nimble_controller)]
-use esp_idf_sys::r_os_mbuf_free as os_mbuf_free;
-
-use super::{L2cap, OnDataReceived};
-use crate::{
-  ble,
-  utilities::{os_mbuf_append, voidp_to_ref},
-  BLEClient, BLEError, Signal,
-};
+use super::{L2cap, ReceivedData};
+use crate::{ble, utilities::voidp_to_ref, BLEClient, BLEError, Channel, Signal};
 
 #[allow(clippy::type_complexity)]
 pub struct L2capClient {
   l2cap: L2cap,
   coc_chan: *mut esp_idf_sys::ble_l2cap_chan,
   signal: Signal<u32>,
-  on_data_received: Option<Box<dyn FnMut(OnDataReceived) + Send + Sync>>,
+  channel: Channel<ReceivedData, 1>,
 }
 
 impl L2capClient {
@@ -28,7 +19,7 @@ impl L2capClient {
       l2cap: Default::default(),
       coc_chan: core::ptr::null_mut(),
       signal: Signal::new(),
-      on_data_received: None,
+      channel: Channel::new(),
     });
 
     ret.l2cap.init(mtu, 3)?;
@@ -60,39 +51,12 @@ impl L2capClient {
     Ok(())
   }
 
-  pub fn send(&mut self, data: &[u8]) -> Result<(), BLEError> {
-    let mtu = L2cap::get_chan_info(self.coc_chan).peer_l2cap_mtu as usize;
-    let mut data = data;
-
-    while !data.is_empty() {
-      let sdu_rx = self.l2cap.sdu_rx();
-      let (data0, data1) = data.split_at(if data.len() < mtu { data.len() } else { mtu });
-
-      let rc = os_mbuf_append(sdu_rx, data0);
-      assert_eq!(rc, 0);
-
-      loop {
-        let rc = unsafe { esp_idf_sys::ble_l2cap_send(self.coc_chan, sdu_rx) };
-        match rc as _ {
-          0 | esp_idf_sys::BLE_HS_ESTALLED => break,
-          esp_idf_sys::BLE_HS_EBUSY => {}
-          rc => return BLEError::convert(rc),
-        }
-        unsafe { esp_idf_sys::vPortYield() };
-      }
-
-      data = data1;
-    }
-
-    Ok(())
+  pub fn tx(&mut self, data: &[u8]) -> Result<(), BLEError> {
+    self.l2cap.tx(self.coc_chan, data)
   }
 
-  pub fn on_data_received(
-    &mut self,
-    callback: impl FnMut(OnDataReceived) + Send + Sync + 'static,
-  ) -> &mut Self {
-    self.on_data_received = Some(Box::new(callback));
-    self
+  pub async fn rx(&mut self) -> ReceivedData {
+    self.channel.receive().await
   }
 
   pub(crate) extern "C" fn blecent_l2cap_coc_event_cb(
@@ -126,11 +90,7 @@ impl L2capClient {
       esp_idf_sys::BLE_L2CAP_EVENT_COC_DATA_RECEIVED => {
         let receive = unsafe { event.__bindgen_anon_1.receive };
         if !receive.sdu_rx.is_null() {
-          if let Some(callback) = &mut client.on_data_received {
-            callback(OnDataReceived::from_raw(receive));
-          } else {
-            unsafe { os_mbuf_free(receive.sdu_rx) };
-          }
+          let _ = client.channel.try_send(ReceivedData::from_raw(receive));
         }
         client.l2cap.ble_l2cap_recv_ready(receive.chan);
         0

src/l2cap/l2cap_core.rs

@@ -0,0 +1,97 @@
+use crate::{ble, utilities::os_mbuf_append, BLEError};
+use alloc::vec::Vec;
+
+#[derive(Default)]
+pub struct L2cap {
+  mempool: esp_idf_sys::os_mempool,
+  mbuf_pool: esp_idf_sys::os_mbuf_pool,
+  coc_memory: Vec<esp_idf_sys::os_membuf_t>,
+}
+
+impl L2cap {
+  pub fn init(&mut self, mtu: u16, coc_buf_count: u16) -> Result<(), BLEError> {
+    self
+      .coc_memory
+      .reserve_exact(os_mempool_size(coc_buf_count as _, mtu as _));
+
+    unsafe {
+      ble!(super::os_mempool_init(
+        &mut self.mempool,
+        coc_buf_count,
+        mtu as _,
+        self.coc_memory.as_mut_ptr() as _,
+        c"coc_sdu_pool".as_ptr()
+      ))?;
+
+      ble!(super::os_mbuf_pool_init(
+        &mut self.mbuf_pool as _,
+        &mut self.mempool as _,
+        mtu,
+        coc_buf_count
+      ))?;
+    }
+
+    Ok(())
+  }
+
+  pub fn tx(
+    &mut self,
+    chan: *mut esp_idf_sys::ble_l2cap_chan,
+    data: &[u8],
+  ) -> Result<(), BLEError> {
+    let mtu = L2cap::get_chan_info(chan).peer_l2cap_mtu as usize;
+    let mut data = data;
+
+    while !data.is_empty() {
+      let sdu_rx = self.sdu_rx();
+      let (data0, data1) = data.split_at(if data.len() < mtu { data.len() } else { mtu });
+
+      let rc = os_mbuf_append(sdu_rx, data0);
+      assert_eq!(rc, 0);
+
+      loop {
+        let rc = unsafe { esp_idf_sys::ble_l2cap_send(chan, sdu_rx) };
+        match rc as _ {
+          0 | esp_idf_sys::BLE_HS_ESTALLED => break,
+          esp_idf_sys::BLE_HS_EBUSY => {}
+          rc => return BLEError::convert(rc),
+        }
+        unsafe { esp_idf_sys::vPortYield() };
+      }
+
+      data = data1;
+    }
+
+    Ok(())
+  }
+
+  pub fn sdu_rx(&mut self) -> *mut esp_idf_sys::os_mbuf {
+    loop {
+      let ret = unsafe { super::os_mbuf_get_pkthdr(&mut self.mbuf_pool, 0) };
+      if !ret.is_null() {
+        return ret;
+      }
+      esp_idf_hal::delay::FreeRtos::delay_ms(10);
+    }
+  }
+
+  pub(crate) fn ble_l2cap_recv_ready(&mut self, chan: *mut esp_idf_sys::ble_l2cap_chan) -> i32 {
+    let sdu_rx = self.sdu_rx();
+    unsafe { esp_idf_sys::ble_l2cap_recv_ready(chan, sdu_rx) }
+  }
+
+  pub(crate) fn get_chan_info(
+    chan: *mut esp_idf_sys::ble_l2cap_chan,
+  ) -> esp_idf_sys::ble_l2cap_chan_info {
+    let mut chan_info = esp_idf_sys::ble_l2cap_chan_info::default();
+    let rc = unsafe { esp_idf_sys::ble_l2cap_get_chan_info(chan, &mut chan_info as _) };
+    assert_eq!(rc, 0);
+    chan_info
+  }
+}
+
+#[inline]
+const fn os_mempool_size(n: usize, blksize: usize) -> usize {
+  let size = core::mem::size_of::<esp_idf_sys::os_membuf_t>();
+  blksize.div_ceil(size) * n
+}

src/l2cap/l2cap_server.rs

@@ -1,15 +1,8 @@
-use alloc::boxed::Box;
-
-#[cfg(not(esp_idf_soc_esp_nimble_controller))]
-use esp_idf_sys::os_mbuf_free;
-#[cfg(esp_idf_soc_esp_nimble_controller)]
-use esp_idf_sys::r_os_mbuf_free as os_mbuf_free;
-
-use super::{L2cap, OnDataReceived};
+use super::{L2cap, ReceivedData};
 use crate::{
   ble,
-  utilities::{mutex::Mutex, voidp_to_ref},
-  BLEError,
+  utilities::{extend_lifetime_mut, mutex::Mutex, voidp_to_ref},
+  BLEError, Channel,
 };
 
 const N: usize = esp_idf_sys::CONFIG_BT_NIMBLE_L2CAP_COC_MAX_NUM as usize;
@@ -18,22 +11,20 @@ static SERVER_LIST: Mutex<heapless::Vec<L2capServer, N>> = Mutex::new(heapless::
 #[allow(clippy::type_complexity)]
 pub struct L2capServer {
   l2cap: L2cap,
+  coc_chan: *mut esp_idf_sys::ble_l2cap_chan,
   peer_sdu_size: u16,
-  on_data_received: Box<dyn FnMut(OnDataReceived) + Send + Sync>,
+  channel: Channel<ReceivedData, 1>,
 }
 
 impl L2capServer {
-  pub fn create(
-    psm: u16,
-    mtu: u16,
-    on_data_received: impl FnMut(OnDataReceived) + Send + Sync + 'static,
-  ) -> Result<(), BLEError> {
+  pub fn create(psm: u16, mtu: u16) -> Result<&'static mut L2capServer, BLEError> {
     let mut list = SERVER_LIST.lock();
     list
       .push(L2capServer {
         l2cap: Default::default(),
+        coc_chan: core::ptr::null_mut(),
         peer_sdu_size: 0,
-        on_data_received: Box::new(on_data_received),
+        channel: Channel::new(),
       })
       .map_err(|_| BLEError::convert(esp_idf_sys::BLE_HS_ENOMEM as _).unwrap_err())?;
 
@@ -48,7 +39,15 @@ impl L2capServer {
         server as *mut Self as _,
       ))?;
     }
-    Ok(())
+    Ok(unsafe { extend_lifetime_mut(server) })
+  }
+
+  pub fn tx(&mut self, data: &[u8]) -> Result<(), BLEError> {
+    self.l2cap.tx(self.coc_chan, data)
+  }
+
+  pub async fn rx(&mut self) -> ReceivedData {
+    self.channel.receive().await
   }
 
   pub(crate) extern "C" fn handle_l2cap_event(
@@ -66,6 +65,13 @@ impl L2capServer {
           return 0;
         }
 
+        server.coc_chan = connect.chan;
+        0
+      }
+      esp_idf_sys::BLE_L2CAP_EVENT_COC_DISCONNECTED => {
+        let disconnect = unsafe { event.__bindgen_anon_1.disconnect };
+        ::log::debug!("LE CoC disconnected: {:?}", disconnect.chan);
+        server.coc_chan = core::ptr::null_mut();
         0
       }
       esp_idf_sys::BLE_L2CAP_EVENT_COC_ACCEPT => {
@@ -77,8 +83,7 @@ impl L2capServer {
       esp_idf_sys::BLE_L2CAP_EVENT_COC_DATA_RECEIVED => {
         let receive = unsafe { event.__bindgen_anon_1.receive };
         if !receive.sdu_rx.is_null() {
-          (server.on_data_received)(OnDataReceived::from_raw(receive));
-          unsafe { os_mbuf_free(receive.sdu_rx) };
+          let _ = server.channel.try_send(ReceivedData::from_raw(receive));
         }
         server.l2cap.ble_l2cap_recv_ready(receive.chan);
         0

src/l2cap/mod.rs

@@ -1,9 +1,19 @@
+#[cfg(not(esp_idf_soc_esp_nimble_controller))]
+use esp_idf_sys::{os_mbuf_free, os_mbuf_get_pkthdr, os_mbuf_pool_init, os_mempool_init};
+#[cfg(esp_idf_soc_esp_nimble_controller)]
+use esp_idf_sys::{
+  r_os_mbuf_free as os_mbuf_free, r_os_mbuf_get_pkthdr as os_mbuf_get_pkthdr,
+  r_os_mbuf_pool_init as os_mbuf_pool_init, r_os_mempool_init as os_mempool_init,
+};
+
 mod l2cap_client;
 pub use l2cap_client::L2capClient;
 
 mod l2cap_server;
 pub use l2cap_server::L2capServer;
 
+mod l2cap_core;
+use l2cap_core::L2cap;
+
 mod utilities;
-pub(crate) use utilities::L2cap;
-pub use utilities::OnDataReceived;
+pub use utilities::ReceivedData;