smoltcp/iface/interface/
sixlowpan.rs

1use super::*;
2use crate::wire::Result;
3
4// Max len of non-fragmented packets after decompression (including ipv6 header and payload)
5// TODO: lower. Should be (6lowpan mtu) - (min 6lowpan header size) + (max ipv6 header size)
6pub(crate) const MAX_DECOMPRESSED_LEN: usize = 1500;
7
8impl Interface {
9    /// Process fragments that still need to be sent for 6LoWPAN packets.
10    #[cfg(feature = "proto-sixlowpan-fragmentation")]
11    pub(super) fn sixlowpan_egress(&mut self, device: &mut (impl Device + ?Sized)) {
12        // Reset the buffer when we transmitted everything.
13        if self.fragmenter.finished() {
14            self.fragmenter.reset();
15        }
16
17        if self.fragmenter.is_empty() {
18            return;
19        }
20
21        let pkt = &self.fragmenter;
22        if pkt.packet_len > pkt.sent_bytes {
23            if let Some(tx_token) = device.transmit(self.inner.now) {
24                self.inner
25                    .dispatch_ieee802154_frag(tx_token, &mut self.fragmenter);
26            }
27        }
28    }
29
30    /// Get the 6LoWPAN address contexts.
31    pub fn sixlowpan_address_context(&self) -> &[SixlowpanAddressContext] {
32        &self.inner.sixlowpan_address_context[..]
33    }
34
35    /// Get a mutable reference to the 6LoWPAN address contexts.
36    pub fn sixlowpan_address_context_mut(
37        &mut self,
38    ) -> &mut Vec<SixlowpanAddressContext, IFACE_MAX_SIXLOWPAN_ADDRESS_CONTEXT_COUNT> {
39        &mut self.inner.sixlowpan_address_context
40    }
41}
42
43impl InterfaceInner {
44    /// Get the next tag for a 6LoWPAN fragment.
45    #[cfg(feature = "proto-sixlowpan-fragmentation")]
46    fn get_sixlowpan_fragment_tag(&mut self) -> u16 {
47        let tag = self.tag;
48        self.tag = self.tag.wrapping_add(1);
49        tag
50    }
51
52    pub(super) fn process_sixlowpan<'output, 'payload: 'output>(
53        &mut self,
54        sockets: &mut SocketSet,
55        meta: PacketMeta,
56        ieee802154_repr: &Ieee802154Repr,
57        payload: &'payload [u8],
58        f: &'output mut FragmentsBuffer,
59    ) -> Option<Packet<'output>> {
60        let payload = match check!(SixlowpanPacket::dispatch(payload)) {
61            #[cfg(not(feature = "proto-sixlowpan-fragmentation"))]
62            SixlowpanPacket::FragmentHeader => {
63                net_debug!(
64                    "Fragmentation is not supported, \
65                    use the `proto-sixlowpan-fragmentation` feature to add support."
66                );
67                return None;
68            }
69            #[cfg(feature = "proto-sixlowpan-fragmentation")]
70            SixlowpanPacket::FragmentHeader => {
71                match self.process_sixlowpan_fragment(ieee802154_repr, payload, f) {
72                    Some(payload) => payload,
73                    None => return None,
74                }
75            }
76            SixlowpanPacket::IphcHeader => {
77                match Self::sixlowpan_to_ipv6(
78                    &self.sixlowpan_address_context,
79                    ieee802154_repr,
80                    payload,
81                    None,
82                    &mut f.decompress_buf,
83                ) {
84                    Ok(len) => &f.decompress_buf[..len],
85                    Err(e) => {
86                        net_debug!("sixlowpan decompress failed: {:?}", e);
87                        return None;
88                    }
89                }
90            }
91        };
92
93        self.process_ipv6(
94            sockets,
95            meta,
96            match ieee802154_repr.src_addr {
97                Some(s) => HardwareAddress::Ieee802154(s),
98                None => HardwareAddress::Ieee802154(Ieee802154Address::Absent),
99            },
100            &check!(Ipv6Packet::new_checked(payload)),
101        )
102    }
103
104    #[cfg(feature = "proto-sixlowpan-fragmentation")]
105    fn process_sixlowpan_fragment<'output, 'payload: 'output>(
106        &mut self,
107        ieee802154_repr: &Ieee802154Repr,
108        payload: &'payload [u8],
109        f: &'output mut FragmentsBuffer,
110    ) -> Option<&'output [u8]> {
111        use crate::iface::fragmentation::{AssemblerError, AssemblerFullError};
112
113        // We have a fragment header, which means we cannot process the 6LoWPAN packet,
114        // unless we have a complete one after processing this fragment.
115        let frag = check!(SixlowpanFragPacket::new_checked(payload));
116
117        // From RFC 4944 § 5.3: "The value of datagram_size SHALL be 40 octets more than the value
118        // of Payload Length in the IPv6 header of the packet."
119        // We should check that this is true, otherwise `buffer.split_at_mut(40)` will panic, since
120        // we assume that the decompressed packet is at least 40 bytes.
121        if frag.datagram_size() < 40 {
122            net_debug!("6LoWPAN: fragment size too small");
123            return None;
124        }
125
126        // The key specifies to which 6LoWPAN fragment it belongs too.
127        // It is based on the link layer addresses, the tag and the size.
128        let key = FragKey::Sixlowpan(frag.get_key(ieee802154_repr));
129
130        // The offset of this fragment in increments of 8 octets.
131        let offset = frag.datagram_offset() as usize * 8;
132
133        // We reserve a spot in the packet assembler set and add the required
134        // information to the packet assembler.
135        // This information is the total size of the packet when it is fully assmbled.
136        // We also pass the header size, since this is needed when other fragments
137        // (other than the first one) are added.
138        let frag_slot = match f.assembler.get(&key, self.now + f.reassembly_timeout) {
139            Ok(frag) => frag,
140            Err(AssemblerFullError) => {
141                net_debug!("No available packet assembler for fragmented packet");
142                return None;
143            }
144        };
145
146        if frag.is_first_fragment() {
147            // The first fragment contains the total size of the IPv6 packet.
148            // However, we received a packet that is compressed following the 6LoWPAN
149            // standard. This means we need to convert the IPv6 packet size to a 6LoWPAN
150            // packet size. The packet size can be different because of first the
151            // compression of the IP header and when UDP is used (because the UDP header
152            // can also be compressed). Other headers are not compressed by 6LoWPAN.
153
154            // First segment tells us the total size.
155            let total_size = frag.datagram_size() as usize;
156            if frag_slot.set_total_size(total_size).is_err() {
157                net_debug!("No available packet assembler for fragmented packet");
158                return None;
159            }
160
161            // Decompress headers+payload into the assembler.
162            if let Err(e) = frag_slot.add_with(0, |buffer| {
163                Self::sixlowpan_to_ipv6(
164                    &self.sixlowpan_address_context,
165                    ieee802154_repr,
166                    frag.payload(),
167                    Some(total_size),
168                    buffer,
169                )
170                .map_err(|_| AssemblerError)
171            }) {
172                net_debug!("fragmentation error: {:?}", e);
173                return None;
174            }
175        } else {
176            // Add the fragment to the packet assembler.
177            if let Err(e) = frag_slot.add(frag.payload(), offset) {
178                net_debug!("fragmentation error: {:?}", e);
179                return None;
180            }
181        }
182
183        match frag_slot.assemble() {
184            Some(payload) => {
185                net_trace!("6LoWPAN: fragmented packet now complete");
186                Some(payload)
187            }
188            None => None,
189        }
190    }
191
192    /// Decompress a 6LoWPAN packet into an IPv6 packet.
193    ///
194    /// The return value is the length of the decompressed packet, but not including the total
195    /// length of the payload of the UDP packet. This value is then used by the assembler to know
196    /// how far in the assembler buffer the packet is.
197    ///
198    /// **NOTE**: when decompressing a fragmented packet, the `total_len` parameter should be
199    /// passed. This is the total length of the IPv6 packet, including the IPv6 header. It is used
200    /// for calculating the length field in the UDP header.
201    fn sixlowpan_to_ipv6(
202        address_context: &[SixlowpanAddressContext],
203        ieee802154_repr: &Ieee802154Repr,
204        iphc_payload: &[u8],
205        total_len: Option<usize>,
206        buffer: &mut [u8],
207    ) -> Result<usize> {
208        let iphc = SixlowpanIphcPacket::new_checked(iphc_payload)?;
209        let iphc_repr = SixlowpanIphcRepr::parse(
210            &iphc,
211            ieee802154_repr.src_addr,
212            ieee802154_repr.dst_addr,
213            address_context,
214        )?;
215
216        // The first thing we have to decompress is the IPv6 header. However, at this point we
217        // don't know the total size of the packet, neither the next header, since that can be a
218        // compressed header. However, we know that the IPv6 header is 40 bytes, so we can reserve
219        // this space in the buffer such that we can decompress the IPv6 header into it at a later
220        // point.
221        let (ipv6_buffer, mut buffer) = buffer.split_at_mut(40);
222        let mut ipv6_header = Ipv6Packet::new_unchecked(ipv6_buffer);
223
224        // If the total length is given, we are dealing with a fragmented packet. The total
225        // length is then used to calculate the length field for the UDP header. If the total
226        // length is not given, we are not working with a fragmented packet, and we need to
227        // calculate the length of the payload ourselves.
228        let mut payload_len = 40;
229        let mut decompressed_len = 40;
230
231        let mut next_header = Some(iphc_repr.next_header);
232        let mut data = iphc.payload();
233
234        while let Some(nh) = next_header {
235            match nh {
236                SixlowpanNextHeader::Compressed => match SixlowpanNhcPacket::dispatch(data)? {
237                    SixlowpanNhcPacket::ExtHeader => {
238                        (buffer, data) = decompress_ext_hdr(
239                            data,
240                            &mut next_header,
241                            buffer,
242                            &mut payload_len,
243                            &mut decompressed_len,
244                        )?;
245                    }
246                    SixlowpanNhcPacket::UdpHeader => {
247                        decompress_udp(
248                            data,
249                            &iphc_repr,
250                            buffer,
251                            total_len,
252                            &mut payload_len,
253                            &mut decompressed_len,
254                        )?;
255
256                        break;
257                    }
258                },
259                SixlowpanNextHeader::Uncompressed(proto) => {
260                    // We have a 6LoWPAN uncompressed header.
261                    match proto {
262                        IpProtocol::Tcp | IpProtocol::Udp | IpProtocol::Icmpv6 => {
263                            // There can be no protocol after this one, so we can just copy the
264                            // rest of the data buffer. There is also no length field in the UDP
265                            // header that we need to correct as this header was not changed by the
266                            // 6LoWPAN compressor.
267                            if data.len() > buffer.len() {
268                                return Err(Error);
269                            }
270                            buffer[..data.len()].copy_from_slice(data);
271                            payload_len += data.len();
272                            decompressed_len += data.len();
273                            break;
274                        }
275                        proto => {
276                            net_debug!("Unsupported uncompressed next header: {:?}", proto);
277                            return Err(Error);
278                        }
279                    }
280                }
281            }
282        }
283
284        let ipv6_repr = Ipv6Repr {
285            src_addr: iphc_repr.src_addr,
286            dst_addr: iphc_repr.dst_addr,
287            next_header: decompress_next_header(iphc_repr.next_header, iphc.payload())?,
288            payload_len: total_len.unwrap_or(payload_len) - 40,
289            hop_limit: iphc_repr.hop_limit,
290        };
291        ipv6_repr.emit(&mut ipv6_header);
292
293        Ok(decompressed_len)
294    }
295
296    pub(super) fn dispatch_sixlowpan<Tx: TxToken>(
297        &mut self,
298        mut tx_token: Tx,
299        meta: PacketMeta,
300        packet: Packet,
301        ieee_repr: Ieee802154Repr,
302        frag: &mut Fragmenter,
303    ) {
304        let packet = match packet {
305            #[cfg(feature = "proto-ipv4")]
306            Packet::Ipv4(_) => unreachable!(),
307            Packet::Ipv6(packet) => packet,
308        };
309
310        // First we calculate the size we are going to need. If the size is bigger than the MTU,
311        // then we use fragmentation.
312        let (total_size, compressed_size, uncompressed_size) =
313            Self::compressed_packet_size(&packet, &ieee_repr);
314
315        let ieee_len = ieee_repr.buffer_len();
316
317        // TODO(thvdveld): use the MTU of the device.
318        if total_size + ieee_len > 125 {
319            #[cfg(feature = "proto-sixlowpan-fragmentation")]
320            {
321                // The packet does not fit in one Ieee802154 frame, so we need fragmentation.
322                // We do this by emitting everything in the `frag.buffer` from the interface.
323                // After emitting everything into that buffer, we send the first fragment heere.
324                // When `poll` is called again, we check if frag was fully sent, otherwise we
325                // call `dispatch_ieee802154_frag`, which will transmit the other fragments.
326
327                // `dispatch_ieee802154_frag` requires some information about the total packet size,
328                // the link local source and destination address...
329
330                let pkt = frag;
331                if pkt.buffer.len() < total_size {
332                    net_debug!(
333                        "dispatch_ieee802154: dropping, \
334                        fragmentation buffer is too small, at least {} needed",
335                        total_size
336                    );
337                    return;
338                }
339
340                let payload_length = packet.header.payload_len;
341
342                Self::ipv6_to_sixlowpan(
343                    &self.checksum_caps(),
344                    packet,
345                    &ieee_repr,
346                    &mut pkt.buffer[..],
347                );
348
349                pkt.sixlowpan.ll_dst_addr = ieee_repr.dst_addr.unwrap();
350                pkt.sixlowpan.ll_src_addr = ieee_repr.src_addr.unwrap();
351                pkt.packet_len = total_size;
352
353                // The datagram size that we need to set in the first fragment header is equal to the
354                // IPv6 payload length + 40.
355                pkt.sixlowpan.datagram_size = (payload_length + 40) as u16;
356
357                let tag = self.get_sixlowpan_fragment_tag();
358                // We save the tag for the other fragments that will be created when calling `poll`
359                // multiple times.
360                pkt.sixlowpan.datagram_tag = tag;
361
362                let frag1 = SixlowpanFragRepr::FirstFragment {
363                    size: pkt.sixlowpan.datagram_size,
364                    tag,
365                };
366                let fragn = SixlowpanFragRepr::Fragment {
367                    size: pkt.sixlowpan.datagram_size,
368                    tag,
369                    offset: 0,
370                };
371
372                // We calculate how much data we can send in the first fragment and the other
373                // fragments. The eventual IPv6 sizes of these fragments need to be a multiple of eight
374                // (except for the last fragment) since the offset field in the fragment is an offset
375                // in multiples of 8 octets. This is explained in [RFC 4944 § 5.3].
376                //
377                // [RFC 4944 § 5.3]: https://datatracker.ietf.org/doc/html/rfc4944#section-5.3
378
379                let header_diff = uncompressed_size - compressed_size;
380                let frag1_size =
381                    (125 - ieee_len - frag1.buffer_len() + header_diff) / 8 * 8 - header_diff;
382
383                pkt.sixlowpan.fragn_size = (125 - ieee_len - fragn.buffer_len()) / 8 * 8;
384                pkt.sent_bytes = frag1_size;
385                pkt.sixlowpan.datagram_offset = frag1_size + header_diff;
386
387                tx_token.set_meta(meta);
388                tx_token.consume(ieee_len + frag1.buffer_len() + frag1_size, |mut tx_buf| {
389                    // Add the IEEE header.
390                    let mut ieee_packet = Ieee802154Frame::new_unchecked(&mut tx_buf[..ieee_len]);
391                    ieee_repr.emit(&mut ieee_packet);
392                    tx_buf = &mut tx_buf[ieee_len..];
393
394                    // Add the first fragment header
395                    let mut frag1_packet = SixlowpanFragPacket::new_unchecked(&mut tx_buf);
396                    frag1.emit(&mut frag1_packet);
397                    tx_buf = &mut tx_buf[frag1.buffer_len()..];
398
399                    // Add the buffer part.
400                    tx_buf[..frag1_size].copy_from_slice(&pkt.buffer[..frag1_size]);
401                });
402            }
403
404            #[cfg(not(feature = "proto-sixlowpan-fragmentation"))]
405            {
406                net_debug!(
407                    "Enable the `proto-sixlowpan-fragmentation` feature for fragmentation support."
408                );
409                return;
410            }
411        } else {
412            tx_token.set_meta(meta);
413
414            // We don't need fragmentation, so we emit everything to the TX token.
415            tx_token.consume(total_size + ieee_len, |mut tx_buf| {
416                let mut ieee_packet = Ieee802154Frame::new_unchecked(&mut tx_buf[..ieee_len]);
417                ieee_repr.emit(&mut ieee_packet);
418                tx_buf = &mut tx_buf[ieee_len..];
419
420                Self::ipv6_to_sixlowpan(&self.checksum_caps(), packet, &ieee_repr, tx_buf);
421            });
422        }
423    }
424
425    fn ipv6_to_sixlowpan(
426        checksum_caps: &ChecksumCapabilities,
427        mut packet: PacketV6,
428        ieee_repr: &Ieee802154Repr,
429        mut buffer: &mut [u8],
430    ) {
431        let last_header = packet.payload.as_sixlowpan_next_header();
432        let next_header = last_header;
433
434        #[cfg(feature = "proto-ipv6-hbh")]
435        let next_header = if packet.hop_by_hop.is_some() {
436            SixlowpanNextHeader::Compressed
437        } else {
438            next_header
439        };
440
441        #[cfg(feature = "proto-ipv6-routing")]
442        let next_header = if packet.routing.is_some() {
443            SixlowpanNextHeader::Compressed
444        } else {
445            next_header
446        };
447
448        let iphc_repr = SixlowpanIphcRepr {
449            src_addr: packet.header.src_addr,
450            ll_src_addr: ieee_repr.src_addr,
451            dst_addr: packet.header.dst_addr,
452            ll_dst_addr: ieee_repr.dst_addr,
453            next_header,
454            hop_limit: packet.header.hop_limit,
455            ecn: None,
456            dscp: None,
457            flow_label: None,
458        };
459
460        iphc_repr.emit(&mut SixlowpanIphcPacket::new_unchecked(
461            &mut buffer[..iphc_repr.buffer_len()],
462        ));
463        buffer = &mut buffer[iphc_repr.buffer_len()..];
464
465        // Emit the Hop-by-Hop header
466        #[cfg(feature = "proto-ipv6-hbh")]
467        if let Some(hbh) = packet.hop_by_hop {
468            #[allow(unused)]
469            let next_header = last_header;
470
471            #[cfg(feature = "proto-ipv6-routing")]
472            let next_header = if packet.routing.is_some() {
473                SixlowpanNextHeader::Compressed
474            } else {
475                last_header
476            };
477
478            let ext_hdr = SixlowpanExtHeaderRepr {
479                ext_header_id: SixlowpanExtHeaderId::HopByHopHeader,
480                next_header,
481                length: hbh.options.iter().map(|o| o.buffer_len()).sum::<usize>() as u8,
482            };
483            ext_hdr.emit(&mut SixlowpanExtHeaderPacket::new_unchecked(
484                &mut buffer[..ext_hdr.buffer_len()],
485            ));
486            buffer = &mut buffer[ext_hdr.buffer_len()..];
487
488            for opt in &hbh.options {
489                opt.emit(&mut Ipv6Option::new_unchecked(
490                    &mut buffer[..opt.buffer_len()],
491                ));
492
493                buffer = &mut buffer[opt.buffer_len()..];
494            }
495        }
496
497        // Emit the Routing header
498        #[cfg(feature = "proto-ipv6-routing")]
499        if let Some(routing) = &packet.routing {
500            let ext_hdr = SixlowpanExtHeaderRepr {
501                ext_header_id: SixlowpanExtHeaderId::RoutingHeader,
502                next_header,
503                length: routing.buffer_len() as u8,
504            };
505            ext_hdr.emit(&mut SixlowpanExtHeaderPacket::new_unchecked(
506                &mut buffer[..ext_hdr.buffer_len()],
507            ));
508            buffer = &mut buffer[ext_hdr.buffer_len()..];
509
510            routing.emit(&mut Ipv6RoutingHeader::new_unchecked(
511                &mut buffer[..routing.buffer_len()],
512            ));
513            buffer = &mut buffer[routing.buffer_len()..];
514        }
515
516        match &mut packet.payload {
517            IpPayload::Icmpv6(icmp_repr) => {
518                icmp_repr.emit(
519                    &packet.header.src_addr,
520                    &packet.header.dst_addr,
521                    &mut Icmpv6Packet::new_unchecked(&mut buffer[..icmp_repr.buffer_len()]),
522                    checksum_caps,
523                );
524            }
525            #[cfg(any(feature = "socket-udp", feature = "socket-dns"))]
526            IpPayload::Udp(udp_repr, payload) => {
527                let udp_repr = SixlowpanUdpNhcRepr(*udp_repr);
528                udp_repr.emit(
529                    &mut SixlowpanUdpNhcPacket::new_unchecked(
530                        &mut buffer[..udp_repr.header_len() + payload.len()],
531                    ),
532                    &iphc_repr.src_addr,
533                    &iphc_repr.dst_addr,
534                    payload.len(),
535                    |buf| buf.copy_from_slice(payload),
536                    checksum_caps,
537                );
538            }
539            #[cfg(feature = "socket-tcp")]
540            IpPayload::Tcp(tcp_repr) => {
541                tcp_repr.emit(
542                    &mut TcpPacket::new_unchecked(&mut buffer[..tcp_repr.buffer_len()]),
543                    &packet.header.src_addr.into(),
544                    &packet.header.dst_addr.into(),
545                    checksum_caps,
546                );
547            }
548            #[cfg(feature = "socket-raw")]
549            IpPayload::Raw(_raw) => todo!(),
550
551            #[allow(unreachable_patterns)]
552            _ => unreachable!(),
553        }
554    }
555
556    /// Calculates three sizes:
557    ///  - total size: the size of a compressed IPv6 packet
558    ///  - compressed header size: the size of the compressed headers
559    ///  - uncompressed header size: the size of the headers that are not compressed
560    ///
561    /// They are returned as a tuple in the same order.
562    fn compressed_packet_size(
563        packet: &PacketV6,
564        ieee_repr: &Ieee802154Repr,
565    ) -> (usize, usize, usize) {
566        let last_header = packet.payload.as_sixlowpan_next_header();
567        let next_header = last_header;
568
569        #[cfg(feature = "proto-ipv6-hbh")]
570        let next_header = if packet.hop_by_hop.is_some() {
571            SixlowpanNextHeader::Compressed
572        } else {
573            next_header
574        };
575
576        #[cfg(feature = "proto-ipv6-routing")]
577        let next_header = if packet.routing.is_some() {
578            SixlowpanNextHeader::Compressed
579        } else {
580            next_header
581        };
582
583        let iphc = SixlowpanIphcRepr {
584            src_addr: packet.header.src_addr,
585            ll_src_addr: ieee_repr.src_addr,
586            dst_addr: packet.header.dst_addr,
587            ll_dst_addr: ieee_repr.dst_addr,
588            next_header,
589            hop_limit: packet.header.hop_limit,
590            ecn: None,
591            dscp: None,
592            flow_label: None,
593        };
594
595        let mut total_size = iphc.buffer_len();
596        let mut compressed_hdr_size = iphc.buffer_len();
597        let mut uncompressed_hdr_size = packet.header.buffer_len();
598
599        // Add the hop-by-hop to the sizes.
600        #[cfg(feature = "proto-ipv6-hbh")]
601        if let Some(hbh) = &packet.hop_by_hop {
602            #[allow(unused)]
603            let next_header = last_header;
604
605            #[cfg(feature = "proto-ipv6-routing")]
606            let next_header = if packet.routing.is_some() {
607                SixlowpanNextHeader::Compressed
608            } else {
609                last_header
610            };
611
612            let options_size = hbh.options.iter().map(|o| o.buffer_len()).sum::<usize>();
613
614            let ext_hdr = SixlowpanExtHeaderRepr {
615                ext_header_id: SixlowpanExtHeaderId::HopByHopHeader,
616                next_header,
617                length: hbh.buffer_len() as u8 + options_size as u8,
618            };
619
620            total_size += ext_hdr.buffer_len() + options_size;
621            compressed_hdr_size += ext_hdr.buffer_len() + options_size;
622            uncompressed_hdr_size += hbh.buffer_len() + options_size;
623        }
624
625        // Add the routing header to the sizes.
626        #[cfg(feature = "proto-ipv6-routing")]
627        if let Some(routing) = &packet.routing {
628            let ext_hdr = SixlowpanExtHeaderRepr {
629                ext_header_id: SixlowpanExtHeaderId::RoutingHeader,
630                next_header,
631                length: routing.buffer_len() as u8,
632            };
633            total_size += ext_hdr.buffer_len() + routing.buffer_len();
634            compressed_hdr_size += ext_hdr.buffer_len() + routing.buffer_len();
635            uncompressed_hdr_size += routing.buffer_len();
636        }
637
638        match packet.payload {
639            #[cfg(any(feature = "socket-udp", feature = "socket-dns"))]
640            IpPayload::Udp(udp_hdr, payload) => {
641                uncompressed_hdr_size += udp_hdr.header_len();
642
643                let udp_hdr = SixlowpanUdpNhcRepr(udp_hdr);
644                compressed_hdr_size += udp_hdr.header_len();
645
646                total_size += udp_hdr.header_len() + payload.len();
647            }
648            _ => {
649                total_size += packet.header.payload_len;
650            }
651        }
652
653        (total_size, compressed_hdr_size, uncompressed_hdr_size)
654    }
655
656    #[cfg(feature = "proto-sixlowpan-fragmentation")]
657    pub(super) fn dispatch_sixlowpan_frag<Tx: TxToken>(
658        &mut self,
659        tx_token: Tx,
660        ieee_repr: Ieee802154Repr,
661        frag: &mut Fragmenter,
662    ) {
663        // Create the FRAG_N header.
664        let fragn = SixlowpanFragRepr::Fragment {
665            size: frag.sixlowpan.datagram_size,
666            tag: frag.sixlowpan.datagram_tag,
667            offset: (frag.sixlowpan.datagram_offset / 8) as u8,
668        };
669
670        let ieee_len = ieee_repr.buffer_len();
671        let frag_size = (frag.packet_len - frag.sent_bytes).min(frag.sixlowpan.fragn_size);
672
673        tx_token.consume(
674            ieee_repr.buffer_len() + fragn.buffer_len() + frag_size,
675            |mut tx_buf| {
676                let mut ieee_packet = Ieee802154Frame::new_unchecked(&mut tx_buf[..ieee_len]);
677                ieee_repr.emit(&mut ieee_packet);
678                tx_buf = &mut tx_buf[ieee_len..];
679
680                let mut frag_packet =
681                    SixlowpanFragPacket::new_unchecked(&mut tx_buf[..fragn.buffer_len()]);
682                fragn.emit(&mut frag_packet);
683                tx_buf = &mut tx_buf[fragn.buffer_len()..];
684
685                // Add the buffer part
686                tx_buf[..frag_size].copy_from_slice(&frag.buffer[frag.sent_bytes..][..frag_size]);
687
688                frag.sent_bytes += frag_size;
689                frag.sixlowpan.datagram_offset += frag_size;
690            },
691        );
692    }
693}
694
695/// Convert a 6LoWPAN next header to an IPv6 next header.
696#[inline]
697fn decompress_next_header(next_header: SixlowpanNextHeader, payload: &[u8]) -> Result<IpProtocol> {
698    match next_header {
699        SixlowpanNextHeader::Compressed => match SixlowpanNhcPacket::dispatch(payload)? {
700            SixlowpanNhcPacket::ExtHeader => {
701                let ext_hdr = SixlowpanExtHeaderPacket::new_checked(payload)?;
702                Ok(ext_hdr.extension_header_id().into())
703            }
704            SixlowpanNhcPacket::UdpHeader => Ok(IpProtocol::Udp),
705        },
706        SixlowpanNextHeader::Uncompressed(proto) => Ok(proto),
707    }
708}
709
710// NOTE: we always inline this function into the sixlowpan_to_ipv6 function, since it is only used there.
711#[inline(always)]
712fn decompress_ext_hdr<'d>(
713    mut data: &'d [u8],
714    next_header: &mut Option<SixlowpanNextHeader>,
715    mut buffer: &'d mut [u8],
716    payload_len: &mut usize,
717    decompressed_len: &mut usize,
718) -> Result<(&'d mut [u8], &'d [u8])> {
719    let ext_hdr = SixlowpanExtHeaderPacket::new_checked(data)?;
720    let ext_repr = SixlowpanExtHeaderRepr::parse(&ext_hdr)?;
721    let nh = decompress_next_header(
722        ext_repr.next_header,
723        &data[ext_repr.length as usize + ext_repr.buffer_len()..],
724    )?;
725    *next_header = Some(ext_repr.next_header);
726    let ipv6_ext_hdr = Ipv6ExtHeaderRepr {
727        next_header: nh,
728        length: ext_repr.length / 8,
729        data: ext_hdr.payload(),
730    };
731    if ipv6_ext_hdr.header_len() + ipv6_ext_hdr.data.len() > buffer.len() {
732        return Err(Error);
733    }
734    ipv6_ext_hdr.emit(&mut Ipv6ExtHeader::new_unchecked(
735        &mut buffer[..ipv6_ext_hdr.header_len()],
736    ));
737    buffer[ipv6_ext_hdr.header_len()..][..ipv6_ext_hdr.data.len()]
738        .copy_from_slice(ipv6_ext_hdr.data);
739    buffer = &mut buffer[ipv6_ext_hdr.header_len() + ipv6_ext_hdr.data.len()..];
740    *payload_len += ipv6_ext_hdr.header_len() + ipv6_ext_hdr.data.len();
741    *decompressed_len += ipv6_ext_hdr.header_len() + ipv6_ext_hdr.data.len();
742    data = &data[ext_repr.buffer_len() + ext_repr.length as usize..];
743    Ok((buffer, data))
744}
745
746// NOTE: we always inline this function into the sixlowpan_to_ipv6 function, since it is only used there.
747#[inline(always)]
748fn decompress_udp(
749    data: &[u8],
750    iphc_repr: &SixlowpanIphcRepr,
751    buffer: &mut [u8],
752    total_len: Option<usize>,
753    payload_len: &mut usize,
754    decompressed_len: &mut usize,
755) -> Result<()> {
756    let udp_packet = SixlowpanUdpNhcPacket::new_checked(data)?;
757    let payload = udp_packet.payload();
758    let udp_repr = SixlowpanUdpNhcRepr::parse(
759        &udp_packet,
760        &iphc_repr.src_addr,
761        &iphc_repr.dst_addr,
762        &ChecksumCapabilities::ignored(),
763    )?;
764    if udp_repr.header_len() + payload.len() > buffer.len() {
765        return Err(Error);
766    }
767    let udp_payload_len = if let Some(total_len) = total_len {
768        total_len - *payload_len - 8
769    } else {
770        payload.len()
771    };
772    *payload_len += udp_payload_len + 8;
773    *decompressed_len += udp_repr.0.header_len() + payload.len();
774    let mut udp = UdpPacket::new_unchecked(&mut buffer[..payload.len() + 8]);
775    udp_repr.0.emit_header(&mut udp, udp_payload_len);
776    buffer[8..][..payload.len()].copy_from_slice(payload);
777    Ok(())
778}
779
780#[cfg(test)]
781#[cfg(all(feature = "proto-rpl", feature = "proto-ipv6-hbh"))]
782mod tests {
783    use super::*;
784
785    static SIXLOWPAN_COMPRESSED_RPL_DAO: [u8; 99] = [
786        0x61, 0xdc, 0x45, 0xcd, 0xab, 0x01, 0x00, 0x01, 0x00, 0x01, 0x00, 0x01, 0x00, 0x03, 0x00,
787        0x03, 0x00, 0x03, 0x00, 0x03, 0x00, 0x7e, 0xf7, 0x00, 0xe0, 0x3a, 0x06, 0x63, 0x04, 0x00,
788        0x1e, 0x08, 0x00, 0x9b, 0x02, 0x3e, 0x63, 0x1e, 0x40, 0x00, 0xf1, 0xfd, 0x00, 0x00, 0x00,
789        0x00, 0x00, 0x00, 0x00, 0x02, 0x01, 0x00, 0x01, 0x00, 0x01, 0x00, 0x01, 0x05, 0x12, 0x00,
790        0x80, 0xfd, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0x00, 0x03, 0x00, 0x03,
791        0x00, 0x03, 0x06, 0x14, 0x00, 0x00, 0x00, 0x1e, 0xfd, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
792        0x00, 0x02, 0x01, 0x00, 0x01, 0x00, 0x01, 0x00, 0x01,
793    ];
794
795    static SIXLOWPAN_UNCOMPRESSED_RPL_DAO: [u8; 114] = [
796        0x60, 0x00, 0x00, 0x00, 0x00, 0x4a, 0x00, 0x40, 0xfd, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
797        0x00, 0x02, 0x03, 0x00, 0x03, 0x00, 0x03, 0x00, 0x03, 0xfd, 0x00, 0x00, 0x00, 0x00, 0x00,
798        0x00, 0x00, 0x02, 0x01, 0x00, 0x01, 0x00, 0x01, 0x00, 0x01, 0x3a, 0x00, 0x63, 0x04, 0x00,
799        0x1e, 0x08, 0x00, 0x9b, 0x02, 0x3e, 0x63, 0x1e, 0x40, 0x00, 0xf1, 0xfd, 0x00, 0x00, 0x00,
800        0x00, 0x00, 0x00, 0x00, 0x02, 0x01, 0x00, 0x01, 0x00, 0x01, 0x00, 0x01, 0x05, 0x12, 0x00,
801        0x80, 0xfd, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0x00, 0x03, 0x00, 0x03,
802        0x00, 0x03, 0x06, 0x14, 0x00, 0x00, 0x00, 0x1e, 0xfd, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
803        0x00, 0x02, 0x01, 0x00, 0x01, 0x00, 0x01, 0x00, 0x01,
804    ];
805
806    #[test]
807    fn test_sixlowpan_decompress_hop_by_hop_with_icmpv6() {
808        let address_context = [SixlowpanAddressContext([
809            0xfd, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
810        ])];
811
812        let ieee_frame = Ieee802154Frame::new_checked(&SIXLOWPAN_COMPRESSED_RPL_DAO).unwrap();
813        let ieee_repr = Ieee802154Repr::parse(&ieee_frame).unwrap();
814
815        let mut buffer = [0u8; 256];
816        let len = InterfaceInner::sixlowpan_to_ipv6(
817            &address_context,
818            &ieee_repr,
819            ieee_frame.payload().unwrap(),
820            None,
821            &mut buffer[..],
822        )
823        .unwrap();
824
825        assert_eq!(&buffer[..len], &SIXLOWPAN_UNCOMPRESSED_RPL_DAO);
826    }
827
828    #[test]
829    fn test_sixlowpan_compress_hop_by_hop_with_icmpv6() {
830        let ieee_repr = Ieee802154Repr {
831            frame_type: Ieee802154FrameType::Data,
832            security_enabled: false,
833            frame_pending: false,
834            ack_request: true,
835            sequence_number: Some(69),
836            pan_id_compression: true,
837            frame_version: Ieee802154FrameVersion::Ieee802154_2006,
838            dst_pan_id: Some(Ieee802154Pan(43981)),
839            dst_addr: Some(Ieee802154Address::Extended([0, 1, 0, 1, 0, 1, 0, 1])),
840            src_pan_id: None,
841            src_addr: Some(Ieee802154Address::Extended([0, 3, 0, 3, 0, 3, 0, 3])),
842        };
843
844        let mut ip_packet = PacketV6 {
845            header: Ipv6Repr {
846                src_addr: Ipv6Address::from_bytes(&[
847                    253, 0, 0, 0, 0, 0, 0, 0, 2, 3, 0, 3, 0, 3, 0, 3,
848                ]),
849                dst_addr: Ipv6Address::from_bytes(&[
850                    253, 0, 0, 0, 0, 0, 0, 0, 2, 1, 0, 1, 0, 1, 0, 1,
851                ]),
852                next_header: IpProtocol::Icmpv6,
853                payload_len: 66,
854                hop_limit: 64,
855            },
856            #[cfg(feature = "proto-ipv6-hbh")]
857            hop_by_hop: None,
858            #[cfg(feature = "proto-ipv6-fragmentation")]
859            fragment: None,
860            #[cfg(feature = "proto-ipv6-routing")]
861            routing: None,
862            payload: IpPayload::Icmpv6(Icmpv6Repr::Rpl(RplRepr::DestinationAdvertisementObject {
863                rpl_instance_id: RplInstanceId::Global(30),
864                expect_ack: false,
865                sequence: 241,
866                dodag_id: Some(Ipv6Address::from_bytes(&[
867                    253, 0, 0, 0, 0, 0, 0, 0, 2, 1, 0, 1, 0, 1, 0, 1,
868                ])),
869                options: &[],
870            })),
871        };
872
873        let (total_size, _, _) = InterfaceInner::compressed_packet_size(&mut ip_packet, &ieee_repr);
874        let mut buffer = vec![0u8; total_size];
875
876        InterfaceInner::ipv6_to_sixlowpan(
877            &ChecksumCapabilities::default(),
878            ip_packet,
879            &ieee_repr,
880            &mut buffer[..total_size],
881        );
882
883        let result = [
884            0x7e, 0x0, 0xfd, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x2, 0x3, 0x0, 0x3, 0x0, 0x3, 0x0,
885            0x3, 0xfd, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x2, 0x1, 0x0, 0x1, 0x0, 0x1, 0x0, 0x1,
886            0xe0, 0x3a, 0x6, 0x63, 0x4, 0x0, 0x1e, 0x3, 0x0, 0x9b, 0x2, 0x3e, 0x63, 0x1e, 0x40,
887            0x0, 0xf1, 0xfd, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x2, 0x1, 0x0, 0x1, 0x0, 0x1, 0x0,
888            0x1, 0x5, 0x12, 0x0, 0x80, 0xfd, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x2, 0x3, 0x0, 0x3,
889            0x0, 0x3, 0x0, 0x3, 0x6, 0x14, 0x0, 0x0, 0x0, 0x1e, 0xfd, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
890            0x0, 0x2, 0x1, 0x0, 0x1, 0x0, 0x1, 0x0, 0x1,
891        ];
892
893        assert_eq!(&result, &result);
894    }
895
896    #[test]
897    fn test_sixlowpan_compress_hop_by_hop_with_udp() {
898        let ieee_repr = Ieee802154Repr {
899            frame_type: Ieee802154FrameType::Data,
900            security_enabled: false,
901            frame_pending: false,
902            ack_request: true,
903            sequence_number: Some(69),
904            pan_id_compression: true,
905            frame_version: Ieee802154FrameVersion::Ieee802154_2006,
906            dst_pan_id: Some(Ieee802154Pan(43981)),
907            dst_addr: Some(Ieee802154Address::Extended([0, 1, 0, 1, 0, 1, 0, 1])),
908            src_pan_id: None,
909            src_addr: Some(Ieee802154Address::Extended([0, 3, 0, 3, 0, 3, 0, 3])),
910        };
911
912        let addr = Ipv6Address::from_bytes(&[253, 0, 0, 0, 0, 0, 0, 0, 2, 3, 0, 3, 0, 3, 0, 3]);
913        let parent_address =
914            Ipv6Address::from_bytes(&[253, 0, 0, 0, 0, 0, 0, 0, 2, 1, 0, 1, 0, 1, 0, 1]);
915
916        let mut hbh_options = heapless::Vec::new();
917        hbh_options
918            .push(Ipv6OptionRepr::Rpl(RplHopByHopRepr {
919                down: false,
920                rank_error: false,
921                forwarding_error: false,
922                instance_id: RplInstanceId::from(0x1e),
923                sender_rank: 0x300,
924            }))
925            .unwrap();
926
927        let mut ip_packet = PacketV6 {
928            header: Ipv6Repr {
929                src_addr: addr,
930                dst_addr: parent_address,
931                next_header: IpProtocol::Icmpv6,
932                payload_len: 66,
933                hop_limit: 64,
934            },
935            #[cfg(feature = "proto-ipv6-hbh")]
936            hop_by_hop: Some(Ipv6HopByHopRepr {
937                options: hbh_options,
938            }),
939            #[cfg(feature = "proto-ipv6-fragmentation")]
940            fragment: None,
941            #[cfg(feature = "proto-ipv6-routing")]
942            routing: None,
943            payload: IpPayload::Icmpv6(Icmpv6Repr::Rpl(RplRepr::DestinationAdvertisementObject {
944                rpl_instance_id: RplInstanceId::Global(30),
945                expect_ack: false,
946                sequence: 241,
947                dodag_id: Some(Ipv6Address::from_bytes(&[
948                    253, 0, 0, 0, 0, 0, 0, 0, 2, 1, 0, 1, 0, 1, 0, 1,
949                ])),
950                options: &[
951                    5, 18, 0, 128, 253, 0, 0, 0, 0, 0, 0, 0, 2, 3, 0, 3, 0, 3, 0, 3, 6, 20, 0, 0,
952                    0, 30, 253, 0, 0, 0, 0, 0, 0, 0, 2, 1, 0, 1, 0, 1, 0, 1,
953                ],
954            })),
955        };
956
957        let (total_size, _, _) = InterfaceInner::compressed_packet_size(&mut ip_packet, &ieee_repr);
958        let mut buffer = vec![0u8; total_size];
959
960        InterfaceInner::ipv6_to_sixlowpan(
961            &ChecksumCapabilities::default(),
962            ip_packet,
963            &ieee_repr,
964            &mut buffer[..total_size],
965        );
966
967        let result = [
968            0x7e, 0x0, 0xfd, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x2, 0x3, 0x0, 0x3, 0x0, 0x3, 0x0,
969            0x3, 0xfd, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x2, 0x1, 0x0, 0x1, 0x0, 0x1, 0x0, 0x1,
970            0xe0, 0x3a, 0x6, 0x63, 0x4, 0x0, 0x1e, 0x3, 0x0, 0x9b, 0x2, 0x3e, 0x63, 0x1e, 0x40,
971            0x0, 0xf1, 0xfd, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x2, 0x1, 0x0, 0x1, 0x0, 0x1, 0x0,
972            0x1, 0x5, 0x12, 0x0, 0x80, 0xfd, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x2, 0x3, 0x0, 0x3,
973            0x0, 0x3, 0x0, 0x3, 0x6, 0x14, 0x0, 0x0, 0x0, 0x1e, 0xfd, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
974            0x0, 0x2, 0x1, 0x0, 0x1, 0x0, 0x1, 0x0, 0x1,
975        ];
976
977        assert_eq!(&buffer[..total_size], &result);
978    }
979}
smoltcp/iface/interface/sixlowpan.rs

smoltcp/iface/interface/
sixlowpan.rs
