#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 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(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(controlPort)); (void)fullConfig.Write("UdpPort", static_cast(streamPort)); (void)fullConfig.Write("LogPort", static_cast(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 &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(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(logPort); (void)lsCdb.Write("Port", p); (void)lsCdb.MoveToAncestor(1u); } (void)lsCdb.MoveToRoot(); ReferenceT 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 " 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( strlen(okResp)); (void) activeClient->Write( okResp, respSz); } else { const char8 *badResp = "ERR AUTH_FAILED\n"; uint32 respSz = static_cast( strlen(badResp)); (void) activeClient->Write( badResp, respSz); } } else if (!clientAuthenticated) { const char8 *needAuth = "ERR AUTH_REQUIRED\n"; uint32 respSz = static_cast( 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(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