use crate::{ByteOrder, Decoder, Encoder, Encoding, Error, Result};
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
pub enum Extensibility {
Final,
Appendable,
Mutable,
}
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
pub enum XcdrFormat {
PlainCdr,
PlCdr,
PlainCdr2,
DelimitedCdr2,
PlCdr2,
}
impl XcdrFormat {
pub fn new(encoding: Encoding, kind: Extensibility) -> Option<XcdrFormat> {
use Extensibility::*;
Some(match (encoding, kind) {
(Encoding::Xcdr1, Final | Appendable) => XcdrFormat::PlainCdr,
(Encoding::Xcdr1, Mutable) => XcdrFormat::PlCdr,
(Encoding::Xcdr2, Final) => XcdrFormat::PlainCdr2,
(Encoding::Xcdr2, Appendable) => XcdrFormat::DelimitedCdr2,
(Encoding::Xcdr2, Mutable) => XcdrFormat::PlCdr2,
_ => return None,
})
}
pub fn encoding(self) -> Encoding {
match self {
XcdrFormat::PlainCdr | XcdrFormat::PlCdr => Encoding::Xcdr1,
_ => Encoding::Xcdr2,
}
}
}
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
pub struct EncapsulationId {
pub format: XcdrFormat,
pub order: ByteOrder,
}
impl EncapsulationId {
pub fn to_bytes(self) -> [u8; 2] {
let base = match self.format {
XcdrFormat::PlainCdr => 0x00,
XcdrFormat::PlCdr => 0x02,
XcdrFormat::PlainCdr2 => 0x10,
XcdrFormat::PlCdr2 => 0x12,
XcdrFormat::DelimitedCdr2 => 0x14,
};
[0x00, base | self.order.flag()]
}
pub fn from_bytes(bytes: [u8; 2]) -> Result<EncapsulationId> {
let [0x00, id] = bytes else {
return Err(Error::InvalidXcdr("unknown encapsulation identifier"));
};
let format = match id & !1 {
0x00 => XcdrFormat::PlainCdr,
0x02 => XcdrFormat::PlCdr,
0x10 | 0x06 => XcdrFormat::PlainCdr2,
0x12 | 0x0a => XcdrFormat::PlCdr2,
0x14 | 0x08 => XcdrFormat::DelimitedCdr2,
_ => return Err(Error::InvalidXcdr("unknown encapsulation identifier")),
};
let order = if id & 1 == 0 {
ByteOrder::Big
} else {
ByteOrder::Little
};
Ok(EncapsulationId { format, order })
}
}
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
pub struct PayloadHeader {
pub id: EncapsulationId,
pub options: [u8; 2],
}
pub const PID_EXTENDED: u16 = 0x7F01;
pub const PID_LIST_END: u16 = 0x7F02;
pub const PID_IGNORE: u16 = 0x3F03;
pub const PID_SENTINEL: u16 = 0x0001;
pub const FLAG_IMPL_EXTENSION: u16 = 0x8000;
pub const FLAG_MUST_UNDERSTAND: u16 = 0x4000;
pub const MAX_MEMBER_ID: u32 = 0x0FFF_FFFF;
pub const MAX_SHORT_PID: u32 = 0x3F00;
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
pub enum ListEnd {
Xtypes,
Rtps,
}
impl ListEnd {
fn pid(self) -> u16 {
match self {
ListEnd::Xtypes => PID_LIST_END,
ListEnd::Rtps => PID_SENTINEL,
}
}
}
#[derive(Debug)]
#[must_use = "a DHEADER is written by end_dheader"]
pub struct DHeaderMark {
at: usize,
start: usize,
}
#[derive(Debug)]
#[must_use = "a member header is written by end_member"]
pub struct MemberMark {
at: usize,
start: usize,
header: u32,
}
#[derive(Debug)]
#[must_use = "a parameter header is written by end_parameter"]
pub struct ParameterMark {
at: usize,
start: usize,
id: u32,
must_understand: bool,
extended: bool,
origin: usize,
}
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[must_use = "end_dheader skips what was not read"]
pub struct Delimited {
end: usize,
}
impl Delimited {
pub fn has_more(&self, dec: &Decoder<'_>) -> bool {
dec.pos < self.end
}
pub fn remaining(&self, dec: &Decoder<'_>) -> usize {
self.end.saturating_sub(dec.pos)
}
}
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[must_use = "end_member skips what was not read"]
pub struct MemberHeader {
pub id: u32,
pub must_understand: bool,
pub len: usize,
end: usize,
}
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[must_use = "end_parameter skips what was not read and restores the alignment origin"]
pub struct Parameter {
pub id: u32,
pub must_understand: bool,
pub impl_extension: bool,
pub len: usize,
end: usize,
origin: usize,
}
impl Encoder {
pub fn xcdr_payload(format: XcdrFormat, order: ByteOrder) -> Encoder {
let mut enc = Encoder::new(order, format.encoding());
enc.write_octets(&EncapsulationId { format, order }.to_bytes());
enc.write_octets(&[0, 0]);
enc.origin = 4;
enc
}
pub fn finish_xcdr_payload(mut self) -> Vec<u8> {
let pad = (4 - self.len() % 4) % 4;
self.buf.resize(self.buf.len() + pad, 0);
if let Some(options) = self.buf.get_mut(3) {
*options = (*options & !3) | pad as u8;
}
self.into_bytes()
}
fn patch_u32(&mut self, at: usize, value: u32) {
let bytes = match self.order {
ByteOrder::Big => value.to_be_bytes(),
ByteOrder::Little => value.to_le_bytes(),
};
self.buf[at..at + 4].copy_from_slice(&bytes);
}
fn patch_u16(&mut self, at: usize, value: u16) {
let bytes = match self.order {
ByteOrder::Big => value.to_be_bytes(),
ByteOrder::Little => value.to_le_bytes(),
};
self.buf[at..at + 2].copy_from_slice(&bytes);
}
fn u16_bytes(&self, value: u16) -> [u8; 2] {
match self.order {
ByteOrder::Big => value.to_be_bytes(),
ByteOrder::Little => value.to_le_bytes(),
}
}
fn u32_bytes(&self, value: u32) -> [u8; 4] {
match self.order {
ByteOrder::Big => value.to_be_bytes(),
ByteOrder::Little => value.to_le_bytes(),
}
}
pub fn begin_dheader(&mut self) -> DHeaderMark {
self.align(4);
let at = self.buf.len();
self.write_u32(0);
DHeaderMark {
at,
start: self.len(),
}
}
pub fn end_dheader(&mut self, mark: DHeaderMark) -> Result<()> {
let size = self.len() - mark.start;
let size = u32::try_from(size).map_err(|_| Error::LengthOverflow(size))?;
self.patch_u32(mark.at, size);
Ok(())
}
pub fn begin_member(&mut self, id: u32, must_understand: bool) -> Result<MemberMark> {
if self.encoding() != Encoding::Xcdr2 {
return Err(Error::InvalidXcdr("EMHEADER outside XCDR2"));
}
if id > MAX_MEMBER_ID {
return Err(Error::InvalidXcdr("member ID larger than 28 bits"));
}
self.align(4);
let at = self.buf.len();
self.write_u32(0);
self.write_u32(0);
Ok(MemberMark {
at,
start: self.len(),
header: (u32::from(must_understand) << 31) | id,
})
}
pub fn end_member(&mut self, mark: MemberMark) -> Result<()> {
let size = self.len() - mark.start;
let lc = match size {
1 => Some(0),
2 => Some(1),
4 => Some(2),
8 => Some(3),
_ => None,
};
match lc {
Some(lc) => {
self.buf.drain(mark.at + 4..mark.at + 8);
self.patch_u32(mark.at, mark.header | (lc << 28));
}
None => {
let size = u32::try_from(size).map_err(|_| Error::LengthOverflow(size))?;
self.patch_u32(mark.at, mark.header | (4 << 28));
self.patch_u32(mark.at + 4, size);
}
}
Ok(())
}
pub fn begin_parameter(&mut self, id: u32, must_understand: bool) -> Result<ParameterMark> {
if id > MAX_MEMBER_ID {
return Err(Error::InvalidXcdr("member ID larger than 28 bits"));
}
self.align(4);
let at = self.buf.len();
let extended = id > MAX_SHORT_PID;
if extended {
self.write_u16(PID_EXTENDED);
self.write_u16(8);
self.write_u32(extended_header(id, must_understand));
self.write_u32(0);
} else {
let flag = if must_understand {
FLAG_MUST_UNDERSTAND
} else {
0
};
self.write_u16(flag | id as u16);
self.write_u16(0);
}
let mark = ParameterMark {
at,
start: self.len(),
id,
must_understand,
extended,
origin: self.origin,
};
self.origin = self.offset();
Ok(mark)
}
pub fn end_parameter(&mut self, mark: ParameterMark) -> Result<()> {
let size = self.len() - mark.start;
let size32 = u32::try_from(size).map_err(|_| Error::LengthOverflow(size))?;
if mark.extended {
self.patch_u32(mark.at + 8, size32);
} else if let Ok(short) = u16::try_from(size) {
self.patch_u16(mark.at + 2, short);
} else {
let mut header = Vec::with_capacity(12);
header.extend_from_slice(&self.u16_bytes(PID_EXTENDED));
header.extend_from_slice(&self.u16_bytes(8));
header
.extend_from_slice(&self.u32_bytes(extended_header(mark.id, mark.must_understand)));
header.extend_from_slice(&self.u32_bytes(size32));
self.buf.splice(mark.at..mark.at + 4, header);
}
self.origin = mark.origin;
Ok(())
}
pub fn end_parameter_list(&mut self, end: ListEnd) {
self.align(4);
self.write_u16(end.pid());
self.write_u16(0);
}
}
fn extended_header(id: u32, must_understand: bool) -> u32 {
(u32::from(must_understand) << 30) | id
}
impl<'a> Decoder<'a> {
pub fn xcdr_payload(buf: &'a [u8]) -> Result<(Decoder<'a>, PayloadHeader)> {
let [a, b, o1, o2, ..] = *buf else {
return Err(Error::UnexpectedEof {
needed: 4,
remaining: buf.len(),
});
};
let id = EncapsulationId::from_bytes([a, b])?;
let pad = usize::from(o2 & 3);
let end = buf
.len()
.checked_sub(pad)
.filter(|&end| end >= 4)
.ok_or(Error::InvalidXcdr("payload padding longer than the data"))?;
let mut dec = Decoder::new(&buf[..end], id.order, id.format.encoding());
dec.pos = 4;
dec.origin = 4;
Ok((
dec,
PayloadHeader {
id,
options: [o1, o2],
},
))
}
pub fn read_dheader(&mut self) -> Result<Delimited> {
let size = self.read_u32()? as usize;
if size > self.remaining() {
return Err(Error::InvalidXcdr("DHEADER longer than the data"));
}
Ok(Delimited {
end: self.pos + size,
})
}
pub fn end_dheader(&mut self, delimited: Delimited) -> Result<()> {
if self.pos > delimited.end {
return Err(Error::InvalidXcdr("object longer than its DHEADER"));
}
self.pos = delimited.end;
Ok(())
}
pub fn read_member_header(&mut self) -> Result<MemberHeader> {
let header = self.read_u32()?;
let must_understand = header >> 31 == 1;
let lc = (header >> 28) & 7;
let id = header & MAX_MEMBER_ID;
let len = match lc {
0 => 1,
1 => 2,
2 => 4,
3 => 8,
_ => {
let next = self.read_u32()? as usize;
if lc == 4 {
next
} else {
self.pos -= 4;
let unit = match lc {
5 => 1,
6 => 4,
_ => 8,
};
next.checked_mul(unit)
.and_then(|n| n.checked_add(4))
.ok_or(Error::InvalidXcdr("member longer than the data"))?
}
}
};
if len > self.remaining() {
return Err(Error::InvalidXcdr("member longer than the data"));
}
Ok(MemberHeader {
id,
must_understand,
len,
end: self.pos + len,
})
}
pub fn end_member(&mut self, header: MemberHeader) -> Result<()> {
if self.pos > header.end {
return Err(Error::InvalidXcdr("member longer than its EMHEADER"));
}
self.pos = header.end;
Ok(())
}
pub fn read_parameter_header(&mut self) -> Result<Parameter> {
self.align(4)?;
let pid = self.read_u16()?;
let short_len = self.read_u16()?;
let (id, must_understand, impl_extension, len) =
if pid & !(FLAG_MUST_UNDERSTAND | FLAG_IMPL_EXTENSION) == PID_EXTENDED & 0x3FFF
&& pid & FLAG_IMPL_EXTENSION == 0
{
if short_len != 8 {
return Err(Error::InvalidXcdr("PID_EXTENDED length is not 8"));
}
let header = self.read_u32()?;
let len = self.read_u32()? as usize;
(
header & MAX_MEMBER_ID,
header & (1 << 30) != 0,
header & (1 << 31) != 0,
len,
)
} else {
(
u32::from(pid & 0x3FFF),
pid & FLAG_MUST_UNDERSTAND != 0,
pid & FLAG_IMPL_EXTENSION != 0,
usize::from(short_len),
)
};
if len > self.remaining() {
return Err(Error::InvalidXcdr("parameter longer than the data"));
}
let origin = self.origin;
self.origin = self.position();
Ok(Parameter {
id,
must_understand,
impl_extension,
len,
end: self.pos + len,
origin,
})
}
pub fn read_parameter(&mut self, list_end: ListEnd) -> Result<Option<Parameter>> {
loop {
self.align(4)?;
if let [a, b, ..] = *self.remaining_bytes() {
let pid = match self.order {
ByteOrder::Big => u16::from_be_bytes([a, b]),
ByteOrder::Little => u16::from_le_bytes([a, b]),
};
if pid & !FLAG_MUST_UNDERSTAND == PID_LIST_END & 0x3FFF
|| list_end == ListEnd::Rtps && pid == PID_SENTINEL
{
self.skip(4)?;
return Ok(None);
}
}
let param = self.read_parameter_header()?;
if !param.impl_extension && param.id == u32::from(PID_IGNORE) {
self.end_parameter(param)?;
continue;
}
return Ok(Some(param));
}
}
pub fn end_parameter(&mut self, param: Parameter) -> Result<()> {
if self.pos > param.end {
return Err(Error::InvalidXcdr("member longer than its parameter"));
}
self.pos = param.end;
self.origin = param.origin;
Ok(())
}
}