Project fixes and correct MARTe style makefile and source structure

Source/Components/Interfaces/DebugService/DebugBrokerWrapper.h

@@ -0,0 +1,436 @@
+#ifndef DEBUGBROKERWRAPPER_H
+#define DEBUGBROKERWRAPPER_H
+
+#include "BrokerI.h"
+#include "DataSourceI.h"
+#include "DebugService.h"
+#include "FastPollingMutexSem.h"
+#include "HighResolutionTimer.h"
+#include "MemoryMapBroker.h"
+#include "ObjectBuilder.h"
+#include "ObjectRegistryDatabase.h"
+#include "Vec.h"
+
+// Original broker headers
+#include "MemoryMapAsyncOutputBroker.h"
+#include "MemoryMapAsyncTriggerOutputBroker.h"
+#include "MemoryMapInputBroker.h"
+#include "MemoryMapInterpolatedInputBroker.h"
+#include "MemoryMapMultiBufferInputBroker.h"
+#include "MemoryMapMultiBufferOutputBroker.h"
+#include "MemoryMapOutputBroker.h"
+#include "MemoryMapSynchronisedInputBroker.h"
+#include "MemoryMapSynchronisedMultiBufferInputBroker.h"
+#include "MemoryMapSynchronisedMultiBufferOutputBroker.h"
+#include "MemoryMapSynchronisedOutputBroker.h"
+
+namespace MARTe {
+
+/**
+ * @brief Helper for optimized signal processing within brokers.
+ */
+class DebugBrokerHelper {
+public:
+  static void Process(DebugService *service,
+                      DebugSignalInfo **signalInfoPointers,
+                      Vec<uint32> &activeIndices, Vec<uint32> &activeSizes,
+                      FastPollingMutexSem &activeMutex) {
+    if (service == NULL_PTR(DebugService *))
+      return;
+
+    // Re-establish break logic
+    while (service->IsPaused()) {
+      Sleep::MSec(10);
+    }
+
+    activeMutex.FastLock();
+    uint32 n = activeIndices.Size();
+    if (n > 0 && signalInfoPointers != NULL_PTR(DebugSignalInfo **)) {
+      // Capture timestamp ONCE per broker cycle for lowest impact
+      uint64 ts = (uint64)((float64)HighResolutionTimer::Counter() *
+                           HighResolutionTimer::Period() * 1000000.0);
+
+      for (uint32 i = 0; i < n; i++) {
+        uint32 idx = activeIndices[i];
+        uint32 size = activeSizes[i];
+        DebugSignalInfo *s = signalInfoPointers[idx];
+        if (s != NULL_PTR(DebugSignalInfo *)) {
+          service->ProcessSignal(s, size, ts);
+        }
+      }
+    }
+    activeMutex.FastUnLock();
+  }
+
+  // Pass numCopies explicitly so we can mock it
+  static void
+  InitSignals(BrokerI *broker, DataSourceI &dataSourceIn,
+              DebugService *&service, DebugSignalInfo **&signalInfoPointers,
+              uint32 numCopies, MemoryMapBrokerCopyTableEntry *copyTable,
+              const char8 *functionName, SignalDirection direction,
+              volatile bool *anyActiveFlag, Vec<uint32> *activeIndices,
+              Vec<uint32> *activeSizes, FastPollingMutexSem *activeMutex) {
+    if (numCopies > 0) {
+      signalInfoPointers = new DebugSignalInfo *[numCopies];
+      for (uint32 i = 0; i < numCopies; i++)
+        signalInfoPointers[i] = NULL_PTR(DebugSignalInfo *);
+    }
+
+    ReferenceContainer *root = ObjectRegistryDatabase::Instance();
+    Reference serviceRef = root->Find("DebugService");
+    if (serviceRef.IsValid()) {
+      service = dynamic_cast<DebugService *>(serviceRef.operator->());
+    }
+
+    if (service && (copyTable != NULL_PTR(MemoryMapBrokerCopyTableEntry *))) {
+
+      StreamString dsPath;
+      DebugService::GetFullObjectName(dataSourceIn, dsPath);
+      fprintf(stderr, ">> %s broker for %s [%d]\n",
+              direction == InputSignals ? "Input" : "Output", dsPath.Buffer(),
+              numCopies);
+      MemoryMapBroker *mmb = dynamic_cast<MemoryMapBroker *>(broker);
+      if (mmb == NULL_PTR(MemoryMapBroker *)) {
+        fprintf(stderr, ">> Impossible to get broker pointer!!\n");
+      }
+
+      for (uint32 i = 0; i < numCopies; i++) {
+        void *addr = copyTable[i].dataSourcePointer;
+        TypeDescriptor type = copyTable[i].type;
+
+        uint32 dsIdx = i;
+        if (mmb != NULL_PTR(MemoryMapBroker *)) {
+          dsIdx = mmb->GetDSCopySignalIndex(i);
+        }
+
+        StreamString signalName;
+        if (!dataSourceIn.GetSignalName(dsIdx, signalName))
+          signalName = "Unknown";
+        fprintf(stderr, ">> registering %s.%s [%p]\n", dsPath.Buffer(),
+                signalName.Buffer(), mmb);
+
+        // Register canonical name
+        StreamString dsFullName;
+        dsFullName.Printf("%s.%s", dsPath.Buffer(), signalName.Buffer());
+        service->RegisterSignal(addr, type, dsFullName.Buffer());
+
+        // Register alias
+        if (functionName != NULL_PTR(const char8 *)) {
+          StreamString gamFullName;
+          const char8 *dirStr =
+              (direction == InputSignals) ? "InputSignals" : "OutputSignals";
+          const char8 *dirStrShort = (direction == InputSignals) ? "In" : "Out";
+
+          // Try to find the GAM with different path variations
+          Reference gamRef =
+              ObjectRegistryDatabase::Instance()->Find(functionName);
+          if (!gamRef.IsValid()) {
+            // Try with "App.Functions." prefix
+            StreamString tryPath;
+            tryPath.Printf("App.Functions.%s", functionName);
+            gamRef = ObjectRegistryDatabase::Instance()->Find(tryPath.Buffer());
+          }
+          if (!gamRef.IsValid()) {
+            // Try with "Functions." prefix
+            StreamString tryPath;
+            tryPath.Printf("Functions.%s", functionName);
+            gamRef = ObjectRegistryDatabase::Instance()->Find(tryPath.Buffer());
+          }
+
+          if (gamRef.IsValid()) {
+            StreamString absGamPath;
+            DebugService::GetFullObjectName(*(gamRef.operator->()), absGamPath);
+            // Register full path (InputSignals/OutputSignals)
+            // gamFullName.fPrintf(stderr, "%s.%s.%s", absGamPath.Buffer(),
+            // dirStr, signalName.Buffer()); signalInfoPointers[i] =
+            // service->RegisterSignal(addr, type, gamFullName.Buffer()); Also
+            // register short path (In/Out) for GUI compatibility
+            gamFullName.Printf("%s.%s.%s", absGamPath.Buffer(), dirStrShort,
+                               signalName.Buffer());
+            signalInfoPointers[i] =
+                service->RegisterSignal(addr, type, gamFullName.Buffer());
+          } else {
+            // Fallback to short name
+            // gamFullName.fPrintf(stderr, "%s.%s.%s", functionName, dirStr,
+            // signalName.Buffer()); signalInfoPointers[i] =
+            // service->RegisterSignal(addr, type, gamFullName.Buffer()); Also
+            // register short form
+            gamFullName.Printf("%s.%s.%s", functionName, dirStrShort,
+                               signalName.Buffer());
+            signalInfoPointers[i] =
+                service->RegisterSignal(addr, type, gamFullName.Buffer());
+          }
+        } else {
+          signalInfoPointers[i] =
+              service->RegisterSignal(addr, type, dsFullName.Buffer());
+        }
+      }
+
+      // Register broker in DebugService for optimized control
+      service->RegisterBroker(signalInfoPointers, numCopies, mmb, anyActiveFlag,
+                              activeIndices, activeSizes, activeMutex);
+    }
+  }
+};
+
+/**
+ * @brief Template class to instrument any MARTe2 Broker.
+ */
+template <typename BaseClass> class DebugBrokerWrapper : public BaseClass {
+public:
+  DebugBrokerWrapper() : BaseClass() {
+    service = NULL_PTR(DebugService *);
+    signalInfoPointers = NULL_PTR(DebugSignalInfo **);
+    numSignals = 0;
+    anyActive = false;
+  }
+
+  virtual ~DebugBrokerWrapper() {
+    if (signalInfoPointers)
+      delete[] signalInfoPointers;
+  }
+
+  virtual bool Execute() {
+    bool ret = BaseClass::Execute();
+    if (ret && (anyActive || (service && service->IsPaused()))) {
+      DebugBrokerHelper::Process(service, signalInfoPointers, activeIndices,
+                                 activeSizes, activeMutex);
+    }
+    return ret;
+  }
+
+  virtual bool Init(SignalDirection direction, DataSourceI &ds,
+                    const char8 *const name, void *gamMem) {
+    bool ret = BaseClass::Init(direction, ds, name, gamMem);
+    fprintf(stderr, ">> INIT BROKER %s %s\n", name,
+            direction == InputSignals ? "In" : "Out");
+    if (ret) {
+      numSignals = this->GetNumberOfCopies();
+      DebugBrokerHelper::InitSignals(this, ds, service, signalInfoPointers,
+                                     numSignals, this->copyTable, name,
+                                     direction, &anyActive, &activeIndices,
+                                     &activeSizes, &activeMutex);
+    }
+    return ret;
+  }
+
+  virtual bool Init(SignalDirection direction, DataSourceI &ds,
+                    const char8 *const name, void *gamMem, const bool optim) {
+    bool ret = BaseClass::Init(direction, ds, name, gamMem, false);
+    fprintf(stderr, ">> INIT optimized BROKER %s %s\n", name,
+            direction == InputSignals ? "In" : "Out");
+    if (ret) {
+      numSignals = this->GetNumberOfCopies();
+      DebugBrokerHelper::InitSignals(this, ds, service, signalInfoPointers,
+                                     numSignals, this->copyTable, name,
+                                     direction, &anyActive, &activeIndices,
+                                     &activeSizes, &activeMutex);
+    }
+    return ret;
+  }
+
+  DebugService *service;
+  DebugSignalInfo **signalInfoPointers;
+  uint32 numSignals;
+  volatile bool anyActive;
+  Vec<uint32> activeIndices;
+  Vec<uint32> activeSizes;
+  FastPollingMutexSem activeMutex;
+};
+
+template <typename BaseClass>
+class DebugBrokerWrapperNoOptim : public BaseClass {
+public:
+  DebugBrokerWrapperNoOptim() : BaseClass() {
+    service = NULL_PTR(DebugService *);
+    signalInfoPointers = NULL_PTR(DebugSignalInfo **);
+    numSignals = 0;
+    anyActive = false;
+  }
+
+  virtual ~DebugBrokerWrapperNoOptim() {
+    if (signalInfoPointers)
+      delete[] signalInfoPointers;
+  }
+
+  virtual bool Execute() {
+    bool ret = BaseClass::Execute();
+    if (ret && (anyActive || (service && service->IsPaused()))) {
+      DebugBrokerHelper::Process(service, signalInfoPointers, activeIndices,
+                                 activeSizes, activeMutex);
+    }
+    return ret;
+  }
+
+  virtual bool Init(SignalDirection direction, DataSourceI &ds,
+                    const char8 *const name, void *gamMem) {
+    bool ret = BaseClass::Init(direction, ds, name, gamMem);
+    if (ret) {
+      numSignals = this->GetNumberOfCopies();
+      DebugBrokerHelper::InitSignals(this, ds, service, signalInfoPointers,
+                                     numSignals, this->copyTable, name,
+                                     direction, &anyActive, &activeIndices,
+                                     &activeSizes, &activeMutex);
+    }
+    return ret;
+  }
+
+  DebugService *service;
+  DebugSignalInfo **signalInfoPointers;
+  uint32 numSignals;
+  volatile bool anyActive;
+  Vec<uint32> activeIndices;
+  Vec<uint32> activeSizes;
+  FastPollingMutexSem activeMutex;
+};
+
+class DebugMemoryMapAsyncOutputBroker : public MemoryMapAsyncOutputBroker {
+public:
+  DebugMemoryMapAsyncOutputBroker() : MemoryMapAsyncOutputBroker() {
+    service = NULL_PTR(DebugService *);
+    signalInfoPointers = NULL_PTR(DebugSignalInfo **);
+    numSignals = 0;
+    anyActive = false;
+  }
+  virtual ~DebugMemoryMapAsyncOutputBroker() {
+    if (signalInfoPointers)
+      delete[] signalInfoPointers;
+  }
+  virtual bool Execute() {
+    bool ret = MemoryMapAsyncOutputBroker::Execute();
+    if (ret && (anyActive || (service && service->IsPaused()))) {
+      DebugBrokerHelper::Process(service, signalInfoPointers, activeIndices,
+                                 activeSizes, activeMutex);
+    }
+    return ret;
+  }
+  virtual bool InitWithBufferParameters(const SignalDirection direction,
+                                        DataSourceI &dataSourceIn,
+                                        const char8 *const functionName,
+                                        void *const gamMemoryAddress,
+                                        const uint32 numberOfBuffersIn,
+                                        const ProcessorType &cpuMaskIn,
+                                        const uint32 stackSizeIn) {
+    bool ret = MemoryMapAsyncOutputBroker::InitWithBufferParameters(
+        direction, dataSourceIn, functionName, gamMemoryAddress,
+        numberOfBuffersIn, cpuMaskIn, stackSizeIn);
+    if (ret) {
+      numSignals = this->GetNumberOfCopies();
+      DebugBrokerHelper::InitSignals(
+          this, dataSourceIn, service, signalInfoPointers, numSignals,
+          this->copyTable, functionName, direction, &anyActive, &activeIndices,
+          &activeSizes, &activeMutex);
+    }
+    return ret;
+  }
+  DebugService *service;
+  DebugSignalInfo **signalInfoPointers;
+  uint32 numSignals;
+  volatile bool anyActive;
+  Vec<uint32> activeIndices;
+  Vec<uint32> activeSizes;
+  FastPollingMutexSem activeMutex;
+};
+
+class DebugMemoryMapAsyncTriggerOutputBroker
+    : public MemoryMapAsyncTriggerOutputBroker {
+public:
+  DebugMemoryMapAsyncTriggerOutputBroker()
+      : MemoryMapAsyncTriggerOutputBroker() {
+    service = NULL_PTR(DebugService *);
+    signalInfoPointers = NULL_PTR(DebugSignalInfo **);
+    numSignals = 0;
+    anyActive = false;
+  }
+  virtual ~DebugMemoryMapAsyncTriggerOutputBroker() {
+    if (signalInfoPointers)
+      delete[] signalInfoPointers;
+  }
+  virtual bool Execute() {
+    bool ret = MemoryMapAsyncTriggerOutputBroker::Execute();
+    if (ret && (anyActive || (service && service->IsPaused()))) {
+      DebugBrokerHelper::Process(service, signalInfoPointers, activeIndices,
+                                 activeSizes, activeMutex);
+    }
+    return ret;
+  }
+  virtual bool InitWithTriggerParameters(
+      const SignalDirection direction, DataSourceI &dataSourceIn,
+      const char8 *const functionName, void *const gamMemoryAddress,
+      const uint32 numberOfBuffersIn, const uint32 preTriggerBuffersIn,
+      const uint32 postTriggerBuffersIn, const ProcessorType &cpuMaskIn,
+      const uint32 stackSizeIn) {
+    bool ret = MemoryMapAsyncTriggerOutputBroker::InitWithTriggerParameters(
+        direction, dataSourceIn, functionName, gamMemoryAddress,
+        numberOfBuffersIn, preTriggerBuffersIn, postTriggerBuffersIn, cpuMaskIn,
+        stackSizeIn);
+    if (ret) {
+      numSignals = this->GetNumberOfCopies();
+      DebugBrokerHelper::InitSignals(
+          this, dataSourceIn, service, signalInfoPointers, numSignals,
+          this->copyTable, functionName, direction, &anyActive, &activeIndices,
+          &activeSizes, &activeMutex);
+    }
+    return ret;
+  }
+  DebugService *service;
+  DebugSignalInfo **signalInfoPointers;
+  uint32 numSignals;
+  volatile bool anyActive;
+  Vec<uint32> activeIndices;
+  Vec<uint32> activeSizes;
+  FastPollingMutexSem activeMutex;
+};
+
+template <typename T> class DebugBrokerBuilder : public ObjectBuilder {
+public:
+  virtual Object *Build(HeapI *const heap) const { return new (heap) T(); }
+};
+
+typedef DebugBrokerWrapper<MemoryMapInputBroker> DebugMemoryMapInputBroker;
+// LCOV_EXCL_START
+typedef DebugBrokerWrapper<MemoryMapOutputBroker> DebugMemoryMapOutputBroker;
+typedef DebugBrokerWrapper<MemoryMapSynchronisedInputBroker>
+    DebugMemoryMapSynchronisedInputBroker;
+typedef DebugBrokerWrapper<MemoryMapSynchronisedOutputBroker>
+    DebugMemoryMapSynchronisedOutputBroker;
+typedef DebugBrokerWrapperNoOptim<MemoryMapInterpolatedInputBroker>
+    DebugMemoryMapInterpolatedInputBroker;
+typedef DebugBrokerWrapper<MemoryMapMultiBufferInputBroker>
+    DebugMemoryMapMultiBufferInputBroker;
+typedef DebugBrokerWrapper<MemoryMapMultiBufferOutputBroker>
+    DebugMemoryMapMultiBufferOutputBroker;
+typedef DebugBrokerWrapper<MemoryMapSynchronisedMultiBufferInputBroker>
+    DebugMemoryMapSynchronisedMultiBufferInputBroker;
+typedef DebugBrokerWrapper<MemoryMapSynchronisedMultiBufferOutputBroker>
+    DebugMemoryMapSynchronisedMultiBufferOutputBroker;
+// LCOV_EXCL_STOP
+
+typedef DebugBrokerBuilder<DebugMemoryMapInputBroker>
+    DebugMemoryMapInputBrokerBuilder;
+// LCOV_EXCL_START
+typedef DebugBrokerBuilder<DebugMemoryMapOutputBroker>
+    DebugMemoryMapOutputBrokerBuilder;
+typedef DebugBrokerBuilder<DebugMemoryMapSynchronisedInputBroker>
+    DebugMemoryMapSynchronisedInputBrokerBuilder;
+typedef DebugBrokerBuilder<DebugMemoryMapSynchronisedOutputBroker>
+    DebugMemoryMapSynchronisedOutputBrokerBuilder;
+typedef DebugBrokerBuilder<DebugMemoryMapInterpolatedInputBroker>
+    DebugMemoryMapInterpolatedInputBrokerBuilder;
+typedef DebugBrokerBuilder<DebugMemoryMapMultiBufferInputBroker>
+    DebugMemoryMapMultiBufferInputBrokerBuilder;
+typedef DebugBrokerBuilder<DebugMemoryMapMultiBufferOutputBroker>
+    DebugMemoryMapMultiBufferOutputBrokerBuilder;
+typedef DebugBrokerBuilder<DebugMemoryMapSynchronisedMultiBufferInputBroker>
+    DebugMemoryMapSynchronisedMultiBufferInputBrokerBuilder;
+typedef DebugBrokerBuilder<DebugMemoryMapSynchronisedMultiBufferOutputBroker>
+    DebugMemoryMapSynchronisedMultiBufferOutputBrokerBuilder;
+typedef DebugBrokerBuilder<DebugMemoryMapAsyncOutputBroker>
+    DebugMemoryMapAsyncOutputBrokerBuilder;
+typedef DebugBrokerBuilder<DebugMemoryMapAsyncTriggerOutputBroker>
+    DebugMemoryMapAsyncTriggerOutputBrokerBuilder;
+// LCOV_EXCL_STOP
+
+} // namespace MARTe
+
+#endif

