add SharedMemoryWriter support

src/SharedMemoryWriter.cpp

@@ -0,0 +1,158 @@
+/*
+ * VizionStreamer - Shared Memory Writer Implementation
+ * Copyright (c) 2025 Maik Jurischka
+ *
+ * Licensed under CC BY-NC-SA 4.0
+ * https://creativecommons.org/licenses/by-nc-sa/4.0/
+ */
+
+#include "vizionstreamer/SharedMemoryWriter.h"
+#include <sys/mman.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <cstring>
+#include <iostream>
+#include <chrono>
+#include <cerrno>
+
+SharedMemoryWriter::SharedMemoryWriter(std::string name)
+    : shm_name_(std::move(name))
+    , shm_fd_(-1)
+    , shm_ptr_(nullptr)
+    , buffer_size_(0)
+    , frame_counter_(0) {
+}
+
+SharedMemoryWriter::~SharedMemoryWriter() {
+    destroy();
+}
+
+bool SharedMemoryWriter::create(const size_t frame_buffer_size) {
+    if (shm_fd_ >= 0) {
+        std::cerr << "Shared memory already created" << std::endl;
+        return false;
+    }
+
+    // Calculate total size (header + frame buffer)
+    buffer_size_ = sizeof(SharedMemoryHeader) + frame_buffer_size;
+
+    // Create shared memory object
+    shm_fd_ = shm_open(shm_name_.c_str(), O_CREAT | O_RDWR, 0666);
+    if (shm_fd_ < 0) {
+        std::cerr << "Failed to create shared memory '" << shm_name_
+                  << "': " << strerror(errno) << std::endl;
+        return false;
+    }
+
+    // Set size
+    if (ftruncate(shm_fd_, static_cast<off_t>(buffer_size_)) != 0) {
+        std::cerr << "Failed to set shared memory size: " << strerror(errno) << std::endl;
+        close(shm_fd_);
+        shm_fd_ = -1;
+        shm_unlink(shm_name_.c_str());
+        return false;
+    }
+
+    // Map memory
+    shm_ptr_ = mmap(nullptr, buffer_size_, PROT_READ | PROT_WRITE, MAP_SHARED, shm_fd_, 0);
+    if (shm_ptr_ == MAP_FAILED) {
+        std::cerr << "Failed to map shared memory: " << strerror(errno) << std::endl;
+        close(shm_fd_);
+        shm_fd_ = -1;
+        shm_unlink(shm_name_.c_str());
+        shm_ptr_ = nullptr;
+        return false;
+    }
+
+    // Initialize header
+    SharedMemoryHeader* header = getHeader();
+    header->magic = 0x56495A4E; // "VIZN"
+    header->width = 0;
+    header->height = 0;
+    header->format = 0;
+    header->data_size = 0;
+    header->timestamp_ns = 0;
+    header->frame_sequence = 0;
+    header->write_sequence.store(0, std::memory_order_release);
+    memset(header->format_str, 0, sizeof(header->format_str));
+    memset(header->reserved, 0, sizeof(header->reserved));
+
+    std::cout << "Shared memory created: " << shm_name_
+              << " (" << buffer_size_ << " bytes)" << std::endl;
+
+    return true;
+}
+
+bool SharedMemoryWriter::writeFrame(const uint8_t* data, const size_t size,
+                                   const int width, const int height,
+                                   const std::string& format,
+                                   const uint64_t timestamp_ns) {
+    if (!shm_ptr_ || shm_fd_ < 0) {
+        return false;
+    }
+
+    // Check size
+    if (size + sizeof(SharedMemoryHeader) > buffer_size_) {
+        std::cerr << "Frame too large for shared memory buffer (frame: "
+                  << size << " bytes, available: "
+                  << (buffer_size_ - sizeof(SharedMemoryHeader)) << " bytes)" << std::endl;
+        return false;
+    }
+
+    SharedMemoryHeader* header = getHeader();
+    auto* frame_data = static_cast<uint8_t*>(shm_ptr_) + sizeof(SharedMemoryHeader);
+
+    // Lock-free write protocol:
+    // 1. Increment sequence (odd = writing in progress)
+    header->write_sequence.fetch_add(1, std::memory_order_acquire);
+
+    // 2. Update header metadata
+    header->width = width;
+    header->height = height;
+    header->data_size = size;
+    header->frame_sequence = ++frame_counter_;
+
+    // Set timestamp (use provided or generate current)
+    if (timestamp_ns > 0) {
+        header->timestamp_ns = timestamp_ns;
+    } else {
+        header->timestamp_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
+            std::chrono::system_clock::now().time_since_epoch()).count();
+    }
+
+    // Copy format string
+    strncpy(header->format_str, format.c_str(), sizeof(header->format_str) - 1);
+    header->format_str[sizeof(header->format_str) - 1] = '\0';
+
+    // 3. Copy frame data
+    memcpy(frame_data, data, size);
+
+    // 4. Increment sequence (even = write complete)
+    header->write_sequence.fetch_add(1, std::memory_order_release);
+
+    return true;
+}
+
+void SharedMemoryWriter::destroy() {
+    // Unmap memory
+    if (shm_ptr_ != nullptr && shm_ptr_ != MAP_FAILED) {
+        munmap(shm_ptr_, buffer_size_);
+        shm_ptr_ = nullptr;
+    }
+
+    // Close and unlink shared memory
+    if (shm_fd_ >= 0) {
+        close(shm_fd_);
+        shm_unlink(shm_name_.c_str());
+        std::cout << "Shared memory destroyed: " << shm_name_ << std::endl;
+        shm_fd_ = -1;
+    }
+
+    buffer_size_ = 0;
+    frame_counter_ = 0;
+}
+
+SharedMemoryHeader* SharedMemoryWriter::getHeader() {
+    return static_cast<SharedMemoryHeader*>(shm_ptr_);
+}