1 //! Implementation of [RFC 6282] which specifies a compression format for IPv6 datagrams over 2 //! IEEE802.154-based networks. 3 //! 4 //! [RFC 6282]: https://datatracker.ietf.org/doc/html/rfc6282 5 6 use super::{Error, Result}; 7 use crate::wire::ieee802154::Address as LlAddress; 8 use crate::wire::ipv6; 9 use crate::wire::IpProtocol; 10 11 const ADDRESS_CONTEXT_LENGTH: usize = 8; 12 13 #[derive(Debug, PartialEq, Eq, Clone, Copy)] 14 #[cfg_attr(feature = "defmt", derive(defmt::Format))] 15 pub struct AddressContext(pub [u8; ADDRESS_CONTEXT_LENGTH]); 16 17 /// The representation of an unresolved address. 6LoWPAN compression of IPv6 addresses can be with 18 /// and without context information. The decompression with context information is not yet 19 /// implemented. 20 #[derive(Debug, PartialEq, Eq, Clone, Copy)] 21 #[cfg_attr(feature = "defmt", derive(defmt::Format))] 22 pub enum UnresolvedAddress<'a> { 23 WithoutContext(AddressMode<'a>), 24 WithContext((usize, AddressMode<'a>)), 25 Reserved, 26 } 27 28 #[derive(Debug, PartialEq, Eq, Clone, Copy)] 29 #[cfg_attr(feature = "defmt", derive(defmt::Format))] 30 pub enum AddressMode<'a> { 31 /// The full address is carried in-line. 32 FullInline(&'a [u8]), 33 /// The first 64-bits of the address are elided. The value of those bits 34 /// is the link-local prefix padded with zeros. The remaining 64 bits are 35 /// carried in-line. 36 InLine64bits(&'a [u8]), 37 /// The first 112 bits of the address are elided. The value of the first 38 /// 64 bits is the link-local prefix padded with zeros. The following 64 bits 39 /// are 0000:00ff:fe00:XXXX, where XXXX are the 16 bits carried in-line. 40 InLine16bits(&'a [u8]), 41 /// The address is fully elided. The first 64 bits of the address are 42 /// the link-local prefix padded with zeros. The remaining 64 bits are 43 /// computed from the encapsulating header (e.g., 802.15.4 or IPv6 source address) 44 /// as specified in Section 3.2.2. 45 FullyElided, 46 /// The address takes the form ffXX::00XX:XXXX:XXXX 47 Multicast48bits(&'a [u8]), 48 /// The address takes the form ffXX::00XX:XXXX. 49 Multicast32bits(&'a [u8]), 50 /// The address takes the form ff02::00XX. 51 Multicast8bits(&'a [u8]), 52 /// The unspecified address. 53 Unspecified, 54 NotSupported, 55 } 56 57 const LINK_LOCAL_PREFIX: [u8; 2] = [0xfe, 0x80]; 58 const EUI64_MIDDLE_VALUE: [u8; 2] = [0xff, 0xfe]; 59 60 impl<'a> UnresolvedAddress<'a> { resolve( self, ll_address: Option<LlAddress>, addr_context: &[AddressContext], ) -> Result<ipv6::Address>61 pub fn resolve( 62 self, 63 ll_address: Option<LlAddress>, 64 addr_context: &[AddressContext], 65 ) -> Result<ipv6::Address> { 66 let mut bytes = [0; 16]; 67 68 let copy_context = |index: usize, bytes: &mut [u8]| -> Result<()> { 69 if index >= addr_context.len() { 70 return Err(Error); 71 } 72 73 let context = addr_context[index]; 74 bytes[..ADDRESS_CONTEXT_LENGTH].copy_from_slice(&context.0); 75 76 Ok(()) 77 }; 78 79 match self { 80 UnresolvedAddress::WithoutContext(mode) => match mode { 81 AddressMode::FullInline(addr) => Ok(ipv6::Address::from_bytes(addr)), 82 AddressMode::InLine64bits(inline) => { 83 bytes[0..2].copy_from_slice(&LINK_LOCAL_PREFIX[..]); 84 bytes[8..].copy_from_slice(inline); 85 Ok(ipv6::Address::from_bytes(&bytes[..])) 86 } 87 AddressMode::InLine16bits(inline) => { 88 bytes[0..2].copy_from_slice(&LINK_LOCAL_PREFIX[..]); 89 bytes[11..13].copy_from_slice(&EUI64_MIDDLE_VALUE[..]); 90 bytes[14..].copy_from_slice(inline); 91 Ok(ipv6::Address::from_bytes(&bytes[..])) 92 } 93 AddressMode::FullyElided => { 94 bytes[0..2].copy_from_slice(&LINK_LOCAL_PREFIX[..]); 95 match ll_address { 96 Some(LlAddress::Short(ll)) => { 97 bytes[11..13].copy_from_slice(&EUI64_MIDDLE_VALUE[..]); 98 bytes[14..].copy_from_slice(&ll); 99 } 100 Some(addr @ LlAddress::Extended(_)) => match addr.as_eui_64() { 101 Some(addr) => bytes[8..].copy_from_slice(&addr), 102 None => return Err(Error), 103 }, 104 Some(LlAddress::Absent) => return Err(Error), 105 None => return Err(Error), 106 } 107 Ok(ipv6::Address::from_bytes(&bytes[..])) 108 } 109 AddressMode::Multicast48bits(inline) => { 110 bytes[0] = 0xff; 111 bytes[1] = inline[0]; 112 bytes[11..].copy_from_slice(&inline[1..][..5]); 113 Ok(ipv6::Address::from_bytes(&bytes[..])) 114 } 115 AddressMode::Multicast32bits(inline) => { 116 bytes[0] = 0xff; 117 bytes[1] = inline[0]; 118 bytes[13..].copy_from_slice(&inline[1..][..3]); 119 Ok(ipv6::Address::from_bytes(&bytes[..])) 120 } 121 AddressMode::Multicast8bits(inline) => { 122 bytes[0] = 0xff; 123 bytes[1] = 0x02; 124 bytes[15] = inline[0]; 125 Ok(ipv6::Address::from_bytes(&bytes[..])) 126 } 127 _ => Err(Error), 128 }, 129 UnresolvedAddress::WithContext(mode) => match mode { 130 (_, AddressMode::Unspecified) => Ok(ipv6::Address::UNSPECIFIED), 131 (index, AddressMode::InLine64bits(inline)) => { 132 copy_context(index, &mut bytes[..])?; 133 bytes[16 - inline.len()..].copy_from_slice(inline); 134 Ok(ipv6::Address::from_bytes(&bytes[..])) 135 } 136 (index, AddressMode::InLine16bits(inline)) => { 137 copy_context(index, &mut bytes[..])?; 138 bytes[16 - inline.len()..].copy_from_slice(inline); 139 Ok(ipv6::Address::from_bytes(&bytes[..])) 140 } 141 (index, AddressMode::FullyElided) => { 142 match ll_address { 143 Some(LlAddress::Short(ll)) => { 144 bytes[11..13].copy_from_slice(&EUI64_MIDDLE_VALUE[..]); 145 bytes[14..].copy_from_slice(&ll); 146 } 147 Some(addr @ LlAddress::Extended(_)) => match addr.as_eui_64() { 148 Some(addr) => bytes[8..].copy_from_slice(&addr), 149 None => return Err(Error), 150 }, 151 Some(LlAddress::Absent) => return Err(Error), 152 None => return Err(Error), 153 } 154 155 copy_context(index, &mut bytes[..])?; 156 157 Ok(ipv6::Address::from_bytes(&bytes[..])) 158 } 159 _ => Err(Error), 160 }, 161 UnresolvedAddress::Reserved => Err(Error), 162 } 163 } 164 } 165 166 #[derive(Debug, Clone, Copy, PartialEq, Eq)] 167 #[cfg_attr(feature = "defmt", derive(defmt::Format))] 168 pub enum SixlowpanPacket { 169 FragmentHeader, 170 IphcHeader, 171 } 172 173 const DISPATCH_FIRST_FRAGMENT_HEADER: u8 = 0b11000; 174 const DISPATCH_FRAGMENT_HEADER: u8 = 0b11100; 175 const DISPATCH_IPHC_HEADER: u8 = 0b011; 176 const DISPATCH_UDP_HEADER: u8 = 0b11110; 177 const DISPATCH_EXT_HEADER: u8 = 0b1110; 178 179 impl SixlowpanPacket { 180 /// Returns the type of the 6LoWPAN header. 181 /// This can either be a fragment header or an IPHC header. 182 /// 183 /// # Errors 184 /// Returns `[Error::Unrecognized]` when neither the Fragment Header dispatch or the IPHC 185 /// dispatch is recognized. dispatch(buffer: impl AsRef<[u8]>) -> Result<Self>186 pub fn dispatch(buffer: impl AsRef<[u8]>) -> Result<Self> { 187 let raw = buffer.as_ref(); 188 189 if raw.is_empty() { 190 return Err(Error); 191 } 192 193 if raw[0] >> 3 == DISPATCH_FIRST_FRAGMENT_HEADER || raw[0] >> 3 == DISPATCH_FRAGMENT_HEADER 194 { 195 Ok(Self::FragmentHeader) 196 } else if raw[0] >> 5 == DISPATCH_IPHC_HEADER { 197 Ok(Self::IphcHeader) 198 } else { 199 Err(Error) 200 } 201 } 202 } 203 204 pub mod frag { 205 //! Implementation of the fragment headers from [RFC 4944 § 5.3]. 206 //! 207 //! [RFC 4944 § 5.3]: https://datatracker.ietf.org/doc/html/rfc4944#section-5.3 208 209 use super::{DISPATCH_FIRST_FRAGMENT_HEADER, DISPATCH_FRAGMENT_HEADER}; 210 use crate::wire::{Error, Result}; 211 use crate::wire::{Ieee802154Address, Ieee802154Repr}; 212 use byteorder::{ByteOrder, NetworkEndian}; 213 214 /// Key used for identifying all the link fragments that belong to the same packet. 215 #[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord)] 216 #[cfg_attr(feature = "defmt", derive(defmt::Format))] 217 pub struct Key { 218 pub(crate) ll_src_addr: Ieee802154Address, 219 pub(crate) ll_dst_addr: Ieee802154Address, 220 pub(crate) datagram_size: u16, 221 pub(crate) datagram_tag: u16, 222 } 223 224 /// A read/write wrapper around a 6LoWPAN Fragment header. 225 /// [RFC 4944 § 5.3] specifies the format of the header. 226 /// 227 /// A First Fragment header has the following format: 228 /// ```txt 229 /// 1 2 3 230 /// 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 231 /// +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 232 /// |1 1 0 0 0| datagram_size | datagram_tag | 233 /// +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 234 /// ``` 235 /// 236 /// Subsequent fragment headers have the following format: 237 /// ```txt 238 /// 1 2 3 239 /// 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 240 /// +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 241 /// |1 1 1 0 0| datagram_size | datagram_tag | 242 /// +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 243 /// |datagram_offset| 244 /// +-+-+-+-+-+-+-+-+ 245 /// ``` 246 /// 247 /// [RFC 4944 § 5.3]: https://datatracker.ietf.org/doc/html/rfc4944#section-5.3 248 #[derive(Debug, Clone)] 249 #[cfg_attr(feature = "defmt", derive(defmt::Format))] 250 pub struct Packet<T: AsRef<[u8]>> { 251 buffer: T, 252 } 253 254 pub const FIRST_FRAGMENT_HEADER_SIZE: usize = 4; 255 pub const NEXT_FRAGMENT_HEADER_SIZE: usize = 5; 256 257 mod field { 258 use crate::wire::field::*; 259 260 pub const DISPATCH: usize = 0; 261 pub const DATAGRAM_SIZE: Field = 0..2; 262 pub const DATAGRAM_TAG: Field = 2..4; 263 pub const DATAGRAM_OFFSET: usize = 4; 264 265 pub const FIRST_FRAGMENT_REST: Rest = super::FIRST_FRAGMENT_HEADER_SIZE..; 266 pub const NEXT_FRAGMENT_REST: Rest = super::NEXT_FRAGMENT_HEADER_SIZE..; 267 } 268 269 impl<T: AsRef<[u8]>> Packet<T> { 270 /// Input a raw octet buffer with a 6LoWPAN Fragment header structure. new_unchecked(buffer: T) -> Self271 pub const fn new_unchecked(buffer: T) -> Self { 272 Self { buffer } 273 } 274 275 /// Shorthand for a combination of [new_unchecked] and [check_len]. 276 /// 277 /// [new_unchecked]: #method.new_unchecked 278 /// [check_len]: #method.check_len new_checked(buffer: T) -> Result<Self>279 pub fn new_checked(buffer: T) -> Result<Self> { 280 let packet = Self::new_unchecked(buffer); 281 packet.check_len()?; 282 283 let dispatch = packet.dispatch(); 284 285 if dispatch != DISPATCH_FIRST_FRAGMENT_HEADER && dispatch != DISPATCH_FRAGMENT_HEADER { 286 return Err(Error); 287 } 288 289 Ok(packet) 290 } 291 292 /// Ensure that no accessor method will panic if called. 293 /// Returns `Err(Error)` if the buffer is too short. check_len(&self) -> Result<()>294 pub fn check_len(&self) -> Result<()> { 295 let buffer = self.buffer.as_ref(); 296 if buffer.is_empty() { 297 return Err(Error); 298 } 299 300 match self.dispatch() { 301 DISPATCH_FIRST_FRAGMENT_HEADER if buffer.len() >= FIRST_FRAGMENT_HEADER_SIZE => { 302 Ok(()) 303 } 304 DISPATCH_FIRST_FRAGMENT_HEADER if buffer.len() < FIRST_FRAGMENT_HEADER_SIZE => { 305 Err(Error) 306 } 307 DISPATCH_FRAGMENT_HEADER if buffer.len() >= NEXT_FRAGMENT_HEADER_SIZE => Ok(()), 308 DISPATCH_FRAGMENT_HEADER if buffer.len() < NEXT_FRAGMENT_HEADER_SIZE => Err(Error), 309 _ => Err(Error), 310 } 311 } 312 313 /// Consumes the frame, returning the underlying buffer. into_inner(self) -> T314 pub fn into_inner(self) -> T { 315 self.buffer 316 } 317 318 /// Return the dispatch field. dispatch(&self) -> u8319 pub fn dispatch(&self) -> u8 { 320 let raw = self.buffer.as_ref(); 321 raw[field::DISPATCH] >> 3 322 } 323 324 /// Return the total datagram size. datagram_size(&self) -> u16325 pub fn datagram_size(&self) -> u16 { 326 let raw = self.buffer.as_ref(); 327 NetworkEndian::read_u16(&raw[field::DATAGRAM_SIZE]) & 0b111_1111_1111 328 } 329 330 /// Return the datagram tag. datagram_tag(&self) -> u16331 pub fn datagram_tag(&self) -> u16 { 332 let raw = self.buffer.as_ref(); 333 NetworkEndian::read_u16(&raw[field::DATAGRAM_TAG]) 334 } 335 336 /// Return the datagram offset. datagram_offset(&self) -> u8337 pub fn datagram_offset(&self) -> u8 { 338 match self.dispatch() { 339 DISPATCH_FIRST_FRAGMENT_HEADER => 0, 340 DISPATCH_FRAGMENT_HEADER => { 341 let raw = self.buffer.as_ref(); 342 raw[field::DATAGRAM_OFFSET] 343 } 344 _ => unreachable!(), 345 } 346 } 347 348 /// Returns `true` when this header is from the first fragment of a link. is_first_fragment(&self) -> bool349 pub fn is_first_fragment(&self) -> bool { 350 self.dispatch() == DISPATCH_FIRST_FRAGMENT_HEADER 351 } 352 353 /// Returns the key for identifying the packet it belongs to. get_key(&self, ieee802154_repr: &Ieee802154Repr) -> Key354 pub fn get_key(&self, ieee802154_repr: &Ieee802154Repr) -> Key { 355 Key { 356 ll_src_addr: ieee802154_repr.src_addr.unwrap(), 357 ll_dst_addr: ieee802154_repr.dst_addr.unwrap(), 358 datagram_size: self.datagram_size(), 359 datagram_tag: self.datagram_tag(), 360 } 361 } 362 } 363 364 impl<'a, T: AsRef<[u8]> + ?Sized> Packet<&'a T> { 365 /// Return the payload. payload(&self) -> &'a [u8]366 pub fn payload(&self) -> &'a [u8] { 367 match self.dispatch() { 368 DISPATCH_FIRST_FRAGMENT_HEADER => { 369 let raw = self.buffer.as_ref(); 370 &raw[field::FIRST_FRAGMENT_REST] 371 } 372 DISPATCH_FRAGMENT_HEADER => { 373 let raw = self.buffer.as_ref(); 374 &raw[field::NEXT_FRAGMENT_REST] 375 } 376 _ => unreachable!(), 377 } 378 } 379 } 380 381 impl<T: AsRef<[u8]> + AsMut<[u8]>> Packet<T> { set_dispatch_field(&mut self, value: u8)382 fn set_dispatch_field(&mut self, value: u8) { 383 let raw = self.buffer.as_mut(); 384 raw[field::DISPATCH] = (raw[field::DISPATCH] & !(0b11111 << 3)) | (value << 3); 385 } 386 set_datagram_size(&mut self, size: u16)387 fn set_datagram_size(&mut self, size: u16) { 388 let raw = self.buffer.as_mut(); 389 let mut v = NetworkEndian::read_u16(&raw[field::DATAGRAM_SIZE]); 390 v = (v & !0b111_1111_1111) | size; 391 392 NetworkEndian::write_u16(&mut raw[field::DATAGRAM_SIZE], v); 393 } 394 set_datagram_tag(&mut self, tag: u16)395 fn set_datagram_tag(&mut self, tag: u16) { 396 let raw = self.buffer.as_mut(); 397 NetworkEndian::write_u16(&mut raw[field::DATAGRAM_TAG], tag); 398 } 399 set_datagram_offset(&mut self, offset: u8)400 fn set_datagram_offset(&mut self, offset: u8) { 401 let raw = self.buffer.as_mut(); 402 raw[field::DATAGRAM_OFFSET] = offset; 403 } 404 } 405 406 /// A high-level representation of a 6LoWPAN Fragment header. 407 #[derive(Debug, PartialEq, Eq, Clone, Copy)] 408 #[cfg_attr(feature = "defmt", derive(defmt::Format))] 409 pub enum Repr { 410 FirstFragment { size: u16, tag: u16 }, 411 Fragment { size: u16, tag: u16, offset: u8 }, 412 } 413 414 impl Repr { 415 /// Parse a 6LoWPAN Fragment header. parse<T: AsRef<[u8]>>(packet: &Packet<T>) -> Result<Self>416 pub fn parse<T: AsRef<[u8]>>(packet: &Packet<T>) -> Result<Self> { 417 let size = packet.datagram_size(); 418 let tag = packet.datagram_tag(); 419 420 match packet.dispatch() { 421 DISPATCH_FIRST_FRAGMENT_HEADER => Ok(Self::FirstFragment { size, tag }), 422 DISPATCH_FRAGMENT_HEADER => Ok(Self::Fragment { 423 size, 424 tag, 425 offset: packet.datagram_offset(), 426 }), 427 _ => Err(Error), 428 } 429 } 430 431 /// Returns the length of the Fragment header. buffer_len(&self) -> usize432 pub const fn buffer_len(&self) -> usize { 433 match self { 434 Self::FirstFragment { .. } => field::FIRST_FRAGMENT_REST.start, 435 Self::Fragment { .. } => field::NEXT_FRAGMENT_REST.start, 436 } 437 } 438 439 /// Emit a high-level representation into a 6LoWPAN Fragment header. emit<T: AsRef<[u8]> + AsMut<[u8]>>(&self, packet: &mut Packet<T>)440 pub fn emit<T: AsRef<[u8]> + AsMut<[u8]>>(&self, packet: &mut Packet<T>) { 441 match self { 442 Self::FirstFragment { size, tag } => { 443 packet.set_dispatch_field(DISPATCH_FIRST_FRAGMENT_HEADER); 444 packet.set_datagram_size(*size); 445 packet.set_datagram_tag(*tag); 446 } 447 Self::Fragment { size, tag, offset } => { 448 packet.set_dispatch_field(DISPATCH_FRAGMENT_HEADER); 449 packet.set_datagram_size(*size); 450 packet.set_datagram_tag(*tag); 451 packet.set_datagram_offset(*offset); 452 } 453 } 454 } 455 } 456 } 457 458 #[derive(Debug, PartialEq, Eq, Clone, Copy)] 459 #[cfg_attr(feature = "defmt", derive(defmt::Format))] 460 pub enum NextHeader { 461 Compressed, 462 Uncompressed(IpProtocol), 463 } 464 465 pub mod iphc { 466 //! Implementation of IP Header Compression from [RFC 6282 § 3.1]. 467 //! It defines the compression of IPv6 headers. 468 //! 469 //! [RFC 6282 § 3.1]: https://datatracker.ietf.org/doc/html/rfc6282#section-3.1 470 471 use super::{ 472 AddressContext, AddressMode, Error, NextHeader, Result, UnresolvedAddress, 473 DISPATCH_IPHC_HEADER, 474 }; 475 use crate::wire::{ieee802154::Address as LlAddress, ipv6, IpProtocol}; 476 use byteorder::{ByteOrder, NetworkEndian}; 477 478 mod field { 479 use crate::wire::field::*; 480 481 pub const IPHC_FIELD: Field = 0..2; 482 } 483 484 macro_rules! get_field { 485 ($name:ident, $mask:expr, $shift:expr) => { 486 fn $name(&self) -> u8 { 487 let data = self.buffer.as_ref(); 488 let raw = NetworkEndian::read_u16(&data[field::IPHC_FIELD]); 489 ((raw >> $shift) & $mask) as u8 490 } 491 }; 492 } 493 494 macro_rules! set_field { 495 ($name:ident, $mask:expr, $shift:expr) => { 496 fn $name(&mut self, val: u8) { 497 let data = &mut self.buffer.as_mut()[field::IPHC_FIELD]; 498 let mut raw = NetworkEndian::read_u16(data); 499 500 raw = (raw & !($mask << $shift)) | ((val as u16) << $shift); 501 NetworkEndian::write_u16(data, raw); 502 } 503 }; 504 } 505 506 /// A read/write wrapper around a 6LoWPAN IPHC header. 507 /// [RFC 6282 § 3.1] specifies the format of the header. 508 /// 509 /// The header always start with the following base format (from [RFC 6282 § 3.1.1]): 510 /// ```txt 511 /// 0 1 512 /// 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 513 /// +---+---+---+---+---+---+---+---+---+---+---+---+---+---+---+---+ 514 /// | 0 | 1 | 1 | TF |NH | HLIM |CID|SAC| SAM | M |DAC| DAM | 515 /// +---+---+---+---+---+---+---+---+---+---+---+---+---+---+---+---+ 516 /// ``` 517 /// With: 518 /// - TF: Traffic Class and Flow Label 519 /// - NH: Next Header 520 /// - HLIM: Hop Limit 521 /// - CID: Context Identifier Extension 522 /// - SAC: Source Address Compression 523 /// - SAM: Source Address Mode 524 /// - M: Multicast Compression 525 /// - DAC: Destination Address Compression 526 /// - DAM: Destination Address Mode 527 /// 528 /// Depending on the flags in the base format, the following fields are added to the header: 529 /// - Traffic Class and Flow Label 530 /// - Next Header 531 /// - Hop Limit 532 /// - IPv6 source address 533 /// - IPv6 destinatino address 534 /// 535 /// [RFC 6282 § 3.1]: https://datatracker.ietf.org/doc/html/rfc6282#section-3.1 536 /// [RFC 6282 § 3.1.1]: https://datatracker.ietf.org/doc/html/rfc6282#section-3.1.1 537 #[derive(Debug, Clone)] 538 #[cfg_attr(feature = "defmt", derive(defmt::Format))] 539 pub struct Packet<T: AsRef<[u8]>> { 540 buffer: T, 541 } 542 543 impl<T: AsRef<[u8]>> Packet<T> { 544 /// Input a raw octet buffer with a 6LoWPAN IPHC header structure. new_unchecked(buffer: T) -> Self545 pub const fn new_unchecked(buffer: T) -> Self { 546 Packet { buffer } 547 } 548 549 /// Shorthand for a combination of [new_unchecked] and [check_len]. 550 /// 551 /// [new_unchecked]: #method.new_unchecked 552 /// [check_len]: #method.check_len new_checked(buffer: T) -> Result<Self>553 pub fn new_checked(buffer: T) -> Result<Self> { 554 let packet = Self::new_unchecked(buffer); 555 packet.check_len()?; 556 Ok(packet) 557 } 558 559 /// Ensure that no accessor method will panic if called. 560 /// Returns `Err(Error)` if the buffer is too short. check_len(&self) -> Result<()>561 pub fn check_len(&self) -> Result<()> { 562 let buffer = self.buffer.as_ref(); 563 if buffer.len() < 2 { 564 return Err(Error); 565 } 566 567 let mut offset = self.ip_fields_start() 568 + self.traffic_class_size() 569 + self.next_header_size() 570 + self.hop_limit_size(); 571 offset += self.src_address_size(); 572 offset += self.dst_address_size(); 573 574 if offset as usize > buffer.len() { 575 return Err(Error); 576 } 577 578 Ok(()) 579 } 580 581 /// Consumes the frame, returning the underlying buffer. into_inner(self) -> T582 pub fn into_inner(self) -> T { 583 self.buffer 584 } 585 586 /// Return the Next Header field. next_header(&self) -> NextHeader587 pub fn next_header(&self) -> NextHeader { 588 let nh = self.nh_field(); 589 590 if nh == 1 { 591 // The next header field is compressed. 592 // It is also encoded using LOWPAN_NHC. 593 NextHeader::Compressed 594 } else { 595 // The full 8 bits for Next Header are carried in-line. 596 let start = (self.ip_fields_start() + self.traffic_class_size()) as usize; 597 598 let data = self.buffer.as_ref(); 599 let nh = data[start..start + 1][0]; 600 NextHeader::Uncompressed(IpProtocol::from(nh)) 601 } 602 } 603 604 /// Return the Hop Limit. hop_limit(&self) -> u8605 pub fn hop_limit(&self) -> u8 { 606 match self.hlim_field() { 607 0b00 => { 608 let start = (self.ip_fields_start() 609 + self.traffic_class_size() 610 + self.next_header_size()) as usize; 611 612 let data = self.buffer.as_ref(); 613 data[start..start + 1][0] 614 } 615 0b01 => 1, 616 0b10 => 64, 617 0b11 => 255, 618 _ => unreachable!(), 619 } 620 } 621 622 /// Return the Source Context Identifier. src_context_id(&self) -> Option<u8>623 pub fn src_context_id(&self) -> Option<u8> { 624 if self.cid_field() == 1 { 625 let data = self.buffer.as_ref(); 626 Some(data[2] >> 4) 627 } else { 628 None 629 } 630 } 631 632 /// Return the Destination Context Identifier. dst_context_id(&self) -> Option<u8>633 pub fn dst_context_id(&self) -> Option<u8> { 634 if self.cid_field() == 1 { 635 let data = self.buffer.as_ref(); 636 Some(data[2] & 0x0f) 637 } else { 638 None 639 } 640 } 641 642 /// Return the ECN field (when it is inlined). ecn_field(&self) -> Option<u8>643 pub fn ecn_field(&self) -> Option<u8> { 644 match self.tf_field() { 645 0b00 | 0b01 | 0b10 => { 646 let start = self.ip_fields_start() as usize; 647 Some(self.buffer.as_ref()[start..][0] & 0b1100_0000) 648 } 649 0b11 => None, 650 _ => unreachable!(), 651 } 652 } 653 654 /// Return the DSCP field (when it is inlined). dscp_field(&self) -> Option<u8>655 pub fn dscp_field(&self) -> Option<u8> { 656 match self.tf_field() { 657 0b00 | 0b10 => { 658 let start = self.ip_fields_start() as usize; 659 Some(self.buffer.as_ref()[start..][0] & 0b1111_11) 660 } 661 0b01 | 0b11 => None, 662 _ => unreachable!(), 663 } 664 } 665 666 /// Return the flow label field (when it is inlined). flow_label_field(&self) -> Option<u16>667 pub fn flow_label_field(&self) -> Option<u16> { 668 match self.tf_field() { 669 0b00 => { 670 let start = self.ip_fields_start() as usize; 671 Some(NetworkEndian::read_u16( 672 &self.buffer.as_ref()[start..][2..4], 673 )) 674 } 675 0b01 => { 676 let start = self.ip_fields_start() as usize; 677 Some(NetworkEndian::read_u16( 678 &self.buffer.as_ref()[start..][1..3], 679 )) 680 } 681 0b10 | 0b11 => None, 682 _ => unreachable!(), 683 } 684 } 685 686 /// Return the Source Address. src_addr(&self) -> Result<UnresolvedAddress>687 pub fn src_addr(&self) -> Result<UnresolvedAddress> { 688 let start = (self.ip_fields_start() 689 + self.traffic_class_size() 690 + self.next_header_size() 691 + self.hop_limit_size()) as usize; 692 693 let data = self.buffer.as_ref(); 694 match (self.sac_field(), self.sam_field()) { 695 (0, 0b00) => Ok(UnresolvedAddress::WithoutContext(AddressMode::FullInline( 696 &data[start..][..16], 697 ))), 698 (0, 0b01) => Ok(UnresolvedAddress::WithoutContext( 699 AddressMode::InLine64bits(&data[start..][..8]), 700 )), 701 (0, 0b10) => Ok(UnresolvedAddress::WithoutContext( 702 AddressMode::InLine16bits(&data[start..][..2]), 703 )), 704 (0, 0b11) => Ok(UnresolvedAddress::WithoutContext(AddressMode::FullyElided)), 705 (1, 0b00) => Ok(UnresolvedAddress::WithContext(( 706 0, 707 AddressMode::Unspecified, 708 ))), 709 (1, 0b01) => { 710 if let Some(id) = self.src_context_id() { 711 Ok(UnresolvedAddress::WithContext(( 712 id as usize, 713 AddressMode::InLine64bits(&data[start..][..8]), 714 ))) 715 } else { 716 Err(Error) 717 } 718 } 719 (1, 0b10) => { 720 if let Some(id) = self.src_context_id() { 721 Ok(UnresolvedAddress::WithContext(( 722 id as usize, 723 AddressMode::InLine16bits(&data[start..][..2]), 724 ))) 725 } else { 726 Err(Error) 727 } 728 } 729 (1, 0b11) => { 730 if let Some(id) = self.src_context_id() { 731 Ok(UnresolvedAddress::WithContext(( 732 id as usize, 733 AddressMode::FullyElided, 734 ))) 735 } else { 736 Err(Error) 737 } 738 } 739 _ => Err(Error), 740 } 741 } 742 743 /// Return the Destination Address. dst_addr(&self) -> Result<UnresolvedAddress>744 pub fn dst_addr(&self) -> Result<UnresolvedAddress> { 745 let start = (self.ip_fields_start() 746 + self.traffic_class_size() 747 + self.next_header_size() 748 + self.hop_limit_size() 749 + self.src_address_size()) as usize; 750 751 let data = self.buffer.as_ref(); 752 match (self.m_field(), self.dac_field(), self.dam_field()) { 753 (0, 0, 0b00) => Ok(UnresolvedAddress::WithoutContext(AddressMode::FullInline( 754 &data[start..][..16], 755 ))), 756 (0, 0, 0b01) => Ok(UnresolvedAddress::WithoutContext( 757 AddressMode::InLine64bits(&data[start..][..8]), 758 )), 759 (0, 0, 0b10) => Ok(UnresolvedAddress::WithoutContext( 760 AddressMode::InLine16bits(&data[start..][..2]), 761 )), 762 (0, 0, 0b11) => Ok(UnresolvedAddress::WithoutContext(AddressMode::FullyElided)), 763 (0, 1, 0b00) => Ok(UnresolvedAddress::Reserved), 764 (0, 1, 0b01) => { 765 if let Some(id) = self.dst_context_id() { 766 Ok(UnresolvedAddress::WithContext(( 767 id as usize, 768 AddressMode::InLine64bits(&data[start..][..8]), 769 ))) 770 } else { 771 Err(Error) 772 } 773 } 774 (0, 1, 0b10) => { 775 if let Some(id) = self.dst_context_id() { 776 Ok(UnresolvedAddress::WithContext(( 777 id as usize, 778 AddressMode::InLine16bits(&data[start..][..2]), 779 ))) 780 } else { 781 Err(Error) 782 } 783 } 784 (0, 1, 0b11) => { 785 if let Some(id) = self.dst_context_id() { 786 Ok(UnresolvedAddress::WithContext(( 787 id as usize, 788 AddressMode::FullyElided, 789 ))) 790 } else { 791 Err(Error) 792 } 793 } 794 (1, 0, 0b00) => Ok(UnresolvedAddress::WithoutContext(AddressMode::FullInline( 795 &data[start..][..16], 796 ))), 797 (1, 0, 0b01) => Ok(UnresolvedAddress::WithoutContext( 798 AddressMode::Multicast48bits(&data[start..][..6]), 799 )), 800 (1, 0, 0b10) => Ok(UnresolvedAddress::WithoutContext( 801 AddressMode::Multicast32bits(&data[start..][..4]), 802 )), 803 (1, 0, 0b11) => Ok(UnresolvedAddress::WithoutContext( 804 AddressMode::Multicast8bits(&data[start..][..1]), 805 )), 806 (1, 1, 0b00) => Ok(UnresolvedAddress::WithContext(( 807 0, 808 AddressMode::NotSupported, 809 ))), 810 (1, 1, 0b01 | 0b10 | 0b11) => Ok(UnresolvedAddress::Reserved), 811 _ => Err(Error), 812 } 813 } 814 815 get_field!(dispatch_field, 0b111, 13); 816 get_field!(tf_field, 0b11, 11); 817 get_field!(nh_field, 0b1, 10); 818 get_field!(hlim_field, 0b11, 8); 819 get_field!(cid_field, 0b1, 7); 820 get_field!(sac_field, 0b1, 6); 821 get_field!(sam_field, 0b11, 4); 822 get_field!(m_field, 0b1, 3); 823 get_field!(dac_field, 0b1, 2); 824 get_field!(dam_field, 0b11, 0); 825 826 /// Return the start for the IP fields. ip_fields_start(&self) -> u8827 fn ip_fields_start(&self) -> u8 { 828 2 + self.cid_size() 829 } 830 831 /// Get the size in octets of the traffic class field. traffic_class_size(&self) -> u8832 fn traffic_class_size(&self) -> u8 { 833 match self.tf_field() { 834 0b00 => 4, 835 0b01 => 3, 836 0b10 => 1, 837 0b11 => 0, 838 _ => unreachable!(), 839 } 840 } 841 842 /// Get the size in octets of the next header field. next_header_size(&self) -> u8843 fn next_header_size(&self) -> u8 { 844 (self.nh_field() != 1) as u8 845 } 846 847 /// Get the size in octets of the hop limit field. hop_limit_size(&self) -> u8848 fn hop_limit_size(&self) -> u8 { 849 (self.hlim_field() == 0b00) as u8 850 } 851 852 /// Get the size in octets of the CID field. cid_size(&self) -> u8853 fn cid_size(&self) -> u8 { 854 (self.cid_field() == 1) as u8 855 } 856 857 /// Get the size in octets of the source address. src_address_size(&self) -> u8858 fn src_address_size(&self) -> u8 { 859 match (self.sac_field(), self.sam_field()) { 860 (0, 0b00) => 16, // The full address is carried in-line. 861 (0, 0b01) => 8, // The first 64 bits are elided. 862 (0, 0b10) => 2, // The first 112 bits are elided. 863 (0, 0b11) => 0, // The address is fully elided. 864 (1, 0b00) => 0, // The UNSPECIFIED address. 865 (1, 0b01) => 8, // Address derived using context information. 866 (1, 0b10) => 2, // Address derived using context information. 867 (1, 0b11) => 0, // Address derived using context information. 868 _ => unreachable!(), 869 } 870 } 871 872 /// Get the size in octets of the address address. dst_address_size(&self) -> u8873 fn dst_address_size(&self) -> u8 { 874 match (self.m_field(), self.dac_field(), self.dam_field()) { 875 (0, 0, 0b00) => 16, // The full address is carried in-line. 876 (0, 0, 0b01) => 8, // The first 64 bits are elided. 877 (0, 0, 0b10) => 2, // The first 112 bits are elided. 878 (0, 0, 0b11) => 0, // The address is fully elided. 879 (0, 1, 0b00) => 0, // Reserved. 880 (0, 1, 0b01) => 8, // Address derived using context information. 881 (0, 1, 0b10) => 2, // Address derived using context information. 882 (0, 1, 0b11) => 0, // Address derived using context information. 883 (1, 0, 0b00) => 16, // The full address is carried in-line. 884 (1, 0, 0b01) => 6, // The address takes the form ffXX::00XX:XXXX:XXXX. 885 (1, 0, 0b10) => 4, // The address takes the form ffXX::00XX:XXXX. 886 (1, 0, 0b11) => 1, // The address takes the form ff02::00XX. 887 (1, 1, 0b00) => 6, // Match Unicast-Prefix-based IPv6. 888 (1, 1, 0b01) => 0, // Reserved. 889 (1, 1, 0b10) => 0, // Reserved. 890 (1, 1, 0b11) => 0, // Reserved. 891 _ => unreachable!(), 892 } 893 } 894 895 /// Return the length of the header. header_len(&self) -> usize896 pub fn header_len(&self) -> usize { 897 let mut len = self.ip_fields_start(); 898 len += self.traffic_class_size(); 899 len += self.next_header_size(); 900 len += self.hop_limit_size(); 901 len += self.src_address_size(); 902 len += self.dst_address_size(); 903 904 len as usize 905 } 906 } 907 908 impl<'a, T: AsRef<[u8]> + ?Sized> Packet<&'a T> { 909 /// Return a pointer to the payload. payload(&self) -> &'a [u8]910 pub fn payload(&self) -> &'a [u8] { 911 let mut len = self.ip_fields_start(); 912 len += self.traffic_class_size(); 913 len += self.next_header_size(); 914 len += self.hop_limit_size(); 915 len += self.src_address_size(); 916 len += self.dst_address_size(); 917 918 let len = len as usize; 919 920 let data = self.buffer.as_ref(); 921 &data[len..] 922 } 923 } 924 925 impl<T: AsRef<[u8]> + AsMut<[u8]>> Packet<T> { 926 /// Set the dispatch field to `0b011`. set_dispatch_field(&mut self)927 fn set_dispatch_field(&mut self) { 928 let data = &mut self.buffer.as_mut()[field::IPHC_FIELD]; 929 let mut raw = NetworkEndian::read_u16(data); 930 931 raw = (raw & !(0b111 << 13)) | (0b11 << 13); 932 NetworkEndian::write_u16(data, raw); 933 } 934 935 set_field!(set_tf_field, 0b11, 11); 936 set_field!(set_nh_field, 0b1, 10); 937 set_field!(set_hlim_field, 0b11, 8); 938 set_field!(set_cid_field, 0b1, 7); 939 set_field!(set_sac_field, 0b1, 6); 940 set_field!(set_sam_field, 0b11, 4); 941 set_field!(set_m_field, 0b1, 3); 942 set_field!(set_dac_field, 0b1, 2); 943 set_field!(set_dam_field, 0b11, 0); 944 set_field(&mut self, idx: usize, value: &[u8])945 fn set_field(&mut self, idx: usize, value: &[u8]) { 946 let raw = self.buffer.as_mut(); 947 raw[idx..idx + value.len()].copy_from_slice(value); 948 } 949 950 /// Set the Next Header. 951 /// 952 /// **NOTE**: `idx` is the offset at which the Next Header needs to be written to. set_next_header(&mut self, nh: NextHeader, mut idx: usize) -> usize953 fn set_next_header(&mut self, nh: NextHeader, mut idx: usize) -> usize { 954 match nh { 955 NextHeader::Uncompressed(nh) => { 956 self.set_nh_field(0); 957 self.set_field(idx, &[nh.into()]); 958 idx += 1; 959 } 960 NextHeader::Compressed => self.set_nh_field(1), 961 } 962 963 idx 964 } 965 966 /// Set the Hop Limit. 967 /// 968 /// **NOTE**: `idx` is the offset at which the Next Header needs to be written to. set_hop_limit(&mut self, hl: u8, mut idx: usize) -> usize969 fn set_hop_limit(&mut self, hl: u8, mut idx: usize) -> usize { 970 match hl { 971 255 => self.set_hlim_field(0b11), 972 64 => self.set_hlim_field(0b10), 973 1 => self.set_hlim_field(0b01), 974 _ => { 975 self.set_hlim_field(0b00); 976 self.set_field(idx, &[hl]); 977 idx += 1; 978 } 979 } 980 981 idx 982 } 983 984 /// Set the Source Address based on the IPv6 address and the Link-Local address. 985 /// 986 /// **NOTE**: `idx` is the offset at which the Next Header needs to be written to. set_src_address( &mut self, src_addr: ipv6::Address, ll_src_addr: Option<LlAddress>, mut idx: usize, ) -> usize987 fn set_src_address( 988 &mut self, 989 src_addr: ipv6::Address, 990 ll_src_addr: Option<LlAddress>, 991 mut idx: usize, 992 ) -> usize { 993 self.set_cid_field(0); 994 self.set_sac_field(0); 995 let src = src_addr.as_bytes(); 996 if src_addr == ipv6::Address::UNSPECIFIED { 997 self.set_sac_field(1); 998 self.set_sam_field(0b00); 999 } else if src_addr.is_link_local() { 1000 // We have a link local address. 1001 // The remainder of the address can be elided when the context contains 1002 // a 802.15.4 short address or a 802.15.4 extended address which can be 1003 // converted to a eui64 address. 1004 let is_eui_64 = ll_src_addr 1005 .map(|addr| { 1006 addr.as_eui_64() 1007 .map(|addr| addr[..] == src[8..]) 1008 .unwrap_or(false) 1009 }) 1010 .unwrap_or(false); 1011 1012 if src[8..14] == [0, 0, 0, 0xff, 0xfe, 0] { 1013 let ll = [src[14], src[15]]; 1014 1015 if ll_src_addr == Some(LlAddress::Short(ll)) { 1016 // We have the context from the 802.15.4 frame. 1017 // The context contains the short address. 1018 // We can elide the source address. 1019 self.set_sam_field(0b11); 1020 } else { 1021 // We don't have the context from the 802.15.4 frame. 1022 // We cannot elide the source address, however we can elide 112 bits. 1023 self.set_sam_field(0b10); 1024 1025 self.set_field(idx, &src[14..]); 1026 idx += 2; 1027 } 1028 } else if is_eui_64 { 1029 // We have the context from the 802.15.4 frame. 1030 // The context contains the extended address. 1031 // We can elide the source address. 1032 self.set_sam_field(0b11); 1033 } else { 1034 // We cannot elide the source address, however we can elide 64 bits. 1035 self.set_sam_field(0b01); 1036 1037 self.set_field(idx, &src[8..]); 1038 idx += 8; 1039 } 1040 } else { 1041 // We cannot elide anything. 1042 self.set_sam_field(0b00); 1043 self.set_field(idx, src); 1044 idx += 16; 1045 } 1046 1047 idx 1048 } 1049 1050 /// Set the Destination Address based on the IPv6 address and the Link-Local address. 1051 /// 1052 /// **NOTE**: `idx` is the offset at which the Next Header needs to be written to. set_dst_address( &mut self, dst_addr: ipv6::Address, ll_dst_addr: Option<LlAddress>, mut idx: usize, ) -> usize1053 fn set_dst_address( 1054 &mut self, 1055 dst_addr: ipv6::Address, 1056 ll_dst_addr: Option<LlAddress>, 1057 mut idx: usize, 1058 ) -> usize { 1059 self.set_dac_field(0); 1060 self.set_dam_field(0); 1061 self.set_m_field(0); 1062 let dst = dst_addr.as_bytes(); 1063 if dst_addr.is_multicast() { 1064 self.set_m_field(1); 1065 1066 if dst[1] == 0x02 && dst[2..15] == [0; 13] { 1067 self.set_dam_field(0b11); 1068 1069 self.set_field(idx, &[dst[15]]); 1070 idx += 1; 1071 } else if dst[2..13] == [0; 11] { 1072 self.set_dam_field(0b10); 1073 1074 self.set_field(idx, &[dst[1]]); 1075 idx += 1; 1076 self.set_field(idx, &dst[13..]); 1077 idx += 3; 1078 } else if dst[2..11] == [0; 9] { 1079 self.set_dam_field(0b01); 1080 1081 self.set_field(idx, &[dst[1]]); 1082 idx += 1; 1083 self.set_field(idx, &dst[11..]); 1084 idx += 5; 1085 } else { 1086 self.set_dam_field(0b11); 1087 1088 self.set_field(idx, dst); 1089 idx += 16; 1090 } 1091 } else if dst_addr.is_link_local() { 1092 let is_eui_64 = ll_dst_addr 1093 .map(|addr| { 1094 addr.as_eui_64() 1095 .map(|addr| addr[..] == dst[8..]) 1096 .unwrap_or(false) 1097 }) 1098 .unwrap_or(false); 1099 1100 if dst[8..14] == [0, 0, 0, 0xff, 0xfe, 0] { 1101 let ll = [dst[14], dst[15]]; 1102 1103 if ll_dst_addr == Some(LlAddress::Short(ll)) { 1104 self.set_dam_field(0b11); 1105 } else { 1106 self.set_dam_field(0b10); 1107 1108 self.set_field(idx, &dst[14..]); 1109 idx += 2; 1110 } 1111 } else if is_eui_64 { 1112 self.set_dam_field(0b11); 1113 } else { 1114 self.set_dam_field(0b01); 1115 1116 self.set_field(idx, &dst[8..]); 1117 idx += 8; 1118 } 1119 } else { 1120 self.set_dam_field(0b00); 1121 1122 self.set_field(idx, dst); 1123 idx += 16; 1124 } 1125 1126 idx 1127 } 1128 1129 /// Return a mutable pointer to the payload. payload_mut(&mut self) -> &mut [u8]1130 pub fn payload_mut(&mut self) -> &mut [u8] { 1131 let mut len = self.ip_fields_start(); 1132 1133 len += self.traffic_class_size(); 1134 len += self.next_header_size(); 1135 len += self.hop_limit_size(); 1136 len += self.src_address_size(); 1137 len += self.dst_address_size(); 1138 1139 let len = len as usize; 1140 1141 let data = self.buffer.as_mut(); 1142 &mut data[len..] 1143 } 1144 } 1145 1146 /// A high-level representation of a 6LoWPAN IPHC header. 1147 #[derive(Debug, PartialEq, Eq, Clone, Copy)] 1148 #[cfg_attr(feature = "defmt", derive(defmt::Format))] 1149 pub struct Repr { 1150 pub src_addr: ipv6::Address, 1151 pub ll_src_addr: Option<LlAddress>, 1152 pub dst_addr: ipv6::Address, 1153 pub ll_dst_addr: Option<LlAddress>, 1154 pub next_header: NextHeader, 1155 pub hop_limit: u8, 1156 // TODO(thvdveld): refactor the following fields into something else 1157 pub ecn: Option<u8>, 1158 pub dscp: Option<u8>, 1159 pub flow_label: Option<u16>, 1160 } 1161 1162 impl Repr { 1163 /// Parse a 6LoWPAN IPHC header and return a high-level representation. 1164 /// 1165 /// The `ll_src_addr` and `ll_dst_addr` are the link-local addresses used for resolving the 1166 /// IPv6 packets. parse<T: AsRef<[u8]> + ?Sized>( packet: &Packet<&T>, ll_src_addr: Option<LlAddress>, ll_dst_addr: Option<LlAddress>, addr_context: &[AddressContext], ) -> Result<Self>1167 pub fn parse<T: AsRef<[u8]> + ?Sized>( 1168 packet: &Packet<&T>, 1169 ll_src_addr: Option<LlAddress>, 1170 ll_dst_addr: Option<LlAddress>, 1171 addr_context: &[AddressContext], 1172 ) -> Result<Self> { 1173 // Ensure basic accessors will work. 1174 packet.check_len()?; 1175 1176 if packet.dispatch_field() != DISPATCH_IPHC_HEADER { 1177 // This is not an LOWPAN_IPHC packet. 1178 return Err(Error); 1179 } 1180 1181 let src_addr = packet.src_addr()?.resolve(ll_src_addr, addr_context)?; 1182 let dst_addr = packet.dst_addr()?.resolve(ll_dst_addr, addr_context)?; 1183 1184 Ok(Self { 1185 src_addr, 1186 ll_src_addr, 1187 dst_addr, 1188 ll_dst_addr, 1189 next_header: packet.next_header(), 1190 hop_limit: packet.hop_limit(), 1191 ecn: packet.ecn_field(), 1192 dscp: packet.dscp_field(), 1193 flow_label: packet.flow_label_field(), 1194 }) 1195 } 1196 1197 /// Return the length of a header that will be emitted from this high-level representation. buffer_len(&self) -> usize1198 pub fn buffer_len(&self) -> usize { 1199 let mut len = 0; 1200 len += 2; // The minimal header length 1201 1202 len += match self.next_header { 1203 NextHeader::Compressed => 0, // The next header is compressed (we don't need to inline what the next header is) 1204 NextHeader::Uncompressed(_) => 1, // The next header field is inlined 1205 }; 1206 1207 // Hop Limit size 1208 len += match self.hop_limit { 1209 255 | 64 | 1 => 0, // We can inline the hop limit 1210 _ => 1, 1211 }; 1212 1213 // Add the length of the source address 1214 len += if self.src_addr == ipv6::Address::UNSPECIFIED { 1215 0 1216 } else if self.src_addr.is_link_local() { 1217 let src = self.src_addr.as_bytes(); 1218 let ll = [src[14], src[15]]; 1219 1220 let is_eui_64 = self 1221 .ll_src_addr 1222 .map(|addr| { 1223 addr.as_eui_64() 1224 .map(|addr| addr[..] == src[8..]) 1225 .unwrap_or(false) 1226 }) 1227 .unwrap_or(false); 1228 1229 if src[8..14] == [0, 0, 0, 0xff, 0xfe, 0] { 1230 if self.ll_src_addr == Some(LlAddress::Short(ll)) { 1231 0 1232 } else { 1233 2 1234 } 1235 } else if is_eui_64 { 1236 0 1237 } else { 1238 8 1239 } 1240 } else { 1241 16 1242 }; 1243 1244 // Add the size of the destination header 1245 let dst = self.dst_addr.as_bytes(); 1246 len += if self.dst_addr.is_multicast() { 1247 if dst[1] == 0x02 && dst[2..15] == [0; 13] { 1248 1 1249 } else if dst[2..13] == [0; 11] { 1250 4 1251 } else if dst[2..11] == [0; 9] { 1252 6 1253 } else { 1254 16 1255 } 1256 } else if self.dst_addr.is_link_local() { 1257 let is_eui_64 = self 1258 .ll_dst_addr 1259 .map(|addr| { 1260 addr.as_eui_64() 1261 .map(|addr| addr[..] == dst[8..]) 1262 .unwrap_or(false) 1263 }) 1264 .unwrap_or(false); 1265 1266 if dst[8..14] == [0, 0, 0, 0xff, 0xfe, 0] { 1267 let ll = [dst[14], dst[15]]; 1268 1269 if self.ll_dst_addr == Some(LlAddress::Short(ll)) { 1270 0 1271 } else { 1272 2 1273 } 1274 } else if is_eui_64 { 1275 0 1276 } else { 1277 8 1278 } 1279 } else { 1280 16 1281 }; 1282 1283 len += match (self.ecn, self.dscp, self.flow_label) { 1284 (Some(_), Some(_), Some(_)) => 4, 1285 (Some(_), None, Some(_)) => 3, 1286 (Some(_), Some(_), None) => 1, 1287 (None, None, None) => 0, 1288 _ => unreachable!(), 1289 }; 1290 1291 len 1292 } 1293 1294 /// Emit a high-level representation into a 6LoWPAN IPHC header. emit<T: AsRef<[u8]> + AsMut<[u8]>>(&self, packet: &mut Packet<T>)1295 pub fn emit<T: AsRef<[u8]> + AsMut<[u8]>>(&self, packet: &mut Packet<T>) { 1296 let idx = 2; 1297 1298 packet.set_dispatch_field(); 1299 1300 // FIXME(thvdveld): we don't set anything from the traffic flow. 1301 packet.set_tf_field(0b11); 1302 1303 let idx = packet.set_next_header(self.next_header, idx); 1304 let idx = packet.set_hop_limit(self.hop_limit, idx); 1305 let idx = packet.set_src_address(self.src_addr, self.ll_src_addr, idx); 1306 packet.set_dst_address(self.dst_addr, self.ll_dst_addr, idx); 1307 } 1308 } 1309 1310 #[cfg(test)] 1311 mod test { 1312 use super::*; 1313 1314 #[test] iphc_fields()1315 fn iphc_fields() { 1316 let bytes = [ 1317 0x7a, 0x33, // IPHC 1318 0x3a, // Next header 1319 ]; 1320 1321 let packet = Packet::new_unchecked(bytes); 1322 1323 assert_eq!(packet.dispatch_field(), 0b011); 1324 assert_eq!(packet.tf_field(), 0b11); 1325 assert_eq!(packet.nh_field(), 0b0); 1326 assert_eq!(packet.hlim_field(), 0b10); 1327 assert_eq!(packet.cid_field(), 0b0); 1328 assert_eq!(packet.sac_field(), 0b0); 1329 assert_eq!(packet.sam_field(), 0b11); 1330 assert_eq!(packet.m_field(), 0b0); 1331 assert_eq!(packet.dac_field(), 0b0); 1332 assert_eq!(packet.dam_field(), 0b11); 1333 1334 assert_eq!( 1335 packet.next_header(), 1336 NextHeader::Uncompressed(IpProtocol::Icmpv6) 1337 ); 1338 1339 assert_eq!(packet.src_address_size(), 0); 1340 assert_eq!(packet.dst_address_size(), 0); 1341 assert_eq!(packet.hop_limit(), 64); 1342 1343 assert_eq!( 1344 packet.src_addr(), 1345 Ok(UnresolvedAddress::WithoutContext(AddressMode::FullyElided)) 1346 ); 1347 assert_eq!( 1348 packet.dst_addr(), 1349 Ok(UnresolvedAddress::WithoutContext(AddressMode::FullyElided)) 1350 ); 1351 1352 let bytes = [ 1353 0x7e, 0xf7, // IPHC, 1354 0x00, // CID 1355 ]; 1356 1357 let packet = Packet::new_unchecked(bytes); 1358 1359 assert_eq!(packet.dispatch_field(), 0b011); 1360 assert_eq!(packet.tf_field(), 0b11); 1361 assert_eq!(packet.nh_field(), 0b1); 1362 assert_eq!(packet.hlim_field(), 0b10); 1363 assert_eq!(packet.cid_field(), 0b1); 1364 assert_eq!(packet.sac_field(), 0b1); 1365 assert_eq!(packet.sam_field(), 0b11); 1366 assert_eq!(packet.m_field(), 0b0); 1367 assert_eq!(packet.dac_field(), 0b1); 1368 assert_eq!(packet.dam_field(), 0b11); 1369 1370 assert_eq!(packet.next_header(), NextHeader::Compressed); 1371 1372 assert_eq!(packet.src_address_size(), 0); 1373 assert_eq!(packet.dst_address_size(), 0); 1374 assert_eq!(packet.hop_limit(), 64); 1375 1376 assert_eq!( 1377 packet.src_addr(), 1378 Ok(UnresolvedAddress::WithContext(( 1379 0, 1380 AddressMode::FullyElided 1381 ))) 1382 ); 1383 assert_eq!( 1384 packet.dst_addr(), 1385 Ok(UnresolvedAddress::WithContext(( 1386 0, 1387 AddressMode::FullyElided 1388 ))) 1389 ); 1390 } 1391 } 1392 } 1393 1394 pub mod nhc { 1395 //! Implementation of Next Header Compression from [RFC 6282 § 4]. 1396 //! 1397 //! [RFC 6282 § 4]: https://datatracker.ietf.org/doc/html/rfc6282#section-4 1398 use super::{Error, NextHeader, Result, DISPATCH_EXT_HEADER, DISPATCH_UDP_HEADER}; 1399 use crate::{ 1400 phy::ChecksumCapabilities, 1401 wire::{ 1402 ip::{checksum, Address as IpAddress}, 1403 ipv6, 1404 udp::Repr as UdpRepr, 1405 IpProtocol, 1406 }, 1407 }; 1408 use byteorder::{ByteOrder, NetworkEndian}; 1409 use ipv6::Address; 1410 1411 macro_rules! get_field { 1412 ($name:ident, $mask:expr, $shift:expr) => { 1413 fn $name(&self) -> u8 { 1414 let data = self.buffer.as_ref(); 1415 let raw = &data[0]; 1416 ((raw >> $shift) & $mask) as u8 1417 } 1418 }; 1419 } 1420 1421 macro_rules! set_field { 1422 ($name:ident, $mask:expr, $shift:expr) => { 1423 fn $name(&mut self, val: u8) { 1424 let data = self.buffer.as_mut(); 1425 let mut raw = data[0]; 1426 raw = (raw & !($mask << $shift)) | (val << $shift); 1427 data[0] = raw; 1428 } 1429 }; 1430 } 1431 1432 #[derive(Debug, Clone)] 1433 #[cfg_attr(feature = "defmt", derive(defmt::Format))] 1434 /// A read/write wrapper around a 6LoWPAN_NHC Header. 1435 /// [RFC 6282 § 4.2] specifies the format of the header. 1436 /// 1437 /// The header has the following format: 1438 /// ```txt 1439 /// 0 1 2 3 4 5 6 7 1440 /// +---+---+---+---+---+---+---+---+ 1441 /// | 1 | 1 | 1 | 0 | EID |NH | 1442 /// +---+---+---+---+---+---+---+---+ 1443 /// ``` 1444 /// 1445 /// With: 1446 /// - EID: the extension header ID 1447 /// - NH: Next Header 1448 /// 1449 /// [RFC 6282 § 4.2]: https://datatracker.ietf.org/doc/html/rfc6282#section-4.2 1450 pub enum NhcPacket { 1451 ExtHeader, 1452 UdpHeader, 1453 } 1454 1455 impl NhcPacket { 1456 /// Returns the type of the Next Header header. 1457 /// This can either be an Extenstion header or an 6LoWPAN Udp header. 1458 /// 1459 /// # Errors 1460 /// Returns `[Error::Unrecognized]` when neither the Extension Header dispatch or the Udp 1461 /// dispatch is recognized. dispatch(buffer: impl AsRef<[u8]>) -> Result<Self>1462 pub fn dispatch(buffer: impl AsRef<[u8]>) -> Result<Self> { 1463 let raw = buffer.as_ref(); 1464 if raw.is_empty() { 1465 return Err(Error); 1466 } 1467 1468 if raw[0] >> 4 == DISPATCH_EXT_HEADER { 1469 // We have a compressed IPv6 Extension Header. 1470 Ok(Self::ExtHeader) 1471 } else if raw[0] >> 3 == DISPATCH_UDP_HEADER { 1472 // We have a compressed UDP header. 1473 Ok(Self::UdpHeader) 1474 } else { 1475 Err(Error) 1476 } 1477 } 1478 } 1479 1480 #[derive(Debug, PartialEq, Eq, Clone, Copy)] 1481 #[cfg_attr(feature = "defmt", derive(defmt::Format))] 1482 pub enum ExtHeaderId { 1483 HopByHopHeader, 1484 RoutingHeader, 1485 FragmentHeader, 1486 DestinationOptionsHeader, 1487 MobilityHeader, 1488 Header, 1489 Reserved, 1490 } 1491 1492 impl From<ExtHeaderId> for IpProtocol { from(val: ExtHeaderId) -> Self1493 fn from(val: ExtHeaderId) -> Self { 1494 match val { 1495 ExtHeaderId::HopByHopHeader => Self::HopByHop, 1496 ExtHeaderId::RoutingHeader => Self::Ipv6Route, 1497 ExtHeaderId::FragmentHeader => Self::Ipv6Frag, 1498 ExtHeaderId::DestinationOptionsHeader => Self::Ipv6Opts, 1499 ExtHeaderId::MobilityHeader => Self::Unknown(0), 1500 ExtHeaderId::Header => Self::Unknown(0), 1501 ExtHeaderId::Reserved => Self::Unknown(0), 1502 } 1503 } 1504 } 1505 1506 /// A read/write wrapper around a 6LoWPAN NHC Extension header. 1507 #[derive(Debug, Clone)] 1508 #[cfg_attr(feature = "defmt", derive(defmt::Format))] 1509 pub struct ExtHeaderPacket<T: AsRef<[u8]>> { 1510 buffer: T, 1511 } 1512 1513 impl<T: AsRef<[u8]>> ExtHeaderPacket<T> { 1514 /// Input a raw octet buffer with a 6LoWPAN NHC Extension Header structure. new_unchecked(buffer: T) -> Self1515 pub const fn new_unchecked(buffer: T) -> Self { 1516 ExtHeaderPacket { buffer } 1517 } 1518 1519 /// Shorthand for a combination of [new_unchecked] and [check_len]. 1520 /// 1521 /// [new_unchecked]: #method.new_unchecked 1522 /// [check_len]: #method.check_len new_checked(buffer: T) -> Result<Self>1523 pub fn new_checked(buffer: T) -> Result<Self> { 1524 let packet = Self::new_unchecked(buffer); 1525 packet.check_len()?; 1526 1527 if packet.eid_field() > 7 { 1528 return Err(Error); 1529 } 1530 1531 Ok(packet) 1532 } 1533 1534 /// Ensure that no accessor method will panic if called. 1535 /// Returns `Err(Error)` if the buffer is too short. check_len(&self) -> Result<()>1536 pub fn check_len(&self) -> Result<()> { 1537 let buffer = self.buffer.as_ref(); 1538 1539 if buffer.is_empty() { 1540 return Err(Error); 1541 } 1542 1543 let mut len = 1; 1544 len += self.next_header_size(); 1545 1546 if len <= buffer.len() { 1547 Ok(()) 1548 } else { 1549 Err(Error) 1550 } 1551 } 1552 1553 /// Consumes the frame, returning the underlying buffer. into_inner(self) -> T1554 pub fn into_inner(self) -> T { 1555 self.buffer 1556 } 1557 1558 get_field!(dispatch_field, 0b1111, 4); 1559 get_field!(eid_field, 0b111, 1); 1560 get_field!(nh_field, 0b1, 0); 1561 1562 /// Return the Extension Header ID. extension_header_id(&self) -> ExtHeaderId1563 pub fn extension_header_id(&self) -> ExtHeaderId { 1564 match self.eid_field() { 1565 0 => ExtHeaderId::HopByHopHeader, 1566 1 => ExtHeaderId::RoutingHeader, 1567 2 => ExtHeaderId::FragmentHeader, 1568 3 => ExtHeaderId::DestinationOptionsHeader, 1569 4 => ExtHeaderId::MobilityHeader, 1570 5 | 6 => ExtHeaderId::Reserved, 1571 7 => ExtHeaderId::Header, 1572 _ => unreachable!(), 1573 } 1574 } 1575 1576 /// Parse the next header field. next_header(&self) -> NextHeader1577 pub fn next_header(&self) -> NextHeader { 1578 if self.nh_field() == 1 { 1579 NextHeader::Compressed 1580 } else { 1581 // The full 8 bits for Next Header are carried in-line. 1582 let start = 1; 1583 1584 let data = self.buffer.as_ref(); 1585 let nh = data[start]; 1586 NextHeader::Uncompressed(IpProtocol::from(nh)) 1587 } 1588 } 1589 1590 /// Return the size of the Next Header field. next_header_size(&self) -> usize1591 fn next_header_size(&self) -> usize { 1592 // If nh is set, then the Next Header is compressed using LOWPAN_NHC 1593 match self.nh_field() { 1594 0 => 1, 1595 1 => 0, 1596 _ => unreachable!(), 1597 } 1598 } 1599 } 1600 1601 impl<'a, T: AsRef<[u8]> + ?Sized> ExtHeaderPacket<&'a T> { 1602 /// Return a pointer to the payload. payload(&self) -> &'a [u8]1603 pub fn payload(&self) -> &'a [u8] { 1604 let start = 1 + self.next_header_size(); 1605 &self.buffer.as_ref()[start..] 1606 } 1607 } 1608 1609 impl<T: AsRef<[u8]> + AsMut<[u8]>> ExtHeaderPacket<T> { 1610 /// Return a mutable pointer to the payload. payload_mut(&mut self) -> &mut [u8]1611 pub fn payload_mut(&mut self) -> &mut [u8] { 1612 let start = 2 + self.next_header_size(); 1613 &mut self.buffer.as_mut()[start..] 1614 } 1615 1616 /// Set the dispatch field to `0b1110`. set_dispatch_field(&mut self)1617 fn set_dispatch_field(&mut self) { 1618 let data = self.buffer.as_mut(); 1619 data[0] = (data[0] & !(0b1111 << 4)) | (DISPATCH_EXT_HEADER << 4); 1620 } 1621 1622 set_field!(set_eid_field, 0b111, 1); 1623 set_field!(set_nh_field, 0b1, 0); 1624 1625 /// Set the Extension Header ID field. set_extension_header_id(&mut self, ext_header_id: ExtHeaderId)1626 fn set_extension_header_id(&mut self, ext_header_id: ExtHeaderId) { 1627 let id = match ext_header_id { 1628 ExtHeaderId::HopByHopHeader => 0, 1629 ExtHeaderId::RoutingHeader => 1, 1630 ExtHeaderId::FragmentHeader => 2, 1631 ExtHeaderId::DestinationOptionsHeader => 3, 1632 ExtHeaderId::MobilityHeader => 4, 1633 ExtHeaderId::Reserved => 5, 1634 ExtHeaderId::Header => 7, 1635 }; 1636 1637 self.set_eid_field(id); 1638 } 1639 1640 /// Set the Next Header. set_next_header(&mut self, next_header: NextHeader)1641 fn set_next_header(&mut self, next_header: NextHeader) { 1642 match next_header { 1643 NextHeader::Compressed => self.set_nh_field(0b1), 1644 NextHeader::Uncompressed(nh) => { 1645 self.set_nh_field(0b0); 1646 1647 let start = 1; 1648 let data = self.buffer.as_mut(); 1649 data[start] = nh.into(); 1650 } 1651 } 1652 } 1653 1654 /// Set the length. set_length(&mut self, length: u8)1655 fn set_length(&mut self, length: u8) { 1656 let start = 1 + self.next_header_size(); 1657 1658 let data = self.buffer.as_mut(); 1659 data[start] = length; 1660 } 1661 } 1662 1663 /// A high-level representation of an 6LoWPAN NHC Extension header. 1664 #[derive(Debug, PartialEq, Eq, Clone, Copy)] 1665 #[cfg_attr(feature = "defmt", derive(defmt::Format))] 1666 pub struct ExtHeaderRepr { 1667 ext_header_id: ExtHeaderId, 1668 next_header: NextHeader, 1669 length: u8, 1670 } 1671 1672 impl ExtHeaderRepr { 1673 /// Parse a 6LoWPAN NHC Extension Header packet and return a high-level representation. parse<T: AsRef<[u8]> + ?Sized>(packet: &ExtHeaderPacket<&T>) -> Result<Self>1674 pub fn parse<T: AsRef<[u8]> + ?Sized>(packet: &ExtHeaderPacket<&T>) -> Result<Self> { 1675 // Ensure basic accessors will work. 1676 packet.check_len()?; 1677 1678 if packet.dispatch_field() != DISPATCH_EXT_HEADER { 1679 return Err(Error); 1680 } 1681 1682 Ok(Self { 1683 ext_header_id: packet.extension_header_id(), 1684 next_header: packet.next_header(), 1685 length: packet.payload().len() as u8, 1686 }) 1687 } 1688 1689 /// Return the length of a header that will be emitted from this high-level representation. buffer_len(&self) -> usize1690 pub fn buffer_len(&self) -> usize { 1691 let mut len = 1; // The minimal header size 1692 1693 if self.next_header != NextHeader::Compressed { 1694 len += 1; 1695 } 1696 1697 len += 1; // The length 1698 1699 len 1700 } 1701 1702 /// Emit a high-level representaiton into a 6LoWPAN NHC Extension Header packet. emit<T: AsRef<[u8]> + AsMut<[u8]>>(&self, packet: &mut ExtHeaderPacket<T>)1703 pub fn emit<T: AsRef<[u8]> + AsMut<[u8]>>(&self, packet: &mut ExtHeaderPacket<T>) { 1704 packet.set_dispatch_field(); 1705 packet.set_extension_header_id(self.ext_header_id); 1706 packet.set_next_header(self.next_header); 1707 packet.set_length(self.length); 1708 } 1709 } 1710 1711 /// A read/write wrapper around a 6LoWPAN_NHC UDP frame. 1712 /// [RFC 6282 § 4.3] specifies the format of the header. 1713 /// 1714 /// The base header has the following formath: 1715 /// ```txt 1716 /// 0 1 2 3 4 5 6 7 1717 /// +---+---+---+---+---+---+---+---+ 1718 /// | 1 | 1 | 1 | 1 | 0 | C | P | 1719 /// +---+---+---+---+---+---+---+---+ 1720 /// With: 1721 /// - C: checksum, specifies if the checksum is elided. 1722 /// - P: ports, specifies if the ports are elided. 1723 /// ``` 1724 /// 1725 /// [RFC 6282 § 4.3]: https://datatracker.ietf.org/doc/html/rfc6282#section-4.3 1726 #[derive(Debug, Clone)] 1727 #[cfg_attr(feature = "defmt", derive(defmt::Format))] 1728 pub struct UdpNhcPacket<T: AsRef<[u8]>> { 1729 buffer: T, 1730 } 1731 1732 impl<T: AsRef<[u8]>> UdpNhcPacket<T> { 1733 /// Input a raw octet buffer with a LOWPAN_NHC frame structure for UDP. new_unchecked(buffer: T) -> Self1734 pub const fn new_unchecked(buffer: T) -> Self { 1735 Self { buffer } 1736 } 1737 1738 /// Shorthand for a combination of [new_unchecked] and [check_len]. 1739 /// 1740 /// [new_unchecked]: #method.new_unchecked 1741 /// [check_len]: #method.check_len new_checked(buffer: T) -> Result<Self>1742 pub fn new_checked(buffer: T) -> Result<Self> { 1743 let packet = Self::new_unchecked(buffer); 1744 packet.check_len()?; 1745 Ok(packet) 1746 } 1747 1748 /// Ensure that no accessor method will panic if called. 1749 /// Returns `Err(Error::Truncated)` if the buffer is too short. check_len(&self) -> Result<()>1750 pub fn check_len(&self) -> Result<()> { 1751 let buffer = self.buffer.as_ref(); 1752 1753 if buffer.is_empty() { 1754 return Err(Error); 1755 } 1756 1757 let index = 1 + self.ports_size() + self.checksum_size(); 1758 if index > buffer.len() { 1759 return Err(Error); 1760 } 1761 1762 Ok(()) 1763 } 1764 1765 /// Consumes the frame, returning the underlying buffer. into_inner(self) -> T1766 pub fn into_inner(self) -> T { 1767 self.buffer 1768 } 1769 1770 get_field!(dispatch_field, 0b11111, 3); 1771 get_field!(checksum_field, 0b1, 2); 1772 get_field!(ports_field, 0b11, 0); 1773 1774 /// Returns the index of the start of the next header compressed fields. nhc_fields_start(&self) -> usize1775 const fn nhc_fields_start(&self) -> usize { 1776 1 1777 } 1778 1779 /// Return the source port number. src_port(&self) -> u161780 pub fn src_port(&self) -> u16 { 1781 match self.ports_field() { 1782 0b00 | 0b01 => { 1783 // The full 16 bits are carried in-line. 1784 let data = self.buffer.as_ref(); 1785 let start = self.nhc_fields_start(); 1786 1787 NetworkEndian::read_u16(&data[start..start + 2]) 1788 } 1789 0b10 => { 1790 // The first 8 bits are elided. 1791 let data = self.buffer.as_ref(); 1792 let start = self.nhc_fields_start(); 1793 1794 0xf000 + data[start] as u16 1795 } 1796 0b11 => { 1797 // The first 12 bits are elided. 1798 let data = self.buffer.as_ref(); 1799 let start = self.nhc_fields_start(); 1800 1801 0xf0b0 + (data[start] >> 4) as u16 1802 } 1803 _ => unreachable!(), 1804 } 1805 } 1806 1807 /// Return the destination port number. dst_port(&self) -> u161808 pub fn dst_port(&self) -> u16 { 1809 match self.ports_field() { 1810 0b00 => { 1811 // The full 16 bits are carried in-line. 1812 let data = self.buffer.as_ref(); 1813 let idx = self.nhc_fields_start(); 1814 1815 NetworkEndian::read_u16(&data[idx + 2..idx + 4]) 1816 } 1817 0b01 => { 1818 // The first 8 bits are elided. 1819 let data = self.buffer.as_ref(); 1820 let idx = self.nhc_fields_start(); 1821 1822 0xf000 + data[idx] as u16 1823 } 1824 0b10 => { 1825 // The full 16 bits are carried in-line. 1826 let data = self.buffer.as_ref(); 1827 let idx = self.nhc_fields_start(); 1828 1829 NetworkEndian::read_u16(&data[idx + 1..idx + 1 + 2]) 1830 } 1831 0b11 => { 1832 // The first 12 bits are elided. 1833 let data = self.buffer.as_ref(); 1834 let start = self.nhc_fields_start(); 1835 1836 0xf0b0 + (data[start] & 0xff) as u16 1837 } 1838 _ => unreachable!(), 1839 } 1840 } 1841 1842 /// Return the checksum. checksum(&self) -> Option<u16>1843 pub fn checksum(&self) -> Option<u16> { 1844 if self.checksum_field() == 0b0 { 1845 // The first 12 bits are elided. 1846 let data = self.buffer.as_ref(); 1847 let start = self.nhc_fields_start() + self.ports_size(); 1848 Some(NetworkEndian::read_u16(&data[start..start + 2])) 1849 } else { 1850 // The checksum is elided and needs to be recomputed on the 6LoWPAN termination point. 1851 None 1852 } 1853 } 1854 1855 // Return the size of the checksum field. checksum_size(&self) -> usize1856 pub(crate) fn checksum_size(&self) -> usize { 1857 match self.checksum_field() { 1858 0b0 => 2, 1859 0b1 => 0, 1860 _ => unreachable!(), 1861 } 1862 } 1863 1864 /// Returns the total size of both port numbers. ports_size(&self) -> usize1865 pub(crate) fn ports_size(&self) -> usize { 1866 match self.ports_field() { 1867 0b00 => 4, // 16 bits + 16 bits 1868 0b01 => 3, // 16 bits + 8 bits 1869 0b10 => 3, // 8 bits + 16 bits 1870 0b11 => 1, // 4 bits + 4 bits 1871 _ => unreachable!(), 1872 } 1873 } 1874 } 1875 1876 impl<'a, T: AsRef<[u8]> + ?Sized> UdpNhcPacket<&'a T> { 1877 /// Return a pointer to the payload. payload(&self) -> &'a [u8]1878 pub fn payload(&self) -> &'a [u8] { 1879 let start = 1 + self.ports_size() + self.checksum_size(); 1880 &self.buffer.as_ref()[start..] 1881 } 1882 } 1883 1884 impl<T: AsRef<[u8]> + AsMut<[u8]>> UdpNhcPacket<T> { 1885 /// Return a mutable pointer to the payload. payload_mut(&mut self) -> &mut [u8]1886 pub fn payload_mut(&mut self) -> &mut [u8] { 1887 let start = 1 + self.ports_size() + 2; // XXX(thvdveld): we assume we put the checksum inlined. 1888 &mut self.buffer.as_mut()[start..] 1889 } 1890 1891 /// Set the dispatch field to `0b11110`. set_dispatch_field(&mut self)1892 fn set_dispatch_field(&mut self) { 1893 let data = self.buffer.as_mut(); 1894 data[0] = (data[0] & !(0b11111 << 3)) | (DISPATCH_UDP_HEADER << 3); 1895 } 1896 1897 set_field!(set_checksum_field, 0b1, 2); 1898 set_field!(set_ports_field, 0b11, 0); 1899 set_ports(&mut self, src_port: u16, dst_port: u16)1900 fn set_ports(&mut self, src_port: u16, dst_port: u16) { 1901 let mut idx = 1; 1902 1903 match (src_port, dst_port) { 1904 (0xf0b0..=0xf0bf, 0xf0b0..=0xf0bf) => { 1905 // We can compress both the source and destination ports. 1906 self.set_ports_field(0b11); 1907 let data = self.buffer.as_mut(); 1908 data[idx] = (((src_port - 0xf0b0) as u8) << 4) & ((dst_port - 0xf0b0) as u8); 1909 } 1910 (0xf000..=0xf0ff, _) => { 1911 // We can compress the source port, but not the destination port. 1912 self.set_ports_field(0b10); 1913 let data = self.buffer.as_mut(); 1914 data[idx] = (src_port - 0xf000) as u8; 1915 idx += 1; 1916 1917 NetworkEndian::write_u16(&mut data[idx..idx + 2], dst_port); 1918 } 1919 (_, 0xf000..=0xf0ff) => { 1920 // We can compress the destination port, but not the source port. 1921 self.set_ports_field(0b01); 1922 let data = self.buffer.as_mut(); 1923 NetworkEndian::write_u16(&mut data[idx..idx + 2], src_port); 1924 idx += 2; 1925 data[idx] = (dst_port - 0xf000) as u8; 1926 } 1927 (_, _) => { 1928 // We cannot compress any port. 1929 self.set_ports_field(0b00); 1930 let data = self.buffer.as_mut(); 1931 NetworkEndian::write_u16(&mut data[idx..idx + 2], src_port); 1932 idx += 2; 1933 NetworkEndian::write_u16(&mut data[idx..idx + 2], dst_port); 1934 } 1935 }; 1936 } 1937 set_checksum(&mut self, checksum: u16)1938 fn set_checksum(&mut self, checksum: u16) { 1939 self.set_checksum_field(0b0); 1940 let idx = 1 + self.ports_size(); 1941 let data = self.buffer.as_mut(); 1942 NetworkEndian::write_u16(&mut data[idx..idx + 2], checksum); 1943 } 1944 } 1945 1946 /// A high-level representation of a 6LoWPAN NHC UDP header. 1947 #[derive(Debug, PartialEq, Eq, Clone, Copy)] 1948 #[cfg_attr(feature = "defmt", derive(defmt::Format))] 1949 pub struct UdpNhcRepr(pub UdpRepr); 1950 1951 impl<'a> UdpNhcRepr { 1952 /// Parse a 6LoWPAN NHC UDP packet and return a high-level representation. parse<T: AsRef<[u8]> + ?Sized>( packet: &UdpNhcPacket<&'a T>, src_addr: &ipv6::Address, dst_addr: &ipv6::Address, checksum_caps: &ChecksumCapabilities, ) -> Result<Self>1953 pub fn parse<T: AsRef<[u8]> + ?Sized>( 1954 packet: &UdpNhcPacket<&'a T>, 1955 src_addr: &ipv6::Address, 1956 dst_addr: &ipv6::Address, 1957 checksum_caps: &ChecksumCapabilities, 1958 ) -> Result<Self> { 1959 packet.check_len()?; 1960 1961 if packet.dispatch_field() != DISPATCH_UDP_HEADER { 1962 return Err(Error); 1963 } 1964 1965 if checksum_caps.udp.rx() { 1966 let payload_len = packet.payload().len(); 1967 let chk_sum = !checksum::combine(&[ 1968 checksum::pseudo_header( 1969 &IpAddress::Ipv6(*src_addr), 1970 &IpAddress::Ipv6(*dst_addr), 1971 crate::wire::ip::Protocol::Udp, 1972 payload_len as u32 + 8, 1973 ), 1974 packet.src_port(), 1975 packet.dst_port(), 1976 payload_len as u16 + 8, 1977 checksum::data(packet.payload()), 1978 ]); 1979 1980 if let Some(checksum) = packet.checksum() { 1981 if chk_sum != checksum { 1982 return Err(Error); 1983 } 1984 } 1985 } 1986 1987 Ok(Self(UdpRepr { 1988 src_port: packet.src_port(), 1989 dst_port: packet.dst_port(), 1990 })) 1991 } 1992 1993 /// Return the length of a packet that will be emitted from this high-level representation. header_len(&self) -> usize1994 pub fn header_len(&self) -> usize { 1995 let mut len = 1; // The minimal header size 1996 1997 len += 2; // XXX We assume we will add the checksum at the end 1998 1999 // Check if we can compress the source and destination ports 2000 match (self.src_port, self.dst_port) { 2001 (0xf0b0..=0xf0bf, 0xf0b0..=0xf0bf) => len + 1, 2002 (0xf000..=0xf0ff, _) | (_, 0xf000..=0xf0ff) => len + 3, 2003 (_, _) => len + 4, 2004 } 2005 } 2006 2007 /// Emit a high-level representation into a LOWPAN_NHC UDP header. emit<T: AsRef<[u8]> + AsMut<[u8]>>( &self, packet: &mut UdpNhcPacket<T>, src_addr: &Address, dst_addr: &Address, payload_len: usize, emit_payload: impl FnOnce(&mut [u8]), )2008 pub fn emit<T: AsRef<[u8]> + AsMut<[u8]>>( 2009 &self, 2010 packet: &mut UdpNhcPacket<T>, 2011 src_addr: &Address, 2012 dst_addr: &Address, 2013 payload_len: usize, 2014 emit_payload: impl FnOnce(&mut [u8]), 2015 ) { 2016 packet.set_dispatch_field(); 2017 packet.set_ports(self.src_port, self.dst_port); 2018 emit_payload(packet.payload_mut()); 2019 2020 let chk_sum = !checksum::combine(&[ 2021 checksum::pseudo_header( 2022 &IpAddress::Ipv6(*src_addr), 2023 &IpAddress::Ipv6(*dst_addr), 2024 crate::wire::ip::Protocol::Udp, 2025 payload_len as u32 + 8, 2026 ), 2027 self.src_port, 2028 self.dst_port, 2029 payload_len as u16 + 8, 2030 checksum::data(packet.payload_mut()), 2031 ]); 2032 2033 packet.set_checksum(chk_sum); 2034 } 2035 } 2036 2037 impl core::ops::Deref for UdpNhcRepr { 2038 type Target = UdpRepr; 2039 deref(&self) -> &Self::Target2040 fn deref(&self) -> &Self::Target { 2041 &self.0 2042 } 2043 } 2044 2045 impl core::ops::DerefMut for UdpNhcRepr { deref_mut(&mut self) -> &mut Self::Target2046 fn deref_mut(&mut self) -> &mut Self::Target { 2047 &mut self.0 2048 } 2049 } 2050 2051 #[cfg(test)] 2052 mod test { 2053 use super::*; 2054 2055 #[test] ext_header_nhc_fields()2056 fn ext_header_nhc_fields() { 2057 let bytes = [0xe3, 0x06, 0x03, 0x00, 0xff, 0x00, 0x00, 0x00]; 2058 2059 let packet = ExtHeaderPacket::new_checked(&bytes[..]).unwrap(); 2060 assert_eq!(packet.next_header_size(), 0); 2061 assert_eq!(packet.dispatch_field(), DISPATCH_EXT_HEADER); 2062 assert_eq!(packet.extension_header_id(), ExtHeaderId::RoutingHeader); 2063 2064 assert_eq!(packet.payload(), [0x06, 0x03, 0x00, 0xff, 0x00, 0x00, 0x00]); 2065 } 2066 2067 #[test] ext_header_emit()2068 fn ext_header_emit() { 2069 let ext_header = ExtHeaderRepr { 2070 ext_header_id: ExtHeaderId::RoutingHeader, 2071 next_header: NextHeader::Compressed, 2072 length: 6, 2073 }; 2074 2075 let len = ext_header.buffer_len(); 2076 let mut buffer = [0u8; 127]; 2077 let mut packet = ExtHeaderPacket::new_unchecked(&mut buffer[..len]); 2078 ext_header.emit(&mut packet); 2079 2080 assert_eq!(packet.dispatch_field(), DISPATCH_EXT_HEADER); 2081 assert_eq!(packet.next_header(), NextHeader::Compressed); 2082 assert_eq!(packet.extension_header_id(), ExtHeaderId::RoutingHeader); 2083 } 2084 2085 #[test] udp_nhc_fields()2086 fn udp_nhc_fields() { 2087 let bytes = [0xf0, 0x16, 0x2e, 0x22, 0x3d, 0x28, 0xc4]; 2088 2089 let packet = UdpNhcPacket::new_checked(&bytes[..]).unwrap(); 2090 assert_eq!(packet.dispatch_field(), DISPATCH_UDP_HEADER); 2091 assert_eq!(packet.checksum(), Some(0x28c4)); 2092 assert_eq!(packet.src_port(), 5678); 2093 assert_eq!(packet.dst_port(), 8765); 2094 } 2095 2096 #[test] udp_emit()2097 fn udp_emit() { 2098 let udp = UdpNhcRepr(UdpRepr { 2099 src_port: 0xf0b1, 2100 dst_port: 0xf001, 2101 }); 2102 2103 let payload = b"Hello World!"; 2104 2105 let src_addr = ipv6::Address::default(); 2106 let dst_addr = ipv6::Address::default(); 2107 2108 let len = udp.header_len() + payload.len(); 2109 let mut buffer = [0u8; 127]; 2110 let mut packet = UdpNhcPacket::new_unchecked(&mut buffer[..len]); 2111 udp.emit(&mut packet, &src_addr, &dst_addr, payload.len(), |buf| { 2112 buf.copy_from_slice(&payload[..]) 2113 }); 2114 2115 assert_eq!(packet.dispatch_field(), DISPATCH_UDP_HEADER); 2116 assert_eq!(packet.src_port(), 0xf0b1); 2117 assert_eq!(packet.dst_port(), 0xf001); 2118 assert_eq!(packet.payload_mut(), b"Hello World!"); 2119 } 2120 } 2121 } 2122 2123 #[cfg(test)] 2124 mod test { 2125 use super::*; 2126 2127 #[test] sixlowpan_fragment_emit()2128 fn sixlowpan_fragment_emit() { 2129 let repr = frag::Repr::FirstFragment { 2130 size: 0xff, 2131 tag: 0xabcd, 2132 }; 2133 let buffer = [0u8; 4]; 2134 let mut packet = frag::Packet::new_unchecked(buffer); 2135 2136 assert_eq!(repr.buffer_len(), 4); 2137 repr.emit(&mut packet); 2138 2139 assert_eq!(packet.datagram_size(), 0xff); 2140 assert_eq!(packet.datagram_tag(), 0xabcd); 2141 assert_eq!(packet.into_inner(), [0xc0, 0xff, 0xab, 0xcd]); 2142 2143 let repr = frag::Repr::Fragment { 2144 size: 0xff, 2145 tag: 0xabcd, 2146 offset: 0xcc, 2147 }; 2148 let buffer = [0u8; 5]; 2149 let mut packet = frag::Packet::new_unchecked(buffer); 2150 2151 assert_eq!(repr.buffer_len(), 5); 2152 repr.emit(&mut packet); 2153 2154 assert_eq!(packet.datagram_size(), 0xff); 2155 assert_eq!(packet.datagram_tag(), 0xabcd); 2156 assert_eq!(packet.into_inner(), [0xe0, 0xff, 0xab, 0xcd, 0xcc]); 2157 } 2158 2159 #[test] sixlowpan_three_fragments()2160 fn sixlowpan_three_fragments() { 2161 use crate::wire::ieee802154::Frame as Ieee802154Frame; 2162 use crate::wire::ieee802154::Repr as Ieee802154Repr; 2163 use crate::wire::Ieee802154Address; 2164 2165 let key = frag::Key { 2166 ll_src_addr: Ieee802154Address::Extended([50, 147, 130, 47, 40, 8, 62, 217]), 2167 ll_dst_addr: Ieee802154Address::Extended([26, 11, 66, 66, 66, 66, 66, 66]), 2168 datagram_size: 307, 2169 datagram_tag: 63, 2170 }; 2171 2172 let frame1: &[u8] = &[ 2173 0x41, 0xcc, 0x92, 0xef, 0xbe, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x0b, 0x1a, 0xd9, 2174 0x3e, 0x08, 0x28, 0x2f, 0x82, 0x93, 0x32, 0xc1, 0x33, 0x00, 0x3f, 0x6e, 0x33, 0x02, 2175 0x35, 0x3d, 0xf0, 0xd2, 0x5f, 0x1b, 0x39, 0xb4, 0x6b, 0x4c, 0x6f, 0x72, 0x65, 0x6d, 2176 0x20, 0x69, 0x70, 0x73, 0x75, 0x6d, 0x20, 0x64, 0x6f, 0x6c, 0x6f, 0x72, 0x20, 0x73, 2177 0x69, 0x74, 0x20, 0x61, 0x6d, 0x65, 0x74, 0x2c, 0x20, 0x63, 0x6f, 0x6e, 0x73, 0x65, 2178 0x63, 0x74, 0x65, 0x74, 0x75, 0x72, 0x20, 0x61, 0x64, 0x69, 0x70, 0x69, 0x73, 0x63, 2179 0x69, 0x6e, 0x67, 0x20, 0x65, 0x6c, 0x69, 0x74, 0x2e, 0x20, 0x41, 0x6c, 0x69, 0x71, 2180 0x75, 0x61, 0x6d, 0x20, 0x64, 0x75, 0x69, 0x20, 0x6f, 0x64, 0x69, 0x6f, 0x2c, 0x20, 2181 0x69, 0x61, 0x63, 0x75, 0x6c, 0x69, 0x73, 0x20, 0x76, 0x65, 0x6c, 0x20, 0x72, 2182 ]; 2183 2184 let ieee802154_frame = Ieee802154Frame::new_checked(frame1).unwrap(); 2185 let ieee802154_repr = Ieee802154Repr::parse(&ieee802154_frame).unwrap(); 2186 2187 let sixlowpan_frame = 2188 SixlowpanPacket::dispatch(ieee802154_frame.payload().unwrap()).unwrap(); 2189 2190 let frag = if let SixlowpanPacket::FragmentHeader = sixlowpan_frame { 2191 frag::Packet::new_checked(ieee802154_frame.payload().unwrap()).unwrap() 2192 } else { 2193 unreachable!() 2194 }; 2195 2196 assert_eq!(frag.datagram_size(), 307); 2197 assert_eq!(frag.datagram_tag(), 0x003f); 2198 assert_eq!(frag.datagram_offset(), 0); 2199 2200 assert_eq!(frag.get_key(&ieee802154_repr), key); 2201 2202 let frame2: &[u8] = &[ 2203 0x41, 0xcc, 0x93, 0xef, 0xbe, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x0b, 0x1a, 0xd9, 2204 0x3e, 0x08, 0x28, 0x2f, 0x82, 0x93, 0x32, 0xe1, 0x33, 0x00, 0x3f, 0x11, 0x75, 0x74, 2205 0x72, 0x75, 0x6d, 0x20, 0x61, 0x74, 0x2c, 0x20, 0x74, 0x72, 0x69, 0x73, 0x74, 0x69, 2206 0x71, 0x75, 0x65, 0x20, 0x6e, 0x6f, 0x6e, 0x20, 0x6e, 0x75, 0x6e, 0x63, 0x20, 0x65, 2207 0x72, 0x61, 0x74, 0x20, 0x63, 0x75, 0x72, 0x61, 0x65, 0x2e, 0x20, 0x4c, 0x6f, 0x72, 2208 0x65, 0x6d, 0x20, 0x69, 0x70, 0x73, 0x75, 0x6d, 0x20, 0x64, 0x6f, 0x6c, 0x6f, 0x72, 2209 0x20, 0x73, 0x69, 0x74, 0x20, 0x61, 0x6d, 0x65, 0x74, 0x2c, 0x20, 0x63, 0x6f, 0x6e, 2210 0x73, 0x65, 0x63, 0x74, 0x65, 0x74, 0x75, 0x72, 0x20, 0x61, 0x64, 0x69, 0x70, 0x69, 2211 0x73, 0x63, 0x69, 0x6e, 0x67, 0x20, 0x65, 0x6c, 0x69, 0x74, 2212 ]; 2213 2214 let ieee802154_frame = Ieee802154Frame::new_checked(frame2).unwrap(); 2215 let ieee802154_repr = Ieee802154Repr::parse(&ieee802154_frame).unwrap(); 2216 2217 let sixlowpan_frame = 2218 SixlowpanPacket::dispatch(ieee802154_frame.payload().unwrap()).unwrap(); 2219 2220 let frag = if let SixlowpanPacket::FragmentHeader = sixlowpan_frame { 2221 frag::Packet::new_checked(ieee802154_frame.payload().unwrap()).unwrap() 2222 } else { 2223 unreachable!() 2224 }; 2225 2226 assert_eq!(frag.datagram_size(), 307); 2227 assert_eq!(frag.datagram_tag(), 0x003f); 2228 assert_eq!(frag.datagram_offset(), 136 / 8); 2229 2230 assert_eq!(frag.get_key(&ieee802154_repr), key); 2231 2232 let frame3: &[u8] = &[ 2233 0x41, 0xcc, 0x94, 0xef, 0xbe, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x0b, 0x1a, 0xd9, 2234 0x3e, 0x08, 0x28, 0x2f, 0x82, 0x93, 0x32, 0xe1, 0x33, 0x00, 0x3f, 0x1d, 0x2e, 0x20, 2235 0x41, 0x6c, 0x69, 0x71, 0x75, 0x61, 0x6d, 0x20, 0x64, 0x75, 0x69, 0x20, 0x6f, 0x64, 2236 0x69, 0x6f, 0x2c, 0x20, 0x69, 0x61, 0x63, 0x75, 0x6c, 0x69, 0x73, 0x20, 0x76, 0x65, 2237 0x6c, 0x20, 0x72, 0x75, 0x74, 0x72, 0x75, 0x6d, 0x20, 0x61, 0x74, 0x2c, 0x20, 0x74, 2238 0x72, 0x69, 0x73, 0x74, 0x69, 0x71, 0x75, 0x65, 0x20, 0x6e, 0x6f, 0x6e, 0x20, 0x6e, 2239 0x75, 0x6e, 0x63, 0x20, 0x65, 0x72, 0x61, 0x74, 0x20, 0x63, 0x75, 0x72, 0x61, 0x65, 2240 0x2e, 0x20, 0x0a, 2241 ]; 2242 2243 let ieee802154_frame = Ieee802154Frame::new_checked(frame3).unwrap(); 2244 let ieee802154_repr = Ieee802154Repr::parse(&ieee802154_frame).unwrap(); 2245 2246 let sixlowpan_frame = 2247 SixlowpanPacket::dispatch(ieee802154_frame.payload().unwrap()).unwrap(); 2248 2249 let frag = if let SixlowpanPacket::FragmentHeader = sixlowpan_frame { 2250 frag::Packet::new_checked(ieee802154_frame.payload().unwrap()).unwrap() 2251 } else { 2252 unreachable!() 2253 }; 2254 2255 assert_eq!(frag.datagram_size(), 307); 2256 assert_eq!(frag.datagram_tag(), 0x003f); 2257 assert_eq!(frag.datagram_offset(), 232 / 8); 2258 2259 assert_eq!(frag.get_key(&ieee802154_repr), key); 2260 } 2261 } 2262