Implemetned better buffering and high frequency tracing

Headers/DebugCore.h

@@ -4,6 +4,7 @@
 #include "CompilerTypes.h"
 #include "TypeDescriptor.h"
 #include "StreamString.h"
+#include <cstring> // For memcpy
 
 namespace MARTe {
 
@@ -58,7 +59,14 @@ public:
         uint32 packetSize = 4 + 4 + size;
         uint32 read = readIndex; 
         uint32 write = writeIndex;
-        uint32 available = (read <= write) ? (bufferSize - (write - read) - 1) : (read - write - 1);
+        
+        // Calculate available space
+        uint32 available = 0;
+        if (read <= write) {
+            available = bufferSize - (write - read) - 1;
+        } else {
+            available = read - write - 1;
+        }
 
         if (available < packetSize) return false;
 
@@ -68,6 +76,9 @@ public:
         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;
@@ -79,20 +90,27 @@ public:
         if (read == write) return false;
 
         uint32 tempRead = read;
-        uint32 tempId, tempSize;
+        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
+            // 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;
     }
@@ -106,18 +124,32 @@ public:
 
 private:
     void WriteToBuffer(uint32 *idx, void* src, uint32 count) {
-        uint8* s = (uint8*)src;
-        for (uint32 i=0; i<count; i++) {
-            buffer[*idx] = s[i];
-            *idx = (*idx + 1) % bufferSize;
+        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) {
-        uint8* d = (uint8*)dst;
-        for (uint32 i=0; i<count; i++) {
-            d[i] = buffer[*idx];
-            *idx = (*idx + 1) % bufferSize;
+        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;
         }
     }