martino/MARTe-Integrated-Components
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
5562877c99fd46a099ba91f0a42ec551e77bcd86
MARTe-Integrated-Components/Source/Components/Interfaces/DebugService/DebugService.cpp
T
Martino FerrariandClaude Opus 4.6 5562877c99 fix(udps): publish the producer's HRT frequency so timestamps survive the hop 
DATA packets timestamp with the raw value of the producer's high-resolution
counter, and the wire never said how fast that counter runs. The hub divided by
its own timer's frequency instead, which is only the same number while producer
and hub share a machine — on x86 it is the TSC frequency and differs from model
to model. Off-box, every accumulated batch was therefore laid out over the wrong
span of time: the samples in it drift away from where they belong and start
colliding with the next packet's, which is the "same" symptom as a stale time
base even though nothing is out of order.

CONFIG now carries the rate as a trailing uint64, alongside the publish-mode
byte and read the same tolerant way: absent or zero means the producer did not
say, and the hub falls back to its own timer as before. Anything below 1 kHz is
not a high-resolution timer and is refused, so a mis-parsed payload cannot
stretch a millisecond batch across seconds.

The Accumulate DATA payload is unchanged, so this costs nothing per packet and
the period *within* a batch is still estimated from the gap between packets.

The Go, C and browser parsers already ignore trailer bytes they do not know,
so they read the new CONFIG unchanged; none of them uses the HRT timestamp.

Also corrects the Accumulate DATA layout in all three protocol documents: they
described it as one snapshot per array signal, where it has always been one per
accumulated cycle.

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
2026-09-02 02:49:50 +02:00

708 lines
28 KiB
C++
Raw Blame History

