martino/MARTe-Integrated-Components
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Implemented better testing and fixed skipepd frames

Browse Source
This commit is contained in:
Martino Ferrari
2026-07-01 16:39:34 +02:00
parent 7a326c5d78
commit 0bea41f866
46 changed files with 4358 additions and 1739 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Source/Components/Interfaces/DebugService/DebugCore.h
+14 -10
View File
@@ -78,8 +78,9 @@ public:
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;
/* 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) {
@@ -96,13 +97,15 @@ public:
WriteToBuffer(&tempWrite, &size, 4);
WriteToBuffer(&tempWrite, data, size);
writeIndex = tempWrite;
// 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) {
uint32 read = readIndex;
uint32 write = writeIndex;
/* 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;
@@ -124,9 +127,9 @@ public:
// 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) {
readIndex = write; // corrupt ring — discard all
__atomic_store_n(&readIndex, write, __ATOMIC_RELEASE); // corrupt ring — discard all
} else {
readIndex = (tempRead + tempSize) % bufferSize;
__atomic_store_n(&readIndex, (tempRead + tempSize) % bufferSize, __ATOMIC_RELEASE);
}
return false;
}
@@ -137,13 +140,14 @@ public:
timestamp = tempTs;
size = tempSize;
readIndex = tempRead;
// HI-9: release-store readIndex after reading data
__atomic_store_n(&readIndex, tempRead, __ATOMIC_RELEASE);
return true;
}
uint32 Count() {
uint32 read = readIndex;
uint32 write = writeIndex;
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);
}
Source/Components/Interfaces/DebugService/DebugService.cpp
+101 -14
View File
@@ -13,6 +13,7 @@
#include "Threads.h"
#include "TimeoutType.h"
#include "UDPSProtocol.h"
#include <string.h>
namespace MARTe {
@@ -122,6 +123,12 @@ bool DebugService::Initialise(StructuredDataI &data) {
suppressTimeoutLogs = (suppress == 1u);
}
StreamString tempToken;
if (data.Read("AuthToken", tempToken)) {
authToken = tempToken;
}
clientAuthenticated = (authToken.Size() == 0u);
// Capture only the local subtree — do NOT call MoveToRoot() on the shared CDB.
(void)data.Copy(fullConfig);
@@ -281,6 +288,8 @@ ErrorManagement::ErrorType DebugService::Server(ExecutionInfo &info) {
cmdCountInWindow = 0u;
cmdWindowStartMs = nowMs;
lastDataTimeMs = nowMs;
/* CR-5: require auth if an AuthToken is configured. */
clientAuthenticated = (authToken.Size() == 0u);
}
} else {
if (nowMs - lastDataTimeMs > CLIENT_IDLE_TIMEOUT_MS) {
@@ -351,23 +360,101 @@ ErrorManagement::ErrorType DebugService::Server(ExecutionInfo &info) {
uint32 cmdLen = len;
command.Write(raw + lineStart, cmdLen);
// Dispatch via base HandleCommand, write response to socket.
StreamString out;
HandleCommand(command, out);
if (out.Size() > 0u) {
const char8 *wPtr = out.Buffer();
uint32 remaining = (uint32)out.Size();
lastDataTimeMs = (uint64)((float64)HighResolutionTimer::Counter() *
HighResolutionTimer::Period() * 1000.0);
while (remaining > 0u) {
uint32 wrote = remaining;
if (!activeClient->Write(wPtr, wrote) || wrote == 0u) {
break;
/* CR-5: Auth token gate. If an AuthToken is
* configured, the client must send
* "AUTH <token>" before any other command. */
if (authToken.Size() > 0u) {
const char8 *cmdPtr = command.Buffer();
if (cmdLen >= 5u &&
strncmp(cmdPtr, "AUTH ", 5u) == 0) {
const char8 *recvToken = cmdPtr + 5u;
uint32 recvLen = cmdLen - 5u;
/* Strip trailing \r if present */
if (recvLen > 0u &&
recvToken[recvLen - 1u] == '\r') {
recvLen--;
}
wPtr += wrote;
remaining -= wrote;
if (recvLen == authToken.Size() &&
strncmp(recvToken,
authToken.Buffer(),
recvLen) == 0) {
clientAuthenticated = true;
const char8 *okResp =
"OK AUTHENTICATED\n";
uint32 respSz =
static_cast<uint32>(
strlen(okResp));
(void) activeClient->Write(
okResp, respSz);
} else {
const char8 *badResp =
"ERR AUTH_FAILED\n";
uint32 respSz =
static_cast<uint32>(
strlen(badResp));
(void) activeClient->Write(
badResp, respSz);
}
} else if (!clientAuthenticated) {
const char8 *needAuth =
"ERR AUTH_REQUIRED\n";
uint32 respSz =
static_cast<uint32>(
strlen(needAuth));
(void) activeClient->Write(
needAuth, respSz);
} else {
// Dispatch via base HandleCommand,
// write response to socket.
StreamString out;
HandleCommand(command, out);
if (out.Size() > 0u) {
const char8 *wPtr = out.Buffer();
uint32 remaining =
(uint32)out.Size();
lastDataTimeMs =
(uint64)((float64)
HighResolutionTimer::Counter() *
HighResolutionTimer::Period() *
1000.0);
while (remaining > 0u) {
uint32 wrote = remaining;
if (!activeClient->Write(
wPtr, wrote) ||
wrote == 0u) {
break;
}
wPtr += wrote;
remaining -= wrote;
lastDataTimeMs =
(uint64)((float64)
HighResolutionTimer::Counter() *
HighResolutionTimer::Period() *
1000.0);
}
}
}
} else {
// No auth token configured — back-compat.
// Dispatch via base HandleCommand, write
// response to socket.
StreamString out;
HandleCommand(command, out);
if (out.Size() > 0u) {
const char8 *wPtr = out.Buffer();
uint32 remaining = (uint32)out.Size();
lastDataTimeMs = (uint64)((float64)HighResolutionTimer::Counter() *
HighResolutionTimer::Period() * 1000.0);
while (remaining > 0u) {
uint32 wrote = remaining;
if (!activeClient->Write(wPtr, wrote) || wrote == 0u) {
break;
}
wPtr += wrote;
remaining -= wrote;
lastDataTimeMs = (uint64)((float64)HighResolutionTimer::Counter() *
HighResolutionTimer::Period() * 1000.0);
}
}
}
}
Source/Components/Interfaces/DebugService/DebugService.h
+7
View File
@@ -76,6 +76,13 @@ private:
bool isServer;
bool suppressTimeoutLogs;
/** Optional authentication token (CR-5). If set (non-empty), the first
* command from a new TCP client must be "AUTH <token>". All other
* commands are rejected until the client authenticates. If empty
* (default), no authentication is required (back-compat). */
StreamString authToken;
bool clientAuthenticated;
BasicTCPSocket tcpServer;
UDPSServer udpsServer; ///< Handles fragmentation and multi-client sending
Source/Components/Interfaces/DebugService/DebugServiceBase.cpp
+25 -4
View File
@@ -181,6 +181,12 @@ static void BuildCDBFromContainer(ReferenceContainer *container,
}
}
/* HI-8: Guard against double-patching (e.g. two DebugService instances).
* Once the registry has been patched, subsequent PatchRegistry() calls are
* no-ops. Original builders are not saved/restored — the debug wrappers
* persist for the process lifetime (intentional for transparent debugging). */
static bool registryPatched = false;
static void PatchItemInternal(const char8 *originalName,
ObjectBuilder *debugBuilder) {
ClassRegistryItem *item =
@@ -215,6 +221,14 @@ DebugServiceBase::~DebugServiceBase() {
// ---------------------------------------------------------------------------
void DebugServiceBase::PatchRegistry() {
/* HI-8: skip if already patched (prevents double-patch leak when multiple
* DebugService instances are created). */
if (registryPatched) {
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"PatchRegistry: registry already patched — skipping (double-patch guard).");
return;
}
registryPatched = true;
PatchItemInternal("MemoryMapInputBroker",
new DebugMemoryMapInputBrokerBuilder());
PatchItemInternal("MemoryMapOutputBroker",
@@ -305,13 +319,20 @@ void DebugServiceBase::ProcessSignal(DebugSignalInfo *signalInfo, uint32 size,
return;
if (signalInfo->isForcing) {
uint32 nEl = signalInfo->numberOfElements;
/* HI-4: clamp size to forcedValue buffer to prevent OOB read */
uint32 forceSize = size;
if (forceSize > static_cast<uint32>(sizeof(signalInfo->forcedValue))) {
forceSize = static_cast<uint32>(sizeof(signalInfo->forcedValue));
}
if (nEl <= 1u) {
// Scalar — single memcpy.
memcpy(signalInfo->memoryAddress, signalInfo->forcedValue, size);
// Scalar — single memcpy (clamped to forcedValue bounds).
memcpy(signalInfo->memoryAddress, signalInfo->forcedValue, forceSize);
} else {
// Array — copy only the elements whose bit is set in forcedMask.
uint32 elemBytes = size / nEl;
for (uint32 e = 0u; e < nEl; e++) {
// HI-4: cap loop at 256 elements (forcedMask is 32 bytes = 256 bits).
uint32 elemBytes = forceSize / nEl;
uint32 nElCapped = (nEl > 256u) ? 256u : nEl;
for (uint32 e = 0u; e < nElCapped; e++) {
if (signalInfo->forcedMask[e >> 3u] & (uint8)(1u << (e & 7u))) {
memcpy((uint8 *)signalInfo->memoryAddress + e * elemBytes,
signalInfo->forcedValue + e * elemBytes,
Source/Components/Interfaces/UDPStream/UDPSClient.cpp
+33
View File
@@ -9,6 +9,7 @@
#include "MemoryOperationsHelper.h"
#include <sys/select.h>
#include <sys/socket.h>
#include <errno.h>
namespace MARTe {
@@ -26,6 +27,7 @@ UDPSClient::UDPSClient()
maxPayloadSize(UDPS_CLIENT_DEFAULT_MAX_PAYLOAD),
cpuMask(0xFFFFFFFFu),
stackSize(65536u),
recvBufferSize(UDPS_CLIENT_DEFAULT_RECV_BUFFER),
listener(NULL_PTR(UDPSClientListener *)),
threadService(*this),
connected(false),
@@ -98,6 +100,9 @@ bool UDPSClient::Initialise(StructuredDataI &data) {
(void) data.Read("CPUMask", cpuMask);
(void) data.Read("StackSize", stackSize);
recvBufferSize = UDPS_CLIENT_DEFAULT_RECV_BUFFER;
(void) data.Read("RecvBufferSize", recvBufferSize);
return true;
}
@@ -209,6 +214,23 @@ bool UDPSClient::Connect() {
return ok;
}
void UDPSClient::SetRecvBufferSize(BasicUDPSocket &sock) {
/* BasicUDPSocket exposes no SO_RCVBUF API; the OS default (Linux
* rmem_default, typically ~208 KiB) is easily overrun by high-throughput
* sources, causing silent kernel-level datagram drops. Work around this
* by calling setsockopt() directly on the raw handle. Best-effort: a
* failure here just leaves the OS default in place. */
Handle fd = sock.GetReadHandle();
if (fd >= 0) {
int32 sz = static_cast<int32>(recvBufferSize);
if (setsockopt(fd, SOL_SOCKET, SO_RCVBUF, &sz, sizeof(sz)) != 0) {
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"UDPSClient: Could not set SO_RCVBUF to %u bytes.",
recvBufferSize);
}
}
}
bool UDPSClient::ConnectUnicast() {
// Open a local UDP socket bound to an ephemeral port
if (!recvSocket.Open()) {
@@ -216,6 +238,7 @@ bool UDPSClient::ConnectUnicast() {
"UDPSClient: Could not open receive socket.");
return false;
}
SetRecvBufferSize(recvSocket);
if (!recvSocket.Listen(0u)) {
REPORT_ERROR_STATIC(ErrorManagement::Warning,
@@ -256,6 +279,7 @@ bool UDPSClient::ConnectMulticast() {
"UDPSClient: Could not open multicast socket.");
return false;
}
SetRecvBufferSize(mcastSocket);
bool ok = mcastSocket.Listen(dataPort);
if (!ok) {
@@ -378,6 +402,15 @@ bool UDPSClient::ReceiveAndProcess() {
return false;
}
/* HI-6: guard against FD_SETSIZE overflow */
if (fd < 0 || fd >= FD_SETSIZE ||
(tcpFd >= 0 && tcpFd >= FD_SETSIZE)) {
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"UDPSClient: fd >= FD_SETSIZE (%d/%d) — skipping select.",
fd, tcpFd);
return false;
}
fd_set rset;
FD_ZERO(&rset);
FD_SET(fd, &rset);
Source/Components/Interfaces/UDPStream/UDPSClient.h
+11
View File
@@ -95,6 +95,12 @@ public:
/** Default maximum payload size (bytes, excluding 17-byte header). */
static const uint32 UDPS_CLIENT_DEFAULT_MAX_PAYLOAD = 1400u;
/** Default OS UDP receive socket buffer size (bytes). The Linux default
* (rmem_default, typically ~208 KiB) is easily overrun by high-throughput
* sources (e.g. multi-hundred-KiB bursts every few ms), causing silent
* kernel-level datagram drops. 4 MiB gives generous burst headroom. */
static const uint32 UDPS_CLIENT_DEFAULT_RECV_BUFFER = 4194304u; // 4 MiB
UDPSClient();
virtual ~UDPSClient();
@@ -111,6 +117,7 @@ public:
* - MaxPayloadSize (uint32) Max payload bytes per datagram, excluding header. Default 1400.
* - CPUMask (uint32) CPU affinity mask for the receive thread. Default 0xFFFFFFFF.
* - StackSize (uint32) Stack size for the receive thread. Default 65536.
* - RecvBufferSize (uint32) OS UDP receive socket buffer size (bytes). Default 4 MiB.
*/
bool Initialise(StructuredDataI &data);
@@ -165,6 +172,9 @@ private:
bool ConnectUnicast();
bool ConnectMulticast();
/** Set the OS receive buffer size (SO_RCVBUF) on a UDP socket's raw handle. */
void SetRecvBufferSize(BasicUDPSocket &sock);
void ProcessDatagram(const uint8 *buf, uint32 size);
/** Read one full UDPS frame (header + payload) from the TCP control socket. */
bool ReceiveTCPFrame();
@@ -188,6 +198,7 @@ private:
uint32 maxPayloadSize;
uint32 cpuMask;
uint32 stackSize;
uint32 recvBufferSize;
// -------------------------------------------------------------------------
// Runtime state
Source/Components/Interfaces/UDPStream/UDPSServer.cpp
+2
View File
@@ -268,6 +268,7 @@ void UDPSServer::ServiceClients() {
}
// Non-blocking peek
int fd = tcpClients[i]->GetReadHandle();
if (fd < 0 || fd >= FD_SETSIZE) { continue; /* HI-6: skip FDs outside select range */ }
fd_set rset;
FD_ZERO(&rset);
FD_SET(fd, &rset);
@@ -303,6 +304,7 @@ void UDPSServer::ServiceClients() {
return;
}
int fd = serverSocket.GetReadHandle();
if (fd < 0 || fd >= FD_SETSIZE) { return; /* HI-6 */ }
fd_set rset;
FD_ZERO(&rset);
FD_SET(fd, &rset);