use crate::{
dynamic_value::{DynamicFieldSchema, DynamicTypeSchema},
xtypes::{FindScope, MatchedEndpoint, TopicDescriptor, TypeInfo},
DataReader, DataWriter, DdsEntity, DdsError, DdsResult, DomainParticipant,
};
use cyclonedds_rust_sys::*;
use std::ffi::CStr;
#[derive(Debug, Clone)]
pub struct DiscoveredType {
pub type_name: String,
pub type_schema: DynamicTypeSchema,
pub topic_descriptor: TopicDescriptor,
}
impl DiscoveredType {
pub fn create_topic(
&self,
participant: &DomainParticipant,
topic_name: &str,
) -> DdsResult<crate::UntypedTopic> {
self.topic_descriptor.create_topic(participant, topic_name)
}
pub fn create_topic_with_qos(
&self,
participant: &DomainParticipant,
topic_name: &str,
qos: &crate::Qos,
) -> DdsResult<crate::UntypedTopic> {
self.topic_descriptor
.create_topic_with_qos(participant, topic_name, qos)
}
}
pub fn discover_type_from_publication(
participant: &DomainParticipant,
reader: &DataReader<impl crate::DdsType>,
publication_handle: dds_instance_handle_t,
timeout: dds_duration_t,
) -> DdsResult<DiscoveredType> {
let endpoint = MatchedEndpoint::from_publication(reader.entity(), publication_handle)?;
discover_type_from_endpoint(participant, &endpoint, timeout)
}
pub fn discover_type_from_subscription(
participant: &DomainParticipant,
writer: &DataWriter<impl crate::DdsType>,
subscription_handle: dds_instance_handle_t,
timeout: dds_duration_t,
) -> DdsResult<DiscoveredType> {
let endpoint = MatchedEndpoint::from_subscription(writer.entity(), subscription_handle)?;
discover_type_from_endpoint(participant, &endpoint, timeout)
}
pub fn discover_type_from_endpoint(
participant: &DomainParticipant,
endpoint: &MatchedEndpoint,
timeout: dds_duration_t,
) -> DdsResult<DiscoveredType> {
let type_info = endpoint.type_info()?;
let type_name = endpoint.type_name();
let descriptor = type_info.create_topic_descriptor(participant, FindScope::Global, timeout)?;
let schema = type_schema_from_descriptor(&descriptor, &type_name)?;
Ok(DiscoveredType {
type_name,
type_schema: schema,
topic_descriptor: descriptor,
})
}
#[cfg_attr(feature = "tracing", tracing::instrument(skip(participant, type_info)))]
pub fn discover_type_from_type_info(
participant: &DomainParticipant,
type_info: &TypeInfo,
type_name: &str,
timeout: dds_duration_t,
) -> DdsResult<DiscoveredType> {
let descriptor = type_info.create_topic_descriptor(participant, FindScope::Global, timeout)?;
let schema = type_schema_from_descriptor(&descriptor, type_name)?;
Ok(DiscoveredType {
type_name: type_name.to_string(),
type_schema: schema,
topic_descriptor: descriptor,
})
}
pub fn discover_all_publication_types(
participant: &DomainParticipant,
reader: &DataReader<impl crate::DdsType>,
timeout: dds_duration_t,
) -> DdsResult<Vec<DiscoveredType>> {
let endpoints = reader.matched_publication_endpoints()?;
let mut results = Vec::with_capacity(endpoints.len());
for endpoint in &endpoints {
match discover_type_from_endpoint(participant, endpoint, timeout) {
Ok(dt) => results.push(dt),
Err(_) => continue, }
}
Ok(results)
}
pub fn discover_all_subscription_types(
participant: &DomainParticipant,
writer: &DataWriter<impl crate::DdsType>,
timeout: dds_duration_t,
) -> DdsResult<Vec<DiscoveredType>> {
let endpoints = writer.matched_subscription_endpoints()?;
let mut results = Vec::with_capacity(endpoints.len());
for endpoint in &endpoints {
match discover_type_from_endpoint(participant, endpoint, timeout) {
Ok(dt) => results.push(dt),
Err(_) => continue,
}
}
Ok(results)
}
fn type_schema_from_descriptor(
descriptor: &TopicDescriptor,
type_name: &str,
) -> DdsResult<DynamicTypeSchema> {
let ops = descriptor.ops();
let flagset = descriptor.flagset();
let extensibility = if flagset & crate::OP_FLAG_EXT != 0 {
let xml = descriptor.metadata_xml();
if xml.contains("xcdr2") || xml.contains("appendable") || xml.contains("@appendable") {
Some(crate::DynamicTypeExtensibility::Appendable)
} else if xml.contains("mutable") || xml.contains("@mutable") {
Some(crate::DynamicTypeExtensibility::Mutable)
} else {
Some(crate::DynamicTypeExtensibility::Appendable)
}
} else {
Some(crate::DynamicTypeExtensibility::Final)
};
let fields = parse_ops_to_fields(ops, descriptor.size())?;
Ok(DynamicTypeSchema::Struct {
name: type_name.to_string(),
base: None,
fields,
extensibility,
autoid: None,
nested: false,
})
}
fn parse_ops_to_fields(ops: &[u32], _struct_size: u32) -> DdsResult<Vec<DynamicFieldSchema>> {
use crate::dynamic_type::DynamicPrimitiveKind;
use crate::dynamic_value::DynamicTypeSchema as Sch;
use crate::topic::*;
let mut fields = Vec::new();
let mut i = 0usize;
let mut field_index = 0u32;
while i < ops.len() {
let op = ops[i];
let opcode = op & DDS_OP_MASK;
match opcode {
OP_ADR => {
let primary = op & DDS_OP_TYPE_MASK;
let subtype = op & DDS_OP_SUBTYPE_MASK_CONST;
let field_schema = match primary {
TYPE_1BY => {
if op & OP_FLAG_SGN != 0 {
Sch::Primitive(DynamicPrimitiveKind::Int8)
} else {
Sch::Primitive(DynamicPrimitiveKind::UInt8)
}
}
TYPE_2BY => {
if op & OP_FLAG_SGN != 0 {
Sch::Primitive(DynamicPrimitiveKind::Int16)
} else {
Sch::Primitive(DynamicPrimitiveKind::UInt16)
}
}
TYPE_4BY => {
if subtype == SUBTYPE_ENU {
Sch::Enum {
name: String::new(),
literals: Vec::new(),
bit_bound: None,
}
} else if op & OP_FLAG_FP != 0 {
Sch::Primitive(DynamicPrimitiveKind::Float32)
} else if op & OP_FLAG_SGN != 0 {
Sch::Primitive(DynamicPrimitiveKind::Int32)
} else {
Sch::Primitive(DynamicPrimitiveKind::UInt32)
}
}
TYPE_8BY => {
if op & OP_FLAG_FP != 0 {
Sch::Primitive(DynamicPrimitiveKind::Float64)
} else if op & OP_FLAG_SGN != 0 {
Sch::Primitive(DynamicPrimitiveKind::Int64)
} else {
Sch::Primitive(DynamicPrimitiveKind::UInt64)
}
}
TYPE_STR => Sch::String { bound: None },
TYPE_BST => {
let capacity = *ops.get(i + 2).ok_or_else(|| {
DdsError::BadParameter(
"bounded string descriptor is missing its capacity".into(),
)
})?;
let bound = capacity.checked_sub(1).ok_or_else(|| {
DdsError::BadParameter(
"bounded string descriptor capacity must include a terminator"
.into(),
)
})?;
Sch::String { bound: Some(bound) }
}
TYPE_SEQ => Sch::Sequence {
name: String::new(),
bound: None,
element: Box::new(Sch::Primitive(DynamicPrimitiveKind::Int8)),
},
TYPE_ARR => Sch::Array {
name: String::new(),
bounds: vec![0],
element: Box::new(Sch::Primitive(DynamicPrimitiveKind::Int8)),
},
TYPE_EXT => Sch::Struct {
name: String::new(),
base: None,
fields: Vec::new(),
extensibility: None,
autoid: None,
nested: true,
},
_ => Sch::Primitive(DynamicPrimitiveKind::Int8),
};
let is_key = op & OP_FLAG_KEY != 0;
let is_optional = op & OP_FLAG_OPT != 0;
fields.push(DynamicFieldSchema {
name: format!("field_{}", field_index),
member_id: field_index,
value: field_schema,
optional: is_optional,
key: is_key,
external: false,
must_understand: false,
hash_id_name: None,
});
field_index += 1;
i += crate::xtypes::adr_step(ops, i);
}
OP_RTS | OP_DLC | OP_JEQ4 | OP_KOF | OP_MID => {
i += 1;
}
_ => {
i += 1;
}
}
}
Ok(fields)
}
struct DynamicCdrDescriptor {
inner: dds_cdrstream_desc,
_key_names: Vec<std::ffi::CString>,
_keys: Vec<dds_key_descriptor>,
}
impl DynamicCdrDescriptor {
fn new(descriptor: &TopicDescriptor) -> DdsResult<Self> {
let key_defs = descriptor.key_descriptors();
let key_names: Vec<std::ffi::CString> = key_defs
.iter()
.map(|key| {
std::ffi::CString::new(key.name.as_str())
.map_err(|_| DdsError::BadParameter("key name contains null".into()))
})
.collect::<DdsResult<_>>()?;
let keys: Vec<dds_key_descriptor> = key_defs
.iter()
.enumerate()
.map(|(index, key)| dds_key_descriptor {
m_name: key_names[index].as_ptr(),
m_offset: key.offset,
m_idx: key.index,
})
.collect();
let key_count = u32::try_from(keys.len())
.map_err(|_| DdsError::BadParameter("too many dynamic type keys".into()))?;
let mut inner = dds_cdrstream_desc::default();
unsafe {
dds_cdrstream_desc_init(
&mut inner,
&dds_cdrstream_default_allocator,
descriptor.size(),
descriptor.align(),
descriptor.flagset(),
descriptor.ops().as_ptr(),
if keys.is_empty() {
std::ptr::null()
} else {
keys.as_ptr()
},
key_count,
);
}
Ok(Self {
inner,
_key_names: key_names,
_keys: keys,
})
}
fn as_ptr(&self) -> *const dds_cdrstream_desc {
&self.inner
}
}
impl Drop for DynamicCdrDescriptor {
fn drop(&mut self) {
unsafe {
dds_cdrstream_desc_fini(&mut self.inner, &dds_cdrstream_default_allocator);
}
}
}
struct DynamicNativeSample<'a> {
ptr: std::ptr::NonNull<u8>,
layout: std::alloc::Layout,
ops: &'a [u32],
}
impl<'a> DynamicNativeSample<'a> {
fn new(descriptor: &'a TopicDescriptor) -> DdsResult<Self> {
let size = usize::try_from(descriptor.size())
.map_err(|_| DdsError::BadParameter("dynamic type size is too large".into()))?;
if size == 0 {
return Err(DdsError::BadParameter(
"dynamic type size must be non-zero".into(),
));
}
let align = usize::try_from(descriptor.align().max(1))
.map_err(|_| DdsError::BadParameter("dynamic type alignment is too large".into()))?;
let layout = std::alloc::Layout::from_size_align(size, align)
.map_err(|_| DdsError::BadParameter("invalid type layout for dynamic data".into()))?;
let ptr = unsafe { std::alloc::alloc_zeroed(layout) };
let ptr = std::ptr::NonNull::new(ptr).ok_or(DdsError::OutOfMemory)?;
Ok(Self {
ptr,
layout,
ops: descriptor.ops(),
})
}
fn as_ptr(&self) -> *const std::ffi::c_void {
self.ptr.as_ptr().cast()
}
fn as_mut_ptr(&mut self) -> *mut std::ffi::c_void {
self.ptr.as_ptr().cast()
}
fn as_mut_bytes(&mut self) -> *mut u8 {
self.ptr.as_ptr()
}
}
impl Drop for DynamicNativeSample<'_> {
fn drop(&mut self) {
unsafe {
dds_stream_free_sample(
self.ptr.as_ptr().cast(),
&dds_cdrstream_default_allocator,
self.ops.as_ptr(),
);
std::alloc::dealloc(self.ptr.as_ptr(), self.layout);
}
}
}
struct DynamicInputStream<'a> {
inner: dds_istream_t,
_data: std::marker::PhantomData<&'a [u8]>,
}
impl<'a> DynamicInputStream<'a> {
fn new(data: &'a [u8]) -> DdsResult<Self> {
let size = u32::try_from(data.len())
.map_err(|_| DdsError::BadParameter("CDR input is too large".into()))?;
let mut inner = dds_istream_t::default();
unsafe {
dds_istream_init(&mut inner, size, data.as_ptr().cast(), 1);
}
Ok(Self {
inner,
_data: std::marker::PhantomData,
})
}
fn as_mut_ptr(&mut self) -> *mut dds_istream_t {
&mut self.inner
}
}
impl Drop for DynamicInputStream<'_> {
fn drop(&mut self) {
unsafe { dds_istream_fini(&mut self.inner) };
}
}
struct DynamicOutputStream {
inner: dds_ostream_t,
}
impl DynamicOutputStream {
fn new() -> Self {
let mut inner = dds_ostream_t::default();
unsafe { dds_ostream_init(&mut inner, &dds_cdrstream_default_allocator, 0, 1) };
Self { inner }
}
fn as_mut_ptr(&mut self) -> *mut dds_ostream_t {
&mut self.inner
}
fn to_vec(&self) -> DdsResult<Vec<u8>> {
let len = usize::try_from(self.inner.m_index)
.map_err(|_| DdsError::BadParameter("serialized CDR size is too large".into()))?;
if len == 0 {
return Ok(Vec::new());
}
let buffer = std::ptr::NonNull::new(self.inner.m_buffer)
.ok_or_else(|| DdsError::Other("CDR writer returned a null buffer".into()))?;
let mut bytes = vec![0u8; len];
unsafe { std::ptr::copy_nonoverlapping(buffer.as_ptr(), bytes.as_mut_ptr(), len) };
Ok(bytes)
}
}
impl Drop for DynamicOutputStream {
fn drop(&mut self) {
unsafe { dds_ostream_fini(&mut self.inner, &dds_cdrstream_default_allocator) };
}
}
fn normalize_dynamic_cdr(cdr_data: &[u8], descriptor: &DynamicCdrDescriptor) -> DdsResult<Vec<u8>> {
let size = u32::try_from(cdr_data.len())
.map_err(|_| DdsError::BadParameter("CDR input is too large".into()))?;
let mut normalized = cdr_data.to_vec();
let mut actual_size = 0u32;
let valid = unsafe {
dds_stream_normalize(
normalized.as_mut_ptr().cast(),
size,
false,
1,
descriptor.as_ptr(),
false,
&mut actual_size,
)
};
if !valid {
return Err(DdsError::BadParameter(
"malformed CDR data for this dynamic type".into(),
));
}
let actual_size = usize::try_from(actual_size)
.map_err(|_| DdsError::BadParameter("normalized CDR size is too large".into()))?;
if actual_size > normalized.len() {
return Err(DdsError::BadParameter(
"normalizer returned an invalid CDR size".into(),
));
}
normalized.truncate(actual_size);
Ok(normalized)
}
pub fn dynamic_data_to_cdr(
data: &crate::DynamicData,
descriptor: &TopicDescriptor,
) -> DdsResult<Vec<u8>> {
validate_data_for_native(data, descriptor.ops())?;
let stream_descriptor = DynamicCdrDescriptor::new(descriptor)?;
let mut native = DynamicNativeSample::new(descriptor)?;
write_data_to_native(data, native.as_mut_bytes(), descriptor.ops())?;
let mut output = DynamicOutputStream::new();
let wrote = unsafe {
dds_stream_write_sample(
output.as_mut_ptr(),
&dds_cdrstream_default_allocator,
native.as_ptr(),
stream_descriptor.as_ptr(),
)
};
if !wrote {
return Err(DdsError::Unsupported(
"CDR serialization of dynamic data failed".into(),
));
}
output.to_vec()
}
pub fn cdr_to_dynamic_data(
cdr_data: &[u8],
schema: &crate::DynamicTypeSchema,
descriptor: &TopicDescriptor,
) -> DdsResult<crate::DynamicData> {
let stream_descriptor = DynamicCdrDescriptor::new(descriptor)?;
let normalized = normalize_dynamic_cdr(cdr_data, &stream_descriptor)?;
let mut input = DynamicInputStream::new(&normalized)?;
let mut native = DynamicNativeSample::new(descriptor)?;
unsafe {
dds_stream_read_sample(
input.as_mut_ptr(),
native.as_mut_ptr(),
&dds_cdrstream_default_allocator,
stream_descriptor.as_ptr(),
);
}
let value = read_value_from_native(native.as_mut_bytes(), schema, descriptor.ops(), 0)?;
crate::DynamicData::from_value(schema, value)
}
pub(crate) fn write_data_to_native(
data: &crate::DynamicData,
base: *mut u8,
ops: &[u32],
) -> DdsResult<()> {
use crate::dynamic_value::DynamicValue as DV;
use crate::topic::*;
validate_data_for_native(data, ops)?;
let struct_fields = match data.value() {
DV::Struct(fields) => fields,
_ => {
return Err(DdsError::BadParameter(
"dynamic native value must be a struct".into(),
))
}
};
let schema_fields = match data.schema() {
crate::DynamicTypeSchema::Struct { fields, .. } => fields,
_ => {
return Err(DdsError::BadParameter(
"dynamic native schema must be a struct".into(),
))
}
};
let mut i = 0usize;
let mut field_index = 0usize;
while i < ops.len() {
let op = ops[i];
let opcode = op & DDS_OP_MASK;
match opcode {
OP_ADR => {
let primary = op & DDS_OP_TYPE_MASK;
let offset = if i + 1 < ops.len() {
ops[i + 1] as usize
} else {
0
};
let dst = unsafe { base.add(offset) };
let field_schema = schema_fields.get(field_index).ok_or_else(|| {
DdsError::BadParameter(
"native descriptor has more fields than the dynamic schema".into(),
)
})?;
let field_val = struct_fields.get(&field_schema.name).ok_or_else(|| {
DdsError::BadParameter(format!(
"missing required dynamic field '{}'",
field_schema.name
))
})?;
let capacity = if primary == TYPE_BST {
Some(bounded_string_capacity(ops, i)?)
} else {
None
};
write_primitive_to_native(dst, field_val, primary, capacity)?;
field_index += 1;
i += crate::xtypes::adr_step(ops, i);
}
OP_RTS | OP_DLC => {
i += 1;
}
_ => {
i += 1;
}
}
}
if field_index != schema_fields.len() {
return Err(DdsError::BadParameter(
"dynamic schema has more fields than the native descriptor".into(),
));
}
Ok(())
}
pub(crate) fn validate_data_for_native(data: &crate::DynamicData, ops: &[u32]) -> DdsResult<()> {
use crate::dynamic_value::DynamicValue as DV;
use crate::topic::*;
data.validate()?;
let fields = match data.value() {
DV::Struct(fields) => fields,
_ => {
return Err(DdsError::BadParameter(
"dynamic native value must be a struct".into(),
))
}
};
let schema_fields = match data.schema() {
crate::DynamicTypeSchema::Struct { fields, .. } => fields,
_ => {
return Err(DdsError::BadParameter(
"dynamic native schema must be a struct".into(),
))
}
};
let mut names = std::collections::BTreeSet::new();
let mut member_ids = std::collections::BTreeSet::new();
for field in schema_fields {
if !names.insert(field.name.as_str()) {
return Err(DdsError::BadParameter(format!(
"duplicate dynamic field name '{}'",
field.name
)));
}
if !member_ids.insert(field.member_id) {
return Err(DdsError::BadParameter(format!(
"duplicate dynamic member id {}",
field.member_id
)));
}
}
if fields.len() != schema_fields.len() {
return Err(DdsError::BadParameter(format!(
"dynamic value has {} fields but schema requires {}",
fields.len(),
schema_fields.len()
)));
}
let mut i = 0usize;
let mut field_index = 0usize;
while i < ops.len() {
let op = ops[i];
match op & DDS_OP_MASK {
OP_ADR => {
let primary = op & DDS_OP_TYPE_MASK;
let field_schema = schema_fields.get(field_index).ok_or_else(|| {
DdsError::BadParameter(
"native descriptor has more fields than the dynamic schema".into(),
)
})?;
let value = fields.get(&field_schema.name).ok_or_else(|| {
DdsError::BadParameter(format!(
"missing required dynamic field '{}'",
field_schema.name
))
})?;
match (primary, value) {
(TYPE_STR, DV::String(string)) => {
std::ffi::CString::new(string.as_str()).map_err(|_| {
DdsError::BadParameter("string contains an interior null".into())
})?;
}
(TYPE_BST, DV::String(string)) => {
let capacity = bounded_string_capacity(ops, i)?;
let bound = capacity.checked_sub(1).ok_or_else(|| {
DdsError::BadParameter(
"bounded string capacity must include a terminator".into(),
)
})?;
if string.len() > bound {
return Err(DdsError::BadParameter(format!(
"string length {} exceeds bound {}",
string.len(),
bound
)));
}
if string.as_bytes().contains(&0) {
return Err(DdsError::BadParameter(
"string contains an interior null".into(),
));
}
}
(TYPE_STR | TYPE_BST, _) => {
return Err(DdsError::BadParameter(format!(
"dynamic field '{}' is not a string",
field_schema.name
)))
}
_ => {}
}
field_index += 1;
i += crate::xtypes::adr_step(ops, i);
}
OP_RTS | OP_DLC => i += 1,
_ => i += 1,
}
}
if field_index != schema_fields.len() {
return Err(DdsError::BadParameter(
"dynamic schema has more fields than the native descriptor".into(),
));
}
Ok(())
}
fn bounded_string_capacity(ops: &[u32], op_index: usize) -> DdsResult<usize> {
let capacity = *ops.get(op_index + 2).ok_or_else(|| {
DdsError::BadParameter("bounded string descriptor is missing its capacity".into())
})?;
let capacity = usize::try_from(capacity)
.map_err(|_| DdsError::BadParameter("bounded string capacity is too large".into()))?;
if capacity == 0 {
return Err(DdsError::BadParameter(
"bounded string capacity must include a terminator".into(),
));
}
Ok(capacity)
}
fn write_primitive_to_native(
dst: *mut u8,
val: &crate::dynamic_value::DynamicValue,
primary_type: u32,
bounded_capacity: Option<usize>,
) -> DdsResult<()> {
use crate::dynamic_value::DynamicValue as DV;
use crate::topic::*;
match (primary_type, val) {
(TYPE_1BY, DV::Bool(b)) => {
let v: u8 = if *b { 1 } else { 0 };
unsafe { std::ptr::write(dst, v) };
}
(TYPE_1BY, DV::Int8(v)) => {
unsafe { std::ptr::write(dst.cast::<i8>(), *v) };
}
(TYPE_1BY, DV::UInt8(v)) => {
unsafe { std::ptr::write(dst, *v) };
}
(TYPE_1BY, DV::Byte(v)) => {
unsafe { std::ptr::write(dst, *v) };
}
(TYPE_2BY, DV::Int16(v)) => {
unsafe { std::ptr::write(dst.cast::<i16>(), *v) };
}
(TYPE_2BY, DV::UInt16(v)) => {
unsafe { std::ptr::write(dst.cast::<u16>(), *v) };
}
(TYPE_4BY, DV::Int32(v)) => {
unsafe { std::ptr::write(dst.cast::<i32>(), *v) };
}
(TYPE_4BY, DV::UInt32(v)) => {
unsafe { std::ptr::write(dst.cast::<u32>(), *v) };
}
(TYPE_4BY, DV::Float32(v)) => {
unsafe { std::ptr::write(dst.cast::<f32>(), *v) };
}
(TYPE_4BY, DV::Enum { value }) => {
unsafe { std::ptr::write(dst.cast::<i32>(), *value) };
}
(TYPE_8BY, DV::Int64(v)) => {
unsafe { std::ptr::write(dst.cast::<i64>(), *v) };
}
(TYPE_8BY, DV::UInt64(v)) => {
unsafe { std::ptr::write(dst.cast::<u64>(), *v) };
}
(TYPE_8BY, DV::Float64(v)) => {
unsafe { std::ptr::write(dst.cast::<f64>(), *v) };
}
(TYPE_STR, DV::String(s)) => {
let leaked = std::ffi::CString::new(s.as_str())
.map_err(|_| DdsError::BadParameter("string contains an interior null".into()))?;
unsafe { std::ptr::write(dst.cast::<*const i8>(), leaked.into_raw()) };
}
(TYPE_BST, DV::String(s)) => {
let bytes = s.as_bytes();
let capacity = bounded_capacity.ok_or_else(|| {
DdsError::BadParameter("bounded string capacity is missing".into())
})?;
if bytes.len() >= capacity {
return Err(DdsError::BadParameter(format!(
"string length {} exceeds bound {}",
bytes.len(),
capacity - 1
)));
}
unsafe { std::ptr::copy_nonoverlapping(bytes.as_ptr(), dst, bytes.len()) };
unsafe { std::ptr::write(dst.add(bytes.len()), 0) };
}
_ => {
}
}
Ok(())
}
pub(crate) fn read_value_from_native_public(
base: *mut u8,
schema: &crate::DynamicTypeSchema,
ops: &[u32],
ops_start: usize,
) -> crate::DynamicValue {
read_value_from_native(base, schema, ops, ops_start).unwrap_or(crate::DynamicValue::Bool(false))
}
fn read_value_from_native(
base: *mut u8,
schema: &crate::DynamicTypeSchema,
ops: &[u32],
ops_start: usize,
) -> DdsResult<crate::DynamicValue> {
use crate::dynamic_value::DynamicValue as DV;
use crate::topic::*;
use std::collections::BTreeMap;
let fields_schema = match schema {
crate::DynamicTypeSchema::Struct { fields, .. } => fields,
_ => return Ok(schema.default_value()),
};
let mut values = BTreeMap::new();
let mut field_idx = 0usize;
let mut i = ops_start;
while i < ops.len() {
let op = ops[i];
let opcode = op & DDS_OP_MASK;
match opcode {
OP_ADR => {
let primary = op & DDS_OP_TYPE_MASK;
let offset = if i + 1 < ops.len() {
ops[i + 1] as usize
} else {
0
};
let src = unsafe { base.add(offset) };
let field_schema = fields_schema.get(field_idx);
let capacity = if primary == TYPE_BST {
Some(bounded_string_capacity(ops, i)?)
} else {
None
};
let val = read_primitive_from_native(src, primary, op, field_schema, capacity)?;
let name = field_schema
.map(|f| f.name.clone())
.unwrap_or_else(|| format!("field_{}", field_idx));
values.insert(name, val);
field_idx += 1;
i += crate::xtypes::adr_step(ops, i);
}
OP_RTS | OP_DLC => {
i += 1;
}
_ => {
i += 1;
}
}
}
Ok(DV::Struct(values))
}
fn decode_bounded_string(bytes: &[u8]) -> DdsResult<String> {
let terminator = bytes.iter().position(|byte| *byte == 0).ok_or_else(|| {
DdsError::BadParameter("bounded native string is not null-terminated".into())
})?;
Ok(String::from_utf8_lossy(&bytes[..terminator]).into_owned())
}
fn read_primitive_from_native(
src: *mut u8,
primary_type: u32,
op: u32,
field_schema: Option<&crate::dynamic_value::DynamicFieldSchema>,
bounded_capacity: Option<usize>,
) -> DdsResult<crate::dynamic_value::DynamicValue> {
use crate::dynamic_value::DynamicValue as DV;
use crate::topic::*;
let value = unsafe {
match primary_type {
TYPE_1BY => {
let v = std::ptr::read(src as *const u8);
if let Some(fs) = field_schema {
match &fs.value {
crate::DynamicTypeSchema::Primitive(
crate::dynamic_type::DynamicPrimitiveKind::Boolean,
) => DV::Bool(v != 0),
crate::DynamicTypeSchema::Primitive(
crate::dynamic_type::DynamicPrimitiveKind::Int8,
) => DV::Int8(v as i8),
_ => DV::Byte(v),
}
} else if op & OP_FLAG_SGN != 0 {
DV::Int8(v as i8)
} else {
DV::Byte(v)
}
}
TYPE_2BY => {
let v = std::ptr::read(src as *const i16);
let vu = std::ptr::read(src as *const u16);
if op & OP_FLAG_SGN != 0 {
DV::Int16(v)
} else {
DV::UInt16(vu)
}
}
TYPE_4BY => {
if op & OP_FLAG_FP != 0 {
DV::Float32(std::ptr::read(src as *const f32))
} else if let Some(fs) = field_schema {
match &fs.value {
crate::DynamicTypeSchema::Enum { .. } => DV::Enum {
value: std::ptr::read(src as *const i32),
},
_ => {
let v = std::ptr::read(src as *const i32);
let vu = std::ptr::read(src as *const u32);
if op & OP_FLAG_SGN != 0 {
DV::Int32(v)
} else {
DV::UInt32(vu)
}
}
}
} else {
let v = std::ptr::read(src as *const i32);
let vu = std::ptr::read(src as *const u32);
if op & OP_FLAG_SGN != 0 {
DV::Int32(v)
} else {
DV::UInt32(vu)
}
}
}
TYPE_8BY => {
if op & OP_FLAG_FP != 0 {
DV::Float64(std::ptr::read(src as *const f64))
} else {
let v = std::ptr::read(src as *const i64);
let vu = std::ptr::read(src as *const u64);
if op & OP_FLAG_SGN != 0 {
DV::Int64(v)
} else {
DV::UInt64(vu)
}
}
}
TYPE_STR => {
let ptr = std::ptr::read(src as *const *const i8);
if ptr.is_null() {
DV::String(String::new())
} else {
DV::String(CStr::from_ptr(ptr).to_string_lossy().into_owned())
}
}
TYPE_BST => {
let capacity = bounded_capacity.ok_or_else(|| {
DdsError::BadParameter("bounded string capacity is missing".into())
})?;
let bytes = std::slice::from_raw_parts(src.cast_const(), capacity);
DV::String(decode_bounded_string(bytes)?)
}
_ => DV::Null,
}
};
Ok(value)
}
#[cfg(test)]
mod tests {
use super::*;
use crate::dynamic_type::DynamicPrimitiveKind;
use crate::dynamic_value::{DynamicTypeSchema, DynamicValue};
fn dynamic_field(name: &str, member_id: u32, kind: DynamicPrimitiveKind) -> DynamicFieldSchema {
DynamicFieldSchema {
name: name.to_string(),
member_id,
value: DynamicTypeSchema::Primitive(kind),
optional: false,
key: false,
external: false,
must_understand: false,
hash_id_name: None,
}
}
fn bounded_string_schema(bound: u32) -> DynamicTypeSchema {
DynamicTypeSchema::Struct {
name: "BoundedString".to_string(),
base: None,
fields: vec![DynamicFieldSchema {
name: "field_0".to_string(),
member_id: 0,
value: DynamicTypeSchema::String { bound: Some(bound) },
optional: false,
key: false,
external: false,
must_understand: false,
hash_id_name: None,
}],
extensibility: None,
autoid: None,
nested: false,
}
}
#[test]
fn public_dynamic_data_accepts_string_at_declared_bound() {
let mut data = crate::DynamicData::new(&bounded_string_schema(4));
let result = data.set_string("field_0", "four");
assert!(result.is_ok());
assert_eq!(data.get_string("field_0").unwrap(), "four");
}
#[test]
fn descriptor_capacity_maps_to_idl_string_bound() {
let ops = [crate::topic::OP_ADR | crate::topic::TYPE_BST, 0, 5];
let fields = parse_ops_to_fields(&ops, 5).unwrap();
assert_eq!(
fields[0].value,
DynamicTypeSchema::String { bound: Some(4) }
);
}
#[test]
fn descriptor_bound_rejects_five_byte_public_value() {
let ops = [crate::topic::OP_ADR | crate::topic::TYPE_BST, 0, 5];
let fields = parse_ops_to_fields(&ops, 5).unwrap();
let schema = DynamicTypeSchema::Struct {
name: "BoundedString".to_string(),
base: None,
fields,
extensibility: None,
autoid: None,
nested: false,
};
let mut data = crate::DynamicData::new(&schema);
let result = data.set_string("field_0", "12345");
assert!(matches!(result, Err(DdsError::BadParameter(_))));
}
#[test]
fn zero_capacity_bounded_string_descriptor_is_rejected() {
let ops = [crate::topic::OP_ADR | crate::topic::TYPE_BST, 0, 0];
let result = parse_ops_to_fields(&ops, 0);
assert!(matches!(result, Err(DdsError::BadParameter(_))));
}
#[test]
fn bounded_native_read_rejects_missing_terminator() {
let native = *b"abcd";
let result = decode_bounded_string(&native);
assert!(matches!(result, Err(DdsError::BadParameter(_))));
}
#[test]
fn malformed_bounded_native_value_is_not_replaced_with_valid_default() {
let mut native = *b"abcd";
let ops = [crate::topic::OP_ADR | crate::topic::TYPE_BST, 0, 4];
let schema = bounded_string_schema(3);
let value = read_value_from_native_public(native.as_mut_ptr(), &schema, &ops, 0);
assert_eq!(value, DynamicValue::Bool(false));
assert!(crate::DynamicData::from_value(&schema, value).is_err());
}
#[test]
fn bounded_native_write_accepts_exact_bound_without_overflow() {
let ops = [crate::topic::OP_ADR | crate::topic::TYPE_BST, 0, 5];
let mut native = [0xa5; 6];
let schema = bounded_string_schema(4);
let data = crate::DynamicData::from_value(
&schema,
DynamicValue::Struct(std::collections::BTreeMap::from([(
"field_0".to_string(),
DynamicValue::String("four".to_string()),
)])),
)
.unwrap();
let result = write_data_to_native(&data, native.as_mut_ptr(), &ops);
assert!(result.is_ok());
assert_eq!(&native[..5], b"four\0");
assert_eq!(native[5], 0xa5);
}
#[test]
fn bounded_native_write_rejects_overlength_before_writing() {
let ops = [crate::topic::OP_ADR | crate::topic::TYPE_BST, 0, 5];
let mut native = [0xa5; 6];
let schema = bounded_string_schema(5);
let data = crate::DynamicData::from_value(
&schema,
DynamicValue::Struct(std::collections::BTreeMap::from([(
"field_0".to_string(),
DynamicValue::String("12345".to_string()),
)])),
)
.unwrap();
let result = write_data_to_native(&data, native.as_mut_ptr(), &ops);
assert!(matches!(result, Err(DdsError::BadParameter(_))));
assert_eq!(native, [0xa5; 6]);
}
#[test]
fn native_mapping_rejects_duplicate_schema_names() {
let schema = DynamicTypeSchema::Struct {
name: "DuplicateNames".to_string(),
base: None,
fields: vec![
dynamic_field("same", 3, DynamicPrimitiveKind::Int32),
dynamic_field("same", 9, DynamicPrimitiveKind::Int32),
],
extensibility: Some(crate::DynamicTypeExtensibility::Final),
autoid: None,
nested: false,
};
let data = crate::DynamicData::new(&schema);
let ops = [
crate::topic::OP_ADR | crate::topic::TYPE_4BY,
0,
crate::topic::OP_ADR | crate::topic::TYPE_4BY,
4,
];
let result = validate_data_for_native(&data, &ops);
assert!(matches!(result, Err(DdsError::BadParameter(_))));
}
#[test]
fn native_mapping_rejects_duplicate_member_ids() {
let schema = DynamicTypeSchema::Struct {
name: "DuplicateIds".to_string(),
base: None,
fields: vec![
dynamic_field("left", 7, DynamicPrimitiveKind::Int32),
dynamic_field("right", 7, DynamicPrimitiveKind::Int32),
],
extensibility: Some(crate::DynamicTypeExtensibility::Final),
autoid: None,
nested: false,
};
let data = crate::DynamicData::new(&schema);
let ops = [
crate::topic::OP_ADR | crate::topic::TYPE_4BY,
0,
crate::topic::OP_ADR | crate::topic::TYPE_4BY,
4,
];
let result = validate_data_for_native(&data, &ops);
assert!(matches!(result, Err(DdsError::BadParameter(_))));
}
#[test]
fn native_mapping_uses_schema_names_and_declaration_order() {
let schema = DynamicTypeSchema::Struct {
name: "CustomNames".to_string(),
base: None,
fields: vec![
dynamic_field("telemetry_code", 31, DynamicPrimitiveKind::Int32),
dynamic_field("voltage", 7, DynamicPrimitiveKind::Float64),
],
extensibility: Some(crate::DynamicTypeExtensibility::Final),
autoid: None,
nested: false,
};
let mut data = crate::DynamicData::new(&schema);
data.set_i32("telemetry_code", 41_337).unwrap();
data.set_f64("voltage", 12.75).unwrap();
let ops = [
crate::topic::OP_ADR | crate::topic::TYPE_4BY | crate::topic::OP_FLAG_SGN,
0,
crate::topic::OP_ADR | crate::topic::TYPE_8BY | crate::topic::OP_FLAG_FP,
8,
];
let mut native = [0u64; 2];
write_data_to_native(&data, native.as_mut_ptr().cast(), &ops).unwrap();
assert_eq!(
i32::from_ne_bytes(native[0].to_ne_bytes()[..4].try_into().unwrap()),
41_337
);
assert_eq!(f64::from_bits(native[1]), 12.75);
}
#[test]
fn dynamic_data_get_set_primitives() {
let schema = DynamicTypeSchema::Struct {
name: "TestStruct".to_string(),
base: None,
fields: vec![
DynamicFieldSchema {
name: "x".to_string(),
member_id: 0,
value: DynamicTypeSchema::Primitive(DynamicPrimitiveKind::Int32),
optional: false,
key: false,
external: false,
must_understand: false,
hash_id_name: None,
},
DynamicFieldSchema {
name: "y".to_string(),
member_id: 1,
value: DynamicTypeSchema::Primitive(DynamicPrimitiveKind::Float64),
optional: false,
key: false,
external: false,
must_understand: false,
hash_id_name: None,
},
DynamicFieldSchema {
name: "label".to_string(),
member_id: 2,
value: DynamicTypeSchema::String { bound: None },
optional: false,
key: false,
external: false,
must_understand: false,
hash_id_name: None,
},
DynamicFieldSchema {
name: "flag".to_string(),
member_id: 3,
value: DynamicTypeSchema::Primitive(DynamicPrimitiveKind::Boolean),
optional: true,
key: false,
external: false,
must_understand: false,
hash_id_name: None,
},
],
extensibility: None,
autoid: None,
nested: false,
};
let mut data = crate::DynamicData::new(&schema);
assert_eq!(data.get_i32("x").unwrap(), 0);
assert_eq!(data.get_f64("y").unwrap(), 0.0);
assert_eq!(data.get_string("label").unwrap(), "");
data.set_i32("x", 42).unwrap();
assert_eq!(data.get_i32("x").unwrap(), 42);
data.set_f64("y", std::f64::consts::PI).unwrap();
assert!(data.get_f64("y").unwrap().abs() - std::f64::consts::PI < 0.001);
data.set_string("label", "hello").unwrap();
assert_eq!(data.get_string("label").unwrap(), "hello");
data.set_bool("flag", true).unwrap();
assert!(data.get_bool("flag").unwrap());
assert!(data.validate().is_ok());
let names = data.field_names();
assert_eq!(names, vec!["x", "y", "label", "flag"]);
data.set_null("flag").unwrap();
assert!(!data.is_set("flag"));
assert!(data.set_null("x").is_err());
assert!(data.get_bool("x").is_err());
assert!(data.get_i32("label").is_err());
}
#[test]
fn dynamic_data_from_value_validates() {
let schema = DynamicTypeSchema::Struct {
name: "S".to_string(),
base: None,
fields: vec![DynamicFieldSchema {
name: "v".to_string(),
member_id: 0,
value: DynamicTypeSchema::Primitive(DynamicPrimitiveKind::Int32),
optional: false,
key: false,
external: false,
must_understand: false,
hash_id_name: None,
}],
extensibility: None,
autoid: None,
nested: false,
};
let val = DynamicValue::Struct({
let mut m = std::collections::BTreeMap::new();
m.insert("v".to_string(), DynamicValue::Int32(99));
m
});
let data = crate::DynamicData::from_value(&schema, val).unwrap();
assert_eq!(data.get_i32("v").unwrap(), 99);
let bad_val = DynamicValue::Struct({
let mut m = std::collections::BTreeMap::new();
m.insert("v".to_string(), DynamicValue::String("not an i32".into()));
m
});
assert!(crate::DynamicData::from_value(&schema, bad_val).is_err());
}
}