cvmmap-streamer/src/sim/wire.cpp

#include "cvmmap_streamer/sim/wire.hpp"

#include <algorithm>
#include <array>

namespace cvmmap_streamer::sim {

namespace {

	constexpr std::size_t kMetaVersionMajorOffset = 8;
	constexpr std::size_t kMetaVersionMinorOffset = 9;
	constexpr std::size_t kMetaFrameCountOffset   = 12;
	constexpr std::size_t kMetaTimestampOffset    = 16;
	constexpr std::size_t kMetaFrameInfoOffset    = 24;

	constexpr std::size_t kFrameInfoWidthOffset      = 0;
	constexpr std::size_t kFrameInfoHeightOffset     = 2;
	constexpr std::size_t kFrameInfoChannelsOffset   = 4;
	constexpr std::size_t kFrameInfoDepthOffset      = 5;
	constexpr std::size_t kFrameInfoPixelFmtOffset   = 6;
	constexpr std::size_t kFrameInfoBufferSizeOffset = 8;

	constexpr std::size_t kSyncMagicOffset      = 0;
	constexpr std::size_t kSyncVersionMajor     = 2;
	constexpr std::size_t kSyncVersionMinor     = 3;
	constexpr std::size_t kSyncFrameCountOffset = 4;
	constexpr std::size_t kSyncTimestampOffset  = 16;
	constexpr std::size_t kSyncLabelOffset      = 24;

	constexpr std::size_t kModuleStatusMagicOffset  = 0;
	constexpr std::size_t kModuleStatusVersionMajor = 2;
	constexpr std::size_t kModuleStatusVersionMinor = 3;
	constexpr std::size_t kModuleStatusCodeOffset   = 4;
	constexpr std::size_t kModuleStatusLabelOffset  = 8;

	void write_u16_le(std::span<std::uint8_t> bytes, std::size_t offset, std::uint16_t value) {
		bytes[offset]     = static_cast<std::uint8_t>(value & 0xffu);
		bytes[offset + 1] = static_cast<std::uint8_t>((value >> 8) & 0xffu);
	}

	void write_u32_le(std::span<std::uint8_t> bytes, std::size_t offset, std::uint32_t value) {
		bytes[offset]     = static_cast<std::uint8_t>(value & 0xffu);
		bytes[offset + 1] = static_cast<std::uint8_t>((value >> 8) & 0xffu);
		bytes[offset + 2] = static_cast<std::uint8_t>((value >> 16) & 0xffu);
		bytes[offset + 3] = static_cast<std::uint8_t>((value >> 24) & 0xffu);
	}

	void write_i32_le(std::span<std::uint8_t> bytes, std::size_t offset, std::int32_t value) {
		write_u32_le(bytes, offset, static_cast<std::uint32_t>(value));
	}

	void write_u64_le(std::span<std::uint8_t> bytes, std::size_t offset, std::uint64_t value) {
		for (std::size_t i = 0; i < sizeof(std::uint64_t); ++i) {
			bytes[offset + i] = static_cast<std::uint8_t>((value >> (i * 8)) & 0xffu);
		}
	}

	void write_label_bytes(std::span<std::uint8_t> out, std::size_t offset, std::string_view label) {
		const auto bounded = std::min<std::size_t>(label.size(), ipc::kLabelLenMax);
		std::fill_n(out.begin() + offset, ipc::kLabelLenMax, static_cast<std::uint8_t>(0));
		for (std::size_t i = 0; i < bounded; ++i) {
			out[offset + i] = static_cast<std::uint8_t>(label[i]);
		}
	}

}

void write_frame_metadata(
	std::span<std::uint8_t> metadata,
	const ipc::FrameInfo &info,
	std::uint32_t frame_count,
	std::uint64_t timestamp_ns) {
	std::fill(metadata.begin(), metadata.end(), static_cast<std::uint8_t>(0));
	std::copy(
		ipc::kFrameMetadataMagic.begin(),
		ipc::kFrameMetadataMagic.end(),
		metadata.begin());

	metadata[kMetaVersionMajorOffset] = ipc::kVersionMajor;
	metadata[kMetaVersionMinorOffset] = ipc::kVersionMinor;
	write_u32_le(metadata, kMetaFrameCountOffset, frame_count);
	write_u64_le(metadata, kMetaTimestampOffset, timestamp_ns);

	auto frame_info = metadata.subspan(kMetaFrameInfoOffset);
	write_u16_le(frame_info, kFrameInfoWidthOffset, info.width);
	write_u16_le(frame_info, kFrameInfoHeightOffset, info.height);
	frame_info[kFrameInfoChannelsOffset] = info.channels;
	frame_info[kFrameInfoDepthOffset]    = static_cast<std::uint8_t>(info.depth);
	frame_info[kFrameInfoPixelFmtOffset] = static_cast<std::uint8_t>(info.pixel_format);
	write_u32_le(frame_info, kFrameInfoBufferSizeOffset, info.buffer_size);
}

void write_sync_message(
	std::span<std::uint8_t> out,
	std::string_view label,
	std::uint32_t frame_count,
	std::uint64_t timestamp_ns) {
	std::fill(out.begin(), out.end(), static_cast<std::uint8_t>(0));
	out[kSyncMagicOffset]  = ipc::kFrameTopicMagic;
	out[kSyncVersionMajor] = ipc::kVersionMajor;
	out[kSyncVersionMinor] = ipc::kVersionMinor;
	write_u32_le(out, kSyncFrameCountOffset, frame_count);
	write_u64_le(out, kSyncTimestampOffset, timestamp_ns);
	write_label_bytes(out, kSyncLabelOffset, label);
}

void write_module_status_message(
	std::span<std::uint8_t> out,
	std::string_view label,
	ipc::ModuleStatus status) {
	std::fill(out.begin(), out.end(), static_cast<std::uint8_t>(0));
	out[kModuleStatusMagicOffset]  = ipc::kModuleStatusMagic;
	out[kModuleStatusVersionMajor] = ipc::kVersionMajor;
	out[kModuleStatusVersionMinor] = ipc::kVersionMinor;
	write_i32_le(out, kModuleStatusCodeOffset, static_cast<std::int32_t>(status));
	write_label_bytes(out, kModuleStatusLabelOffset, label);
}

void write_deterministic_payload(
	std::span<std::uint8_t> out,
	std::uint32_t frame_count,
	std::uint16_t width,
	std::uint16_t height,
	std::uint8_t channels) {
	const auto row_stride = static_cast<std::size_t>(width) * channels;
	for (std::size_t idx = 0; idx < out.size(); ++idx) {
		const auto pixel = idx / channels;
		const auto row   = pixel / width;
		const auto col   = pixel % width;
		const auto ch    = idx % channels;
		out[idx]         = static_cast<std::uint8_t>((frame_count + (row * 7u) + (col * 13u) + (ch * 17u) + row_stride + height) & 0xffu);
	}
}

}