use core::fmt;
use as_slice::{AsMutSlice, AsSlice};
use byteorder::{ByteOrder, NetworkEndian as NE, LE};
use owning_slice::Truncate;
use crate::{
icmpv6, ipv6,
sixlowpan::{iphc, nhc},
traits::UncheckedIndex,
};
const CONTROLL: usize = 0;
mod frame_type {
pub const MASK: u8 = (1 << SIZE) - 1;
pub const OFFSET: u8 = 0;
pub const SIZE: u8 = 3;
}
mod security_enabled {
pub const MASK: u8 = (1 << SIZE) - 1;
pub const OFFSET: u8 = super::frame_type::OFFSET + super::frame_type::SIZE;
pub const SIZE: u8 = 1;
}
mod frame_pending {
pub const MASK: u8 = (1 << SIZE) - 1;
pub const OFFSET: u8 = super::security_enabled::OFFSET + super::security_enabled::SIZE;
pub const SIZE: u8 = 1;
}
mod ack_request {
pub const MASK: u8 = (1 << SIZE) - 1;
pub const OFFSET: u8 = super::frame_pending::OFFSET + super::frame_pending::SIZE;
pub const SIZE: u8 = 1;
}
mod intra_pan {
pub const MASK: u8 = (1 << SIZE) - 1;
pub const OFFSET: u8 = super::ack_request::OFFSET + super::ack_request::SIZE;
pub const SIZE: u8 = 1;
}
const CONTROLH: usize = 1;
mod dest_addr_mode {
pub const MASK: u8 = (1 << SIZE) - 1;
pub const OFFSET: u8 = 2;
pub const SIZE: u8 = 2;
}
mod src_addr_mode {
pub const MASK: u8 = (1 << SIZE) - 1;
pub const OFFSET: u8 = 6;
pub const SIZE: u8 = 2;
}
const SEQUENCE: usize = 2;
const HEADER_SIZE: u8 = SEQUENCE as u8 + 1;
#[derive(Clone, Copy)]
pub struct Frame<BUFFER>
where
BUFFER: AsSlice<Element = u8>,
{
buffer: BUFFER,
payload: u8,
}
impl<B> Frame<B>
where
B: AsSlice<Element = u8>,
{
pub fn parse(bytes: B) -> Result<Self, B> {
let len = (|| {
let slice = bytes.as_slice();
let mut len = 3u8;
if slice.len() < usize::from(len) {
return Err(());
}
let ftype = Type::from(get!(slice[CONTROLL], frame_type));
let dest_addr_mode =
AddrMode::checked(get!(slice[CONTROLH], dest_addr_mode)).ok_or(())?;
let src_addr_mode = AddrMode::checked(get!(slice[CONTROLH], src_addr_mode)).ok_or(())?;
len += match dest_addr_mode {
AddrMode::None => {
if ftype != Type::Acknowledgment || ftype != Type::Beacon {
if src_addr_mode == AddrMode::None {
return Err(());
}
}
0
}
AddrMode::Short => 2,
AddrMode::Extended => 8,
};
len += match src_addr_mode {
AddrMode::None => {
if ftype != Type::Acknowledgment {
if dest_addr_mode == AddrMode::None {
return Err(());
}
}
0
}
AddrMode::Short => 2,
AddrMode::Extended => 8,
};
if dest_addr_mode != AddrMode::None {
len += 2;
}
let intra_pan = get!(slice[CONTROLL], intra_pan);
if src_addr_mode != AddrMode::None && intra_pan == 0 {
len += 2;
}
if slice.len() < usize::from(len) {
Err(())
} else {
Ok(len)
}
})();
if let Ok(len) = len {
Ok(Frame {
payload: len,
buffer: bytes,
})
} else {
Err(bytes)
}
}
pub fn get_type(&self) -> Type {
Type::from(get!(self.header_()[CONTROLL], frame_type) & 0b111)
}
pub fn get_security_enabled(&self) -> bool {
get!(self.header_()[CONTROLL], security_enabled) == 1
}
pub fn get_frame_pending(&self) -> bool {
get!(self.header_()[CONTROLL], frame_pending) == 1
}
pub fn get_ack_request(&self) -> bool {
get!(self.header_()[CONTROLL], ack_request) == 1
}
pub fn get_intra_pan(&self) -> bool {
get!(self.header_()[CONTROLL], intra_pan) == 1
}
pub fn get_dest_addr_mode(&self) -> AddrMode {
unsafe { AddrMode::unchecked(get!(self.header_()[CONTROLH], dest_addr_mode)) }
}
pub fn get_src_addr_mode(&self) -> AddrMode {
unsafe { AddrMode::unchecked(get!(self.header_()[CONTROLH], src_addr_mode)) }
}
pub fn get_sequence_number(&self) -> u8 {
self.header_()[SEQUENCE]
}
pub fn get_dest_pan_id(&self) -> Option<PanId> {
if self.get_dest_addr_mode() == AddrMode::None {
None
} else {
Some(PanId(LE::read_u16(unsafe { self.as_slice().r(3..5) })))
}
}
pub fn get_dest_addr(&self) -> Option<Addr> {
let mut start = 3;
if self.get_dest_pan_id().is_some() {
start += 2;
}
Some(match self.get_dest_addr_mode() {
AddrMode::None => return None,
AddrMode::Short => Addr::Short(ShortAddr(LE::read_u16(unsafe {
self.as_slice().r(start..start + 2)
}))),
AddrMode::Extended => Addr::Extended(ExtendedAddr(LE::read_u64(unsafe {
self.as_slice().r(start..start + 8)
}))),
})
}
pub fn get_src_pan_id(&self) -> Option<PanId> {
if self.get_src_addr_mode() != AddrMode::None && !self.get_intra_pan() {
let mut start = 3;
if self.get_dest_pan_id().is_some() {
start += 2
}
start += match self.get_dest_addr_mode() {
AddrMode::None => 0,
AddrMode::Short => 2,
AddrMode::Extended => 8,
};
Some(PanId(LE::read_u16(unsafe {
self.as_slice().r(start..start + 2)
})))
} else {
None
}
}
pub fn get_src_addr(&self) -> Option<Addr> {
let mut start = 3;
if self.get_dest_pan_id().is_some() {
start += 2;
}
start += match self.get_dest_addr_mode() {
AddrMode::None => 0,
AddrMode::Short => 2,
AddrMode::Extended => 8,
};
if self.get_src_pan_id().is_some() {
start += 2;
}
Some(match self.get_src_addr_mode() {
AddrMode::None => return None,
AddrMode::Short => Addr::Short(ShortAddr(LE::read_u16(unsafe {
self.as_slice().r(start..start + 2)
}))),
AddrMode::Extended => Addr::Extended(ExtendedAddr(LE::read_u64(unsafe {
self.as_slice().r(start..start + 8)
}))),
})
}
pub fn header(&self) -> &[u8] {
unsafe { self.as_slice().rt(..usize::from(self.payload)) }
}
pub fn payload(&self) -> &[u8] {
unsafe { self.as_slice().rf(usize::from(self.payload)..) }
}
pub fn as_bytes(&self) -> &[u8] {
self.as_slice()
}
pub fn free(self) -> B {
self.buffer
}
fn as_slice(&self) -> &[u8] {
self.buffer.as_slice()
}
fn header_(&self) -> &[u8; HEADER_SIZE as usize] {
debug_assert!(self.as_slice().len() >= HEADER_SIZE as usize);
unsafe { &*(self.as_slice().as_ptr() as *const _) }
}
}
impl<B> fmt::Debug for Frame<B>
where
B: AsSlice<Element = u8>,
{
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
use crate::fmt::{Display, Quoted};
let mut s = f.debug_struct("ieee802154::Frame");
s.field("type", &self.get_type())
.field("security_enabled", &self.get_security_enabled())
.field("frame_pending", &self.get_frame_pending())
.field("ack_request", &self.get_ack_request())
.field("intra_pan", &self.get_intra_pan())
.field("dest_addr_mode", &self.get_dest_addr_mode())
.field("src_addr_mode", &self.get_src_addr_mode())
.field("sequence_number", &self.get_sequence_number());
if let Some(pan_id) = self.get_dest_pan_id() {
s.field("dest_pan_id", &Display(pan_id));
}
match self.get_dest_addr() {
Some(Addr::Short(sa)) => {
s.field("dest_addr", &Display(sa));
}
Some(Addr::Extended(ea)) => {
s.field("dest_addr", &Quoted(ea));
}
None => {}
}
if let Some(pan_id) = self.get_src_pan_id() {
s.field("src_pan_id", &Display(pan_id));
}
match self.get_src_addr() {
Some(Addr::Short(sa)) => {
s.field("src_addr", &Display(sa));
}
Some(Addr::Extended(ea)) => {
s.field("src_addr", &Quoted(ea));
}
_ => {}
}
s.finish()
}
}
impl<B> Frame<B>
where
B: AsMutSlice<Element = u8>,
{
pub fn data(mut buffer: B, src_dest: SrcDest) -> Self {
let payload = 3 + src_dest.size();
assert!(buffer.as_slice().len() >= usize::from(payload));
buffer.as_mut_slice()[..3].copy_from_slice(&[0, 0, 0]);
let mut frame = Frame { buffer, payload };
frame.set_frame_type(Type::Data);
match src_dest {
SrcDest::PanCoordToNode { .. } => unimplemented!(),
SrcDest::NodeToPanCoord { .. } => unimplemented!(),
SrcDest::IntraPan {
pan_id,
src_addr,
dest_addr,
} => {
frame.set_intra_pan(1);
let mut start = 3;
LE::write_u16(&mut frame.as_mut_slice()[start..start + 2], pan_id.0);
start += 2;
frame.set_dest_addr_mode(dest_addr.mode());
match dest_addr {
Addr::Short(sa) => {
LE::write_u16(&mut frame.as_mut_slice()[start..start + 2], sa.0);
start += 2;
}
Addr::Extended(ea) => {
LE::write_u64(&mut frame.as_mut_slice()[start..start + 8], ea.0);
start += 8;
}
}
frame.set_src_addr_mode(src_addr.mode());
match src_addr {
Addr::Short(sa) => {
LE::write_u16(&mut frame.as_mut_slice()[start..start + 2], sa.0);
}
Addr::Extended(ea) => {
LE::write_u64(&mut frame.as_mut_slice()[start..start + 8], ea.0);
}
}
frame
}
SrcDest::InterPan { .. } => unimplemented!(),
}
}
pub fn set_ack_request(&mut self, ack: bool) {
set!(
self.header_mut_()[CONTROLL],
ack_request,
if ack { 1 } else { 0 }
)
}
pub fn set_sequence_number(&mut self, seq: u8) {
self.header_mut_()[SEQUENCE] = seq;
}
pub fn payload_mut(&mut self) -> &mut [u8] {
let start = usize::from(self.payload);
&mut self.as_mut_slice()[start..]
}
fn set_frame_type(&mut self, ftype: Type) {
set!(self.header_mut_()[CONTROLL], frame_type, u8::from(ftype))
}
fn set_intra_pan(&mut self, ip: u8) {
set!(self.header_mut_()[CONTROLL], intra_pan, ip)
}
fn set_dest_addr_mode(&mut self, am: AddrMode) {
set!(self.header_mut_()[CONTROLH], dest_addr_mode, u8::from(am))
}
fn set_src_addr_mode(&mut self, am: AddrMode) {
set!(self.header_mut_()[CONTROLH], src_addr_mode, u8::from(am))
}
fn as_mut_slice(&mut self) -> &mut [u8] {
self.buffer.as_mut_slice()
}
fn header_mut_(&mut self) -> &mut [u8; HEADER_SIZE as usize] {
debug_assert!(self.as_slice().len() >= HEADER_SIZE as usize);
unsafe { &mut *(self.as_mut_slice().as_mut_ptr() as *mut _) }
}
}
impl<B> Frame<B>
where
B: AsMutSlice<Element = u8> + Truncate<u8>,
{
pub fn set_payload(&mut self, payload: &[u8]) {
assert!(self.payload().len() >= payload.len());
let plen = payload.len();
self.payload_mut()[..plen].copy_from_slice(payload);
self.buffer.truncate(self.payload + plen as u8);
}
pub fn echo_reply<F>(&mut self, src: ipv6::Addr, dest: ipv6::Addr, f: F)
where
F: FnOnce(&mut icmpv6::Message<&mut [u8], icmpv6::EchoReply>),
{
const HOP_LIMIT: u8 = 64;
let ctxt = iphc::Context {
source: self.get_src_addr(),
destination: self.get_dest_addr(),
};
let mut packet = iphc::Packet::new(
self.payload_mut(),
Some(ipv6::NextHeader::Ipv6Icmp),
HOP_LIMIT,
src,
dest,
&ctxt,
);
let mut message = icmpv6::Message::echo_reply(packet.payload_mut());
f(&mut message);
message.update_checksum(src, dest);
let len = (message.as_bytes().len() + packet.header().len() + self.header().len()) as u8;
self.buffer.truncate(len);
}
pub fn neighbor_advertisement<F>(
&mut self,
src: ipv6::Addr,
dest: ipv6::Addr,
target_ll_addr: Option<ExtendedAddr>,
f: F,
) where
F: FnOnce(&mut icmpv6::Message<&mut [u8], icmpv6::NeighborAdvertisement>),
{
const HOP_LIMIT: u8 = 255;
let ctxt = iphc::Context {
source: self.get_src_addr(),
destination: self.get_dest_addr(),
};
let mut packet = iphc::Packet::new(
self.payload_mut(),
Some(ipv6::NextHeader::Ipv6Icmp),
HOP_LIMIT,
src,
dest,
&ctxt,
);
let mut message = icmpv6::Message::neighbor_advertisement(
packet.payload_mut(),
if target_ll_addr.is_some() { 2 } else { 0 },
);
f(&mut message);
if let Some(target_ll_addr) = target_ll_addr {
unsafe {
message.set_target_ieee802154_addr(target_ll_addr);
}
}
message.update_checksum(src, dest);
let len = (message.as_bytes().len() + packet.header().len() + self.header().len()) as u8;
self.buffer.truncate(len);
}
pub fn udp<F>(
&mut self,
src_addr: ipv6::Addr,
src_port: u16,
dest_addr: ipv6::Addr,
dest_port: u16,
elide_checksum: bool,
f: F,
) where
F: FnOnce(&mut nhc::UdpPacket<&mut [u8]>),
{
use nhc::UdpPacket;
const HOP_LIMIT: u8 = 64;
let ctxt = iphc::Context {
source: self.get_src_addr(),
destination: self.get_dest_addr(),
};
let mut ip_packet = iphc::Packet::new(
self.payload_mut(),
None,
HOP_LIMIT,
src_addr,
dest_addr,
&ctxt,
);
let mut udp_packet =
UdpPacket::new(ip_packet.payload_mut(), elide_checksum, src_port, dest_port);
f(&mut udp_packet);
if !elide_checksum {
udp_packet.update_checksum(src_addr, dest_addr);
}
let len = (udp_packet.bytes().len() + ip_packet.header().len() + self.header().len()) as u8;
self.buffer.truncate(len);
}
}
pub enum SrcDest {
PanCoordToNode {
pan_id: PanId,
dest_addr: Addr,
},
NodeToPanCoord {
pan_id: PanId,
src_addr: Addr,
},
IntraPan {
pan_id: PanId,
src_addr: Addr,
dest_addr: Addr,
},
InterPan {
src_pan_id: PanId,
src_addr: Addr,
dest_pan_id: PanId,
dest_addr: Addr,
},
}
impl SrcDest {
fn size(&self) -> u8 {
match *self {
SrcDest::PanCoordToNode { .. } => unimplemented!(),
SrcDest::NodeToPanCoord { .. } => unimplemented!(),
SrcDest::IntraPan {
src_addr,
dest_addr,
..
} => 2 + src_addr.size() + dest_addr.size(),
SrcDest::InterPan {
src_addr,
dest_addr,
..
} => 2 + 2 + src_addr.size() + dest_addr.size(),
}
}
}
full_range!(
u8,
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub enum Type {
Beacon = 0b000,
Data = 0b001,
Acknowledgment = 0b010,
MacCommand = 0b011,
}
);
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub enum AddrMode {
None = 0b00,
Short = 0b10,
Extended = 0b11,
}
impl AddrMode {
fn checked(bits: u8) -> Option<Self> {
Some(match bits & 0b11 {
0b00 => AddrMode::None,
0b01 => return None,
0b10 => AddrMode::Short,
0b11 => AddrMode::Extended,
_ => unreachable!(),
})
}
unsafe fn unchecked(bits: u8) -> Self {
Self::checked(bits).unwrap_or_else(|| debug_unreachable!())
}
}
impl From<AddrMode> for u8 {
fn from(am: AddrMode) -> u8 {
am as u8
}
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub enum Addr {
Short(ShortAddr),
Extended(ExtendedAddr),
}
impl Addr {
fn mode(&self) -> AddrMode {
match *self {
Addr::Short(_) => AddrMode::Short,
Addr::Extended(_) => AddrMode::Extended,
}
}
fn size(&self) -> u8 {
match *self {
Addr::Short(..) => 2,
Addr::Extended(..) => 8,
}
}
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct PanId(pub u16);
impl fmt::Display for PanId {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
write!(f, "{:#04x}", self.0)
}
}
impl PanId {
pub const BROADCAST: PanId = PanId(0xffff);
pub fn is_broadcast(&self) -> bool {
*self == Self::BROADCAST
}
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct ShortAddr(pub u16);
impl fmt::Display for ShortAddr {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
write!(f, "{:#04x}", self.0)
}
}
impl ShortAddr {
pub const BROADCAST: ShortAddr = ShortAddr(0xffff);
pub fn is_broadcast(&self) -> bool {
*self == Self::BROADCAST
}
}
impl From<ShortAddr> for Addr {
fn from(sa: ShortAddr) -> Addr {
Addr::Short(sa)
}
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct ExtendedAddr(pub u64);
impl ExtendedAddr {
pub fn ne_bytes(&self) -> [u8; 8] {
let mut bytes = [0; 8];
NE::write_u64(&mut bytes, self.0);
bytes
}
pub fn eui_64(&self) -> [u8; 8] {
let mut bytes = [0; 8];
NE::write_u64(&mut bytes, self.0);
bytes[0] ^= 1 << 1;
bytes
}
pub fn into_link_local_address(self) -> ipv6::Addr {
let mut bytes = [0; 16];
bytes[0] = 0xfe;
bytes[1] = 0x80;
bytes[8..].copy_from_slice(&self.eui_64());
ipv6::Addr(bytes)
}
}
impl fmt::Display for ExtendedAddr {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
let mut is_first = true;
for byte in self.ne_bytes().iter() {
if is_first {
is_first = false;
} else {
f.write_str(":")?;
}
write!(f, "{:02x}", byte)?;
}
Ok(())
}
}
impl From<ExtendedAddr> for Addr {
fn from(ea: ExtendedAddr) -> Addr {
Addr::Extended(ea)
}
}
#[cfg(test)]
mod tests {
use rand::{self, RngCore};
use super::{Addr, ExtendedAddr, Frame, PanId, ShortAddr, SrcDest, Type};
#[test]
fn data() {
macro_rules! test {
($src:expr, $dest:expr) => {{
let src: Addr = $src.into();
let dest: Addr = $dest.into();
let mut buf = [0; 128];
rand::thread_rng().fill_bytes(&mut buf);
let mut frame = Frame::data(
&mut buf[..],
SrcDest::IntraPan {
pan_id: PanId(0xbeef),
dest_addr: dest,
src_addr: src,
},
);
frame.set_payload(&[]);
assert_eq!(frame.get_type(), Type::Data);
assert_eq!(frame.get_security_enabled(), false);
assert_eq!(frame.get_frame_pending(), false);
assert_eq!(frame.get_ack_request(), false);
assert_eq!(frame.get_intra_pan(), true);
assert_eq!(frame.get_dest_addr_mode(), dest.mode());
assert_eq!(frame.get_src_addr_mode(), src.mode());
assert_eq!(frame.get_dest_pan_id(), Some(PanId(0xbeef)));
assert_eq!(frame.get_dest_addr(), Some(dest));
assert_eq!(frame.get_src_pan_id(), None);
assert_eq!(frame.get_src_addr(), Some(src));
assert_eq!(frame.payload(), &[]);
}};
}
test!(ShortAddr(0x01_02), ShortAddr(0x03_04));
test!(ShortAddr(0x01_02), ExtendedAddr(0x03_04_05_06_07_08_09_0A));
test!(ExtendedAddr(0x01_02_03_04_05_06_07_08), ShortAddr(0x09_0A));
test!(
ExtendedAddr(0x01_02_03_04_05_06_07_08),
ExtendedAddr(0x09_0A_0B_0C_0D_0E_0F_10)
);
}
}