diff --git a/README.md b/README.md index 4082ce2..f25ff4f 100644 --- a/README.md +++ b/README.md @@ -4,10 +4,7 @@ This is a BLE peripheral stack in Rust. (no-std / no-alloc) To use it you need an implementation of embedded-io offering communication with HCI. -This is early POC / WIP! You are warned! - -The goal is just to have _something_ that works for testing and demos. -I have no plan to add advanced features like pairing and bonding (while it should be possible to add that). +The goal is just to have _something_ that works for testing,demos and personal projects - no intentions to make this pass certification tests. ## License diff --git a/bleps-dedup/src/lib.rs b/bleps-dedup/src/lib.rs index c3a3b26..162d731 100644 --- a/bleps-dedup/src/lib.rs +++ b/bleps-dedup/src/lib.rs @@ -1,4 +1,4 @@ -use proc_macro::TokenStream; +use proc_macro::{Delimiter, TokenStream}; use proc_macro_error::proc_macro_error; #[derive(Debug)] @@ -40,7 +40,7 @@ pub fn dedup(input: TokenStream) -> TokenStream { current.extend([tok]); } }, - proc_macro::TokenTree::Group(_group) => { + proc_macro::TokenTree::Group(group) if group.delimiter() == Delimiter::Brace => { if !current.is_empty() { impls.push(Implementation { is_async: impl_is_async, @@ -109,8 +109,16 @@ fn de_async(input: proc_macro2::TokenStream) -> proc_macro2::TokenStream { } proc_macro2::TokenTree::Punct(p) => { if p.as_char() == '.' { + if let Some(prev) = prev.clone() { + output.extend([prev]); + } prev = Some(tok); } else { + if let Some(prev) = prev.clone() { + output.extend([prev]); + } + prev = None; + output.extend([tok]); } } @@ -135,5 +143,9 @@ fn de_async(input: proc_macro2::TokenStream) -> proc_macro2::TokenStream { } } + if let Some(prev) = prev.clone() { + output.extend([prev]); + } + output } diff --git a/bleps/Cargo.toml b/bleps/Cargo.toml index 34bcc8c..4bd02e8 100644 --- a/bleps/Cargo.toml +++ b/bleps/Cargo.toml @@ -22,17 +22,25 @@ bleps-dedup = { path = "../bleps-dedup" } log = "0.4.16" embedded-io-blocking = { package = "embedded-io", version = "0.6.1" } embedded-io-async = { version = "0.6.0", optional = true } +bitfield = "0.14.0" futures = { version = "0.3", default-features = false, optional = true } critical-section = { version = "1.0.1", optional = true } defmt = {version = "0.3", optional = true } bleps-macros = { path = "../bleps-macros", optional = true } +rand_core = "0.6.4" + +p256 = { version = "0.13.2", default-features = false, features = ["ecdh","arithmetic"], optional = true } +aes = { version = "0.8.2", optional = true } +cmac = { version = "0.7.2", optional = true } [dev-dependencies] env_logger = "0.10.0" +p256 = { version = "0.13.2", default-features = true } [features] async = [ "dep:embedded-io-async", "dep:futures", "dep:critical-section", "bleps-dedup/generate-async" ] macros = [ "bleps-macros" ] +crypto = [ "dep:p256", "dep:aes", "dep:cmac" ] defmt = [ "dep:defmt" ] mtu128 = [] mtu256 = [] diff --git a/bleps/src/acl.rs b/bleps/src/acl.rs index f72ec48..5a0a3b8 100644 --- a/bleps/src/acl.rs +++ b/bleps/src/acl.rs @@ -38,8 +38,15 @@ impl AclPacket { pub fn read(connector: &dyn HciConnection) -> Self { let raw_handle_buffer = [connector.read().unwrap(), connector.read().unwrap()]; let (pb, bc, handle) = Self::decode_raw_handle(raw_handle_buffer); + log::debug!( + "raw handle {:08b} {:08b} - boundary {:?}", + raw_handle_buffer[0], + raw_handle_buffer[1], + pb + ); let len = u16::from_le_bytes([connector.read().unwrap(), connector.read().unwrap()]); + log::info!("read len {}", len); let data = Data::read(connector, len as usize); Self { diff --git a/bleps/src/async_attribute_server.rs b/bleps/src/async_attribute_server.rs index 9e465e7..f5e2f70 100644 --- a/bleps/src/async_attribute_server.rs +++ b/bleps/src/async_attribute_server.rs @@ -1,17 +1,25 @@ +#[cfg(not(feature = "crypto"))] +use core::marker::PhantomData; + use core::cell::RefCell; use critical_section::Mutex; use futures::future::Either; use futures::pin_mut; +use rand_core::{CryptoRng, RngCore}; + +#[cfg(feature = "crypto")] +use crate::sm::AsyncSecurityManager; use crate::{ asynch::Ble, att::Uuid, attribute::Attribute, attribute_server::{AttributeServerError, NotificationData, WorkResult}, + Addr, }; -pub struct AttributeServer<'a, T> +pub struct AttributeServer<'a, T, R: CryptoRng + RngCore> where T: embedded_io_async::Read + embedded_io_async::Write, { @@ -19,13 +27,46 @@ where pub(crate) src_handle: u16, pub(crate) mtu: u16, pub(crate) attributes: &'a mut [Attribute<'a>], + + #[cfg(feature = "crypto")] + pub(crate) security_manager: AsyncSecurityManager<'a, Ble, R>, + + #[cfg(feature = "crypto")] + pub(crate) pin_callback: Option<&'a mut dyn FnMut(u32)>, + + #[cfg(not(feature = "crypto"))] + phantom: PhantomData, } -impl<'a, T> AttributeServer<'a, T> +impl<'a, T, R: CryptoRng + RngCore> AttributeServer<'a, T, R> where T: embedded_io_async::Read + embedded_io_async::Write, { - pub fn new(ble: &'a mut Ble, attributes: &'a mut [Attribute<'a>]) -> AttributeServer<'a, T> { + /// Create a new instance of the AttributeServer + /// + /// When _NOT_ using the `crypto` feature you can pass a mutual reference to `bleps::no_rng::NoRng` + pub fn new( + ble: &'a mut Ble, + attributes: &'a mut [Attribute<'a>], + rng: &'a mut R, + ) -> AttributeServer<'a, T, R> { + AttributeServer::new_with_ltk( + ble, + attributes, + Addr::from_le_bytes(false, [0u8; 6]), + None, + rng, + ) + } + + /// Create a new instance, optionally provide an LTK + pub fn new_with_ltk( + ble: &'a mut Ble, + attributes: &'a mut [Attribute<'a>], + _local_addr: Addr, + _ltk: Option, + _rng: &'a mut R, + ) -> AttributeServer<'a, T, R> { for (i, attr) in attributes.iter_mut().enumerate() { attr.handle = i as u16 + 1; } @@ -41,19 +82,45 @@ where log::trace!("{:#x?}", &attributes); + #[cfg(feature = "crypto")] + let mut security_manager = AsyncSecurityManager::new(_rng); + #[cfg(feature = "crypto")] + { + security_manager.local_address = Some(_local_addr); + security_manager.ltk = _ltk; + } + AttributeServer { ble, src_handle: 0, mtu: crate::attribute_server::BASE_MTU, attributes, + + #[cfg(feature = "crypto")] + security_manager, + + #[cfg(feature = "crypto")] + pin_callback: None, + + #[cfg(not(feature = "crypto"))] + phantom: PhantomData::default(), } } + /// Get the current LTK + pub fn get_ltk(&self) -> Option { + #[cfg(feature = "crypto")] + return self.security_manager.ltk; + + #[cfg(not(feature = "crypto"))] + None + } + /// Run the GATT server until disconnect - pub async fn run(&mut self, notifier: &'a mut F) -> Result<(), AttributeServerError> + pub async fn run(&mut self, notifier: &'a mut F) -> Result<(), AttributeServerError> where - F: FnMut() -> R, - R: core::future::Future, + F: FnMut() -> N, + N: core::future::Future, { let notification_to_send = Mutex::new(RefCell::new(None)); loop { diff --git a/bleps/src/attribute_server.rs b/bleps/src/attribute_server.rs index e36e19b..9eff5d7 100644 --- a/bleps/src/attribute_server.rs +++ b/bleps/src/attribute_server.rs @@ -1,3 +1,10 @@ +#[cfg(not(feature = "crypto"))] +use core::marker::PhantomData; + +use rand_core::{CryptoRng, RngCore}; + +#[cfg(feature = "crypto")] +use crate::sm::SecurityManager; use crate::{ acl::{AclPacket, BoundaryFlag, HostBroadcastFlag}, att::{ @@ -10,7 +17,7 @@ use crate::{ command::{Command, LE_OGF, SET_ADVERTISING_DATA_OCF}, event::EventType, l2cap::{L2capDecodeError, L2capPacket}, - Ble, Data, Error, + Addr, Ble, Data, Error, }; pub const PRIMARY_SERVICE_UUID16: Uuid = Uuid::Uuid16(0x2800); @@ -40,6 +47,7 @@ pub enum WorkResult { pub enum AttributeServerError { L2capError(L2capDecodeError), AttError(AttDecodeError), + SecurityManagerError, } impl From for AttributeServerError { @@ -54,7 +62,7 @@ impl From for AttributeServerError { } } -pub struct AttributeServer<'a> { +pub struct AttributeServer<'a, R: CryptoRng + RngCore> { ble: &'a mut Ble<'a>, // The MTU negotiated for this server. In principle this can be different per-client, // but we only support one client at a time, so we only need to store one value @@ -62,13 +70,22 @@ pub struct AttributeServer<'a> { mtu: u16, src_handle: u16, attributes: &'a mut [Attribute<'a>], + + #[cfg(feature = "crypto")] + security_manager: SecurityManager<'a, Ble<'a>, R>, + + #[cfg(feature = "crypto")] + pub(crate) pin_callback: Option<&'a mut dyn FnMut(u32)>, + + #[cfg(not(feature = "crypto"))] + phantom: PhantomData, } // Using the bleps-dedup proc-macro to de-duplicate the async/sync code // The macro will remove async/await for the SYNC implementation bleps_dedup::dedup! { - impl<'a> SYNC AttributeServer<'a> - impl<'a, T> ASYNC crate::async_attribute_server::AttributeServer<'a, T> + impl<'a, R: CryptoRng + RngCore> SYNC AttributeServer<'a, R> + impl<'a, T, R: CryptoRng + RngCore> ASYNC crate::async_attribute_server::AttributeServer<'a, T, R> where T: embedded_io_async::Read + embedded_io_async::Write, { @@ -136,22 +153,54 @@ bleps_dedup::dedup! { match packet { None => Ok(WorkResult::DidWork), Some(packet) => match packet { - crate::PollResult::Event(evt) => { - if let EventType::DisconnectComplete { - handle: _, - status: _, - reason: _, - } = evt - { + crate::PollResult::Event(EventType::DisconnectComplete { + handle: _, + status: _, + reason: _, + }) => { // Reset the MTU; the next connection will need to renegotiate it. self.mtu = BASE_MTU; Ok(WorkResult::GotDisconnected) - } else { - Ok(WorkResult::DidWork) + } + crate::PollResult::Event(EventType::ConnectionComplete { + status: _status, + handle: _, + role: _, + peer_address: _peer_address, + interval: _, + latency: _, + timeout: _, + }) => { + #[cfg(feature = "crypto")] + if _status == 0 { + self.security_manager.peer_address = Some(_peer_address); } + Ok(WorkResult::DidWork) + } + crate::PollResult::Event(EventType::LongTermKeyRequest { + handle: _handle, + random: _, + diversifier: _, + }) => { + #[cfg(feature = "crypto")] + self.ble + .cmd_long_term_key_request_reply( + _handle, + self.security_manager.ltk.unwrap(), + ).await + .unwrap(); + Ok(WorkResult::DidWork) } + crate::PollResult::Event(_) => Ok(WorkResult::DidWork), crate::PollResult::AsyncData(packet) => { let (src_handle, l2cap_packet) = L2capPacket::decode(packet)?; + if l2cap_packet.channel == 6 { + // handle SM + #[cfg(feature = "crypto")] + self.security_manager + .handle(self.ble, src_handle, l2cap_packet.payload, &mut self.pin_callback).await?; + Ok(WorkResult::DidWork) + } else { let packet = Att::decode(l2cap_packet)?; log::trace!("att: {:x?}", packet); match packet { @@ -233,8 +282,10 @@ bleps_dedup::dedup! { } } + Ok(WorkResult::DidWork) } + } }, } } @@ -514,8 +565,32 @@ bleps_dedup::dedup! { } } -impl<'a> AttributeServer<'a> { - pub fn new(ble: &'a mut Ble<'a>, attributes: &'a mut [Attribute<'a>]) -> AttributeServer<'a> { +impl<'a, R: CryptoRng + RngCore> AttributeServer<'a, R> { + /// Create a new instance of the AttributeServer + /// + /// When _NOT_ using the `crypto` feature you can pass a mutual reference to `bleps::no_rng::NoRng` + pub fn new( + ble: &'a mut Ble<'a>, + attributes: &'a mut [Attribute<'a>], + rng: &'a mut R, + ) -> AttributeServer<'a, R> { + AttributeServer::new_with_ltk( + ble, + attributes, + Addr::from_le_bytes(false, [0u8; 6]), + None, + rng, + ) + } + + /// Create a new instance, optionally provide an LTK + pub fn new_with_ltk( + ble: &'a mut Ble<'a>, + attributes: &'a mut [Attribute<'a>], + _local_addr: Addr, + _ltk: Option, + _rng: &'a mut R, + ) -> AttributeServer<'a, R> { for (i, attr) in attributes.iter_mut().enumerate() { attr.handle = i as u16 + 1; } @@ -531,13 +606,43 @@ impl<'a> AttributeServer<'a> { log::trace!("{:#x?}", &attributes); + #[cfg(feature = "crypto")] + let mut security_manager = SecurityManager::new(_rng); + #[cfg(feature = "crypto")] + { + security_manager.local_address = Some(_local_addr); + security_manager.ltk = _ltk; + } + AttributeServer { ble, mtu: BASE_MTU, src_handle: 0, attributes, + + #[cfg(feature = "crypto")] + security_manager, + #[cfg(feature = "crypto")] + pin_callback: None, + + #[cfg(not(feature = "crypto"))] + phantom: PhantomData::default(), } } + + /// Get the current LTK + pub fn get_ltk(&self) -> Option { + #[cfg(feature = "crypto")] + return self.security_manager.ltk; + + #[cfg(not(feature = "crypto"))] + None + } + + #[cfg(feature = "crypto")] + pub fn set_pin_callback(&mut self, pin_callback: Option<&'a mut dyn FnMut(u32)>) { + self.pin_callback = pin_callback; + } } #[derive(Debug)] diff --git a/bleps/src/command.rs b/bleps/src/command.rs index 68e4979..e54bc83 100644 --- a/bleps/src/command.rs +++ b/bleps/src/command.rs @@ -2,16 +2,21 @@ use crate::{AdvertisingParameters, Data}; pub const CONTROLLER_OGF: u8 = 0x03; pub const RESET_OCF: u16 = 0x03; +pub const SET_EVENT_MASK_OCF: u16 = 0x01; pub const LE_OGF: u8 = 0x08; pub const SET_ADVERTISING_PARAMETERS_OCF: u16 = 0x06; pub const SET_ADVERTISING_DATA_OCF: u16 = 0x08; pub const SET_SCAN_RSP_DATA_OCF: u16 = 0x09; pub const SET_ADVERTISE_ENABLE_OCF: u16 = 0x0a; +pub const LONG_TERM_KEY_REQUEST_REPLY_OCF: u16 = 0x1a; pub const LINK_CONTROL_OGF: u8 = 0x01; pub const DISCONNECT_OCF: u16 = 0x06; +pub const INFORMATIONAL_OGF: u8 = 0x04; +pub const READ_BD_ADDR_OCF: u16 = 0x09; + #[derive(Debug)] pub struct CommandHeader { pub opcode: u16, @@ -58,12 +63,16 @@ pub enum Command<'a> { LeSetScanRspData { data: Data }, LeSetAdvertiseEnable(bool), Disconnect { connection_handle: u16, reason: u8 }, + LeLongTermKeyRequestReply { handle: u16, ltk: u128 }, + ReadBrAddr, + SetEventMask { events: [u8; 8] }, } impl<'a> Command<'a> { pub fn encode(self) -> Data { match self { Command::Reset => { + log::info!("encode reset command"); let mut data = [0u8; 4]; data[0] = 0x01; CommandHeader::from_ogf_ocf(CONTROLLER_OGF, RESET_OCF, 0x00) @@ -136,6 +145,32 @@ impl<'a> Command<'a> { data[6] = reason; Data::new(&data) } + Command::LeLongTermKeyRequestReply { handle, ltk } => { + let mut data = [0u8; 22]; + data[0] = 0x01; + CommandHeader::from_ogf_ocf(LE_OGF, LONG_TERM_KEY_REQUEST_REPLY_OCF, 18) + .write_into(&mut data[1..]); + data[4..][..2].copy_from_slice(&handle.to_le_bytes()); + data[6..].copy_from_slice(<k.to_le_bytes()); + Data::new(&data) + } + Command::ReadBrAddr => { + log::info!("command read br addr"); + let mut data = [0u8; 4]; + data[0] = 0x01; + CommandHeader::from_ogf_ocf(INFORMATIONAL_OGF, READ_BD_ADDR_OCF, 0x00) + .write_into(&mut data[1..]); + Data::new(&data) + } + Command::SetEventMask { events } => { + log::info!("command set event mask"); + let mut data = [0u8; 12]; + data[0] = 0x01; + CommandHeader::from_ogf_ocf(CONTROLLER_OGF, SET_EVENT_MASK_OCF, 0x08) + .write_into(&mut data[1..]); + data[4..].copy_from_slice(&events); + Data::new(&data) + } } } } diff --git a/bleps/src/crypto.rs b/bleps/src/crypto.rs new file mode 100644 index 0000000..b779bd8 --- /dev/null +++ b/bleps/src/crypto.rs @@ -0,0 +1,665 @@ +// This file contains code from Blackrock User-Mode Bluetooth LE Library (https://github.com/mxk/burble) + +use crate::Addr; + +use cmac::digest; +use p256::{ecdh, elliptic_curve::rand_core}; +use rand_core::CryptoRng; +use rand_core::RngCore; + +/// LE Secure Connections Long Term Key. +#[derive(Eq, PartialEq)] +#[must_use] +#[repr(transparent)] +pub struct LTK(pub u128); + +impl LTK { + /// Creates a Long Term Key from a `u128` value. + #[inline(always)] + pub const fn new(k: u128) -> Self { + Self(k) + } +} + +impl From<<K> for u128 { + #[inline(always)] + fn from(k: <K) -> Self { + k.0 + } +} + +/// RFC-4493 AES-CMAC ([Vol 3] Part H, Section 2.2.5). +#[derive(Debug)] +#[repr(transparent)] +pub struct AesCmac(cmac::Cmac); + +impl AesCmac { + /// Creates new AES-CMAC state using key `k`. + #[inline(always)] + #[must_use] + pub(super) fn new(k: &Key) -> Self { + Self(digest::KeyInit::new(&k.0)) + } + + /// Creates new AES-CMAC state using an all-zero key for GAP database hash + /// calculation ([Vol 3] Part G, Section 7.3.1). + #[inline(always)] + #[must_use] + pub fn db_hash() -> Self { + Self::new(&Key::new(0)) + } + + /// Updates CMAC state. + #[inline(always)] + pub fn update(&mut self, b: impl AsRef<[u8]>) -> &mut Self { + digest::Update::update(&mut self.0, b.as_ref()); + self + } + + /// Computes the final MAC value. + #[inline(always)] + #[must_use] + pub fn finalize(self) -> u128 { + u128::from_be_bytes(*digest::FixedOutput::finalize_fixed(self.0).as_ref()) + } + + /// Computes the final MAC value for use as a future key and resets the + /// state. + #[inline(always)] + pub(super) fn finalize_key(&mut self) -> Key { + // Best effort to avoid leaving copies + let mut k = Key::new(0); + digest::FixedOutputReset::finalize_into_reset(&mut self.0, &mut k.0); + k + } +} + +/// LE Secure Connections check value generated by [`MacKey::f6`]. +#[derive(Clone, Copy, Debug, PartialEq, Eq)] +#[must_use] +#[repr(transparent)] +pub struct Check(pub u128); + +#[repr(transparent)] +pub(super) struct Key(aes::cipher::Key); + +impl Key { + /// Creates a key from a `u128` value. + #[inline(always)] + pub fn new(k: u128) -> Self { + Self(k.to_be_bytes().into()) + } +} + +impl From<&Key> for u128 { + #[inline(always)] + fn from(k: &Key) -> Self { + Self::from_be_bytes(k.0.into()) + } +} + +/// Concatenated `AuthReq`, OOB data flag, and IO capability parameters used by +/// [`MacKey::f6`] function ([Vol 3] Part H, Section 2.2.8). +#[repr(transparent)] +#[derive(Clone, Copy, Debug)] +pub struct IoCap([u8; 3]); + +impl IoCap { + /// Creates new `IoCap` parameter. + #[inline(always)] + pub fn new(auth_req: u8, oob_data: bool, io_cap: u8) -> Self { + Self([auth_req, u8::from(oob_data), io_cap]) + } +} + +/// 128-bit key used to compute LE Secure Connections check value +/// ([Vol 3] Part H, Section 2.2.8). +#[must_use] +#[repr(transparent)] +pub struct MacKey(Key); + +impl MacKey { + /// Generates LE Secure Connections check value + /// ([Vol 3] Part H, Section 2.2.8). + #[inline] + pub fn f6(&self, n1: Nonce, n2: Nonce, r: u128, io_cap: IoCap, a1: Addr, a2: Addr) -> Check { + let mut m = AesCmac::new(&self.0); + m.update(n1.0.to_be_bytes()) + .update(n2.0.to_be_bytes()) + .update(r.to_be_bytes()) + .update(io_cap.0) + .update(a1.0) + .update(a2.0); + Check(m.finalize()) + } +} + +/// 128-bit random nonce value ([Vol 3] Part H, Section 2.3.5.6). +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +#[repr(transparent)] +pub struct Nonce(pub u128); + +impl Nonce { + /// Generates a new non-zero random nonce value from the OS CSPRNG. + /// + /// # Panics + /// + /// Panics if the OS CSPRNG is broken. + #[allow(clippy::new_without_default)] + #[inline] + pub fn new(rng: &mut T) -> Self { + let mut b = [0; core::mem::size_of::()]; + rng.fill_bytes(b.as_mut_slice()); + let n = u128::from_ne_bytes(b); + assert_ne!(n, 0); + Self(n) + } + + /// Generates LE Secure Connections confirm value + /// ([Vol 3] Part H, Section 2.2.6). + #[inline] + pub fn f4(&self, u: &PublicKeyX, v: &PublicKeyX, z: u8) -> Confirm { + let mut m = AesCmac::new(&Key::new(self.0)); + m.update(u.as_be_bytes()) + .update(v.as_be_bytes()) + .update([z]); + Confirm(m.finalize()) + } + + /// Generates LE Secure Connections numeric comparison value + /// ([Vol 3] Part H, Section 2.2.9). + #[inline] + pub fn g2(&self, pkax: &PublicKeyX, pkbx: &PublicKeyX, nb: &Self) -> NumCompare { + let mut m = AesCmac::new(&Key::new(self.0)); + m.update(pkax.as_be_bytes()) + .update(pkbx.as_be_bytes()) + .update(nb.0.to_be_bytes()); + #[allow(clippy::cast_possible_truncation)] + NumCompare(m.finalize() as u32 % 1_000_000) + } +} + +/// LE Secure Connections confirm value generated by [`Nonce::f4`]. +#[derive(Clone, Copy, Debug, PartialEq, Eq)] +#[must_use] +#[repr(transparent)] +pub struct Confirm(pub u128); + +/// 6-digit LE Secure Connections numeric comparison value generated by +/// [`Nonce::g2`]. +#[derive(Clone, Copy, Eq, PartialEq, Debug)] +#[must_use] +#[repr(transparent)] +pub struct NumCompare(pub u32); + +/// P-256 elliptic curve secret key. +#[must_use] +#[repr(transparent)] +pub struct SecretKey(p256::NonZeroScalar); + +impl SecretKey { + /// Generates a new random secret key. + #[allow(clippy::new_without_default)] + #[inline(always)] + pub fn new(rng: &mut T) -> Self { + Self(p256::NonZeroScalar::random(rng)) + } + + /// Computes the associated public key. + pub fn public_key(&self) -> PublicKey { + use p256::elliptic_curve::sec1::{Coordinates::Uncompressed, ToEncodedPoint}; + let p = p256::PublicKey::from_secret_scalar(&self.0).to_encoded_point(false); + match p.coordinates() { + Uncompressed { x, y } => PublicKey { + x: PublicKeyX(Coord(*x.as_ref())), + y: Coord(*y.as_ref()), + }, + _ => unreachable!("invalid secret key"), + } + } + + /// Computes a shared secret from the local secret key and remote public + /// key. Returns [`None`] if the public key is either invalid or derived + /// from the same secret key ([Vol 3] Part H, Section 2.3.5.6.1). + #[must_use] + pub fn dh_key(&self, pk: PublicKey) -> Option { + use p256::elliptic_curve::sec1::FromEncodedPoint; + if pk.is_debug() { + return None; // TODO: Compile-time option for debug-only mode + } + + let (x, y) = (&pk.x.0 .0.into(), &pk.y.0.into()); + let rep = p256::EncodedPoint::from_affine_coordinates(x, y, false); + let lpk = p256::PublicKey::from_secret_scalar(&self.0); + // Constant-time ops not required: + // https://github.com/RustCrypto/traits/issues/1227 + let rpk = Option::from(p256::PublicKey::from_encoded_point(&rep)).unwrap_or(lpk); + (rpk != lpk).then(|| DHKey(ecdh::diffie_hellman(&self.0, rpk.as_affine()))) + } +} + +/// P-256 elliptic curve public key ([Vol 3] Part H, Section 3.5.6). +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +#[must_use] +pub struct PublicKey { + pub x: PublicKeyX, + pub y: Coord, +} + +impl PublicKey { + pub fn from_bytes(bytes: &[u8]) -> Self { + let mut x = [0u8; 32]; + let mut y = [0u8; 32]; + + x.copy_from_slice(&bytes[..32]); + y.copy_from_slice(&bytes[32..]); + + x.reverse(); + y.reverse(); + + Self { + x: PublicKeyX(Coord(x)), + y: Coord(y), + } + } + + /// Returns the public key X coordinate. + #[inline(always)] + pub const fn x(&self) -> &PublicKeyX { + &self.x + } + + /// Returns whether `self` is the debug public key + /// ([Vol 3] Part H, Section 2.3.5.6.1). + #[allow(clippy::unreadable_literal)] + #[allow(clippy::unusual_byte_groupings)] + fn is_debug(&self) -> bool { + let (x, y) = (&self.x.0 .0, &self.y.0); + x[..16] == u128::to_be_bytes(0x20b003d2_f297be2c_5e2c83a7_e9f9a5b9) + && x[16..] == u128::to_be_bytes(0xeff49111_acf4fddb_cc030148_0e359de6) + && y[..16] == u128::to_be_bytes(0xdc809c49_652aeb6d_63329abf_5a52155c) + && y[16..] == u128::to_be_bytes(0x766345c2_8fed3024_741c8ed0_1589d28b) + } +} + +// impl Codec for PublicKey { +// #[inline] +// fn pack(&self, p: &mut Packer) { +// let (mut x, mut y) = (self.x.0 .0, self.y.0); +// x.reverse(); +// y.reverse(); +// p.put(x).put(y); +// } + +// #[inline] +// fn unpack(p: &mut Unpacker) -> Option { +// let (mut x, mut y) = (PublicKeyX(Coord(p.bytes())), Coord(p.bytes())); +// x.0 .0.reverse(); +// y.0.reverse(); +// Some(Self { x, y }) +// } +// } + +/// 256-bit elliptic curve coordinate in big-endian byte order. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +#[repr(transparent)] +pub struct Coord([u8; 256 / u8::BITS as usize]); + +impl Coord { + /// Returns the coordinate in big-endian byte order. + #[inline(always)] + pub(super) const fn as_be_bytes(&self) -> &[u8; core::mem::size_of::()] { + &self.0 + } +} + +/// P-256 elliptic curve public key affine X coordinate. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +#[must_use] +#[repr(transparent)] +pub struct PublicKeyX(Coord); + +impl PublicKeyX { + /// Creates the coordinate from a big-endian encoded byte array. + #[cfg(test)] + #[inline] + pub(super) const fn from_be_bytes(x: [u8; core::mem::size_of::()]) -> Self { + Self(Coord(x)) + } + + /// Returns the coordinate in big-endian byte order. + #[inline(always)] + pub(super) const fn as_be_bytes(&self) -> &[u8; core::mem::size_of::()] { + &self.0 .0 + } +} + +/// P-256 elliptic curve shared secret ([Vol 3] Part H, Section 2.3.5.6.1). +#[must_use] +#[repr(transparent)] +pub struct DHKey(ecdh::SharedSecret); + +impl DHKey { + /// Generates LE Secure Connections `MacKey` and `LTK` + /// ([Vol 3] Part H, Section 2.2.7). + #[inline] + pub fn f5(&self, n1: Nonce, n2: Nonce, a1: Addr, a2: Addr) -> (MacKey, LTK) { + let n1 = n1.0.to_be_bytes(); + let n2 = n2.0.to_be_bytes(); + let half = |m: &mut AesCmac, counter: u8| { + m.update([counter]) + .update(b"btle") + .update(n1) + .update(n2) + .update(a1.0) + .update(a2.0) + .update(256_u16.to_be_bytes()) + .finalize_key() + }; + let mut m = AesCmac::new(&Key::new(0x6C88_8391_AAF5_A538_6037_0BDB_5A60_83BE)); + m.update(self.0.raw_secret_bytes()); + let mut m = AesCmac::new(&m.finalize_key()); + (MacKey(half(&mut m, 0)), LTK(u128::from(&half(&mut m, 1)))) + } +} + +/// Combines `hi` and `lo` values into a big-endian byte array. +#[allow(clippy::redundant_pub_crate)] +#[cfg(test)] +pub(super) fn u256>(hi: u128, lo: u128) -> T { + let mut b = [0; 32]; + b[..16].copy_from_slice(&hi.to_be_bytes()); + b[16..].copy_from_slice(&lo.to_be_bytes()); + T::from(b) +} + +#[allow(clippy::unreadable_literal)] +#[allow(clippy::unusual_byte_groupings)] +#[cfg(test)] +mod tests { + use p256::elliptic_curve::rand_core::OsRng; + + use super::*; + extern crate std; + + #[test] + fn sizes() { + assert_eq!(core::mem::size_of::(), 32); + assert_eq!(core::mem::size_of::(), 64); + assert_eq!(core::mem::size_of::(), 32); + assert_eq!(core::mem::size_of::(), 32); + } + + /// Debug mode key ([Vol 3] Part H, Section 2.3.5.6.1). + #[test] + fn debug_key() { + let sk = secret_key( + 0x3f49f6d4_a3c55f38_74c9b3e3_d2103f50, + 0x4aff607b_eb40b799_5899b8a6_cd3c1abd, + ); + let pk = PublicKey { + x: PublicKeyX(Coord(u256( + 0x20b003d2_f297be2c_5e2c83a7_e9f9a5b9, + 0xeff49111_acf4fddb_cc030148_0e359de6, + ))), + y: Coord(u256( + 0xdc809c49_652aeb6d_63329abf_5a52155c, + 0x766345c2_8fed3024_741c8ed0_1589d28b, + )), + }; + assert_eq!(sk.public_key(), pk); + assert!(pk.is_debug()); + } + + /// P-256 data set 1 ([Vol 2] Part G, Section 7.1.2.1). + #[test] + fn p256_1() { + let (ska, skb) = ( + secret_key( + 0x3f49f6d4_a3c55f38_74c9b3e3_d2103f50, + 0x4aff607b_eb40b799_5899b8a6_cd3c1abd, + ), + secret_key( + 0x55188b3d_32f6bb9a_900afcfb_eed4e72a, + 0x59cb9ac2_f19d7cfb_6b4fdd49_f47fc5fd, + ), + ); + let (pka, pkb) = ( + PublicKey { + x: PublicKeyX(Coord(u256( + 0x20b003d2_f297be2c_5e2c83a7_e9f9a5b9, + 0xeff49111_acf4fddb_cc030148_0e359de6, + ))), + y: Coord(u256( + 0xdc809c49_652aeb6d_63329abf_5a52155c, + 0x766345c2_8fed3024_741c8ed0_1589d28b, + )), + }, + PublicKey { + x: PublicKeyX(Coord(u256( + 0x1ea1f0f0_1faf1d96_09592284_f19e4c00, + 0x47b58afd_8615a69f_559077b2_2faaa190, + ))), + y: Coord(u256( + 0x4c55f33e_429dad37_7356703a_9ab85160, + 0x472d1130_e28e3676_5f89aff9_15b1214a, + )), + }, + ); + let dh_key = shared_secret( + 0xec0234a3_57c8ad05_341010a6_0a397d9b, + 0x99796b13_b4f866f1_868d34f3_73bfa698, + ); + assert_eq!(ska.public_key(), pka); + assert_eq!(skb.public_key(), pkb); + assert_eq!( + ska.dh_key(pkb).unwrap().0.raw_secret_bytes(), + dh_key.0.raw_secret_bytes() + ); + + assert!(!pkb.is_debug()); + assert!(skb.dh_key(pkb).is_none()); + } + + /// P-256 data set 2 ([Vol 2] Part G, Section 7.1.2.2). + #[test] + fn p256_2() { + let (ska, skb) = ( + secret_key( + 0x06a51669_3c9aa31a_6084545d_0c5db641, + 0xb48572b9_7203ddff_b7ac73f7_d0457663, + ), + secret_key( + 0x529aa067_0d72cd64_97502ed4_73502b03, + 0x7e8803b5_c60829a5_a3caa219_505530ba, + ), + ); + let (pka, pkb) = ( + PublicKey { + x: PublicKeyX(Coord(u256( + 0x2c31a47b_5779809e_f44cb5ea_af5c3e43, + 0xd5f8faad_4a8794cb_987e9b03_745c78dd, + ))), + y: Coord(u256( + 0x91951218_3898dfbe_cd52e240_8e43871f, + 0xd0211091_17bd3ed4_eaf84377_43715d4f, + )), + }, + PublicKey { + x: PublicKeyX(Coord(u256( + 0xf465e43f_f23d3f1b_9dc7dfc0_4da87581, + 0x84dbc966_204796ec_cf0d6cf5_e16500cc, + ))), + y: Coord(u256( + 0x0201d048_bcbbd899_eeefc424_164e33c2, + 0x01c2b010_ca6b4d43_a8a155ca_d8ecb279, + )), + }, + ); + let dh_key = shared_secret( + 0xab85843a_2f6d883f_62e5684b_38e30733, + 0x5fe6e194_5ecd1960_4105c6f2_3221eb69, + ); + assert_eq!(ska.public_key(), pka); + assert_eq!(skb.public_key(), pkb); + assert_eq!( + ska.dh_key(pkb).unwrap().0.raw_secret_bytes(), + dh_key.0.raw_secret_bytes() + ); + } + + /// Key generation function ([Vol 3] Part H, Section D.3). + #[test] + fn dh_key_f5() { + let w = shared_secret( + 0xec0234a3_57c8ad05_341010a6_0a397d9b, + 0x99796b13_b4f866f1_868d34f3_73bfa698, + ); + let n1 = Nonce(0xd5cb8454_d177733e_ffffb2ec_712baeab); + let n2 = Nonce(0xa6e8e7cc_25a75f6e_216583f7_ff3dc4cf); + let a1 = Addr([0x00, 0x56, 0x12, 0x37, 0x37, 0xbf, 0xce]); + let a2 = Addr([0x00, 0xa7, 0x13, 0x70, 0x2d, 0xcf, 0xc1]); + let (mk, ltk) = w.f5(n1, n2, a1, a2); + assert_eq!(ltk.0, 0x69867911_69d7cd23_980522b5_94750a38); + assert_eq!(u128::from(&mk.0), 0x2965f176_a1084a02_fd3f6a20_ce636e20); + } + + #[inline] + fn secret_key(hi: u128, lo: u128) -> SecretKey { + SecretKey(p256::NonZeroScalar::from_repr(u256(hi, lo)).unwrap()) + } + + #[inline] + fn shared_secret(hi: u128, lo: u128) -> DHKey { + DHKey(ecdh::SharedSecret::from(u256::(hi, lo))) + } + + #[test] + fn testtest() { + let skb = SecretKey::new(&mut OsRng::default()); + let _pkb = skb.public_key(); + + let ska = SecretKey::new(&mut OsRng::default()); + let pka = ska.public_key(); + + let _dh_key = skb.dh_key(pka).unwrap(); + } + + #[test] + fn testtest2() { + let bytes = [ + 0x1eu8, 0x3b, 0x26, 0x40, 0x0e, 0xba, 0x72, 0x51, 0x81, 0xf9, 0x3d, 0x16, 0xb3, 0xc4, + 0x11, 0x55, 0x3f, 0xa8, 0x88, 0x47, 0x08, 0x1c, 0x4a, 0x42, 0x88, 0xbb, 0x68, 0x1d, + 0x93, 0xe5, 0xab, 0xb3, 0x72, 0xfa, 0x93, 0xb4, 0xa0, 0xfe, 0x3f, 0x83, 0x9c, 0x85, + 0x5b, 0x5f, 0xb6, 0x30, 0x09, 0x85, 0x47, 0xfd, 0xa8, 0xfa, 0x11, 0x71, 0xe4, 0x95, + 0x17, 0x71, 0x98, 0x82, 0x8f, 0xf8, 0x79, 0x94, + ]; + + let skb = SecretKey::new(&mut OsRng::default()); + let _pkb = skb.public_key(); + + let pka = PublicKey::from_bytes(&bytes); + + let _dh_key = skb.dh_key(pka).unwrap(); + } + + #[test] + fn nonce() { + // No fair dice rolls for us! + assert_ne!( + Nonce::new(&mut OsRng::default()), + Nonce::new(&mut OsRng::default()) + ); + } + + /// Confirm value generation function ([Vol 3] Part H, Section D.2). + #[test] + fn nonce_f4() { + let u = PublicKeyX::from_be_bytes(u256( + 0x20b003d2_f297be2c_5e2c83a7_e9f9a5b9, + 0xeff49111_acf4fddb_cc030148_0e359de6, + )); + let v = PublicKeyX::from_be_bytes(u256( + 0x55188b3d_32f6bb9a_900afcfb_eed4e72a, + 0x59cb9ac2_f19d7cfb_6b4fdd49_f47fc5fd, + )); + let x = Nonce(0xd5cb8454_d177733e_ffffb2ec_712baeab); + assert_eq!(x.f4(&u, &v, 0).0, 0xf2c916f1_07a9bd1c_f1eda1be_a974872d); + } + + /// Numeric comparison generation function ([Vol 3] Part H, Section D.5). + #[allow(clippy::unreadable_literal)] + #[test] + fn nonce_g2() { + let u = PublicKeyX::from_be_bytes(u256( + 0x20b003d2_f297be2c_5e2c83a7_e9f9a5b9, + 0xeff49111_acf4fddb_cc030148_0e359de6, + )); + let v = PublicKeyX::from_be_bytes(u256( + 0x55188b3d_32f6bb9a_900afcfb_eed4e72a, + 0x59cb9ac2_f19d7cfb_6b4fdd49_f47fc5fd, + )); + let x = Nonce(0xd5cb8454_d177733e_ffffb2ec_712baeab); + let y = Nonce(0xa6e8e7cc_25a75f6e_216583f7_ff3dc4cf); + assert_eq!(x.g2(&u, &v, &y), NumCompare(0x2f9ed5ba % 1_000_000)); + } + + /// Check value generation function ([Vol 3] Part H, Section D.4). + #[test] + fn mac_key_f6() { + let k = MacKey(Key::new(0x2965f176_a1084a02_fd3f6a20_ce636e20)); + let n1 = Nonce(0xd5cb8454_d177733e_ffffb2ec_712baeab); + let n2 = Nonce(0xa6e8e7cc_25a75f6e_216583f7_ff3dc4cf); + let r = 0x12a3343b_b453bb54_08da42d2_0c2d0fc8; + let io_cap = IoCap([0x01, 0x01, 0x02]); + let a1 = Addr([0x00, 0x56, 0x12, 0x37, 0x37, 0xbf, 0xce]); + let a2 = Addr([0x00, 0xa7, 0x13, 0x70, 0x2d, 0xcf, 0xc1]); + let c = k.f6(n1, n2, r, io_cap, a1, a2); + assert_eq!(c.0, 0xe3c47398_9cd0e8c5_d26c0b09_da958f61); + } + + #[test] + fn nonce_f4_test() { + let ra = [ + 0x11u8, 0x3a, 0x7a, 0x69, 0x11, 0xcd, 0x44, 0x15, 0x52, 0xf7, 0x47, 0xe8, 0x26, 0x67, + 0x72, 0xca, + ]; + + let rb = [ + 0xa5u8, 0x9e, 0x9a, 0x32, 0xc0, 0x97, 0x1c, 0xf7, 0x72, 0x1c, 0x29, 0xa7, 0x8c, 0x1e, + 0xfd, 0x18, + ]; + + let pkb = [ + 0xd, 0x80, 0x33, 0x93, 0xad, 0x1f, 0x7e, 0x9a, 0x30, 0xc9, 0x6e, 0x1, 0x78, 0xf3, 0x43, + 0x14, 0xa0, 0x57, 0xae, 0xa5, 0xa8, 0xee, 0x75, 0x51, 0x3f, 0xaa, 0xb1, 0x80, 0x75, + 0xc7, 0x14, 0x50, 0x73, 0x9a, 0x98, 0x95, 0x36, 0x2e, 0xe6, 0x81, 0x5f, 0xbf, 0x16, + 0xa2, 0x8c, 0xf6, 0x9d, 0xdc, 0x1f, 0xb8, 0x84, 0x8c, 0x7d, 0x37, 0x36, 0xe4, 0x36, + 0x3c, 0xb3, 0xe8, 0xfe, 0x4a, 0x73, 0xc6, + ]; + + let pka = [ + 0x97, 0x20, 0x0f, 0xfe, 0xf0, 0xec, 0xdd, 0x11, 0xda, 0xa8, 0xa8, 0x07, 0x3e, 0xd7, + 0xc6, 0xf2, 0x68, 0x5d, 0xc2, 0x58, 0x71, 0x1e, 0x34, 0x4f, 0xa1, 0xc4, 0x44, 0xa9, + 0x7c, 0x71, 0xee, 0x54, 0x0d, 0xad, 0xb7, 0x69, 0x89, 0x9d, 0x4f, 0x83, 0x37, 0xcd, + 0x43, 0xd3, 0x9f, 0x05, 0x13, 0x99, 0x6f, 0xbc, 0x1a, 0x89, 0xed, 0xb4, 0x7f, 0x80, + 0x98, 0xcf, 0xad, 0x7c, 0x4c, 0x57, 0xbf, 0xe1, + ]; + + let mut pkb_x = [0u8; 32]; + pkb_x.copy_from_slice(&pkb[..32]); + pkb_x.reverse(); + let mut pka_x = [0u8; 32]; + pka_x.copy_from_slice(&pka[..32]); + pka_x.reverse(); + + let pkbx = PublicKeyX::from_be_bytes(pkb_x); + let pkax = PublicKeyX::from_be_bytes(pka_x); + extern crate std; + + let x = Nonce(u128::from_le_bytes(ra)); + let y = Nonce(u128::from_le_bytes(rb)); + + assert_eq!(x.g2(&pkax, &pkbx, &y).0, 991180); + } +} diff --git a/bleps/src/event.rs b/bleps/src/event.rs index eacdd6f..fb4191d 100644 --- a/bleps/src/event.rs +++ b/bleps/src/event.rs @@ -1,4 +1,4 @@ -use crate::{Data, Error, HciConnection}; +use crate::{Addr, Data, Error, HciConnection}; #[derive(Debug)] pub struct Event { @@ -23,6 +23,20 @@ pub enum EventType { connection_handles: u16, // should be list completed_packets: u16, // should be list }, + ConnectionComplete { + status: u8, + handle: u16, + role: u8, + peer_address: Addr, + interval: u16, + latency: u16, + timeout: u16, + }, + LongTermKeyRequest { + handle: u16, + random: u64, + diversifier: u16, + }, Unknown, } @@ -70,6 +84,10 @@ impl ErrorCode { const EVENT_COMMAND_COMPLETE: u8 = 0x0e; const EVENT_DISCONNECTION_COMPLETE: u8 = 0x05; const EVENT_NUMBER_OF_COMPLETED_PACKETS: u8 = 0x13; +const EVENT_LE_META: u8 = 0x3e; +const EVENT_LE_META_CONNECTION_COMPLETE: u8 = 0x01; +// TODO ENHANCED_CONNECTION_COMPLETE +const EVENT_LE_META_LONG_TERM_KEY_REQUEST: u8 = 0x05; impl EventType { pub fn check_command_completed(self) -> Result { @@ -128,6 +146,51 @@ impl EventType { completed_packets: completed_packet, } } + EVENT_LE_META => { + let sub_event = event.data.as_slice()[0]; + let data = &event.data.as_slice()[1..]; + + match sub_event { + EVENT_LE_META_CONNECTION_COMPLETE => { + let status = data[0]; + let handle = ((data[2] as u16) << 8) + data[1] as u16; + let role = data[3]; + let peer_address = + Addr::from_le_bytes(data[4] != 0, data[5..][..6].try_into().unwrap()); + let interval = ((data[2] as u16) << 8) + data[1] as u16; + let latency = ((data[2] as u16) << 8) + data[1] as u16; + let timeout = ((data[2] as u16) << 8) + data[1] as u16; + + Self::ConnectionComplete { + status, + handle, + role, + peer_address, + interval, + latency, + timeout, + } + } + EVENT_LE_META_LONG_TERM_KEY_REQUEST => { + let handle = ((data[1] as u16) << 8) + data[0] as u16; + let random = u64::from_be_bytes((&data[2..][..8]).try_into().unwrap()); + let diversifier = ((data[11] as u16) << 8) + data[10] as u16; + Self::LongTermKeyRequest { + handle, + random, + diversifier, + } + } + _ => { + log::warn!( + "Ignoring unknown le-meta event {:02x} data = {:02x?}", + sub_event, + data + ); + Self::Unknown + } + } + } _ => { log::warn!( "Ignoring unknown event {:02x} data = {:02x?}", @@ -183,6 +246,51 @@ impl EventType { completed_packets: completed_packet, } } + EVENT_LE_META => { + let sub_event = event.data.as_slice()[0]; + let data = &event.data.as_slice()[1..]; + + match sub_event { + EVENT_LE_META_CONNECTION_COMPLETE => { + let status = data[0]; + let handle = ((data[2] as u16) << 8) + data[1] as u16; + let role = data[3]; + let peer_address = + Addr::from_le_bytes(data[4] != 0, data[5..][..6].try_into().unwrap()); + let interval = ((data[2] as u16) << 8) + data[1] as u16; + let latency = ((data[2] as u16) << 8) + data[1] as u16; + let timeout = ((data[2] as u16) << 8) + data[1] as u16; + + Self::ConnectionComplete { + status, + handle, + role, + peer_address, + interval, + latency, + timeout, + } + } + EVENT_LE_META_LONG_TERM_KEY_REQUEST => { + let handle = ((data[1] as u16) << 8) + data[0] as u16; + let random = u64::from_be_bytes((&data[2..][..8]).try_into().unwrap()); + let diversifier = ((data[11] as u16) << 8) + data[10] as u16; + Self::LongTermKeyRequest { + handle, + random, + diversifier, + } + } + _ => { + log::warn!( + "Ignoring unknown le-meta event {:02x} data = {:02x?}", + sub_event, + data + ); + Self::Unknown + } + } + } _ => { log::warn!( "Ignoring unknown event {:02x} data = {:02x?}", diff --git a/bleps/src/l2cap.rs b/bleps/src/l2cap.rs index 720c3bd..2294744 100644 --- a/bleps/src/l2cap.rs +++ b/bleps/src/l2cap.rs @@ -15,6 +15,7 @@ pub enum L2capDecodeError { impl L2capPacket { pub fn decode(packet: AclPacket) -> Result<(u16, Self), L2capDecodeError> { let data = packet.data.as_slice(); + log::debug!("L2CAP {:02x?}", data); let length = (data[0] as u16) + ((data[1] as u16) << 8); let channel = (data[2] as u16) + ((data[3] as u16) << 8); let payload = Data::new(&data[4..]); @@ -42,4 +43,18 @@ impl L2capPacket { data } + + pub fn encode_sm(att_data: Data) -> Data { + let mut data = Data::new(&[ + 0, 0, // len set later + 0x06, 0x00, // channel + ]); + data.append(att_data.as_slice()); + + let len = data.len - 4; + data.set(0, (len & 0xff) as u8); + data.set(1, ((len >> 8) & 0xff) as u8); + + data + } } diff --git a/bleps/src/lib.rs b/bleps/src/lib.rs index 4ac5a9b..4c91ca3 100644 --- a/bleps/src/lib.rs +++ b/bleps/src/lib.rs @@ -1,13 +1,16 @@ #![no_std] #![feature(assert_matches)] +#![allow(stable_features)] #![cfg_attr(feature = "async", feature(async_fn_in_trait))] +#![cfg_attr(feature = "async", allow(async_fn_in_trait))] #![cfg_attr(feature = "async", allow(incomplete_features))] use core::cell::RefCell; use acl::AclPacket; use command::{ - opcode, Command, SET_ADVERTISE_ENABLE_OCF, SET_ADVERTISING_DATA_OCF, SET_SCAN_RSP_DATA_OCF, + opcode, Command, INFORMATIONAL_OGF, LONG_TERM_KEY_REQUEST_REPLY_OCF, READ_BD_ADDR_OCF, + SET_ADVERTISE_ENABLE_OCF, SET_ADVERTISING_DATA_OCF, SET_EVENT_MASK_OCF, SET_SCAN_RSP_DATA_OCF, }; use command::{LE_OGF, SET_ADVERTISING_PARAMETERS_OCF}; use embedded_io_blocking::{Read, Write}; @@ -25,6 +28,11 @@ pub mod ad_structure; pub mod attribute; pub mod attribute_server; +#[cfg(feature = "crypto")] +pub mod crypto; +#[cfg(feature = "crypto")] +pub mod sm; + #[cfg(feature = "async")] pub mod async_attribute_server; @@ -56,6 +64,25 @@ impl defmt::Format for Error { } } +/// 56-bit device address in big-endian byte order used by [`DHKey::f5`] and +/// [`MacKey::f6`] functions ([Vol 3] Part H, Section 2.2.7 and 2.2.8). +#[derive(Clone, Copy, Debug)] +#[must_use] +#[repr(transparent)] +pub struct Addr(pub [u8; 7]); + +impl Addr { + /// Creates a device address from a little-endian byte array. + #[inline] + pub fn from_le_bytes(is_random: bool, mut v: [u8; 6]) -> Self { + v.reverse(); + let mut a = [0; 7]; + a[0] = u8::from(is_random); + a[1..].copy_from_slice(&v); + Self(a) + } +} + #[derive(Debug)] pub enum PollResult { Event(EventType), @@ -219,11 +246,13 @@ impl<'a> Ble<'a> { Ble { connector } } - pub fn init(&mut self) -> Result + pub fn init(&mut self) -> Result<(), Error> where Self: Sized, { - Ok(self.cmd_reset()?) + self.cmd_reset()?; + self.cmd_set_event_mask([0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff])?; + Ok(()) } pub fn cmd_reset(&mut self) -> Result @@ -235,6 +264,15 @@ impl<'a> Ble<'a> { .check_command_completed() } + pub fn cmd_set_event_mask(&mut self, events: [u8; 8]) -> Result + where + Self: Sized, + { + self.write_bytes(Command::SetEventMask { events }.encode().as_slice()); + self.wait_for_command_complete(CONTROLLER_OGF, SET_EVENT_MASK_OCF)? + .check_command_completed() + } + pub fn cmd_set_le_advertising_parameters(&mut self) -> Result where Self: Sized, @@ -287,6 +325,47 @@ impl<'a> Ble<'a> { .check_command_completed() } + pub fn cmd_long_term_key_request_reply( + &mut self, + handle: u16, + ltk: u128, + ) -> Result + where + Self: Sized, + { + log::info!("before, key = {:x}, hanlde = {:x}", ltk, handle); + self.write_bytes( + Command::LeLongTermKeyRequestReply { handle, ltk } + .encode() + .as_slice(), + ); + log::info!("done writing command"); + let res = self + .wait_for_command_complete(LE_OGF, LONG_TERM_KEY_REQUEST_REPLY_OCF)? + .check_command_completed(); + log::info!("got completion event"); + + res + } + + pub fn cmd_read_br_addr(&mut self) -> Result<[u8; 6], Error> + where + Self: Sized, + { + self.write_bytes(Command::ReadBrAddr.encode().as_slice()); + let res = self + .wait_for_command_complete(INFORMATIONAL_OGF, READ_BD_ADDR_OCF)? + .check_command_completed()?; + match res { + EventType::CommandComplete { + num_packets: _, + opcode: _, + data, + } => Ok(data.as_slice()[1..][..6].try_into().unwrap()), + _ => Err(Error::Failed(0)), + } + } + fn wait_for_command_complete(&mut self, ogf: u8, ocf: u16) -> Result where Self: Sized, @@ -294,6 +373,9 @@ impl<'a> Ble<'a> { let timeout_at = self.connector.millis() + TIMEOUT_MILLIS; loop { let res = self.poll(); + if res.is_some() { + log::info!("polled while waiting {:?}", res); + } match res { Some(PollResult::Event(event)) => match event { @@ -322,7 +404,28 @@ impl<'a> Ble<'a> { Some(packet_type) => match packet_type { PACKET_TYPE_COMMAND => {} PACKET_TYPE_ASYNC_DATA => { - let acl_packet = AclPacket::read(self.connector); + let mut acl_packet = AclPacket::read(self.connector); + let wanted = + u16::from_le_bytes(acl_packet.data.as_slice()[..2].try_into().unwrap()) + as usize; + + // somewhat dirty way to handle re-assembling fragmented packets + loop { + log::debug!("Wanted = {}, actual = {}", wanted, acl_packet.data.len()); + + if wanted == acl_packet.data.len() - 4 { + break; + } + + log::debug!("Need more!"); + if self.connector.read() != Some(PACKET_TYPE_ASYNC_DATA) { + log::error!("Expected async data"); + } + + let next_acl_packet = AclPacket::read(self.connector); + acl_packet.data.append(next_acl_packet.data.as_slice()); + } + return Some(PollResult::AsyncData(acl_packet)); } PACKET_TYPE_EVENT => { @@ -450,7 +553,10 @@ pub mod asynch { where Self: Sized, { - Ok(self.cmd_reset().await?) + let res = self.cmd_reset().await?; + self.cmd_set_event_mask([0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff]) + .await?; + Ok(res) } pub async fn cmd_reset(&mut self) -> Result @@ -463,6 +569,17 @@ pub mod asynch { .check_command_completed() } + pub async fn cmd_set_event_mask(&mut self, events: [u8; 8]) -> Result + where + Self: Sized, + { + self.write_bytes(Command::SetEventMask { events }.encode().as_slice()) + .await; + self.wait_for_command_complete(CONTROLLER_OGF, SET_EVENT_MASK_OCF) + .await? + .check_command_completed() + } + pub async fn cmd_set_le_advertising_parameters(&mut self) -> Result where Self: Sized, @@ -517,6 +634,51 @@ pub mod asynch { .check_command_completed() } + pub async fn cmd_long_term_key_request_reply( + &mut self, + handle: u16, + ltk: u128, + ) -> Result + where + Self: Sized, + { + log::info!("before, key = {:x}, hanlde = {:x}", ltk, handle); + self.write_bytes( + Command::LeLongTermKeyRequestReply { handle, ltk } + .encode() + .as_slice(), + ) + .await; + log::info!("done writing command"); + let res = self + .wait_for_command_complete(LE_OGF, LONG_TERM_KEY_REQUEST_REPLY_OCF) + .await? + .check_command_completed(); + log::info!("got completion event"); + + res + } + + pub async fn cmd_read_br_addr(&mut self) -> Result<[u8; 6], Error> + where + Self: Sized, + { + self.write_bytes(Command::ReadBrAddr.encode().as_slice()) + .await; + let res = self + .wait_for_command_complete(INFORMATIONAL_OGF, READ_BD_ADDR_OCF) + .await? + .check_command_completed()?; + match res { + EventType::CommandComplete { + num_packets: _, + opcode: _, + data, + } => Ok(data.as_slice()[1..][..6].try_into().unwrap()), + _ => Err(Error::Failed(0)), + } + } + pub(crate) async fn wait_for_command_complete( &mut self, ogf: u8, @@ -562,7 +724,36 @@ pub mod asynch { Some(packet_type) => match packet_type { PACKET_TYPE_COMMAND => {} PACKET_TYPE_ASYNC_DATA => { - let acl_packet = AclPacket::async_read(&mut *self.hci.borrow_mut()).await; + let mut acl_packet = + AclPacket::async_read(&mut *self.hci.borrow_mut()).await; + + let wanted = + u16::from_le_bytes(acl_packet.data.as_slice()[..2].try_into().unwrap()) + as usize; + + // somewhat dirty way to handle re-assembling fragmented packets + loop { + log::debug!("Wanted = {}, actual = {}", wanted, acl_packet.data.len()); + + if wanted == acl_packet.data.len() - 4 { + break; + } + + log::debug!("Need more!"); + let mut buffer = [0u8; 1]; + (&mut *self.hci.borrow_mut()) + .read(&mut buffer) + .await + .unwrap(); + if buffer[0] != PACKET_TYPE_ASYNC_DATA { + log::error!("Expected async data"); + } + + let next_acl_packet = + AclPacket::async_read(&mut *self.hci.borrow_mut()).await; + acl_packet.data.append(next_acl_packet.data.as_slice()); + } + return Some(PollResult::AsyncData(acl_packet)); } PACKET_TYPE_EVENT => { @@ -609,3 +800,28 @@ pub mod asynch { } } } + +#[cfg(not(feature = "crypto"))] +pub mod no_rng { + pub struct NoRng; + + impl rand_core::CryptoRng for NoRng {} + + impl rand_core::RngCore for NoRng { + fn next_u32(&mut self) -> u32 { + unimplemented!() + } + + fn next_u64(&mut self) -> u64 { + unimplemented!() + } + + fn fill_bytes(&mut self, _dest: &mut [u8]) { + unimplemented!() + } + + fn try_fill_bytes(&mut self, _dest: &mut [u8]) -> Result<(), rand_core::Error> { + unimplemented!() + } + } +} diff --git a/bleps/src/sm.rs b/bleps/src/sm.rs new file mode 100644 index 0000000..3e47fbe --- /dev/null +++ b/bleps/src/sm.rs @@ -0,0 +1,451 @@ +use core::marker::PhantomData; + +use bitfield::bitfield; +use p256::elliptic_curve::rand_core::{CryptoRng, RngCore}; + +use crate::{ + acl::{AclPacket, BoundaryFlag, HostBroadcastFlag}, + attribute_server::AttributeServerError, + crypto::{Check, Confirm, DHKey, IoCap, MacKey, Nonce, PublicKey, SecretKey}, + l2cap::L2capPacket, + Addr, Ble, Data, +}; + +#[derive(Debug, Clone, Copy)] +#[repr(u8)] +pub enum SecurityManagerError { + PasskeyEntryFailed = 1, + OobNotAvailable, + AuthenticationRequirements, + ConfirmValueFailed, + PairingNotSupported, + EncryptionKeySize, + CommandNotSupported, + UnspecifiedReason, + RepeatedAttempts, + InvalidParameters, + DHKeyCheckFailed, + NumericComparisonFailed, + BrEdrPairingInProgress, + GenerationNotAllowed, + KeyRejected, +} + +#[derive(Debug, Clone, Copy)] +#[repr(u8)] +pub enum IoCapability { + DisplayOnly = 0, + DisplayYesNo = 1, + KeyboardOnly = 2, + NoInputNoOutput = 3, + KeyboardDisplay = 4, +} + +#[derive(Debug, Clone, Copy)] +#[repr(u8)] +pub enum OobDataFlag { + NotPresent = 0, + Present = 1, +} + +bitfield! { + pub struct AuthReq(u8); + impl Debug; + + pub bonding_flags, set_bonding_flags: 1, 0; + pub mitm, set_mitm: 2, 2; + pub sc, set_sc: 3, 3; + pub keypress, set_keypress: 4, 4; + pub ct2, set_ct2: 5, 5; + pub rfu, set_rfu: 7, 6; +} + +const SM_PAIRING_REQUEST: u8 = 0x01; +const SM_PAIRING_RESPONSE: u8 = 0x02; +const SM_PAIRING_CONFIRM: u8 = 0x03; +const SM_PAIRING_RANDOM: u8 = 0x04; +const SM_PAIRING_FAILED: u8 = 0x05; +const SM_PAIRING_PUBLIC_KEY: u8 = 0x0c; +const SM_PAIRING_DHKEY_CHECK: u8 = 0x0d; + +pub struct SecurityManager<'a, B, R: CryptoRng> { + ioa: Option, + + skb: Option, + pkb: Option, + + pka: Option, + + confirm: Option, + + na: Option, + nb: Option, + + dh_key: Option, + + mac_key: Option, + + eb: Option, + + pub local_address: Option, + pub peer_address: Option, + pub ltk: Option, + + rng: &'a mut R, + phantom: PhantomData, +} + +pub trait BleWriter { + fn write_bytes(&mut self, bytes: &[u8]); +} + +impl<'a> BleWriter for Ble<'a> { + fn write_bytes(&mut self, bytes: &[u8]) { + self.write_bytes(bytes); + } +} + +impl<'a, B, R: CryptoRng> SecurityManager<'a, B, R> { + pub fn new(rng: &'a mut R) -> Self { + Self { + ioa: None, + skb: None, + pkb: None, + pka: None, + confirm: None, + na: None, + nb: None, + dh_key: None, + mac_key: None, + eb: None, + local_address: None, + peer_address: None, + ltk: None, + rng, + phantom: PhantomData::default(), + } + } +} + +#[cfg(feature = "async")] +pub struct AsyncSecurityManager<'a, B, R: CryptoRng> { + ioa: Option, + + skb: Option, + pkb: Option, + + pka: Option, + + confirm: Option, + + na: Option, + nb: Option, + + dh_key: Option, + + mac_key: Option, + eb: Option, + + pub local_address: Option, + pub peer_address: Option, + pub ltk: Option, + + rng: &'a mut R, + phantom: PhantomData, +} + +#[cfg(feature = "async")] +pub trait AsyncBleWriter { + async fn write_bytes(&mut self, bytes: &[u8]); +} + +#[cfg(feature = "async")] +impl AsyncBleWriter for crate::asynch::Ble +where + T: embedded_io_async::Read + embedded_io_async::Write, +{ + async fn write_bytes(&mut self, bytes: &[u8]) { + self.write_bytes(bytes).await + } +} + +#[cfg(feature = "async")] +impl<'a, B, R: CryptoRng> AsyncSecurityManager<'a, B, R> { + pub fn new(rng: &'a mut R) -> Self { + Self { + ioa: None, + skb: None, + pkb: None, + pka: None, + confirm: None, + na: None, + nb: None, + dh_key: None, + mac_key: None, + eb: None, + local_address: None, + peer_address: None, + ltk: None, + rng, + phantom: PhantomData::default(), + } + } +} + +fn make_auth_req() -> AuthReq { + let mut auth_req = AuthReq(0); + auth_req.set_bonding_flags(1); + auth_req.set_mitm(1); + auth_req.set_sc(1); + auth_req.set_keypress(0); + auth_req.set_ct2(1); + auth_req +} + +bleps_dedup::dedup! { +impl<'a, B, R> SYNC SecurityManager<'a, B, R> where B: BleWriter, R: CryptoRng + RngCore +impl<'a, B, R> ASYNC AsyncSecurityManager<'a, B, R> where B: AsyncBleWriter, R: CryptoRng + RngCore + { + pub(crate) async fn handle(&mut self, ble: &mut B, src_handle: u16, payload: crate::Data, pin_callback: &mut Option<&mut dyn FnMut(u32)>) -> Result<(), AttributeServerError> { + log::info!("SM packet {:02x?}", payload.as_slice()); + + let data = &payload.as_slice()[1..]; + let command = payload.as_slice()[0]; + + match command { + SM_PAIRING_REQUEST => { + self.handle_pairing_request(ble, src_handle, data).await; + } + SM_PAIRING_PUBLIC_KEY => { + self.handle_pairing_public_key(ble, src_handle, data).await?; + } + SM_PAIRING_RANDOM => { + self.handle_pairing_random(ble, src_handle, data, pin_callback).await?; + } + SM_PAIRING_DHKEY_CHECK => { + self.handle_pairing_dhkey_check(ble, src_handle, data).await?; + } + _ => { + // handle FAILURE + log::error!("Unknown SM command {}", command); + self.report_error(ble, src_handle, SecurityManagerError::CommandNotSupported).await; + return Err(AttributeServerError::SecurityManagerError); + } + } + + Ok(()) + } + + async fn handle_pairing_request(&mut self, ble: &mut B, src_handle: u16, data: &[u8]) { + self.ioa = Some(IoCap::new(data[2], data[1] != 0, data[0])); + log::info!("got pairing request"); + + let mut data = Data::new(&[SM_PAIRING_RESPONSE]); + data.append_value(IoCapability::DisplayYesNo as u8); + data.append_value(OobDataFlag::NotPresent as u8); + data.append_value(make_auth_req().0); + data.append_value(0x10u8); + data.append_value(0u8); // 3 + data.append_value(0u8); // 3 + + self.write_sm(ble, src_handle, data).await; + } + + async fn handle_pairing_public_key(&mut self, ble: &mut B, src_handle: u16, pka: &[u8]) -> Result<(), AttributeServerError> { + log::info!("got public key"); + + log::info!("key len = {} {:02x?}", pka.len(), pka); + let pka = PublicKey::from_bytes(pka); + + // Send the local public key before validating the remote key to allow + // parallel computation of DHKey. No security risk in doing so. + + let mut data = Data::new(&[SM_PAIRING_PUBLIC_KEY]); + + let skb = SecretKey::new(self.rng); + let pkb = skb.public_key(); + + let mut x = [0u8; 32]; + let mut y = [0u8; 32]; + x.copy_from_slice(pkb.x.as_be_bytes()); + y.copy_from_slice(pkb.y.as_be_bytes()); + x.reverse(); + y.reverse(); + + data.append(&x); + data.append(&y); + self.write_sm(ble, src_handle, data).await; + + let dh_key = match skb.dh_key(pka) { + Some(dh_key) => Ok(dh_key), + None => Err(AttributeServerError::SecurityManagerError), + }?; + + // SUBTLE: The order of these send/recv ops is important. See last + // paragraph of Section 2.3.5.6.2. + let nb = Nonce::new(self.rng); + let cb = nb.f4(pkb.x(), pka.x(), 0); + + let mut data = Data::new(&[SM_PAIRING_CONFIRM]); + let confirm_value = cb.0.to_le_bytes(); + data.append(&confirm_value); + self.write_sm(ble, src_handle, data).await; + + self.pka = Some(pka); + self.pkb = Some(pkb); + self.skb = Some(skb); + self.confirm = Some(cb); + self.nb = Some(nb); + self.dh_key = Some(dh_key); + + Ok(()) + } + + async fn handle_pairing_random(&mut self, ble: &mut B, src_handle: u16, random: &[u8], pin_callback: &mut Option<&mut dyn FnMut(u32)>) -> Result<(), AttributeServerError> { + log::info!("got pairing random {:02x?}", random); + + if *&(self.nb).is_none() { + self.report_error(ble, src_handle, SecurityManagerError::UnspecifiedReason).await; + return Err(AttributeServerError::SecurityManagerError); + } + + if *&(self.pka).is_none() { + self.report_error(ble, src_handle, SecurityManagerError::UnspecifiedReason).await; + return Err(AttributeServerError::SecurityManagerError); + } + + if *&(self.pkb).is_none() { + self.report_error(ble, src_handle, SecurityManagerError::UnspecifiedReason).await; + return Err(AttributeServerError::SecurityManagerError); + } + + if *&(self.peer_address).is_none() { + self.report_error(ble, src_handle, SecurityManagerError::UnspecifiedReason).await; + return Err(AttributeServerError::SecurityManagerError); + } + + if *&(self.local_address).is_none() { + self.report_error(ble, src_handle, SecurityManagerError::UnspecifiedReason).await; + return Err(AttributeServerError::SecurityManagerError); + } + + let mut data = Data::new(&[SM_PAIRING_RANDOM]); + data.append(&self.nb.unwrap().0.to_le_bytes()); + self.write_sm(ble, src_handle, data).await; + + let na = Nonce(u128::from_le_bytes(random.try_into().unwrap())); + self.na = Some(na); + let nb = self.nb.unwrap(); + let vb = na.g2( + self.pka.as_ref().unwrap().x(), + self.pkb.as_ref().unwrap().x(), + &nb, + ); + + // should display the code and get confirmation from user (pin ok or not) - if not okay send a pairing-failed + // assume it's correct or the user will cancel on central + log::info!("Display code is {}", vb.0); + if let Some(pin_callback) = pin_callback { + pin_callback(vb.0); + } + + // Authentication stage 2 and long term key calculation + // ([Vol 3] Part H, Section 2.3.5.6.5 and C.2.2.4). + + let a = self.peer_address.unwrap(); + let b = self.local_address.unwrap(); + let ra = 0; + log::info!("a = {:02x?}", a.0); + log::info!("b = {:02x?}", b.0); + + let io_cap = IoCapability::DisplayYesNo as u8; + let iob = IoCap::new(make_auth_req().0, false, io_cap); + let dh_key = self.dh_key.as_ref().unwrap(); + + let (mac_key, ltk) = dh_key.f5(na, nb, a, b); + let eb = mac_key.f6(nb, na, ra, iob, b, a); + + self.mac_key = Some(mac_key); + self.ltk = Some(ltk.0); + self.eb = Some(eb); + + Ok(()) + } + + async fn handle_pairing_dhkey_check(&mut self, ble: &mut B, src_handle: u16, ea: &[u8]) -> Result<(), AttributeServerError> { + log::info!("got dhkey_check {:02x?}", ea); + + if *&(self.na).is_none() { + self.report_error(ble, src_handle, SecurityManagerError::UnspecifiedReason).await; + return Err(AttributeServerError::SecurityManagerError); + } + + if *&(self.nb).is_none() { + self.report_error(ble, src_handle, SecurityManagerError::UnspecifiedReason).await; + return Err(AttributeServerError::SecurityManagerError); + } + + if *&(self.ioa).is_none() { + self.report_error(ble, src_handle, SecurityManagerError::UnspecifiedReason).await; + return Err(AttributeServerError::SecurityManagerError); + } + + if *&(self.peer_address).is_none() { + self.report_error(ble, src_handle, SecurityManagerError::UnspecifiedReason).await; + return Err(AttributeServerError::SecurityManagerError); + } + + if *&(self.local_address).is_none() { + self.report_error(ble, src_handle, SecurityManagerError::UnspecifiedReason).await; + return Err(AttributeServerError::SecurityManagerError); + } + + let expected = self + .mac_key + .as_ref() + .unwrap() + .f6( + self.na.unwrap(), + self.nb.unwrap(), + 0, + self.ioa.unwrap(), + self.peer_address.unwrap(), + self.local_address.unwrap(), + ) + .0 + .to_le_bytes(); + if ea != expected { + log::warn!("DH check failed"); + } + + let mut data = Data::new(&[SM_PAIRING_DHKEY_CHECK]); + data.append(&self.eb.as_ref().unwrap().0.to_le_bytes()); + self.write_sm(ble, src_handle, data).await; + + Ok(()) + } + + async fn write_sm(&self, ble: &mut B, handle: u16, data: Data) { + log::debug!("data {:x?}", data.as_slice()); + + let res = L2capPacket::encode_sm(data); + log::info!("encoded_l2cap {:x?}", res.as_slice()); + + let res = AclPacket::encode( + handle, + BoundaryFlag::FirstAutoFlushable, + HostBroadcastFlag::NoBroadcast, + res, + ); + + log::info!("writing {:02x?}", res.as_slice()); + ble.write_bytes(res.as_slice()).await; + } + + async fn report_error(&self, ble: &mut B, src_handle: u16, error: SecurityManagerError) { + let mut data = Data::new(&[SM_PAIRING_FAILED]); + data.append(&[error as u8]); + self.write_sm(ble, src_handle, data).await; + } +} +} diff --git a/bleps/tests/test_ble.rs b/bleps/tests/test_ble.rs index 00960e8..30e8a87 100644 --- a/bleps/tests/test_ble.rs +++ b/bleps/tests/test_ble.rs @@ -18,6 +18,7 @@ use bleps::{ l2cap::L2capPacket, Ble, Data, HciConnection, PollResult, }; +use p256::elliptic_curve::rand_core::OsRng; struct TestConnector { to_read: RefCell<[u8; 128]>, @@ -158,17 +159,32 @@ fn init_works() { let connector = connector(); let mut ble = Ble::new(&connector); - connector.provide_data_to_read(&[0x04, 0x0e, 0x04, 0x05, 0x03, 0x0c, 0x00]); + connector.provide_data_to_read(&[ + 0x04, 0x0e, 0x04, 0x05, 0x03, 0x0c, 0x00, 0x04, 0x0e, 0x04, 0x05, 0x01, 0x0c, 0x00, + ]); let res = ble.init(); - assert_matches!(res, Ok(EventType::CommandComplete{ num_packets: 5, opcode: 0x0c03, data}) if data.as_slice() == &[0]); + assert_matches!(res, Ok(())); - assert_eq!(connector.get_write_idx(), 4); + assert_eq!(connector.get_write_idx(), 16); assert_eq!(connector.get_to_write_at(0), 0x01); assert_eq!(connector.get_to_write_at(1), 0x03); assert_eq!(connector.get_to_write_at(2), 0x0c); assert_eq!(connector.get_to_write_at(3), 0x00); + + assert_eq!(connector.get_to_write_at(4), 0x01); + assert_eq!(connector.get_to_write_at(5), 0x01); + assert_eq!(connector.get_to_write_at(6), 0x0c); + assert_eq!(connector.get_to_write_at(7), 0x08); + assert_eq!(connector.get_to_write_at(8), 0xff); + assert_eq!(connector.get_to_write_at(9), 0xff); + assert_eq!(connector.get_to_write_at(10), 0xff); + assert_eq!(connector.get_to_write_at(11), 0xff); + assert_eq!(connector.get_to_write_at(12), 0xff); + assert_eq!(connector.get_to_write_at(13), 0xff); + assert_eq!(connector.get_to_write_at(14), 0xff); + assert_eq!(connector.get_to_write_at(15), 0xff); } #[test] @@ -686,7 +702,9 @@ fn attribute_server_discover_two_services() { custom_char_att_data_attr2, val, ]; - let mut srv = AttributeServer::new(&mut ble, attributes); + + let mut rng = OsRng::default(); + let mut srv = AttributeServer::new(&mut ble, attributes, &mut rng); // ReadByGroupTypeReq { start: 1, end: ffff, group_type: Uuid16(2800) } connector.provide_data_to_read(&[ diff --git a/example/Cargo.toml b/example/Cargo.toml index 83816d6..01f8933 100644 --- a/example/Cargo.toml +++ b/example/Cargo.toml @@ -8,6 +8,7 @@ serialport = "4.2.0" critical-section = "1.1.1" embedded-io-adapters = { version = "0.6.1", features = ["std"] } embedded-io-blocking = { package = "embedded-io", version = "0.6.1" } -bleps = { path = "../bleps", features = ["macros", "async"] } +bleps = { path = "../bleps", features = ["macros", "async", "crypto"] } env_logger = "0.10.0" -crossterm = "0.25.0" +crossterm = "0.27.0" +rand_core = { version = "0.6.4", features = ["std"] } diff --git a/example/src/main.rs b/example/src/main.rs index 135cda1..069aeec 100644 --- a/example/src/main.rs +++ b/example/src/main.rs @@ -11,10 +11,11 @@ use bleps::{ create_advertising_data, AdStructure, BR_EDR_NOT_SUPPORTED, LE_GENERAL_DISCOVERABLE, }, attribute_server::{AttributeServer, NotificationData, WorkResult}, - gatt, Ble, HciConnector, + gatt, Addr, Ble, HciConnector, }; use embedded_io_adapters::std::FromStd; use embedded_io_blocking::{Error, ErrorType, Read, Write}; +use rand_core::OsRng; fn main() { env_logger::init(); @@ -27,7 +28,7 @@ fn main() { let args: Vec = std::env::args().collect(); let port = serialport::new(&args[1], 115_200) - .timeout(Duration::from_millis(100)) + .timeout(Duration::from_millis(300)) .open() .expect("Failed to open port"); @@ -53,12 +54,17 @@ fn main() { crossterm::terminal::enable_raw_mode().unwrap(); + let mut ltk = None; + loop { let connector = BleConnector::new(&mut serial); let hci = HciConnector::new(connector, current_millis); let mut ble = Ble::new(&hci); println!("{:?}", ble.init()); + + let local_addr = Addr::from_le_bytes(false, ble.cmd_read_br_addr().unwrap()); + println!("{:?}", ble.cmd_set_le_advertising_parameters()); println!( "{:?}", @@ -145,7 +151,20 @@ fn main() { ], },]); - let mut srv = AttributeServer::new(&mut ble, &mut gatt_attributes); + let mut rng = OsRng::default(); + let mut srv = AttributeServer::new_with_ltk( + &mut ble, + &mut gatt_attributes, + local_addr, + ltk, + &mut rng, + ); + + let mut pin_callback = |pin: u32| { + println!("PIN is {pin}"); + }; + + srv.set_pin_callback(Some(&mut pin_callback)); let mut response = [b'H', b'e', b'l', b'l', b'o', b'0']; @@ -203,6 +222,8 @@ fn main() { } } } + + ltk = srv.get_ltk(); } }