feat(downstream): add cvmmap downstream runtime implementation
This commit introduces the full downstream runtime implementation needed to ingest, transform, and publish streams. It preserves the original upstream request boundary by packaging the entire cvmmap-streamer module (build config, public API, protocol and IPC glue, and simulator/tester entrypoints) in one coherent core unit. Keeping this group isolated enables reviewers to validate runtime behavior and correctness without mixing test evidence or process documentation changes. Ultraworked with [Sisyphus](https://github.com/code-yeongyu/oh-my-opencode) Co-authored-by: Sisyphus <clio-agent@sisyphuslabs.ai>
This commit is contained in:
@@ -0,0 +1,356 @@
|
||||
#include "cvmmap_streamer/ipc/contracts.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <limits>
|
||||
|
||||
namespace cvmmap_streamer::ipc {
|
||||
|
||||
namespace {
|
||||
|
||||
constexpr std::size_t kSyncMessageSize = 48;
|
||||
constexpr std::size_t kModuleStatusMessageSize = 32;
|
||||
constexpr std::size_t kControlRequestBaseSize = 36;
|
||||
constexpr std::size_t kControlResponseBaseSize = 40;
|
||||
constexpr std::size_t kFrameMetadataRequiredBytes = 36;
|
||||
|
||||
constexpr std::size_t kSyncFrameCountOffset = 4;
|
||||
constexpr std::size_t kSyncTimestampOffset = 16;
|
||||
constexpr std::size_t kSyncLabelOffset = 24;
|
||||
|
||||
constexpr std::size_t kModuleStatusCodeOffset = 4;
|
||||
constexpr std::size_t kModuleStatusLabelOffset = 8;
|
||||
|
||||
constexpr std::size_t kControlCommandOffset = 4;
|
||||
constexpr std::size_t kControlReqLabelOffset = 8;
|
||||
constexpr std::size_t kControlReqLengthOffset = 32;
|
||||
constexpr std::size_t kControlReqPayloadOffset = 34;
|
||||
|
||||
constexpr std::size_t kControlRespCodeOffset = 8;
|
||||
constexpr std::size_t kControlRespLabelOffset = 12;
|
||||
constexpr std::size_t kControlRespLengthOffset = 36;
|
||||
constexpr std::size_t kControlRespPayloadOffset = 38;
|
||||
|
||||
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;
|
||||
|
||||
[[nodiscard]]
|
||||
std::uint16_t read_u16_le(std::span<const std::uint8_t> bytes, std::size_t offset) {
|
||||
return static_cast<std::uint16_t>(bytes[offset]) |
|
||||
(static_cast<std::uint16_t>(bytes[offset + 1]) << 8);
|
||||
}
|
||||
|
||||
[[nodiscard]]
|
||||
std::uint32_t read_u32_le(std::span<const std::uint8_t> bytes, std::size_t offset) {
|
||||
return static_cast<std::uint32_t>(bytes[offset]) |
|
||||
(static_cast<std::uint32_t>(bytes[offset + 1]) << 8) |
|
||||
(static_cast<std::uint32_t>(bytes[offset + 2]) << 16) |
|
||||
(static_cast<std::uint32_t>(bytes[offset + 3]) << 24);
|
||||
}
|
||||
|
||||
[[nodiscard]]
|
||||
std::int32_t read_i32_le(std::span<const std::uint8_t> bytes, std::size_t offset) {
|
||||
return static_cast<std::int32_t>(read_u32_le(bytes, offset));
|
||||
}
|
||||
|
||||
[[nodiscard]]
|
||||
std::uint64_t read_u64_le(std::span<const std::uint8_t> bytes, std::size_t offset) {
|
||||
return static_cast<std::uint64_t>(bytes[offset]) |
|
||||
(static_cast<std::uint64_t>(bytes[offset + 1]) << 8) |
|
||||
(static_cast<std::uint64_t>(bytes[offset + 2]) << 16) |
|
||||
(static_cast<std::uint64_t>(bytes[offset + 3]) << 24) |
|
||||
(static_cast<std::uint64_t>(bytes[offset + 4]) << 32) |
|
||||
(static_cast<std::uint64_t>(bytes[offset + 5]) << 40) |
|
||||
(static_cast<std::uint64_t>(bytes[offset + 6]) << 48) |
|
||||
(static_cast<std::uint64_t>(bytes[offset + 7]) << 56);
|
||||
}
|
||||
|
||||
[[nodiscard]]
|
||||
bool is_supported_version(std::uint8_t major, std::uint8_t minor) {
|
||||
return major == kVersionMajor && minor == kVersionMinor;
|
||||
}
|
||||
|
||||
[[nodiscard]]
|
||||
std::expected<Depth, ParseError> validate_depth(std::uint8_t depth_raw) {
|
||||
if (depth_raw > static_cast<std::uint8_t>(Depth::F16)) {
|
||||
return std::unexpected(ParseError::InvalidDepth);
|
||||
}
|
||||
return static_cast<Depth>(depth_raw);
|
||||
}
|
||||
|
||||
[[nodiscard]]
|
||||
std::expected<PixelFormat, ParseError> validate_pixel_format(std::uint8_t pixel_format_raw) {
|
||||
if (pixel_format_raw > static_cast<std::uint8_t>(PixelFormat::YUYV)) {
|
||||
return std::unexpected(ParseError::InvalidPixelFormat);
|
||||
}
|
||||
return static_cast<PixelFormat>(pixel_format_raw);
|
||||
}
|
||||
|
||||
[[nodiscard]]
|
||||
std::expected<ModuleStatus, ParseError> validate_module_status(std::int32_t status_raw) {
|
||||
switch (status_raw) {
|
||||
case static_cast<std::int32_t>(ModuleStatus::Online):
|
||||
return ModuleStatus::Online;
|
||||
case static_cast<std::int32_t>(ModuleStatus::Offline):
|
||||
return ModuleStatus::Offline;
|
||||
case static_cast<std::int32_t>(ModuleStatus::StreamReset):
|
||||
return ModuleStatus::StreamReset;
|
||||
default:
|
||||
return std::unexpected(ParseError::InvalidModuleStatus);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
std::string_view to_string(ParseError error) {
|
||||
switch (error) {
|
||||
case ParseError::BufferTooSmall:
|
||||
return "buffer too small";
|
||||
case ParseError::InvalidSize:
|
||||
return "invalid message size";
|
||||
case ParseError::InvalidMagic:
|
||||
return "invalid magic";
|
||||
case ParseError::UnsupportedVersion:
|
||||
return "unsupported version";
|
||||
case ParseError::InvalidDepth:
|
||||
return "invalid depth";
|
||||
case ParseError::InvalidPixelFormat:
|
||||
return "invalid pixel format";
|
||||
case ParseError::InvalidModuleStatus:
|
||||
return "invalid module status";
|
||||
case ParseError::PayloadLengthMismatch:
|
||||
return "payload length mismatch";
|
||||
}
|
||||
return "unknown parse error";
|
||||
}
|
||||
|
||||
std::string_view to_string(SnapshotError error) {
|
||||
switch (error) {
|
||||
case SnapshotError::InvalidShmLayout:
|
||||
return "invalid shared memory layout";
|
||||
case SnapshotError::DestinationTooSmall:
|
||||
return "destination buffer too small";
|
||||
case SnapshotError::TornRead:
|
||||
return "torn read";
|
||||
}
|
||||
return "unknown snapshot error";
|
||||
}
|
||||
|
||||
std::expected<FrameMetadata, ParseError> parse_frame_metadata(std::span<const std::uint8_t> bytes) {
|
||||
if (bytes.size() < kFrameMetadataRequiredBytes) {
|
||||
return std::unexpected(ParseError::BufferTooSmall);
|
||||
}
|
||||
|
||||
FrameMetadata metadata{};
|
||||
std::copy_n(bytes.begin(), kFrameMetadataMagic.size(), metadata.magic.begin());
|
||||
if (metadata.magic != kFrameMetadataMagic) {
|
||||
return std::unexpected(ParseError::InvalidMagic);
|
||||
}
|
||||
|
||||
metadata.versions_major = bytes[kMetaVersionMajorOffset];
|
||||
metadata.versions_minor = bytes[kMetaVersionMinorOffset];
|
||||
if (!is_supported_version(metadata.versions_major, metadata.versions_minor)) {
|
||||
return std::unexpected(ParseError::UnsupportedVersion);
|
||||
}
|
||||
|
||||
metadata.frame_count = read_u32_le(bytes, kMetaFrameCountOffset);
|
||||
metadata.timestamp_ns = read_u64_le(bytes, kMetaTimestampOffset);
|
||||
|
||||
auto frame_info_bytes = bytes.subspan(kMetaFrameInfoOffset);
|
||||
auto depth = validate_depth(frame_info_bytes[kFrameInfoDepthOffset]);
|
||||
if (!depth) {
|
||||
return std::unexpected(depth.error());
|
||||
}
|
||||
auto pixel_format = validate_pixel_format(frame_info_bytes[kFrameInfoPixelFmtOffset]);
|
||||
if (!pixel_format) {
|
||||
return std::unexpected(pixel_format.error());
|
||||
}
|
||||
|
||||
metadata.info.width = read_u16_le(frame_info_bytes, kFrameInfoWidthOffset);
|
||||
metadata.info.height = read_u16_le(frame_info_bytes, kFrameInfoHeightOffset);
|
||||
metadata.info.channels = frame_info_bytes[kFrameInfoChannelsOffset];
|
||||
metadata.info.depth = *depth;
|
||||
metadata.info.pixel_format = *pixel_format;
|
||||
metadata.info.buffer_size = read_u32_le(frame_info_bytes, kFrameInfoBufferSizeOffset);
|
||||
|
||||
return metadata;
|
||||
}
|
||||
|
||||
std::expected<SyncMessage, ParseError> parse_sync_message(std::span<const std::uint8_t> bytes) {
|
||||
if (bytes.size() < kSyncMessageSize) {
|
||||
return std::unexpected(ParseError::BufferTooSmall);
|
||||
}
|
||||
if (bytes.size() != kSyncMessageSize) {
|
||||
return std::unexpected(ParseError::InvalidSize);
|
||||
}
|
||||
if (bytes[0] != kFrameTopicMagic) {
|
||||
return std::unexpected(ParseError::InvalidMagic);
|
||||
}
|
||||
if (!is_supported_version(bytes[2], bytes[3])) {
|
||||
return std::unexpected(ParseError::UnsupportedVersion);
|
||||
}
|
||||
|
||||
SyncMessage message{};
|
||||
message.versions_major = bytes[2];
|
||||
message.versions_minor = bytes[3];
|
||||
message.frame_count = read_u32_le(bytes, kSyncFrameCountOffset);
|
||||
message.timestamp_ns = read_u64_le(bytes, kSyncTimestampOffset);
|
||||
std::copy_n(bytes.begin() + kSyncLabelOffset, kLabelLenMax, message.label_bytes.begin());
|
||||
|
||||
return message;
|
||||
}
|
||||
|
||||
std::expected<ModuleStatusMessage, ParseError> parse_module_status_message(std::span<const std::uint8_t> bytes) {
|
||||
if (bytes.size() < kModuleStatusMessageSize) {
|
||||
return std::unexpected(ParseError::BufferTooSmall);
|
||||
}
|
||||
if (bytes.size() != kModuleStatusMessageSize) {
|
||||
return std::unexpected(ParseError::InvalidSize);
|
||||
}
|
||||
if (bytes[0] != kModuleStatusMagic) {
|
||||
return std::unexpected(ParseError::InvalidMagic);
|
||||
}
|
||||
if (!is_supported_version(bytes[2], bytes[3])) {
|
||||
return std::unexpected(ParseError::UnsupportedVersion);
|
||||
}
|
||||
|
||||
auto status = validate_module_status(read_i32_le(bytes, kModuleStatusCodeOffset));
|
||||
if (!status) {
|
||||
return std::unexpected(status.error());
|
||||
}
|
||||
|
||||
ModuleStatusMessage message{};
|
||||
message.versions_major = bytes[2];
|
||||
message.versions_minor = bytes[3];
|
||||
message.module_status = *status;
|
||||
std::copy_n(bytes.begin() + kModuleStatusLabelOffset, kLabelLenMax, message.label_bytes.begin());
|
||||
|
||||
return message;
|
||||
}
|
||||
|
||||
std::expected<ControlRequestMessage, ParseError> parse_control_request_message(std::span<const std::uint8_t> bytes) {
|
||||
if (bytes.size() < kControlRequestBaseSize) {
|
||||
return std::unexpected(ParseError::BufferTooSmall);
|
||||
}
|
||||
if (bytes[0] != kControlRequestMagic) {
|
||||
return std::unexpected(ParseError::InvalidMagic);
|
||||
}
|
||||
if (!is_supported_version(bytes[2], bytes[3])) {
|
||||
return std::unexpected(ParseError::UnsupportedVersion);
|
||||
}
|
||||
|
||||
const auto payload_size = static_cast<std::size_t>(read_u16_le(bytes, kControlReqLengthOffset));
|
||||
if (payload_size > bytes.size() - kControlReqPayloadOffset) {
|
||||
return std::unexpected(ParseError::PayloadLengthMismatch);
|
||||
}
|
||||
if (bytes.size() != kControlRequestBaseSize + payload_size) {
|
||||
return std::unexpected(ParseError::InvalidSize);
|
||||
}
|
||||
|
||||
ControlRequestMessage message{};
|
||||
message.versions_major = bytes[2];
|
||||
message.versions_minor = bytes[3];
|
||||
message.command_id = read_i32_le(bytes, kControlCommandOffset);
|
||||
std::copy_n(bytes.begin() + kControlReqLabelOffset, kLabelLenMax, message.label_bytes.begin());
|
||||
message.request_payload = bytes.subspan(kControlReqPayloadOffset, payload_size);
|
||||
|
||||
return message;
|
||||
}
|
||||
|
||||
std::expected<ControlResponseMessage, ParseError> parse_control_response_message(std::span<const std::uint8_t> bytes) {
|
||||
if (bytes.size() < kControlResponseBaseSize) {
|
||||
return std::unexpected(ParseError::BufferTooSmall);
|
||||
}
|
||||
if (bytes[0] != kControlResponseMagic) {
|
||||
return std::unexpected(ParseError::InvalidMagic);
|
||||
}
|
||||
if (!is_supported_version(bytes[2], bytes[3])) {
|
||||
return std::unexpected(ParseError::UnsupportedVersion);
|
||||
}
|
||||
|
||||
const auto payload_size = static_cast<std::size_t>(read_u16_le(bytes, kControlRespLengthOffset));
|
||||
if (payload_size > bytes.size() - kControlRespPayloadOffset) {
|
||||
return std::unexpected(ParseError::PayloadLengthMismatch);
|
||||
}
|
||||
if (bytes.size() != kControlResponseBaseSize + payload_size) {
|
||||
return std::unexpected(ParseError::InvalidSize);
|
||||
}
|
||||
|
||||
ControlResponseMessage message{};
|
||||
message.versions_major = bytes[2];
|
||||
message.versions_minor = bytes[3];
|
||||
message.command_id = read_i32_le(bytes, kControlCommandOffset);
|
||||
message.response_code = read_i32_le(bytes, kControlRespCodeOffset);
|
||||
std::copy_n(bytes.begin() + kControlRespLabelOffset, kLabelLenMax, message.label_bytes.begin());
|
||||
message.response_payload = bytes.subspan(kControlRespPayloadOffset, payload_size);
|
||||
|
||||
return message;
|
||||
}
|
||||
|
||||
std::expected<ValidatedShmView, ParseError> validate_shm_region(std::span<const std::uint8_t> shm_region) {
|
||||
if (shm_region.size() < kShmPayloadOffset) {
|
||||
return std::unexpected(ParseError::BufferTooSmall);
|
||||
}
|
||||
|
||||
auto metadata_result = parse_frame_metadata(shm_region);
|
||||
if (!metadata_result) {
|
||||
return std::unexpected(metadata_result.error());
|
||||
}
|
||||
|
||||
const auto payload_size = static_cast<std::size_t>(metadata_result->info.buffer_size);
|
||||
if (payload_size > std::numeric_limits<std::size_t>::max() - kShmPayloadOffset) {
|
||||
return std::unexpected(ParseError::InvalidSize);
|
||||
}
|
||||
if (payload_size > shm_region.size() - kShmPayloadOffset) {
|
||||
return std::unexpected(ParseError::InvalidSize);
|
||||
}
|
||||
|
||||
return ValidatedShmView{
|
||||
.metadata = *metadata_result,
|
||||
.payload = shm_region.subspan(kShmPayloadOffset, payload_size)};
|
||||
}
|
||||
|
||||
std::expected<CoherentSnapshot, SnapshotError> read_coherent_snapshot(
|
||||
std::span<const std::uint8_t> shm_region,
|
||||
std::span<std::uint8_t> destination,
|
||||
const SnapshotReadHook &before_second_metadata_read) {
|
||||
auto first = validate_shm_region(shm_region);
|
||||
if (!first) {
|
||||
return std::unexpected(SnapshotError::InvalidShmLayout);
|
||||
}
|
||||
|
||||
if (destination.size() < first->payload.size()) {
|
||||
return std::unexpected(SnapshotError::DestinationTooSmall);
|
||||
}
|
||||
|
||||
std::copy(first->payload.begin(), first->payload.end(), destination.begin());
|
||||
|
||||
if (before_second_metadata_read) {
|
||||
before_second_metadata_read();
|
||||
}
|
||||
|
||||
auto second = validate_shm_region(shm_region);
|
||||
if (!second) {
|
||||
return std::unexpected(SnapshotError::InvalidShmLayout);
|
||||
}
|
||||
|
||||
if (first->metadata.frame_count != second->metadata.frame_count ||
|
||||
first->metadata.timestamp_ns != second->metadata.timestamp_ns) {
|
||||
return std::unexpected(SnapshotError::TornRead);
|
||||
}
|
||||
|
||||
return CoherentSnapshot{
|
||||
.metadata = first->metadata,
|
||||
.bytes_copied = first->payload.size()};
|
||||
}
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user