vizionStreamer/src/SharedMemoryWriter.cpp

/*
 * 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);

    // Debug: Log every 60 frames
    static uint64_t debug_counter = 0;
    if (++debug_counter % 60 == 0) {
        std::cout << "[SHM] Frame #" << frame_counter_
                  << " written. write_seq=" << header->write_sequence.load()
                  << " size=" << size
                  << " first 4 bytes: ";
        for (int i = 0; i < 4 && i < size; i++) {
            printf("%02x ", frame_data[i]);
        }
        std::cout << std::endl;
    }

    // 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_);
}