diff --git a/examples/nrf-sdc/Cargo.toml b/examples/nrf-sdc/Cargo.toml index c274924..2ceba1b 100644 --- a/examples/nrf-sdc/Cargo.toml +++ b/examples/nrf-sdc/Cargo.toml @@ -12,7 +12,7 @@ embassy-futures = "0.1.1" embassy-sync = "0.5" futures = { version = "0.3", default-features = false, features = ["async-await"]} -nrf-sdc = { version = "0.1.0", default-features = false, features = ["defmt", "nrf52833", "peripheral"] } +nrf-sdc = { version = "0.1.0", default-features = false, features = ["defmt", "nrf52833", "peripheral", "central"] } nrf-mpsl = { version = "0.1.0", default-features = false, features = ["defmt", "critical-section-impl"] } bt-hci = { version = "0.1.0", default-features = false, features = ["defmt"] } trouble-host = { version = "0.1.0", path = "../../host", features = ["defmt"] } diff --git a/examples/nrf-sdc/src/bin/ble_l2cap_central.rs b/examples/nrf-sdc/src/bin/ble_l2cap_central.rs new file mode 100644 index 0000000..ef7c062 --- /dev/null +++ b/examples/nrf-sdc/src/bin/ble_l2cap_central.rs @@ -0,0 +1,150 @@ +#![no_std] +#![no_main] +#![feature(type_alias_impl_trait)] + +use bt_hci::cmd::SyncCmd; +use bt_hci::param::BdAddr; +use defmt::{info, unwrap}; +use embassy_executor::Spawner; +use embassy_futures::join::join; +use embassy_nrf::{bind_interrupts, pac}; +use embassy_sync::blocking_mutex::raw::NoopRawMutex; +use embassy_time::{Duration, Timer}; +use nrf_sdc::{self as sdc, mpsl, mpsl::MultiprotocolServiceLayer}; +use sdc::rng_pool::RngPool; +use sdc::vendor::ZephyrWriteBdAddr; +use static_cell::StaticCell; +use trouble_host::{ + adapter::AdvertiseConfig, + adapter::ScanConfig, + adapter::{Adapter, HostResources}, + connection::Connection, + l2cap::L2capChannel, + scanner::Scanner, + PacketQos, +}; + +use {defmt_rtt as _, panic_probe as _}; + +bind_interrupts!(struct Irqs { + RNG => nrf_sdc::rng_pool::InterruptHandler; + SWI0_EGU0 => nrf_sdc::mpsl::LowPrioInterruptHandler; + POWER_CLOCK => nrf_sdc::mpsl::ClockInterruptHandler; + RADIO => nrf_sdc::mpsl::HighPrioInterruptHandler; + TIMER0 => nrf_sdc::mpsl::HighPrioInterruptHandler; + RTC0 => nrf_sdc::mpsl::HighPrioInterruptHandler; +}); + +#[embassy_executor::task] +async fn mpsl_task(mpsl: &'static MultiprotocolServiceLayer<'static>) -> ! { + mpsl.run().await +} + +fn bd_addr() -> BdAddr { + unsafe { + let ficr = &*pac::FICR::ptr(); + let high = u64::from((ficr.deviceid[1].read().bits() & 0x0000ffff) | 0x0000c000); + let addr = high << 32 | u64::from(ficr.deviceid[0].read().bits()); + BdAddr::new(unwrap!(addr.to_le_bytes()[..6].try_into())) + } +} + +fn build_sdc<'d, const N: usize>( + p: nrf_sdc::Peripherals<'d>, + rng: &'d RngPool<'d>, + mpsl: &'d MultiprotocolServiceLayer<'d>, + mem: &'d mut sdc::Mem, +) -> Result, nrf_sdc::Error> { + sdc::Builder::new()? + .support_scan()? + .support_central()? + .central_count(1)? + .buffer_cfg(27, 27, 20, 20)? + .build(p, rng, mpsl, mem) +} + +#[embassy_executor::main] +async fn main(spawner: Spawner) { + let p = embassy_nrf::init(Default::default()); + let pac_p = pac::Peripherals::take().unwrap(); + + let mpsl_p = mpsl::Peripherals::new( + pac_p.CLOCK, + pac_p.RADIO, + p.RTC0, + p.TIMER0, + p.TEMP, + p.PPI_CH19, + p.PPI_CH30, + p.PPI_CH31, + ); + let lfclk_cfg = mpsl::raw::mpsl_clock_lfclk_cfg_t { + source: mpsl::raw::MPSL_CLOCK_LF_SRC_RC as u8, + rc_ctiv: mpsl::raw::MPSL_RECOMMENDED_RC_CTIV as u8, + rc_temp_ctiv: mpsl::raw::MPSL_RECOMMENDED_RC_TEMP_CTIV as u8, + accuracy_ppm: mpsl::raw::MPSL_DEFAULT_CLOCK_ACCURACY_PPM as u16, + skip_wait_lfclk_started: mpsl::raw::MPSL_DEFAULT_SKIP_WAIT_LFCLK_STARTED != 0, + }; + static MPSL: StaticCell = StaticCell::new(); + let mpsl = MPSL.init(unwrap!(mpsl::MultiprotocolServiceLayer::new(mpsl_p, Irqs, lfclk_cfg))); + spawner.must_spawn(mpsl_task(&*mpsl)); + + let sdc_p = sdc::Peripherals::new( + pac_p.ECB, pac_p.AAR, p.NVMC, p.PPI_CH17, p.PPI_CH18, p.PPI_CH20, p.PPI_CH21, p.PPI_CH22, p.PPI_CH23, + p.PPI_CH24, p.PPI_CH25, p.PPI_CH26, p.PPI_CH27, p.PPI_CH28, p.PPI_CH29, + ); + + let mut pool = [0; 256]; + let rng = sdc::rng_pool::RngPool::new(p.RNG, Irqs, &mut pool, 64); + + let mut sdc_mem = sdc::Mem::<6544>::new(); + let sdc = unwrap!(build_sdc(sdc_p, &rng, mpsl, &mut sdc_mem)); + + info!("Our address = {:02x}", bd_addr()); + unwrap!(ZephyrWriteBdAddr::new(bd_addr()).exec(&sdc).await); + Timer::after(Duration::from_millis(200)).await; + + static HOST_RESOURCES: StaticCell> = StaticCell::new(); + let host_resources = HOST_RESOURCES.init(HostResources::new(PacketQos::Guaranteed(4))); + + static ADAPTER: StaticCell> = StaticCell::new(); + let adapter = ADAPTER.init(Adapter::new(host_resources)); + + let config = ScanConfig { params: None }; + let mut scanner = unwrap!(adapter.scan(&sdc, config).await); + + // NOTE: Modify this to match the address of the peripheral you want to connect to + let target: BdAddr = BdAddr::new([0xf5, 0x9f, 0x1a, 0x05, 0xe4, 0xee]); + + info!("Scanning for peripheral..."); + let _ = join(adapter.run(&sdc), async { + loop { + let reports = scanner.next().await; + for report in reports.iter() { + let report = report.unwrap(); + if report.addr == target { + let conn = Connection::connect(adapter, report.addr).await; + info!("Connected, creating l2cap channel"); + let mut ch1: L2capChannel<'_, 27> = unwrap!(L2capChannel::create(adapter, &conn, 0x2349).await); + info!("New l2cap channel created, sending some data!"); + for i in 0..10 { + let mut tx = [i; 27]; + let _ = unwrap!(ch1.send(&mut tx).await); + } + info!("Sent data, waiting for them to be sent back"); + let mut rx = [0; 27]; + for i in 0..10 { + let len = unwrap!(ch1.receive(&mut rx).await); + assert_eq!(len, rx.len()); + assert_eq!(rx, [i; 27]); + } + + info!("Received successfully!"); + + Timer::after(Duration::from_secs(60)).await; + } + } + } + }) + .await; +} diff --git a/examples/nrf-sdc/src/bin/ble_l2cap_peripheral.rs b/examples/nrf-sdc/src/bin/ble_l2cap_peripheral.rs index 3b86325..8bbb197 100644 --- a/examples/nrf-sdc/src/bin/ble_l2cap_peripheral.rs +++ b/examples/nrf-sdc/src/bin/ble_l2cap_peripheral.rs @@ -17,7 +17,6 @@ use static_cell::StaticCell; use trouble_host::{ ad_structure::{AdStructure, BR_EDR_NOT_SUPPORTED, LE_GENERAL_DISCOVERABLE}, adapter::AdvertiseConfig, - adapter::Config as BleConfig, adapter::{Adapter, HostResources}, attribute::{AttributesBuilder, CharacteristicProp, ServiceBuilder, Uuid}, connection::Connection, @@ -99,10 +98,10 @@ 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::<3128>::new(); + let mut sdc_mem = sdc::Mem::<6224>::new(); let sdc = unwrap!(build_sdc(sdc_p, &rng, mpsl, &mut sdc_mem)); - info!("Advertising as {:02x}", bd_addr()); + info!("Our address = {:02x}", bd_addr()); unwrap!(ZephyrWriteBdAddr::new(bd_addr()).exec(&sdc).await); Timer::after(Duration::from_millis(200)).await; @@ -112,15 +111,13 @@ async fn main(spawner: Spawner) { static ADAPTER: StaticCell> = StaticCell::new(); let adapter = ADAPTER.init(Adapter::new(host_resources)); - let config = BleConfig { - advertise: Some(AdvertiseConfig { - params: None, - data: &[ - AdStructure::Flags(LE_GENERAL_DISCOVERABLE | BR_EDR_NOT_SUPPORTED), - AdStructure::ServiceUuids16(&[Uuid::Uuid16([0x0f, 0x18])]), - AdStructure::CompleteLocalName("Trouble"), - ], - }), + let config = AdvertiseConfig { + params: None, + data: &[ + AdStructure::Flags(LE_GENERAL_DISCOVERABLE | BR_EDR_NOT_SUPPORTED), + AdStructure::ServiceUuids16(&[Uuid::Uuid16([0x0f, 0x18])]), + AdStructure::CompleteLocalName("Trouble"), + ], }; let mut attributes: AttributesBuilder<'_, 10> = AttributesBuilder::new(); @@ -135,7 +132,6 @@ async fn main(spawner: Spawner) { unwrap!(adapter.advertise(&sdc, config).await); - info!("Starting advertising and GATT service"); let _ = join( adapter.run(&sdc), async { @@ -149,12 +145,14 @@ async fn main(spawner: Spawner) { }, async { loop { + info!("Waiting for connection..."); let conn = Connection::accept(adapter).await; - info!("New connection accepted!"); + + info!("Connection established"); let mut ch1: L2capChannel<'_, 27> = unwrap!(L2capChannel::accept(adapter, &conn, 0x2349).await); - info!("New l2cap channel created by remote!"); + info!("L2CAP channel accepted"); let mut rx = [0; 27]; for i in 0..10 { let len = unwrap!(ch1.receive(&mut rx).await); @@ -162,13 +160,13 @@ async fn main(spawner: Spawner) { assert_eq!(rx, [i; 27]); } - info!("Received successfully! Sending bytes back"); + info!("L2CAP data received, echoing"); Timer::after(Duration::from_secs(1)).await; for i in 0..10 { let mut tx = [i; 27]; let _ = unwrap!(ch1.send(&mut tx).await); } - info!("Bytes sent"); + info!("L2CAP data echoed"); Timer::after(Duration::from_secs(60)).await; } diff --git a/host/Cargo.toml b/host/Cargo.toml index aa18db4..902dd85 100644 --- a/host/Cargo.toml +++ b/host/Cargo.toml @@ -18,6 +18,7 @@ resolver = "2" bt-hci = { version = "0.1.0" } embedded-io-async = { version = "0.6" } embassy-sync = "0.5" +embassy-time = "0.3" embassy-futures = "0.1" futures = { version = "0.3", default-features = false } heapless = "0.8" diff --git a/host/src/adapter.rs b/host/src/adapter.rs index 111fdee..e8392d7 100644 --- a/host/src/adapter.rs +++ b/host/src/adapter.rs @@ -5,21 +5,26 @@ use crate::cursor::{ReadCursor, WriteCursor}; use crate::l2cap::{L2capPacket, L2CAP_CID_ATT, L2CAP_CID_DYN_START, L2CAP_CID_LE_U_SIGNAL}; //self, L2capLeSignal, L2capPacket, L2capState, LeCreditConnReq, SignalCode}; use crate::packet_pool::{DynamicPacketPool, PacketPool, Qos, ATT_ID}; use crate::pdu::Pdu; +use crate::scanner::{ScanReports, Scanner}; use crate::types::l2cap::L2capLeSignal; use crate::{codec, Error}; use bt_hci::cmd::controller_baseband::SetEventMask; -use bt_hci::cmd::le::{LeSetAdvData, LeSetAdvEnable, LeSetAdvParams}; +use bt_hci::cmd::le::{ + LeCreateConn, LeCreateConnParams, LeSetAdvData, LeSetAdvEnable, LeSetAdvParams, LeSetScanEnable, + LeSetScanEnableParams, LeSetScanParams, +}; use bt_hci::cmd::link_control::{Disconnect, DisconnectParams}; -use bt_hci::cmd::SyncCmd; +use bt_hci::cmd::{AsyncCmd, SyncCmd}; use bt_hci::data::{AclBroadcastFlag, AclPacket, AclPacketBoundary}; use bt_hci::event::le::LeEvent; use bt_hci::event::Event; use bt_hci::param::{BdAddr, ConnHandle, DisconnectReason, EventMask}; -use bt_hci::ControllerCmdSync; -use bt_hci::ControllerToHostPacket; +use bt_hci::{AsHciBytes, ControllerToHostPacket}; +use bt_hci::{ControllerCmdAsync, ControllerCmdSync}; use embassy_futures::select::{select4, Either4}; use embassy_sync::blocking_mutex::raw::RawMutex; use embassy_sync::channel::Channel; +use heapless::Vec; pub struct HostResources { pool: PacketPool, @@ -40,14 +45,8 @@ pub struct AdvertiseConfig<'d> { pub data: &'d [AdStructure<'d>], } -pub struct Config<'a> { - pub advertise: Option>, -} - -impl<'a> Default for Config<'a> { - fn default() -> Self { - Self { advertise: None } - } +pub struct ScanConfig { + pub params: Option, } pub struct Adapter<'d, M, const CONNS: usize, const CHANNELS: usize, const L2CAP_TXQ: usize, const L2CAP_RXQ: usize> @@ -61,10 +60,12 @@ where pub(crate) outbound: Channel), L2CAP_TXQ>, pub(crate) control: Channel, + pub(crate) scanner: Channel, } pub(crate) enum ControlCommand { Disconnect(DisconnectParams), + Connect(LeCreateConnParams), } #[derive(Debug)] @@ -94,40 +95,64 @@ where channels: ChannelManager::new(&host_resources.pool), pool: &host_resources.pool, att_inbound: Channel::new(), + scanner: Channel::new(), outbound: Channel::new(), control: Channel::new(), } } - pub async fn advertise(&self, controller: &T, config: Config<'_>) -> Result<(), Error> + pub async fn stop_scan(&self, controller: &T) -> Result<(), Error> + where + T: ControllerCmdSync + ControllerCmdSync, + { + LeSetScanEnable::new(false, false).exec(controller).await?; + Ok(()) + } + + pub async fn scan(&self, controller: &T, scan: ScanConfig) -> Result, Error> + where + T: ControllerCmdSync + ControllerCmdSync, + { + let params = &scan.params.unwrap_or(LeSetScanParams::new( + bt_hci::param::LeScanKind::Passive, + bt_hci::param::Duration::from_millis(1_000), + bt_hci::param::Duration::from_millis(1_000), + bt_hci::param::AddrKind::PUBLIC, + bt_hci::param::ScanningFilterPolicy::BasicUnfiltered, + )); + params.exec(controller).await?; + + LeSetScanEnable::new(true, true).exec(controller).await?; + Ok(Scanner::new(self.scanner.receiver().into())) + } + + pub async fn advertise(&self, controller: &T, adv: AdvertiseConfig<'_>) -> Result<(), Error> where T: ControllerCmdSync + ControllerCmdSync + ControllerCmdSync, { - if let Some(adv) = &config.advertise { - let params = &adv.params.unwrap_or(LeSetAdvParams::new( - bt_hci::param::Duration::from_millis(400), - bt_hci::param::Duration::from_millis(400), - bt_hci::param::AdvKind::AdvInd, - bt_hci::param::AddrKind::PUBLIC, - bt_hci::param::AddrKind::PUBLIC, - BdAddr::default(), - bt_hci::param::AdvChannelMap::ALL, - bt_hci::param::AdvFilterPolicy::default(), - )); + let params = &adv.params.unwrap_or(LeSetAdvParams::new( + bt_hci::param::Duration::from_millis(400), + bt_hci::param::Duration::from_millis(400), + bt_hci::param::AdvKind::AdvInd, + bt_hci::param::AddrKind::PUBLIC, + bt_hci::param::AddrKind::PUBLIC, + BdAddr::default(), + bt_hci::param::AdvChannelMap::ALL, + bt_hci::param::AdvFilterPolicy::default(), + )); - params.exec(controller).await?; + params.exec(controller).await?; - let mut data = [0; 31]; - let mut w = WriteCursor::new(&mut data[..]); - for item in adv.data.iter() { - item.encode(&mut w)?; - } - let len = w.len(); - drop(w); - LeSetAdvData::new(len as u8, data).exec(controller).await?; - LeSetAdvEnable::new(true).exec(controller).await?; + let mut data = [0; 31]; + let mut w = WriteCursor::new(&mut data[..]); + for item in adv.data.iter() { + item.encode(&mut w)?; } + let len = w.len(); + drop(w); + LeSetAdvData::new(len as u8, data).exec(controller).await?; + LeSetAdvEnable::new(true).exec(controller).await?; Ok(()) } @@ -178,7 +203,10 @@ where pub async fn run(&'d self, controller: &T) -> Result<(), Error> where - T: ControllerCmdSync + ControllerCmdSync, + T: ControllerCmdSync + + ControllerCmdSync + + ControllerCmdAsync + + ControllerCmdSync, { SetEventMask::new( EventMask::new() @@ -194,6 +222,7 @@ where loop { let mut rx = [0u8; 259]; let mut tx = [0u8; 259]; + // info!("Entering select"); match select4( controller.read(&mut rx), self.outbound.receive(), @@ -202,74 +231,119 @@ where ) .await { - Either4::First(result) => match result { - Ok(ControllerToHostPacket::Acl(acl)) => match self.handle_acl(acl).await { - Ok(_) => {} - Err(e) => { - info!("Error processing ACL packet: {:?}", e); - } - }, - Ok(ControllerToHostPacket::Event(event)) => match event { - Event::Le(event) => match event { - LeEvent::LeConnectionComplete(e) => { - if let Err(err) = self.connections.connect( - e.handle, - ConnectionInfo { - handle: e.handle, - status: e.status, - role: e.role, - peer_address: e.peer_addr, - interval: e.conn_interval.as_u16(), - latency: e.peripheral_latency, - timeout: e.supervision_timeout.as_u16(), - }, - ) { - warn!("Error establishing connection: {:?}", err); - Disconnect::new(e.handle, DisconnectReason::RemoteDeviceTerminatedConnLowResources) + Either4::First(result) => { + // info!("Incoming event"); + match result { + Ok(ControllerToHostPacket::Acl(acl)) => match self.handle_acl(acl).await { + Ok(_) => {} + Err(e) => { + info!("Error processing ACL packet: {:?}", e); + } + }, + Ok(ControllerToHostPacket::Event(event)) => match event { + Event::Le(event) => match event { + LeEvent::LeConnectionComplete(e) => { + if let Err(err) = self.connections.connect( + e.handle, + ConnectionInfo { + handle: e.handle, + status: e.status, + role: e.role, + peer_address: e.peer_addr, + interval: e.conn_interval.as_u16(), + latency: e.peripheral_latency, + timeout: e.supervision_timeout.as_u16(), + }, + ) { + warn!("Error establishing connection: {:?}", err); + Disconnect::new( + e.handle, + DisconnectReason::RemoteDeviceTerminatedConnLowResources, + ) .exec(controller) .await .unwrap(); + } + } + LeEvent::LeAdvertisingReport(data) => { + let mut reports = Vec::new(); + reports.extend_from_slice(&data.reports.bytes).unwrap(); + self.scanner + .send(ScanReports { + num_reports: data.reports.num_reports, + reports, + }) + .await; } + _ => { + warn!("Unknown event: {:?}", event); + } + }, + Event::DisconnectionComplete(e) => { + info!("Disconnected: {:?}", e); + let _ = self.connections.disconnect(e.handle); + } + Event::NumberOfCompletedPackets(c) => { + //info!("Confirmed {} packets sent", c.completed_packets.len()); } _ => { warn!("Unknown event: {:?}", event); } }, - Event::DisconnectionComplete(e) => { - info!("Disconnected: {:?}", e); - let _ = self.connections.disconnect(e.handle); + Ok(p) => { + info!("Ignoring packet: {:?}", p); } - Event::NumberOfCompletedPackets(c) => {} - _ => { - warn!("Unknown event: {:?}", event); + Err(e) => { + info!("Error from controller: {:?}", e); } - }, - Ok(p) => { - info!("Ignoring packet: {:?}", p); } - Err(e) => { - info!("Error from controller: {:?}", e); - } - }, - Either4::Second((handle, pdu)) => { + } + Either4::Second((handle, mut pdu)) => { + // info!("Outgoing packet"); let acl = AclPacket::new(handle, pdu.pb, AclBroadcastFlag::PointToPoint, pdu.as_ref()); match controller.write_acl_data(&acl).await { - Ok(_) => {} + Ok(_) => { + pdu.as_mut().iter_mut().for_each(|b| *b = 0xFF); + } Err(e) => { warn!("Error writing some ACL data to controller: {:?}", e); panic!(":("); } } } - Either4::Third(command) => match command { - ControlCommand::Disconnect(params) => { - Disconnect::new(params.handle, params.reason) + Either4::Third(command) => { + // info!("Outgoing command"); + match command { + ControlCommand::Connect(params) => { + LeSetScanEnable::new(false, false).exec(controller).await.unwrap(); + LeCreateConn::new( + params.le_scan_interval, + params.le_scan_window, + params.use_filter_accept_list, + params.peer_addr_kind, + params.peer_addr, + params.own_addr_kind, + params.conn_interval_min, + params.conn_interval_max, + params.max_latency, + params.supervision_timeout, + params.min_ce_length, + params.max_ce_length, + ) .exec(controller) .await .unwrap(); + } + ControlCommand::Disconnect(params) => { + Disconnect::new(params.handle, params.reason) + .exec(controller) + .await + .unwrap(); + } } - }, + } Either4::Fourth((handle, response)) => { + // info!("Outgoing signal: {:?}", response); let mut w = WriteCursor::new(&mut tx); let (mut header, mut body) = w.split(4)?; diff --git a/host/src/channel_manager.rs b/host/src/channel_manager.rs index bd3ee2d..efc7417 100644 --- a/host/src/channel_manager.rs +++ b/host/src/channel_manager.rs @@ -26,6 +26,7 @@ struct State { channels: [ChannelState; CHANNELS], accept_waker: WakerRegistration, create_waker: WakerRegistration, + credit_wakers: [WakerRegistration; CHANNELS], } /// Channel manager for L2CAP channels used directly by clients. @@ -38,7 +39,7 @@ pub struct ChannelManager<'d, M: RawMutex, const CHANNELS: usize, const L2CAP_TX } pub trait DynamicChannelManager { - fn request_to_send(&self, cid: u16, credits: usize) -> Result<(), ()>; + fn poll_request_to_send(&self, cid: u16, credits: usize, cx: &mut Context<'_>) -> Poll>; fn confirm_received(&self, cid: u16, credits: usize) -> Result<(), ()>; } @@ -48,6 +49,7 @@ impl<'d, M: RawMutex, const CHANNELS: usize, const L2CAP_TXQ: usize, const L2CAP const TX_CHANNEL: Channel, L2CAP_TXQ> = Channel::new(); const RX_CHANNEL: Channel>, L2CAP_RXQ> = Channel::new(); const DISCONNECTED: ChannelState = ChannelState::Disconnected; + const CREDIT_WAKER: WakerRegistration = WakerRegistration::new(); pub fn new(pool: &'d dyn DynamicPacketPool<'d>) -> Self { Self { pool, @@ -55,6 +57,7 @@ impl<'d, M: RawMutex, const CHANNELS: usize, const L2CAP_TXQ: usize, const L2CAP channels: [Self::DISCONNECTED; CHANNELS], accept_waker: WakerRegistration::new(), create_waker: WakerRegistration::new(), + credit_wakers: [Self::CREDIT_WAKER; CHANNELS], })), signal: Channel::new(), inbound: [Self::RX_CHANNEL; CHANNELS], @@ -143,10 +146,11 @@ impl<'d, M: RawMutex, const CHANNELS: usize, const L2CAP_TXQ: usize, const L2CAP fn remote_credits(&self, cid: u16, credits: u16) -> Result<(), ()> { self.state.lock(|state| { let mut state = state.borrow_mut(); - for storage in state.channels.iter_mut() { + for (idx, storage) in state.channels.iter_mut().enumerate() { match storage { - ChannelState::Connected(state) if state.peer_cid == cid => { - state.peer_credits += credits; + ChannelState::Connected(s) if s.peer_cid == cid => { + s.peer_credits += credits; + state.credit_wakers[idx].wake(); return Ok(()); } _ => {} @@ -191,7 +195,7 @@ impl<'d, M: RawMutex, const CHANNELS: usize, const L2CAP_TXQ: usize, const L2CAP let (idx, state) = poll_fn(|cx| { self.poll_accept(conn, psm, cx, |idx, req| { req_id = req.request_id; - let mps = req.mps.min(self.pool.mtu() as u16); + let mps = req.mps.min(self.pool.mtu() as u16 - 4); mtu = req.mtu.min(mtu); let credits = self.pool.min_available(AllocId::dynamic(idx)) as u16; ConnectedState { @@ -246,7 +250,7 @@ impl<'d, M: RawMutex, const CHANNELS: usize, const L2CAP_TXQ: usize, const L2CAP 0, L2capLeSignalData::LeCreditConnReq(LeCreditConnReq { psm, - mps: self.pool.mtu() as u16, + mps: self.pool.mtu() as u16 - 4, scid: cid, mtu, credits: self.pool.min_available(AllocId::dynamic(idx)) as u16, @@ -300,6 +304,7 @@ impl<'d, M: RawMutex, const CHANNELS: usize, const L2CAP_TXQ: usize, const L2CAP } pub fn control(&self, conn: ConnHandle, signal: L2capLeSignal) -> Result<(), ()> { + // info!("Inbound signal: {:?}", signal); match signal.data { L2capLeSignalData::LeCreditConnReq(req) => { if let Err(e) = self.connect(ConnectingState { @@ -395,23 +400,24 @@ impl<'d, M: RawMutex, const CHANNELS: usize, const L2CAP_TXQ: usize, const L2CAP }) } - fn request_to_send(&self, cid: u16, credits: usize) -> Result<(), ()> { + fn poll_request_to_send(&self, cid: u16, credits: usize, cx: &mut Context<'_>) -> Poll> { self.state.lock(|state| { let mut state = state.borrow_mut(); - for storage in state.channels.iter_mut() { + for (idx, storage) in state.channels.iter_mut().enumerate() { match storage { - ChannelState::Connected(state) if cid == state.cid => { - if credits <= state.peer_credits as usize { - state.peer_credits = state.peer_credits - credits as u16; - return Ok(()); + ChannelState::Connected(s) if cid == s.cid => { + if credits <= s.peer_credits as usize { + s.peer_credits = s.peer_credits - credits as u16; + return Poll::Ready(Ok(())); } else { - return Err(()); + state.credit_wakers[idx].register(cx.waker()); + return Poll::Pending; } } _ => {} } } - return Err(()); + return Poll::Ready(Err(())); }) } } diff --git a/host/src/connection.rs b/host/src/connection.rs index 72bbaad..55b7703 100644 --- a/host/src/connection.rs +++ b/host/src/connection.rs @@ -1,6 +1,6 @@ use bt_hci::{ - cmd::link_control::DisconnectParams, - param::{ConnHandle, DisconnectReason}, + cmd::{le::LeCreateConnParams, link_control::DisconnectParams}, + param::{AddrKind, BdAddr, ConnHandle, DisconnectReason, Duration}, }; use embassy_sync::{blocking_mutex::raw::RawMutex, channel::DynamicSender}; @@ -26,7 +26,7 @@ impl<'d> Connection<'d> { >( adapter: &'d Adapter<'d, M, CONNS, CHANNELS, L2CAP_TXQ, L2CAP_RXQ>, ) -> Self { - let handle = adapter.connections.accept().await; + let handle = adapter.connections.accept(None).await; Connection { handle, control: adapter.control.sender().into(), @@ -41,4 +41,37 @@ impl<'d> Connection<'d> { })) .await; } + + pub async fn connect< + M: RawMutex, + const CONNS: usize, + const CHANNELS: usize, + const L2CAP_TXQ: usize, + const L2CAP_RXQ: usize, + >( + adapter: &'d Adapter<'d, M, CONNS, CHANNELS, L2CAP_TXQ, L2CAP_RXQ>, + peer_addr: BdAddr, + ) -> Self { + // TODO: Make this configurable + let params = LeCreateConnParams { + le_scan_interval: Duration::from_micros(1707500), + le_scan_window: Duration::from_micros(312500), + use_filter_accept_list: false, + peer_addr_kind: AddrKind::PUBLIC, + peer_addr, + own_addr_kind: AddrKind::PUBLIC, + conn_interval_min: Duration::from_millis(25), + conn_interval_max: Duration::from_millis(50), + max_latency: 0, + supervision_timeout: Duration::from_millis(250), + min_ce_length: Duration::from_millis(0), + max_ce_length: Duration::from_millis(0), + }; + adapter.control.send(ControlCommand::Connect(params)).await; + let handle = adapter.connections.accept(Some(params.peer_addr)).await; + Connection { + handle, + control: adapter.control.sender().into(), + } + } } diff --git a/host/src/connection_manager.rs b/host/src/connection_manager.rs index 50ac0e6..7321d43 100644 --- a/host/src/connection_manager.rs +++ b/host/src/connection_manager.rs @@ -65,15 +65,23 @@ impl ConnectionManager { }) } - pub fn poll_accept(&self, cx: &mut Context<'_>) -> Poll { + pub fn poll_accept(&self, peer: Option, cx: &mut Context<'_>) -> Poll { self.state.lock(|state| { let mut state = state.borrow_mut(); for storage in state.connections.iter_mut() { match storage { ConnectionState::Connecting(handle, info) => { - let handle = handle.clone(); - *storage = ConnectionState::Connected(handle.clone(), info.clone()); - return Poll::Ready(handle); + if let Some(peer) = peer { + if info.peer_address == peer { + let handle = handle.clone(); + *storage = ConnectionState::Connected(handle.clone(), info.clone()); + return Poll::Ready(handle); + } + } else { + let handle = handle.clone(); + *storage = ConnectionState::Connected(handle.clone(), info.clone()); + return Poll::Ready(handle); + } } _ => {} } @@ -83,8 +91,8 @@ impl ConnectionManager { }) } - pub async fn accept(&self) -> ConnHandle { - poll_fn(move |cx| self.poll_accept(cx)).await + pub async fn accept(&self, peer: Option) -> ConnHandle { + poll_fn(move |cx| self.poll_accept(peer, cx)).await } } diff --git a/host/src/l2cap.rs b/host/src/l2cap.rs index c47b2c4..aee73d6 100644 --- a/host/src/l2cap.rs +++ b/host/src/l2cap.rs @@ -1,3 +1,5 @@ +use core::future::poll_fn; + use crate::adapter::Adapter; use crate::channel_manager::DynamicChannelManager; use crate::codec; @@ -9,6 +11,7 @@ use bt_hci::data::AclPacket; use bt_hci::param::ConnHandle; use embassy_sync::blocking_mutex::raw::RawMutex; use embassy_sync::channel::{DynamicReceiver, DynamicSender}; +use embassy_time::{Duration, Timer}; pub(crate) const L2CAP_CID_ATT: u16 = 0x0004; pub(crate) const L2CAP_CID_LE_U_SIGNAL: u16 = 0x0005; @@ -66,7 +69,7 @@ impl<'d, const MTU: usize> L2capChannel<'d, MTU> { return Err(()); } - self.manager.request_to_send(self.cid, n_packets)?; + poll_fn(|cx| self.manager.poll_request_to_send(self.cid, n_packets, cx)).await?; // Segment using mps let (first, remaining) = buf.split_at(self.mps - 2); @@ -86,6 +89,8 @@ impl<'d, const MTU: usize> L2capChannel<'d, MTU> { Pdu::new(packet, len) }; self.tx.send((self.conn, pdu)).await; + } else { + return Err(()); } let chunks = remaining.chunks(self.mps); @@ -110,8 +115,6 @@ impl<'d, const MTU: usize> L2capChannel<'d, MTU> { } } - info!("Sent PDU ({} fragments)", n_packets); - Ok(()) } @@ -159,6 +162,7 @@ impl<'d, const MTU: usize> L2capChannel<'d, MTU> { let channels = &adapter.channels; let (state, rx) = channels.accept(connection.handle(), psm, MTU as u16).await?; + let tx = adapter.outbound.sender().into(); Ok(Self { conn: connection.handle(), @@ -185,7 +189,8 @@ impl<'d, const MTU: usize> L2capChannel<'d, MTU> { psm: u16, ) -> Result { // TODO: Use unique signal ID to ensure no collision of signal messages - let (state, rx) = adapter.channels.accept(connection.handle(), psm, MTU as u16).await?; + // + let (state, rx) = adapter.channels.create(connection.handle(), psm, MTU as u16).await?; let tx = adapter.outbound.sender().into(); Ok(Self { diff --git a/host/src/lib.rs b/host/src/lib.rs index 0da9b1c..b1c900b 100644 --- a/host/src/lib.rs +++ b/host/src/lib.rs @@ -31,6 +31,7 @@ pub mod adapter; pub mod connection; pub mod gatt; pub mod l2cap; +pub mod scanner; pub mod ad_structure; diff --git a/host/src/packet_pool.rs b/host/src/packet_pool.rs index 951138d..e185ba3 100644 --- a/host/src/packet_pool.rs +++ b/host/src/packet_pool.rs @@ -89,7 +89,7 @@ impl State State { + data: Vec, + reports: DynamicReceiver<'d, ScanReports>, +} + +impl<'d> Scanner<'d> { + pub(crate) fn new(reports: DynamicReceiver<'d, ScanReports>) -> Self { + Self { + data: Vec::new(), + reports, + } + } + + pub async fn next(&mut self) -> LeAdvReports { + let next = self.reports.receive().await; + self.data = next.reports; + let (bytes, _) = RemainingBytes::from_hci_bytes(&self.data).unwrap(); + LeAdvReports { + num_reports: next.num_reports, + bytes, + } + } +} + +pub struct ScanReports { + pub(crate) num_reports: u8, + pub(crate) reports: Vec, +}