MARTe-Integrated-Components/Source/Components/Interfaces/DebugService/DebugCore.h

#ifndef DEBUGCORE_H
#define DEBUGCORE_H

#include "CompilerTypes.h"
#include "TypeDescriptor.h"
#include "StreamString.h"
#include <string.h> 

namespace MARTe {

// Break condition operators stored in DebugSignalInfo::breakOp
enum BreakOp {
    BREAK_OFF = 0,
    BREAK_GT  = 1,  // >
    BREAK_LT  = 2,  // <
    BREAK_EQ  = 3,  // ==
    BREAK_GEQ = 4,  // >=
    BREAK_LEQ = 5,  // <=
    BREAK_NEQ = 6   // !=
};

struct DebugSignalInfo {
    void* memoryAddress;
    TypeDescriptor type;
    StreamString name;
    uint8 numberOfDimensions;
    uint32 numberOfElements;
    volatile bool isTracing;
    volatile bool isForcing;
    uint8 forcedValue[1024];
    uint8 forcedMask[32];   // bit e set → element e is forced; supports up to 256 elements
    uint32 internalID;
    volatile uint32 decimationFactor;
    volatile uint32 decimationCounter;
    // Conditional break fields (zero-cost when breakOp == BREAK_OFF)
    volatile uint8 breakOp;         // BreakOp enum value
    float64 breakThreshold;         // comparison threshold
};

#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)

/**
 * @brief Ring buffer for high-frequency signal tracing.
 * @details New format per sample: [ID:4][Timestamp:8][Size:4][Data:N]
 */
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, uint64 timestamp, void* data, uint32 size) {
        uint32 packetSize = 4 + 8 + 4 + size; // ID + TS + Size + Data
        /* HI-9: use atomic loads for cross-thread index reads */
        uint32 read = __atomic_load_n(&readIndex, __ATOMIC_ACQUIRE);
        uint32 write = __atomic_load_n(&writeIndex, __ATOMIC_ACQUIRE);
        
        uint32 available = 0;
        if (read <= write) {
            available = bufferSize - (write - read) - 1;
        } else {
            available = read - write - 1;
        }

        if (available < packetSize) return false;

        uint32 tempWrite = write;
        WriteToBuffer(&tempWrite, &signalID, 4);
        WriteToBuffer(&tempWrite, &timestamp, 8);
        WriteToBuffer(&tempWrite, &size, 4);
        WriteToBuffer(&tempWrite, data, size);
        
        // HI-9: release store so data writes are visible before index update
        __atomic_store_n(&writeIndex, tempWrite, __ATOMIC_RELEASE);
        return true;
    }

    bool Pop(uint32 &signalID, uint64 &timestamp, void* dataBuffer, uint32 &size, uint32 maxSize) {
        /* HI-9: acquire-load writeIndex to see data written by Push */
        uint32 read = __atomic_load_n(&readIndex, __ATOMIC_ACQUIRE);
        uint32 write = __atomic_load_n(&writeIndex, __ATOMIC_ACQUIRE);
        if (read == write) return false;

        uint32 tempRead = read;
        uint32 tempId = 0;
        uint64 tempTs = 0;
        uint32 tempSize = 0;
        
        ReadFromBuffer(&tempRead, &tempId, 4);
        ReadFromBuffer(&tempRead, &tempTs, 8);
        ReadFromBuffer(&tempRead, &tempSize, 4);
        
        if (tempSize > maxSize) {
            // FIX #5: Skip only the current entry rather than discarding the
            // entire ring buffer.  tempRead is already past the 16-byte header;
            // advancing by tempSize lands at the start of the next entry.
            //
            // Safety fallback: if tempSize >= bufferSize the stored size field
            // is corrupt (it can never be that large).  In that case we cannot
            // locate the next entry safely, so fall back to discarding everything
            // to avoid reading garbage as sample headers on future Pop() calls.
            if (tempSize >= bufferSize) {
                __atomic_store_n(&readIndex, write, __ATOMIC_RELEASE);  // corrupt ring — discard all
            } else {
                __atomic_store_n(&readIndex, (tempRead + tempSize) % bufferSize, __ATOMIC_RELEASE);
            }
            return false;
        }

        ReadFromBuffer(&tempRead, dataBuffer, tempSize);
        
        signalID = tempId;
        timestamp = tempTs;
        size = tempSize;
        
        // HI-9: release-store readIndex after reading data
        __atomic_store_n(&readIndex, tempRead, __ATOMIC_RELEASE);
        return true;
    }

    uint32 Count() {
        uint32 read = __atomic_load_n(&readIndex, __ATOMIC_ACQUIRE);
        uint32 write = __atomic_load_n(&writeIndex, __ATOMIC_ACQUIRE);
        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) {
            memcpy(&buffer[current], src, count);
            *idx = (current + count) % bufferSize;
        } else {
            memcpy(&buffer[current], src, spaceToEnd);
            uint32 remaining = count - spaceToEnd;
            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) {
            memcpy(dst, &buffer[current], count);
            *idx = (current + count) % bufferSize;
        } else {
            memcpy(dst, &buffer[current], spaceToEnd);
            uint32 remaining = count - spaceToEnd;
            memcpy((uint8*)dst + spaceToEnd, &buffer[0], remaining);
            *idx = remaining;
        }
    }

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

}

#endif