File xcdr2_codec.hpp
File List > astutedds > cdr > xcdr2_codec.hpp
Go to the documentation of this file
#ifndef ASTUTEDDS_XCDR2_CODEC_HPP
#define ASTUTEDDS_XCDR2_CODEC_HPP
#include "cdr_types.hpp"
#include <cstring>
#include <stack>
#include <vector>
#include <string>
#include <stdexcept>
namespace astutedds::cdr {
class XCDR2Encoder {
public:
XCDR2Encoder(std::vector<uint8_t>& buffer, bool big_endian = false,
ExtensibilityKind extensibility = ExtensibilityKind::FINAL)
: buffer_(buffer), swap_bytes_((!big_endian) != is_little_endian()) {
// Write encapsulation header — the encapsulation kind must match
// the type's extensibility so receivers can decode correctly.
// FINAL → XCDR2_LE / XCDR2_BE (0x0007 / 0x0006)
// APPENDABLE → D_CDR2_LE / D_CDR2_BE (0x0009 / 0x0008)
// MUTABLE → PL_CDR2_LE / PL_CDR2_BE (0x000B / 0x000A)
EncapsulationHeader header;
switch (extensibility) {
case ExtensibilityKind::APPENDABLE:
header.encoding = big_endian ? EncodingKind::D_CDR2_BE : EncodingKind::D_CDR2_LE;
break;
case ExtensibilityKind::MUTABLE:
header.encoding = big_endian ? EncodingKind::PL_CDR2_BE : EncodingKind::PL_CDR2_LE;
break;
case ExtensibilityKind::FINAL:
default:
header.encoding = big_endian ? EncodingKind::XCDR2_BE : EncodingKind::XCDR2_LE;
break;
}
header.options = 0;
write_encapsulation_header(header);
}
// Primitive type encoding (all aligned to 4 bytes in XCDR2)
void encode_uint8(uint8_t value) { write_aligned(&value, sizeof(value), 1); }
void encode_int8(int8_t value) { write_aligned(&value, sizeof(value), 1); }
void encode_uint16(uint16_t value) {
if (swap_bytes_) value = swap_endian(value);
write_aligned(&value, sizeof(value), 2);
}
void encode_int16(int16_t value) {
if (swap_bytes_) value = swap_endian(value);
write_aligned(&value, sizeof(value), 2);
}
void encode_uint32(uint32_t value) {
if (swap_bytes_) value = swap_endian(value);
write_aligned(&value, sizeof(value), 4);
}
void encode_int32(int32_t value) {
if (swap_bytes_) value = swap_endian(value);
write_aligned(&value, sizeof(value), 4);
}
void encode_uint64(uint64_t value) {
if (swap_bytes_) value = swap_endian(value);
write_aligned(&value, sizeof(value), 8);
}
void encode_int64(int64_t value) {
if (swap_bytes_) value = swap_endian(value);
write_aligned(&value, sizeof(value), 8);
}
void encode_float(float value) {
uint32_t bits;
std::memcpy(&bits, &value, sizeof(value));
if (swap_bytes_) bits = swap_endian(bits);
write_aligned(&bits, sizeof(bits), 4);
}
void encode_double(double value) {
uint64_t bits;
std::memcpy(&bits, &value, sizeof(value));
if (swap_bytes_) bits = swap_endian(bits);
write_aligned(&bits, sizeof(bits), 8);
}
void encode_bool(bool value) {
uint8_t byte = value ? 1 : 0;
write_aligned(&byte, sizeof(byte), 1);
}
// String encoding (length prefix + data)
void encode_string(const std::string& str) {
uint32_t length = static_cast<uint32_t>(str.size() + 1); // Include null
encode_uint32(length);
buffer_.insert(buffer_.end(), str.begin(), str.end());
buffer_.push_back(0); // Null terminator
}
// Wide string encoding -- see decode_wstring() for the wire format.
void encode_wstring(const std::wstring& str) {
encode_uint32(static_cast<uint32_t>(str.size()));
for (wchar_t wc : str) {
encode_uint16(static_cast<uint16_t>(wc));
}
}
// Sequence length encoding
void encode_sequence_length(uint32_t length) {
encode_uint32(length);
}
void begin_delimited() {
// Align to 4 bytes
align_to_4();
// Save position where DHEADER will be written
dheader_positions_.push(buffer_.size());
// Write placeholder DHEADER (4 bytes)
encode_uint32(0);
}
void end_delimited() {
if (dheader_positions_.empty()) {
throw std::runtime_error("end_delimited() called without matching begin_delimited()");
}
// Calculate size of delimited data (excluding DHEADER itself)
size_t dheader_pos = dheader_positions_.top();
dheader_positions_.pop();
size_t current_pos = buffer_.size();
uint32_t dheader_size = static_cast<uint32_t>(current_pos - dheader_pos - 4);
// Update DHEADER at saved position
if (swap_bytes_) dheader_size = swap_endian(dheader_size);
std::memcpy(&buffer_[dheader_pos], &dheader_size, 4);
}
template <typename WriteMemberFn>
void encode_member(uint32_t member_id, bool must_understand, WriteMemberFn&& write_member) {
// Reserve space for the EMHEADER; its final value is backpatched below once
// the member's encoded size (and hence Length Code) is known. The EMHEADER is
// itself 4-byte aligned, so any alignment padding must be inserted *before*
// header_pos is captured -- otherwise header_pos would point at the padding
// rather than the header, corrupting both the backpatched header and the
// bytes immediately following it (only an issue when the previous member's
// encoded size is not a multiple of 4, e.g. a preceding string/sequence).
align_to_4();
const size_t header_pos = buffer_.size();
encode_uint32(0);
// Unconditionally reserve the 4-byte NEXTINT slot *before* writing the
// member's payload, rather than measuring the payload first and
// inserting a NEXTINT prefix afterwards. write_member() may contain
// values requiring >4-byte alignment (e.g. int64/uint64/double), whose
// internal padding is computed relative to the buffer position at the
// time write_member() runs. If that position is computed *before* a
// NEXTINT would later be spliced in with buffer_.insert(), the splice
// shifts the already-written (and already alignment-padded) payload by
// 4 bytes -- flipping 8-byte-alignment parity and corrupting whatever
// alignment padding was baked into the payload. Reserving the slot
// upfront guarantees write_member() always executes at the payload's
// true final wire position, so its internal alignment decisions are
// always correct. See docs/omg/OMG_COMPLIANCE_PLAN.md §4.5 history for
// the analogous, previously-fixed `header_pos`-before-alignment bug;
// this is the same class of defect one level deeper (the NEXTINT, not
// just the EMHEADER, must precede any alignment-sensitive write).
const size_t nextint_pos = buffer_.size();
encode_uint32(0);
const size_t data_start = buffer_.size();
write_member();
const size_t data_end = buffer_.size();
const uint32_t member_size = static_cast<uint32_t>(data_end - data_start);
uint8_t lc;
switch (member_size) {
case 1:
case 2:
case 4: {
// Safe to drop the reserved NEXTINT slot: removing exactly 4 bytes
// preserves 4-byte alignment parity, and payloads of this size never
// require more than 4-byte internal alignment, so nothing written by
// write_member() can have relied on 8-byte-alignment padding that the
// shift would invalidate.
lc = (member_size == 1) ? 0 : (member_size == 2) ? 1 : 2;
buffer_.erase(buffer_.begin() + static_cast<std::ptrdiff_t>(nextint_pos),
buffer_.begin() + static_cast<std::ptrdiff_t>(nextint_pos + 4));
break;
}
default:
// LC=3 (fixed 8-byte) is deliberately never produced here, even when
// member_size == 8: the payload may rely on 8-byte alignment computed
// relative to the reserved NEXTINT slot's position, and dropping that
// slot would flip 8-byte-alignment parity for the shifted payload.
// Always keep the reserved slot and use LC=4 (NEXTINT) instead --
// wire-valid per §7.4.3.4.2, just marginally less compact than LC=3.
lc = 4;
uint32_t nextint = member_size;
if (swap_bytes_) nextint = swap_endian(nextint);
std::memcpy(&buffer_[nextint_pos], &nextint, 4);
break;
}
uint32_t header = (member_id & 0x0FFFFFFFu) | (static_cast<uint32_t>(lc) << 28);
if (must_understand) {
header |= 0x80000000u;
}
if (swap_bytes_) header = swap_endian(header);
std::memcpy(&buffer_[header_pos], &header, 4);
}
void encode_member_header(uint32_t member_id, bool must_understand = false) {
// EMHEADER format:
// - Bit 31: must_understand flag
// - Bits 30-28: length code (0-7)
// - Bits 27-0: member_id
uint32_t header = member_id & 0x0FFFFFFF; // 28 bits for member ID
if (must_understand) {
header |= 0x80000000; // Set bit 31
}
encode_uint32(header);
}
size_t size() const { return buffer_.size(); }
private:
void write_encapsulation_header(const EncapsulationHeader& header) {
// Per CDR encapsulation rules, kind/options are always written in network byte order (big-endian),
// independent of the payload endianness.
const uint16_t encoding_value = static_cast<uint16_t>(header.encoding);
const uint16_t options_value = header.options;
buffer_.resize(4);
buffer_[0] = static_cast<uint8_t>((encoding_value >> 8) & 0xFF);
buffer_[1] = static_cast<uint8_t>(encoding_value & 0xFF);
buffer_[2] = static_cast<uint8_t>((options_value >> 8) & 0xFF);
buffer_[3] = static_cast<uint8_t>(options_value & 0xFF);
}
void write_aligned(const void* data, size_t size, size_t alignment) {
size_t current_pos = buffer_.size();
size_t aligned_pos = align_offset(current_pos, alignment);
// Add padding
if (aligned_pos > current_pos) {
buffer_.resize(aligned_pos, 0);
}
// Write data
size_t old_size = buffer_.size();
buffer_.resize(old_size + size);
std::memcpy(&buffer_[old_size], data, size);
}
void align_to_4() {
size_t current_pos = buffer_.size();
size_t aligned_pos = align_offset(current_pos, 4);
if (aligned_pos > current_pos) {
buffer_.resize(aligned_pos, 0);
}
}
std::vector<uint8_t>& buffer_;
bool swap_bytes_;
std::stack<size_t> dheader_positions_;
};
class XCDR2Decoder {
public:
XCDR2Decoder(const std::vector<uint8_t>& buffer)
: buffer_(buffer), position_(0) {
// Read and validate encapsulation header
if (buffer_.size() < 4) {
throw std::runtime_error("Buffer too small for CDR encapsulation header");
}
// Encapsulation encoding is always in big-endian (network byte order)
uint16_t encoding_value = (buffer_[0] << 8) | buffer_[1];
const EncodingKind encoding = static_cast<EncodingKind>(encoding_value);
// Determine payload endianness from the encapsulation kind.
// (We treat PL_CDR2_* and D_CDR2_* as XCDR2-family for endianness purposes.)
const bool data_is_big_endian = (
encoding == EncodingKind::CDR2_BE ||
encoding == EncodingKind::PL_CDR2_BE ||
encoding == EncodingKind::D_CDR2_BE
);
// Swap bytes if data endianness differs from machine endianness.
const bool machine_is_little_endian = is_little_endian();
swap_bytes_ = (data_is_big_endian == machine_is_little_endian);
position_ = 4; // Skip encapsulation header
}
// Primitive type decoding
uint8_t decode_uint8() { return read_aligned<uint8_t>(1); }
int8_t decode_int8() { return read_aligned<int8_t>(1); }
uint16_t decode_uint16() {
uint16_t value = read_aligned<uint16_t>(2);
return swap_bytes_ ? swap_endian(value) : value;
}
int16_t decode_int16() {
int16_t value = read_aligned<int16_t>(2);
return swap_bytes_ ? swap_endian(value) : value;
}
uint32_t decode_uint32() {
uint32_t value = read_aligned<uint32_t>(4);
return swap_bytes_ ? swap_endian(value) : value;
}
int32_t decode_int32() {
int32_t value = read_aligned<int32_t>(4);
return swap_bytes_ ? swap_endian(value) : value;
}
uint64_t decode_uint64() {
uint64_t value = read_aligned<uint64_t>(8);
return swap_bytes_ ? swap_endian(value) : value;
}
int64_t decode_int64() {
int64_t value = read_aligned<int64_t>(8);
return swap_bytes_ ? swap_endian(value) : value;
}
float decode_float() {
uint32_t bits = read_aligned<uint32_t>(4);
if (swap_bytes_) bits = swap_endian(bits);
float value;
std::memcpy(&value, &bits, sizeof(value));
return value;
}
double decode_double() {
uint64_t bits = read_aligned<uint64_t>(8);
if (swap_bytes_) bits = swap_endian(bits);
double value;
std::memcpy(&value, &bits, sizeof(value));
return value;
}
bool decode_bool() {
return read_aligned<uint8_t>(1) != 0;
}
// String decoding
std::string decode_string() {
uint32_t length = decode_uint32();
if (length == 0) {
return "";
}
if (position_ + length > buffer_.size()) {
throw std::runtime_error(
"XCDR2Decoder: buffer underflow reading string at pos=" +
std::to_string(position_) + " len=" + std::to_string(length) +
" bufsize=" + std::to_string(buffer_.size()));
}
std::string result(reinterpret_cast<const char*>(&buffer_[position_]), length - 1);
position_ += length;
return result;
}
// Sequence length decoding
uint32_t decode_sequence_length() {
return decode_uint32();
}
// Wide string decoding -- one uint16 code unit per wchar_t, no null terminator.
std::wstring decode_wstring() {
uint32_t length = decode_uint32();
std::wstring result;
result.reserve(length);
for (uint32_t i = 0; i < length; ++i) {
result.push_back(static_cast<wchar_t>(decode_uint16()));
}
return result;
}
uint32_t begin_delimited() {
// Align to 4 bytes
align_to_4();
// Read DHEADER
uint32_t dheader_size = decode_uint32();
// Save the end position for this delimited block
dheader_end_positions_.push(position_ + dheader_size);
return dheader_size;
}
void end_delimited() {
if (dheader_end_positions_.empty()) {
throw std::runtime_error("end_delimited() called without matching begin_delimited()");
}
size_t end_pos = dheader_end_positions_.top();
dheader_end_positions_.pop();
// Skip to end position if we didn't read all data
if (position_ < end_pos) {
position_ = end_pos;
}
}
bool decode_member_header(uint32_t& member_id, bool& must_understand) {
// Check if we're at the end of a delimited block
if (!dheader_end_positions_.empty() && position_ >= dheader_end_positions_.top()) {
return false;
}
// Read EMHEADER
uint32_t header = decode_uint32();
must_understand = (header & 0x80000000) != 0;
member_id = header & 0x0FFFFFFF;
const uint8_t lc = static_cast<uint8_t>((header >> 28) & 0x7);
switch (lc) {
case 0: current_member_size_ = 1; break;
case 1: current_member_size_ = 2; break;
case 2: current_member_size_ = 4; break;
case 3: current_member_size_ = 8; break;
default:
// LC 4-6: a NEXTINT giving the arbitrary/element-multiple byte count follows.
current_member_size_ = decode_uint32();
break;
}
current_member_data_start_ = position_;
return true;
}
void skip_member() {
position_ = current_member_data_start_ + current_member_size_;
}
size_t position() const { return position_; }
bool at_end() const { return position_ >= buffer_.size(); }
bool at_delimited_end() const {
if (!dheader_end_positions_.empty()) {
return position_ >= dheader_end_positions_.top();
}
return position_ >= buffer_.size();
}
private:
template<typename T>
T read_aligned(size_t alignment) {
position_ = align_offset(position_, alignment);
if (position_ + sizeof(T) > buffer_.size()) {
throw std::runtime_error(
"XCDR2Decoder: buffer underflow at pos=" + std::to_string(position_) +
" need=" + std::to_string(sizeof(T)) +
" align=" + std::to_string(alignment) +
" bufsize=" + std::to_string(buffer_.size()));
}
T value;
std::memcpy(&value, &buffer_[position_], sizeof(T));
position_ += sizeof(T);
return value;
}
void align_to_4() {
position_ = align_offset(position_, 4);
}
const std::vector<uint8_t>& buffer_;
size_t position_;
bool swap_bytes_;
std::stack<size_t> dheader_end_positions_;
size_t current_member_size_ = 0;
size_t current_member_data_start_ = 0;
};
} // namespace astutedds::cdr
#endif // ASTUTEDDS_XCDR2_CODEC_HPP