From c81717caef403794c00fa5894c0b84ea8d74ddcd Mon Sep 17 00:00:00 2001 From: Ulf Lilleengen Date: Thu, 14 Mar 2024 14:24:03 +0100 Subject: [PATCH] rework gatt support * New interface for configuring attributes: AttributeTable. * Track MTU per connections. * Track notifications per connection per characteristic internally. * Ability to read/write/notify values in gatt table. --- .../nrf-sdc/src/bin/ble_bas_peripheral.rs | 58 +- host/src/adapter.rs | 21 +- host/src/attribute.rs | 724 +++++++++--------- host/src/attribute_server.rs | 404 +++++----- host/src/connection_manager.rs | 38 + host/src/cursor.rs | 40 +- host/src/gatt.rs | 169 ++-- host/src/lib.rs | 8 - 8 files changed, 814 insertions(+), 648 deletions(-) diff --git a/examples/nrf-sdc/src/bin/ble_bas_peripheral.rs b/examples/nrf-sdc/src/bin/ble_bas_peripheral.rs index 2bf7915..31d64d2 100644 --- a/examples/nrf-sdc/src/bin/ble_bas_peripheral.rs +++ b/examples/nrf-sdc/src/bin/ble_bas_peripheral.rs @@ -4,7 +4,7 @@ use bt_hci::cmd::SyncCmd; use bt_hci::param::BdAddr; -use defmt::{info, unwrap}; +use defmt::{error, info, unwrap}; use embassy_executor::Spawner; use embassy_futures::join::join3; use embassy_nrf::{bind_interrupts, pac}; @@ -17,8 +17,7 @@ use static_cell::StaticCell; use trouble_host::{ adapter::{Adapter, HostResources}, advertise::{AdStructure, AdvertiseConfig, BR_EDR_NOT_SUPPORTED, LE_GENERAL_DISCOVERABLE}, - attribute::{AttributesBuilder, CharacteristicProp, ServiceBuilder, Uuid}, - gatt::{GattEvent, GattServer}, + attribute::{AttributeTable, Characteristic, CharacteristicProp, Service, Uuid}, PacketQos, }; @@ -94,7 +93,7 @@ async fn main(spawner: Spawner) { let mut pool = [0; 256]; let rng = sdc::rng_pool::RngPool::new(p.RNG, Irqs, &mut pool, 64); - let mut sdc_mem = sdc::Mem::<1672>::new(); + let mut sdc_mem = sdc::Mem::<3312>::new(); let sdc = unwrap!(build_sdc(sdc_p, &rng, mpsl, &mut sdc_mem)); info!("Advertising as {:02x}", bd_addr()); @@ -114,15 +113,36 @@ async fn main(spawner: Spawner) { ], }; - let mut attributes: AttributesBuilder<'_, 10> = AttributesBuilder::new(); - static DATA: StaticCell<[u8; 1]> = StaticCell::new(); - let data = DATA.init([32; 1]); - ServiceBuilder::new(&mut attributes, 0x180f_u16.into()) - .add_characteristic(0x2a19.into(), &[CharacteristicProp::Read], data) - .done(); - let mut attributes = attributes.build(); + let mut table: AttributeTable<'_, NoopRawMutex, 10> = AttributeTable::new(); + + // Generic Access Service (mandatory) + let mut id = [b'T', b'r', b'o', b'u', b'b', b'l', b'e']; + let mut appearance = [0x80, 0x07]; + let mut bat_level = [0; 1]; + let handle = { + let mut svc = table.add_service(Service::new(0x1800)); + let _ = svc.add_characteristic(Characteristic::new(0x2a00, &[CharacteristicProp::Read], &mut id[..])); + let _ = svc.add_characteristic(Characteristic::new( + 0x2a01, + &[CharacteristicProp::Read], + &mut appearance[..], + )); + drop(svc); + + // Generic attribute service (mandatory) + table.add_service(Service::new(0x1801)); + + // Battery service + let mut svc = table.add_service(Service::new(0x180f)); + + svc.add_characteristic(Characteristic::new( + 0x2a19, + &[CharacteristicProp::Read, CharacteristicProp::Notify], + &mut bat_level, + )) + }; - let mut server = GattServer::new(&adapter, &mut attributes[..]); + let server = adapter.gatt_server(&table); info!("Starting advertising and GATT service"); let _ = join3( @@ -130,17 +150,23 @@ async fn main(spawner: Spawner) { async { loop { match server.next().await { - GattEvent::Write(_conn, attribute) => { - info!("Attribute was written: {:?}", attribute); + Ok(event) => { + info!("Gatt event: {:?}", event); + } + Err(e) => { + error!("Error processing GATT events: {:?}", e); } } } }, async { - let _conn = unwrap!(adapter.advertise(&config).await); + let conn = unwrap!(adapter.advertise(&config).await); // Keep connection alive + let mut tick: u8 = 0; loop { - Timer::after(Duration::from_secs(60)).await + Timer::after(Duration::from_secs(10)).await; + tick += 1; + unwrap!(server.notify(handle, &conn, &[tick]).await); } }, ) diff --git a/host/src/adapter.rs b/host/src/adapter.rs index 05f5242..876eb6a 100644 --- a/host/src/adapter.rs +++ b/host/src/adapter.rs @@ -1,10 +1,13 @@ use crate::advertise::AdvertiseConfig; +use crate::attribute::AttributeTable; +use crate::attribute_server::AttributeServer; use crate::channel_manager::ChannelManager; use crate::connection::Connection; use crate::connection_manager::{ConnectionInfo, ConnectionManager}; use crate::cursor::{ReadCursor, WriteCursor}; +use crate::gatt::GattServer; use crate::l2cap::{L2capPacket, L2CAP_CID_ATT, L2CAP_CID_DYN_START, L2CAP_CID_LE_U_SIGNAL}; -use crate::packet_pool::{DynamicPacketPool, PacketPool, Qos, ATT_ID}; +use crate::packet_pool::{self, DynamicPacketPool, PacketPool, Qos, ATT_ID}; use crate::pdu::Pdu; use crate::scan::{ScanConfig, ScanReport}; use crate::types::l2cap::L2capLeSignal; @@ -159,6 +162,21 @@ where Ok(conn) } + /// Creates a GATT server capable of processing the GATT protocol using the provided table of attributes. + pub fn gatt_server<'reference, 'values, const MAX: usize>( + &'reference self, + table: &'reference AttributeTable<'values, M, MAX>, + ) -> GattServer<'reference, 'values, 'd, M, MAX> { + GattServer { + server: AttributeServer::new(table), + pool: self.pool, + pool_id: packet_pool::ATT_ID, + rx: self.att_inbound.receiver().into(), + tx: self.outbound.sender().into(), + connections: &self.connections, + } + } + async fn handle_acl(&self, acl: AclPacket<'_>) -> Result<(), HandleError> { let (conn, packet) = L2capPacket::decode(acl)?; match packet.channel { @@ -256,6 +274,7 @@ where interval: e.conn_interval.as_u16(), latency: e.peripheral_latency, timeout: e.supervision_timeout.as_u16(), + att_mtu: 23, }, ) { warn!("Error establishing connection: {:?}", err); diff --git a/host/src/attribute.rs b/host/src/attribute.rs index 6608e02..4968774 100644 --- a/host/src/attribute.rs +++ b/host/src/attribute.rs @@ -1,4 +1,6 @@ -use core::{fmt, mem::size_of, slice}; +use core::{cell::RefCell, fmt}; + +use embassy_sync::blocking_mutex::{raw::RawMutex, Mutex}; pub use crate::types::uuid::Uuid; use crate::{att::AttErrorCode, cursor::WriteCursor}; @@ -11,6 +13,7 @@ pub const GENERIC_ATTRIBUTE_SERVICE_UUID16: Uuid = Uuid::Uuid16(0x1801u16.to_le_ pub const PRIMARY_SERVICE_UUID16: Uuid = Uuid::Uuid16(0x2800u16.to_le_bytes()); pub const CHARACTERISTIC_UUID16: Uuid = Uuid::Uuid16(0x2803u16.to_le_bytes()); +pub const CHARACTERISTIC_CCCD_UUID16: Uuid = Uuid::Uuid16(0x2902u16.to_le_bytes()); pub const GENERIC_ATTRIBUTE_UUID16: Uuid = Uuid::Uuid16(0x1801u16.to_le_bytes()); #[derive(Debug, Clone, Copy)] @@ -26,317 +29,77 @@ pub enum CharacteristicProp { Extended = 0x80, } -pub trait AttData { - fn readable(&self) -> bool { - false - } - - fn read(&mut self, _offset: usize, _data: &mut [u8]) -> Result { - Ok(0) - } - - fn writable(&self) -> bool { - false - } - - fn write(&mut self, _offset: usize, _data: &[u8]) -> Result<(), AttErrorCode> { - Ok(()) - } -} - -impl AttData for [u8; N] { - fn readable(&self) -> bool { - true - } - - fn read(&mut self, offset: usize, data: &mut [u8]) -> Result { - if offset > N { - return Ok(0); - } - let len = data.len().min(N - offset); - if len > 0 { - data[..len].copy_from_slice(&self[offset..offset + len]); - } - Ok(len) - } -} - -impl<'a, const N: usize> AttData for &'a [u8; N] { - fn readable(&self) -> bool { - true - } - - fn read(&mut self, offset: usize, data: &mut [u8]) -> Result { - if offset > N { - return Ok(0); - } - let len = data.len().min(N - offset); - if len > 0 { - data[..len].copy_from_slice(&self[offset..offset + len]); - } - Ok(len) - } -} - -impl<'a, const N: usize> AttData for &'a mut [u8; N] { - fn readable(&self) -> bool { - true - } - - fn read(&mut self, offset: usize, data: &mut [u8]) -> Result { - if offset > N { - return Ok(0); - } - let len = data.len().min(N - offset); - if len > 0 { - data[..len].copy_from_slice(&self[offset..offset + len]); - } - Ok(len) - } - - fn writable(&self) -> bool { - true - } - - fn write(&mut self, offset: usize, data: &[u8]) -> Result<(), AttErrorCode> { - if offset > N { - return Ok(()); - } - let len = data.len().min(N - offset); - if len > 0 { - self[offset..offset + len].copy_from_slice(&data[..len]); - } - Ok(()) - } -} - -impl<'a> AttData for &'a [u8] { - fn readable(&self) -> bool { - true - } - - fn read(&mut self, offset: usize, data: &mut [u8]) -> Result { - let len = self.len(); - if offset > len { - return Ok(0); - } - let len = data.len().min(len - offset); - data[..len].copy_from_slice(&self[offset..offset + len]); - Ok(len) - } -} - -impl<'a> AttData for &'a mut [u8] { - fn readable(&self) -> bool { - true - } - - fn read(&mut self, offset: usize, data: &mut [u8]) -> Result { - let len = self.len(); - if offset > len { - return Ok(0); - } - let len = data.len().min(len - offset); - data[..len].copy_from_slice(&self[offset..offset + len]); - Ok(len) - } - - fn writable(&self) -> bool { - true - } - - fn write(&mut self, offset: usize, data: &[u8]) -> Result<(), AttErrorCode> { - let len = self.len(); - if offset > len { - return Ok(()); - } - let len = data.len().min(len - offset); - self[offset..offset + len].copy_from_slice(&data[..len]); - Ok(()) - } -} - -impl<'a, T: Sized + 'static> AttData for &'a (T,) { - fn readable(&self) -> bool { - true - } - - fn read(&mut self, offset: usize, data: &mut [u8]) -> Result { - if offset > size_of::() { - return Ok(0); - } - let len = data.len().min(size_of::() - offset); - if len > 0 { - let slice = unsafe { slice::from_raw_parts(&self.0 as *const T as *const u8, size_of::()) }; - // TODO: Handle big endian case - data[..len].copy_from_slice(&slice[offset..offset + len]); - } - Ok(len) - } -} - -impl AttData for Uuid { - fn readable(&self) -> bool { - true - } - - fn read(&mut self, offset: usize, data: &mut [u8]) -> Result { - let val = self.as_raw(); - if offset > val.len() { - return Ok(0); - } - let len = data.len().min(val.len() - offset); - if len > 0 { - data[..len].copy_from_slice(&val[offset..offset + len]); - } - Ok(len) - } -} - -impl<'a, T: Sized + 'static> AttData for &'a mut (T,) { - fn readable(&self) -> bool { - true - } - - fn read(&mut self, offset: usize, data: &mut [u8]) -> Result { - if offset > size_of::() { - return Ok(0); - } - let len = data.len().min(size_of::() - offset); - if len > 0 { - let slice = unsafe { slice::from_raw_parts(&self.0 as *const T as *const u8, size_of::()) }; - // TODO: Handle big endian case - data[..len].copy_from_slice(&slice[offset..offset + len]); - } - Ok(len) - } - - fn writable(&self) -> bool { - true - } - - fn write(&mut self, offset: usize, data: &[u8]) -> Result<(), AttErrorCode> { - if offset > size_of::() { - return Ok(()); - } - let len = data.len().min(size_of::() - offset); - if len > 0 { - let slice = unsafe { slice::from_raw_parts_mut(&mut self.0 as *mut T as *mut u8, size_of::()) }; - // TODO: Handle big endian case - slice[offset..offset + len].copy_from_slice(&data[..len]); - } - Ok(()) - } -} - -trait IntoResult { - fn into_result(self) -> Result; -} - -impl IntoResult for T { - fn into_result(self) -> Result { - Ok(self) - } -} - -impl IntoResult for Result { - fn into_result(self) -> Result { - self - } -} - -impl AttData for (R, ()) -where - T: IntoResult, - R: FnMut(usize, &mut [u8]) -> T, -{ - fn readable(&self) -> bool { - true - } - - fn read(&mut self, offset: usize, data: &mut [u8]) -> Result { - self.0(offset, data).into_result() - } -} - -impl AttData for ((), W) -where - U: IntoResult<()>, - W: FnMut(usize, &[u8]) -> U, -{ - fn writable(&self) -> bool { - true - } - - fn write(&mut self, offset: usize, data: &[u8]) -> Result<(), AttErrorCode> { - self.1(offset, data).into_result() - } -} - -impl AttData for (R, W) -where - T: IntoResult, - U: IntoResult<()>, - R: FnMut(usize, &mut [u8]) -> T, - W: FnMut(usize, &[u8]) -> U, -{ - fn readable(&self) -> bool { - true - } - - fn read(&mut self, offset: usize, data: &mut [u8]) -> Result { - self.0(offset, data).into_result() - } - - fn writable(&self) -> bool { - true - } - - fn write(&mut self, offset: usize, data: &[u8]) -> Result<(), AttErrorCode> { - self.1(offset, data).into_result() - } -} - -pub const ATT_READABLE: u8 = 0x02; -pub const ATT_WRITEABLE: u8 = 0x08; - pub struct Attribute<'a> { pub uuid: Uuid, pub handle: u16, pub last_handle_in_group: u16, pub data: AttributeData<'a>, } + +impl<'a> Attribute<'a> { + const EMPTY: Option> = None; +} + pub enum AttributeData<'d> { - Service(Uuid), - Ref(&'d mut dyn AttData), - Slice(&'static [u8]), - CharDeclaration(u8, u16, Uuid), + Service { + uuid: Uuid, + }, + Data { + props: CharacteristicProps, + value: &'d mut [u8], + }, + Declaration { + props: CharacteristicProps, + handle: u16, + uuid: Uuid, + }, + Cccd { + notifications: bool, + indications: bool, + }, } impl<'d> AttributeData<'d> { pub fn readable(&self) -> bool { match self { - Self::Ref(d) => d.readable(), - Self::Slice(_) => true, - Self::CharDeclaration(_, _, _) => true, - Self::Service(_) => true, + Self::Data { props, value } => props.0 & (CharacteristicProp::Read as u8) != 0, + _ => true, } } - pub fn read(&mut self, offset: usize, data: &mut [u8]) -> Result { + pub fn writable(&self) -> bool { match self { - Self::Ref(d) => d.read(offset, data), - Self::Slice(val) => { - if offset > val.len() { + Self::Data { props, value } => { + props.0 + & (CharacteristicProp::Write as u8 + | CharacteristicProp::WriteWithoutResponse as u8 + | CharacteristicProp::AuthenticatedWrite as u8) + != 0 + } + Self::Cccd { + notifications, + indications, + } => true, + _ => false, + } + } + + pub fn read(&self, offset: usize, data: &mut [u8]) -> Result { + if !self.readable() { + return Err(AttErrorCode::ReadNotPermitted); + } + match self { + Self::Data { props, value } => { + if offset > value.len() { return Ok(0); } - let len = data.len().min(val.len() - offset); + let len = data.len().min(value.len() - offset); if len > 0 { - data[..len].copy_from_slice(&val[offset..offset + len]); + data[..len].copy_from_slice(&value[offset..offset + len]); } Ok(len) } - Self::Service(uuid) => { + Self::Service { uuid } => { let val = uuid.as_raw(); if offset > val.len() { return Ok(0); @@ -347,14 +110,35 @@ impl<'d> AttributeData<'d> { } Ok(len) } - Self::CharDeclaration(props, handle, uuid) => { + Self::Cccd { + notifications, + indications, + } => { + if offset > 0 { + return Err(AttErrorCode::InvalidOffset); + } + if data.len() < 2 { + return Err(AttErrorCode::UnlikelyError); + } + let mut v = 0; + if *notifications { + v |= 0x01; + } + + if *indications { + v |= 0x02; + } + data[0] = v; + Ok(2) + } + Self::Declaration { props, handle, uuid } => { let val = uuid.as_raw(); if offset > val.len() + 3 { return Ok(0); } let mut w = WriteCursor::new(data); if offset == 0 { - w.write(*props)?; + w.write(props.0)?; w.write(*handle)?; } else if offset == 1 { w.write(*handle)?; @@ -372,21 +156,40 @@ impl<'d> AttributeData<'d> { } } - pub fn writable(&self) -> bool { - match self { - Self::Ref(d) => d.writable(), - Self::Slice(_) => false, - Self::CharDeclaration(_, _, _) => false, - Self::Service(_) => false, - } - } - pub fn write(&mut self, offset: usize, data: &[u8]) -> Result<(), AttErrorCode> { + let writable = self.writable(); + match self { - Self::Ref(d) => d.write(offset, data), - Self::Slice(_) => Err(AttErrorCode::WriteNotPermitted), - Self::CharDeclaration(_, _, _) => Err(AttErrorCode::WriteNotPermitted), - Self::Service(_) => Err(AttErrorCode::WriteNotPermitted), + Self::Data { value, props } => { + if !writable { + return Err(AttErrorCode::WriteNotPermitted); + } + + if offset + data.len() < value.len() { + value[offset..offset + data.len()].copy_from_slice(data); + Ok(()) + } else { + Err(AttErrorCode::InvalidOffset) + } + } + Self::Cccd { + notifications, + indications, + } => { + if offset > 0 { + return Err(AttErrorCode::InvalidOffset); + } + + if data.is_empty() { + return Err(AttErrorCode::UnlikelyError); + } + + *notifications = data[0] & 0x01 != 0; + *indications = data[0] & 0x02 != 0; + Ok(()) + } + Self::Declaration { .. } => Err(AttErrorCode::WriteNotPermitted), + Self::Service { .. } => Err(AttErrorCode::WriteNotPermitted), } } } @@ -419,92 +222,281 @@ impl<'a> Attribute<'a> { last_handle_in_group: 0xffff, } } - - /* - pub(crate) fn value(&mut self) -> Result { - let mut data = Data::default(); - if self.data.readable() { - let len = self.data.read(0, data.as_slice_mut())?; - data.append_len(len); - } - Ok(data) - }*/ } -use heapless::Vec; -pub struct AttributesBuilder<'a, const N: usize> { - attributes: Vec, N>, +pub struct AttributeTable<'d, M: RawMutex, const MAX: usize> { + inner: Mutex>>, handle: u16, } -impl<'a, const N: usize> AttributesBuilder<'a, N> { +pub struct InnerTable<'d, const MAX: usize> { + attributes: [Option>; MAX], + len: usize, +} + +impl<'d, const MAX: usize> InnerTable<'d, MAX> { + fn push(&mut self, attribute: Attribute<'d>) { + if self.len == MAX { + panic!("no space for more attributes") + } + self.attributes[self.len].replace(attribute); + self.len += 1; + } +} + +impl<'d, M: RawMutex, const MAX: usize> AttributeTable<'d, M, MAX> { pub fn new() -> Self { - let mut me = Self { - attributes: Vec::new(), + Self { handle: 1, - }; - me.push( - PRIMARY_SERVICE_UUID16, - AttributeData::Service(GENERIC_ACCESS_SERVICE_UUID16), - ); - me.push(CHARACTERISTIC_DEVICE_NAME_UUID16, AttributeData::Slice(b"Trouble")); - me.push(CHARACTERISTIC_APPEARANCE_UUID16, AttributeData::Slice(&[0x02, 0x00])); - me.finish_group(); - me + inner: Mutex::new(RefCell::new(InnerTable { + len: 0, + attributes: [Attribute::EMPTY; MAX], + })), + } } - fn finish_group(&mut self) { - for att in self.attributes.iter_mut() { - if att.last_handle_in_group == 0 { - att.last_handle_in_group = self.handle; - } + pub fn with_inner)>(&self, f: F) { + self.inner.lock(|inner| { + let mut table = inner.borrow_mut(); + f(&mut table); + }) + } + + pub fn iterate) -> R, R>(&self, mut f: F) -> R { + self.inner.lock(|inner| { + let mut table = inner.borrow_mut(); + let len = table.len; + let it = AttributeIterator { + attributes: &mut table.attributes[..], + pos: 0, + len, + }; + f(it) + }) + } + + fn push(&mut self, mut attribute: Attribute<'d>) -> u16 { + let handle = self.handle; + attribute.handle = handle; + self.inner.lock(|inner| { + let mut inner = inner.borrow_mut(); + inner.push(attribute); + }); + self.handle += 1; + handle + } + + pub fn add_service(&mut self, service: Service) -> ServiceBuilder<'_, 'd, M, MAX> { + let len = self.inner.lock(|i| i.borrow().len); + self.push(Attribute { + uuid: PRIMARY_SERVICE_UUID16, + handle: 0, + last_handle_in_group: 0, + data: AttributeData::Service { uuid: service.uuid }, + }); + ServiceBuilder { + start: len, + table: self, } - // Jump to next 0x10 aligned handle - self.handle = self.handle + (0x10 - (self.handle % 0x10)); } - pub fn push(&mut self, uuid: Uuid, data: AttributeData<'a>) { - self.attributes - .push(Attribute { - uuid, - handle: self.handle, - data, + /// Set the value of a characteristic + /// + /// The provided data must exactly match the size of the storage for the characteristic, + /// otherwise this function will panic. + /// + /// If the characteristic for the handle cannot be found, an error is returned. + pub fn set(&self, handle: CharacteristicHandle, input: &[u8]) -> Result<(), ()> { + self.iterate(|mut it| { + while let Some(att) = it.next() { + if att.handle == handle.handle { + if let AttributeData::Data { props, value } = &mut att.data { + assert_eq!(value.len(), input.len()); + value.copy_from_slice(input); + return Ok(()); + } + } + } + Err(()) + }) + } + + /// Read the value of the characteristic and pass the value to the provided closure. + /// + /// The return value of the closure is returned in this function and is assumed to be infallible. + /// + /// If the characteristic for the handle cannot be found, an error is returned. + pub fn get T, T>(&self, handle: CharacteristicHandle, mut f: F) -> Result { + self.iterate(|mut it| { + while let Some(att) = it.next() { + if att.handle == handle.handle { + if let AttributeData::Data { props, value } = &mut att.data { + let v = f(value); + return Ok(v); + } + } + } + Err(()) + }) + } +} + +pub struct ServiceBuilder<'r, 'd, M: RawMutex, const MAX: usize> { + start: usize, + table: &'r mut AttributeTable<'d, M, MAX>, +} + +impl<'r, 'd, M: RawMutex, const MAX: usize> ServiceBuilder<'r, 'd, M, MAX> { + pub fn add_characteristic(&mut self, c: Characteristic<'d>) -> CharacteristicHandle { + // First the characteristic declaration + let next = self.table.handle + 1; + let cccd = self.table.handle + 2; + self.table.push(Attribute { + uuid: CHARACTERISTIC_UUID16, + handle: 0, + last_handle_in_group: 0, + data: AttributeData::Declaration { + props: c.props, + handle: next, + uuid: c.uuid, + }, + }); + + // Then the value declaration + self.table.push(Attribute { + uuid: c.uuid, + handle: 0, + last_handle_in_group: 0, + data: AttributeData::Data { + props: c.props, + value: c.storage, + }, + }); + + // Add optional CCCD handle + let cccd_handle = if c.props.any(&[CharacteristicProp::Notify, CharacteristicProp::Indicate]) { + self.table.push(Attribute { + uuid: CHARACTERISTIC_CCCD_UUID16, + handle: 0, last_handle_in_group: 0, - }) - .unwrap(); - self.handle += 1; + data: AttributeData::Cccd { + notifications: false, + indications: false, + }, + }); + Some(cccd) + } else { + None + }; + + CharacteristicHandle { + handle: next, + cccd_handle, + } } +} + +impl<'r, 'd, M: RawMutex, const MAX: usize> Drop for ServiceBuilder<'r, 'd, M, MAX> { + fn drop(&mut self) { + let last_handle = self.table.handle + 1; + self.table.with_inner(|inner| { + for item in inner.attributes[self.start..inner.len].iter_mut() { + item.as_mut().unwrap().last_handle_in_group = last_handle; + } + }); + + // Jump to next 16-aligned + self.table.handle = self.table.handle + (0x10 - (self.table.handle % 0x10)); + } +} + +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +#[derive(Clone, Copy, Debug)] +pub struct CharacteristicHandle { + pub(crate) cccd_handle: Option, + pub(crate) handle: u16, +} + +pub struct AttributeIterator<'a, 'd> { + attributes: &'a mut [Option>], + pos: usize, + len: usize, +} - pub fn build(self) -> Vec, N> { - self.attributes +impl<'a, 'd> AttributeIterator<'a, 'd> { + pub fn next<'m>(&'m mut self) -> Option<&'m mut Attribute<'d>> { + if self.pos < self.len { + let i = self.attributes[self.pos].as_mut(); + self.pos += 1; + i + } else { + None + } } } -pub struct ServiceBuilder<'a, 'b, const N: usize> { - attributes: &'b mut AttributesBuilder<'a, N>, +pub struct Service { + pub uuid: Uuid, } -impl<'a, 'b, const N: usize> ServiceBuilder<'a, 'b, N> { - pub fn new(attributes: &'b mut AttributesBuilder<'a, N>, uuid: Uuid) -> Self { - attributes.push(PRIMARY_SERVICE_UUID16, AttributeData::Service(uuid)); - Self { attributes } +impl Service { + pub fn new>(uuid: U) -> Self { + Self { uuid: uuid.into() } } +} - pub fn add_characteristic(self, uuid: Uuid, props: &[CharacteristicProp], value: &'a mut impl AttData) -> Self { - let mut prop = 0u8; - for p in props { - prop |= *p as u8 +impl<'d> Characteristic<'d> { + pub fn new>(uuid: U, props: &[CharacteristicProp], storage: &'d mut [u8]) -> Self { + Self { + uuid: uuid.into(), + props: props.into(), + storage, + } + } +} + +pub struct Characteristic<'d> { + pub uuid: Uuid, + pub props: CharacteristicProps, + pub storage: &'d mut [u8], +} + +#[derive(Clone, Copy)] +pub struct CharacteristicProps(u8); + +impl<'a> From<&'a [CharacteristicProp]> for CharacteristicProps { + fn from(props: &'a [CharacteristicProp]) -> Self { + let mut val: u8 = 0; + for prop in props { + val |= *prop as u8; } - self.attributes.push( - CHARACTERISTIC_UUID16, - AttributeData::CharDeclaration(prop, self.attributes.handle + 1, uuid), - ); + CharacteristicProps(val) + } +} - self.attributes.push(uuid, AttributeData::Ref(value)); - self +impl From<[CharacteristicProp; T]> for CharacteristicProps { + fn from(props: [CharacteristicProp; T]) -> Self { + let mut val: u8 = 0; + for prop in props { + val |= prop as u8; + } + CharacteristicProps(val) } +} - pub fn done(self) { - self.attributes.finish_group(); +impl CharacteristicProps { + fn any(&self, props: &[CharacteristicProp]) -> bool { + for p in props { + if (*p as u8) & self.0 != 0 { + return true; + } + } + false } } + +pub struct AttributeValue<'d, M: RawMutex> { + value: Mutex, +} + +impl<'d, M: RawMutex> AttributeValue<'d, M> {} diff --git a/host/src/attribute_server.rs b/host/src/attribute_server.rs index 307ef88..f126376 100644 --- a/host/src/attribute_server.rs +++ b/host/src/attribute_server.rs @@ -1,11 +1,15 @@ +use bt_hci::param::ConnHandle; +use embassy_sync::blocking_mutex::raw::RawMutex; + use crate::{ att::{self, Att, AttDecodeError, AttErrorCode}, - attribute::Attribute, + attribute::{AttributeData, AttributeTable}, codec, cursor::WriteCursor, types::uuid::Uuid, - ATT_MTU, }; +use core::cell::RefCell; +use embassy_sync::blocking_mutex::Mutex; #[derive(Debug, PartialEq)] pub enum WorkResult { @@ -31,55 +35,94 @@ impl From for AttributeServerError { } } -pub struct AttributeServer<'a, 'd> { - pub(crate) buf: [u8; ATT_MTU], - pub(crate) mtu: u16, - pub(crate) attributes: &'a mut [Attribute<'d>], +const MAX_NOTIFICATIONS: usize = 4; +pub struct NotificationTable { + state: [(u16, ConnHandle); ENTRIES], } -impl<'a, 'd> AttributeServer<'a, 'd> { - /// Create a new instance of the AttributeServer - pub fn new(attributes: &'a mut [Attribute<'d>]) -> AttributeServer<'a, 'd> { - AttributeServer::new_inner(attributes) - } +pub struct AttributeServer<'c, 'd, M: RawMutex, const MAX: usize> { + pub(crate) table: &'c AttributeTable<'d, M, MAX>, + pub(crate) notification: Mutex>>, +} - fn new_inner(attributes: &'a mut [Attribute<'d>]) -> AttributeServer<'a, 'd> { +impl<'c, 'd, M: RawMutex, const MAX: usize> AttributeServer<'c, 'd, M, MAX> { + /// Create a new instance of the AttributeServer + pub fn new(table: &'c AttributeTable<'d, M, MAX>) -> AttributeServer<'c, 'd, M, MAX> { AttributeServer { - mtu: ATT_MTU as u16, - attributes, - buf: [0; ATT_MTU], + table, + notification: Mutex::new(RefCell::new(NotificationTable { + state: [(0, ConnHandle::new(0)); 4], + })), } } + pub(crate) fn should_notify(&self, conn: ConnHandle, cccd_handle: u16) -> bool { + self.notification.lock(|n| { + let n = n.borrow(); + for entry in n.state.iter() { + if entry.0 == cccd_handle && entry.1 == conn { + return true; + } + } + false + }) + } + + fn set_notify(&self, conn: ConnHandle, cccd_handle: u16, enable: bool) { + self.notification.lock(|n| { + let mut n = n.borrow_mut(); + if enable { + for entry in n.state.iter_mut() { + if entry.0 == 0 { + entry.0 = cccd_handle; + entry.1 = conn; + return; + } + } + } else { + for entry in n.state.iter_mut() { + if entry.0 == cccd_handle && entry.1 == conn { + entry.0 = 0; + entry.1 = ConnHandle::new(0); + return; + } + } + } + }) + } + fn handle_read_by_type_req( - &mut self, + &self, + buf: &mut [u8], start: u16, end: u16, attribute_type: Uuid, ) -> Result { let mut handle = start; - let mut data = WriteCursor::new(&mut self.buf); - let mut err = Err(AttErrorCode::AttributeNotFound); + let mut data = WriteCursor::new(buf); let (mut header, mut body) = data.split(2)?; - for att in self.attributes.iter_mut() { - // trace!("Check attribute {:x} {}", att.uuid, att.handle); - if att.uuid == attribute_type && att.handle >= start && att.handle <= end { - body.write(att.handle)?; - handle = att.handle; - - if att.data.readable() { - let mut writer = body.write_buf(); - err = att.data.read(0, writer.as_mut()); - if let Ok(len) = &err { - writer.finish(*len)?; + let err = self.table.iterate(|mut it| { + let mut err = Err(AttErrorCode::AttributeNotFound); + while let Some(att) = it.next() { + // trace!("Check attribute {:x} {}", att.uuid, att.handle); + if att.uuid == attribute_type && att.handle >= start && att.handle <= end { + body.write(att.handle)?; + handle = att.handle; + + if att.data.readable() { + err = att.data.read(0, body.write_buf()); + if let Ok(len) = &err { + body.commit(*len)?; + } } - } - //debug!("found! {:x} {}", att.uuid, att.handle); - break; + //debug!("found! {:x} {}", att.uuid, att.handle); + break; + } } - } + err + }); match err { Ok(len) => { @@ -97,36 +140,39 @@ impl<'a, 'd> AttributeServer<'a, 'd> { } fn handle_read_by_group_type_req( - &mut self, + &self, + buf: &mut [u8], start: u16, end: u16, group_type: Uuid, ) -> Result { // TODO respond with all finds - not just one let mut handle = start; - let mut data = WriteCursor::new(&mut self.buf); - let mut err = Err(AttErrorCode::AttributeNotFound); + let mut data = WriteCursor::new(buf); let (mut header, mut body) = data.split(2)?; - for att in self.attributes.iter_mut() { - // trace!("Check attribute {:x} {}", att.uuid, att.handle); - if att.uuid == group_type && att.handle >= start && att.handle <= end { - //debug!("found! {:x} {}", att.uuid, att.handle); - handle = att.handle; - - body.write(att.handle)?; - body.write(att.last_handle_in_group)?; - - if att.data.readable() { - let mut writer = body.write_buf(); - err = att.data.read(0, writer.as_mut()); - if let Ok(len) = &err { - writer.finish(*len)?; + let err = self.table.iterate(|mut it| { + let mut err = Err(AttErrorCode::AttributeNotFound); + while let Some(att) = it.next() { + // trace!("Check attribute {:x} {}", att.uuid, att.handle); + if att.uuid == group_type && att.handle >= start && att.handle <= end { + //debug!("found! {:x} {}", att.uuid, att.handle); + handle = att.handle; + + body.write(att.handle)?; + body.write(att.last_handle_in_group)?; + + if att.data.readable() { + err = att.data.read(0, body.write_buf()); + if let Ok(len) = &err { + body.commit(*len)?; + } } + break; } - break; } - } + err + }); match err { Ok(len) => { @@ -143,96 +189,79 @@ impl<'a, 'd> AttributeServer<'a, 'd> { } } - pub fn get_characteristic_value(&mut self, handle: u16, offset: u16, buffer: &mut [u8]) -> Option { - let att = &mut self.attributes[handle as usize]; - - if att.data.readable() { - att.data.read(offset as usize, buffer).ok() - } else { - None - } - } - - /* - pub fn update_le_advertising_data(&mut self, data: Data) -> Result> { - self.ble - .write_command(Command::LeSetAdvertisingData { data }.encode().as_slice()) - .await?; - self.ble - .wait_for_command_complete(LE_OGF, SET_ADVERTISING_DATA_OCF) - .await? - .check_command_completed() - } - - pub fn disconnect(&mut self, reason: u8) -> Result> { - self.ble - .write_command( - Command::Disconnect { - connection_handle: 0, - reason, - } - .encode() - .as_slice(), - ) - .await?; - Ok(EventType::Unknown) - }*/ - - fn handle_read_req(&mut self, handle: u16) -> Result { - let mut data = WriteCursor::new(&mut self.buf); - let mut err = Err(AttErrorCode::AttributeNotFound); + fn handle_read_req(&self, buf: &mut [u8], handle: u16) -> Result { + let mut data = WriteCursor::new(buf); data.write(att::ATT_READ_RESPONSE_OPCODE)?; - for att in self.attributes.iter_mut() { - if att.handle == handle { - if att.data.readable() { - let mut b = data.write_buf(); - err = att.data.read(0, b.as_mut()); - if let Ok(len) = err { - b.finish(len)?; + let err = self.table.iterate(|mut it| { + let mut err = Err(AttErrorCode::AttributeNotFound); + while let Some(att) = it.next() { + if att.handle == handle { + if att.data.readable() { + err = att.data.read(0, data.write_buf()); + if let Ok(len) = err { + data.commit(len)?; + } } + break; } - break; } - } + err + }); match err { - Ok(_) => { - data.truncate(self.mtu as usize); - Ok(data.len()) - } + Ok(_) => Ok(data.len()), Err(e) => Ok(Self::error_response(data, att::ATT_READ_REQUEST_OPCODE, handle, e)?), } } - fn handle_write_cmd(&mut self, handle: u16, data: &[u8]) -> Result { + fn handle_write_cmd(&self, buf: &mut [u8], handle: u16, data: &[u8]) -> Result { // TODO: Generate event - for att in self.attributes.iter_mut() { - if att.handle == handle { - if att.data.writable() { - // Write commands can't respond with an error. - att.data.write(0, data).unwrap(); + self.table.iterate(|mut it| { + while let Some(att) = it.next() { + if att.handle == handle { + if att.data.writable() { + // Write commands can't respond with an error. + att.data.write(0, data).unwrap(); + } + break; } - break; } - } - Ok(0) + Ok(0) + }) } - fn handle_write_req(&mut self, handle: u16, data: &[u8]) -> Result { - let mut err = Err(AttErrorCode::AttributeNotFound); - for att in self.attributes.iter_mut() { - if att.handle == handle { - if att.data.writable() { - err = att.data.write(0, data); + fn handle_write_req( + &self, + conn: ConnHandle, + buf: &mut [u8], + handle: u16, + data: &[u8], + ) -> Result { + let err = self.table.iterate(|mut it| { + let mut err = Err(AttErrorCode::AttributeNotFound); + while let Some(att) = it.next() { + if att.handle == handle { + if att.data.writable() { + err = att.data.write(0, data); + if let Ok(_) = &err { + if let AttributeData::Cccd { + notifications, + indications, + } = att.data + { + self.set_notify(conn, handle, notifications); + } + } + } + break; } - break; } - } - - let mut w = WriteCursor::new(&mut self.buf); + err + }); + let mut w = WriteCursor::new(buf); match err { Ok(()) => { w.write(att::ATT_WRITE_RESPONSE_OPCODE)?; @@ -242,16 +271,9 @@ impl<'a, 'd> AttributeServer<'a, 'd> { } } - fn handle_exchange_mtu(&mut self, mtu: u16) -> Result { - self.mtu = mtu.min(ATT_MTU as u16); - let mut b = WriteCursor::new(&mut self.buf); - b.write(att::ATT_EXCHANGE_MTU_RESPONSE_OPCODE)?; - b.write(self.mtu)?; - Ok(b.len()) - } - fn handle_find_type_value( - &mut self, + &self, + buf: &mut [u8], start: u16, _end: u16, _attr_type: u16, @@ -261,32 +283,35 @@ impl<'a, 'd> AttributeServer<'a, 'd> { // respond with error Ok(Self::error_response( - WriteCursor::new(&mut self.buf), + WriteCursor::new(buf), att::ATT_FIND_BY_TYPE_VALUE_REQUEST_OPCODE, start, AttErrorCode::AttributeNotFound, )?) } - fn handle_find_information(&mut self, start: u16, end: u16) -> Result { - let mut w = WriteCursor::new(&mut self.buf); + fn handle_find_information(&self, buf: &mut [u8], start: u16, end: u16) -> Result { + let mut w = WriteCursor::new(buf); let (mut header, mut body) = w.split(2)?; header.write(att::ATT_FIND_INFORMATION_RSP_OPCODE)?; let mut t = 0; - for att in self.attributes.iter_mut() { - if att.handle >= start && att.handle <= end { - if t == 0 { - t = att.uuid.get_type(); - } else if t != att.uuid.get_type() { - break; + self.table.iterate(|mut it| { + while let Some(att) = it.next() { + if att.handle >= start && att.handle <= end { + if t == 0 { + t = att.uuid.get_type(); + } else if t != att.uuid.get_type() { + break; + } + body.write(att.handle)?; + body.append(att.uuid.as_raw())?; } - body.write(att.handle)?; - body.append(att.uuid.as_raw())?; } - } + Ok::<(), AttributeServerError>(()) + })?; header.write(t)?; if body.len() > 2 { @@ -315,22 +340,31 @@ impl<'a, 'd> AttributeServer<'a, 'd> { Ok(w.len()) } - fn handle_prepare_write(&mut self, handle: u16, offset: u16, value: &[u8]) -> Result { - let mut w = WriteCursor::new(&mut self.buf); + fn handle_prepare_write( + &self, + buf: &mut [u8], + handle: u16, + offset: u16, + value: &[u8], + ) -> Result { + let mut w = WriteCursor::new(buf); w.write(att::ATT_PREPARE_WRITE_RESP_OPCODE)?; w.write(handle)?; w.write(offset)?; - let mut err = Err(AttErrorCode::AttributeNotFound); - for att in self.attributes.iter_mut() { - if att.handle == handle { - if att.data.writable() { - err = att.data.write(offset as usize, value); + let err = self.table.iterate(|mut it| { + let mut err = Err(AttErrorCode::AttributeNotFound); + while let Some(att) = it.next() { + if att.handle == handle { + if att.data.writable() { + err = att.data.write(offset as usize, value); + } + w.append(value)?; + break; } - w.append(value)?; - break; } - } + err + }); match err { Ok(()) => Ok(w.len()), @@ -338,29 +372,31 @@ impl<'a, 'd> AttributeServer<'a, 'd> { } } - fn handle_execute_write(&mut self, _flags: u8) -> Result { - let mut w = WriteCursor::new(&mut self.buf); + fn handle_execute_write(&self, buf: &mut [u8], _flags: u8) -> Result { + let mut w = WriteCursor::new(buf); w.write(att::ATT_EXECUTE_WRITE_RESP_OPCODE)?; Ok(w.len()) } - fn handle_read_blob(&mut self, handle: u16, offset: u16) -> Result { - let mut w = WriteCursor::new(&mut self.buf); + fn handle_read_blob(&self, buf: &mut [u8], handle: u16, offset: u16) -> Result { + let mut w = WriteCursor::new(buf); w.write(att::ATT_READ_BLOB_RESP_OPCODE)?; - let mut err = Err(AttErrorCode::AttributeNotFound); - - for att in self.attributes.iter_mut() { - if att.handle == handle { - if att.data.readable() { - let mut buf = w.write_buf(); - err = att.data.read(offset as usize, buf.as_mut()); - if let Ok(n) = &err { - buf.finish(*n)?; + + let err = self.table.iterate(|mut it| { + let mut err = Err(AttErrorCode::AttributeNotFound); + while let Some(att) = it.next() { + if att.handle == handle { + if att.data.readable() { + err = att.data.read(offset as usize, w.write_buf()); + if let Ok(n) = &err { + w.commit(*n)?; + } } + break; } - break; } - } + err + }); match err { Ok(_) => Ok(w.len()), @@ -368,8 +404,8 @@ impl<'a, 'd> AttributeServer<'a, 'd> { } } - fn handle_read_multiple(&mut self, handles: &[u8]) -> Result { - let w = WriteCursor::new(&mut self.buf); + fn handle_read_multiple(&self, buf: &mut [u8], handles: &[u8]) -> Result { + let w = WriteCursor::new(buf); Ok(Self::error_response( w, att::ATT_READ_MULTIPLE_REQ_OPCODE, @@ -379,50 +415,50 @@ impl<'a, 'd> AttributeServer<'a, 'd> { } /// Process an adapter event and produce a response if necessary - pub fn process(&mut self, packet: Att) -> Result, AttributeServerError> { + pub fn process(&self, conn: ConnHandle, packet: Att, rx: &mut [u8]) -> Result, AttributeServerError> { let len = match packet { Att::ReadByTypeReq { start, end, attribute_type, - } => self.handle_read_by_type_req(start, end, attribute_type)?, + } => self.handle_read_by_type_req(rx, start, end, attribute_type)?, Att::ReadByGroupTypeReq { start, end, group_type } => { - self.handle_read_by_group_type_req(start, end, group_type)? + self.handle_read_by_group_type_req(rx, start, end, group_type)? } Att::FindInformation { start_handle, end_handle, - } => self.handle_find_information(start_handle, end_handle)?, + } => self.handle_find_information(rx, start_handle, end_handle)?, - Att::ReadReq { handle } => self.handle_read_req(handle)?, + Att::ReadReq { handle } => self.handle_read_req(rx, handle)?, Att::WriteCmd { handle, data } => { - self.handle_write_cmd(handle, data)?; + self.handle_write_cmd(rx, handle, data)?; 0 } - Att::WriteReq { handle, data } => self.handle_write_req(handle, data)?, + Att::WriteReq { handle, data } => self.handle_write_req(conn, rx, handle, data)?, - Att::ExchangeMtu { mtu } => self.handle_exchange_mtu(mtu)?, + Att::ExchangeMtu { mtu } => 0, // Done outside, Att::FindByTypeValue { start_handle, end_handle, att_type, att_value, - } => self.handle_find_type_value(start_handle, end_handle, att_type, att_value)?, + } => self.handle_find_type_value(rx, start_handle, end_handle, att_type, att_value)?, - Att::PrepareWriteReq { handle, offset, value } => self.handle_prepare_write(handle, offset, value)?, + Att::PrepareWriteReq { handle, offset, value } => self.handle_prepare_write(rx, handle, offset, value)?, - Att::ExecuteWriteReq { flags } => self.handle_execute_write(flags)?, + Att::ExecuteWriteReq { flags } => self.handle_execute_write(rx, flags)?, - Att::ReadBlobReq { handle, offset } => self.handle_read_blob(handle, offset)?, + Att::ReadBlobReq { handle, offset } => self.handle_read_blob(rx, handle, offset)?, - Att::ReadMultipleReq { handles } => self.handle_read_multiple(handles)?, + Att::ReadMultipleReq { handles } => self.handle_read_multiple(rx, handles)?, }; if len > 0 { - Ok(Some(&self.buf[..len])) + Ok(Some(len)) } else { Ok(None) } diff --git a/host/src/connection_manager.rs b/host/src/connection_manager.rs index 7321d43..636cb2e 100644 --- a/host/src/connection_manager.rs +++ b/host/src/connection_manager.rs @@ -102,6 +102,43 @@ pub enum ConnectionState { Connected(ConnHandle, ConnectionInfo), } +pub trait DynamicConnectionManager { + fn get_att_mtu(&self, conn: ConnHandle) -> u16; + fn exchange_att_mtu(&self, conn: ConnHandle, mtu: u16) -> u16; +} + +impl DynamicConnectionManager for ConnectionManager { + fn get_att_mtu(&self, conn: ConnHandle) -> u16 { + self.state.lock(|state| { + let mut state = state.borrow_mut(); + for storage in state.connections.iter_mut() { + match storage { + ConnectionState::Connected(handle, info) if *handle == conn => { + return info.att_mtu; + } + _ => {} + } + } + 23 // Minimum value + }) + } + fn exchange_att_mtu(&self, conn: ConnHandle, mtu: u16) -> u16 { + self.state.lock(|state| { + let mut state = state.borrow_mut(); + for storage in state.connections.iter_mut() { + match storage { + ConnectionState::Connected(handle, info) if *handle == conn => { + info.att_mtu = info.att_mtu.min(mtu); + return info.att_mtu; + } + _ => {} + } + } + mtu + }) + } +} + #[derive(Clone, Copy)] pub struct ConnectionInfo { pub handle: ConnHandle, @@ -111,4 +148,5 @@ pub struct ConnectionInfo { pub interval: u16, pub latency: u16, pub timeout: u16, + pub att_mtu: u16, } diff --git a/host/src/cursor.rs b/host/src/cursor.rs index daf4546..ded02a2 100644 --- a/host/src/cursor.rs +++ b/host/src/cursor.rs @@ -68,8 +68,17 @@ impl<'d> WriteCursor<'d> { } // Reserve a spot for a slice of data and return it - pub fn write_buf<'m>(&'m mut self) -> SliceWriter<'m, 'd> { - SliceWriter { writer: self } + pub fn write_buf(&mut self) -> &mut [u8] { + &mut self.data[self.pos..] + } + + pub fn commit(&mut self, len: usize) -> Result<(), Error> { + if self.available() < len { + Err(Error::InsufficientSpace) + } else { + self.pos += len; + Ok(()) + } } pub fn available(&self) -> usize { @@ -85,33 +94,6 @@ impl<'d> WriteCursor<'d> { } } -pub struct SliceWriter<'m, 'd> { - writer: &'m mut WriteCursor<'d>, -} - -impl<'m, 'd> SliceWriter<'m, 'd> { - pub fn finish(self, len: usize) -> Result<(), Error> { - if self.writer.available() < len { - Err(Error::InsufficientSpace) - } else { - self.writer.pos += len; - Ok(()) - } - } -} - -impl<'m, 'd> AsRef<[u8]> for SliceWriter<'m, 'd> { - fn as_ref(&self) -> &[u8] { - &self.writer.data[self.writer.pos..] - } -} - -impl<'m, 'd> AsMut<[u8]> for SliceWriter<'m, 'd> { - fn as_mut(&mut self) -> &mut [u8] { - &mut self.writer.data[self.writer.pos..] - } -} - // Not a byte reader. It is just a cursor to track where a byte slice is being written. pub struct ReadCursor<'d> { pos: usize, diff --git a/host/src/gatt.rs b/host/src/gatt.rs index ead18ff..35e6695 100644 --- a/host/src/gatt.rs +++ b/host/src/gatt.rs @@ -1,67 +1,148 @@ -use crate::adapter::Adapter; -use crate::att::Att; -use crate::attribute::Attribute; +use core::fmt; + +use crate::att::{self, Att, ATT_HANDLE_VALUE_NTF_OPTCODE}; +use crate::attribute::CharacteristicHandle; use crate::attribute_server::AttributeServer; use crate::connection::Connection; -use crate::l2cap::L2capPacket; +use crate::connection_manager::DynamicConnectionManager; +use crate::cursor::WriteCursor; +use crate::packet_pool::{AllocId, DynamicPacketPool}; use crate::pdu::Pdu; use bt_hci::param::ConnHandle; use embassy_sync::blocking_mutex::raw::RawMutex; use embassy_sync::channel::{DynamicReceiver, DynamicSender}; -pub struct GattServer<'a, 'b, 'c, 'd> { - server: AttributeServer<'a, 'b>, - rx: DynamicReceiver<'c, (ConnHandle, Pdu<'d>)>, - tx: DynamicSender<'c, (ConnHandle, Pdu<'d>)>, +pub struct GattServer<'reference, 'values, 'resources, M: RawMutex, const MAX: usize> { + pub(crate) server: AttributeServer<'reference, 'values, M, MAX>, + pub(crate) rx: DynamicReceiver<'reference, (ConnHandle, Pdu<'resources>)>, + pub(crate) tx: DynamicSender<'reference, (ConnHandle, Pdu<'resources>)>, + pub(crate) pool_id: AllocId, + pub(crate) pool: &'resources dyn DynamicPacketPool<'resources>, + pub(crate) connections: &'reference dyn DynamicConnectionManager, } -impl<'a, 'b, 'c, 'd> GattServer<'a, 'b, 'c, 'd> { - pub fn new< - M: RawMutex, - T, - const CONNS: usize, - const CHANNELS: usize, - const L2CAP_TXQ: usize, - const L2CAP_RXQ: usize, - >( - adapter: &'c Adapter<'d, M, T, CONNS, CHANNELS, L2CAP_TXQ, L2CAP_RXQ>, - attributes: &'a mut [Attribute<'b>], - ) -> Self { - Self { - server: AttributeServer::new(attributes), - rx: adapter.att_inbound.receiver().into(), - tx: adapter.outbound.sender().into(), - } - } - - // TODO: Actually return events - pub async fn next(&mut self) -> GattEvent<'d> { +impl<'reference, 'values, 'resources, M: RawMutex, const MAX: usize> + GattServer<'reference, 'values, 'resources, M, MAX> +{ + pub async fn next(&self) -> Result, ()> { loop { let (handle, pdu) = self.rx.receive().await; match Att::decode(pdu.as_ref()) { - Ok(att) => match self.server.process(att) { - Ok(Some(payload)) => { - let mut data = pdu.packet; - let packet = L2capPacket { channel: 4, payload }; - let len = packet.encode(data.as_mut()).unwrap(); - self.tx.send((handle, Pdu::new(data, len))).await; - } - Ok(None) => { - debug!("No response sent"); - } - Err(e) => { - warn!("Error processing attribute: {:?}", e); + Ok(att) => { + let Some(mut response) = self.pool.alloc(self.pool_id) else { + return Err(()); + }; + let mut w = WriteCursor::new(response.as_mut()); + let (mut header, mut data) = w.split(4).map_err(|_| ())?; + + match att { + Att::ExchangeMtu { mtu } => { + let mtu = self.connections.exchange_att_mtu(handle, mtu); + data.write(att::ATT_EXCHANGE_MTU_RESPONSE_OPCODE).map_err(|_| ())?; + data.write(mtu).map_err(|_| ())?; + + header.write(data.len() as u16).map_err(|_| ())?; + header.write(4 as u16).map_err(|_| ())?; + let len = header.len() + data.len(); + drop(header); + drop(data); + drop(w); + self.tx.send((handle, Pdu::new(response, len))).await; + } + _ => match self.server.process(handle, att, data.write_buf()) { + Ok(Some(written)) => { + let mtu = self.connections.get_att_mtu(handle); + data.commit(written).map_err(|_| ())?; + data.truncate(mtu as usize); + header.write(written as u16).map_err(|_| ())?; + header.write(4 as u16).map_err(|_| ())?; + let len = header.len() + data.len(); + drop(header); + drop(data); + drop(w); + self.tx.send((handle, Pdu::new(response, len))).await; + } + Ok(None) => { + debug!("No response sent"); + } + Err(e) => { + warn!("Error processing attribute: {:?}", e); + } + }, } - }, + } Err(e) => { warn!("Error decoding attribute request: {:02x}", e); } } } } + + /// Write a value to a characteristic, and notify a connection with the new value of the characteristic. + /// + /// If the provided connection has not subscribed for this characteristic, it will not be notified. + /// + /// If the characteristic for the handle cannot be found, an error is returned. + pub async fn notify( + &self, + handle: CharacteristicHandle, + connection: &Connection<'_>, + value: &[u8], + ) -> Result<(), ()> { + let conn = connection.handle(); + self.server.table.set(handle, value).map_err(|_| ())?; + + let cccd_handle = handle.cccd_handle.ok_or(())?; + + if !self.server.should_notify(conn, cccd_handle) { + // No reason to fail? + return Ok(()); + } + + let Some(mut packet) = self.pool.alloc(self.pool_id) else { + return Err(()); + }; + let mut w = WriteCursor::new(packet.as_mut()); + let (mut header, mut data) = w.split(4).map_err(|_| ())?; + data.write(ATT_HANDLE_VALUE_NTF_OPTCODE).map_err(|_| ())?; + data.write(handle.handle).map_err(|_| ())?; + data.append(value).map_err(|_| ())?; + + header.write(data.len() as u16).map_err(|_| ())?; + header.write(4 as u16).map_err(|_| ())?; + let total = header.len() + data.len(); + drop(header); + drop(data); + drop(w); + self.tx.send((conn, Pdu::new(packet, total))).await; + Ok(()) + } } #[derive(Clone)] -pub enum GattEvent<'a> { - Write(Connection<'a>, &'a Attribute<'a>), +pub enum GattEvent<'reference, 'values> { + Write { + connection: Connection<'reference>, + handle: CharacteristicHandle, + value: &'values [u8], + }, +} + +impl<'reference, 'values> fmt::Debug for GattEvent<'reference, 'values> { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + match self { + Self::Write { + connection: _, + handle: _, + value: _, + } => f.debug_struct("GattEvent::Write").finish(), + } + } +} + +#[cfg(feature = "defmt")] +impl<'reference, 'values> defmt::Format for GattEvent<'reference, 'values> { + fn format(&self, fmt: defmt::Formatter) { + defmt::write!(fmt, "{}", defmt::Debug2Format(self)) + } } diff --git a/host/src/lib.rs b/host/src/lib.rs index 896ec73..141225d 100644 --- a/host/src/lib.rs +++ b/host/src/lib.rs @@ -6,14 +6,6 @@ use advertise::AdvertisementDataError; use bt_hci::FromHciBytesError; -// TODO: Make these configurable -pub(crate) const ATT_MTU: usize = 64; - -pub(crate) const ATT_RXQ: usize = 3; -pub(crate) const L2CAP_RXQ: usize = 3; -// NOTE: This one is actually shared for all connections -pub(crate) const L2CAP_TXQ: usize = 3; - mod fmt; mod att;