use crate::mailbox::{Mailbox, EXPANDER_BT_ON};
use crate::timer::Timer;
use crate::uart::Uart;
pub mod bredr;
pub mod gatt;
pub mod gatt_client;
pub mod hid_host;
pub mod l2cap;
pub mod sdp;
pub mod smp;
pub const H4_COMMAND: u8 = 0x01;
pub const H4_ACL: u8 = 0x02;
pub const H4_EVENT: u8 = 0x04;
const EVT_COMMAND_COMPLETE: u8 = 0x0e;
const EVT_COMMAND_STATUS: u8 = 0x0f;
const EVT_DISCONNECTION_COMPLETE: u8 = 0x05;
const EVT_NUM_COMPLETED_PACKETS: u8 = 0x13;
const EVT_ENCRYPTION_CHANGE: u8 = 0x08;
const EVT_INQUIRY_COMPLETE: u8 = 0x01;
const EVT_INQUIRY_RESULT: u8 = 0x02;
const EVT_INQUIRY_RESULT_WITH_RSSI: u8 = 0x22;
const EVT_EXTENDED_INQUIRY_RESULT: u8 = 0x2f;
const EVT_CONNECTION_COMPLETE: u8 = 0x03;
const EVT_AUTHENTICATION_COMPLETE: u8 = 0x06;
const EVT_LINK_KEY_REQUEST: u8 = 0x17;
const EVT_LINK_KEY_NOTIFICATION: u8 = 0x18;
const EVT_IO_CAPABILITY_REQUEST: u8 = 0x31;
const EVT_USER_CONFIRMATION_REQUEST: u8 = 0x33;
const EVT_SIMPLE_PAIRING_COMPLETE: u8 = 0x36;
const OP_RESET: u16 = 0x0c03;
const OP_READ_LOCAL_VERSION: u16 = 0x1001;
const OP_READ_BD_ADDR: u16 = 0x1009;
const OP_READ_BUFFER_SIZE: u16 = 0x1005;
const OP_DISCONNECT: u16 = 0x0406;
const OP_LE_READ_BUFFER_SIZE: u16 = 0x2002;
const OP_LE_ENCRYPT: u16 = 0x2017;
const OP_LE_RAND: u16 = 0x2018;
const OP_LE_START_ENCRYPTION: u16 = 0x2019;
const OP_LE_LTK_REQUEST_REPLY: u16 = 0x201a;
const OP_LE_LTK_REQUEST_NEG_REPLY: u16 = 0x201b;
const OP_BCM_DOWNLOAD_MINIDRIVER: u16 = 0xfc2e;
const OP_LE_SET_ADV_PARAMS: u16 = 0x2006;
const OP_LE_SET_ADV_DATA: u16 = 0x2008;
const OP_LE_SET_ADV_ENABLE: u16 = 0x200a;
const OP_LE_SET_SCAN_PARAMS: u16 = 0x200b;
const OP_LE_SET_SCAN_ENABLE: u16 = 0x200c;
const OP_LE_CREATE_CONNECTION: u16 = 0x200d;
const OP_INQUIRY: u16 = 0x0401;
const OP_INQUIRY_CANCEL: u16 = 0x0402;
const OP_WRITE_INQUIRY_MODE: u16 = 0x0c45;
const OP_CREATE_CONNECTION: u16 = 0x0405;
const OP_AUTHENTICATION_REQUESTED: u16 = 0x0411;
const OP_SET_CONNECTION_ENCRYPTION: u16 = 0x0413;
const OP_LINK_KEY_REQUEST_NEGATIVE_REPLY: u16 = 0x040c;
const OP_IO_CAPABILITY_REQUEST_REPLY: u16 = 0x042b;
const OP_USER_CONFIRMATION_REQUEST_REPLY: u16 = 0x042c;
const OP_SET_EVENT_MASK: u16 = 0x0c01;
const OP_WRITE_SIMPLE_PAIRING_MODE: u16 = 0x0c56;
const OP_LE_CREATE_CONNECTION_CANCEL: u16 = 0x200e;
const EVT_LE_META: u8 = 0x3e;
const LE_SUBEVENT_CONNECTION_COMPLETE: u8 = 0x01;
const LE_SUBEVENT_LTK_REQUEST: u8 = 0x05;
const LE_SUBEVENT_ADV_REPORT: u8 = 0x02;
const CONN_HANDLE_MASK: u16 = 0x0fff;
const ACL_PB_FIRST_NON_FLUSH: u16 = 0x00;
const ACL_PB_CONTINUATION: u16 = 0x01;
const MAX_ACL_DATA: usize = 255;
const OP_BCM_UPDATE_BAUDRATE: u16 = 0xfc18;
const BT_ON_SETTLE_MS: u32 = 150;
const MINIDRIVER_SETTLE_MS: u32 = 50;
const LAUNCH_SETTLE_MS: u32 = 250;
const BAUD_SETTLE_MS: u32 = 10;
const COMMAND_TIMEOUT_MS: u32 = 1000;
const MAX_EVENT_PARAMS: usize = 255;
const ADV_INTERVAL: u16 = 0x00a0;
const ADV_CHANNEL_ALL: u8 = 0x07;
const AD_TYPE_FLAGS: u8 = 0x01;
const AD_TYPE_NAME_SHORT: u8 = 0x08;
const AD_TYPE_NAME_COMPLETE: u8 = 0x09;
const AD_FLAGS_LE_GENERAL: u8 = 0x06;
const ADV_DATA_MAX: usize = 31;
const SCAN_TYPE_ACTIVE: u8 = 0x01;
const SCAN_INTERVAL: u16 = 0x0010;
const SCAN_WINDOW: u16 = 0x0010;
const GIAC_LAP: [u8; 3] = [0x33, 0x8b, 0x9e];
const INQUIRY_LENGTH: u8 = 0x08;
const INQUIRY_UNLIMITED_RESPONSES: u8 = 0x00;
const INQUIRY_MODE_EXTENDED: u8 = 0x02;
const EIR_NAME_MAX: usize = 32;
const COD_MAJOR_PERIPHERAL: u8 = 0x05;
const CLASSIC_PACKET_TYPES: u16 = 0xcc18;
const PAGE_SCAN_REPETITION_R1: u8 = 0x01;
const IO_CAP_NO_INPUT_NO_OUTPUT: u8 = 0x03;
const OOB_DATA_NONE: u8 = 0x00;
const AUTH_REQ_GENERAL_BONDING: u8 = 0x04;
const CLASSIC_ENCRYPTION_ON: u8 = 0x01;
const EVENT_MASK_ALL: u64 = 0xffff_ffff_ffff_ffff;
const CLASSIC_CONNECT_TIMEOUT_MS: u32 = 15_000;
const CLASSIC_PAIR_TIMEOUT_MS: u32 = 15_000;
const CLASSIC_ENCRYPT_TIMEOUT_MS: u32 = 5_000;
const CONN_SCAN_INTERVAL: u16 = 0x0060;
const CONN_SCAN_WINDOW: u16 = 0x0030;
const CONN_INTERVAL_MIN: u16 = 0x0018;
const CONN_INTERVAL_MAX: u16 = 0x0028;
const CONN_LATENCY: u16 = 0x0000;
const CONN_SUPERVISION_TIMEOUT: u16 = 0x00c8;
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum Advertising {
Connectable,
NonConnectable,
}
impl Advertising {
fn adv_type(self) -> u8 {
match self {
Advertising::Connectable => 0x00,
Advertising::NonConnectable => 0x03,
}
}
}
#[derive(Clone, Copy, Debug)]
pub struct AdvReport {
pub event_type: u8,
pub address_type: u8,
pub address: [u8; 6],
pub rssi: i8,
data_buf: [u8; ADV_DATA_MAX],
data_len: u8,
}
impl AdvReport {
pub fn data(&self) -> &[u8] {
&self.data_buf[..self.data_len as usize]
}
pub fn name(&self) -> Option<&str> {
let data = self.data();
let bytes = find_ad_structure(data, AD_TYPE_NAME_COMPLETE)
.or_else(|| find_ad_structure(data, AD_TYPE_NAME_SHORT))?;
core::str::from_utf8(bytes).ok()
}
}
#[derive(Clone, Copy, Debug)]
pub struct InquiryResult {
pub bd_addr: [u8; 6],
pub class_of_device: [u8; 3],
pub rssi: Option<i8>,
name_buf: [u8; EIR_NAME_MAX],
name_len: u8,
}
impl InquiryResult {
pub fn name(&self) -> Option<&str> {
if self.name_len == 0 {
return None;
}
core::str::from_utf8(&self.name_buf[..self.name_len as usize]).ok()
}
pub fn class_of_device_u24(&self) -> u32 {
let c = self.class_of_device;
u32::from(c[0]) | (u32::from(c[1]) << 8) | (u32::from(c[2]) << 16)
}
pub fn major_device_class(&self) -> u8 {
((self.class_of_device_u24() >> 8) & 0x1f) as u8
}
pub fn is_gamepad(&self) -> bool {
let minor_kind = ((self.class_of_device_u24() >> 2) & 0x0f) as u8;
self.major_device_class() == COD_MAJOR_PERIPHERAL
&& (minor_kind == 0x01 || minor_kind == 0x02)
}
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum Role {
Central,
Peripheral,
}
impl Role {
fn from_byte(byte: u8) -> Self {
match byte {
0x00 => Role::Central,
_ => Role::Peripheral,
}
}
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub struct Connection {
pub handle: u16,
pub role: Role,
pub peer_address_type: u8,
pub peer_address: [u8; 6],
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub struct AclData {
pub handle: u16,
pub first_fragment: bool,
data_buf: [u8; MAX_ACL_DATA],
data_len: u16,
}
impl AclData {
pub fn data(&self) -> &[u8] {
&self.data_buf[..self.data_len as usize]
}
}
#[allow(clippy::large_enum_variant)]
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum Event {
Connected(Connection),
Disconnected {
handle: u16,
reason: u8,
},
Acl(AclData),
LongTermKeyRequest {
handle: u16,
ediv: u16,
rand: [u8; 8],
},
EncryptionChange {
handle: u16,
enabled: bool,
},
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum Error {
Timeout,
CommandFailed {
opcode: u16,
status: u8,
},
ShortResponse,
UnsupportedBaud(u32),
BadFirmware,
NoAclBuffers,
PayloadTooLarge,
CryptoSelfTest,
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub struct LocalVersion {
pub hci_version: u8,
pub hci_revision: u16,
pub lmp_version: u8,
pub manufacturer: u16,
pub lmp_subversion: u16,
}
pub struct Bluetooth {
uart: Uart,
acl_packet_len: u16,
acl_credits: u16,
}
enum Packet<'a> {
Event {
code: u8,
params: &'a [u8],
},
Acl {
handle: u16,
first_fragment: bool,
data: &'a [u8],
},
}
impl Bluetooth {
pub fn new(uart: Uart, mailbox: &mut Mailbox, timer: &Timer) -> Self {
let _ = mailbox.set_expander_gpio(EXPANDER_BT_ON, true);
timer.delay_ms(BT_ON_SETTLE_MS);
Self {
uart,
acl_packet_len: 0,
acl_credits: 0,
}
}
pub fn load_firmware(&mut self, hcd: &[u8], timer: &Timer) -> Result<(), Error> {
self.reset(timer)?;
self.command(OP_BCM_DOWNLOAD_MINIDRIVER, &[], &mut [], timer)?;
timer.delay_ms(MINIDRIVER_SETTLE_MS);
let mut offset = 0;
let mut discard = [0u8; MAX_EVENT_PARAMS];
while offset + 3 <= hcd.len() {
let opcode = u16::from_le_bytes([hcd[offset], hcd[offset + 1]]);
let len = hcd[offset + 2] as usize;
let params_start = offset + 3;
let params_end = params_start + len;
if params_end > hcd.len() {
return Err(Error::BadFirmware);
}
self.command(opcode, &hcd[params_start..params_end], &mut discard, timer)?;
offset = params_end;
}
if offset != hcd.len() {
return Err(Error::BadFirmware);
}
timer.delay_ms(LAUNCH_SETTLE_MS);
self.reset(timer)?;
Ok(())
}
pub fn set_baud(&mut self, baud: u32, timer: &Timer) -> Result<(), Error> {
let mut params = [0u8; 6];
params[2..6].copy_from_slice(&baud.to_le_bytes());
self.command(OP_BCM_UPDATE_BAUDRATE, ¶ms, &mut [], timer)?;
if !self.uart.set_baud(baud) {
return Err(Error::UnsupportedBaud(baud));
}
timer.delay_ms(BAUD_SETTLE_MS);
Ok(())
}
pub fn start_advertising(
&mut self,
name: &str,
kind: Advertising,
timer: &Timer,
) -> Result<(), Error> {
let mut adv_data = [0u8; ADV_DATA_MAX];
let len = build_adv_data(name, &mut adv_data);
self.advertise(&adv_data[..len], kind, timer)
}
pub fn start_advertising_raw(
&mut self,
adv_data: &[u8],
kind: Advertising,
timer: &Timer,
) -> Result<(), Error> {
self.advertise(adv_data, kind, timer)
}
fn advertise(
&mut self,
adv_data: &[u8],
kind: Advertising,
timer: &Timer,
) -> Result<(), Error> {
let mut params = [0u8; 15];
params[0..2].copy_from_slice(&ADV_INTERVAL.to_le_bytes());
params[2..4].copy_from_slice(&ADV_INTERVAL.to_le_bytes());
params[4] = kind.adv_type();
params[13] = ADV_CHANNEL_ALL;
self.command(OP_LE_SET_ADV_PARAMS, ¶ms, &mut [], timer)?;
let len = adv_data.len().min(ADV_DATA_MAX);
let mut data = [0u8; 1 + ADV_DATA_MAX];
data[0] = len as u8;
data[1..1 + len].copy_from_slice(&adv_data[..len]);
self.command(OP_LE_SET_ADV_DATA, &data, &mut [], timer)?;
self.command(OP_LE_SET_ADV_ENABLE, &[0x01], &mut [], timer)?;
Ok(())
}
pub fn stop_advertising(&mut self, timer: &Timer) -> Result<(), Error> {
self.command(OP_LE_SET_ADV_ENABLE, &[0x00], &mut [], timer)?;
Ok(())
}
pub fn start_scan(&mut self, timer: &Timer) -> Result<(), Error> {
let mut params = [0u8; 7];
params[0] = SCAN_TYPE_ACTIVE;
params[1..3].copy_from_slice(&SCAN_INTERVAL.to_le_bytes());
params[3..5].copy_from_slice(&SCAN_WINDOW.to_le_bytes());
self.command(OP_LE_SET_SCAN_PARAMS, ¶ms, &mut [], timer)?;
self.command(OP_LE_SET_SCAN_ENABLE, &[0x01, 0x01], &mut [], timer)?;
Ok(())
}
pub fn stop_scan(&mut self, timer: &Timer) -> Result<(), Error> {
self.command(OP_LE_SET_SCAN_ENABLE, &[0x00, 0x00], &mut [], timer)?;
Ok(())
}
pub fn set_inquiry_mode_extended(&mut self, timer: &Timer) -> Result<(), Error> {
self.command(
OP_WRITE_INQUIRY_MODE,
&[INQUIRY_MODE_EXTENDED],
&mut [],
timer,
)?;
Ok(())
}
pub fn start_inquiry(&mut self, timer: &Timer) -> Result<(), Error> {
self.issue_inquiry(timer)
}
fn issue_inquiry(&mut self, timer: &Timer) -> Result<(), Error> {
let mut params = [0u8; 5];
params[0..3].copy_from_slice(&GIAC_LAP);
params[3] = INQUIRY_LENGTH;
params[4] = INQUIRY_UNLIMITED_RESPONSES;
self.command(OP_INQUIRY, ¶ms, &mut [], timer)?;
Ok(())
}
pub fn inquiry_cancel(&mut self, timer: &Timer) -> Result<(), Error> {
self.command(OP_INQUIRY_CANCEL, &[], &mut [], timer)?;
Ok(())
}
pub fn next_inquiry_result(
&mut self,
timer: &Timer,
timeout_ms: u32,
) -> Result<Option<InquiryResult>, Error> {
let deadline = timer.now_micros() + (timeout_ms as u64) * 1000;
let mut buf = [0u8; MAX_EVENT_PARAMS];
loop {
let (code, params) = match self.read_packet(&mut buf, timer, deadline) {
Ok(Packet::Event { code, params }) => (code, params),
Ok(Packet::Acl { .. }) => continue,
Err(Error::Timeout) => return Ok(None),
Err(other) => return Err(other),
};
match code {
EVT_INQUIRY_COMPLETE => {
self.issue_inquiry(timer)?;
}
EVT_INQUIRY_RESULT | EVT_INQUIRY_RESULT_WITH_RSSI | EVT_EXTENDED_INQUIRY_RESULT => {
if let Some(result) = parse_inquiry_result(code, params) {
return Ok(Some(result));
}
}
_ => {}
}
}
}
pub fn set_event_mask(&mut self, mask: u64, timer: &Timer) -> Result<(), Error> {
self.command(OP_SET_EVENT_MASK, &mask.to_le_bytes(), &mut [], timer)?;
Ok(())
}
pub fn set_simple_pairing_mode(&mut self, enabled: bool, timer: &Timer) -> Result<(), Error> {
self.command(
OP_WRITE_SIMPLE_PAIRING_MODE,
&[enabled as u8],
&mut [],
timer,
)?;
Ok(())
}
pub fn classic_connect(&mut self, bd_addr: &[u8; 6], timer: &Timer) -> Result<u16, Error> {
let mut params = [0u8; 13];
params[0..6].copy_from_slice(bd_addr);
params[6..8].copy_from_slice(&CLASSIC_PACKET_TYPES.to_le_bytes());
params[8] = PAGE_SCAN_REPETITION_R1;
params[12] = 0x01; self.command(OP_CREATE_CONNECTION, ¶ms, &mut [], timer)?;
let deadline = timer.now_micros() + (CLASSIC_CONNECT_TIMEOUT_MS as u64) * 1000;
let mut buf = [0u8; MAX_EVENT_PARAMS];
loop {
let (code, params) = match self.read_packet(&mut buf, timer, deadline)? {
Packet::Event { code, params } => (code, params),
Packet::Acl { .. } => continue,
};
if code == EVT_CONNECTION_COMPLETE && params.len() >= 3 {
if params[0] != 0 {
return Err(Error::CommandFailed {
opcode: OP_CREATE_CONNECTION,
status: params[0],
});
}
return Ok(u16::from_le_bytes([params[1], params[2]]) & CONN_HANDLE_MASK);
}
}
}
pub fn classic_pair(
&mut self,
bd_addr: &[u8; 6],
handle: u16,
timer: &Timer,
) -> Result<[u8; 16], Error> {
self.set_event_mask(EVENT_MASK_ALL, timer)?;
self.command(
OP_AUTHENTICATION_REQUESTED,
&(handle & CONN_HANDLE_MASK).to_le_bytes(),
&mut [],
timer,
)?;
let deadline = timer.now_micros() + (CLASSIC_PAIR_TIMEOUT_MS as u64) * 1000;
let mut buf = [0u8; MAX_EVENT_PARAMS];
let mut link_key = [0u8; 16];
loop {
let (code, params) = match self.read_packet(&mut buf, timer, deadline)? {
Packet::Event { code, params } => (code, params),
Packet::Acl { .. } => continue,
};
match code {
EVT_LINK_KEY_REQUEST => {
self.send_command(OP_LINK_KEY_REQUEST_NEGATIVE_REPLY, bd_addr);
}
EVT_IO_CAPABILITY_REQUEST => {
let mut reply = [0u8; 9];
reply[0..6].copy_from_slice(bd_addr);
reply[6] = IO_CAP_NO_INPUT_NO_OUTPUT;
reply[7] = OOB_DATA_NONE;
reply[8] = AUTH_REQ_GENERAL_BONDING;
self.send_command(OP_IO_CAPABILITY_REQUEST_REPLY, &reply);
}
EVT_USER_CONFIRMATION_REQUEST => {
self.send_command(OP_USER_CONFIRMATION_REQUEST_REPLY, bd_addr);
}
EVT_LINK_KEY_NOTIFICATION if params.len() >= 22 => {
link_key.copy_from_slice(¶ms[6..22]);
}
EVT_SIMPLE_PAIRING_COMPLETE if !params.is_empty() => {
if params[0] != 0 {
return Err(Error::CommandFailed {
opcode: OP_AUTHENTICATION_REQUESTED,
status: params[0],
});
}
}
EVT_AUTHENTICATION_COMPLETE if !params.is_empty() => {
if params[0] != 0 {
return Err(Error::CommandFailed {
opcode: OP_AUTHENTICATION_REQUESTED,
status: params[0],
});
}
return Ok(link_key);
}
_ => {}
}
}
}
pub fn classic_set_encryption(&mut self, handle: u16, timer: &Timer) -> Result<(), Error> {
let mut params = [0u8; 3];
params[0..2].copy_from_slice(&(handle & CONN_HANDLE_MASK).to_le_bytes());
params[2] = CLASSIC_ENCRYPTION_ON;
self.command(OP_SET_CONNECTION_ENCRYPTION, ¶ms, &mut [], timer)?;
let deadline = timer.now_micros() + (CLASSIC_ENCRYPT_TIMEOUT_MS as u64) * 1000;
let mut buf = [0u8; MAX_EVENT_PARAMS];
loop {
let (code, params) = match self.read_packet(&mut buf, timer, deadline)? {
Packet::Event { code, params } => (code, params),
Packet::Acl { .. } => continue,
};
if code == EVT_ENCRYPTION_CHANGE && params.len() >= 4 {
let ev_handle = u16::from_le_bytes([params[1], params[2]]) & CONN_HANDLE_MASK;
if ev_handle != (handle & CONN_HANDLE_MASK) {
continue;
}
if params[0] != 0 || params[3] == 0 {
return Err(Error::CommandFailed {
opcode: OP_SET_CONNECTION_ENCRYPTION,
status: params[0],
});
}
return Ok(());
}
}
}
pub fn connect(
&mut self,
peer_address_type: u8,
peer_address: &[u8; 6],
timer: &Timer,
) -> Result<(), Error> {
let mut params = [0u8; 25];
params[0..2].copy_from_slice(&CONN_SCAN_INTERVAL.to_le_bytes());
params[2..4].copy_from_slice(&CONN_SCAN_WINDOW.to_le_bytes());
params[4] = 0x00; params[5] = peer_address_type;
params[6..12].copy_from_slice(peer_address);
params[12] = 0x00; params[13..15].copy_from_slice(&CONN_INTERVAL_MIN.to_le_bytes());
params[15..17].copy_from_slice(&CONN_INTERVAL_MAX.to_le_bytes());
params[17..19].copy_from_slice(&CONN_LATENCY.to_le_bytes());
params[19..21].copy_from_slice(&CONN_SUPERVISION_TIMEOUT.to_le_bytes());
self.command(OP_LE_CREATE_CONNECTION, ¶ms, &mut [], timer)?;
Ok(())
}
pub fn connect_cancel(&mut self, timer: &Timer) -> Result<(), Error> {
self.command(OP_LE_CREATE_CONNECTION_CANCEL, &[], &mut [], timer)?;
Ok(())
}
pub fn next_advertising_report(
&mut self,
timer: &Timer,
timeout_ms: u32,
) -> Result<Option<AdvReport>, Error> {
let deadline = timer.now_micros() + (timeout_ms as u64) * 1000;
let mut buf = [0u8; MAX_EVENT_PARAMS];
loop {
let (code, params) = match self.read_packet(&mut buf, timer, deadline) {
Ok(Packet::Event { code, params }) => (code, params),
Ok(Packet::Acl { .. }) => continue,
Err(Error::Timeout) => return Ok(None),
Err(other) => return Err(other),
};
if code != EVT_LE_META || params.first() != Some(&LE_SUBEVENT_ADV_REPORT) {
continue;
}
if params.get(1) != Some(&1) {
continue;
}
let report = ¶ms[2..];
if report.len() < 10 {
continue;
}
let data = &report[9..report.len() - 1];
let rssi = report[report.len() - 1] as i8;
let copy = data.len().min(ADV_DATA_MAX);
let mut data_buf = [0u8; ADV_DATA_MAX];
data_buf[..copy].copy_from_slice(&data[..copy]);
let mut address = [0u8; 6];
address.copy_from_slice(&report[2..8]);
return Ok(Some(AdvReport {
event_type: report[0],
address_type: report[1],
address,
rssi,
data_buf,
data_len: copy as u8,
}));
}
}
pub fn le_read_buffer_size(&mut self, timer: &Timer) -> Result<(u16, u8), Error> {
let mut ret = [0u8; 3];
let n = self.command(OP_LE_READ_BUFFER_SIZE, &[], &mut ret, timer)?;
if n < ret.len() {
return Err(Error::ShortResponse);
}
let packet_len = u16::from_le_bytes([ret[0], ret[1]]);
let total_packets = ret[2];
self.acl_packet_len = packet_len;
self.acl_credits = total_packets as u16;
Ok((packet_len, total_packets))
}
pub fn read_buffer_size(&mut self, timer: &Timer) -> Result<(u16, u16), Error> {
let mut ret = [0u8; 7];
let n = self.command(OP_READ_BUFFER_SIZE, &[], &mut ret, timer)?;
if n < ret.len() {
return Err(Error::ShortResponse);
}
let packet_len = u16::from_le_bytes([ret[0], ret[1]]);
let total_packets = u16::from_le_bytes([ret[3], ret[4]]);
self.acl_packet_len = packet_len;
self.acl_credits = total_packets;
Ok((packet_len, total_packets))
}
pub fn le_encrypt(
&mut self,
key: &[u8; 16],
plaintext: &[u8; 16],
timer: &Timer,
) -> Result<[u8; 16], Error> {
let mut params = [0u8; 32];
params[..16].copy_from_slice(key);
params[16..].copy_from_slice(plaintext);
let mut ret = [0u8; 16];
let n = self.command(OP_LE_ENCRYPT, ¶ms, &mut ret, timer)?;
if n < ret.len() {
return Err(Error::ShortResponse);
}
Ok(ret)
}
pub fn le_rand(&mut self, timer: &Timer) -> Result<[u8; 8], Error> {
let mut ret = [0u8; 8];
let n = self.command(OP_LE_RAND, &[], &mut ret, timer)?;
if n < ret.len() {
return Err(Error::ShortResponse);
}
Ok(ret)
}
pub fn le_start_encryption(
&mut self,
handle: u16,
rand: [u8; 8],
ediv: u16,
ltk: &[u8; 16],
timer: &Timer,
) -> Result<(), Error> {
let mut params = [0u8; 28];
params[0..2].copy_from_slice(&(handle & CONN_HANDLE_MASK).to_le_bytes());
params[2..10].copy_from_slice(&rand);
params[10..12].copy_from_slice(&ediv.to_le_bytes());
params[12..28].copy_from_slice(ltk);
self.command(OP_LE_START_ENCRYPTION, ¶ms, &mut [], timer)?;
Ok(())
}
pub fn le_ltk_request_reply(
&mut self,
handle: u16,
ltk: &[u8; 16],
timer: &Timer,
) -> Result<(), Error> {
let mut params = [0u8; 18];
params[0..2].copy_from_slice(&(handle & CONN_HANDLE_MASK).to_le_bytes());
params[2..18].copy_from_slice(ltk);
self.command(OP_LE_LTK_REQUEST_REPLY, ¶ms, &mut [], timer)?;
Ok(())
}
pub fn le_ltk_request_negative_reply(
&mut self,
handle: u16,
timer: &Timer,
) -> Result<(), Error> {
let params = (handle & CONN_HANDLE_MASK).to_le_bytes();
self.command(OP_LE_LTK_REQUEST_NEG_REPLY, ¶ms, &mut [], timer)?;
Ok(())
}
pub fn send_acl(&mut self, handle: u16, data: &[u8]) -> Result<(), Error> {
let frag_len = if self.acl_packet_len == 0 {
data.len().max(1)
} else {
self.acl_packet_len as usize
};
let fragments = data.len().div_ceil(frag_len).max(1);
if (fragments as u16) > self.acl_credits {
return Err(Error::NoAclBuffers);
}
let mut offset = 0;
let mut first = true;
while offset < data.len() || first {
let end = (offset + frag_len).min(data.len());
let chunk = &data[offset..end];
let pb = if first {
ACL_PB_FIRST_NON_FLUSH
} else {
ACL_PB_CONTINUATION
};
self.send_acl_fragment(handle, pb, chunk);
self.acl_credits -= 1;
offset = end;
first = false;
}
self.uart.flush();
Ok(())
}
fn send_acl_fragment(&mut self, handle: u16, pb: u16, data: &[u8]) {
let handle_flags = (handle & CONN_HANDLE_MASK) | (pb << 12);
let handle_flags = handle_flags.to_le_bytes();
let len = (data.len() as u16).to_le_bytes();
self.uart.write_byte(H4_ACL);
self.uart.write_byte(handle_flags[0]);
self.uart.write_byte(handle_flags[1]);
self.uart.write_byte(len[0]);
self.uart.write_byte(len[1]);
for &byte in data {
self.uart.write_byte(byte);
}
}
pub fn poll(&mut self, timer: &Timer, timeout_ms: u32) -> Result<Option<Event>, Error> {
let deadline = timer.now_micros() + (timeout_ms as u64) * 1000;
let mut buf = [0u8; MAX_ACL_DATA];
loop {
let packet = match self.read_packet(&mut buf, timer, deadline) {
Ok(packet) => packet,
Err(Error::Timeout) => return Ok(None),
Err(other) => return Err(other),
};
match packet {
Packet::Acl {
handle,
first_fragment,
data,
} => {
let mut data_buf = [0u8; MAX_ACL_DATA];
let copy = data.len().min(MAX_ACL_DATA);
data_buf[..copy].copy_from_slice(&data[..copy]);
return Ok(Some(Event::Acl(AclData {
handle,
first_fragment,
data_buf,
data_len: copy as u16,
})));
}
Packet::Event { code, params } => {
match code {
EVT_LE_META if params.first() == Some(&LE_SUBEVENT_CONNECTION_COMPLETE) => {
if params.len() < 12 || params[1] != 0 {
continue;
}
let handle =
u16::from_le_bytes([params[2], params[3]]) & CONN_HANDLE_MASK;
let mut peer_address = [0u8; 6];
peer_address.copy_from_slice(¶ms[6..12]);
return Ok(Some(Event::Connected(Connection {
handle,
role: Role::from_byte(params[4]),
peer_address_type: params[5],
peer_address,
})));
}
EVT_LE_META if params.first() == Some(&LE_SUBEVENT_LTK_REQUEST) => {
if params.len() < 13 {
continue;
}
let handle =
u16::from_le_bytes([params[1], params[2]]) & CONN_HANDLE_MASK;
let mut rand = [0u8; 8];
rand.copy_from_slice(¶ms[3..11]);
let ediv = u16::from_le_bytes([params[11], params[12]]);
return Ok(Some(Event::LongTermKeyRequest { handle, ediv, rand }));
}
EVT_DISCONNECTION_COMPLETE if params.len() >= 4 => {
let handle =
u16::from_le_bytes([params[1], params[2]]) & CONN_HANDLE_MASK;
return Ok(Some(Event::Disconnected {
handle,
reason: params[3],
}));
}
EVT_ENCRYPTION_CHANGE if params.len() >= 4 => {
let handle =
u16::from_le_bytes([params[1], params[2]]) & CONN_HANDLE_MASK;
return Ok(Some(Event::EncryptionChange {
handle,
enabled: params[3] != 0,
}));
}
EVT_NUM_COMPLETED_PACKETS => {
self.credit_completed_packets(params);
continue;
}
_ => continue,
}
}
}
}
}
fn credit_completed_packets(&mut self, params: &[u8]) {
let Some(&num_handles) = params.first() else {
return;
};
for i in 0..num_handles as usize {
let base = 1 + i * 4;
if base + 4 > params.len() {
break;
}
let completed = u16::from_le_bytes([params[base + 2], params[base + 3]]);
self.acl_credits = self.acl_credits.saturating_add(completed);
}
}
pub fn disconnect(&mut self, handle: u16, reason: u8, timer: &Timer) -> Result<(), Error> {
let mut params = [0u8; 3];
params[0..2].copy_from_slice(&(handle & CONN_HANDLE_MASK).to_le_bytes());
params[2] = reason;
self.command(OP_DISCONNECT, ¶ms, &mut [], timer)?;
Ok(())
}
pub fn reset(&mut self, timer: &Timer) -> Result<(), Error> {
self.command(OP_RESET, &[], &mut [], timer)?;
Ok(())
}
pub fn read_local_version(&mut self, timer: &Timer) -> Result<LocalVersion, Error> {
let mut ret = [0u8; 8];
let n = self.command(OP_READ_LOCAL_VERSION, &[], &mut ret, timer)?;
if n < ret.len() {
return Err(Error::ShortResponse);
}
Ok(LocalVersion {
hci_version: ret[0],
hci_revision: u16::from_le_bytes([ret[1], ret[2]]),
lmp_version: ret[3],
manufacturer: u16::from_le_bytes([ret[4], ret[5]]),
lmp_subversion: u16::from_le_bytes([ret[6], ret[7]]),
})
}
pub fn read_bd_addr(&mut self, timer: &Timer) -> Result<[u8; 6], Error> {
let mut ret = [0u8; 6];
let n = self.command(OP_READ_BD_ADDR, &[], &mut ret, timer)?;
if n < ret.len() {
return Err(Error::ShortResponse);
}
Ok(ret)
}
pub fn free(self) -> Uart {
self.uart
}
fn command(
&mut self,
opcode: u16,
params: &[u8],
ret: &mut [u8],
timer: &Timer,
) -> Result<usize, Error> {
self.send_command(opcode, params);
let deadline = timer.now_micros() + (COMMAND_TIMEOUT_MS as u64) * 1000;
let mut buf = [0u8; MAX_EVENT_PARAMS];
loop {
let (code, params) = match self.read_packet(&mut buf, timer, deadline)? {
Packet::Event { code, params } => (code, params),
Packet::Acl { .. } => continue,
};
match code {
EVT_COMMAND_COMPLETE if params.len() >= 3 => {
if u16::from_le_bytes([params[1], params[2]]) != opcode {
continue;
}
let status = *params.get(3).ok_or(Error::ShortResponse)?;
if status != 0 {
return Err(Error::CommandFailed { opcode, status });
}
let payload = ¶ms[4..];
let copied = payload.len().min(ret.len());
ret[..copied].copy_from_slice(&payload[..copied]);
return Ok(payload.len());
}
EVT_COMMAND_STATUS if params.len() >= 4 => {
if u16::from_le_bytes([params[2], params[3]]) != opcode {
continue;
}
if params[0] != 0 {
return Err(Error::CommandFailed {
opcode,
status: params[0],
});
}
return Ok(0);
}
_ => continue,
}
}
}
fn send_command(&mut self, opcode: u16, params: &[u8]) {
let opcode = opcode.to_le_bytes();
self.uart.write_byte(H4_COMMAND);
self.uart.write_byte(opcode[0]);
self.uart.write_byte(opcode[1]);
self.uart.write_byte(params.len() as u8);
for &byte in params {
self.uart.write_byte(byte);
}
self.uart.flush();
}
fn read_packet<'a>(
&mut self,
buf: &'a mut [u8],
timer: &Timer,
deadline: u64,
) -> Result<Packet<'a>, Error> {
loop {
match self.read_byte_by(timer, deadline)? {
H4_EVENT => {
let code = self.read_byte_by(timer, deadline)?;
let len = self.read_byte_by(timer, deadline)? as usize;
let params = self.read_payload(buf, len, timer, deadline)?;
return Ok(Packet::Event { code, params });
}
H4_ACL => {
let lo = self.read_byte_by(timer, deadline)?;
let hi = self.read_byte_by(timer, deadline)?;
let handle_flags = u16::from_le_bytes([lo, hi]);
let len_lo = self.read_byte_by(timer, deadline)?;
let len_hi = self.read_byte_by(timer, deadline)?;
let len = u16::from_le_bytes([len_lo, len_hi]) as usize;
let data = self.read_payload(buf, len, timer, deadline)?;
return Ok(Packet::Acl {
handle: handle_flags & CONN_HANDLE_MASK,
first_fragment: (handle_flags >> 12) & 0x03 != ACL_PB_CONTINUATION,
data,
});
}
_ => continue,
}
}
}
fn read_payload<'a>(
&mut self,
buf: &'a mut [u8],
len: usize,
timer: &Timer,
deadline: u64,
) -> Result<&'a [u8], Error> {
let stored = len.min(buf.len());
for slot in buf[..stored].iter_mut() {
*slot = self.read_byte_by(timer, deadline)?;
}
for _ in stored..len {
self.read_byte_by(timer, deadline)?;
}
Ok(&buf[..stored])
}
fn read_byte_by(&mut self, timer: &Timer, deadline: u64) -> Result<u8, Error> {
loop {
if let Some(byte) = self.uart.try_read_byte() {
return Ok(byte);
}
if timer.now_micros() >= deadline {
return Err(Error::Timeout);
}
}
}
}
fn build_adv_data(name: &str, out: &mut [u8]) -> usize {
out[0] = 2;
out[1] = AD_TYPE_FLAGS;
out[2] = AD_FLAGS_LE_GENERAL;
let mut len = 3;
let name = name.as_bytes();
let room = ADV_DATA_MAX - len - 2;
let take = name.len().min(room);
let ad_type = if take < name.len() {
AD_TYPE_NAME_SHORT
} else {
AD_TYPE_NAME_COMPLETE
};
out[len] = (take + 1) as u8;
out[len + 1] = ad_type;
out[len + 2..len + 2 + take].copy_from_slice(&name[..take]);
len += 2 + take;
len
}
fn parse_inquiry_result(code: u8, params: &[u8]) -> Option<InquiryResult> {
match code {
EVT_EXTENDED_INQUIRY_RESULT | EVT_INQUIRY_RESULT_WITH_RSSI => {
if params.len() < 15 {
return None;
}
let mut bd_addr = [0u8; 6];
bd_addr.copy_from_slice(¶ms[1..7]);
let mut class_of_device = [0u8; 3];
class_of_device.copy_from_slice(¶ms[9..12]);
let rssi = Some(params[14] as i8);
let (name_buf, name_len) = if code == EVT_EXTENDED_INQUIRY_RESULT && params.len() > 15 {
extract_eir_name(¶ms[15..])
} else {
([0u8; EIR_NAME_MAX], 0)
};
Some(InquiryResult {
bd_addr,
class_of_device,
rssi,
name_buf,
name_len,
})
}
EVT_INQUIRY_RESULT => {
let num = *params.first()? as usize;
if num == 0 {
return None;
}
let cod_off = 1 + 9 * num;
if params.len() < cod_off + 3 {
return None;
}
let mut bd_addr = [0u8; 6];
bd_addr.copy_from_slice(¶ms[1..7]);
let mut class_of_device = [0u8; 3];
class_of_device.copy_from_slice(¶ms[cod_off..cod_off + 3]);
Some(InquiryResult {
bd_addr,
class_of_device,
rssi: None,
name_buf: [0u8; EIR_NAME_MAX],
name_len: 0,
})
}
_ => None,
}
}
fn extract_eir_name(eir: &[u8]) -> ([u8; EIR_NAME_MAX], u8) {
let mut buf = [0u8; EIR_NAME_MAX];
let mut len = 0u8;
if let Some(name) = find_ad_structure(eir, AD_TYPE_NAME_COMPLETE)
.or_else(|| find_ad_structure(eir, AD_TYPE_NAME_SHORT))
{
let n = name.len().min(EIR_NAME_MAX);
buf[..n].copy_from_slice(&name[..n]);
len = n as u8;
}
(buf, len)
}
fn find_ad_structure(data: &[u8], ad_type: u8) -> Option<&[u8]> {
let mut i = 0;
while i < data.len() {
let len = data[i] as usize;
if len == 0 {
break;
}
let end = i + 1 + len;
if end > data.len() {
break;
}
if data[i + 1] == ad_type {
return Some(&data[i + 2..end]);
}
i = end;
}
None
}