Source/Components/Interfaces/DebugService/DebugCore.h

@@ -0,0 +1,159 @@
+#ifndef DEBUGCORE_H
+#define DEBUGCORE_H
+
+#include "CompilerTypes.h"
+#include "TypeDescriptor.h"
+#include "StreamString.h"
+#include <string.h> 
+
+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)
+
+/**
+ * @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
+        uint32 read = readIndex; 
+        uint32 write = writeIndex;
+        
+        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);
+        
+        writeIndex = tempWrite;
+        return true;
+    }
+
+    bool Pop(uint32 &signalID, uint64 &timestamp, void* dataBuffer, uint32 &size, uint32 maxSize) {
+        uint32 read = readIndex;
+        uint32 write = writeIndex;
+        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) {
+            readIndex = write;
+            return false;
+        }
+
+        ReadFromBuffer(&tempRead, dataBuffer, tempSize);
+        
+        signalID = tempId;
+        timestamp = tempTs;
+        size = tempSize;
+        
+        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) {
+            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

Source/Components/Interfaces/DebugService/DebugService.cpp

@@ -0,0 +1,972 @@
+#include "BasicTCPSocket.h"
+#include "ClassRegistryItem.h"
+#include "ConfigurationDatabase.h"
+#include "DebugBrokerWrapper.h"
+#include "DebugService.h"
+#include "GAM.h"
+#include "HighResolutionTimer.h"
+#include "ObjectBuilder.h"
+#include "ObjectRegistryDatabase.h"
+#include "StreamString.h"
+#include "TimeoutType.h"
+#include "TypeConversion.h"
+
+namespace MARTe {
+
+DebugService *DebugService::instance = (DebugService *)0;
+
+static void EscapeJson(const char8 *src, StreamString &dst) {
+  if (src == NULL_PTR(const char8 *))
+    return;
+  while (*src != '\0') {
+    if (*src == '"')
+      dst += "\\\"";
+    else if (*src == '\\')
+      dst += "\\\\";
+    else if (*src == '\n')
+      dst += "\\n";
+    else if (*src == '\r')
+      dst += "\\r";
+    else if (*src == '\t')
+      dst += "\\t";
+    else
+      dst += *src;
+    src++;
+  }
+}
+
+static bool SuffixMatch(const char8 *target, const char8 *pattern) {
+  uint32 tLen = StringHelper::Length(target);
+  uint32 pLen = StringHelper::Length(pattern);
+  if (pLen > tLen)
+    return false;
+  const char8 *suffix = target + (tLen - pLen);
+  if (StringHelper::Compare(suffix, pattern) == 0) {
+    if (tLen == pLen || *(suffix - 1) == '.')
+      return true;
+  }
+  return false;
+}
+
+static bool FindPathInContainer(ReferenceContainer *container,
+                                const Object *target, StreamString &path) {
+  if (container == NULL_PTR(ReferenceContainer *))
+    return false;
+  uint32 n = container->Size();
+  for (uint32 i = 0u; i < n; i++) {
+    Reference ref = container->Get(i);
+    if (ref.IsValid()) {
+      if (ref.operator->() == target) {
+        path = ref->GetName();
+        return true;
+      }
+      ReferenceContainer *sub =
+          dynamic_cast<ReferenceContainer *>(ref.operator->());
+      if (sub != NULL_PTR(ReferenceContainer *)) {
+        if (FindPathInContainer(sub, target, path)) {
+          StreamString full;
+          full.Printf("%s.%s", ref->GetName(), path.Buffer());
+          path = full;
+          return true;
+        }
+      }
+    }
+  }
+  return false;
+}
+
+CLASS_REGISTER(DebugService, "1.0")
+
+DebugService::DebugService()
+    : ReferenceContainer(), EmbeddedServiceMethodBinderI(),
+      binderServer(this, ServiceBinder::ServerType),
+      binderStreamer(this, ServiceBinder::StreamerType),
+      threadService(binderServer), streamerService(binderStreamer) {
+  controlPort = 0;
+  streamPort = 8081;
+  streamIP = "127.0.0.1";
+  isServer = false;
+  suppressTimeoutLogs = true;
+  isPaused = false;
+  activeClient = NULL_PTR(BasicTCPSocket *);
+}
+
+DebugService::~DebugService() {
+  if (instance == this) {
+    instance = NULL_PTR(DebugService *);
+  }
+  threadService.Stop();
+  streamerService.Stop();
+  tcpServer.Close();
+  udpSocket.Close();
+  if (activeClient != NULL_PTR(BasicTCPSocket *)) {
+    activeClient->Close();
+    delete activeClient;
+  }
+  for (uint32 i = 0; i < signals.Size(); i++) {
+    delete signals[i];
+  }
+}
+
+bool DebugService::Initialise(StructuredDataI &data) {
+  if (!ReferenceContainer::Initialise(data))
+    return false;
+
+  uint32 port = 0;
+  if (data.Read("ControlPort", port)) {
+    controlPort = (uint16)port;
+  } else {
+    (void)data.Read("TcpPort", port);
+    controlPort = (uint16)port;
+  }
+
+  if (controlPort > 0) {
+    isServer = true;
+    instance = this;
+  }
+
+  port = 8081;
+  if (data.Read("StreamPort", port)) {
+    streamPort = (uint16)port;
+  } else {
+    (void)data.Read("UdpPort", port);
+    streamPort = (uint16)port;
+  }
+  StreamString tempIP;
+  if (data.Read("StreamIP", tempIP)) {
+    streamIP = tempIP;
+  } else {
+    streamIP = "127.0.0.1";
+  }
+  uint32 suppress = 1;
+  if (data.Read("SuppressTimeoutLogs", suppress)) {
+    suppressTimeoutLogs = (suppress == 1);
+  }
+
+  // Try to capture full configuration autonomously if data is a
+  // ConfigurationDatabase
+  ConfigurationDatabase *cdb = dynamic_cast<ConfigurationDatabase *>(&data);
+  if (cdb != NULL_PTR(ConfigurationDatabase *)) {
+    // Save current position
+    StreamString currentPath;
+    // In MARTe2 ConfigurationDatabase there isn't a direct GetCurrentPath,
+    // but we can at least try to copy from root if we are at root.
+    // For now, we rely on explicit SetFullConfig or documentary injection.
+  }
+
+  // Copy local branch as fallback
+  (void)data.Copy(fullConfig);
+
+  if (isServer) {
+    if (!traceBuffer.Init(8 * 1024 * 1024))
+      return false;
+    PatchRegistry();
+    ConfigurationDatabase threadData;
+    threadData.Write("Timeout", (uint32)1000);
+    threadService.Initialise(threadData);
+    streamerService.Initialise(threadData);
+    if (!tcpServer.Open())
+      return false;
+    if (!tcpServer.Listen(controlPort))
+      return false;
+    if (!udpSocket.Open())
+      return false;
+    if (threadService.Start() != ErrorManagement::NoError)
+      return false;
+    if (streamerService.Start() != ErrorManagement::NoError)
+      return false;
+  }
+  return true;
+}
+
+void DebugService::SetFullConfig(ConfigurationDatabase &config) {
+  config.MoveToRoot();
+  config.Copy(fullConfig);
+}
+
+static void PatchItemInternal(const char8 *originalName,
+                              ObjectBuilder *debugBuilder) {
+  ClassRegistryItem *item =
+      ClassRegistryDatabase::Instance()->Find(originalName);
+  if (item != NULL_PTR(ClassRegistryItem *)) {
+    item->SetObjectBuilder(debugBuilder);
+  }
+}
+
+void DebugService::PatchRegistry() {
+  DebugMemoryMapInputBrokerBuilder *b1 = new DebugMemoryMapInputBrokerBuilder();
+  PatchItemInternal("MemoryMapInputBroker", b1);
+  DebugMemoryMapOutputBrokerBuilder *b2 =
+      new DebugMemoryMapOutputBrokerBuilder();
+  PatchItemInternal("MemoryMapOutputBroker", b2);
+  DebugMemoryMapSynchronisedInputBrokerBuilder *b3 =
+      new DebugMemoryMapSynchronisedInputBrokerBuilder();
+  PatchItemInternal("MemoryMapSynchronisedInputBroker", b3);
+  DebugMemoryMapSynchronisedOutputBrokerBuilder *b4 =
+      new DebugMemoryMapSynchronisedOutputBrokerBuilder();
+  PatchItemInternal("MemoryMapSynchronisedOutputBroker", b4);
+  DebugMemoryMapInterpolatedInputBrokerBuilder *b5 =
+      new DebugMemoryMapInterpolatedInputBrokerBuilder();
+  PatchItemInternal("MemoryMapInterpolatedInputBroker", b5);
+  DebugMemoryMapMultiBufferInputBrokerBuilder *b6 =
+      new DebugMemoryMapMultiBufferInputBrokerBuilder();
+  PatchItemInternal("MemoryMapMultiBufferInputBroker", b6);
+  DebugMemoryMapMultiBufferOutputBrokerBuilder *b7 =
+      new DebugMemoryMapMultiBufferOutputBrokerBuilder();
+  PatchItemInternal("MemoryMapMultiBufferOutputBroker", b7);
+  DebugMemoryMapSynchronisedMultiBufferInputBrokerBuilder *b8 =
+      new DebugMemoryMapSynchronisedMultiBufferInputBrokerBuilder();
+  PatchItemInternal("MemoryMapSynchronisedMultiBufferInputBroker", b8);
+  DebugMemoryMapSynchronisedMultiBufferOutputBrokerBuilder *b9 =
+      new DebugMemoryMapSynchronisedMultiBufferOutputBrokerBuilder();
+  PatchItemInternal("MemoryMapSynchronisedMultiBufferOutputBroker", b9);
+  DebugMemoryMapAsyncOutputBrokerBuilder *b10 =
+      new DebugMemoryMapAsyncOutputBrokerBuilder();
+  PatchItemInternal("MemoryMapAsyncOutputBroker", b10);
+  DebugMemoryMapAsyncTriggerOutputBrokerBuilder *b11 =
+      new DebugMemoryMapAsyncTriggerOutputBrokerBuilder();
+  PatchItemInternal("MemoryMapAsyncTriggerOutputBroker", b11);
+}
+
+DebugSignalInfo *DebugService::RegisterSignal(void *memoryAddress,
+                                              TypeDescriptor type,
+                                              const char8 *name) {
+  printf("<debug> registering: %s\n", name);
+  mutex.FastLock();
+  DebugSignalInfo *res = NULL_PTR(DebugSignalInfo *);
+  uint32 sigIdx = 0xFFFFFFFF;
+  for (uint32 i = 0; i < signals.Size(); i++) {
+    if (signals[i]->memoryAddress == memoryAddress) {
+      res = signals[i];
+      sigIdx = i;
+      break;
+    }
+  }
+  if (res == NULL_PTR(DebugSignalInfo *)) {
+    sigIdx = signals.Size();
+    res = new DebugSignalInfo();
+    res->memoryAddress = memoryAddress;
+    res->type = type;
+    res->name = name;
+    res->isTracing = false;
+    res->isForcing = false;
+    res->internalID = sigIdx;
+    res->decimationFactor = 1;
+    res->decimationCounter = 0;
+    signals.Push(res);
+  }
+  if (sigIdx != 0xFFFFFFFF) {
+    bool foundAlias = false;
+    for (uint32 i = 0; i < aliases.Size(); i++) {
+      if (aliases[i].name == name) {
+        foundAlias = true;
+        break;
+      }
+    }
+    if (!foundAlias) {
+      SignalAlias a;
+      a.name = name;
+      a.signalIndex = sigIdx;
+      aliases.Push(a);
+    }
+  }
+  mutex.FastUnLock();
+  return res;
+}
+
+void DebugService::ProcessSignal(DebugSignalInfo *signalInfo, uint32 size,
+                                 uint64 timestamp) {
+  if (signalInfo == NULL_PTR(DebugSignalInfo *))
+    return;
+  if (signalInfo->isForcing) {
+    memcpy(signalInfo->memoryAddress, signalInfo->forcedValue, size);
+  }
+  if (signalInfo->isTracing) {
+    if (signalInfo->decimationCounter == 0) {
+      traceBuffer.Push(signalInfo->internalID, timestamp,
+                       (uint8 *)signalInfo->memoryAddress, size);
+    }
+    signalInfo->decimationCounter =
+        (signalInfo->decimationCounter + 1) % signalInfo->decimationFactor;
+  }
+}
+
+void DebugService::RegisterBroker(DebugSignalInfo **signalPointers,
+                                  uint32 numSignals, MemoryMapBroker *broker,
+                                  volatile bool *anyActiveFlag,
+                                  Vec<uint32> *activeIndices,
+                                  Vec<uint32> *activeSizes,
+                                  FastPollingMutexSem *activeMutex) {
+  mutex.FastLock();
+  BrokerInfo b;
+  b.signalPointers = signalPointers;
+  b.numSignals = numSignals;
+  b.broker = broker;
+  b.anyActiveFlag = anyActiveFlag;
+  b.activeIndices = activeIndices;
+  b.activeSizes = activeSizes;
+  b.activeMutex = activeMutex;
+  brokers.Push(b);
+  mutex.FastUnLock();
+}
+
+void DebugService::UpdateBrokersActiveStatus() {
+  for (uint32 i = 0; i < brokers.Size(); i++) {
+    uint32 count = 0;
+    for (uint32 j = 0; j < brokers[i].numSignals; j++) {
+      DebugSignalInfo *s = brokers[i].signalPointers[j];
+      if (s != NULL_PTR(DebugSignalInfo *) && (s->isTracing || s->isForcing)) {
+        count++;
+      }
+    }
+
+    Vec<uint32> tempInd;
+    Vec<uint32> tempSizes;
+    for (uint32 j = 0; j < brokers[i].numSignals; j++) {
+      DebugSignalInfo *s = brokers[i].signalPointers[j];
+      if (s != NULL_PTR(DebugSignalInfo *) && (s->isTracing || s->isForcing)) {
+        tempInd.Push(j);
+        tempSizes.Push((brokers[i].broker != NULL_PTR(MemoryMapBroker *))
+                           ? brokers[i].broker->GetCopyByteSize(j)
+                           : 4);
+      }
+    }
+
+    if (brokers[i].activeMutex)
+      brokers[i].activeMutex->FastLock();
+
+    if (brokers[i].activeIndices)
+      *(brokers[i].activeIndices) = tempInd;
+    if (brokers[i].activeSizes)
+      *(brokers[i].activeSizes) = tempSizes;
+    if (brokers[i].anyActiveFlag)
+      *(brokers[i].anyActiveFlag) = (count > 0);
+
+    if (brokers[i].activeMutex)
+      brokers[i].activeMutex->FastUnLock();
+  }
+}
+
+ErrorManagement::ErrorType DebugService::Execute(ExecutionInfo &info) {
+  return ErrorManagement::FatalError;
+}
+
+ErrorManagement::ErrorType DebugService::Server(ExecutionInfo &info) {
+  if (info.GetStage() == ExecutionInfo::TerminationStage)
+    return ErrorManagement::NoError;
+  if (info.GetStage() == ExecutionInfo::StartupStage) {
+    serverThreadId = Threads::Id();
+    return ErrorManagement::NoError;
+  }
+  while (info.GetStage() == ExecutionInfo::MainStage) {
+    while (activeClient == NULL_PTR(BasicTCPSocket *)) {
+      BasicTCPSocket *newClient = tcpServer.WaitConnection(TTInfiniteWait);
+      if (newClient != NULL_PTR(BasicTCPSocket *)) {
+        // Single connection mode: disconnect any existing client first
+        activeClient = newClient;
+      }
+    }
+    // Single connection mode: only check client 0
+    {
+      if (activeClient != NULL_PTR(BasicTCPSocket *)) {
+        // Check if client is still connected
+        if (!activeClient->IsConnected()) {
+          activeClient->Close();
+          delete activeClient;
+          activeClient = NULL_PTR(BasicTCPSocket *);
+
+        } else {
+          char buffer[1024];
+          uint32 size = 1024;
+          if (activeClient->Read(buffer, size)) {
+            if (size > 0) {
+              // Process each line separately
+              char *ptr = buffer;
+              char *end = buffer + size;
+              while (ptr < end) {
+                char *newline = (char *)memchr(ptr, '\n', end - ptr);
+                if (!newline) {
+                  break;
+                }
+                *newline = '\0';
+                // Skip carriage return if present
+                if (newline > ptr && *(newline - 1) == '\r')
+                  *(newline - 1) = '\0';
+                StreamString command;
+                uint32 len = (uint32)(newline - ptr);
+                command.Write(ptr, len);
+                if (command.Size() > 0) {
+                  HandleCommand(command, activeClient);
+                }
+                ptr = newline + 1;
+              }
+            }
+          } else {
+            // // Read failed (client disconnected or error), clean up
+            if (activeClient != NULL_PTR(BasicTCPSocket *)) {
+              activeClient->Close();
+              delete activeClient;
+              activeClient = NULL_PTR(BasicTCPSocket *);
+            }
+          }
+        }
+      }
+    }
+    Sleep::MSec(10);
+  }
+  return ErrorManagement::NoError;
+}
+
+ErrorManagement::ErrorType DebugService::Streamer(ExecutionInfo &info) {
+  if (info.GetStage() == ExecutionInfo::TerminationStage)
+    return ErrorManagement::NoError;
+  if (info.GetStage() == ExecutionInfo::StartupStage) {
+    streamerThreadId = Threads::Id();
+    return ErrorManagement::NoError;
+  }
+  InternetHost dest(streamPort, streamIP.Buffer());
+  (void)udpSocket.SetDestination(dest);
+  uint8 packetBuffer[4096];
+  uint32 packetOffset = 0;
+  uint32 sequenceNumber = 0;
+  while (info.GetStage() == ExecutionInfo::MainStage) {
+    uint32 id, size;
+    uint64 ts;
+    uint8 sampleData[1024];
+    bool hasData = false;
+    while ((info.GetStage() == ExecutionInfo::MainStage) &&
+           traceBuffer.Pop(id, ts, sampleData, size, 1024)) {
+      hasData = true;
+      if (packetOffset == 0) {
+        TraceHeader header;
+        header.magic = 0xDA7A57AD;
+        header.seq = sequenceNumber++;
+        header.timestamp = HighResolutionTimer::Counter();
+        header.count = 0;
+        memcpy(packetBuffer, &header, sizeof(TraceHeader));
+        packetOffset = sizeof(TraceHeader);
+      }
+      if (packetOffset + 16 + size > 1400) {
+        uint32 toWrite = packetOffset;
+        (void)udpSocket.Write((char8 *)packetBuffer, toWrite);
+        TraceHeader header;
+        header.magic = 0xDA7A57AD;
+        header.seq = sequenceNumber++;
+        header.timestamp = HighResolutionTimer::Counter();
+        header.count = 0;
+        memcpy(packetBuffer, &header, sizeof(TraceHeader));
+        packetOffset = sizeof(TraceHeader);
+      }
+      memcpy(&packetBuffer[packetOffset], &id, 4);
+      memcpy(&packetBuffer[packetOffset + 4], &ts, 8);
+      memcpy(&packetBuffer[packetOffset + 12], &size, 4);
+      memcpy(&packetBuffer[packetOffset + 16], sampleData, size);
+      packetOffset += (16 + size);
+      ((TraceHeader *)packetBuffer)->count++;
+    }
+    if (packetOffset > 0) {
+      uint32 toWrite = packetOffset;
+      (void)udpSocket.Write((char8 *)packetBuffer, toWrite);
+      packetOffset = 0;
+    }
+    if (!hasData)
+      Sleep::MSec(1);
+  }
+  return ErrorManagement::NoError;
+}
+
+bool DebugService::GetFullObjectName(const Object &obj,
+                                     StreamString &fullPath) {
+  fullPath = "";
+  if (FindPathInContainer(ObjectRegistryDatabase::Instance(), &obj, fullPath)) {
+    return true;
+  }
+  const char8 *name = obj.GetName();
+  if (name != NULL_PTR(const char8 *))
+    fullPath = name;
+  return true;
+}
+
+void DebugService::HandleCommand(StreamString cmd, BasicTCPSocket *client) {
+  StreamString token;
+  cmd.Seek(0);
+  char8 term;
+  const char8 *delims = " \r\n";
+  if (cmd.GetToken(token, delims, term)) {
+    if (token == "FORCE") {
+      StreamString name, val;
+      if (cmd.GetToken(name, delims, term) && cmd.GetToken(val, delims, term)) {
+        uint32 count = ForceSignal(name.Buffer(), val.Buffer());
+        if (client) {
+          StreamString resp;
+          resp.Printf("OK FORCE %u\n", count);
+          uint32 s = resp.Size();
+          (void)client->Write(resp.Buffer(), s);
+        }
+      }
+    } else if (token == "UNFORCE") {
+      StreamString name;
+      if (cmd.GetToken(name, delims, term)) {
+        uint32 count = UnforceSignal(name.Buffer());
+        if (client) {
+          StreamString resp;
+          resp.Printf("OK UNFORCE %u\n", count);
+          uint32 s = resp.Size();
+          (void)client->Write(resp.Buffer(), s);
+        }
+      }
+    } else if (token == "TRACE") {
+      StreamString name, state, decim;
+      if (cmd.GetToken(name, delims, term) &&
+          cmd.GetToken(state, delims, term)) {
+        bool enable = (state == "1");
+        uint32 d = 1;
+        if (cmd.GetToken(decim, delims, term)) {
+          AnyType decimVal(UnsignedInteger32Bit, 0u, &d);
+          AnyType decimStr(CharString, 0u, decim.Buffer());
+          (void)TypeConvert(decimVal, decimStr);
+        }
+        uint32 count = TraceSignal(name.Buffer(), enable, d);
+        if (client) {
+          StreamString resp;
+          resp.Printf("OK TRACE %u\n", count);
+          uint32 s = resp.Size();
+          (void)client->Write(resp.Buffer(), s);
+        }
+      }
+    } else if (token == "DISCOVER")
+      Discover(client);
+    else if (token == "CONFIG")
+      ServeConfig(client);
+    else if (token == "PAUSE") {
+      SetPaused(true);
+      if (client) {
+        uint32 s = 3;
+        (void)client->Write("OK\n", s);
+      }
+    } else if (token == "RESUME") {
+      SetPaused(false);
+      if (client) {
+        uint32 s = 3;
+        (void)client->Write("OK\n", s);
+      }
+    } else if (token == "TREE") {
+      StreamString json;
+      json = "{\"Name\": \"Root\", \"Class\": \"ObjectRegistryDatabase\", "
+             "\"Children\": [\n";
+      (void)ExportTree(ObjectRegistryDatabase::Instance(), json);
+      json += "\n]}\nOK TREE\n";
+      uint32 s = json.Size();
+      if (client)
+        (void)client->Write(json.Buffer(), s);
+    } else if (token == "INFO") {
+      StreamString path;
+      if (cmd.GetToken(path, delims, term))
+        InfoNode(path.Buffer(), client);
+    } else if (token == "LS") {
+      StreamString path;
+      if (cmd.GetToken(path, delims, term))
+        ListNodes(path.Buffer(), client);
+      else
+        ListNodes(NULL_PTR(const char8 *), client);
+    }
+  }
+}
+
+void DebugService::EnrichWithConfig(const char8 *path, StreamString &json) {
+  if (path == NULL_PTR(const char8 *))
+    return;
+  fullConfig.MoveToRoot();
+
+  const char8 *current = path;
+  bool ok = true;
+  while (ok) {
+    const char8 *nextDot = StringHelper::SearchString(current, ".");
+    StreamString part;
+    if (nextDot != NULL_PTR(const char8 *)) {
+      uint32 len = (uint32)(nextDot - current);
+      (void)part.Write(current, len);
+      current = nextDot + 1;
+    } else {
+      part = current;
+      ok = false;
+    }
+
+    if (fullConfig.MoveRelative(part.Buffer())) {
+      // Found exact
+    } else {
+      bool found = false;
+      if (part == "In") {
+        if (fullConfig.MoveRelative("InputSignals")) {
+          found = true;
+        }
+      } else if (part == "Out") {
+        if (fullConfig.MoveRelative("OutputSignals")) {
+          found = true;
+        }
+      }
+
+      if (!found) {
+        StreamString prefixed;
+        prefixed.Printf("+%s", part.Buffer());
+        if (fullConfig.MoveRelative(prefixed.Buffer())) {
+          // Found prefixed
+        } else {
+          return; // Not found
+        }
+      }
+    }
+  }
+
+  ConfigurationDatabase db;
+  fullConfig.Copy(db);
+  fullConfig.MoveToRoot();
+  db.MoveToRoot();
+  uint32 n = db.GetNumberOfChildren();
+  for (uint32 i = 0u; i < n; i++) {
+    const char8 *name = db.GetChildName(i);
+    AnyType at = db.GetType(name);
+    if (!at.GetTypeDescriptor().isStructuredData) {
+      json += ", \"";
+      EscapeJson(name, json);
+      json += "\": \"";
+      char8 buf[1024];
+      AnyType st(CharString, 0u, buf);
+      st.SetNumberOfElements(0, 1024);
+      if (TypeConvert(st, at)) {
+        EscapeJson(buf, json);
+      }
+      json += "\"";
+    }
+  }
+}
+
+void DebugService::JsonifyDatabase(ConfigurationDatabase &db,
+                                   StreamString &json) {
+  json += "{";
+  uint32 n = db.GetNumberOfChildren();
+  for (uint32 i = 0u; i < n; i++) {
+    const char8 *name = db.GetChildName(i);
+    json += "\"";
+    EscapeJson(name, json);
+    json += "\": ";
+    if (db.MoveRelative(name)) {
+      ConfigurationDatabase child;
+      db.Copy(child);
+      JsonifyDatabase(child, json);
+      db.MoveToAncestor(1u);
+    } else {
+      AnyType at = db.GetType(name);
+      char8 buf[1024];
+      AnyType st(CharString, 0u, buf);
+      st.SetNumberOfElements(0, 1024);
+      if (TypeConvert(st, at)) {
+        json += "\"";
+        EscapeJson(buf, json);
+        json += "\"";
+      } else {
+        json += "null";
+      }
+    }
+    if (i < n - 1)
+      json += ", ";
+  }
+  json += "}";
+}
+
+void DebugService::ServeConfig(BasicTCPSocket *client) {
+  if (client == NULL_PTR(BasicTCPSocket *))
+    return;
+  StreamString json;
+  fullConfig.MoveToRoot();
+  JsonifyDatabase(fullConfig, json);
+  json += "\nOK CONFIG\n";
+  uint32 s = json.Size();
+  (void)client->Write(json.Buffer(), s);
+}
+
+void DebugService::InfoNode(const char8 *path, BasicTCPSocket *client) {
+  if (!client)
+    return;
+  Reference ref = ObjectRegistryDatabase::Instance()->Find(path);
+  StreamString json = "{";
+  if (ref.IsValid()) {
+    json += "\"Name\": \"";
+    EscapeJson(ref->GetName(), json);
+    json += "\", \"Class\": \"";
+    EscapeJson(ref->GetClassProperties()->GetName(), json);
+    json += "\"";
+    ConfigurationDatabase db;
+    if (ref->ExportData(db)) {
+      json += ", \"Config\": {";
+      db.MoveToRoot();
+      uint32 nChildren = db.GetNumberOfChildren();
+      for (uint32 i = 0; i < nChildren; i++) {
+        const char8 *cname = db.GetChildName(i);
+        AnyType at = db.GetType(cname);
+        char8 valBuf[1024];
+        AnyType strType(CharString, 0u, valBuf);
+        strType.SetNumberOfElements(0, 1024);
+        if (TypeConvert(strType, at)) {
+          json += "\"";
+          EscapeJson(cname, json);
+          json += "\": \"";
+          EscapeJson(valBuf, json);
+          json += "\"";
+          if (i < nChildren - 1)
+            json += ", ";
+        }
+      }
+      json += "}";
+    }
+    EnrichWithConfig(path, json);
+  } else {
+    mutex.FastLock();
+    bool found = false;
+    for (uint32 i = 0; i < aliases.Size(); i++) {
+      if (aliases[i].name == path ||
+          SuffixMatch(aliases[i].name.Buffer(), path)) {
+        DebugSignalInfo *s = signals[aliases[i].signalIndex];
+        const char8 *tname =
+            TypeDescriptor::GetTypeNameFromTypeDescriptor(s->type);
+        json.Printf("\"Name\": \"%s\", \"Class\": \"Signal\", \"Type\": "
+                    "\"%s\", \"ID\": %d",
+                    s->name.Buffer(), tname ? tname : "Unknown", s->internalID);
+        EnrichWithConfig(aliases[i].name.Buffer(), json);
+        found = true;
+        break;
+      }
+    }
+    mutex.FastUnLock();
+    if (!found)
+      json += "\"Error\": \"Object not found\"";
+  }
+  json += "}\nOK INFO\n";
+  uint32 s = json.Size();
+  (void)client->Write(json.Buffer(), s);
+}
+
+uint32 DebugService::ExportTree(ReferenceContainer *container,
+                                StreamString &json) {
+  if (container == NULL_PTR(ReferenceContainer *))
+    return 0;
+  uint32 size = container->Size();
+  uint32 validCount = 0;
+  for (uint32 i = 0u; i < size; i++) {
+    Reference child = container->Get(i);
+    if (child.IsValid()) {
+      if (validCount > 0u)
+        json += ",\n";
+      StreamString nodeJson;
+      const char8 *cname = child->GetName();
+      if (cname == NULL_PTR(const char8 *))
+        cname = "unnamed";
+      nodeJson += "{\"Name\": \"";
+      EscapeJson(cname, nodeJson);
+      nodeJson += "\", \"Class\": \"";
+      EscapeJson(child->GetClassProperties()->GetName(), nodeJson);
+      nodeJson += "\"";
+      ReferenceContainer *inner =
+          dynamic_cast<ReferenceContainer *>(child.operator->());
+      DataSourceI *ds = dynamic_cast<DataSourceI *>(child.operator->());
+      GAM *gam = dynamic_cast<GAM *>(child.operator->());
+      if ((inner != NULL_PTR(ReferenceContainer *)) ||
+          (ds != NULL_PTR(DataSourceI *)) || (gam != NULL_PTR(GAM *))) {
+        nodeJson += ", \"Children\": [\n";
+        uint32 subCount = 0u;
+        if (inner != NULL_PTR(ReferenceContainer *))
+          subCount += ExportTree(inner, nodeJson);
+        if (ds != NULL_PTR(DataSourceI *)) {
+          uint32 nSignals = ds->GetNumberOfSignals();
+          for (uint32 j = 0u; j < nSignals; j++) {
+            if (subCount > 0u)
+              nodeJson += ",\n";
+            subCount++;
+            StreamString sname;
+            (void)ds->GetSignalName(j, sname);
+            const char8 *stype = TypeDescriptor::GetTypeNameFromTypeDescriptor(
+                ds->GetSignalType(j));
+            uint8 dims = 0u;
+            (void)ds->GetSignalNumberOfDimensions(j, dims);
+            uint32 elems = 0u;
+            (void)ds->GetSignalNumberOfElements(j, elems);
+            nodeJson += "{\"Name\": \"";
+            EscapeJson(sname.Buffer(), nodeJson);
+            nodeJson += "\", \"Class\": \"Signal\", \"Type\": \"";
+            EscapeJson(stype ? stype : "Unknown", nodeJson);
+            nodeJson.Printf("\", \"Dimensions\": %d, \"Elements\": %u}", dims,
+                            elems);
+          }
+        }
+        if (gam != NULL_PTR(GAM *)) {
+          uint32 nIn = gam->GetNumberOfInputSignals();
+          for (uint32 j = 0u; j < nIn; j++) {
+            if (subCount > 0u)
+              nodeJson += ",\n";
+            subCount++;
+            StreamString sname;
+            (void)gam->GetSignalName(InputSignals, j, sname);
+            const char8 *stype = TypeDescriptor::GetTypeNameFromTypeDescriptor(
+                gam->GetSignalType(InputSignals, j));
+            uint32 dims = 0u;
+            (void)gam->GetSignalNumberOfDimensions(InputSignals, j, dims);
+            uint32 elems = 0u;
+            (void)gam->GetSignalNumberOfElements(InputSignals, j, elems);
+            nodeJson += "{\"Name\": \"In.";
+            EscapeJson(sname.Buffer(), nodeJson);
+            nodeJson += "\", \"Class\": \"InputSignal\", \"Type\": \"";
+            EscapeJson(stype ? stype : "Unknown", nodeJson);
+            nodeJson.Printf("\", \"Dimensions\": %u, \"Elements\": %u}", dims,
+                            elems);
+          }
+          uint32 nOut = gam->GetNumberOfOutputSignals();
+          for (uint32 j = 0u; j < nOut; j++) {
+            if (subCount > 0u)
+              nodeJson += ",\n";
+            subCount++;
+            StreamString sname;
+            (void)gam->GetSignalName(OutputSignals, j, sname);
+            const char8 *stype = TypeDescriptor::GetTypeNameFromTypeDescriptor(
+                gam->GetSignalType(OutputSignals, j));
+            uint32 dims = 0u;
+            (void)gam->GetSignalNumberOfDimensions(OutputSignals, j, dims);
+            uint32 elems = 0u;
+            (void)gam->GetSignalNumberOfElements(OutputSignals, j, elems);
+            nodeJson += "{\"Name\": \"Out.";
+            EscapeJson(sname.Buffer(), nodeJson);
+            nodeJson += "\", \"Class\": \"OutputSignal\", \"Type\": \"";
+            EscapeJson(stype ? stype : "Unknown", nodeJson);
+            nodeJson.Printf("\", \"Dimensions\": %u, \"Elements\": %u}", dims,
+                            elems);
+          }
+        }
+        nodeJson += "\n]";
+      }
+      nodeJson += "}";
+      json += nodeJson;
+      validCount++;
+    }
+  }
+  return validCount;
+}
+
+uint32 DebugService::ForceSignal(const char8 *name, const char8 *valueStr) {
+  mutex.FastLock();
+  uint32 count = 0;
+  for (uint32 i = 0; i < aliases.Size(); i++) {
+    if (aliases[i].name == name ||
+        SuffixMatch(aliases[i].name.Buffer(), name)) {
+
+      DebugSignalInfo *s = signals[aliases[i].signalIndex];
+      s->isForcing = true;
+      AnyType dest(s->type, 0u, s->forcedValue);
+      AnyType source(CharString, 0u, valueStr);
+      (void)TypeConvert(dest, source);
+      count++;
+    }
+  }
+  UpdateBrokersActiveStatus();
+  mutex.FastUnLock();
+  return count;
+}
+
+uint32 DebugService::UnforceSignal(const char8 *name) {
+  mutex.FastLock();
+  uint32 count = 0;
+  for (uint32 i = 0; i < aliases.Size(); i++) {
+    if (aliases[i].name == name ||
+        SuffixMatch(aliases[i].name.Buffer(), name)) {
+      signals[aliases[i].signalIndex]->isForcing = false;
+      count++;
+    }
+  }
+  UpdateBrokersActiveStatus();
+  mutex.FastUnLock();
+  return count;
+}
+
+uint32 DebugService::TraceSignal(const char8 *name, bool enable,
+                                 uint32 decimation) {
+  mutex.FastLock();
+  uint32 count = 0;
+  for (uint32 i = 0; i < aliases.Size(); i++) {
+    printf("<debug>%s\n", aliases[i].name.Buffer());
+    if (aliases[i].name == name ||
+        SuffixMatch(aliases[i].name.Buffer(), name)) {
+      DebugSignalInfo *s = signals[aliases[i].signalIndex];
+      s->isTracing = enable;
+      s->decimationFactor = decimation;
+      s->decimationCounter = 0;
+      count++;
+    }
+  }
+  if (count == 0) {
+    printf("<!!> signal %s not found\n", name);
+  }
+  UpdateBrokersActiveStatus();
+  mutex.FastUnLock();
+  return count;
+}
+
+void DebugService::Discover(BasicTCPSocket *client) {
+  if (client) {
+    StreamString header = "{\n \"Signals\": [\n";
+    uint32 s = header.Size();
+    (void)client->Write(header.Buffer(), s);
+    mutex.FastLock();
+    for (uint32 i = 0; i < aliases.Size(); i++) {
+      StreamString line;
+      DebugSignalInfo *sig = signals[aliases[i].signalIndex];
+      const char8 *typeName =
+          TypeDescriptor::GetTypeNameFromTypeDescriptor(sig->type);
+      line.Printf("  {\"name\": \"%s\", \"id\": %d, \"type\": \"%s\"",
+                  aliases[i].name.Buffer(), sig->internalID,
+                  typeName ? typeName : "Unknown");
+      EnrichWithConfig(aliases[i].name.Buffer(), line);
+      line += "}";
+      if (i < aliases.Size() - 1)
+        line += ",";
+      line += "\n";
+      s = line.Size();
+      (void)client->Write(line.Buffer(), s);
+    }
+    mutex.FastUnLock();
+    StreamString footer = " ]\n}\nOK DISCOVER\n";
+    s = footer.Size();
+    (void)client->Write(footer.Buffer(), s);
+  }
+}
+
+void DebugService::ListNodes(const char8 *path, BasicTCPSocket *client) {
+  if (!client)
+    return;
+  Reference ref =
+      (path == NULL_PTR(const char8 *) || StringHelper::Length(path) == 0 ||
+       StringHelper::Compare(path, "/") == 0)
+          ? ObjectRegistryDatabase::Instance()
+          : ObjectRegistryDatabase::Instance()->Find(path);
+  if (ref.IsValid()) {
+    StreamString out;
+    out.Printf("Nodes under %s:\n", path ? path : "/");
+    ReferenceContainer *container =
+        dynamic_cast<ReferenceContainer *>(ref.operator->());
+    if (container) {
+      for (uint32 i = 0; i < container->Size(); i++) {
+        Reference child = container->Get(i);
+        if (child.IsValid())
+          out.Printf("  %s [%s]\n", child->GetName(),
+                     child->GetClassProperties()->GetName());
+      }
+    }
+    const char *okMsg = "OK LS\n";
+    out += okMsg;
+    uint32 s = out.Size();
+    (void)client->Write(out.Buffer(), s);
+  } else {
+    const char *msg = "ERROR: Path not found\n";
+    uint32 s = StringHelper::Length(msg);
+    (void)client->Write(msg, s);
+  }
+}
+
+} // namespace MARTe

Source/Components/Interfaces/DebugService/DebugService.h

@@ -0,0 +1,139 @@
+#ifndef DEBUGSERVICE_H
+#define DEBUGSERVICE_H
+
+#include "BasicTCPSocket.h"
+#include "BasicUDPSocket.h"
+#include "ConfigurationDatabase.h"
+#include "DebugCore.h"
+#include "EmbeddedServiceMethodBinderI.h"
+#include "MessageI.h"
+#include "Object.h"
+#include "ReferenceContainer.h"
+#include "SingleThreadService.h"
+#include "StreamString.h"
+#include "Vec.h"
+
+namespace MARTe {
+
+class MemoryMapBroker;
+
+struct SignalAlias {
+  StreamString name;
+  uint32 signalIndex;
+};
+
+struct BrokerInfo {
+  DebugSignalInfo **signalPointers;
+  uint32 numSignals;
+  MemoryMapBroker *broker;
+  volatile bool *anyActiveFlag;
+  Vec<uint32> *activeIndices;
+  Vec<uint32> *activeSizes;
+  FastPollingMutexSem *activeMutex;
+};
+
+class DebugService : public ReferenceContainer,
+                     public MessageI,
+                     public EmbeddedServiceMethodBinderI {
+public:
+  friend class DebugServiceTest;
+  CLASS_REGISTER_DECLARATION()
+
+  DebugService();
+  virtual ~DebugService();
+
+  virtual bool Initialise(StructuredDataI &data);
+
+  DebugSignalInfo *RegisterSignal(void *memoryAddress, TypeDescriptor type,
+                                  const char8 *name);
+  void ProcessSignal(DebugSignalInfo *signalInfo, uint32 size,
+                     uint64 timestamp);
+
+  void RegisterBroker(DebugSignalInfo **signalPointers, uint32 numSignals,
+                      MemoryMapBroker *broker, volatile bool *anyActiveFlag,
+                      Vec<uint32> *activeIndices, Vec<uint32> *activeSizes,
+                      FastPollingMutexSem *activeMutex);
+
+  virtual ErrorManagement::ErrorType Execute(ExecutionInfo &info);
+
+  bool IsPaused() const { return isPaused; }
+  void SetPaused(bool paused) { isPaused = paused; }
+
+  static bool GetFullObjectName(const Object &obj, StreamString &fullPath);
+
+  uint32 ForceSignal(const char8 *name, const char8 *valueStr);
+  uint32 UnforceSignal(const char8 *name);
+  uint32 TraceSignal(const char8 *name, bool enable, uint32 decimation = 1);
+  void Discover(BasicTCPSocket *client);
+  void InfoNode(const char8 *path, BasicTCPSocket *client);
+  void ListNodes(const char8 *path, BasicTCPSocket *client);
+  void ServeConfig(BasicTCPSocket *client);
+  void SetFullConfig(ConfigurationDatabase &config);
+
+private:
+  void HandleCommand(StreamString cmd, BasicTCPSocket *client);
+  void UpdateBrokersActiveStatus();
+
+  uint32 ExportTree(ReferenceContainer *container, StreamString &json);
+  void PatchRegistry();
+
+  void EnrichWithConfig(const char8 *path, StreamString &json);
+  static void JsonifyDatabase(ConfigurationDatabase &db, StreamString &json);
+
+  ErrorManagement::ErrorType Server(ExecutionInfo &info);
+  ErrorManagement::ErrorType Streamer(ExecutionInfo &info);
+
+  uint16 controlPort;
+  uint16 streamPort;
+  StreamString streamIP;
+  bool isServer;
+  bool suppressTimeoutLogs;
+  volatile bool isPaused;
+
+  BasicTCPSocket tcpServer;
+  BasicUDPSocket udpSocket;
+
+  class ServiceBinder : public EmbeddedServiceMethodBinderI {
+  public:
+    enum ServiceType { ServerType, StreamerType };
+    ServiceBinder(DebugService *parent, ServiceType type)
+        : parent(parent), type(type) {}
+    virtual ErrorManagement::ErrorType Execute(ExecutionInfo &info) {
+      if (type == StreamerType) {
+        return parent->Streamer(info);
+      }
+      printf("serve TCP\n");
+      return parent->Server(info);
+    }
+
+  private:
+    DebugService *parent;
+    ServiceType type;
+  };
+
+  ServiceBinder binderServer;
+  ServiceBinder binderStreamer;
+
+  SingleThreadService threadService;
+  SingleThreadService streamerService;
+
+  ThreadIdentifier serverThreadId;
+  ThreadIdentifier streamerThreadId;
+
+  Vec<DebugSignalInfo *> signals;
+  Vec<SignalAlias> aliases;
+  Vec<BrokerInfo> brokers;
+
+  FastPollingMutexSem mutex;
+  TraceRingBuffer traceBuffer;
+
+  BasicTCPSocket *activeClient;
+
+  ConfigurationDatabase fullConfig;
+
+  static DebugService *instance;
+};
+
+} // namespace MARTe
+
+#endif

Source/Components/Interfaces/DebugService/Makefile.gcc

@@ -0,0 +1,28 @@
+#############################################################
+#
+# Copyright 2015 F4E | European Joint Undertaking for ITER 
+#  and the Development of Fusion Energy ('Fusion for Energy')
+#
+# Licensed under the EUPL, Version 1.1 or - as soon they 
+# will be approved by the European Commission - subsequent  
+# versions of the EUPL (the "Licence"); 
+# You may not use this work except in compliance with the 
+# Licence. 
+# You may obtain a copy of the Licence at: 
+#  
+# http://ec.europa.eu/idabc/eupl
+#
+# Unless required by applicable law or agreed to in 
+# writing, software distributed under the Licence is 
+# distributed on an "AS IS" basis, 
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either 
+# express or implied. 
+# See the Licence for the specific language governing 
+# permissions and limitations under the Licence. 
+#
+# $Id: Makefile.gcc 3 2015-01-15 16:26:07Z aneto $
+#
+#############################################################
+
+
+include Makefile.inc

Source/Components/Interfaces/DebugService/Makefile.inc

@@ -0,0 +1,58 @@
+#############################################################
+#
+# Copyright 2015 F4E | European Joint Undertaking for ITER 
+#  and the Development of Fusion Energy ('Fusion for Energy')
+#
+# Licensed under the EUPL, Version 1.1 or - as soon they 
+# will be approved by the European Commission - subsequent  
+# versions of the EUPL (the "Licence"); 
+# You may not use this work except in compliance with the 
+# Licence. 
+# You may obtain a copy of the Licence at: 
+#  
+# http://ec.europa.eu/idabc/eupl
+#
+# Unless required by applicable law or agreed to in 
+# writing, software distributed under the Licence is 
+# distributed on an "AS IS" basis, 
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either 
+# express or implied. 
+# See the Licence for the specific language governing 
+# permissions and limitations under the Licence. 
+#
+# $Id: Makefile.inc 3 2012-01-15 16:26:07Z aneto $
+#
+#############################################################
+OBJSX=DebugService.x
+
+PACKAGE=Components/Interfaces
+
+ROOT_DIR=../../../../
+MAKEDEFAULTDIR=$(MARTe2_DIR)/MakeDefaults
+include $(MAKEDEFAULTDIR)/MakeStdLibDefs.$(TARGET)
+
+INCLUDES += -I$(ROOT_DIR)/Source/Core/Types/Result
+INCLUDES += -I$(ROOT_DIR)/Source/Core/Types/Vec
+INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L0Types
+INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L1Portability
+INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L2Objects
+INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L3Streams
+INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L4Messages
+INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L4Logger
+INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L4Configuration
+INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L5GAMs
+INCLUDES += -I$(MARTe2_DIR)/Source/Core/Scheduler/L1Portability
+INCLUDES += -I$(MARTe2_DIR)/Source/Core/Scheduler/L3Services
+INCLUDES += -I$(MARTe2_DIR)/Source/Core/Scheduler/L4Messages
+INCLUDES += -I$(MARTe2_DIR)/Source/Core/Scheduler/L4LoggerService
+INCLUDES += -I$(MARTe2_DIR)/Source/Core/Scheduler/L5GAMs
+INCLUDES += -I$(MARTe2_DIR)/Source/Core/FileSystem/L1Portability
+INCLUDES += -I$(MARTe2_DIR)/Source/Core/FileSystem/L3Streams
+
+all: $(OBJS) $(SUBPROJ) \
+	$(BUILD_DIR)/DebugService$(LIBEXT) \
+	$(BUILD_DIR)/DebugService$(DLLEXT)
+	    echo  $(OBJS)
+
+include $(MAKEDEFAULTDIR)/MakeStdLibRules.$(TARGET)
+

Source/Components/Interfaces/Makefile.gcc

@@ -0,0 +1,26 @@
+#############################################################
+#
+# Copyright 2015 F4E | European Joint Undertaking for ITER 
+#  and the Development of Fusion Energy ('Fusion for Energy')
+# 
+# Licensed under the EUPL, Version 1.1 or - as soon they 
+# will be approved by the European Commission - subsequent  
+# versions of the EUPL (the "Licence"); 
+# You may not use this work except in compliance with the 
+# Licence. 
+# You may obtain a copy of the Licence at: 
+#  
+# http://ec.europa.eu/idabc/eupl
+#
+# Unless required by applicable law or agreed to in 
+# writing, software distributed under the Licence is 
+# distributed on an "AS IS" basis, 
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either 
+# express or implied. 
+# See the Licence for the specific language governing 
+# permissions and limitations under the Licence. 
+#
+#############################################################
+
+
+include Makefile.inc

Source/Components/Interfaces/Makefile.inc

@@ -0,0 +1,43 @@
+#############################################################
+#
+# Copyright 2015 F4E | European Joint Undertaking for ITER
+#  and the Development of Fusion Energy ('Fusion for Energy')
+#
+# Licensed under the EUPL, Version 1.1 or - as soon they
+# will be approved by the European Commission - subsequent
+# versions of the EUPL (the "Licence");
+# You may not use this work except in compliance with the
+# Licence.
+# You may obtain a copy of the Licence at:
+#
+# http://ec.europa.eu/idabc/eupl
+#
+# Unless required by applicable law or agreed to in
+# writing, software distributed under the Licence is
+# distributed on an "AS IS" basis,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either
+# express or implied.
+# See the Licence for the specific language governing
+# permissions and limitations under the Licence.
+#
+#############################################################
+
+OBJSX=
+
+SPB = TCPLogger.x DebugService.x
+
+ROOT_DIR=../../..
+
+
+PACKAGE=Components
+ROOT_DIR=../../..
+ABS_ROOT_DIR=$(abspath $(ROOT_DIR))
+MAKEDEFAULTDIR=$(MARTe2_DIR)/MakeDefaults
+
+include $(MAKEDEFAULTDIR)/MakeStdLibDefs.$(TARGET)
+
+all: $(OBJS)  $(SUBPROJ)
+	echo  $(OBJS)
+
+include $(MAKEDEFAULTDIR)/MakeStdLibRules.$(TARGET)
+

Source/Components/Interfaces/TCPLogger/Makefile.gcc

@@ -0,0 +1 @@
+include Makefile.inc

Source/Components/Interfaces/TCPLogger/Makefile.inc

@@ -0,0 +1,31 @@
+OBJSX=TcpLogger.x
+
+PACKAGE=Components/Interfaces
+
+ROOT_DIR=../../../../
+MAKEDEFAULTDIR=$(MARTe2_DIR)/MakeDefaults
+include $(MAKEDEFAULTDIR)/MakeStdLibDefs.$(TARGET)
+
+INCLUDES += -I.
+INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L0Types
+INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L1Portability
+INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L2Objects
+INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L3Streams
+INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L4Messages
+INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L4Logger
+INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L4Configuration
+INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L5GAMs
+INCLUDES += -I$(MARTe2_DIR)/Source/Core/Scheduler/L1Portability
+INCLUDES += -I$(MARTe2_DIR)/Source/Core/Scheduler/L3Services
+INCLUDES += -I$(MARTe2_DIR)/Source/Core/Scheduler/L4Messages
+INCLUDES += -I$(MARTe2_DIR)/Source/Core/Scheduler/L4LoggerService
+INCLUDES += -I$(MARTe2_DIR)/Source/Core/Scheduler/L5GAMs
+INCLUDES += -I$(MARTe2_DIR)/Source/Core/FileSystem/L1Portability
+INCLUDES += -I$(MARTe2_DIR)/Source/Core/FileSystem/L3Streams
+
+all: $(OBJS) $(SUBPROJ) \
+	$(BUILD_DIR)/TcpLogger$(LIBEXT) \
+	$(BUILD_DIR)/TcpLogger$(DLLEXT)
+	echo  $(OBJS)
+
+include $(MAKEDEFAULTDIR)/MakeStdLibRules.$(TARGET)

Source/Components/Interfaces/TCPLogger/TcpLogger.cpp

@@ -0,0 +1,156 @@
+#include "TcpLogger.h"
+#include "Threads.h"
+#include "StringHelper.h"
+#include "ConfigurationDatabase.h"
+
+namespace MARTe {
+
+CLASS_REGISTER(TcpLogger, "1.0")
+
+TcpLogger::TcpLogger() : 
+    ReferenceContainer(), LoggerConsumerI(), EmbeddedServiceMethodBinderI(),
+    service(*this)
+{
+    port = 8082;
+    readIdx = 0;
+    writeIdx = 0;
+    workerThreadId = InvalidThreadIdentifier;
+    for (uint32 i=0; i<MAX_CLIENTS; i++) {
+        activeClients[i] = NULL_PTR(BasicTCPSocket*);
+    }
+}
+
+TcpLogger::~TcpLogger() {
+    service.Stop();
+    server.Close();
+
+    clientsMutex.FastLock();
+    for (uint32 i=0; i<MAX_CLIENTS; i++) {
+        if (activeClients[i] != NULL_PTR(BasicTCPSocket*)) {
+            activeClients[i]->Close();
+            delete activeClients[i];
+            activeClients[i] = NULL_PTR(BasicTCPSocket*);
+        }
+    }
+    clientsMutex.FastUnLock();
+}
+
+bool TcpLogger::Initialise(StructuredDataI & data) {
+    if (!ReferenceContainer::Initialise(data)) return false;
+    
+    if (!data.Read("Port", port)) {
+        (void)data.Read("TcpPort", port);
+    }
+
+    (void)eventSem.Create();
+
+    ConfigurationDatabase threadData;
+    threadData.Write("Timeout", (uint32)1000);
+    if (!service.Initialise(threadData)) return false;
+
+    if (!server.Open()) return false;
+    if (!server.Listen(port)) {
+        return false;
+    }
+    printf("[TcpLogger] Listening on port %u\n", port);
+
+    if (service.Start() != ErrorManagement::NoError) {
+        return false;
+    }
+
+    return true;
+}
+
+void TcpLogger::ConsumeLogMessage(LoggerPage *logPage) {
+    if (logPage == NULL_PTR(LoggerPage*)) return;
+
+    // 1. Mirror to stdout
+    StreamString levelStr;
+    ErrorManagement::ErrorCodeToStream(logPage->errorInfo.header.errorType, levelStr);
+    printf("[%s] %s\n", levelStr.Buffer(), logPage->errorStrBuffer);
+    fflush(stdout);
+
+    // 2. Queue for TCP
+    InsertLogIntoQueue(logPage->errorInfo, logPage->errorStrBuffer);
+}
+
+void TcpLogger::InsertLogIntoQueue(const ErrorManagement::ErrorInformation &info, const char8 * const description) {
+    uint32 next = (writeIdx + 1) % QUEUE_SIZE;
+    if (next != readIdx) {
+        TcpLogEntry &entry = queue[writeIdx % QUEUE_SIZE];
+        entry.info = info;
+        StringHelper::Copy(entry.description, description);
+        writeIdx = next;
+        (void)eventSem.Post();
+    }
+}
+
+ErrorManagement::ErrorType TcpLogger::Execute(ExecutionInfo & info) {
+    if (info.GetStage() == ExecutionInfo::TerminationStage) return ErrorManagement::NoError;
+    if (info.GetStage() == ExecutionInfo::StartupStage) {
+        workerThreadId = Threads::Id();
+        return ErrorManagement::NoError;
+    }
+
+    while (info.GetStage() == ExecutionInfo::MainStage) {
+        // 1. Check for new connections
+        BasicTCPSocket *newClient = server.WaitConnection(1); 
+        if (newClient != NULL_PTR(BasicTCPSocket *)) {
+            clientsMutex.FastLock();
+            bool added = false;
+            for (uint32 i=0; i<MAX_CLIENTS; i++) {
+                if (activeClients[i] == NULL_PTR(BasicTCPSocket*)) {
+                    activeClients[i] = newClient;
+                    added = true;
+                    break;
+                }
+            }
+            clientsMutex.FastUnLock();
+            if (!added) {
+                newClient->Close();
+                delete newClient;
+            } else {
+                (void)newClient->SetBlocking(false);
+            }
+        }
+
+        // 2. Stream data to clients
+        bool hadData = false;
+        while (readIdx != writeIdx) {
+            hadData = true;
+            uint32 idx = readIdx % QUEUE_SIZE;
+            TcpLogEntry &entry = queue[idx];
+            
+            StreamString level;
+            ErrorManagement::ErrorCodeToStream(entry.info.header.errorType, level);
+            
+            StreamString packet;
+            packet.Printf("LOG %s %s\n", level.Buffer(), entry.description);
+            uint32 size = packet.Size();
+
+            clientsMutex.FastLock();
+            for (uint32 j=0; j<MAX_CLIENTS; j++) {
+                if (activeClients[j] != NULL_PTR(BasicTCPSocket*)) {
+                    uint32 s = size;
+                    if (!activeClients[j]->Write(packet.Buffer(), s)) {
+                        activeClients[j]->Close();
+                        delete activeClients[j];
+                        activeClients[j] = NULL_PTR(BasicTCPSocket*);
+                    }
+                }
+            }
+            clientsMutex.FastUnLock();
+            readIdx = (readIdx + 1) % QUEUE_SIZE;
+        }
+
+        if (!hadData) {
+            (void)eventSem.Wait(TimeoutType(100));
+            eventSem.Reset();
+        } else {
+            Sleep::MSec(1);
+        }
+    }
+    return ErrorManagement::NoError;
+}
+
+}

Source/Components/Interfaces/TCPLogger/TcpLogger.h

@@ -0,0 +1,65 @@
+#ifndef TCPLOGGER_H
+#define TCPLOGGER_H
+
+#include "LoggerConsumerI.h"
+#include "ReferenceContainer.h"
+#include "EmbeddedServiceMethodBinderI.h"
+#include "BasicTCPSocket.h"
+#include "SingleThreadService.h"
+#include "FastPollingMutexSem.h"
+#include "EventSem.h"
+
+namespace MARTe {
+
+struct TcpLogEntry {
+    ErrorManagement::ErrorInformation info;
+    char8 description[MAX_ERROR_MESSAGE_SIZE];
+};
+
+/**
+ * @brief Logger consumer that publishes framework logs to TCP and stdout.
+ * @details Implements LoggerConsumerI to be used inside a LoggerService.
+ */
+class TcpLogger : public ReferenceContainer, public LoggerConsumerI, public EmbeddedServiceMethodBinderI {
+public:
+    CLASS_REGISTER_DECLARATION()
+
+    TcpLogger();
+    virtual ~TcpLogger();
+
+    virtual bool Initialise(StructuredDataI & data);
+
+    /**
+     * @brief Implementation of LoggerConsumerI.
+     * Called by LoggerService.
+     */
+    virtual void ConsumeLogMessage(LoggerPage *logPage);
+
+    /**
+     * @brief Worker thread method for TCP streaming.
+     */
+    virtual ErrorManagement::ErrorType Execute(ExecutionInfo & info);
+
+private:
+    void InsertLogIntoQueue(const ErrorManagement::ErrorInformation &info, const char8 * const description);
+
+    uint16 port;
+    BasicTCPSocket server;
+    
+    static const uint32 MAX_CLIENTS = 8;
+    BasicTCPSocket* activeClients[MAX_CLIENTS];
+    FastPollingMutexSem clientsMutex;
+
+    SingleThreadService service;
+    ThreadIdentifier workerThreadId;
+
+    static const uint32 QUEUE_SIZE = 1024;
+    TcpLogEntry queue[QUEUE_SIZE];
+    volatile uint32 readIdx;
+    volatile uint32 writeIdx;
+    EventSem eventSem;
+};
+
+}
+
+#endif