From afd0671ec30c4d531d9570af964726ec8e7520bb Mon Sep 17 00:00:00 2001 From: Ulf Lilleengen Date: Wed, 22 May 2024 13:41:54 +0200 Subject: [PATCH] handle att mtu even if gatt is not enabled Handle error conditions more gracefully --- host/src/gatt.rs | 42 +++++++-------------- host/src/host.rs | 83 +++++++++++++++++++++++++++++++++-------- host/src/lib.rs | 7 ++++ host/src/packet_pool.rs | 12 ++---- 4 files changed, 91 insertions(+), 53 deletions(-) diff --git a/host/src/gatt.rs b/host/src/gatt.rs index 0e2e0776..84370282 100644 --- a/host/src/gatt.rs +++ b/host/src/gatt.rs @@ -5,7 +5,7 @@ use bt_hci::param::ConnHandle; use embassy_sync::blocking_mutex::raw::RawMutex; use embassy_sync::channel::DynamicReceiver; -use crate::att::{self, Att, ATT_HANDLE_VALUE_NTF_OPTCODE}; +use crate::att::{Att, ATT_HANDLE_VALUE_NTF_OPTCODE}; use crate::attribute::CharacteristicHandle; use crate::attribute_server::AttributeServer; use crate::connection::Connection; @@ -39,13 +39,12 @@ impl<'reference, 'values, 'resources, M: RawMutex, T: Controller, const MAX: usi let mut w = WriteCursor::new(response.as_mut()); let (mut header, mut data) = w.split(4)?; - match att { - Att::ExchangeMtu { mtu } => { - let mtu = self.connections.exchange_att_mtu(handle, mtu); - data.write(att::ATT_EXCHANGE_MTU_RESPONSE_OPCODE)?; - data.write(mtu)?; - - header.write(data.len() as u16)?; + match self.server.process(handle, att, data.write_buf()) { + Ok(Some(written)) => { + let mtu = self.connections.get_att_mtu(handle); + data.commit(written)?; + data.truncate(mtu as usize); + header.write(written as u16)?; header.write(4_u16)?; let len = header.len() + data.len(); self.tx @@ -54,27 +53,12 @@ impl<'reference, 'values, 'resources, M: RawMutex, T: Controller, const MAX: usi .send(Pdu::new(response, len).as_ref()) .await?; } - _ => match self.server.process(handle, att, data.write_buf()) { - Ok(Some(written)) => { - let mtu = self.connections.get_att_mtu(handle); - data.commit(written)?; - data.truncate(mtu as usize); - header.write(written as u16)?; - header.write(4_u16)?; - let len = header.len() + data.len(); - self.tx - .acl(handle, 1) - .await? - .send(Pdu::new(response, len).as_ref()) - .await?; - } - Ok(None) => { - debug!("No response sent"); - } - Err(e) => { - warn!("Error processing attribute: {:?}", e); - } - }, + Ok(None) => { + debug!("No response sent"); + } + Err(e) => { + warn!("Error processing attribute: {:?}", e); + } } } Err(e) => { diff --git a/host/src/host.rs b/host/src/host.rs index df933926..26912f2a 100644 --- a/host/src/host.rs +++ b/host/src/host.rs @@ -24,7 +24,7 @@ use bt_hci::param::{ FilterDuplicates, InitiatingPhy, LeEventMask, Operation, PhyParams, ScanningPhy, }; use bt_hci::{ControllerToHostPacket, FromHciBytes, WriteHci}; -use embassy_futures::select::{select, Either}; +use embassy_futures::select::{select, select3, Either, Either3}; use embassy_sync::blocking_mutex::raw::NoopRawMutex; use embassy_sync::channel::{Channel, TryReceiveError}; use embassy_sync::once_lock::OnceLock; @@ -33,7 +33,7 @@ use futures::pin_mut; use crate::advertise::{Advertisement, AdvertisementParameters, AdvertisementSet, RawAdvertisement}; use crate::channel_manager::{ChannelManager, ChannelStorage, PacketChannel}; use crate::connection::{ConnectConfig, Connection}; -use crate::connection_manager::{ConnectionManager, ConnectionStorage, PacketGrant}; +use crate::connection_manager::{ConnectionManager, ConnectionStorage, DynamicConnectionManager, PacketGrant}; use crate::cursor::WriteCursor; use crate::l2cap::sar::{PacketReassembly, SarType, EMPTY_SAR}; use crate::packet_pool::{AllocId, GlobalPacketPool, PacketPool, Qos}; @@ -42,9 +42,9 @@ use crate::scan::{PhySet, ScanConfig, ScanReport}; use crate::types::l2cap::{ L2capHeader, L2capSignal, L2capSignalHeader, L2CAP_CID_ATT, L2CAP_CID_DYN_START, L2CAP_CID_LE_U_SIGNAL, }; +use crate::{att, Address, BleHostError, Error}; #[cfg(feature = "gatt")] use crate::{attribute::AttributeTable, gatt::GattServer}; -use crate::{Address, BleHostError, Error}; const L2CAP_RXQ: usize = 1; @@ -97,6 +97,7 @@ pub struct BleHost<'d, T> { pub(crate) channels: ChannelManager<'d, L2CAP_RXQ>, pub(crate) att_inbound: Channel, pub(crate) pool: &'static dyn GlobalPacketPool, + outbound: Channel, pub(crate) scanner: Channel, 1>, advertiser_terminations: Channel, @@ -138,6 +139,7 @@ where att_inbound: Channel::new(), scanner: Channel::new(), advertiser_terminations: Channel::new(), + outbound: Channel::new(), } } @@ -601,10 +603,18 @@ where } async fn handle_acl(&self, acl: AclPacket<'_>) -> Result<(), Error> { - let (header, packet) = match acl.boundary_flag() { + let (header, mut packet) = match acl.boundary_flag() { AclPacketBoundary::FirstFlushable => { let (header, data) = L2capHeader::from_hci_bytes(acl.data())?; + // Ignore channels we don't support + if header.channel < L2CAP_CID_DYN_START + && !(&[L2CAP_CID_LE_U_SIGNAL, L2CAP_CID_ATT].contains(&header.channel)) + { + warn!("[host] unsupported l2cap channel id {}", header.channel); + return Ok(()); + } + // Avoids using the packet buffer for signalling packets if header.channel == L2CAP_CID_LE_U_SIGNAL { assert!(data.len() == header.length as usize); @@ -614,7 +624,7 @@ where let Some(mut p) = self.pool.alloc(AllocId::from_channel(header.channel)) else { info!("No memory for packets on channel {}", header.channel); - return Err(Error::OutOfMemory); + return Err(Error::OutOfMemory.into()); }; p.as_mut()[..data.len()].copy_from_slice(data); @@ -636,18 +646,40 @@ where } other => { warn!("Unexpected boundary flag: {:?}!", other); - return Err(Error::NotSupported); + return Err(Error::NotSupported.into()); } }; match header.channel { L2CAP_CID_ATT => { - #[cfg(feature = "gatt")] - self.att_inbound - .send((acl.handle(), Pdu::new(packet, header.length as usize))) - .await; - #[cfg(not(feature = "gatt"))] - return Err(Error::NotSupported); + // Handle ATT MTU exchange here since it doesn't strictly require + // gatt to be enabled. + if let att::Att::ExchangeMtu { mtu } = att::Att::decode(&packet.as_ref()[..header.length as usize])? { + let mut w = WriteCursor::new(packet.as_mut()); + + let mtu = self.connections.exchange_att_mtu(acl.handle(), mtu); + + let l2cap = L2capHeader { + channel: L2CAP_CID_ATT, + length: 3, + }; + + w.write_hci(&l2cap)?; + w.write(att::ATT_EXCHANGE_MTU_RESPONSE_OPCODE)?; + w.write(mtu)?; + let len = w.len(); + w.finish(); + + self.outbound.send((acl.handle(), Pdu::new(packet, len))).await; + } else { + #[cfg(feature = "gatt")] + self.att_inbound + .send((acl.handle(), Pdu::new(packet, header.length as usize))) + .await; + + #[cfg(not(feature = "gatt"))] + return Err(Error::NotSupported.into()); + } } L2CAP_CID_LE_U_SIGNAL => { panic!("le signalling channel was fragmented, impossible!"); @@ -753,6 +785,25 @@ where }; pin_mut!(control_fut); + let tx_fut = async { + loop { + let (conn, pdu) = self.outbound.receive().await; + match self.acl(conn, 1).await { + Ok(mut sender) => { + if let Err(e) = sender.send(pdu.as_ref()).await { + warn!("[host] error sending outbound pdu"); + return Err(e); + } + } + Err(e) => { + warn!("[host] error requesting sending outbound pdu"); + return Err(e); + } + } + } + }; + pin_mut!(tx_fut); + loop { // Task handling receiving data from the controller. let rx_fut = async { @@ -848,9 +899,11 @@ where }; // info!("Entering select loop"); - let result: Result<(), BleHostError> = match select(&mut control_fut, rx_fut).await { - Either::First(result) => result, - Either::Second(result) => result, + let result: Result<(), BleHostError> = match select3(&mut control_fut, rx_fut, &mut tx_fut).await + { + Either3::First(result) => result, + Either3::Second(result) => result, + Either3::Third(result) => result, }; result?; } diff --git a/host/src/lib.rs b/host/src/lib.rs index 51edc348..06fb1405 100644 --- a/host/src/lib.rs +++ b/host/src/lib.rs @@ -72,6 +72,7 @@ pub enum BleHostError { pub enum Error { HciEncode(bt_hci::param::Error), HciDecode(FromHciBytesError), + Att(att::AttDecodeError), InsufficientSpace, InvalidValue, Advertisement(AdvertisementDataError), @@ -95,6 +96,12 @@ impl From for BleHostError { } } +impl From for Error { + fn from(error: att::AttDecodeError) -> Self { + Self::Att(error) + } +} + impl From for Error { fn from(error: FromHciBytesError) -> Self { Self::HciDecode(error) diff --git a/host/src/packet_pool.rs b/host/src/packet_pool.rs index c456f80f..8f8be5e2 100644 --- a/host/src/packet_pool.rs +++ b/host/src/packet_pool.rs @@ -6,7 +6,6 @@ use embassy_sync::blocking_mutex::Mutex; use crate::types::l2cap::{L2CAP_CID_ATT, L2CAP_CID_DYN_START}; // Generic client ID used by ATT PDU -#[cfg(feature = "gatt")] pub(crate) const ATT_ID: AllocId = AllocId(0); #[derive(Clone, Copy, Debug)] @@ -16,22 +15,18 @@ pub struct AllocId(usize); impl AllocId { pub fn dynamic(idx: usize) -> AllocId { // Dynamic range starts at 2 - #[cfg(feature = "gatt")] return AllocId(1 + idx); - #[cfg(not(feature = "gatt"))] - return AllocId(idx); } pub fn from_channel(cid: u16) -> AllocId { match cid { L2CAP_CID_ATT => { - #[cfg(feature = "gatt")] return ATT_ID; - #[cfg(not(feature = "gatt"))] - panic!("gatt feature must be enabled to support gatt"); } cid if cid >= L2CAP_CID_DYN_START => Self::dynamic((cid - L2CAP_CID_DYN_START) as usize), - _ => unimplemented!(), + cid => { + panic!("unexpected channel id {}", cid); + } } } } @@ -147,7 +142,6 @@ pub struct PacketPool PacketPool { pub fn new(qos: Qos) -> Self { // Need at least 1 for gatt - #[cfg(feature = "gatt")] assert!(CLIENTS >= 1); Self { state: Mutex::new(State::new()),