use crate::link::ad_structure::{AdStructure, Flags};
use crate::link::{channel_map::ChannelMap, AddressKind, DeviceAddress};
use crate::utils::{Hex, HexSlice};
use crate::{bytes::*, time::Duration, Error};
use core::{convert::TryInto, fmt, iter};
pub const CRC_PRESET: u32 = 0x00555555;
pub const MAX_PAYLOAD_SIZE: usize = 37;
pub const ACCESS_ADDRESS: u32 = 0x8E89BED6;
#[derive(Debug, Copy, Clone)]
pub enum Pdu<'a> {
ConnectableUndirected {
advertiser_addr: DeviceAddress,
advertising_data: BytesOr<'a, [AdStructure<'a>]>,
},
ConnectableDirected {
advertiser_addr: DeviceAddress,
initiator_addr: DeviceAddress,
},
NonconnectableUndirected {
advertiser_addr: DeviceAddress,
advertising_data: BytesOr<'a, [AdStructure<'a>]>,
},
ScannableUndirected {
advertiser_addr: DeviceAddress,
advertising_data: BytesOr<'a, [AdStructure<'a>]>,
},
ScanRequest {
scanner_addr: DeviceAddress,
advertiser_addr: DeviceAddress,
},
ScanResponse {
advertiser_addr: DeviceAddress,
scan_data: BytesOr<'a, [AdStructure<'a>]>,
},
ConnectRequest {
initiator_addr: DeviceAddress,
advertiser_addr: DeviceAddress,
lldata: ConnectRequestData,
},
}
impl<'a> Pdu<'a> {
pub fn from_header_and_payload(
header: Header,
payload: &mut ByteReader<'a>,
) -> Result<Self, Error> {
use self::Pdu::*;
if usize::from(header.payload_length()) != payload.bytes_left() {
return Err(Error::InvalidLength);
}
Ok(match header.type_() {
PduType::AdvInd => ConnectableUndirected {
advertiser_addr: {
let kind = if header.tx_add() {
AddressKind::Random
} else {
AddressKind::Public
};
DeviceAddress::new(payload.read_array::<[u8; 6]>()?, kind)
},
advertising_data: BytesOr::from_bytes(payload)?,
},
PduType::AdvDirectInd => ConnectableDirected {
advertiser_addr: {
let kind = if header.tx_add() {
AddressKind::Random
} else {
AddressKind::Public
};
DeviceAddress::new(payload.read_array::<[u8; 6]>()?, kind)
},
initiator_addr: {
let kind = if header.rx_add() {
AddressKind::Random
} else {
AddressKind::Public
};
DeviceAddress::new(payload.read_array::<[u8; 6]>()?, kind)
},
},
PduType::AdvNonconnInd => NonconnectableUndirected {
advertiser_addr: {
let kind = if header.tx_add() {
AddressKind::Random
} else {
AddressKind::Public
};
DeviceAddress::new(payload.read_array::<[u8; 6]>()?, kind)
},
advertising_data: BytesOr::from_bytes(payload)?,
},
PduType::AdvScanInd => ScannableUndirected {
advertiser_addr: {
let kind = if header.tx_add() {
AddressKind::Random
} else {
AddressKind::Public
};
DeviceAddress::new(payload.read_array::<[u8; 6]>()?, kind)
},
advertising_data: BytesOr::from_bytes(payload)?,
},
PduType::ScanReq => ScanRequest {
scanner_addr: {
let kind = if header.tx_add() {
AddressKind::Random
} else {
AddressKind::Public
};
DeviceAddress::new(payload.read_array::<[u8; 6]>()?, kind)
},
advertiser_addr: {
let kind = if header.rx_add() {
AddressKind::Random
} else {
AddressKind::Public
};
DeviceAddress::new(payload.read_array::<[u8; 6]>()?, kind)
},
},
PduType::ScanRsp => ScanResponse {
advertiser_addr: {
let kind = if header.tx_add() {
AddressKind::Random
} else {
AddressKind::Public
};
DeviceAddress::new(payload.read_array::<[u8; 6]>()?, kind)
},
scan_data: BytesOr::from_bytes(payload)?,
},
PduType::ConnectReq => ConnectRequest {
initiator_addr: {
let kind = if header.tx_add() {
AddressKind::Random
} else {
AddressKind::Public
};
DeviceAddress::new(payload.read_array::<[u8; 6]>()?, kind)
},
advertiser_addr: {
let kind = if header.rx_add() {
AddressKind::Random
} else {
AddressKind::Public
};
DeviceAddress::new(payload.read_array::<[u8; 6]>()?, kind)
},
lldata: ConnectRequestData::from_bytes(payload)?,
},
PduType::Unknown(_) => return Err(Error::InvalidValue),
})
}
pub fn sender(&self) -> &DeviceAddress {
use self::Pdu::*;
match self {
ConnectableUndirected {
advertiser_addr, ..
}
| ConnectableDirected {
advertiser_addr, ..
}
| NonconnectableUndirected {
advertiser_addr, ..
}
| ScannableUndirected {
advertiser_addr, ..
}
| ScanResponse {
advertiser_addr, ..
} => advertiser_addr,
ScanRequest { scanner_addr, .. } => scanner_addr,
ConnectRequest { initiator_addr, .. } => initiator_addr,
}
}
pub fn receiver(&self) -> Option<&DeviceAddress> {
use self::Pdu::*;
match self {
ConnectableUndirected { .. }
| NonconnectableUndirected { .. }
| ScannableUndirected { .. }
| ScanResponse { .. } => None,
ConnectableDirected { initiator_addr, .. } => Some(initiator_addr),
ScanRequest {
advertiser_addr, ..
}
| ConnectRequest {
advertiser_addr, ..
} => Some(advertiser_addr),
}
}
pub fn ty(&self) -> PduType {
use self::Pdu::*;
match self {
ConnectableUndirected { .. } => PduType::AdvInd,
ConnectableDirected { .. } => PduType::AdvDirectInd,
NonconnectableUndirected { .. } => PduType::AdvNonconnInd,
ScannableUndirected { .. } => PduType::AdvScanInd,
ScanRequest { .. } => PduType::ScanReq,
ScanResponse { .. } => PduType::ScanRsp,
ConnectRequest { .. } => PduType::ConnectReq,
}
}
pub fn advertising_data(&self) -> Option<impl Iterator<Item = AdStructure<'a>>> {
use self::Pdu::*;
match self {
ConnectableUndirected {
advertising_data, ..
}
| NonconnectableUndirected {
advertising_data, ..
}
| ScannableUndirected {
advertising_data, ..
} => Some(advertising_data.iter()),
ScanResponse { scan_data, .. } => Some(scan_data.iter()),
ScanRequest { .. } | ConnectableDirected { .. } | ConnectRequest { .. } => None,
}
}
}
impl<'a> FromBytes<'a> for Pdu<'a> {
fn from_bytes(bytes: &mut ByteReader<'a>) -> Result<Self, Error> {
let header = Header::from_bytes(bytes)?;
Self::from_header_and_payload(header, bytes)
}
}
#[derive(Copy, Clone, Debug)]
pub struct ConnectRequestData {
access_address: Hex<u32>,
crc_init: Hex<u32>,
win_size: Duration,
win_offset: Duration,
interval: Duration,
latency: u16,
timeout: Duration,
chm: ChannelMap,
hop: u8,
sca: SleepClockAccuracy,
}
impl ConnectRequestData {
pub fn access_address(&self) -> u32 {
self.access_address.0
}
pub fn crc_init(&self) -> u32 {
self.crc_init.0
}
pub fn channel_map(&self) -> &ChannelMap {
&self.chm
}
pub fn hop(&self) -> u8 {
self.hop
}
pub fn end_of_tx_window(&self) -> Duration {
let transmit_window_delay = Duration::from_micros(1250);
self.win_offset + self.win_size + transmit_window_delay
}
pub fn interval(&self) -> Duration {
self.interval
}
pub fn slave_latency(&self) -> u16 {
self.latency
}
pub fn supervision_timeout(&self) -> Duration {
self.timeout
}
}
impl FromBytes<'_> for ConnectRequestData {
fn from_bytes(bytes: &mut ByteReader<'_>) -> Result<Self, Error> {
let sca;
Ok(Self {
access_address: Hex(bytes.read_u32_le()?),
crc_init: {
let mut le_bytes = [0u8; 4];
le_bytes[..3].copy_from_slice(bytes.read_slice(3)?);
Hex(u32::from_le_bytes(le_bytes))
},
win_size: Duration::from_micros(u32::from(bytes.read_u8()?) * 1250),
win_offset: Duration::from_micros(u32::from(bytes.read_u16_le()?) * 1250),
interval: Duration::from_micros(u32::from(bytes.read_u16_le()?) * 1250),
latency: bytes.read_u16_le()?,
timeout: Duration::from_micros(u32::from(bytes.read_u16_le()?) * 10_000),
chm: ChannelMap::from_raw(bytes.read_array()?),
hop: {
let hop_and_sca = bytes.read_u8()?;
sca = (hop_and_sca >> 5) & 0b111;
hop_and_sca & 0b11111
},
sca: {
use self::SleepClockAccuracy::*;
match sca {
0 => Ppm251To500,
1 => Ppm151To250,
2 => Ppm101To150,
3 => Ppm76To100,
4 => Ppm51To75,
5 => Ppm31To50,
6 => Ppm21To30,
7 => Ppm0To20,
_ => unreachable!(), }
},
})
}
}
#[derive(Copy, Clone, Debug)]
pub enum SleepClockAccuracy {
Ppm251To500,
Ppm151To250,
Ppm101To150,
Ppm76To100,
Ppm51To75,
Ppm31To50,
Ppm21To30,
Ppm0To20,
}
pub struct PduBuf {
header: Header,
payload_buf: [u8; MAX_PAYLOAD_SIZE],
}
impl PduBuf {
fn adv(
ty: PduType,
adv: DeviceAddress,
adv_data: &mut dyn Iterator<Item = &AdStructure<'_>>,
) -> Result<Self, Error> {
let mut payload = [0; MAX_PAYLOAD_SIZE];
let mut buf = ByteWriter::new(&mut payload[..]);
buf.write_slice(adv.raw()).unwrap();
for ad in adv_data {
ad.to_bytes(&mut buf)?;
}
let left = buf.space_left();
let used = payload.len() - left;
let mut header = Header::new(ty);
header.set_payload_length(used as u8);
header.set_tx_add(adv.is_random());
header.set_rx_add(false);
Ok(Self {
header,
payload_buf: payload,
})
}
pub fn connectable_undirected(
advertiser_addr: DeviceAddress,
advertiser_data: &[AdStructure<'_>],
) -> Result<Self, Error> {
Self::adv(
PduType::AdvInd,
advertiser_addr,
&mut advertiser_data.iter(),
)
}
pub fn connectable_directed(
advertiser_addr: DeviceAddress,
initiator_addr: DeviceAddress,
) -> Self {
let mut payload = [0; 37];
payload[0..6].copy_from_slice(advertiser_addr.raw());
payload[6..12].copy_from_slice(initiator_addr.raw());
let mut header = Header::new(PduType::AdvDirectInd);
header.set_payload_length(6 + 6);
header.set_tx_add(advertiser_addr.is_random());
header.set_rx_add(initiator_addr.is_random());
Self {
header,
payload_buf: payload,
}
}
pub fn nonconnectable_undirected(
advertiser_addr: DeviceAddress,
advertiser_data: &[AdStructure<'_>],
) -> Result<Self, Error> {
Self::adv(
PduType::AdvNonconnInd,
advertiser_addr,
&mut advertiser_data.iter(),
)
}
pub fn scannable_undirected(
advertiser_addr: DeviceAddress,
advertiser_data: &[AdStructure<'_>],
) -> Result<Self, Error> {
Self::adv(
PduType::AdvScanInd,
advertiser_addr,
&mut advertiser_data.iter(),
)
}
pub fn beacon(
advertiser_addr: DeviceAddress,
advertiser_data: &[AdStructure<'_>],
) -> Result<Self, Error> {
Self::nonconnectable_undirected(advertiser_addr, advertiser_data)
}
pub fn discoverable(
advertiser_addr: DeviceAddress,
advertiser_data: &[AdStructure<'_>],
) -> Result<Self, Error> {
Self::adv(
PduType::AdvInd,
advertiser_addr,
&mut iter::once(&AdStructure::from(Flags::discoverable())).chain(advertiser_data),
)
}
pub fn scan_request(_scanner: DeviceAddress, _adv: DeviceAddress) -> Result<Self, Error> {
unimplemented!()
}
pub fn scan_response(
advertiser_addr: DeviceAddress,
scan_data: &[AdStructure<'_>],
) -> Result<Self, Error> {
let mut payload = [0; MAX_PAYLOAD_SIZE];
let mut buf = ByteWriter::new(&mut payload[..]);
buf.write_slice(advertiser_addr.raw()).unwrap();
for ad in scan_data {
ad.to_bytes(&mut buf)?;
}
let left = buf.space_left();
let used = payload.len() - left;
let mut header = Header::new(PduType::ScanRsp);
header.set_payload_length(used as u8);
header.set_tx_add(advertiser_addr.is_random());
header.set_rx_add(false);
Ok(Self {
header,
payload_buf: payload,
})
}
pub fn header(&self) -> Header {
self.header
}
pub fn payload(&self) -> &[u8] {
let len = self.header.payload_length() as usize;
&self.payload_buf[..len]
}
}
impl fmt::Debug for PduBuf {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
write!(f, "({:?}, {:?})", self.header(), HexSlice(self.payload()))
}
}
#[derive(Copy, Clone)]
pub struct Header(u16);
const TXADD_MASK: u16 = 0b00000000_01000000;
const RXADD_MASK: u16 = 0b00000000_10000000;
impl Header {
pub fn new(ty: PduType) -> Self {
Header(u16::from(u8::from(ty)))
}
pub fn parse(raw: &[u8]) -> Self {
let bytes: [u8; 2] = raw[..2].try_into().expect("raw has fewer than 2 bytes");
Header(u16::from_le_bytes(bytes))
}
pub fn to_u16(&self) -> u16 {
self.0
}
fn set_header_bits(&mut self, mask: u16) {
self.0 |= mask;
}
fn clear_header_bits(&mut self, mask: u16) {
self.0 &= !mask;
}
pub fn type_(&self) -> PduType {
PduType::from((self.0 & 0b00000000_00001111) as u8)
}
pub fn tx_add(&self) -> bool {
self.0 & TXADD_MASK != 0
}
pub fn set_tx_add(&mut self, value: bool) {
if value {
self.set_header_bits(TXADD_MASK);
} else {
self.clear_header_bits(TXADD_MASK);
}
}
pub fn rx_add(&self) -> bool {
self.0 & RXADD_MASK != 0
}
pub fn set_rx_add(&mut self, value: bool) {
if value {
self.set_header_bits(RXADD_MASK);
} else {
self.clear_header_bits(RXADD_MASK);
}
}
pub fn payload_length(&self) -> u8 {
((self.0 & 0b00111111_00000000) >> 8) as u8
}
pub fn set_payload_length(&mut self, length: u8) {
assert!(6 <= length && length <= 37);
let header = self.0 & !0b00111111_00000000;
self.0 = header | (u16::from(length) << 8);
}
}
impl fmt::Debug for Header {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
f.debug_struct("Header")
.field("PDU Type", &self.type_())
.field("TxAdd", &self.tx_add())
.field("RxAdd", &self.rx_add())
.field("len", &self.payload_length())
.finish()
}
}
impl<'a> FromBytes<'a> for Header {
fn from_bytes(bytes: &mut ByteReader<'a>) -> Result<Self, Error> {
let raw = bytes.read_u16_le()?;
Ok(Header(raw))
}
}
impl ToBytes for Header {
fn to_bytes(&self, writer: &mut ByteWriter<'_>) -> Result<(), Error> {
writer.write_u16_le(self.0)
}
}
enum_with_unknown! {
#[derive(Debug, PartialEq, Eq)]
pub enum PduType(u8) {
AdvInd = 0b0000,
AdvDirectInd = 0b0001,
AdvNonconnInd = 0b0010,
AdvScanInd = 0b0110,
ScanReq = 0b0011,
ScanRsp = 0b0100,
ConnectReq = 0b0101,
}
}
impl PduType {
pub fn is_beacon(&self) -> bool {
*self == PduType::AdvNonconnInd
}
pub fn allows_adv_data(&self) -> bool {
match self {
PduType::AdvInd | PduType::AdvNonconnInd | PduType::AdvScanInd | PduType::ScanRsp => {
true
}
PduType::AdvDirectInd
| PduType::ScanReq
| PduType::ConnectReq
| PduType::Unknown(_) => false,
}
}
}