diff options
| -rw-r--r-- | examples/src/bin/ble_l2cap_central.rs | 3 | ||||
| -rw-r--r-- | nrf-softdevice/src/ble/l2cap.rs | 56 |
2 files changed, 47 insertions, 12 deletions
diff --git a/examples/src/bin/ble_l2cap_central.rs b/examples/src/bin/ble_l2cap_central.rs index 55ff261..179fff7 100644 --- a/examples/src/bin/ble_l2cap_central.rs +++ b/examples/src/bin/ble_l2cap_central.rs @@ -90,7 +90,7 @@ async fn ble_central_task(sd: &'static Softdevice) { for i in 0..10 { let mut v = Vec::with_capacity(Packet::MTU); v.extend(&[i; Packet::MTU]); - unwrap!(ch.tx(Packet(v))); + unwrap!(ch.tx(Packet(v)).await); info!("l2cap tx done"); } futures::future::pending::<()>().await; @@ -98,6 +98,7 @@ async fn ble_central_task(sd: &'static Softdevice) { use alloc::vec::Vec; +#[derive(defmt::Format)] struct Packet(Vec<u8>); impl l2cap::Packet for Packet { const MTU: usize = 512; diff --git a/nrf-softdevice/src/ble/l2cap.rs b/nrf-softdevice/src/ble/l2cap.rs index 689fd1b..b5bf219 100644 --- a/nrf-softdevice/src/ble/l2cap.rs +++ b/nrf-softdevice/src/ble/l2cap.rs @@ -24,6 +24,7 @@ pub(crate) unsafe fn on_evt(ble_evt: *const raw::ble_evt_t) { raw::BLE_L2CAP_EVTS_BLE_L2CAP_EVT_CH_TX => { let params = &l2cap_evt.params.tx; let pkt = unwrap!(NonNull::new(params.sdu_buf.p_data)); + portal(l2cap_evt.conn_handle).call(ble_evt); (unwrap!(PACKET_FREE))(pkt) } _ => { @@ -33,18 +34,19 @@ pub(crate) unsafe fn on_evt(ble_evt: *const raw::ble_evt_t) { } #[cfg_attr(feature = "defmt", derive(defmt::Format))] -pub enum TxError { +pub enum TxError<P: Packet> { Disconnected, + TxQueueFull(P), Raw(RawError), } -impl From<DisconnectedError> for TxError { +impl<P: Packet> From<DisconnectedError> for TxError<P> { fn from(_err: DisconnectedError) -> Self { TxError::Disconnected } } -impl From<RawError> for TxError { +impl<P: Packet> From<RawError> for TxError<P> { fn from(err: RawError) -> Self { TxError::Raw(err) } @@ -303,7 +305,7 @@ impl<P: Packet> Channel<P> { &self.conn } - pub fn tx(&self, sdu: P) -> Result<(), TxError> { + pub fn try_tx(&self, sdu: P) -> Result<(), TxError<P>> { let conn_handle = self.conn.with_state(|s| s.check_connected())?; let (ptr, len) = sdu.into_raw_parts(); @@ -314,15 +316,47 @@ impl<P: Packet> Channel<P> { }; let ret = unsafe { raw::sd_ble_l2cap_ch_tx(conn_handle, self.cid, &data) }; - if let Err(err) = RawError::convert(ret) { - warn!("sd_ble_l2cap_ch_tx err {:?}", err); - // The SD didn't take ownership of the buffer, so it's on us to free it. - // Reconstruct the P and let it get dropped. - unsafe { P::from_raw_parts(ptr, len) }; - return Err(err.into()); + match RawError::convert(ret) { + Err(RawError::Resources) => { + Err(TxError::TxQueueFull(unsafe { P::from_raw_parts(ptr, len) })) + } + Err(err) => { + warn!("sd_ble_l2cap_ch_tx err {:?}", err); + // The SD didn't take ownership of the buffer, so it's on us to free it. + // Reconstruct the P and let it get dropped. + unsafe { P::from_raw_parts(ptr, len) }; + + Err(err.into()) + } + Ok(()) => Ok(()), } + } + + pub async fn tx(&self, mut sdu: P) -> Result<(), TxError<P>> { + let conn_handle = self.conn.with_state(|s| s.check_connected())?; - Ok(()) + loop { + match self.try_tx(sdu) { + Ok(()) => { + return Ok(()); + } + Err(TxError::TxQueueFull(ret_sdu)) => { + sdu = ret_sdu; + portal(conn_handle) + .wait_once(|ble_evt| unsafe { + match (*ble_evt).header.evt_id as u32 { + raw::BLE_L2CAP_EVTS_BLE_L2CAP_EVT_CH_TX => (), + _ => unreachable!("Invalid event"), + } + }) + .await; + continue; + } + Err(e) => { + return Err(e); + } + } + } } pub async fn rx(&self) -> Result<P, RxError> { |