marte-debug/Headers/DebugCore.h

#ifndef DEBUGCORE_H
#define DEBUGCORE_H

#include "CompilerTypes.h"
#include "TypeDescriptor.h"
#include "StreamString.h"
#include <cstring> // For memcpy

namespace MARTe {

struct DebugSignalInfo {
    void* memoryAddress;
    TypeDescriptor type;
    StreamString name;
    volatile bool isTracing;
    volatile bool isForcing;
    uint8 forcedValue[1024]; 
    uint32 internalID;
    volatile uint32 decimationFactor;
    volatile uint32 decimationCounter;
};

#pragma pack(push, 1)
struct TraceHeader {
    uint32 magic;      // 0xDA7A57AD
    uint32 seq;        // Sequence number
    uint64 timestamp;  // HighRes timestamp
    uint32 count;      // Number of samples in payload
};
#pragma pack(pop)

class TraceRingBuffer {
public:
    TraceRingBuffer() {
        bufferSize = 0;
        buffer = NULL_PTR(uint8*);
        readIndex = 0;
        writeIndex = 0;
    }

    ~TraceRingBuffer() {
        if (buffer != NULL_PTR(uint8*)) {
            delete[] buffer;
        }
    }

    bool Init(uint32 size) {
        if (buffer != NULL_PTR(uint8*)) {
            delete[] buffer;
        }
        bufferSize = size;
        buffer = new uint8[bufferSize];
        readIndex = 0;
        writeIndex = 0;
        return (buffer != NULL_PTR(uint8*));
    }

    bool Push(uint32 signalID, void* data, uint32 size) {
        uint32 packetSize = 4 + 4 + size;
        uint32 read = readIndex; 
        uint32 write = writeIndex;
        
        // Calculate available space
        uint32 available = 0;
        if (read <= write) {
            available = bufferSize - (write - read) - 1;
        } else {
            available = read - write - 1;
        }

        if (available < packetSize) return false;

        // Use temporary write index to ensure atomic update
        uint32 tempWrite = write;
        WriteToBuffer(&tempWrite, &signalID, 4);
        WriteToBuffer(&tempWrite, &size, 4);
        WriteToBuffer(&tempWrite, data, size);
        
        // Memory Barrier to ensure data is visible before index update
        // __sync_synchronize(); 
        
        // Final atomic update
        writeIndex = tempWrite;
        return true;
    }

    bool Pop(uint32 &signalID, void* dataBuffer, uint32 &size, uint32 maxSize) {
        uint32 read = readIndex;
        uint32 write = writeIndex;
        if (read == write) return false;

        uint32 tempRead = read;
        uint32 tempId = 0;
        uint32 tempSize = 0;
        
        // Peek header
        ReadFromBuffer(&tempRead, &tempId, 4);
        ReadFromBuffer(&tempRead, &tempSize, 4);
        
        if (tempSize > maxSize) {
            // Error case: drop data up to writeIndex (resync)
            readIndex = write;
            return false;
        }

        ReadFromBuffer(&tempRead, dataBuffer, tempSize);
        
        signalID = tempId;
        size = tempSize;
        
        // Memory Barrier
        // __sync_synchronize();

        readIndex = tempRead;
        return true;
    }

    uint32 Count() {
        uint32 read = readIndex;
        uint32 write = writeIndex;
        if (write >= read) return write - read;
        return bufferSize - (read - write);
    }

private:
    void WriteToBuffer(uint32 *idx, void* src, uint32 count) {
        uint32 current = *idx;
        uint32 spaceToEnd = bufferSize - current;
        
        if (count <= spaceToEnd) {
            std::memcpy(&buffer[current], src, count);
            *idx = (current + count) % bufferSize;
        } else {
            std::memcpy(&buffer[current], src, spaceToEnd);
            uint32 remaining = count - spaceToEnd;
            std::memcpy(&buffer[0], (uint8*)src + spaceToEnd, remaining);
            *idx = remaining;
        }
    }

    void ReadFromBuffer(uint32 *idx, void* dst, uint32 count) {
        uint32 current = *idx;
        uint32 spaceToEnd = bufferSize - current;
        
        if (count <= spaceToEnd) {
            std::memcpy(dst, &buffer[current], count);
            *idx = (current + count) % bufferSize;
        } else {
            std::memcpy(dst, &buffer[current], spaceToEnd);
            uint32 remaining = count - spaceToEnd;
            std::memcpy((uint8*)dst + spaceToEnd, &buffer[0], remaining);
            *idx = remaining;
        }
    }

    volatile uint32 readIndex;
    volatile uint32 writeIndex;
    uint32 bufferSize;
    uint8 *buffer;
};

}

#endif