Skip to content

Commit

Permalink
Merge pull request #888 from thvdveld/decompress-sixlowpan
Browse files Browse the repository at this point in the history 
chore(iface/sixlowpan): rewrite sixlowpan_to_ipv6
  • Loading branch information
@thvdveld
thvdveld authored Jan 5, 2024
2 parents 2e60fc2 + 2051840 commit 4877a39
Show file tree
Hide file tree
Showing 2 changed files with 153 additions and 156 deletions.
8 changes: 3 additions & 5 deletions src/iface/interface/ieee802154.rs
View file
Original file line number Diff line number Diff line change
@@ -1,8 +1,5 @@
use super::*;

use crate::phy::TxToken;
use crate::wire::*;

impl InterfaceInner {
pub(super) fn process_ieee802154<'output, 'payload: 'output>(
&mut self,
Expand All @@ -12,12 +9,13 @@ impl InterfaceInner {
_fragments: &'output mut FragmentsBuffer,
) -> Option<Packet<'output>> {
let ieee802154_frame = check!(Ieee802154Frame::new_checked(sixlowpan_payload));
let ieee802154_repr = check!(Ieee802154Repr::parse(&ieee802154_frame));

if ieee802154_repr.frame_type != Ieee802154FrameType::Data {
if ieee802154_frame.frame_type() != Ieee802154FrameType::Data {
return None;
}

let ieee802154_repr = check!(Ieee802154Repr::parse(&ieee802154_frame));

// Drop frames when the user has set a PAN id and the PAN id from frame is not equal to this
// When the user didn't set a PAN id (so it is None), then we accept all PAN id's.
// We always accept the broadcast PAN id.
Expand Down
301 changes: 150 additions & 151 deletions src/iface/interface/sixlowpan.rs
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
use super::*;
use crate::wire::Result;

use crate::phy::ChecksumCapabilities;
use crate::wire::*;
Expand Down Expand Up @@ -131,13 +132,22 @@ impl InterfaceInner {
}
}

/// Decompress a 6LoWPAN packet into an IPv6 packet.
///
/// The return value is the length of the decompressed packet, but not including the total
/// length of the payload of the UDP packet. This value is then used by the assembler to know
/// how far in the assembler buffer the packet is.
///
/// **NOTE**: when decompressing a fragmented packet, the `total_len` parameter should be
/// passed. This is the total length of the IPv6 packet, including the IPv6 header. It is used
/// for calculating the length field in the UDP header.
fn sixlowpan_to_ipv6(
address_context: &[SixlowpanAddressContext],
ieee802154_repr: &Ieee802154Repr,
iphc_payload: &[u8],
total_size: Option<usize>,
total_len: Option<usize>,
buffer: &mut [u8],
) -> core::result::Result<usize, crate::wire::Error> {
) -> Result<usize> {
let iphc = SixlowpanIphcPacket::new_checked(iphc_payload)?;
let iphc_repr = SixlowpanIphcRepr::parse(
&iphc,
Expand All @@ -146,180 +156,84 @@ impl InterfaceInner {
address_context,
)?;

let first_next_header = match iphc_repr.next_header {
SixlowpanNextHeader::Compressed => {
match SixlowpanNhcPacket::dispatch(iphc.payload())? {
SixlowpanNhcPacket::ExtHeader => {
SixlowpanExtHeaderPacket::new_checked(iphc.payload())?
.extension_header_id()
.into()
}
SixlowpanNhcPacket::UdpHeader => IpProtocol::Udp,
}
}
SixlowpanNextHeader::Uncompressed(proto) => proto,
};
// The first thing we have to decompress is the IPv6 header. However, at this point we
// don't know the total size of the packet, neither the next header, since that can be a
// compressed header. However, we know that the IPv6 header is 40 bytes, so we can reserve
// this space in the buffer such that we can decompress the IPv6 header into it at a later
// point.
let (ipv6_buffer, mut buffer) = buffer.split_at_mut(40);
let mut ipv6_header = Ipv6Packet::new_unchecked(ipv6_buffer);

// If the total length is given, we are dealing with a fragmented packet. The total
// length is then used to calculate the length field for the UDP header. If the total
// length is not given, we are not working with a fragmented packet, and we need to
// calculate the length of the payload ourselves.
let mut payload_len = 40;
let mut decompressed_len = 40;

let mut decompressed_size = 40 + iphc.payload().len();
let mut next_header = Some(iphc_repr.next_header);
let mut data = iphc.payload();

while let Some(nh) = next_header {
match nh {
SixlowpanNextHeader::Compressed => match SixlowpanNhcPacket::dispatch(data)? {
SixlowpanNhcPacket::ExtHeader => {
let ext_hdr = SixlowpanExtHeaderPacket::new_checked(data)?;
let ext_repr = SixlowpanExtHeaderRepr::parse(&ext_hdr)?;
decompressed_size += 2;
decompressed_size -= ext_repr.buffer_len();
next_header = Some(ext_repr.next_header);

if ext_repr.buffer_len() + ext_repr.length as usize > data.len() {
return Err(Error);
}

data = &data[ext_repr.buffer_len() + ext_repr.length as usize..];
(buffer, data) = decompress_ext_hdr(
data,
&mut next_header,
buffer,
&mut payload_len,
&mut decompressed_len,
)?;
}
SixlowpanNhcPacket::UdpHeader => {
let udp_packet = SixlowpanUdpNhcPacket::new_checked(data)?;
let udp_repr = SixlowpanUdpNhcRepr::parse(
&udp_packet,
&iphc_repr.src_addr,
&iphc_repr.dst_addr,
&crate::phy::ChecksumCapabilities::ignored(),
decompress_udp(
data,
&iphc_repr,
buffer,
total_len,
&mut payload_len,
&mut decompressed_len,
)?;

decompressed_size += 8;
decompressed_size -= udp_repr.header_len();
break;
}
},
SixlowpanNextHeader::Uncompressed(proto) => match proto {
IpProtocol::Tcp => break,
IpProtocol::Udp => break,
IpProtocol::Icmpv6 => break,
proto => {
net_debug!("unable to decompress Uncompressed({})", proto);
return Err(Error);
SixlowpanNextHeader::Uncompressed(proto) => {
// We have a 6LoWPAN uncompressed header.
match proto {
IpProtocol::Tcp | IpProtocol::Udp | IpProtocol::Icmpv6 => {
// There can be no protocol after this one, so we can just copy the
// rest of the data buffer. There is also no length field in the UDP
// header that we need to correct as this header was not changed by the
// 6LoWPAN compressor.
if data.len() > buffer.len() {
return Err(Error);
}
buffer[..data.len()].copy_from_slice(data);
payload_len += data.len();
decompressed_len += data.len();
break;
}
proto => {
net_debug!("Unsupported uncompressed next header: {:?}", proto);
return Err(Error);
}
}
},
}
}
}

if buffer.len() < decompressed_size {
net_debug!("sixlowpan decompress: buffer too short");
return Err(crate::wire::Error);
}
let buffer = &mut buffer[..decompressed_size];

let total_size = if let Some(size) = total_size {
size
} else {
decompressed_size
};

let mut rest_size = total_size;

let ipv6_repr = Ipv6Repr {
src_addr: iphc_repr.src_addr,
dst_addr: iphc_repr.dst_addr,
next_header: first_next_header,
payload_len: total_size - 40,
next_header: decompress_next_header(iphc_repr.next_header, iphc.payload())?,
payload_len: total_len.unwrap_or(payload_len) - 40,
hop_limit: iphc_repr.hop_limit,
};
rest_size -= 40;

// Emit the decompressed IPHC header (decompressed to an IPv6 header).
let mut ipv6_packet = Ipv6Packet::new_unchecked(&mut buffer[..ipv6_repr.buffer_len()]);
ipv6_repr.emit(&mut ipv6_packet);
let mut buffer = &mut buffer[ipv6_repr.buffer_len()..];

let mut next_header = Some(iphc_repr.next_header);
let mut data = iphc.payload();

while let Some(nh) = next_header {
match nh {
SixlowpanNextHeader::Compressed => match SixlowpanNhcPacket::dispatch(data)? {
SixlowpanNhcPacket::ExtHeader => {
let ext_hdr = SixlowpanExtHeaderPacket::new_checked(data)?;
let ext_repr = SixlowpanExtHeaderRepr::parse(&ext_hdr)?;

let nh = match ext_repr.next_header {
SixlowpanNextHeader::Compressed => {
let d = &data[ext_repr.length as usize + ext_repr.buffer_len()..];
match SixlowpanNhcPacket::dispatch(d)? {
SixlowpanNhcPacket::ExtHeader => {
SixlowpanExtHeaderPacket::new_checked(d)?
.extension_header_id()
.into()
}
SixlowpanNhcPacket::UdpHeader => IpProtocol::Udp,
}
}
SixlowpanNextHeader::Uncompressed(proto) => proto,
};
next_header = Some(ext_repr.next_header);

let ipv6_ext_hdr = Ipv6ExtHeaderRepr {
next_header: nh,
length: ext_repr.length / 8,
data: &ext_hdr.payload()[..ext_repr.length as usize],
};

ipv6_ext_hdr.emit(&mut Ipv6ExtHeader::new_unchecked(
&mut buffer[..ipv6_ext_hdr.header_len()],
));
buffer[ipv6_ext_hdr.header_len()..][..ipv6_ext_hdr.data.len()]
.copy_from_slice(ipv6_ext_hdr.data);

buffer = &mut buffer[ipv6_ext_hdr.header_len() + ipv6_ext_hdr.data.len()..];

rest_size -= ipv6_ext_hdr.header_len() + ipv6_ext_hdr.data.len();
data = &data[ext_repr.buffer_len() + ext_repr.length as usize..];
}
SixlowpanNhcPacket::UdpHeader => {
let udp_packet = SixlowpanUdpNhcPacket::new_checked(data)?;
let payload = udp_packet.payload();
let udp_repr = SixlowpanUdpNhcRepr::parse(
&udp_packet,
&iphc_repr.src_addr,
&iphc_repr.dst_addr,
&ChecksumCapabilities::ignored(),
)?;

if payload.len() + 8 > buffer.len() {
return Err(Error);
}

let mut udp = UdpPacket::new_unchecked(&mut buffer[..payload.len() + 8]);
udp_repr
.0
.emit_header(&mut udp, rest_size - udp_repr.0.header_len());
buffer[8..][..payload.len()].copy_from_slice(payload);
ipv6_repr.emit(&mut ipv6_header);

break;
}
},
SixlowpanNextHeader::Uncompressed(proto) => match proto {
IpProtocol::HopByHop => unreachable!(),
IpProtocol::Tcp => {
buffer.copy_from_slice(data);
break;
}
IpProtocol::Udp => {
buffer.copy_from_slice(data);
break;
}
IpProtocol::Icmpv6 => {
buffer.copy_from_slice(data);
break;
}
_ => unreachable!(),
},
}
}

Ok(decompressed_size)
Ok(decompressed_len)
}

pub(super) fn dispatch_sixlowpan<Tx: TxToken>(
Expand Down Expand Up @@ -720,6 +634,91 @@ impl InterfaceInner {
}
}

/// Convert a 6LoWPAN next header to an IPv6 next header.
#[inline]
fn decompress_next_header(next_header: SixlowpanNextHeader, payload: &[u8]) -> Result<IpProtocol> {
match next_header {
SixlowpanNextHeader::Compressed => match SixlowpanNhcPacket::dispatch(payload)? {
SixlowpanNhcPacket::ExtHeader => {
let ext_hdr = SixlowpanExtHeaderPacket::new_checked(payload)?;
Ok(ext_hdr.extension_header_id().into())
}
SixlowpanNhcPacket::UdpHeader => Ok(IpProtocol::Udp),
},
SixlowpanNextHeader::Uncompressed(proto) => Ok(proto),
}
}

// NOTE: we always inline this function into the sixlowpan_to_ipv6 function, since it is only used there.
#[inline(always)]
fn decompress_ext_hdr<'d>(
mut data: &'d [u8],
next_header: &mut Option<SixlowpanNextHeader>,
mut buffer: &'d mut [u8],
payload_len: &mut usize,
decompressed_len: &mut usize,
) -> Result<(&'d mut [u8], &'d [u8])> {
let ext_hdr = SixlowpanExtHeaderPacket::new_checked(data)?;
let ext_repr = SixlowpanExtHeaderRepr::parse(&ext_hdr)?;
let nh = decompress_next_header(
ext_repr.next_header,
&data[ext_repr.length as usize + ext_repr.buffer_len()..],
)?;
*next_header = Some(ext_repr.next_header);
let ipv6_ext_hdr = Ipv6ExtHeaderRepr {
next_header: nh,
length: ext_repr.length / 8,
data: ext_hdr.payload(),
};
if ipv6_ext_hdr.header_len() + ipv6_ext_hdr.data.len() > buffer.len() {
return Err(Error);
}
ipv6_ext_hdr.emit(&mut Ipv6ExtHeader::new_unchecked(
&mut buffer[..ipv6_ext_hdr.header_len()],
));
buffer[ipv6_ext_hdr.header_len()..][..ipv6_ext_hdr.data.len()]
.copy_from_slice(ipv6_ext_hdr.data);
buffer = &mut buffer[ipv6_ext_hdr.header_len() + ipv6_ext_hdr.data.len()..];
*payload_len += ipv6_ext_hdr.header_len() + ipv6_ext_hdr.data.len();
*decompressed_len += ipv6_ext_hdr.header_len() + ipv6_ext_hdr.data.len();
data = &data[ext_repr.buffer_len() + ext_repr.length as usize..];
Ok((buffer, data))
}

// NOTE: we always inline this function into the sixlowpan_to_ipv6 function, since it is only used there.
#[inline(always)]
fn decompress_udp(
data: &[u8],
iphc_repr: &SixlowpanIphcRepr,
buffer: &mut [u8],
total_len: Option<usize>,
payload_len: &mut usize,
decompressed_len: &mut usize,
) -> Result<()> {
let udp_packet = SixlowpanUdpNhcPacket::new_checked(data)?;
let payload = udp_packet.payload();
let udp_repr = SixlowpanUdpNhcRepr::parse(
&udp_packet,
&iphc_repr.src_addr,
&iphc_repr.dst_addr,
&ChecksumCapabilities::ignored(),
)?;
if udp_repr.header_len() + payload.len() > buffer.len() {
return Err(Error);
}
let udp_payload_len = if let Some(total_len) = total_len {
total_len - *payload_len - 8
} else {
payload.len()
};
*payload_len += udp_payload_len + 8;
*decompressed_len += udp_repr.0.header_len() + payload.len();
let mut udp = UdpPacket::new_unchecked(&mut buffer[..payload.len() + 8]);
udp_repr.0.emit_header(&mut udp, udp_payload_len);
buffer[8..][..payload.len()].copy_from_slice(payload);
Ok(())
}

#[cfg(test)]
#[cfg(all(feature = "proto-rpl", feature = "proto-ipv6-hbh"))]
mod tests {
Expand Down

0 comments on commit 4877a39

Please sign in to comment.