#include "BasicTCPSocket.h"
#include "ConfigurationDatabase.h"
#include "DebugService.h"
#include "GlobalObjectsDatabase.h"
#include "HighResolutionTimer.h"
#include "LoggerService.h"
#include "Message.h"
#include "ObjectRegistryDatabase.h"
#include "ReferenceT.h"
#include "Sleep.h"
#include "StreamString.h"
#include "StringHelper.h"
#include "Threads.h"
#include "TimeoutType.h"
#include "UDPSProtocol.h"
#include <string.h>
namespace MARTe {
CLASS_REGISTER(DebugService, "1.0")
// C++98 ODR definitions for static constants
const uint32 DebugService::CMD_RATE_LIMIT;
const uint32 DebugService::CLIENT_IDLE_TIMEOUT_MS;
const uint32 DebugService::INPUT_BUFFER_MAX;
// ---------------------------------------------------------------------------
// Constructor / Destructor
// ---------------------------------------------------------------------------
DebugService::DebugService()
: DebugServiceBase(),
EmbeddedServiceMethodBinderI(),
binderServer(this, ServiceBinder::ServerType),
binderStreamer(this, ServiceBinder::StreamerType),
threadService(binderServer),
streamerService(binderStreamer) {
controlPort = 0u;
streamPort = 8081u;
logPort = 8082u;
streamIP = "127.0.0.1";
isServer = false;
suppressTimeoutLogs = true;
activeClient = NULL_PTR(BasicTCPSocket *);
cmdCountInWindow = 0u;
cmdWindowStartMs = 0u;
lastDataTimeMs = 0u;
inputBuffer = "";
// UDPS members
udpsNumSlots = 0u;
udpsDataPayload = NULL_PTR(uint8 *);
udpsDataPayloadSize = 0u;
udpsPacketCounter = 0u;
udpsConfigPending = false;
}
DebugService::~DebugService() {
if (DebugServiceI::GetInstance() == this) {
DebugServiceI::SetInstance(NULL_PTR(DebugServiceI *));
}
threadService.Stop();
streamerService.Stop();
tcpServer.Close();
(void) udpsServer.Stop();
if (activeClient != NULL_PTR(BasicTCPSocket *)) {
activeClient->Close();
delete activeClient;
activeClient = NULL_PTR(BasicTCPSocket *);
}
if (udpsDataPayload != NULL_PTR(uint8 *)) {
delete[] udpsDataPayload;
udpsDataPayload = NULL_PTR(uint8 *);
}
// signals owned by DebugServiceBase destructor
}
// ---------------------------------------------------------------------------
// Initialise
// ---------------------------------------------------------------------------
bool DebugService::Initialise(StructuredDataI &data) {
if (!ReferenceContainer::Initialise(data)) return false;
uint32 port = 0u;
if (data.Read("ControlPort", port)) {
controlPort = (uint16)port;
} else {
(void)data.Read("TcpPort", port);
controlPort = (uint16)port;
}
if (controlPort > 0u) {
isServer = true;
DebugServiceI::SetInstance(this);
}
port = 8081u;
if (data.Read("StreamPort", port)) {
streamPort = (uint16)port;
} else {
(void)data.Read("UdpPort", port);
streamPort = (uint16)port;
}
port = 8082u;
if (data.Read("LogPort", port)) {
logPort = (uint16)port;
} else {
(void)data.Read("TcpLogPort", port);
logPort = (uint16)port;
}
StreamString tempIP;
if (data.Read("StreamIP", tempIP)) {
streamIP = tempIP;
} else {
streamIP = "127.0.0.1";
}
uint32 suppress = 1u;
if (data.Read("SuppressTimeoutLogs", suppress)) {
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);
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;
// Initialise UDP streamer in push-only mode (port=0 skips serverSocket bind)
ConfigurationDatabase udpsConfig;
(void)udpsConfig.Write("Port", 0u);
(void)udpsConfig.Write("MaxPayloadSize", static_cast<uint32>(UDPSServer::UDPS_SERVER_DEFAULT_MAX_PAYLOAD));
if (!udpsServer.Initialise(udpsConfig)) return false;
if (!udpsServer.Start()) return false;
if (streamPort > 0u) {
(void)udpsServer.AddStaticClient(streamIP.Buffer(), streamPort);
}
if (threadService.Start() != ErrorManagement::NoError) return false;
if (streamerService.Start() != ErrorManagement::NoError) return false;
// Send initial (empty) CONFIG so clients know the schema version
SendUDPSConfig();
}
return true;
}
// ---------------------------------------------------------------------------
// Transport config hook (called by RebuildConfigFromRegistry in base)
// ---------------------------------------------------------------------------
void DebugService::RebuildTransportConfig() {
const char8 *myName = GetName();
if (myName != NULL_PTR(const char8 *)) {
if (fullConfig.MoveRelative(myName)) {
(void)fullConfig.Write("ControlPort", static_cast<uint32>(controlPort));
(void)fullConfig.Write("UdpPort", static_cast<uint32>(streamPort));
(void)fullConfig.Write("LogPort", static_cast<uint32>(logPort));
if (streamIP.Size() > 0u)
(void)fullConfig.Write("StreamIP", streamIP.Buffer());
(void)fullConfig.MoveToAncestor(1u);
}
}
}
// ---------------------------------------------------------------------------
// SERVICE_INFO hook
// ---------------------------------------------------------------------------
void DebugService::GetServiceInfo(StreamString &out) {
out.Printf("OK SERVICE_INFO TCP_CTRL:%u UDP_STREAM:%u TCP_LOG:%u STATE:%s\n",
controlPort, streamPort, logPort,
isPaused ? "PAUSED" : "RUNNING");
}
// ---------------------------------------------------------------------------
// Execute / HandleMessage (framework boilerplate)
// ---------------------------------------------------------------------------
ErrorManagement::ErrorType DebugService::Execute(ExecutionInfo &info) {
(void)info;
return ErrorManagement::FatalError;
}
ErrorManagement::ErrorType DebugService::HandleMessage(ReferenceT<Message> &data) {
(void)data;
return ErrorManagement::NoError;
}
// ---------------------------------------------------------------------------
// TraceSignal override — sets config-pending flag after base handling
// ---------------------------------------------------------------------------
uint32 DebugService::TraceSignal(const char8 *name, bool enable, uint32 decimation) {
uint32 ret = DebugServiceBase::TraceSignal(name, enable, decimation);
udpsConfigPending = true;
return ret;
}
// ---------------------------------------------------------------------------
// InjectTcpLoggerIfNeeded
// ---------------------------------------------------------------------------
void DebugService::InjectTcpLoggerIfNeeded() {
if (logPort == 0u) return;
Reference existing = ObjectRegistryDatabase::Instance()->Find("LoggerService");
if (existing.IsValid()) {
ReferenceContainer *rc =
dynamic_cast<ReferenceContainer *>(existing.operator->());
if (rc != NULL_PTR(ReferenceContainer *)) {
for (uint32 i = 0u; i < rc->Size(); i++) {
Reference child = rc->Get(i);
// Check by class name to avoid a direct symbol dependency on TcpLogger.so
if (child.IsValid()) {
const ClassProperties *cp = child->GetClassProperties();
if (cp != NULL_PTR(const ClassProperties *)) {
if (StringHelper::Compare(cp->GetName(), "TcpLogger") == 0) {
return; // already has a TcpLogger
}
}
}
}
}
}
ConfigurationDatabase lsCdb;
(void)lsCdb.Write("Class", "LoggerService");
uint32 cpus = 1u;
(void)lsCdb.Write("CPUs", cpus);
if (lsCdb.CreateRelative("+DebugConsumer")) {
(void)lsCdb.Write("Class", "TcpLogger");
uint32 p = static_cast<uint32>(logPort);
(void)lsCdb.Write("Port", p);
(void)lsCdb.MoveToAncestor(1u);
}
(void)lsCdb.MoveToRoot();
ReferenceT<LoggerService> ls(
"LoggerService", GlobalObjectsDatabase::Instance()->GetStandardHeap());
if (!ls.IsValid()) return;
ls->SetName("LoggerService");
if (!ls->Initialise(lsCdb)) return;
(void)ObjectRegistryDatabase::Instance()->Insert(ls);
}
// ---------------------------------------------------------------------------
// Server thread
// ---------------------------------------------------------------------------
ErrorManagement::ErrorType DebugService::Server(ExecutionInfo &info) {
if (info.GetStage() == ExecutionInfo::TerminationStage)
return ErrorManagement::NoError;
if (info.GetStage() == ExecutionInfo::StartupStage) {
serverThreadId = Threads::Id();
Sleep::MSec(500u);
InjectTcpLoggerIfNeeded();
return ErrorManagement::NoError;
}
uint64 nowMs = (uint64)((float64)HighResolutionTimer::Counter() *
HighResolutionTimer::Period() * 1000.0);
if (activeClient == NULL_PTR(BasicTCPSocket *)) {
BasicTCPSocket *newClient = tcpServer.WaitConnection(TimeoutType(100));
if (newClient != NULL_PTR(BasicTCPSocket *)) {
clientMutex.FastLock();
activeClient = newClient;
clientMutex.FastUnLock();
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) {
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"Server: TCP client idle for >%u ms — closing connection.",
CLIENT_IDLE_TIMEOUT_MS);
inputBuffer = "";
clientMutex.FastLock();
activeClient->Close();
delete activeClient;
activeClient = NULL_PTR(BasicTCPSocket *);
clientMutex.FastUnLock();
cmdCountInWindow = 0u;
} else if (!activeClient->IsConnected()) {
inputBuffer = "";
clientMutex.FastLock();
activeClient->Close();
delete activeClient;
activeClient = NULL_PTR(BasicTCPSocket *);
clientMutex.FastUnLock();
} else {
char buffer[1024];
uint32 size = 1024u;
if (activeClient->Read(buffer, size)) {
if (size > 0u) {
lastDataTimeMs = nowMs;
if (nowMs - cmdWindowStartMs >= 1000u) {
cmdWindowStartMs = nowMs;
cmdCountInWindow = 0u;
}
if (inputBuffer.Size() + (uint32)size > INPUT_BUFFER_MAX) {
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"Server: input buffer overflow (>%u bytes without newline) "
"— disconnecting.", INPUT_BUFFER_MAX);
inputBuffer = "";
clientMutex.FastLock();
activeClient->Close();
delete activeClient;
activeClient = NULL_PTR(BasicTCPSocket *);
clientMutex.FastUnLock();
cmdCountInWindow = 0u;
} else {
(void)inputBuffer.Seek(inputBuffer.Size());
uint32 writeSize = (uint32)size;
inputBuffer.Write(buffer, writeSize);
const char8 *raw = inputBuffer.Buffer();
uint32 total = (uint32)inputBuffer.Size();
uint32 lineStart = 0u;
bool rateLimitExceeded = false;
for (uint32 pos = 0u; pos < total && !rateLimitExceeded; pos++) {
if (raw[pos] != '\n') continue;
uint32 len = pos - lineStart;
if (len > 0u && raw[lineStart + len - 1u] == '\r') len--;
if (len > 0u) {
cmdCountInWindow++;
if (cmdCountInWindow > CMD_RATE_LIMIT) {
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"Server: client exceeded rate limit (%u cmd/s) "
"— disconnecting.", CMD_RATE_LIMIT);
rateLimitExceeded = true;
break;
}
StreamString command;
uint32 cmdLen = len;
command.Write(raw + lineStart, cmdLen);
/* 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--;
}
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);
}
}
}
}
lineStart = pos + 1u;
}
if (rateLimitExceeded) {
inputBuffer = "";
clientMutex.FastLock();
activeClient->Close();
delete activeClient;
activeClient = NULL_PTR(BasicTCPSocket *);
clientMutex.FastUnLock();
cmdCountInWindow = 0u;
} else {
StreamString newInputBuffer;
if (lineStart < total) {
uint32 remLen = total - lineStart;
newInputBuffer.Write(raw + lineStart, remLen);
}
inputBuffer = newInputBuffer;
}
}
}
} else {
inputBuffer = "";
clientMutex.FastLock();
activeClient->Close();
delete activeClient;
activeClient = NULL_PTR(BasicTCPSocket *);
clientMutex.FastUnLock();
}
}
}
return ErrorManagement::NoError;
}
// ---------------------------------------------------------------------------
// Streamer thread — UDPS binary telemetry
// ---------------------------------------------------------------------------
ErrorManagement::ErrorType DebugService::Streamer(ExecutionInfo &info) {
if (info.GetStage() == ExecutionInfo::TerminationStage) {
if (udpsDataPayload != NULL_PTR(uint8 *)) {
delete[] udpsDataPayload;
udpsDataPayload = NULL_PTR(uint8 *);
}
return ErrorManagement::NoError;
}
if (info.GetStage() == ExecutionInfo::StartupStage) {
streamerThreadId = Threads::Id();
return ErrorManagement::NoError;
}
// a) Poll for new CONNECT clients (no-op in push-only mode with port=0)
udpsServer.ServiceClients();
// b) If config has changed, rebuild and send CONFIG packet
if (udpsConfigPending) {
SendUDPSConfig();
udpsConfigPending = false;
}
// b) Drain traceBuffer — pack each sample into udpsDataPayload
bool anyData = false;
bool pendingInDrain[UDPS_MAX_SLOTS];
for (uint32 i = 0u; i < udpsNumSlots; i++) {
pendingInDrain[i] = false;
}
uint32 id, size;
uint64 ts;
uint8 udpsSampleBuf[UDPS_MAX_SAMPLE_BYTES];
while (traceBuffer.Pop(id, ts, udpsSampleBuf, size, UDPS_MAX_SAMPLE_BYTES)) {
// Find matching slot by internalID
for (uint32 i = 0u; i < udpsNumSlots; i++) {
if (udpsSlots[i].internalID == id) {
if (pendingInDrain[i]) {
// This slot already holds an unflushed sample from earlier
// in this same drain pass — flush it now instead of
// silently overwriting it, or lossless tracing would drop
// a real sample whenever the Streamer thread falls behind
// by more than one RT cycle.
FlushUdpsFrame();
for (uint32 j = 0u; j < udpsNumSlots; j++) {
pendingInDrain[j] = false;
}
anyData = false;
}
if ((udpsDataPayload != NULL_PTR(uint8 *)) &&
(8u + udpsSlots[i].wireOffset + udpsSlots[i].wireSize <= udpsDataPayloadSize)) {
uint32 copySize = size;
if (copySize > udpsSlots[i].wireSize) copySize = udpsSlots[i].wireSize;
memcpy(udpsDataPayload + 8u + udpsSlots[i].wireOffset, udpsSampleBuf, copySize);
udpsSlots[i].everFilled = true;
}
pendingInDrain[i] = true;
anyData = true;
break;
}
}
}
// c) If we have data, stamp with HRT and send via udpsServer
if (anyData) {
FlushUdpsFrame();
} else {
Sleep::MSec(1u);
}
return ErrorManagement::NoError;
}
void DebugService::FlushUdpsFrame() {
if (udpsNumSlots > 0u && udpsDataPayload != NULL_PTR(uint8 *)) {
uint64 hrt = HighResolutionTimer::Counter();
memcpy(udpsDataPayload, &hrt, 8u);
udpsPacketCounter++;
(void)udpsServer.SendData(udpsPacketCounter, udpsDataPayload, udpsDataPayloadSize);
}
}
// ---------------------------------------------------------------------------
// SendUDPSConfig — build and send a UDPS CONFIG packet
// ---------------------------------------------------------------------------
bool DebugService::SendUDPSConfig() {
// Snapshot traced signals under mutex
mutex.FastLock();
// Count traced signals
uint32 tracedCount = 0u;
for (uint32 i = 0u; i < signals.GetNumberOfElements(); i++) {
if (signals[i]->isTracing) {
tracedCount++;
}
}
// Build CONFIG payload into a local stack buffer
static const uint32 CFG_BUF_SIZE = 65535u;
uint8 cfgBuf[CFG_BUF_SIZE];
uint8 *payload = cfgBuf;
// Write numTraced (uint32 LE)
memcpy(payload, &tracedCount, 4u);
uint32 payloadOffset = 4u;
// Build slot table in parallel
uint32 currentOffset = 0u;
uint32 newNumSlots = 0u;
uint32 totalWireBytes = 0u;
for (uint32 i = 0u; i < signals.GetNumberOfElements(); i++) {
if (!signals[i]->isTracing) continue;
if (newNumSlots >= UDPS_MAX_SLOTS) break;
uint8 typeCode = UDPSTypeDescriptorToCode(signals[i]->type);
if (typeCode == UDPS_TYPECODE_UNKNOWN) {
// Skip unsupported types
continue;
}
uint32 numElements = signals[i]->numberOfElements;
if (numElements == 0u) numElements = 1u;
uint32 numRows, numCols;
if (signals[i]->numberOfDimensions >= 2u) {
// For matrix: approximate square root
numCols = numElements;
numRows = 1u;
} else if (signals[i]->numberOfDimensions == 1u) {
numRows = numElements;
numCols = 1u;
} else {
numRows = 1u;
numCols = 1u;
}
uint32 wireSize = UDPSTypeCodeByteSize(typeCode) * numElements;
// Write signal descriptor (136 bytes) to payload
if (payloadOffset + UDPS_SIGNAL_DESC_SIZE <= CFG_BUF_SIZE - 1u) {
UDPSSignalDescriptor *desc = reinterpret_cast<UDPSSignalDescriptor *>(payload + payloadOffset);
memset(desc, 0, UDPS_SIGNAL_DESC_SIZE);
if (signals[i]->name.Size() > 0u) {
strncpy(desc->name, signals[i]->name.Buffer(), UDPS_MAX_SIGNAL_NAME - 1u);
}
desc->typeCode = typeCode;
desc->quantType = UDPS_QUANT_NONE;
desc->numDimensions = signals[i]->numberOfDimensions;
desc->numRows = numRows;
desc->numCols = numCols;
desc->rangeMin = 0.0;
desc->rangeMax = 0.0;
desc->timeMode = UDPS_TIMEMODE_PACKET;
desc->samplingRate = 0.0;
desc->timeSignalIdx = UDPS_NO_TIME_SIGNAL;
// unit stays zero
payloadOffset += UDPS_SIGNAL_DESC_SIZE;
}
// Fill slot table entry
udpsSlots[newNumSlots].internalID = signals[i]->internalID;
udpsSlots[newNumSlots].wireOffset = currentOffset;
udpsSlots[newNumSlots].wireSize = wireSize;
udpsSlots[newNumSlots].everFilled = false;
newNumSlots++;
currentOffset += wireSize;
totalWireBytes += wireSize;
}
// Write publish mode byte
if (payloadOffset < CFG_BUF_SIZE) {
payload[payloadOffset] = UDPS_PUBLISH_STRICT;
payloadOffset++;
}
// Write this host's HRT tick rate: DATA packets carry raw counter values,
// which a receiver on another machine cannot convert to seconds without it.
if ((payloadOffset + 8u) <= CFG_BUF_SIZE) {
uint64 hrtFrequency = HighResolutionTimer::Frequency();
memcpy(payload + payloadOffset, &hrtFrequency, 8u);
payloadOffset += 8u;
}
udpsNumSlots = newNumSlots;
mutex.FastUnLock();
// Reallocate data payload buffer
if (udpsDataPayload != NULL_PTR(uint8 *)) {
delete[] udpsDataPayload;
udpsDataPayload = NULL_PTR(uint8 *);
}
udpsDataPayloadSize = 8u + totalWireBytes; // 8 bytes HRT + signal data
if (udpsDataPayloadSize > 0u) {
udpsDataPayload = new uint8[udpsDataPayloadSize];
memset(udpsDataPayload, 0, udpsDataPayloadSize);
}
// Send CONFIG packet (also cached in udpsServer for new CONNECT clients)
udpsPacketCounter++;
(void)udpsServer.SendConfig(payload, payloadOffset);
return true;
}
} // namespace MARTe
Reference in New Issue View Git Blame Copy Permalink