martino/MARTe-Integrated-Components
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
0ab49078cc51cf6ad9d0bbe1198a580ddec47b03
MARTe-Integrated-Components/Source/Components/Interfaces/DebugService/DebugService.cpp
T
Martino Ferrari f25bd7f08e Implemented new C++ logic
2026-06-12 15:25:13 +02:00

592 lines
22 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"
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);
}
// 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;
}
} 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);
// 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;
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 ((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;
}
anyData = true;
break;
}
}
}
// c) If we have data, stamp with HRT and send via udpsServer
if (anyData && udpsNumSlots > 0u && udpsDataPayload != NULL_PTR(uint8 *)) {
uint64 hrt = HighResolutionTimer::Counter();
memcpy(udpsDataPayload, &hrt, 8u);
udpsPacketCounter++;
(void)udpsServer.SendData(udpsPacketCounter, udpsDataPayload, udpsDataPayloadSize);
}
if (!anyData) {
Sleep::MSec(1u);
}
return ErrorManagement::NoError;
}
// ---------------------------------------------------------------------------
// 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++;
}
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