From 3d891260bfc059bda4c6b1c11a6fcb6f851f7d35 Mon Sep 17 00:00:00 2001 From: Ulf Lilleengen Date: Wed, 20 Mar 2024 16:22:06 +0100 Subject: [PATCH 1/4] Update serial example to work * Add embedded example using uart with flow control * Use tokio for std example * Update adapter to be able to process incoming packets even as outgoing are sent --- examples/embedded-io-hci/.cargo/config.toml | 12 ++ examples/embedded-io-hci/Cargo.toml | 41 +++++ examples/embedded-io-hci/build.rs | 35 ++++ examples/embedded-io-hci/memory.x | 7 + examples/embedded-io-hci/src/main.rs | 132 ++++++++++++++ examples/serial-hci/Cargo.toml | 19 +- examples/serial-hci/src/main.rs | 183 ++++--------------- examples/serial-hci/src/serial_port.rs | 66 ------- host/src/adapter.rs | 189 +++++++++++--------- host/src/channel_manager.rs | 2 +- host/src/gatt.rs | 2 +- host/src/lib.rs | 10 ++ host/src/types/l2cap.rs | 2 +- 13 files changed, 387 insertions(+), 313 deletions(-) create mode 100644 examples/embedded-io-hci/.cargo/config.toml create mode 100644 examples/embedded-io-hci/Cargo.toml create mode 100644 examples/embedded-io-hci/build.rs create mode 100644 examples/embedded-io-hci/memory.x create mode 100644 examples/embedded-io-hci/src/main.rs delete mode 100644 examples/serial-hci/src/serial_port.rs diff --git a/examples/embedded-io-hci/.cargo/config.toml b/examples/embedded-io-hci/.cargo/config.toml new file mode 100644 index 0000000..581c1e5 --- /dev/null +++ b/examples/embedded-io-hci/.cargo/config.toml @@ -0,0 +1,12 @@ +[target.'cfg(all(target_arch = "arm", target_os = "none"))'] +runner = "probe-rs run --chip nRF52833_xxAA" + +[build] +# Pick ONE of these compilation targets +# target = "thumbv6m-none-eabi" # Cortex-M0 and Cortex-M0+ +# target = "thumbv7m-none-eabi" # Cortex-M3 +# target = "thumbv7em-none-eabi" # Cortex-M4 and Cortex-M7 (no FPU) +target = "thumbv7em-none-eabihf" # Cortex-M4F and Cortex-M7F (with FPU) + +[env] +DEFMT_LOG = "info" diff --git a/examples/embedded-io-hci/Cargo.toml b/examples/embedded-io-hci/Cargo.toml new file mode 100644 index 0000000..e420b50 --- /dev/null +++ b/examples/embedded-io-hci/Cargo.toml @@ -0,0 +1,41 @@ +[package] +name = "serial-hci" +version = "0.1.0" +edition = "2021" + +[dependencies] +embassy-executor = { version = "0.5", default-features = false, features = ["nightly", "arch-cortex-m", "executor-thread", "defmt", "integrated-timers", "executor-interrupt"] } +embassy-time = { version = "0.3.0", default-features = false, features = ["defmt", "defmt-timestamp-uptime"] } +embassy-nrf = { version = "0.1.0", default-features = false, features = ["defmt", "nrf52833", "time-driver-rtc1", "gpiote", "unstable-pac", "rt"] } +embedded-io-async = { version = "0.6.1", features = ["defmt-03"] } +embedded-io = { version = "0.6.1", features = ["defmt-03"] } +embassy-sync = { version = "0.5.0", features = ["defmt"] } +embassy-futures = "0.1.1" +cortex-m = { version = "0.7.6", features = ["critical-section-single-core"] } +cortex-m-rt = "0.7.0" +static_cell = "2" +defmt = "0.3" +defmt-rtt = "0.4.0" + +panic-probe = { version = "0.3", features = ["print-defmt"] } + +bt-hci = { version = "0.1.0", default-features = false, features = ["defmt"] } +trouble-host = { version = "0.1.0", path = "../../host", features = ["defmt"] } + +[patch.crates-io] +bt-hci = { git = "https://github.com/alexmoon/bt-hci.git", branch = "serial-controller" } +embassy-executor = { git = "https://github.com/embassy-rs/embassy.git", branch = "main" } +embassy-time = { git = "https://github.com/embassy-rs/embassy.git", branch = "main" } +embassy-nrf = { git = "https://github.com/embassy-rs/embassy.git", branch = "main" } +embassy-sync = { git = "https://github.com/embassy-rs/embassy.git", branch = "main" } +embassy-futures = { git = "https://github.com/embassy-rs/embassy.git", branch = "main" } +# +#embassy-executor = { path = "../../../embassy/embassy-executor" } +#embassy-time = { path = "../../../embassy/embassy-time"} +#embassy-nrf = { path = "../../../embassy/embassy-nrf"} +#embassy-sync = { path = "../../../embassy/embassy-sync"} +#embassy-futures = { path = "../../../embassy/embassy-futures"} +#bt-hci = { path = "../../../bt-hci" } + +[profile.release] +debug = 2 diff --git a/examples/embedded-io-hci/build.rs b/examples/embedded-io-hci/build.rs new file mode 100644 index 0000000..30691aa --- /dev/null +++ b/examples/embedded-io-hci/build.rs @@ -0,0 +1,35 @@ +//! This build script copies the `memory.x` file from the crate root into +//! a directory where the linker can always find it at build time. +//! For many projects this is optional, as the linker always searches the +//! project root directory -- wherever `Cargo.toml` is. However, if you +//! are using a workspace or have a more complicated build setup, this +//! build script becomes required. Additionally, by requesting that +//! Cargo re-run the build script whenever `memory.x` is changed, +//! updating `memory.x` ensures a rebuild of the application with the +//! new memory settings. + +use std::env; +use std::fs::File; +use std::io::Write; +use std::path::PathBuf; + +fn main() { + // Put `memory.x` in our output directory and ensure it's + // on the linker search path. + let out = &PathBuf::from(env::var_os("OUT_DIR").unwrap()); + File::create(out.join("memory.x")) + .unwrap() + .write_all(include_bytes!("memory.x")) + .unwrap(); + println!("cargo:rustc-link-search={}", out.display()); + + // By default, Cargo will re-run a build script whenever + // any file in the project changes. By specifying `memory.x` + // here, we ensure the build script is only re-run when + // `memory.x` is changed. + println!("cargo:rerun-if-changed=memory.x"); + + println!("cargo:rustc-link-arg-bins=--nmagic"); + println!("cargo:rustc-link-arg-bins=-Tlink.x"); + println!("cargo:rustc-link-arg-bins=-Tdefmt.x"); +} diff --git a/examples/embedded-io-hci/memory.x b/examples/embedded-io-hci/memory.x new file mode 100644 index 0000000..a136848 --- /dev/null +++ b/examples/embedded-io-hci/memory.x @@ -0,0 +1,7 @@ +MEMORY +{ + /* NOTE 1 K = 1 KiBi = 1024 bytes */ + /* These values correspond to the NRF52840 */ + FLASH : ORIGIN = 0x00000000, LENGTH = 512K + RAM : ORIGIN = 0x20000000, LENGTH = 128K +} diff --git a/examples/embedded-io-hci/src/main.rs b/examples/embedded-io-hci/src/main.rs new file mode 100644 index 0000000..8bf2999 --- /dev/null +++ b/examples/embedded-io-hci/src/main.rs @@ -0,0 +1,132 @@ +#![no_std] +#![no_main] +#![feature(type_alias_impl_trait)] + +use bt_hci::cmd::SyncCmd; +use bt_hci::param::BdAddr; +use bt_hci::serial::SerialController; +use defmt::{error, info, unwrap}; +use embassy_executor::Spawner; +use embassy_futures::join::join3; +use embassy_nrf::peripherals; +use embassy_nrf::{bind_interrupts, pac}; +use embassy_nrf::{buffered_uarte, uarte}; +use embassy_sync::blocking_mutex::raw::NoopRawMutex; +use embassy_time::{Duration, Timer}; +use static_cell::StaticCell; +use trouble_host::{ + adapter::{Adapter, HostResources}, + advertise::{AdStructure, AdvertiseConfig, BR_EDR_NOT_SUPPORTED, LE_GENERAL_DISCOVERABLE}, + attribute::{AttributeTable, CharacteristicProp, Service, Uuid}, + PacketQos, +}; + +use {defmt_rtt as _, panic_probe as _}; + +bind_interrupts!(struct Irqs { + UARTE0_UART0 => buffered_uarte::InterruptHandler; +}); + +#[embassy_executor::main] +async fn main(_s: Spawner) { + let p = embassy_nrf::init(Default::default()); + + let uart_tx = p.P0_01; + let uart_rx = p.P0_17; + let uart_cts = p.P0_13; + let uart_rts = p.P1_02; + + let mut config = uarte::Config::default(); + config.parity = uarte::Parity::EXCLUDED; + config.baudrate = uarte::Baudrate::BAUD115200; + + let mut tx_buffer = [0u8; 4096]; + let mut rx_buffer = [0u8; 4096]; + + let mut u = buffered_uarte::BufferedUarte::new_with_rtscts( + p.UARTE0, + p.TIMER0, + p.PPI_CH0, + p.PPI_CH1, + p.PPI_GROUP0, + Irqs, + uart_rx, + uart_tx, + uart_cts, + uart_rts, + config, + &mut rx_buffer, + &mut tx_buffer, + ); + + let (reader, writer) = u.split(); + + let controller: SerialController = SerialController::new(reader, writer); + static HOST_RESOURCES: StaticCell> = StaticCell::new(); + let host_resources = HOST_RESOURCES.init(HostResources::new(PacketQos::None)); + + let adapter: Adapter<'_, NoopRawMutex, _, 2, 4, 1, 1> = Adapter::new(controller, host_resources); + 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 table: AttributeTable<'_, NoopRawMutex, 10> = AttributeTable::new(); + + // Generic Access Service (mandatory) + let id = b"Trouble"; + let appearance = [0x80, 0x07]; + let mut bat_level = [0; 1]; + let handle = { + let mut svc = table.add_service(Service::new(0x1800)); + let _ = svc.add_characteristic_ro(0x2a00, id); + let _ = svc.add_characteristic_ro(0x2a01, &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( + 0x2a19, + &[CharacteristicProp::Read, CharacteristicProp::Notify], + &mut bat_level, + ) + }; + + let server = adapter.gatt_server(&table); + + info!("Starting advertising and GATT service"); + let _ = join3( + adapter.run(), + async { + loop { + match server.next().await { + Ok(event) => { + info!("Gatt event: {:?}", event); + } + Err(e) => { + error!("Error processing GATT events: {:?}", e); + } + } + } + }, + async { + let conn = adapter.advertise(&config).await.unwrap(); + // Keep connection alive + let mut tick: u8 = 0; + loop { + Timer::after(Duration::from_secs(10)).await; + tick += 1; + server.notify(handle, &conn, &[tick]).await.unwrap(); + } + }, + ) + .await; +} diff --git a/examples/serial-hci/Cargo.toml b/examples/serial-hci/Cargo.toml index 3f14b3a..a801a57 100644 --- a/examples/serial-hci/Cargo.toml +++ b/examples/serial-hci/Cargo.toml @@ -9,23 +9,22 @@ env_logger = "0.10.0" log = "0.4" crossterm = "0.27.0" rand_core = { version = "0.6.4", features = ["std"] } -embassy-executor = { version = "0.5.0", features = ["task-arena-size-32768", "arch-std", "executor-thread", "log", "integrated-timers"] } -embedded-io-adapters = { version = "0.6.1", features = ["futures-03"] } +# embassy-executor = { version = "0.5.0", features = ["task-arena-size-32768", "arch-std", "executor-thread", "log", "integrated-timers"] } +embedded-io-adapters = { version = "0.6.1", features = ["tokio-1"] } embedded-io-async = { version = "0.6.1" } -embassy-time = { version = "0.3.0", features = ["log", "std", ] } embassy-sync = { version = "0.5.0", features = ["log"] } critical-section = { version = "1.1", features = ["std"] } embassy-futures = { version = "0.1" } nix = "0.26.2" -async-io = "1.6.0" static_cell = "2" -futures = { version = "0.3.17" } +tokio = { version = "1", features = ["full"] } +tokio-serial = "5.4" -bt-hci = { version = "0.1.0", default-features = false } #features = ["log"] } -trouble-host = { version = "0.1.0", path = "../../host" } #, features = ["log"] } +bt-hci = { version = "0.1.0", default-features = false, features = ["log"] } +trouble-host = { version = "0.1.0", path = "../../host", features = ["log"] } [patch.crates-io] -bt-hci = { git = "https://github.com/alexmoon/bt-hci.git", branch = "main" } +bt-hci = { git = "https://github.com/alexmoon/bt-hci.git", branch = "serial-controller" } embassy-sync = { git = "https://github.com/embassy-rs/embassy.git", branch = "main" } -embassy-time = { git = "https://github.com/embassy-rs/embassy.git", branch = "main" } -embassy-executor = { git = "https://github.com/embassy-rs/embassy.git", branch = "main" } +#embassy-executor = { git = "https://github.com/embassy-rs/embassy.git", branch = "main" } +#bt-hci = { path = "../../../bt-hci" } diff --git a/examples/serial-hci/src/main.rs b/examples/serial-hci/src/main.rs index 0ad1edc..557e692 100644 --- a/examples/serial-hci/src/main.rs +++ b/examples/serial-hci/src/main.rs @@ -1,9 +1,9 @@ // Use with any serial HCI -use async_io::Async; use bt_hci::cmd::AsyncCmd; use bt_hci::cmd::SyncCmd; use bt_hci::data; use bt_hci::param; +use bt_hci::serial::SerialController; use bt_hci::Controller; use bt_hci::ControllerCmdAsync; use bt_hci::ControllerCmdSync; @@ -13,44 +13,32 @@ use bt_hci::WithIndicator; use bt_hci::WriteHci; use core::future::Future; use core::ops::DerefMut; -use embassy_executor::Executor; use embassy_futures::join::join3; use embassy_sync::blocking_mutex::raw::NoopRawMutex; use embassy_sync::mutex::Mutex; -use embassy_time as _; -use embassy_time::{Duration, Timer}; use embedded_io_async::Read; use log::*; use nix::sys::termios; use static_cell::StaticCell; +use tokio::io::AsyncReadExt; +use tokio::time::Duration; +use tokio_serial::SerialStream; +use tokio_serial::{DataBits, Parity, StopBits}; use trouble_host::{ adapter::{Adapter, HostResources}, advertise::{AdStructure, AdvertiseConfig, BR_EDR_NOT_SUPPORTED, LE_GENERAL_DISCOVERABLE}, - attribute::{AttributeTable, Characteristic, CharacteristicProp, Service, Uuid}, + attribute::{AttributeTable, CharacteristicProp, Service, Uuid}, PacketQos, }; -mod serial_port; -use self::serial_port::SerialPort; - -static EXECUTOR: StaticCell = StaticCell::new(); - -fn main() { +#[tokio::main] +async fn main() { env_logger::builder() - .filter_level(log::LevelFilter::Debug) - .filter_module("async_io", log::LevelFilter::Info) + .filter_level(log::LevelFilter::Trace) .format_timestamp_nanos() .init(); - let executor = EXECUTOR.init(Executor::new()); - executor.run(|spawner| { - spawner.spawn(run()).unwrap(); - }); -} - -#[embassy_executor::task] -async fn run() { - let baudrate = termios::BaudRate::B115200; + let baudrate = 1000000; if std::env::args().len() != 2 { println!("Provide the serial port as the one and only command line argument."); @@ -59,28 +47,32 @@ async fn run() { let args: Vec = std::env::args().collect(); - let port = SerialPort::new(args[1].as_str(), baudrate).unwrap(); - let port = Async::new(port).unwrap(); - let mut port = embedded_io_adapters::futures_03::FromFutures::new(port); - - println!("Reset the target"); - let mut buffer = [0u8; 1]; + let mut port = SerialStream::open( + &tokio_serial::new(args[1].as_str(), baudrate) + .baud_rate(baudrate) + .data_bits(DataBits::Eight) + .parity(Parity::None) + .stop_bits(StopBits::One), + ) + .unwrap(); + // Drain input + tokio::time::sleep(Duration::from_secs(1)).await; loop { - match port.read(&mut buffer).await { - Ok(_len) => { - if buffer[0] == 0xff { - break; - } - } - Err(_) => (), + let mut buf = [0; 1]; + match port.try_read(&mut buf[..]) { + Err(e) if e.kind() == std::io::ErrorKind::WouldBlock => break, + _ => {} } } + info!("Ready!"); + + let (reader, writer) = tokio::io::split(port); - println!("Connected"); - println!("Q to exit, N to notify, X force disconnect"); + let mut reader = embedded_io_adapters::tokio_1::FromTokio::new(reader); + let mut writer = embedded_io_adapters::tokio_1::FromTokio::new(writer); - let controller = SerialController::new(port); + let controller: SerialController = SerialController::new(reader, writer); static HOST_RESOURCES: StaticCell> = StaticCell::new(); let host_resources = HOST_RESOURCES.init(HostResources::new(PacketQos::None)); @@ -90,24 +82,20 @@ async fn run() { data: &[ AdStructure::Flags(LE_GENERAL_DISCOVERABLE | BR_EDR_NOT_SUPPORTED), AdStructure::ServiceUuids16(&[Uuid::Uuid16([0x0f, 0x18])]), - AdStructure::CompleteLocalName("Trouble"), + AdStructure::CompleteLocalName("Trouble HCI"), ], }; 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 id = b"Trouble HCI"; + let 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[..], - )); + let _ = svc.add_characteristic_ro(0x2a00, id); + let _ = svc.add_characteristic_ro(0x2a01, &appearance[..]); drop(svc); // Generic attribute service (mandatory) @@ -116,11 +104,11 @@ async fn run() { // Battery service let mut svc = table.add_service(Service::new(0x180f)); - svc.add_characteristic(Characteristic::new( + svc.add_characteristic( 0x2a19, &[CharacteristicProp::Read, CharacteristicProp::Notify], &mut bat_level, - )) + ) }; let server = adapter.gatt_server(&table); @@ -145,7 +133,7 @@ async fn run() { // Keep connection alive let mut tick: u8 = 0; loop { - Timer::after(Duration::from_secs(10)).await; + tokio::time::sleep(Duration::from_secs(10)).await; tick += 1; server.notify(handle, &conn, &[tick]).await.unwrap(); } @@ -153,98 +141,3 @@ async fn run() { ) .await; } - -pub struct SerialController -where - T: embedded_io_async::Read + embedded_io_async::Write, -{ - io: Mutex, -} - -impl SerialController -where - T: embedded_io_async::Read + embedded_io_async::Write, -{ - pub fn new(io: T) -> Self { - Self { io: Mutex::new(io) } - } -} - -impl Controller for SerialController -where - T: embedded_io_async::Read + embedded_io_async::Write, -{ - type Error = T::Error; - fn write_acl_data(&self, packet: &data::AclPacket) -> impl Future> { - async { - let mut io = self.io.lock().await; - WithIndicator::new(packet) - .write_hci_async(io.deref_mut()) - .await - .unwrap(); - Ok(()) - } - } - - fn write_sync_data(&self, packet: &data::SyncPacket) -> impl Future> { - async { - let mut io = self.io.lock().await; - WithIndicator::new(packet) - .write_hci_async(io.deref_mut()) - .await - .unwrap(); - Ok(()) - } - } - - fn write_iso_data(&self, packet: &data::IsoPacket) -> impl Future> { - async { - let mut io = self.io.lock().await; - WithIndicator::new(packet) - .write_hci_async(io.deref_mut()) - .await - .unwrap(); - Ok(()) - } - } - - fn read<'a>(&self, buf: &'a mut [u8]) -> impl Future, Self::Error>> { - async { - let mut io = self.io.lock().await; - let value = ControllerToHostPacket::read_hci_async(io.deref_mut(), buf) - .await - .unwrap(); - Ok(value) - } - } -} - -impl ControllerCmdSync for SerialController -where - T: embedded_io_async::Read + embedded_io_async::Write, - C: SyncCmd, - C::Return: bt_hci::FixedSizeValue, -{ - fn exec(&self, cmd: &C) -> impl Future> { - async { - let mut buf = [0; 512]; - let mut io = self.io.lock().await; - WithIndicator::new(cmd).write_hci_async(io.deref_mut()).await.unwrap(); - let value = C::Return::read_hci_async(io.deref_mut(), &mut buf[..]).await.unwrap(); - Ok(value) - } - } -} - -impl ControllerCmdAsync for SerialController -where - T: embedded_io_async::Read + embedded_io_async::Write, - C: AsyncCmd, -{ - fn exec(&self, cmd: &C) -> impl Future> { - async { - let mut io = self.io.lock().await; - Ok(WithIndicator::new(cmd).write_hci_async(io.deref_mut()).await.unwrap()) - } - } -} diff --git a/examples/serial-hci/src/serial_port.rs b/examples/serial-hci/src/serial_port.rs deleted file mode 100644 index c41abd4..0000000 --- a/examples/serial-hci/src/serial_port.rs +++ /dev/null @@ -1,66 +0,0 @@ -use std::io; -use std::os::unix::io::{AsRawFd, RawFd}; - -use nix::errno::Errno; -use nix::fcntl::OFlag; -use nix::sys::termios; - -pub struct SerialPort { - fd: RawFd, -} - -impl SerialPort { - pub fn new(path: &P, baudrate: termios::BaudRate) -> io::Result { - let fd = nix::fcntl::open( - path, - OFlag::O_RDWR | OFlag::O_NOCTTY | OFlag::O_NONBLOCK, - nix::sys::stat::Mode::empty(), - ) - .map_err(to_io_error)?; - - let mut cfg = termios::tcgetattr(fd).map_err(to_io_error)?; - cfg.input_flags = termios::InputFlags::empty(); - cfg.output_flags = termios::OutputFlags::empty(); - cfg.control_flags = termios::ControlFlags::empty(); - cfg.local_flags = termios::LocalFlags::empty(); - termios::cfmakeraw(&mut cfg); - cfg.input_flags |= termios::InputFlags::IGNBRK; - cfg.control_flags |= termios::ControlFlags::CREAD; - //cfg.control_flags |= termios::ControlFlags::CRTSCTS; - termios::cfsetospeed(&mut cfg, baudrate).map_err(to_io_error)?; - termios::cfsetispeed(&mut cfg, baudrate).map_err(to_io_error)?; - termios::cfsetspeed(&mut cfg, baudrate).map_err(to_io_error)?; - // Set VMIN = 1 to block until at least one character is received. - cfg.control_chars[termios::SpecialCharacterIndices::VMIN as usize] = 1; - termios::tcsetattr(fd, termios::SetArg::TCSANOW, &cfg).map_err(to_io_error)?; - termios::tcflush(fd, termios::FlushArg::TCIOFLUSH).map_err(to_io_error)?; - - Ok(Self { fd }) - } -} - -impl AsRawFd for SerialPort { - fn as_raw_fd(&self) -> RawFd { - self.fd - } -} - -impl io::Read for SerialPort { - fn read(&mut self, buf: &mut [u8]) -> io::Result { - nix::unistd::read(self.fd, buf).map_err(to_io_error) - } -} - -impl io::Write for SerialPort { - fn write(&mut self, buf: &[u8]) -> io::Result { - nix::unistd::write(self.fd, buf).map_err(to_io_error) - } - - fn flush(&mut self) -> io::Result<()> { - Ok(()) - } -} - -fn to_io_error(e: Errno) -> io::Error { - e.into() -} diff --git a/host/src/adapter.rs b/host/src/adapter.rs index 66d3375..719734c 100644 --- a/host/src/adapter.rs +++ b/host/src/adapter.rs @@ -65,6 +65,7 @@ pub struct Adapter< } pub(crate) enum ControlCommand { + Init, Disconnect(DisconnectParams), Connect(LeCreateConnParams), } @@ -236,94 +237,16 @@ where + ControllerCmdAsync + ControllerCmdSync, { - SetEventMask::new( - EventMask::new() - .enable_le_meta(true) - .enable_conn_request(true) - .enable_conn_complete(true) - .enable_hardware_error(true) - .enable_disconnection_complete(true), - ) - .exec(&self.controller) - .await?; + self.control.send(ControlCommand::Init).await; loop { let mut rx = [0u8; 259]; let mut tx = [0u8; 259]; - // info!("Entering select"); - match select4( + // info!("Entering select loop"); + let result: Result<(), Error> = match select4( self.controller.read(&mut rx), - self.outbound.receive(), - self.control.receive(), - self.channels.signal(), - ) - .await - { - 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(), - att_mtu: 23, - }, - ) { - warn!("Error establishing connection: {:?}", err); - Disconnect::new( - e.handle, - DisconnectReason::RemoteDeviceTerminatedConnLowResources, - ) - .exec(&self.controller) - .await - .unwrap(); - } - } - LeEvent::LeAdvertisingReport(data) => { - self.scanner - .send(ScanReport::new(data.reports.num_reports, &data.reports.bytes)) - .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); - } - }, - Ok(p) => { - info!("Ignoring packet: {:?}", p); - } - Err(e) => { - info!("Error from controller: {:?}", e); - } - } - } - Either4::Second((handle, pdu)) => { - // info!("Outgoing packet"); + async { + let (handle, pdu) = self.outbound.receive().await; let acl = AclPacket::new(handle, pdu.pb, AclBroadcastFlag::PointToPoint, pdu.as_ref()); match self.controller.write_acl_data(&acl).await { Ok(_) => {} @@ -332,9 +255,10 @@ where panic!(":("); } } - } - Either4::Third(command) => { - // info!("Outgoing command"); + Ok(()) + }, + async { + let command = self.control.receive().await; match command { ControlCommand::Connect(params) => { LeSetScanEnable::new(false, false).exec(&self.controller).await.unwrap(); @@ -363,9 +287,24 @@ where .await .unwrap(); } + ControlCommand::Init => { + SetEventMask::new( + EventMask::new() + .enable_le_meta(true) + .enable_conn_request(true) + .enable_conn_complete(true) + .enable_hardware_error(true) + .enable_disconnection_complete(true), + ) + .exec(&self.controller) + .await + .unwrap(); + } } - } - Either4::Fourth((handle, response)) => { + Ok(()) + }, + async { + let (handle, response) = self.channels.signal().await; // info!("Outgoing signal: {:?}", response); let mut w = WriteCursor::new(&mut tx); let (mut header, mut body) = w.split(4)?; @@ -394,8 +333,80 @@ where panic!(":("); } } + Ok(()) + }, + ) + .await + { + Either4::First(result) => { + // info!("Incoming event: {:?}", 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(), + att_mtu: 23, + }, + ) { + warn!("Error establishing connection: {:?}", err); + Disconnect::new( + e.handle, + DisconnectReason::RemoteDeviceTerminatedConnLowResources, + ) + .exec(&self.controller) + .await + .unwrap(); + } + } + LeEvent::LeAdvertisingReport(data) => { + self.scanner + .send(ScanReport::new(data.reports.num_reports, &data.reports.bytes)) + .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); + } + }, + Ok(p) => { + info!("Ignoring packet: {:?}", p); + } + Err(e) => { + info!("Error from controller: {:?}", e); + } + } + Ok(()) } - } + Either4::Second(result) => result, + Either4::Third(result) => result, + Either4::Fourth(result) => result, + }; + result?; } } } diff --git a/host/src/channel_manager.rs b/host/src/channel_manager.rs index 9863ed5..4c52e90 100644 --- a/host/src/channel_manager.rs +++ b/host/src/channel_manager.rs @@ -377,7 +377,7 @@ impl<'d, M: RawMutex, const CHANNELS: usize, const L2CAP_TXQ: usize, const L2CAP Ok(()) } L2capLeSignalData::CommandRejectRes(reject) => { - warn!("Rejected: {:02x}", reject); + warn!("Rejected: {:?}", reject); Ok(()) } L2capLeSignalData::DisconnectionReq(req) => { diff --git a/host/src/gatt.rs b/host/src/gatt.rs index 35e6695..db33c19 100644 --- a/host/src/gatt.rs +++ b/host/src/gatt.rs @@ -72,7 +72,7 @@ impl<'reference, 'values, 'resources, M: RawMutex, const MAX: usize> } } Err(e) => { - warn!("Error decoding attribute request: {:02x}", e); + warn!("Error decoding attribute request: {:?}", e); } } } diff --git a/host/src/lib.rs b/host/src/lib.rs index 141225d..88c6ea7 100644 --- a/host/src/lib.rs +++ b/host/src/lib.rs @@ -47,6 +47,16 @@ impl From for Error { } } +impl From> for Error { + fn from(error: bt_hci::CmdError) -> Self { + match error { + bt_hci::CmdError::Param(p) => Self::Encode(p), + bt_hci::CmdError::Controller(p) => Self::Controller(p), + + } + } +} + impl From for Error { fn from(error: bt_hci::param::Error) -> Self { Self::Encode(error) diff --git a/host/src/types/l2cap.rs b/host/src/types/l2cap.rs index f9507f7..9c76d5d 100644 --- a/host/src/types/l2cap.rs +++ b/host/src/types/l2cap.rs @@ -139,7 +139,7 @@ impl L2capLeSignalData { Self::DisconnectionRes(res) } code => { - warn!("Unimplemented signal code: {:02x}", code); + warn!("Unimplemented signal code: {:?}", code); panic!(); } }) From 8102fa20536a87dd2837bc16858f68c7f92f9e39 Mon Sep 17 00:00:00 2001 From: Ulf Lilleengen Date: Wed, 20 Mar 2024 23:34:38 +0100 Subject: [PATCH 2/4] Update and remove embedded-io-hci example for now --- examples/embedded-io-hci/.cargo/config.toml | 12 -- examples/embedded-io-hci/Cargo.toml | 41 ------ examples/embedded-io-hci/build.rs | 35 ------ examples/embedded-io-hci/memory.x | 7 -- examples/embedded-io-hci/src/main.rs | 132 -------------------- examples/nrf-sdc/Cargo.toml | 2 +- examples/serial-hci/src/main.rs | 27 +--- host/Cargo.toml | 2 +- host/src/att.rs | 1 - rust-toolchain.toml | 4 +- 10 files changed, 9 insertions(+), 254 deletions(-) delete mode 100644 examples/embedded-io-hci/.cargo/config.toml delete mode 100644 examples/embedded-io-hci/Cargo.toml delete mode 100644 examples/embedded-io-hci/build.rs delete mode 100644 examples/embedded-io-hci/memory.x delete mode 100644 examples/embedded-io-hci/src/main.rs diff --git a/examples/embedded-io-hci/.cargo/config.toml b/examples/embedded-io-hci/.cargo/config.toml deleted file mode 100644 index 581c1e5..0000000 --- a/examples/embedded-io-hci/.cargo/config.toml +++ /dev/null @@ -1,12 +0,0 @@ -[target.'cfg(all(target_arch = "arm", target_os = "none"))'] -runner = "probe-rs run --chip nRF52833_xxAA" - -[build] -# Pick ONE of these compilation targets -# target = "thumbv6m-none-eabi" # Cortex-M0 and Cortex-M0+ -# target = "thumbv7m-none-eabi" # Cortex-M3 -# target = "thumbv7em-none-eabi" # Cortex-M4 and Cortex-M7 (no FPU) -target = "thumbv7em-none-eabihf" # Cortex-M4F and Cortex-M7F (with FPU) - -[env] -DEFMT_LOG = "info" diff --git a/examples/embedded-io-hci/Cargo.toml b/examples/embedded-io-hci/Cargo.toml deleted file mode 100644 index e420b50..0000000 --- a/examples/embedded-io-hci/Cargo.toml +++ /dev/null @@ -1,41 +0,0 @@ -[package] -name = "serial-hci" -version = "0.1.0" -edition = "2021" - -[dependencies] -embassy-executor = { version = "0.5", default-features = false, features = ["nightly", "arch-cortex-m", "executor-thread", "defmt", "integrated-timers", "executor-interrupt"] } -embassy-time = { version = "0.3.0", default-features = false, features = ["defmt", "defmt-timestamp-uptime"] } -embassy-nrf = { version = "0.1.0", default-features = false, features = ["defmt", "nrf52833", "time-driver-rtc1", "gpiote", "unstable-pac", "rt"] } -embedded-io-async = { version = "0.6.1", features = ["defmt-03"] } -embedded-io = { version = "0.6.1", features = ["defmt-03"] } -embassy-sync = { version = "0.5.0", features = ["defmt"] } -embassy-futures = "0.1.1" -cortex-m = { version = "0.7.6", features = ["critical-section-single-core"] } -cortex-m-rt = "0.7.0" -static_cell = "2" -defmt = "0.3" -defmt-rtt = "0.4.0" - -panic-probe = { version = "0.3", features = ["print-defmt"] } - -bt-hci = { version = "0.1.0", default-features = false, features = ["defmt"] } -trouble-host = { version = "0.1.0", path = "../../host", features = ["defmt"] } - -[patch.crates-io] -bt-hci = { git = "https://github.com/alexmoon/bt-hci.git", branch = "serial-controller" } -embassy-executor = { git = "https://github.com/embassy-rs/embassy.git", branch = "main" } -embassy-time = { git = "https://github.com/embassy-rs/embassy.git", branch = "main" } -embassy-nrf = { git = "https://github.com/embassy-rs/embassy.git", branch = "main" } -embassy-sync = { git = "https://github.com/embassy-rs/embassy.git", branch = "main" } -embassy-futures = { git = "https://github.com/embassy-rs/embassy.git", branch = "main" } -# -#embassy-executor = { path = "../../../embassy/embassy-executor" } -#embassy-time = { path = "../../../embassy/embassy-time"} -#embassy-nrf = { path = "../../../embassy/embassy-nrf"} -#embassy-sync = { path = "../../../embassy/embassy-sync"} -#embassy-futures = { path = "../../../embassy/embassy-futures"} -#bt-hci = { path = "../../../bt-hci" } - -[profile.release] -debug = 2 diff --git a/examples/embedded-io-hci/build.rs b/examples/embedded-io-hci/build.rs deleted file mode 100644 index 30691aa..0000000 --- a/examples/embedded-io-hci/build.rs +++ /dev/null @@ -1,35 +0,0 @@ -//! This build script copies the `memory.x` file from the crate root into -//! a directory where the linker can always find it at build time. -//! For many projects this is optional, as the linker always searches the -//! project root directory -- wherever `Cargo.toml` is. However, if you -//! are using a workspace or have a more complicated build setup, this -//! build script becomes required. Additionally, by requesting that -//! Cargo re-run the build script whenever `memory.x` is changed, -//! updating `memory.x` ensures a rebuild of the application with the -//! new memory settings. - -use std::env; -use std::fs::File; -use std::io::Write; -use std::path::PathBuf; - -fn main() { - // Put `memory.x` in our output directory and ensure it's - // on the linker search path. - let out = &PathBuf::from(env::var_os("OUT_DIR").unwrap()); - File::create(out.join("memory.x")) - .unwrap() - .write_all(include_bytes!("memory.x")) - .unwrap(); - println!("cargo:rustc-link-search={}", out.display()); - - // By default, Cargo will re-run a build script whenever - // any file in the project changes. By specifying `memory.x` - // here, we ensure the build script is only re-run when - // `memory.x` is changed. - println!("cargo:rerun-if-changed=memory.x"); - - println!("cargo:rustc-link-arg-bins=--nmagic"); - println!("cargo:rustc-link-arg-bins=-Tlink.x"); - println!("cargo:rustc-link-arg-bins=-Tdefmt.x"); -} diff --git a/examples/embedded-io-hci/memory.x b/examples/embedded-io-hci/memory.x deleted file mode 100644 index a136848..0000000 --- a/examples/embedded-io-hci/memory.x +++ /dev/null @@ -1,7 +0,0 @@ -MEMORY -{ - /* NOTE 1 K = 1 KiBi = 1024 bytes */ - /* These values correspond to the NRF52840 */ - FLASH : ORIGIN = 0x00000000, LENGTH = 512K - RAM : ORIGIN = 0x20000000, LENGTH = 128K -} diff --git a/examples/embedded-io-hci/src/main.rs b/examples/embedded-io-hci/src/main.rs deleted file mode 100644 index 8bf2999..0000000 --- a/examples/embedded-io-hci/src/main.rs +++ /dev/null @@ -1,132 +0,0 @@ -#![no_std] -#![no_main] -#![feature(type_alias_impl_trait)] - -use bt_hci::cmd::SyncCmd; -use bt_hci::param::BdAddr; -use bt_hci::serial::SerialController; -use defmt::{error, info, unwrap}; -use embassy_executor::Spawner; -use embassy_futures::join::join3; -use embassy_nrf::peripherals; -use embassy_nrf::{bind_interrupts, pac}; -use embassy_nrf::{buffered_uarte, uarte}; -use embassy_sync::blocking_mutex::raw::NoopRawMutex; -use embassy_time::{Duration, Timer}; -use static_cell::StaticCell; -use trouble_host::{ - adapter::{Adapter, HostResources}, - advertise::{AdStructure, AdvertiseConfig, BR_EDR_NOT_SUPPORTED, LE_GENERAL_DISCOVERABLE}, - attribute::{AttributeTable, CharacteristicProp, Service, Uuid}, - PacketQos, -}; - -use {defmt_rtt as _, panic_probe as _}; - -bind_interrupts!(struct Irqs { - UARTE0_UART0 => buffered_uarte::InterruptHandler; -}); - -#[embassy_executor::main] -async fn main(_s: Spawner) { - let p = embassy_nrf::init(Default::default()); - - let uart_tx = p.P0_01; - let uart_rx = p.P0_17; - let uart_cts = p.P0_13; - let uart_rts = p.P1_02; - - let mut config = uarte::Config::default(); - config.parity = uarte::Parity::EXCLUDED; - config.baudrate = uarte::Baudrate::BAUD115200; - - let mut tx_buffer = [0u8; 4096]; - let mut rx_buffer = [0u8; 4096]; - - let mut u = buffered_uarte::BufferedUarte::new_with_rtscts( - p.UARTE0, - p.TIMER0, - p.PPI_CH0, - p.PPI_CH1, - p.PPI_GROUP0, - Irqs, - uart_rx, - uart_tx, - uart_cts, - uart_rts, - config, - &mut rx_buffer, - &mut tx_buffer, - ); - - let (reader, writer) = u.split(); - - let controller: SerialController = SerialController::new(reader, writer); - static HOST_RESOURCES: StaticCell> = StaticCell::new(); - let host_resources = HOST_RESOURCES.init(HostResources::new(PacketQos::None)); - - let adapter: Adapter<'_, NoopRawMutex, _, 2, 4, 1, 1> = Adapter::new(controller, host_resources); - 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 table: AttributeTable<'_, NoopRawMutex, 10> = AttributeTable::new(); - - // Generic Access Service (mandatory) - let id = b"Trouble"; - let appearance = [0x80, 0x07]; - let mut bat_level = [0; 1]; - let handle = { - let mut svc = table.add_service(Service::new(0x1800)); - let _ = svc.add_characteristic_ro(0x2a00, id); - let _ = svc.add_characteristic_ro(0x2a01, &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( - 0x2a19, - &[CharacteristicProp::Read, CharacteristicProp::Notify], - &mut bat_level, - ) - }; - - let server = adapter.gatt_server(&table); - - info!("Starting advertising and GATT service"); - let _ = join3( - adapter.run(), - async { - loop { - match server.next().await { - Ok(event) => { - info!("Gatt event: {:?}", event); - } - Err(e) => { - error!("Error processing GATT events: {:?}", e); - } - } - } - }, - async { - let conn = adapter.advertise(&config).await.unwrap(); - // Keep connection alive - let mut tick: u8 = 0; - loop { - Timer::after(Duration::from_secs(10)).await; - tick += 1; - server.notify(handle, &conn, &[tick]).await.unwrap(); - } - }, - ) - .await; -} diff --git a/examples/nrf-sdc/Cargo.toml b/examples/nrf-sdc/Cargo.toml index bd03fc0..f7fd0e7 100644 --- a/examples/nrf-sdc/Cargo.toml +++ b/examples/nrf-sdc/Cargo.toml @@ -39,7 +39,7 @@ embassy-time-driver = { git = "https://github.com/embassy-rs/embassy.git", branc embassy-embedded-hal = { git = "https://github.com/embassy-rs/embassy.git", branch = "main" } nrf-sdc = { git = "https://github.com/alexmoon/nrf-sdc.git", branch = "main" } nrf-mpsl = { git = "https://github.com/alexmoon/nrf-sdc.git", branch = "main" } -bt-hci = { git = "https://github.com/alexmoon/bt-hci.git", branch = "main" } +bt-hci = { git = "https://github.com/alexmoon/bt-hci.git", branch = "serial-controller" } #embassy-executor = {path = "../../../embassy/embassy-executor"} #embassy-nrf = {path = "../../../embassy/embassy-nrf"} diff --git a/examples/serial-hci/src/main.rs b/examples/serial-hci/src/main.rs index 557e692..f686751 100644 --- a/examples/serial-hci/src/main.rs +++ b/examples/serial-hci/src/main.rs @@ -1,26 +1,10 @@ // Use with any serial HCI -use bt_hci::cmd::AsyncCmd; -use bt_hci::cmd::SyncCmd; -use bt_hci::data; -use bt_hci::param; -use bt_hci::serial::SerialController; -use bt_hci::Controller; -use bt_hci::ControllerCmdAsync; -use bt_hci::ControllerCmdSync; -use bt_hci::ControllerToHostPacket; -use bt_hci::ReadHci; -use bt_hci::WithIndicator; -use bt_hci::WriteHci; -use core::future::Future; -use core::ops::DerefMut; +use bt_hci::driver::HciController; +use bt_hci::serial::SerialHciDriver; use embassy_futures::join::join3; use embassy_sync::blocking_mutex::raw::NoopRawMutex; -use embassy_sync::mutex::Mutex; -use embedded_io_async::Read; use log::*; -use nix::sys::termios; use static_cell::StaticCell; -use tokio::io::AsyncReadExt; use tokio::time::Duration; use tokio_serial::SerialStream; use tokio_serial::{DataBits, Parity, StopBits}; @@ -69,10 +53,11 @@ async fn main() { let (reader, writer) = tokio::io::split(port); - let mut reader = embedded_io_adapters::tokio_1::FromTokio::new(reader); - let mut writer = embedded_io_adapters::tokio_1::FromTokio::new(writer); + let reader = embedded_io_adapters::tokio_1::FromTokio::new(reader); + let writer = embedded_io_adapters::tokio_1::FromTokio::new(writer); - let controller: SerialController = SerialController::new(reader, writer); + let driver: SerialHciDriver = SerialHciDriver::new(reader, writer); + let controller: HciController<_, 10> = HciController::new(driver); static HOST_RESOURCES: StaticCell> = StaticCell::new(); let host_resources = HOST_RESOURCES.init(HostResources::new(PacketQos::None)); diff --git a/host/Cargo.toml b/host/Cargo.toml index 94af1fa..ee405bf 100644 --- a/host/Cargo.toml +++ b/host/Cargo.toml @@ -32,5 +32,5 @@ defmt = {version = "0.3", optional = true } defmt = [ "dep:defmt" ] [patch.crates-io] -bt-hci = { git = "https://github.com/alexmoon/bt-hci.git", branch = "main" } +bt-hci = { git = "https://github.com/alexmoon/bt-hci.git", branch = "serial-controller" } embassy-sync = { git = "https://github.com/embassy-rs/embassy.git", branch = "main" } diff --git a/host/src/att.rs b/host/src/att.rs index 82a815d..d49719c 100644 --- a/host/src/att.rs +++ b/host/src/att.rs @@ -1,7 +1,6 @@ use crate::codec; use crate::cursor::ReadCursor; use crate::types::uuid::*; -use core::convert::TryInto; pub const ATT_READ_BY_GROUP_TYPE_REQUEST_OPCODE: u8 = 0x10; pub const ATT_READ_BY_GROUP_TYPE_RESPONSE_OPCODE: u8 = 0x11; diff --git a/rust-toolchain.toml b/rust-toolchain.toml index 7526fb1..dc95fd6 100644 --- a/rust-toolchain.toml +++ b/rust-toolchain.toml @@ -1,10 +1,8 @@ # Before upgrading check that everything is available on all tier1 targets here: # https://rust-lang.github.io/rustup-components-history [toolchain] -channel = "nightly-2024-02-18" +channel = "nightly-2024-03-20" components = [ "rust-src", "rustfmt", "llvm-tools-preview" ] targets = [ "thumbv7em-none-eabi", - "thumbv7em-none-eabihf", - "thumbv8m.main-none-eabihf", ] From f64ee675c15d668a8abfe25962836de73eac10af Mon Sep 17 00:00:00 2001 From: Ulf Lilleengen Date: Wed, 20 Mar 2024 23:59:20 +0100 Subject: [PATCH 3/4] Add pico w example --- examples/rp-pico-w/.cargo/config.toml | 8 ++ examples/rp-pico-w/Cargo.toml | 59 +++++++++ examples/rp-pico-w/build.rs | 36 ++++++ examples/rp-pico-w/memory.x | 17 +++ examples/rp-pico-w/src/main.rs | 167 ++++++++++++++++++++++++++ host/src/adapter.rs | 6 + rust-toolchain.toml | 1 + 7 files changed, 294 insertions(+) create mode 100644 examples/rp-pico-w/.cargo/config.toml create mode 100644 examples/rp-pico-w/Cargo.toml create mode 100644 examples/rp-pico-w/build.rs create mode 100644 examples/rp-pico-w/memory.x create mode 100644 examples/rp-pico-w/src/main.rs diff --git a/examples/rp-pico-w/.cargo/config.toml b/examples/rp-pico-w/.cargo/config.toml new file mode 100644 index 0000000..3d7d617 --- /dev/null +++ b/examples/rp-pico-w/.cargo/config.toml @@ -0,0 +1,8 @@ +[target.'cfg(all(target_arch = "arm", target_os = "none"))'] +runner = "probe-rs run --chip RP2040" + +[build] +target = "thumbv6m-none-eabi" # Cortex-M0 and Cortex-M0+ + +[env] +DEFMT_LOG = "debug" diff --git a/examples/rp-pico-w/Cargo.toml b/examples/rp-pico-w/Cargo.toml new file mode 100644 index 0000000..6b73bf7 --- /dev/null +++ b/examples/rp-pico-w/Cargo.toml @@ -0,0 +1,59 @@ +[package] +name = "rp-pico-w" +version = "0.1.0" +edition = "2021" + +[dependencies] +bt-hci = { version = "0.1.0", default-features = false, features = ["defmt"] } +trouble-host = { version = "0.1.0", path = "../../host", features = ["defmt"] } + +embassy-embedded-hal = { version = "0.1.0", features = ["defmt"] } +embassy-sync = { version = "0.5.0", features = ["defmt"] } +embassy-executor = { version = "0.5.0", features = ["arch-cortex-m", "executor-thread", "executor-interrupt", "defmt", "integrated-timers"] } +embassy-time = { version = "0.3.0", features = ["defmt-timestamp-uptime"] } +embassy-rp = { version = "0.1.0", features = ["defmt", "unstable-pac", "time-driver", "critical-section-impl"] } +embassy-futures = { version = "0.1.0" } +cyw43 = { version = "0.1.0", features = ["firmware-logs"] } +cyw43-pio = { version = "0.1.0", features = ["overclock"] } + +defmt = "0.3" +defmt-rtt = "0.4" +fixed = "1.23.1" +fixed-macro = "1.2" + +#cortex-m = { version = "0.7.6", features = ["critical-section-single-core"] } +cortex-m = { version = "0.7.6", features = ["inline-asm"] } +cortex-m-rt = "0.7.0" +panic-probe = { version = "0.3", features = ["print-defmt"] } +futures = { version = "0.3.17", default-features = false, features = ["async-await", "cfg-target-has-atomic", "unstable"] } +byte-slice-cast = { version = "1.2.0", default-features = false } +heapless = "0.8" +usbd-hid = "0.7.0" + +embedded-hal-1 = { package = "embedded-hal", version = "1.0" } +embedded-hal-async = "1.0" +embedded-hal-bus = { version = "0.1", features = ["async"] } +embedded-io-async = { version = "0.6.1", features = ["defmt-03"] } +embedded-io = { version = "0.6.1", features = ["defmt-03"] } +embedded-storage = { version = "0.3" } +static_cell = "2" +portable-atomic = { version = "1.5", features = ["critical-section"] } +pio-proc = "0.2" +pio = "0.2.1" +rand = { version = "0.8.5", default-features = false } + +[profile.release] +debug = 2 + +[patch.crates-io] +#bt-hci = { git = "https://github.com/alexmoon/bt-hci.git", branch = "serial-controller" } +embassy-sync = { git = "https://github.com/embassy-rs/embassy.git", branch = "pico-bluetooth" } +embassy-embedded-hal = { git = "https://github.com/embassy-rs/embassy.git", branch = "pico-bluetooth" } +embassy-executor = { git = "https://github.com/embassy-rs/embassy.git", branch = "pico-bluetooth" } +embassy-time = { git = "https://github.com/embassy-rs/embassy.git", branch = "pico-bluetooth" } +embassy-rp = { git = "https://github.com/embassy-rs/embassy.git", branch = "pico-bluetooth" } +embassy-futures = { git = "https://github.com/embassy-rs/embassy.git", branch = "pico-bluetooth" } +cyw43 = { git = "https://github.com/embassy-rs/embassy.git", branch = "pico-bluetooth" } +cyw43-pio = { git = "https://github.com/embassy-rs/embassy.git", branch = "pico-bluetooth" } +#embassy-executor = { git = "https://github.com/embassy-rs/embassy.git", branch = "main" } +bt-hci = { path = "../../../bt-hci" } diff --git a/examples/rp-pico-w/build.rs b/examples/rp-pico-w/build.rs new file mode 100644 index 0000000..3f915f9 --- /dev/null +++ b/examples/rp-pico-w/build.rs @@ -0,0 +1,36 @@ +//! This build script copies the `memory.x` file from the crate root into +//! a directory where the linker can always find it at build time. +//! For many projects this is optional, as the linker always searches the +//! project root directory -- wherever `Cargo.toml` is. However, if you +//! are using a workspace or have a more complicated build setup, this +//! build script becomes required. Additionally, by requesting that +//! Cargo re-run the build script whenever `memory.x` is changed, +//! updating `memory.x` ensures a rebuild of the application with the +//! new memory settings. + +use std::env; +use std::fs::File; +use std::io::Write; +use std::path::PathBuf; + +fn main() { + // Put `memory.x` in our output directory and ensure it's + // on the linker search path. + let out = &PathBuf::from(env::var_os("OUT_DIR").unwrap()); + File::create(out.join("memory.x")) + .unwrap() + .write_all(include_bytes!("memory.x")) + .unwrap(); + println!("cargo:rustc-link-search={}", out.display()); + + // By default, Cargo will re-run a build script whenever + // any file in the project changes. By specifying `memory.x` + // here, we ensure the build script is only re-run when + // `memory.x` is changed. + println!("cargo:rerun-if-changed=memory.x"); + + println!("cargo:rustc-link-arg-bins=--nmagic"); + println!("cargo:rustc-link-arg-bins=-Tlink.x"); + println!("cargo:rustc-link-arg-bins=-Tlink-rp.x"); + println!("cargo:rustc-link-arg-bins=-Tdefmt.x"); +} diff --git a/examples/rp-pico-w/memory.x b/examples/rp-pico-w/memory.x new file mode 100644 index 0000000..ef19dff --- /dev/null +++ b/examples/rp-pico-w/memory.x @@ -0,0 +1,17 @@ +MEMORY { + BOOT2 : ORIGIN = 0x10000000, LENGTH = 0x100 + FLASH : ORIGIN = 0x10000100, LENGTH = 2048K - 0x100 + + /* Pick one of the two options for RAM layout */ + + /* OPTION A: Use all RAM banks as one big block */ + /* Reasonable, unless you are doing something */ + /* really particular with DMA or other concurrent */ + /* access that would benefit from striping */ + RAM : ORIGIN = 0x20000000, LENGTH = 264K + + /* OPTION B: Keep the unstriped sections separate */ + /* RAM: ORIGIN = 0x20000000, LENGTH = 256K */ + /* SCRATCH_A: ORIGIN = 0x20040000, LENGTH = 4K */ + /* SCRATCH_B: ORIGIN = 0x20041000, LENGTH = 4K */ +} diff --git a/examples/rp-pico-w/src/main.rs b/examples/rp-pico-w/src/main.rs new file mode 100644 index 0000000..79c74af --- /dev/null +++ b/examples/rp-pico-w/src/main.rs @@ -0,0 +1,167 @@ +#![no_std] +#![no_main] +use bt_hci::driver::{HciController, HciDriver}; +use bt_hci::{ + data, param, Controller, ControllerCmdAsync, ControllerCmdSync, ControllerToHostPacket, FromHciBytes, PacketKind, + ReadHci, WithIndicator, WriteHci, +}; +use core::cell::RefCell; +use core::future::{pending, Future}; +use core::ops::DerefMut; +use cyw43_pio::PioSpi; +use defmt::{assert_eq, todo, *}; +use embassy_executor::{Executor, Spawner}; +use embassy_futures::join::join3; +use embassy_futures::yield_now; +use embassy_rp::bind_interrupts; +use embassy_rp::gpio::{Level, Output}; +use embassy_rp::peripherals::{DMA_CH0, PIO0}; +use embassy_rp::pio::{InterruptHandler, Pio}; +use embassy_sync::blocking_mutex::raw::NoopRawMutex; +use embassy_sync::mutex::Mutex; +use embassy_time::{Duration, Timer}; +use embedded_io_async::Read; +use static_cell::StaticCell; +use trouble_host::adapter::{Adapter, HostResources}; +use trouble_host::advertise::{AdStructure, AdvertiseConfig, BR_EDR_NOT_SUPPORTED, LE_GENERAL_DISCOVERABLE}; +use trouble_host::attribute::{AttributeTable, CharacteristicProp, Service, Uuid}; +use trouble_host::PacketQos; +use {defmt_rtt as _, embassy_time as _, panic_probe as _}; + +bind_interrupts!(struct Irqs { + PIO0_IRQ_0 => InterruptHandler; +}); + +#[embassy_executor::main] +async fn main(spawner: Spawner) { + let p = embassy_rp::init(Default::default()); + let fw = include_bytes!("../../../../embassy/cyw43-firmware/43439A0.bin"); + let clm = include_bytes!("../../../../embassy/cyw43-firmware/43439A0_clm.bin"); + let btfw = include_bytes!("../../../../embassy/cyw43-firmware/43439A0_btfw.bin"); + + // To make flashing faster for development, you may want to flash the firmwares independently + // at hardcoded addresses, instead of baking them into the program with `include_bytes!`: + // probe-rs download 43439A0.bin --format bin --chip RP2040 --base-address 0x10100000 + // probe-rs download 43439A0_clm.bin --format bin --chip RP2040 --base-address 0x10140000 + //let fw = unsafe { core::slice::from_raw_parts(0x10100000 as *const u8, 224190) }; + //let clm = unsafe { core::slice::from_raw_parts(0x10140000 as *const u8, 4752) }; + + let pwr = Output::new(p.PIN_23, Level::Low); + let cs = Output::new(p.PIN_25, Level::High); + let mut pio = Pio::new(p.PIO0, Irqs); + let spi = PioSpi::new(&mut pio.common, pio.sm0, pio.irq0, cs, p.PIN_24, p.PIN_29, p.DMA_CH0); + + static STATE: StaticCell = StaticCell::new(); + let state = STATE.init(cyw43::State::new()); + let (_net_device, mut control, runner) = cyw43::new_with_bluetooth(state, pwr, spi, fw, btfw).await; + + let driver = PicoWController::new(runner); + let controller: HciController<_, 10> = HciController::new(driver); + static HOST_RESOURCES: StaticCell> = StaticCell::new(); + let host_resources = HOST_RESOURCES.init(HostResources::new(PacketQos::None)); + + let adapter: Adapter<'_, NoopRawMutex, _, 2, 4, 1, 1> = Adapter::new(controller, host_resources); + let config = AdvertiseConfig { + params: None, + data: &[ + AdStructure::Flags(LE_GENERAL_DISCOVERABLE | BR_EDR_NOT_SUPPORTED), + AdStructure::ServiceUuids16(&[Uuid::Uuid16([0x0f, 0x18])]), + AdStructure::CompleteLocalName("Trouble PicoW"), + ], + }; + + let mut table: AttributeTable<'_, NoopRawMutex, 10> = AttributeTable::new(); + + // Generic Access Service (mandatory) + let id = b"Trouble PicoW"; + let appearance = [0x80, 0x07]; + let mut bat_level = [0; 1]; + let handle = { + let mut svc = table.add_service(Service::new(0x1800)); + let _ = svc.add_characteristic_ro(0x2a00, id); + let _ = svc.add_characteristic_ro(0x2a01, &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( + 0x2a19, + &[CharacteristicProp::Read, CharacteristicProp::Notify], + &mut bat_level, + ) + }; + + let server = adapter.gatt_server(&table); + + info!("Starting advertising and GATT service"); + let _ = join3( + adapter.run(), + async { + loop { + match server.next().await { + Ok(event) => { + info!("Gatt event: {:?}", event); + } + Err(e) => { + error!("Error processing GATT events: {:?}", e); + } + } + } + }, + async { + let conn = adapter.advertise(&config).await.unwrap(); + // Keep connection alive + let mut tick: u8 = 0; + loop { + Timer::after(Duration::from_secs(10)).await; + tick += 1; + server.notify(handle, &conn, &[tick]).await.unwrap(); + } + }, + ) + .await; +} + +struct PicoWController { + runner: Mutex, PioSpi<'static, PIO0, 0, DMA_CH0>>>, +} + +impl PicoWController { + pub fn new(runner: cyw43::Runner<'static, Output<'static>, PioSpi<'static, PIO0, 0, DMA_CH0>>) -> Self { + Self { + runner: Mutex::new(runner), + } + } +} + +#[derive(Debug, defmt::Format)] +pub struct Error; + +impl embedded_io::Error for Error { + fn kind(&self) -> embedded_io::ErrorKind { + embedded_io::ErrorKind::Other + } +} + +impl HciDriver for PicoWController { + type Error = Error; + fn read(&self, rx: &mut [u8]) -> impl Future> { + async { + let mut runner = self.runner.lock().await; + let n = runner.hci_read(rx).await as usize; + Ok(n) + } + } + + fn write(&self, tx: &[u8]) -> impl Future> { + async { + let mut runner = self.runner.lock().await; + runner.hci_write(tx).await; + Ok(()) + } + } +} diff --git a/host/src/adapter.rs b/host/src/adapter.rs index 719734c..8787571 100644 --- a/host/src/adapter.rs +++ b/host/src/adapter.rs @@ -251,6 +251,8 @@ where match self.controller.write_acl_data(&acl).await { Ok(_) => {} Err(e) => { + #[cfg(feature = "defmt")] + let e = defmt::Debug2Format(&e); warn!("Error writing some ACL data to controller: {:?}", e); panic!(":("); } @@ -329,6 +331,8 @@ where match self.controller.write_acl_data(&acl).await { Ok(_) => {} Err(e) => { + #[cfg(feature = "defmt")] + let e = defmt::Debug2Format(&e); warn!("Error writing some ACL data to controller: {:?}", e); panic!(":("); } @@ -397,6 +401,8 @@ where info!("Ignoring packet: {:?}", p); } Err(e) => { + #[cfg(feature = "defmt")] + let e = defmt::Debug2Format(&e); info!("Error from controller: {:?}", e); } } diff --git a/rust-toolchain.toml b/rust-toolchain.toml index dc95fd6..681e009 100644 --- a/rust-toolchain.toml +++ b/rust-toolchain.toml @@ -5,4 +5,5 @@ channel = "nightly-2024-03-20" components = [ "rust-src", "rustfmt", "llvm-tools-preview" ] targets = [ "thumbv7em-none-eabi", + "thumbv6m-none-eabi", ] From b72fdc20f9048d43538704e5b162b9d0b2d6c41c Mon Sep 17 00:00:00 2001 From: Ulf Lilleengen Date: Thu, 21 Mar 2024 00:35:15 +0100 Subject: [PATCH 4/4] Update to fixed nrf-sdc --- examples/nrf-sdc/Cargo.toml | 4 ++-- examples/nrf-sdc/src/bin/ble_bas_peripheral.rs | 2 +- examples/nrf-sdc/src/bin/ble_l2cap_central.rs | 2 +- examples/nrf-sdc/src/bin/ble_l2cap_peripheral.rs | 2 +- rust-toolchain.toml | 1 + 5 files changed, 6 insertions(+), 5 deletions(-) diff --git a/examples/nrf-sdc/Cargo.toml b/examples/nrf-sdc/Cargo.toml index f7fd0e7..b2fb29d 100644 --- a/examples/nrf-sdc/Cargo.toml +++ b/examples/nrf-sdc/Cargo.toml @@ -37,8 +37,8 @@ embassy-futures = { git = "https://github.com/embassy-rs/embassy.git", branch = embassy-time = { git = "https://github.com/embassy-rs/embassy.git", branch = "main" } embassy-time-driver = { git = "https://github.com/embassy-rs/embassy.git", branch = "main" } embassy-embedded-hal = { git = "https://github.com/embassy-rs/embassy.git", branch = "main" } -nrf-sdc = { git = "https://github.com/alexmoon/nrf-sdc.git", branch = "main" } -nrf-mpsl = { git = "https://github.com/alexmoon/nrf-sdc.git", branch = "main" } +nrf-sdc = { git = "https://github.com/alexmoon/nrf-sdc.git", branch = "update-api" } +nrf-mpsl = { git = "https://github.com/alexmoon/nrf-sdc.git", branch = "update-api" } bt-hci = { git = "https://github.com/alexmoon/bt-hci.git", branch = "serial-controller" } #embassy-executor = {path = "../../../embassy/embassy-executor"} diff --git a/examples/nrf-sdc/src/bin/ble_bas_peripheral.rs b/examples/nrf-sdc/src/bin/ble_bas_peripheral.rs index f409971..ded0a3b 100644 --- a/examples/nrf-sdc/src/bin/ble_bas_peripheral.rs +++ b/examples/nrf-sdc/src/bin/ble_bas_peripheral.rs @@ -1,6 +1,6 @@ #![no_std] #![no_main] -#![feature(type_alias_impl_trait)] +#![feature(impl_trait_in_assoc_type)] use bt_hci::cmd::SyncCmd; use bt_hci::param::BdAddr; diff --git a/examples/nrf-sdc/src/bin/ble_l2cap_central.rs b/examples/nrf-sdc/src/bin/ble_l2cap_central.rs index 49a2ae0..6c1e2fd 100644 --- a/examples/nrf-sdc/src/bin/ble_l2cap_central.rs +++ b/examples/nrf-sdc/src/bin/ble_l2cap_central.rs @@ -1,6 +1,6 @@ #![no_std] #![no_main] -#![feature(type_alias_impl_trait)] +#![feature(impl_trait_in_assoc_type)] use bt_hci::cmd::SyncCmd; use bt_hci::param::BdAddr; diff --git a/examples/nrf-sdc/src/bin/ble_l2cap_peripheral.rs b/examples/nrf-sdc/src/bin/ble_l2cap_peripheral.rs index c6a541a..0128070 100644 --- a/examples/nrf-sdc/src/bin/ble_l2cap_peripheral.rs +++ b/examples/nrf-sdc/src/bin/ble_l2cap_peripheral.rs @@ -1,6 +1,6 @@ #![no_std] #![no_main] -#![feature(type_alias_impl_trait)] +#![feature(impl_trait_in_assoc_type)] use bt_hci::cmd::SyncCmd; use bt_hci::param::BdAddr; diff --git a/rust-toolchain.toml b/rust-toolchain.toml index 681e009..4401c0e 100644 --- a/rust-toolchain.toml +++ b/rust-toolchain.toml @@ -5,5 +5,6 @@ channel = "nightly-2024-03-20" components = [ "rust-src", "rustfmt", "llvm-tools-preview" ] targets = [ "thumbv7em-none-eabi", + "thumbv7em-none-eabihf", "thumbv6m-none-eabi", ]