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
}
// 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);
}
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();
}
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;
return true;
}
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_;
};
} // namespace astutedds::cdr
#endif // ASTUTEDDS_XCDR2_CODEC_HPP