capsules_extra/net/sixlowpan/
sixlowpan_compression.rs

1// Licensed under the Apache License, Version 2.0 or the MIT License.
2// SPDX-License-Identifier: Apache-2.0 OR MIT
3// Copyright Tock Contributors 2022.
4
5use crate::net::ieee802154::MacAddress;
6use crate::net::ipv6::ip_utils::{compute_udp_checksum, ip6_nh, IPAddr};
7use crate::net::ipv6::{IP6Header, IP6Packet, TransportHeader};
8use crate::net::udp::UDPHeader;
9use crate::net::util;
10use crate::net::util::{network_slice_to_u16, u16_to_network_slice};
11/// Implements the 6LoWPAN specification for sending IPv6 datagrams over
12/// 802.15.4 packets efficiently, as detailed in RFC 6282.
13use core::mem;
14
15/// Contains bit masks and constants related to the two-byte header of the
16/// LoWPAN_IPHC encoding format.
17mod iphc {
18    pub const DISPATCH: [u8; 2] = [0x60, 0x00];
19
20    // First byte masks
21
22    pub const TF_TRAFFIC_CLASS: u8 = 0x08;
23    pub const TF_FLOW_LABEL: u8 = 0x10;
24
25    pub const NH: u8 = 0x04;
26
27    pub const HLIM_MASK: u8 = 0x03;
28    pub const HLIM_INLINE: u8 = 0x00;
29    pub const HLIM_1: u8 = 0x01;
30    pub const HLIM_64: u8 = 0x02;
31    pub const HLIM_255: u8 = 0x03;
32
33    // Second byte masks
34
35    pub const CID: u8 = 0x80;
36
37    pub const SAC: u8 = 0x40;
38
39    pub const SAM_MASK: u8 = 0x30;
40    pub const SAM_INLINE: u8 = 0x00;
41    pub const SAM_MODE1: u8 = 0x10;
42    pub const SAM_MODE2: u8 = 0x20;
43    pub const SAM_MODE3: u8 = 0x30;
44
45    pub const MULTICAST: u8 = 0x08;
46
47    pub const DAC: u8 = 0x04;
48    pub const DAM_MASK: u8 = 0x03;
49    pub const DAM_INLINE: u8 = 0x00;
50    pub const DAM_MODE1: u8 = 0x01;
51    pub const DAM_MODE2: u8 = 0x02;
52    pub const DAM_MODE3: u8 = 0x03;
53
54    // Address compression
55    pub const MAC_BASE: [u8; 8] = [0, 0, 0, 0xff, 0xfe, 0, 0, 0];
56    pub const MAC_UL: u8 = 0x02;
57}
58
59/// Contains bit masks and constants related to LoWPAN_NHC encoding,
60/// including some specific to UDP header encoding
61mod nhc {
62    pub const DISPATCH_NHC: u8 = 0xe0;
63    pub const DISPATCH_UDP: u8 = 0xf0;
64    pub const DISPATCH_MASK: u8 = 0xf0;
65
66    pub const EID_MASK: u8 = 0x0e;
67    pub const HOP_OPTS: u8 = 0 << 1;
68    pub const ROUTING: u8 = 1 << 1;
69    pub const FRAGMENT: u8 = 2 << 1;
70    pub const DST_OPTS: u8 = 3 << 1;
71    pub const MOBILITY: u8 = 4 << 1;
72    pub const IP6: u8 = 7 << 1;
73
74    pub const NH: u8 = 0x01;
75
76    // UDP header compression
77
78    pub const UDP_4BIT_PORT: u16 = 0xf0b0;
79    pub const UDP_4BIT_PORT_MASK: u16 = 0xfff0;
80    pub const UDP_8BIT_PORT: u16 = 0xf000;
81    pub const UDP_8BIT_PORT_MASK: u16 = 0xff00;
82
83    pub const UDP_CHECKSUM_FLAG: u8 = 0b100;
84    pub const UDP_SRC_PORT_FLAG: u8 = 0b010;
85    pub const UDP_DST_PORT_FLAG: u8 = 0b001;
86    pub const UDP_PORTS_SIZE: u16 = 4;
87}
88
89#[derive(Copy, Clone, Debug)]
90pub struct Context {
91    pub prefix: [u8; 16],
92    pub prefix_len: u8,
93    pub id: u8,
94    pub compress: bool,
95}
96
97/// LoWPan ContextStore
98///
99/// LoWPAN encoding requires being able to look up the existence of contexts,
100/// which are essentially IPv6 address prefixes. Any implementation must ensure
101/// that context 0 is always available and contains the mesh-local prefix.
102pub trait ContextStore {
103    fn get_context_from_addr(&self, ip_addr: IPAddr) -> Option<Context>;
104    fn get_context_from_id(&self, ctx_id: u8) -> Option<Context>;
105    fn get_context_0(&self) -> Context {
106        match self.get_context_from_id(0) {
107            Some(ctx) => ctx,
108            None => panic!("Context 0 not found"),
109        }
110    }
111    fn get_context_from_prefix(&self, prefix: &[u8], prefix_len: u8) -> Option<Context>;
112}
113
114/// Computes the LoWPAN Interface Identifier from either the 16-bit short MAC or
115/// the IEEE EUI-64 that is derived from the 48-bit MAC.
116pub fn compute_iid(mac_addr: &MacAddress) -> [u8; 8] {
117    match *mac_addr {
118        MacAddress::Short(short_addr) => {
119            // IID is 0000:00ff:fe00:XXXX, where XXXX is 16-bit MAC
120            let mut iid: [u8; 8] = iphc::MAC_BASE;
121            iid[6] = (short_addr >> 1) as u8;
122            iid[7] = (short_addr & 0xff) as u8;
123            iid
124        }
125        MacAddress::Long(long_addr) => {
126            // IID is IEEE EUI-64 with universal/local bit inverted
127            let mut iid: [u8; 8] = long_addr;
128            iid[0] ^= iphc::MAC_UL;
129            iid
130        }
131    }
132}
133
134impl ContextStore for Context {
135    fn get_context_from_addr(&self, ip_addr: IPAddr) -> Option<Context> {
136        if util::matches_prefix(&ip_addr.0, &self.prefix, self.prefix_len) {
137            Some(*self)
138        } else {
139            None
140        }
141    }
142
143    fn get_context_from_id(&self, ctx_id: u8) -> Option<Context> {
144        if ctx_id == 0 {
145            Some(*self)
146        } else {
147            None
148        }
149    }
150
151    fn get_context_from_prefix(&self, prefix: &[u8], prefix_len: u8) -> Option<Context> {
152        if prefix_len == self.prefix_len && util::matches_prefix(prefix, &self.prefix, prefix_len) {
153            Some(*self)
154        } else {
155            None
156        }
157    }
158}
159
160pub fn is_lowpan(packet: &[u8]) -> bool {
161    (packet[0] & iphc::DISPATCH[0]) == iphc::DISPATCH[0]
162}
163
164/// Maps a LoWPAN_NHC header the corresponding IPv6 next header type,
165/// or an error if the NHC header is invalid
166fn nhc_to_ip6_nh(nhc: u8) -> Result<u8, ()> {
167    match nhc & nhc::DISPATCH_MASK {
168        nhc::DISPATCH_NHC => match nhc & nhc::EID_MASK {
169            nhc::HOP_OPTS => Ok(ip6_nh::HOP_OPTS),
170            nhc::ROUTING => Ok(ip6_nh::ROUTING),
171            nhc::FRAGMENT => Ok(ip6_nh::FRAGMENT),
172            nhc::DST_OPTS => Ok(ip6_nh::DST_OPTS),
173            nhc::MOBILITY => Ok(ip6_nh::MOBILITY),
174            nhc::IP6 => Ok(ip6_nh::IP6),
175            _ => Err(()),
176        },
177        nhc::DISPATCH_UDP => Ok(ip6_nh::UDP),
178        _ => Err(()),
179    }
180}
181
182/// Compresses an IPv6 header into a 6loWPAN header
183///
184/// Constructs a 6LoWPAN header in `buf` from the given IPv6 datagram and
185/// 16-bit MAC addresses. If the compression was successful, returns
186/// `Ok((consumed, written))`, where `consumed` is the number of header
187/// bytes consumed from the IPv6 datagram `written` is the number of
188/// compressed header bytes written into `buf`. Payload bytes and
189/// non-compressed next headers are not written, so the remaining `buf.len()
190/// - consumed` bytes must still be copied over to `buf`.
191pub fn compress<'a>(
192    ctx_store: &dyn ContextStore,
193    ip6_packet: &'a IP6Packet<'a>,
194    src_mac_addr: MacAddress,
195    dst_mac_addr: MacAddress,
196    buf: &mut [u8],
197) -> Result<(usize, usize), ()> {
198    // Note that consumed should be constant, and equal sizeof(IP6Header)
199    //let (mut consumed, ip6_header) = IP6Header::decode(ip6_datagram).done().ok_or(())?;
200    let mut consumed = 40; // TODO
201    let ip6_header = ip6_packet.header;
202
203    //let mut next_headers: &[u8] = &ip6_datagram[consumed..];
204
205    // The first two bytes are the LOWPAN_IPHC header
206    let mut written: usize = 2;
207
208    // Initialize the LOWPAN_IPHC header
209    buf[0..2].copy_from_slice(&iphc::DISPATCH);
210
211    let mut src_ctx: Option<Context> = ctx_store.get_context_from_addr(ip6_header.src_addr);
212    let mut dst_ctx: Option<Context> = if ip6_header.dst_addr.is_multicast() {
213        let prefix_len: u8 = ip6_header.dst_addr.0[3];
214        let prefix: &[u8] = &ip6_header.dst_addr.0[4..12];
215        // This also implicitly verifies that prefix_len <= 64
216        if util::verify_prefix_len(prefix, prefix_len) {
217            ctx_store.get_context_from_prefix(prefix, prefix_len)
218        } else {
219            None
220        }
221    } else {
222        ctx_store.get_context_from_addr(ip6_header.dst_addr)
223    };
224
225    // Do not contexts that are not marked to be available for compression
226    src_ctx = src_ctx.and_then(|ctx| if ctx.compress { Some(ctx) } else { None });
227    dst_ctx = dst_ctx.and_then(|ctx| if ctx.compress { Some(ctx) } else { None });
228
229    // Context Identifier Extension
230    compress_cie(src_ctx.as_ref(), dst_ctx.as_ref(), buf, &mut written);
231
232    // Traffic Class & Flow Label
233    compress_tf(&ip6_header, buf, &mut written);
234
235    // Next Header
236
237    //let (mut is_nhc, mut nh_len): (bool, u8) = is_ip6_nh_compressible(ip6_packet)?;
238    let is_nhc = ip6_header.next_header == ip6_nh::UDP;
239    compress_nh(&ip6_header, is_nhc, buf, &mut written);
240
241    // Hop Limit
242    compress_hl(&ip6_header, buf, &mut written);
243
244    // Source Address
245    compress_src(
246        &ip6_header.src_addr,
247        &src_mac_addr,
248        src_ctx.as_ref(),
249        buf,
250        &mut written,
251    );
252
253    // Destination Address
254    if ip6_header.dst_addr.is_multicast() {
255        compress_multicast(&ip6_header.dst_addr, dst_ctx.as_ref(), buf, &mut written);
256    } else {
257        compress_dst(
258            &ip6_header.dst_addr,
259            &dst_mac_addr,
260            dst_ctx.as_ref(),
261            buf,
262            &mut written,
263        );
264    }
265
266    // Next Headers
267    // At each iteration, next_headers begins at the first byte of the
268    // current uncompressed next header.
269    // Since we aren't recursing, we only handle UDP
270    if is_nhc {
271        match ip6_packet.payload.header {
272            TransportHeader::UDP(udp_header) => {
273                let mut nhc_header = nhc::DISPATCH_UDP;
274
275                // Leave a space for the UDP LoWPAN_NHC byte
276                let udp_nh_offset = written;
277                written += 1;
278
279                // Compress ports and checksum
280                nhc_header |= compress_udp_ports(&udp_header, buf, &mut written);
281                nhc_header |= compress_udp_checksum(&udp_header, buf, &mut written);
282
283                // Write the UDP LoWPAN_NHC byte
284                buf[udp_nh_offset] = nhc_header;
285                consumed += 8;
286            }
287            // Return an error, as there is a conflict between IPv6 next
288            // header and actual IPv6 payload
289            _ => return Err(()),
290        }
291    }
292    Ok((consumed, written))
293}
294
295fn compress_cie(
296    src_ctx: Option<&Context>,
297    dst_ctx: Option<&Context>,
298    buf: &mut [u8],
299    written: &mut usize,
300) {
301    let mut cie: u8 = 0;
302
303    src_ctx.as_ref().map(|ctx| {
304        if ctx.id != 0 {
305            cie |= ctx.id << 4;
306        }
307    });
308    dst_ctx.as_ref().map(|ctx| {
309        if ctx.id != 0 {
310            cie |= ctx.id;
311        }
312    });
313
314    if cie != 0 {
315        buf[1] |= iphc::CID;
316        buf[*written] = cie;
317        *written += 1;
318    }
319}
320
321fn compress_tf(ip6_header: &IP6Header, buf: &mut [u8], written: &mut usize) {
322    let ecn = ip6_header.get_ecn();
323    let dscp = ip6_header.get_dscp();
324    let flow = ip6_header.get_flow_label();
325
326    let mut tf_encoding = 0;
327    let old_offset = *written;
328
329    // If ECN != 0 we are forced to at least have one byte,
330    // otherwise we can elide dscp
331    if dscp == 0 && (ecn == 0 || flow != 0) {
332        tf_encoding |= iphc::TF_TRAFFIC_CLASS;
333    } else {
334        buf[*written] = dscp;
335        *written += 1;
336    }
337
338    // We can elide flow if it is 0
339    if flow == 0 {
340        tf_encoding |= iphc::TF_FLOW_LABEL;
341    } else {
342        buf[*written] = ((flow >> 16) & 0x0f) as u8;
343        buf[*written + 1] = (flow >> 8) as u8;
344        buf[*written + 2] = flow as u8;
345        *written += 3;
346    }
347
348    if *written != old_offset {
349        buf[old_offset] |= ecn << 6;
350    }
351    buf[0] |= tf_encoding;
352}
353
354fn compress_nh(ip6_header: &IP6Header, is_nhc: bool, buf: &mut [u8], written: &mut usize) {
355    if is_nhc {
356        buf[0] |= iphc::NH;
357    } else {
358        buf[*written] = ip6_header.next_header;
359        *written += 1;
360    }
361}
362
363fn compress_hl(ip6_header: &IP6Header, buf: &mut [u8], written: &mut usize) {
364    let hop_limit_flag = match ip6_header.hop_limit {
365        1 => iphc::HLIM_1,
366        64 => iphc::HLIM_64,
367        255 => iphc::HLIM_255,
368        _ => {
369            buf[*written] = ip6_header.hop_limit;
370            *written += 1;
371            iphc::HLIM_INLINE
372        }
373    };
374    buf[0] |= hop_limit_flag;
375}
376
377// TODO: We should check to see whether context or link local compression
378// schemes gives the better compression; currently, we will always match
379// on link local even if we could get better compression through context.
380fn compress_src(
381    src_ip_addr: &IPAddr,
382    src_mac_addr: &MacAddress,
383    src_ctx: Option<&Context>,
384    buf: &mut [u8],
385    written: &mut usize,
386) {
387    if src_ip_addr.is_unspecified() {
388        // SAC = 1, SAM = 00
389        buf[1] |= iphc::SAC;
390    } else if src_ip_addr.is_unicast_link_local() {
391        // SAC = 0, SAM = 01, 10, 11
392        compress_iid(src_ip_addr, src_mac_addr, true, buf, written);
393    } else if src_ctx.is_some() {
394        // SAC = 1, SAM = 01, 10, 11
395        buf[1] |= iphc::SAC;
396        compress_iid(src_ip_addr, src_mac_addr, true, buf, written);
397    } else {
398        // SAC = 0, SAM = 00
399        buf[*written..*written + 16].copy_from_slice(&src_ip_addr.0);
400        *written += 16;
401    }
402}
403
404// TODO: For the SAC = 0, SAM = 11 case in IPv6-encapsulated headers,
405// it might be that we have to compute the IID from the encapsulating
406// IPv6 header address instead of the EUI-64 from the 802.15.4 layer
407fn compress_iid(
408    ip_addr: &IPAddr,
409    mac_addr: &MacAddress,
410    is_src: bool,
411    buf: &mut [u8],
412    written: &mut usize,
413) {
414    let iid: [u8; 8] = compute_iid(mac_addr);
415    if ip_addr.0[8..16] == iid {
416        // SAM/DAM = 11, 0 bits
417        buf[1] |= if is_src {
418            iphc::SAM_MODE3
419        } else {
420            iphc::DAM_MODE3
421        };
422    } else if ip_addr.0[8..14] == iphc::MAC_BASE[0..6] {
423        // SAM/DAM = 10, 16 bits
424        buf[1] |= if is_src {
425            iphc::SAM_MODE2
426        } else {
427            iphc::DAM_MODE2
428        };
429        buf[*written..*written + 2].copy_from_slice(&ip_addr.0[14..16]);
430        *written += 2;
431    } else {
432        // SAM/DAM = 01, 64 bits
433        buf[1] |= if is_src {
434            iphc::SAM_MODE1
435        } else {
436            iphc::DAM_MODE1
437        };
438        buf[*written..*written + 8].copy_from_slice(&ip_addr.0[8..16]);
439        *written += 8;
440    }
441}
442
443// Compresses non-multicast destination address
444// TODO: We should check to see whether context or link local compression
445// schemes gives the better compression; currently, we will always match
446// on link local even if we could get better compression through context.
447fn compress_dst(
448    dst_ip_addr: &IPAddr,
449    dst_mac_addr: &MacAddress,
450    dst_ctx: Option<&Context>,
451    buf: &mut [u8],
452    written: &mut usize,
453) {
454    // Assumes dst_ip_addr is not a multicast address (prefix ffXX)
455    if dst_ip_addr.is_unicast_link_local() {
456        // Link local compression
457        // M = 0, DAC = 0, DAM = 01, 10, 11
458        compress_iid(dst_ip_addr, dst_mac_addr, false, buf, written);
459    } else if dst_ctx.is_some() {
460        // Context compression
461        // DAC = 1, DAM = 01, 10, 11
462        buf[1] |= iphc::DAC;
463        compress_iid(dst_ip_addr, dst_mac_addr, false, buf, written);
464    } else {
465        // Full address inline
466        // DAC = 0, DAM = 00
467        buf[*written..*written + 16].copy_from_slice(&dst_ip_addr.0);
468        *written += 16;
469    }
470}
471
472// Compresses multicast destination addresses
473fn compress_multicast(
474    dst_ip_addr: &IPAddr,
475    dst_ctx: Option<&Context>,
476    buf: &mut [u8],
477    written: &mut usize,
478) {
479    // Assumes dst_ip_addr is indeed a multicast address (prefix ffXX)
480    buf[1] |= iphc::MULTICAST;
481    if dst_ctx.is_some() {
482        // M = 1, DAC = 1, DAM = 00
483        buf[1] |= iphc::DAC;
484        buf[*written..*written + 2].copy_from_slice(&dst_ip_addr.0[1..3]);
485        buf[*written + 2..*written + 6].copy_from_slice(&dst_ip_addr.0[12..16]);
486        *written += 6;
487    } else {
488        // M = 1, DAC = 0
489        if dst_ip_addr.0[1] == 0x02 && dst_ip_addr.0[2..15].iter().all(|&b| b == 0) {
490            // DAM = 11
491            buf[1] |= iphc::DAM_MODE3;
492            buf[*written] = dst_ip_addr.0[15];
493            *written += 1;
494        } else {
495            if !dst_ip_addr.0[2..11].iter().all(|&b| b == 0) {
496                // DAM = 00
497                buf[1] |= iphc::DAM_INLINE;
498                buf[*written..*written + 16].copy_from_slice(&dst_ip_addr.0);
499                *written += 16;
500            } else if !dst_ip_addr.0[11..13].iter().all(|&b| b == 0) {
501                // DAM = 01, ffXX::00XX:XXXX:XXXX
502                buf[1] |= iphc::DAM_MODE1;
503                buf[*written] = dst_ip_addr.0[1];
504                buf[*written + 1..*written + 6].copy_from_slice(&dst_ip_addr.0[11..16]);
505                *written += 6;
506            } else {
507                // DAM = 10, ffXX::00XX:XXXX
508                buf[1] |= iphc::DAM_MODE2;
509                buf[*written] = dst_ip_addr.0[1];
510                buf[*written + 1..*written + 4].copy_from_slice(&dst_ip_addr.0[13..16]);
511                *written += 4;
512            }
513        }
514    }
515}
516
517fn compress_udp_ports(udp_header: &UDPHeader, buf: &mut [u8], written: &mut usize) -> u8 {
518    // Need to deal with fields in network byte order when writing directly to buf
519    let src_port = udp_header.get_src_port().to_be();
520    let dst_port = udp_header.get_dst_port().to_be();
521
522    let mut udp_port_nhc = 0;
523    if (src_port & nhc::UDP_4BIT_PORT_MASK) == nhc::UDP_4BIT_PORT
524        && (dst_port & nhc::UDP_4BIT_PORT_MASK) == nhc::UDP_4BIT_PORT
525    {
526        // Both can be compressed to 4 bits
527        udp_port_nhc |= nhc::UDP_SRC_PORT_FLAG | nhc::UDP_DST_PORT_FLAG;
528        // This should compress the ports to a single 8-bit value,
529        // with the source port before the destination port
530        buf[*written] = (((src_port & !nhc::UDP_4BIT_PORT_MASK) << 4)
531            | (dst_port & !nhc::UDP_4BIT_PORT_MASK)) as u8;
532        *written += 1;
533    } else if (src_port & nhc::UDP_8BIT_PORT_MASK) == nhc::UDP_8BIT_PORT {
534        // Source port compressed to 8 bits, destination port uncompressed
535        udp_port_nhc |= nhc::UDP_SRC_PORT_FLAG;
536        buf[*written] = (src_port & !nhc::UDP_8BIT_PORT_MASK) as u8;
537        u16_to_network_slice(dst_port.to_be(), &mut buf[*written + 1..*written + 3]);
538        *written += 3;
539    } else if (dst_port & nhc::UDP_8BIT_PORT_MASK) == nhc::UDP_8BIT_PORT {
540        udp_port_nhc |= nhc::UDP_DST_PORT_FLAG;
541        u16_to_network_slice(src_port.to_be(), &mut buf[*written..*written + 2]);
542        buf[*written + 3] = (dst_port & !nhc::UDP_8BIT_PORT_MASK) as u8;
543        *written += 3;
544    } else {
545        buf[*written] = src_port as u8;
546        buf[*written + 1] = (src_port >> 8) as u8;
547        buf[*written + 2] = dst_port as u8;
548        buf[*written + 3] = (dst_port >> 8) as u8;
549        //buf[*written..*written + 4].copy_from_slice(&udp_header[0..4]);
550        *written += 4;
551    }
552    udp_port_nhc
553}
554
555// NOTE: We currently only support (or intend to support) carrying the UDP
556// checksum inline.
557fn compress_udp_checksum(udp_header: &UDPHeader, buf: &mut [u8], written: &mut usize) -> u8 {
558    // get_cksum returns cksum in host byte order
559    let cksum = udp_header.get_cksum().to_be();
560    buf[*written] = cksum as u8;
561    buf[*written + 1] = (cksum >> 8) as u8;
562    *written += 2;
563    // Inline checksum corresponds to the 0 flag
564    0
565}
566
567/// Decompresses a 6loWPAN header into a full IPv6 header
568///
569/// This function decompresses the header found in `buf` and writes it
570/// `out_buf`. It does not, though, copy payload bytes or a non-compressed next
571/// header. As a result, the caller should copy the `buf.len - consumed`
572/// remaining bytes from `buf` to `out_buf`.
573///
574///
575/// Note that in the case of fragmentation, the total length of the IPv6
576/// packet cannot be inferred from a single frame, and is instead provided
577/// by the dgram_size field in the fragmentation header. Thus, if we are
578/// decompressing a fragment, we rely on the dgram_size field; otherwise,
579/// we infer the length from the size of buf.
580///
581/// # Arguments
582///
583/// * `ctx_store` - ???
584///
585/// * `buf` - A slice containing the 6LowPAN packet along with its payload.
586///
587/// * `src_mac_addr` - the 16-bit MAC address of the frame sender.
588///
589/// * `dst_mac_addr` - the 16-bit MAC address of the frame receiver.
590///
591/// * `out_buf` - A buffer to write the output to. Must be at least large enough
592/// to store an IPv6 header (XX bytes).
593///
594/// * `dgram_size` - If `is_fragment` is `true`, this is used as the IPv6
595/// packets total payload size. Otherwise, this is ignored.
596///
597/// * `is_fragment` - ???
598///
599/// # Returns
600///
601/// `Ok((consumed, written))` if decompression is successful.
602///
603/// * `consumed` is the number of header bytes consumed from the 6LoWPAN header
604///
605/// * `written` is the number of uncompressed header bytes written into
606/// `out_buf`.
607pub fn decompress(
608    ctx_store: &dyn ContextStore,
609    buf: &[u8],
610    src_mac_addr: MacAddress,
611    dst_mac_addr: MacAddress,
612    out_buf: &mut [u8],
613    dgram_size: u16,
614    is_fragment: bool,
615) -> Result<(usize, usize), ()> {
616    // Get the LOWPAN_IPHC header (the first two bytes are the header)
617    let iphc_header_1: u8 = buf[0];
618    let iphc_header_2: u8 = buf[1];
619    let mut consumed: usize = 2;
620
621    let mut ip6_header = IP6Header::new();
622    let mut written: usize = mem::size_of::<IP6Header>();
623
624    // Decompress CID and CIE fields if they exist
625    let (src_ctx, dst_ctx) = decompress_cie(ctx_store, iphc_header_1, buf, &mut consumed)?;
626
627    // Traffic Class & Flow Label
628    decompress_tf(&mut ip6_header, iphc_header_1, buf, &mut consumed);
629
630    // Next Header
631    let (mut is_nhc, mut next_header) = decompress_nh(iphc_header_1, buf, &mut consumed);
632
633    // Hop Limit
634    decompress_hl(&mut ip6_header, iphc_header_1, buf, &mut consumed)?;
635
636    // Source Address
637    decompress_src(
638        &mut ip6_header,
639        iphc_header_2,
640        &src_mac_addr,
641        &src_ctx,
642        buf,
643        &mut consumed,
644    )?;
645
646    // Destination Address
647    if (iphc_header_2 & iphc::MULTICAST) != 0 {
648        decompress_multicast(&mut ip6_header, iphc_header_2, &dst_ctx, buf, &mut consumed)?;
649    } else {
650        decompress_dst(
651            &mut ip6_header,
652            iphc_header_2,
653            &dst_mac_addr,
654            &dst_ctx,
655            buf,
656            &mut consumed,
657        )?;
658    }
659
660    // next_header is already set if is_nhc is false, otherwise it can be
661    // determined from the LoWPAN NHC header byte
662    if is_nhc {
663        next_header = nhc_to_ip6_nh(buf[consumed])?;
664    }
665    ip6_header.set_next_header(next_header);
666
667    // Next headers after the IPv6 fixed header
668    // At each iteration, consumed points to the first byte of the compressed
669    // next header in buf.
670    while is_nhc {
671        // Advance past the LoWPAN NHC byte
672        let nhc_header = buf[consumed];
673        consumed += 1;
674
675        // Scoped mutable borrow of out_buf
676        let next_headers: &mut [u8] = &mut out_buf[written..];
677
678        match next_header {
679            ip6_nh::IP6 => {
680                let (encap_consumed, encap_written) = decompress(
681                    ctx_store,
682                    &buf[consumed..],
683                    src_mac_addr,
684                    dst_mac_addr,
685                    next_headers,
686                    dgram_size,
687                    is_fragment,
688                )?;
689                consumed += encap_consumed;
690                written += encap_written;
691                break;
692            }
693            ip6_nh::UDP => {
694                // UDP length includes UDP header and data in bytes
695                // Below line works bc udp nh must be last nh per 6282
696                let mut udp_length = if is_fragment {
697                    dgram_size - written as u16
698                } else {
699                    buf.len() as u16 - consumed as u16
700                };
701
702                // Decompress UDP header fields
703                let consumed_before_port_decompress = consumed;
704                let (src_port, dst_port) = decompress_udp_ports(nhc_header, buf, &mut consumed);
705
706                //need to add any growth from decompression to the udp length if we used the buf
707                //len to calculate the length
708                if !is_fragment {
709                    // Expansion from port compression
710                    udp_length +=
711                        nhc::UDP_PORTS_SIZE - ((consumed - consumed_before_port_decompress) as u16);
712                    // Expansion from length elision (always applied per RFC 6282, length is 2
713                    // bytes)
714                    udp_length += 2;
715                    // UDP checksum elision
716                    if (nhc_header & nhc::UDP_CHECKSUM_FLAG) != 0 {
717                        udp_length += 2;
718                    }
719                }
720
721                // Fill in uncompressed UDP header
722                // TODO: The current implementation works, but I don't understand the calls to
723                // to_be(), because src_port.to_be() returns the src_port in little endian..
724                // Accordingly, the udp_length must also be written in little endian for this
725                // to work.
726                u16_to_network_slice(src_port.to_be(), &mut next_headers[0..2]);
727                u16_to_network_slice(dst_port.to_be(), &mut next_headers[2..4]);
728                u16_to_network_slice(udp_length, &mut next_headers[4..6]);
729                // Need to fill in header values before computing the checksum
730                let udp_checksum = decompress_udp_checksum(
731                    nhc_header,
732                    &next_headers[0..8],
733                    udp_length,
734                    &ip6_header,
735                    buf,
736                    &mut consumed,
737                    is_fragment,
738                );
739                u16_to_network_slice(udp_checksum.to_be(), &mut next_headers[6..8]);
740
741                written += 8;
742                break;
743            }
744            ip6_nh::FRAGMENT
745            | ip6_nh::HOP_OPTS
746            | ip6_nh::ROUTING
747            | ip6_nh::DST_OPTS
748            | ip6_nh::MOBILITY => {
749                // True if the next header is also compressed
750                is_nhc = (nhc_header & nhc::NH) != 0;
751
752                // len is the number of octets following the length field
753                let len = buf[consumed] as usize;
754                consumed += 1;
755
756                // Check that there is a next header in the buffer,
757                // which must be the case if the last next header specifies
758                // NH = 1
759                if consumed + len >= buf.len() {
760                    return Err(());
761                }
762
763                // Length in 8-octet units after the first 8 octets
764                // (per the IPv6 ext hdr spec)
765                let mut hdr_len_field = (len - 6) / 8;
766                if (len - 6) % 8 != 0 {
767                    hdr_len_field += 1;
768                }
769
770                // Gets the type of the subsequent next header.  If is_nhc
771                // is true, there must be a LoWPAN NHC header byte,
772                // otherwise there is either an uncompressed next header.
773                next_header = if is_nhc {
774                    // The next header is LoWPAN NHC-compressed
775                    nhc_to_ip6_nh(buf[consumed + len])?
776                } else {
777                    // The next header is uncompressed
778                    buf[consumed + len]
779                };
780
781                // Fill in the extended header in uncompressed IPv6 format
782                next_headers[0] = next_header;
783                next_headers[1] = hdr_len_field as u8;
784                // Copies over the remaining options.
785                next_headers[2..2 + len].copy_from_slice(&buf[consumed..consumed + len]);
786
787                // Fill in padding
788                let pad_bytes = hdr_len_field * 8 - len + 6;
789                if pad_bytes == 1 {
790                    // Pad1
791                    next_headers[2 + len] = 0;
792                } else {
793                    // PadN, 2 <= pad_bytes <= 7
794                    next_headers[2 + len] = 1;
795                    next_headers[2 + len + 1] = pad_bytes as u8 - 2;
796                    for i in 2..pad_bytes {
797                        next_headers[2 + len + i] = 0;
798                    }
799                }
800
801                written += 8 + hdr_len_field * 8;
802                consumed += len;
803            }
804            _ => panic!("Unreachable case"),
805        }
806    }
807
808    // The IPv6 header length field is the size of the IPv6 payload,
809    // including extension headers. This is thus the uncompressed
810    // size of the IPv6 packet - the fixed IPv6 header.
811    let payload_len = if is_fragment {
812        (dgram_size as usize) - mem::size_of::<IP6Header>()
813    } else {
814        written + (buf.len() - consumed) - mem::size_of::<IP6Header>()
815    };
816    ip6_header.payload_len = (payload_len as u16).to_be();
817    IP6Header::encode(&ip6_header, out_buf).done().ok_or(())?;
818    Ok((consumed, written))
819}
820
821fn decompress_cie(
822    ctx_store: &dyn ContextStore,
823    iphc_header: u8,
824    buf: &[u8],
825    consumed: &mut usize,
826) -> Result<(Context, Context), ()> {
827    let ctx_0 = ctx_store.get_context_0();
828    let (mut src_ctx, mut dst_ctx) = (ctx_0, ctx_0);
829    if iphc_header & iphc::CID != 0 {
830        let sci = buf[*consumed] >> 4;
831        let dci = buf[*consumed] & 0xf;
832        *consumed += 1;
833
834        if sci != 0 {
835            src_ctx = ctx_store.get_context_from_id(sci).ok_or(())?;
836        }
837        if dci != 0 {
838            dst_ctx = ctx_store.get_context_from_id(dci).ok_or(())?;
839        }
840    }
841    Ok((src_ctx, dst_ctx))
842}
843
844fn decompress_tf(ip6_header: &mut IP6Header, iphc_header: u8, buf: &[u8], consumed: &mut usize) {
845    let fl_compressed = (iphc_header & iphc::TF_FLOW_LABEL) != 0;
846    let tc_compressed = (iphc_header & iphc::TF_TRAFFIC_CLASS) != 0;
847
848    // Determine ECN and DSCP separately because the order is different
849    // from the IPv6 traffic class field.
850    if !fl_compressed || !tc_compressed {
851        let ecn = buf[*consumed] >> 6;
852        ip6_header.set_ecn(ecn);
853    }
854    if !tc_compressed {
855        let dscp = buf[*consumed] & 0b111111;
856        ip6_header.set_dscp(dscp);
857        *consumed += 1;
858    }
859
860    // Flow label is always in the same bit position relative to the last
861    // three bytes in the inline fields
862    if fl_compressed {
863        ip6_header.set_flow_label(0);
864    } else {
865        let flow = (((buf[*consumed] & 0x0f) as u32) << 16)
866            | ((buf[*consumed + 1] as u32) << 8)
867            | (buf[*consumed + 2] as u32);
868        *consumed += 3;
869        ip6_header.set_flow_label(flow);
870    }
871}
872
873fn decompress_nh(iphc_header: u8, buf: &[u8], consumed: &mut usize) -> (bool, u8) {
874    let is_nhc = (iphc_header & iphc::NH) != 0;
875    let mut next_header: u8 = 0;
876    if !is_nhc {
877        next_header = buf[*consumed];
878        *consumed += 1;
879    }
880    (is_nhc, next_header)
881}
882
883fn decompress_hl(
884    ip6_header: &mut IP6Header,
885    iphc_header: u8,
886    buf: &[u8],
887    consumed: &mut usize,
888) -> Result<(), ()> {
889    let hop_limit = match iphc_header & iphc::HLIM_MASK {
890        iphc::HLIM_1 => 1,
891        iphc::HLIM_64 => 64,
892        iphc::HLIM_255 => 255,
893        iphc::HLIM_INLINE => {
894            let hl = buf[*consumed];
895            *consumed += 1;
896            hl
897        }
898        _ => panic!("Unreachable case"),
899    };
900    ip6_header.set_hop_limit(hop_limit);
901    Ok(())
902}
903
904fn decompress_src(
905    ip6_header: &mut IP6Header,
906    iphc_header: u8,
907    mac_addr: &MacAddress,
908    ctx: &Context,
909    buf: &[u8],
910    consumed: &mut usize,
911) -> Result<(), ()> {
912    let uses_context = (iphc_header & iphc::SAC) != 0;
913    let sam_mode = iphc_header & iphc::SAM_MASK;
914    if uses_context && sam_mode == iphc::SAM_INLINE {
915        // SAC = 1, SAM = 00: UNSPECIFIED (::), which is already the default
916    } else if uses_context {
917        // SAC = 1, SAM = 01, 10, 11
918        decompress_iid_context(
919            sam_mode,
920            &mut ip6_header.src_addr,
921            mac_addr,
922            ctx,
923            buf,
924            consumed,
925        )?;
926    } else {
927        // SAC = 0, SAM = 00, 01, 10, 11
928        decompress_iid_link_local(sam_mode, &mut ip6_header.src_addr, mac_addr, buf, consumed)?;
929    }
930    Ok(())
931}
932
933fn decompress_dst(
934    ip6_header: &mut IP6Header,
935    iphc_header: u8,
936    mac_addr: &MacAddress,
937    ctx: &Context,
938    buf: &[u8],
939    consumed: &mut usize,
940) -> Result<(), ()> {
941    let uses_context = (iphc_header & iphc::DAC) != 0;
942    let dam_mode = iphc_header & iphc::DAM_MASK;
943    if uses_context && dam_mode == iphc::DAM_INLINE {
944        // DAC = 1, DAM = 00: Reserved
945        return Err(());
946    } else if uses_context {
947        // DAC = 1, DAM = 01, 10, 11
948        decompress_iid_context(
949            dam_mode,
950            &mut ip6_header.dst_addr,
951            mac_addr,
952            ctx,
953            buf,
954            consumed,
955        )?;
956    } else {
957        // DAC = 0, DAM = 00, 01, 10, 11
958        decompress_iid_link_local(dam_mode, &mut ip6_header.dst_addr, mac_addr, buf, consumed)?;
959    }
960    Ok(())
961}
962
963fn decompress_multicast(
964    ip6_header: &mut IP6Header,
965    iphc_header: u8,
966    ctx: &Context,
967    buf: &[u8],
968    consumed: &mut usize,
969) -> Result<(), ()> {
970    let uses_context = (iphc_header & iphc::DAC) != 0;
971    let dam_mode = iphc_header & iphc::DAM_MASK;
972    let ip_addr: &mut IPAddr = &mut ip6_header.dst_addr;
973    if uses_context {
974        match dam_mode {
975            iphc::DAM_INLINE => {
976                // DAC = 1, DAM = 00: 48 bits
977                // ffXX:XXLL:PPPP:PPPP:PPPP:PPPP:XXXX:XXXX
978                let prefix_bytes = ctx.prefix_len.div_ceil(8) as usize;
979                if prefix_bytes > 8 {
980                    // The maximum prefix length for this mode is 64 bits.
981                    // If the specified prefix exceeds this length, the
982                    // compression is invalid.
983                    return Err(());
984                }
985                ip_addr.0[0] = 0xff;
986                ip_addr.0[1] = buf[*consumed];
987                ip_addr.0[2] = buf[*consumed + 1];
988                ip_addr.0[3] = ctx.prefix_len;
989                ip_addr.0[4..4 + prefix_bytes].copy_from_slice(&ctx.prefix[0..prefix_bytes]);
990                ip_addr.0[12..16].copy_from_slice(&buf[*consumed + 2..*consumed + 6]);
991                *consumed += 6;
992            }
993            _ => {
994                // DAC = 1, DAM = 01, 10, 11: Reserved
995                return Err(());
996            }
997        }
998    } else {
999        match dam_mode {
1000            // DAC = 0, DAM = 00: Inline
1001            iphc::DAM_INLINE => {
1002                ip_addr.0.copy_from_slice(&buf[*consumed..*consumed + 16]);
1003                *consumed += 16;
1004            }
1005            // DAC = 0, DAM = 01: 48 bits
1006            // ffXX::00XX:XXXX:XXXX
1007            iphc::DAM_MODE1 => {
1008                ip_addr.0[0] = 0xff;
1009                ip_addr.0[1] = buf[*consumed];
1010                *consumed += 1;
1011                ip_addr.0[11..16].copy_from_slice(&buf[*consumed..*consumed + 5]);
1012                *consumed += 5;
1013            }
1014            // DAC = 0, DAM = 10: 32 bits
1015            // ffXX::00XX:XXXX
1016            iphc::DAM_MODE2 => {
1017                ip_addr.0[0] = 0xff;
1018                ip_addr.0[1] = buf[*consumed];
1019                *consumed += 1;
1020                ip_addr.0[13..16].copy_from_slice(&buf[*consumed..*consumed + 3]);
1021                *consumed += 3;
1022            }
1023            // DAC = 0, DAM = 11: 8 bits
1024            // ff02::00XX
1025            iphc::DAM_MODE3 => {
1026                ip_addr.0[0] = 0xff;
1027                ip_addr.0[1] = 0x02;
1028                ip_addr.0[15] = buf[*consumed];
1029                *consumed += 1;
1030            }
1031            _ => panic!("Unreachable case"),
1032        }
1033    }
1034    Ok(())
1035}
1036
1037fn decompress_iid_link_local(
1038    addr_mode: u8,
1039    ip_addr: &mut IPAddr,
1040    mac_addr: &MacAddress,
1041    buf: &[u8],
1042    consumed: &mut usize,
1043) -> Result<(), ()> {
1044    let mode = addr_mode & (iphc::SAM_MASK | iphc::DAM_MASK);
1045    match mode {
1046        // SAM, DAM = 00: Inline
1047        iphc::SAM_INLINE => {
1048            // SAM_INLINE is equivalent to DAM_INLINE
1049            ip_addr.0.copy_from_slice(&buf[*consumed..*consumed + 16]);
1050            *consumed += 16;
1051        }
1052        // SAM, DAM = 01: 64 bits
1053        // Link-local prefix (64 bits) + 64 bits carried inline
1054        iphc::SAM_MODE1 | iphc::DAM_MODE1 => {
1055            ip_addr.set_unicast_link_local();
1056            ip_addr.0[8..16].copy_from_slice(&buf[*consumed..*consumed + 8]);
1057            *consumed += 8;
1058        }
1059        // SAM, DAM = 11: 16 bits
1060        // Link-local prefix (112 bits) + 0000:00ff:fe00:XXXX
1061        iphc::SAM_MODE2 | iphc::DAM_MODE2 => {
1062            ip_addr.set_unicast_link_local();
1063            ip_addr.0[11..13].copy_from_slice(&iphc::MAC_BASE[3..5]);
1064            ip_addr.0[14..16].copy_from_slice(&buf[*consumed..*consumed + 2]);
1065            *consumed += 2;
1066        }
1067        // SAM, DAM = 11: 0 bits
1068        // Linx-local prefix (64 bits) + IID from outer header (64 bits)
1069        iphc::SAM_MODE3 | iphc::DAM_MODE3 => {
1070            ip_addr.set_unicast_link_local();
1071            ip_addr.0[8..16].copy_from_slice(&compute_iid(mac_addr));
1072        }
1073        _ => panic!("Unreachable case"),
1074    }
1075    Ok(())
1076}
1077
1078fn decompress_iid_context(
1079    addr_mode: u8,
1080    ip_addr: &mut IPAddr,
1081    mac_addr: &MacAddress,
1082    ctx: &Context,
1083    buf: &[u8],
1084    consumed: &mut usize,
1085) -> Result<(), ()> {
1086    let mode = addr_mode & (iphc::SAM_MASK | iphc::DAM_MASK);
1087    match mode {
1088        // DAM = 00: Reserved
1089        // SAM = 0 is handled separately outside this method
1090        iphc::DAM_INLINE => {
1091            return Err(());
1092        }
1093        // SAM, DAM = 01: 64 bits
1094        // Suffix is the 64 bits carried inline
1095        iphc::SAM_MODE1 | iphc::DAM_MODE1 => {
1096            ip_addr.0[8..16].copy_from_slice(&buf[*consumed..*consumed + 8]);
1097            *consumed += 8;
1098        }
1099        // SAM, DAM = 10: 16 bits
1100        // Suffix is 0000:00ff:fe00:XXXX
1101        iphc::SAM_MODE2 | iphc::DAM_MODE2 => {
1102            ip_addr.0[8..16].copy_from_slice(&iphc::MAC_BASE);
1103            ip_addr.0[14..16].copy_from_slice(&buf[*consumed..*consumed + 2]);
1104            *consumed += 2;
1105        }
1106        // SAM, DAM = 11: 0 bits
1107        // Suffix is the IID computed from the encapsulating header
1108        iphc::SAM_MODE3 | iphc::DAM_MODE3 => {
1109            let iid = compute_iid(mac_addr);
1110            ip_addr.0[8..16].copy_from_slice(&iid[0..8]);
1111        }
1112        _ => panic!("Unreachable case"),
1113    }
1114    // The bits covered by the provided context are always used, so we copy
1115    // the context bits into the address after the non-context bits are set.
1116    ip_addr.set_prefix(&ctx.prefix, ctx.prefix_len);
1117    Ok(())
1118}
1119
1120// Returns the UDP ports in host byte-order
1121fn decompress_udp_ports(udp_nhc: u8, buf: &[u8], consumed: &mut usize) -> (u16, u16) {
1122    let src_compressed = (udp_nhc & nhc::UDP_SRC_PORT_FLAG) != 0;
1123    let dst_compressed = (udp_nhc & nhc::UDP_DST_PORT_FLAG) != 0;
1124
1125    let src_port;
1126    let dst_port;
1127    if src_compressed && dst_compressed {
1128        // Both src and dst are compressed to 4 bits
1129        let src_short = ((buf[*consumed] >> 4) & 0xf) as u16;
1130        let dst_short = (buf[*consumed] & 0xf) as u16;
1131        src_port = nhc::UDP_4BIT_PORT | src_short;
1132        dst_port = nhc::UDP_4BIT_PORT | dst_short;
1133        *consumed += 1;
1134    } else if src_compressed {
1135        // Source port is compressed to 8 bits
1136        src_port = nhc::UDP_8BIT_PORT | (buf[*consumed] as u16);
1137        // Destination port is uncompressed
1138        dst_port = u16::from_be(network_slice_to_u16(&buf[*consumed + 1..*consumed + 3]));
1139        *consumed += 3;
1140    } else if dst_compressed {
1141        // Source port is uncompressed
1142        src_port = u16::from_be(network_slice_to_u16(&buf[*consumed..*consumed + 2]));
1143        // Destination port is compressed to 8 bits
1144        dst_port = nhc::UDP_8BIT_PORT | (buf[*consumed + 2] as u16);
1145        *consumed += 3;
1146    } else {
1147        // Both ports are uncompressed
1148        src_port = u16::from_be(network_slice_to_u16(&buf[*consumed..*consumed + 2]));
1149        dst_port = u16::from_be(network_slice_to_u16(&buf[*consumed + 2..*consumed + 4]));
1150        *consumed += 4;
1151    }
1152    (src_port, dst_port)
1153}
1154
1155// Returns the UDP checksum in host byte-order
1156fn decompress_udp_checksum(
1157    udp_nhc: u8,
1158    udp_header: &[u8],
1159    udp_length: u16,
1160    ip6_header: &IP6Header,
1161    buf: &[u8],
1162    consumed: &mut usize,
1163    is_fragment: bool,
1164) -> u16 {
1165    // TODO: In keeping with Postel's Law, we accept UDP packets that elide the
1166    // checksum (per RFC 6282). We are not sure if we should continue to support
1167    // this feature however.
1168    // Also, this implementation currently does not work for multi-frame packets,
1169    // as decompress is called on the first frame before the others arrive.
1170    if (udp_nhc & nhc::UDP_CHECKSUM_FLAG) != 0 && !is_fragment {
1171        let mut udp_header_copy: [u8; 8] = [0, 0, 0, 0, 0, 0, 0, 0];
1172        udp_header_copy.copy_from_slice(udp_header);
1173        match UDPHeader::decode(&udp_header_copy).done() {
1174            Some((_offset, hdr)) => u16::from_be(compute_udp_checksum(
1175                ip6_header,
1176                &hdr,
1177                udp_length,
1178                &buf[*consumed..],
1179            )),
1180            None => 0, //Will be dropped  by IP layer
1181        }
1182    } else {
1183        let checksum = u16::from_be(network_slice_to_u16(&buf[*consumed..*consumed + 2]));
1184        *consumed += 2;
1185        checksum
1186    }
1187}
capsules_extra/net/sixlowpan/sixlowpan_compression.rs

capsules_extra/net/sixlowpan/
sixlowpan_compression.rs
