use crate::bytes::ToFromBytesEndian;
use crate::control::ControlOpcode;
use crate::crypto::{AID, AKF, MIC};
use crate::mesh::{IVIndex, SequenceNumber, CTL, U24};
use core::convert::{TryFrom, TryInto};
#[derive(Copy, Clone, Hash, Debug, Ord, PartialOrd, Eq, PartialEq)]
pub struct SZMIC(bool);
impl From<SZMIC> for bool {
fn from(s: SZMIC) -> Self {
s.0
}
}
impl From<bool> for SZMIC {
fn from(b: bool) -> Self {
SZMIC(b)
}
}
pub const SEQ_ZERO_MAX: u16 = (1u16 << 13) - 1;
#[derive(Copy, Clone, Hash, Debug, Ord, PartialOrd, Eq, PartialEq)]
#[cfg_attr(feature = "serde-1", derive(serde::Serialize, serde::Deserialize))]
pub struct SeqZero(u16);
impl SeqZero {
pub fn new(seq_zero: u16) -> Self {
assert!(seq_zero <= SEQ_ZERO_MAX);
SeqZero(seq_zero)
}
pub fn original_seq(&self, seq: SequenceNumber) -> SequenceNumber {
SequenceNumber(U24::new(
(u32::from(seq.0) & !u32::from(SEQ_ZERO_MAX)) & u32::from(self.0),
))
}
}
impl From<SequenceNumber> for SeqZero {
fn from(n: SequenceNumber) -> Self {
SeqZero(
(n.0.value() & u32::from(SEQ_ZERO_MAX))
.try_into()
.expect("masking upper bits"),
)
}
}
impl From<SeqZero> for u16 {
fn from(s: SeqZero) -> Self {
s.0
}
}
#[derive(Copy, Clone, Ord, PartialOrd, Eq, PartialEq, Hash, Debug)]
pub struct SeqAuth {
pub first_seq: SequenceNumber,
pub iv_index: IVIndex,
}
impl SeqAuth {
pub fn new(first_seq: SequenceNumber, iv_index: IVIndex) -> Self {
SeqAuth {
first_seq,
iv_index,
}
}
pub fn from_seq_zero(seq_zero: SeqZero, seq: SequenceNumber, iv_index: IVIndex) -> Self {
SeqAuth::new(seq_zero.original_seq(seq), iv_index)
}
pub fn valid_seq(&self, new_seq: SequenceNumber) -> bool {
new_seq >= self.first_seq && (new_seq - self.first_seq) < 8192
}
pub fn seq_zero(&self) -> SeqZero {
self.first_seq.into()
}
}
pub const SEG_MAX: u8 = 0x1F;
#[derive(Copy, Clone, Hash, Debug, Ord, PartialOrd, Eq, PartialEq)]
pub struct SegO(u8);
impl SegO {
pub fn new(v: u8) -> Self {
assert!(v <= SEG_MAX);
Self(v)
}
}
impl From<SegO> for u8 {
fn from(s: SegO) -> Self {
s.0
}
}
#[derive(Copy, Clone, Hash, Debug, Ord, PartialOrd, Eq, PartialEq)]
pub struct SegN(u8);
impl SegN {
pub fn new(v: u8) -> Self {
assert!(v <= SEG_MAX);
Self(v)
}
pub fn next(&self) -> SegN {
if self.0 >= SEG_MAX {
*self
} else {
SegN(self.0 + 1)
}
}
}
impl From<SegN> for u8 {
fn from(s: SegN) -> Self {
s.0
}
}
#[derive(Copy, Clone, Hash, Debug, Ord, PartialOrd, Eq, PartialEq, Default)]
pub struct BlockAck(pub u32);
impl BlockAck {
pub const fn new() -> Self {
Self(0)
}
pub fn new_all_acked(seg_o: SegO) -> Self {
Self((1 << u32::from(u8::from(seg_o) + 1u8)) - 1)
}
pub fn set(&mut self, bit: u8) {
debug_assert!(bit < 32, "{} index overflow into u32", bit);
if bit >= 32 {
return;
}
(self.0) |= 1_u32 << u32::from(bit);
}
#[must_use]
pub fn get(self, bit: u8) -> bool {
debug_assert!(bit < 32, "{} index overflow into u32", bit);
if bit >= 32 {
false
} else {
(self.0 & (1_u32 << u32::from(bit))) != 0
}
}
#[must_use]
pub fn all_acked(self, seg_o: SegO) -> bool {
self == Self::new_all_acked(seg_o)
}
pub const fn max_len() -> usize {
32
}
pub fn count_ones(self) -> u8 {
self.0
.count_ones()
.try_into()
.expect("count_ones can only return <= 32")
}
pub fn seg_left(mut self, seg_o: SegO) -> u8 {
self = BlockAck(self.0 & Self::new_all_acked(seg_o).0);
u8::from(seg_o) - self.count_ones()
}
pub fn valid_for(self, seg_o: SegO) -> bool {
self <= Self::new_all_acked(seg_o)
}
pub fn is_new(self, maybe_new: Self) -> bool {
maybe_new > self && ((maybe_new.0 & self.0) == maybe_new.0)
}
pub const fn cancel() -> Self {
BlockAck::new()
}
}
#[derive(Copy, Clone, Hash, Debug, Ord, PartialOrd, Eq, PartialEq)]
pub struct SEG(bool);
impl SEG {
pub fn new_upper_masked(v: u8) -> SEG {
SEG(v & 0x80 != 0)
}
}
impl From<SEG> for bool {
fn from(s: SEG) -> Self {
s.0
}
}
impl From<bool> for SEG {
fn from(b: bool) -> Self {
SEG(b)
}
}
#[derive(Copy, Clone, Hash, Debug, Ord, PartialOrd, Eq, PartialEq)]
pub struct OBO(bool);
#[derive(Copy, Clone, Hash, Debug, Ord, PartialOrd, Eq, PartialEq)]
pub struct SegmentHeader {
pub flag: bool,
pub seq_zero: SeqZero,
pub seg_o: SegO,
pub seg_n: SegN,
}
impl SegmentHeader {
#[must_use]
pub fn new(flag: bool, seq_zero: SeqZero, seg_o: SegO, seg_n: SegN) -> Self {
Self {
flag,
seq_zero,
seg_o,
seg_n,
}
}
#[must_use]
pub fn pack_into_u24(&self) -> U24 {
let mut out = 0u32;
out |= u32::from(u8::from(self.seg_n));
out |= u32::from(u8::from(self.seg_o) << 5);
out |= u32::from(u16::from(self.seq_zero) << 10);
out |= u32::from(self.flag) << 23;
U24::new(out)
}
#[must_use]
pub fn unpack_from_u24(b: U24) -> Self {
let bytes = b.to_bytes_be();
let flag = bytes[0] & 0x80 != 0;
let seq_high = bytes[0] & 0x7F;
let seq_low = (bytes[1] & 0xFC) >> 2;
let seq_zero = SeqZero::new(u16::from(seq_low) | (u16::from(seq_high) << 6));
let seg_o_high = bytes[1] & 0x02;
let seg_n = SegN::new(bytes[2] & SEG_MAX);
let seg_o_low = (bytes[2] & !SEG_MAX) >> 5;
let seg_o = SegO::new(seg_o_low | (seg_o_high << 3));
Self::new(flag, seq_zero, seg_o, seg_n)
}
}
const UNSEGMENTED_ACCESS_PDU_MAX_LEN: usize = 15;
const UNSEGMENTED_ACCESS_PDU_MIN_LEN: usize = 5;
#[derive(Ord, PartialOrd, Eq, PartialEq, Copy, Clone, Hash, Debug)]
pub struct UnsegmentedAccessPDU {
aid: Option<AID>,
access_pdu_buf: [u8; UNSEGMENTED_ACCESS_PDU_MAX_LEN],
access_pdu_len: usize,
}
impl UnsegmentedAccessPDU {
pub fn new(aid: Option<AID>, data: &[u8]) -> UnsegmentedAccessPDU {
assert!(data.len() <= UNSEGMENTED_ACCESS_PDU_MAX_LEN);
assert!(data.len() >= UNSEGMENTED_ACCESS_PDU_MIN_LEN);
let len = data.len();
let buf = [0_u8; UNSEGMENTED_ACCESS_PDU_MAX_LEN];
UnsegmentedAccessPDU {
aid,
access_pdu_buf: buf,
access_pdu_len: len,
}
}
#[must_use]
pub const fn max_len() -> usize {
UNSEGMENTED_ACCESS_PDU_MAX_LEN + 1
}
#[must_use]
pub fn upper_pdu_len(&self) -> usize {
self.access_pdu_len
}
#[must_use]
pub fn len(&self) -> usize {
1 + self.access_pdu_len
}
#[must_use]
pub fn upper_pdu(&self) -> &[u8] {
&self.access_pdu_buf[..self.access_pdu_len]
}
#[must_use]
pub fn aid(&self) -> Option<AID> {
self.aid
}
#[must_use]
pub fn akf(&self) -> AKF {
self.aid().is_some().into()
}
#[must_use]
pub fn pack_into(&self, bytes: &mut [u8]) {
assert!(self.len() <= bytes.len());
bytes[0] = self
.aid
.unwrap_or_default()
.with_flags(self.akf().into(), false);
bytes[1..self.len()].copy_from_slice(self.upper_pdu());
}
#[must_use]
pub fn unpack_from(bytes: &[u8]) -> Option<Self> {
if SEG::new_upper_masked(bytes[0]).0
|| bytes.len() > UNSEGMENTED_ACCESS_PDU_MAX_LEN + 1
|| bytes.len() < UNSEGMENTED_ACCESS_PDU_MIN_LEN
{
None
} else {
let akf = AKF::from(bytes[0] & 0x40 != 0);
let aid = AID::new_masked(bytes[0]);
if !bool::from(akf) && u8::from(aid) == 0 {
return None;
}
let aid = if bool::from(akf) { Some(aid) } else { None };
Some(Self::new(aid, &bytes[1..]))
}
}
#[must_use]
pub fn mic(&self) -> MIC {
MIC::try_from_bytes_be(
&self.access_pdu_buf[self.access_pdu_len - MIC::small_size()..self.access_pdu_len],
)
.expect("all access PDUs have small MIC")
}
}
#[derive(Ord, PartialOrd, Eq, PartialEq, Copy, Clone, Hash, Debug)]
pub struct SegmentedAccessPDU {
aid: Option<AID>,
segment_header: SegmentHeader,
segment_buf: [u8; SegmentedAccessPDU::max_seg_len()],
len: usize,
}
impl SegmentedAccessPDU {
pub fn new(
aid: Option<AID>,
sz_mic: SZMIC,
seq_zero: SeqZero,
seg_o: SegO,
seg_n: SegN,
data: &[u8],
) -> Self {
assert!(data.len() < Self::max_seg_len());
let mut buf = [0_u8; SegmentedAccessPDU::max_seg_len()];
buf[..data.len()].copy_from_slice(data);
Self {
aid,
segment_header: SegmentHeader {
flag: sz_mic.0,
seq_zero,
seg_o,
seg_n,
},
segment_buf: buf,
len: data.len(),
}
}
pub fn segment_data(&self) -> &[u8] {
&self.segment_buf[..self.len]
}
pub fn segment_len(&self) -> usize {
self.len
}
pub fn len(&self) -> usize {
self.segment_len() + 4
}
#[must_use]
pub fn akf(&self) -> AKF {
self.aid.is_some().into()
}
#[must_use]
pub fn aid(&self) -> Option<AID> {
self.aid
}
#[must_use]
pub const fn min_len() -> usize {
5
}
#[must_use]
pub fn pack_into(&self, buffer: &mut [u8]) {
assert!(buffer.len() >= self.len());
let bytes = &mut buffer[..self.len()];
bytes.as_mut()[0] = self
.aid()
.unwrap_or(AID::new(0))
.with_flags(self.akf().into(), true);
bytes.as_mut()[1..4].copy_from_slice(&self.segment_header.pack_into_u24().to_bytes_be());
bytes.as_mut()[4..].copy_from_slice(self.segment_data());
}
#[must_use]
pub fn unpack_from(bytes: &[u8]) -> Option<Self> {
if bytes.len() < Self::min_len() || bytes.len() > Self::max_seg_len() + 4 {
return None;
}
let (aid, akf, seg) = AID::from_flags(bytes[0]);
if !seg {
return None;
}
if !akf && aid != AID::default() {
return None;
}
let aid = if akf { None } else { Some(aid) };
let packed_header = U24::from_bytes_be(&bytes[1..4]).expect("seq_zero should ways exist");
let segment_header = SegmentHeader::unpack_from_u24(packed_header);
Some(SegmentedAccessPDU::new(
aid,
segment_header.flag.into(),
segment_header.seq_zero,
segment_header.seg_o,
segment_header.seg_n,
&bytes[4..],
))
}
pub const fn max_seg_len() -> usize {
12
}
}
const UNSEGMENTED_CONTROL_PDU_LEN: usize = 11;
#[derive(Ord, PartialOrd, Eq, PartialEq, Copy, Clone, Hash, Debug)]
pub struct UnsegmentedControlPDU {
parameters_buf: [u8; UNSEGMENTED_CONTROL_PDU_LEN],
parameters_len: usize,
opcode: ControlOpcode,
}
impl UnsegmentedControlPDU {
#[must_use]
pub fn new(opcode: ControlOpcode, parameters: &[u8]) -> UnsegmentedControlPDU {
assert!(parameters.len() <= UNSEGMENTED_CONTROL_PDU_LEN);
let mut buf = [0_u8; UNSEGMENTED_CONTROL_PDU_LEN];
buf[..parameters.len()].copy_from_slice(parameters);
UnsegmentedControlPDU {
parameters_buf: buf,
parameters_len: parameters.len(),
opcode,
}
}
#[must_use]
pub const fn parameters_len(&self) -> usize {
self.parameters_len
}
#[must_use]
pub fn data(&self) -> &[u8] {
&self.parameters_buf[..self.parameters_len()]
}
#[must_use]
pub fn data_mut(&mut self) -> &mut [u8] {
let l = self.parameters_len();
&mut self.parameters_buf[..l]
}
#[must_use]
pub const fn min_parameters_size() -> usize {
0
}
#[must_use]
pub const fn max_parameters_size() -> usize {
UNSEGMENTED_CONTROL_PDU_LEN
}
pub fn len(&self) -> usize {
self.parameters_len() + 1
}
#[must_use]
pub fn pack_into(&self, buffer: &mut [u8]) {
assert!(buffer.len() >= self.len());
buffer[0] = self.opcode.into();
buffer[0] &= !0x80;
buffer[1..self.len()].copy_from_slice(self.data());
}
#[must_use]
pub fn unpack_from(bytes: &[u8]) -> Option<Self> {
if bytes.len() < 1 || bytes.len() > Self::max_parameters_size() + 1 {
return None;
}
if bytes[0] & 0x80 != 0 {
return None;
}
let opcode = ControlOpcode::new(bytes[0] & 0x7F)?;
Some(Self::new(opcode, &bytes[1..]))
}
#[must_use]
pub fn opcode(&self) -> ControlOpcode {
self.opcode
}
}
const MAX_SEGMENTED_CONTROL_PDU_LEN: usize = 8;
#[derive(Ord, PartialOrd, Eq, PartialEq, Copy, Clone, Hash, Debug)]
pub struct SegmentedControlPDU {
opcode: ControlOpcode,
segment_header: SegmentHeader,
segment_buf: [u8; MAX_SEGMENTED_CONTROL_PDU_LEN],
segment_buf_len: usize,
}
impl SegmentedControlPDU {
#[must_use]
pub fn new(opcode: ControlOpcode, header: SegmentHeader, data: &[u8]) -> SegmentedControlPDU {
assert!(
data.len() < MAX_SEGMENTED_CONTROL_PDU_LEN,
"segment overflow ({} > {})",
data.len(),
MAX_SEGMENTED_CONTROL_PDU_LEN
);
let mut buf = [0_u8; MAX_SEGMENTED_CONTROL_PDU_LEN];
buf[..data.len()].copy_from_slice(data);
SegmentedControlPDU {
opcode,
segment_header: header,
segment_buf: buf,
segment_buf_len: data.len(),
}
}
#[must_use]
pub const fn max_len() -> usize {
4 + MAX_SEGMENTED_CONTROL_PDU_LEN
}
#[must_use]
pub const fn min_len() -> usize {
4 + 1
}
#[must_use]
pub fn len(&self) -> usize {
self.segment_buf_len + 4
}
#[must_use]
pub fn segment_len(&self) -> usize {
self.segment_buf_len
}
#[must_use]
pub fn segment_data(&self) -> &[u8] {
&self.segment_buf[..self.segment_len()]
}
#[must_use]
pub fn segment_data_mut(&mut self) -> &mut [u8] {
let l = self.segment_len();
&mut self.segment_buf[..l]
}
#[must_use]
pub const fn opcode(&self) -> ControlOpcode {
self.opcode
}
#[must_use]
pub const fn header(&self) -> &SegmentHeader {
&self.segment_header
}
#[must_use]
pub fn pack_into(&self, buffer: &mut [u8]) {
assert!(buffer.len() >= self.len());
let buffer = &mut buffer[..self.len()];
buffer[0] = self.opcode.into();
buffer[0] |= 0x80;
buffer[1..4].copy_from_slice(&self.segment_header.pack_into_u24().to_bytes_be());
buffer[4..].copy_from_slice(self.segment_data());
}
#[must_use]
pub fn unpack_from(bytes: &[u8]) -> Option<Self> {
if bytes.len() < Self::min_len() || bytes.len() > Self::max_len() {
return None;
}
if bytes[0] & 0x80 == 0 {
return None;
}
let opcode = ControlOpcode::new(bytes[0] & 0x7F)?;
let packed_header =
U24::from_bytes_be(&bytes[1..4]).expect("packed header should always be here");
let segment_header = SegmentHeader::unpack_from_u24(packed_header);
Some(Self::new(opcode, segment_header, &bytes[4..]))
}
#[must_use]
pub const fn max_seg_len() -> usize {
MAX_SEGMENTED_CONTROL_PDU_LEN
}
}
#[derive(Ord, PartialOrd, Eq, PartialEq, Copy, Clone, Hash, Debug)]
pub struct SegmentAckPDU {
seq_zero: SeqZero,
block_ack: BlockAck,
obo: OBO,
}
impl SegmentAckPDU {
#[must_use]
pub const fn opcode() -> ControlOpcode {
ControlOpcode::Ack
}
}
#[derive(Ord, PartialOrd, Eq, PartialEq, Copy, Clone, Hash, Debug)]
pub enum PDU {
UnsegmentedAccess(UnsegmentedAccessPDU),
SegmentedAccess(SegmentedAccessPDU),
UnsegmentedControl(UnsegmentedControlPDU),
SegmentedControl(SegmentedControlPDU),
}
impl PDU {
#[must_use]
pub fn is_seg(&self) -> bool {
match self {
PDU::UnsegmentedAccess(_) | PDU::UnsegmentedControl(_) => false,
PDU::SegmentedAccess(_) | PDU::SegmentedControl(_) => true,
}
}
#[must_use]
pub fn is_control(&self) -> bool {
match self {
PDU::UnsegmentedAccess(_) | PDU::SegmentedAccess(_) => false,
PDU::UnsegmentedControl(_) | PDU::SegmentedControl(_) => true,
}
}
pub fn len(&self) -> usize {
match self {
PDU::UnsegmentedAccess(p) => p.len(),
PDU::SegmentedAccess(p) => p.len(),
PDU::UnsegmentedControl(p) => p.len(),
PDU::SegmentedControl(p) => p.len(),
}
}
pub fn seq_zero(&self) -> Option<SeqZero> {
match self {
PDU::UnsegmentedAccess(_) => None,
PDU::SegmentedAccess(pdu) => Some(pdu.segment_header.seq_zero),
PDU::UnsegmentedControl(_) => None,
PDU::SegmentedControl(pdu) => Some(pdu.segment_header.seq_zero),
}
}
pub const fn max_len() -> usize {
16
}
pub fn pack_into(&self, buffer: &mut [u8]) {
match self {
PDU::UnsegmentedAccess(p) => p.pack_into(buffer),
PDU::SegmentedAccess(p) => p.pack_into(buffer),
PDU::UnsegmentedControl(p) => p.pack_into(buffer),
PDU::SegmentedControl(p) => p.pack_into(buffer),
}
}
pub fn unpack_from(bytes: &[u8], ctl: CTL) -> Option<Self> {
Some(match (bool::from(ctl), SEG::new_upper_masked(bytes[0]).0) {
(true, true) => PDU::SegmentedControl(SegmentedControlPDU::unpack_from(bytes)?),
(true, false) => PDU::UnsegmentedControl(UnsegmentedControlPDU::unpack_from(bytes)?),
(false, false) => PDU::UnsegmentedAccess(UnsegmentedAccessPDU::unpack_from(bytes)?),
(false, true) => PDU::SegmentedAccess(SegmentedAccessPDU::unpack_from(bytes)?),
})
}
pub fn segmented(&self) -> Option<SegmentedPDU> {
match self {
PDU::SegmentedAccess(seg) => Some(SegmentedPDU::Access(*seg)),
PDU::SegmentedControl(seg) => Some(SegmentedPDU::Control(*seg)),
_ => None,
}
}
}
#[derive(Ord, PartialOrd, Eq, PartialEq, Copy, Clone, Hash, Debug)]
pub struct PDUBytes {
buf: [u8; PDU::max_len()],
buf_len: usize,
}
impl PDUBytes {
pub fn new(buffer: &[u8]) -> PDUBytes {
buffer.try_into().expect("bad buffer length")
}
pub fn new_zeroed(len: usize) -> PDUBytes {
PDUBytes {
buf: [0_u8; PDU::max_len()],
buf_len: len,
}
}
pub fn len(&self) -> usize {
self.buf_len
}
pub fn is_empty(&self) -> bool {
self.buf_len == 0
}
pub fn seg(&self) -> SEG {
debug_assert!(!self.is_empty());
SEG(self.buf[0] & 0x80 != 0)
}
}
impl AsRef<[u8]> for PDUBytes {
fn as_ref(&self) -> &[u8] {
&self.buf[..self.buf_len]
}
}
impl AsMut<[u8]> for PDUBytes {
fn as_mut(&mut self) -> &mut [u8] {
&mut self.buf[..self.buf_len]
}
}
#[derive(Ord, PartialOrd, Eq, PartialEq, Copy, Clone, Hash, Debug)]
pub struct PDUBytesError;
impl TryFrom<&[u8]> for PDUBytes {
type Error = PDUBytesError;
fn try_from(value: &[u8]) -> Result<Self, Self::Error> {
let l = value.len();
if l == 0 || l > PDU::max_len() {
Err(PDUBytesError)
} else {
let mut buf = [0_u8; PDU::max_len()];
buf[..l].copy_from_slice(value);
Ok(Self { buf, buf_len: l })
}
}
}
#[derive(Copy, Clone, Ord, PartialOrd, Eq, PartialEq, Debug, Hash)]
pub enum SegmentedPDU {
Access(SegmentedAccessPDU),
Control(SegmentedControlPDU),
}
impl SegmentedPDU {
pub fn segment_header(&self) -> &SegmentHeader {
match self {
SegmentedPDU::Access(pdu) => &pdu.segment_header,
SegmentedPDU::Control(pdu) => &pdu.segment_header,
}
}
pub fn seq_zero(&self) -> SeqZero {
self.segment_header().seq_zero
}
pub fn seg_data(&self) -> &[u8] {
match self {
SegmentedPDU::Access(pdu) => pdu.segment_data(),
SegmentedPDU::Control(pdu) => pdu.segment_data(),
}
}
pub fn szmic(&self) -> Option<bool> {
match self {
SegmentedPDU::Access(pdu) => Some(pdu.segment_header.flag),
SegmentedPDU::Control(_) => None,
}
}
}
impl From<&SegmentedPDU> for PDU {
fn from(pdu: &SegmentedPDU) -> Self {
match pdu {
SegmentedPDU::Access(pdu) => PDU::SegmentedAccess(*pdu),
SegmentedPDU::Control(pdu) => PDU::SegmentedControl(*pdu),
}
}
}
impl From<SegmentedPDU> for PDU {
fn from(pdu: SegmentedPDU) -> Self {
(&pdu).into()
}
}