#include "DebugService.h" #include "StandardParser.h" #include "StreamString.h" #include "BasicSocket.h" #include "DebugBrokerWrapper.h" #include "ObjectRegistryDatabase.h" #include "ClassRegistryItem.h" #include "ObjectBuilder.h" #include "TypeConversion.h" #include "HighResolutionTimer.h" #include "ConfigurationDatabase.h" #include "GAM.h" #include "Atomic.h" namespace MARTe { DebugService* GlobalDebugServiceInstance = NULL_PTR(DebugService*); static void EscapeJson(const char8* src, StreamString &dst) { if (src == NULL_PTR(const char8*)) return; while (*src != '\0') { if (*src == '"') dst += "\\\""; else if (*src == '\\') dst += "\\\\"; else if (*src == '\n') dst += "\\n"; else if (*src == '\r') dst += "\\r"; else if (*src == '\t') dst += "\\t"; else dst += *src; src++; } } static bool SuffixMatch(const char8* target, const char8* pattern) { uint32 tLen = StringHelper::Length(target); uint32 pLen = StringHelper::Length(pattern); if (pLen > tLen) return false; const char8* suffix = target + (tLen - pLen); if (StringHelper::Compare(suffix, pattern) == 0) { if (tLen == pLen || *(suffix - 1) == '.') return true; } return false; } static bool RecursiveGetFullObjectName(ReferenceContainer *container, const Object &obj, StreamString &path) { uint32 size = container->Size(); for (uint32 i=0; iGet(i); if (child.IsValid()) { if (child.operator->() == &obj) { path = child->GetName(); return true; } ReferenceContainer *inner = dynamic_cast(child.operator->()); if (inner) { if (RecursiveGetFullObjectName(inner, obj, path)) { StreamString prefix = child->GetName(); prefix += "."; prefix += path; path = prefix; return true; } } } } return false; } bool DebugService::GetFullObjectName(const Object &obj, StreamString &fullPath) { fullPath = ""; if (RecursiveGetFullObjectName(ObjectRegistryDatabase::Instance(), obj, fullPath)) return true; return false; } CLASS_REGISTER(DebugService, "1.0") DebugService* DebugService::Instance() { return GlobalDebugServiceInstance; } void PatchItemInternal(const char8* className, ObjectBuilder* builder) { ClassRegistryDatabase *db = ClassRegistryDatabase::Instance(); ClassRegistryItem *item = (ClassRegistryItem*)db->Find(className); if (item != NULL_PTR(ClassRegistryItem*)) { item->SetObjectBuilder(builder); } } uint32 DebugService::ForceSignal(const char8* name, const char8* valueStr) { mutex.FastLock(); uint32 count = 0; for (uint32 i = 0; i < numberOfAliases; i++) { if (aliases[i].name == name || SuffixMatch(aliases[i].name.Buffer(), name)) { DebugSignalInfo &s = signals[aliases[i].signalIndex]; s.isForcing = true; AnyType dest(s.type, 0u, s.forcedValue); AnyType source(CharString, 0u, valueStr); (void)TypeConvert(dest, source); count++; } } mutex.FastUnLock(); return count; } uint32 DebugService::UnforceSignal(const char8* name) { mutex.FastLock(); uint32 count = 0; for (uint32 i = 0; i < numberOfAliases; i++) { if (aliases[i].name == name || SuffixMatch(aliases[i].name.Buffer(), name)) { signals[aliases[i].signalIndex].isForcing = false; count++; } } mutex.FastUnLock(); return count; } uint32 DebugService::TraceSignal(const char8* name, bool enable, uint32 decimation) { mutex.FastLock(); uint32 count = 0; for (uint32 i = 0; i < numberOfAliases; i++) { if (aliases[i].name == name || SuffixMatch(aliases[i].name.Buffer(), name)) { uint32 sigIdx = aliases[i].signalIndex; DebugSignalInfo &s = signals[sigIdx]; s.isTracing = enable; s.decimationFactor = decimation; s.decimationCounter = 0; count++; } } mutex.FastUnLock(); return count; } ErrorManagement::ErrorType DebugService::Execute(ExecutionInfo & info) { return ErrorManagement::FatalError; } ErrorManagement::ErrorType DebugService::Server(ExecutionInfo & info) { if (info.GetStage() == ExecutionInfo::TerminationStage) return ErrorManagement::NoError; if (info.GetStage() == ExecutionInfo::StartupStage) { serverThreadId = Threads::Id(); return ErrorManagement::NoError; } while (info.GetStage() == ExecutionInfo::MainStage) { BasicTCPSocket *newClient = tcpServer.WaitConnection(10); if (newClient != NULL_PTR(BasicTCPSocket *)) { clientsMutex.FastLock(); bool added = false; for (uint32 i=0; iClose(); delete newClient; } } for (uint32 i=0; iRead(buffer, size, timeout) && size > 0) { StreamString command; command.Write(buffer, size); HandleCommand(command, client); } else if (!client->IsValid()) { clientsMutex.FastLock(); client->Close(); delete client; activeClients[i] = NULL_PTR(BasicTCPSocket*); clientsMutex.FastUnLock(); } } } Sleep::MSec(10); } return ErrorManagement::NoError; } ErrorManagement::ErrorType DebugService::Streamer(ExecutionInfo & info) { if (info.GetStage() == ExecutionInfo::TerminationStage) return ErrorManagement::NoError; if (info.GetStage() == ExecutionInfo::StartupStage) { streamerThreadId = Threads::Id(); return ErrorManagement::NoError; } InternetHost dest(streamPort, streamIP.Buffer()); (void)udpSocket.SetDestination(dest); uint8 packetBuffer[4096]; uint32 packetOffset = 0; uint32 sequenceNumber = 0; while (info.GetStage() == ExecutionInfo::MainStage) { uint32 id, size; uint64 ts; uint8 sampleData[1024]; bool hasData = false; while ((info.GetStage() == ExecutionInfo::MainStage) && traceBuffer.Pop(id, ts, sampleData, size, 1024)) { hasData = true; if (packetOffset == 0) { TraceHeader header; header.magic = 0xDA7A57AD; header.seq = sequenceNumber++; header.timestamp = HighResolutionTimer::Counter(); header.count = 0; std::memcpy(packetBuffer, &header, sizeof(TraceHeader)); packetOffset = sizeof(TraceHeader); } if (packetOffset + 16 + size > 1400) { uint32 toWrite = packetOffset; (void)udpSocket.Write((char8*)packetBuffer, toWrite); TraceHeader header; header.magic = 0xDA7A57AD; header.seq = sequenceNumber++; header.timestamp = HighResolutionTimer::Counter(); header.count = 0; std::memcpy(packetBuffer, &header, sizeof(TraceHeader)); packetOffset = sizeof(TraceHeader); } std::memcpy(&packetBuffer[packetOffset], &id, 4); std::memcpy(&packetBuffer[packetOffset + 4], &ts, 8); std::memcpy(&packetBuffer[packetOffset + 12], &size, 4); std::memcpy(&packetBuffer[packetOffset + 16], sampleData, size); packetOffset += (16 + size); ((TraceHeader*)packetBuffer)->count++; } if (packetOffset > 0) { uint32 toWrite = packetOffset; (void)udpSocket.Write((char8*)packetBuffer, toWrite); packetOffset = 0; } if (!hasData) Sleep::MSec(1); } return ErrorManagement::NoError; } DebugService::DebugService() : ReferenceContainer(), EmbeddedServiceMethodBinderI(), binderServer(this, ServiceBinder::ServerType), binderStreamer(this, ServiceBinder::StreamerType), threadService(binderServer), streamerService(binderStreamer) { GlobalDebugServiceInstance = this; controlPort = 0; streamPort = 8081; streamIP = "127.0.0.1"; numberOfSignals = 0; numberOfAliases = 0; numberOfBrokers = 0; isServer = false; suppressTimeoutLogs = true; isPaused = false; for (uint32 i=0; i B1; PatchItemInternal("MemoryMapInputBroker", new B1()); typedef DebugBrokerBuilder B2; PatchItemInternal("MemoryMapOutputBroker", new B2()); typedef DebugBrokerBuilder B3; PatchItemInternal("MemoryMapSynchronisedInputBroker", new B3()); typedef DebugBrokerBuilder B4; PatchItemInternal("MemoryMapSynchronisedOutputBroker", new B4()); typedef DebugBrokerBuilder B5; PatchItemInternal("MemoryMapInterpolatedInputBroker", new B5()); } DebugService::~DebugService() { if (GlobalDebugServiceInstance == this) GlobalDebugServiceInstance = NULL_PTR(DebugService*); threadService.Stop(); streamerService.Stop(); tcpServer.Close(); udpSocket.Close(); for (uint32 i=0; iClose(); delete activeClients[i]; } } for (uint32 i=0; i 0) { isServer = true; GlobalDebugServiceInstance = this; } if (!data.Read("StreamPort", streamPort)) (void)data.Read("UdpPort", streamPort); StreamString tempIP; if (data.Read("StreamIP", tempIP)) streamIP = tempIP; else streamIP = "127.0.0.1"; uint32 suppress = 1; if (data.Read("SuppressTimeoutLogs", suppress)) suppressTimeoutLogs = (suppress == 1); if (isServer) { if (!traceBuffer.Init(8 * 1024 * 1024)) return false; ConfigurationDatabase threadData; threadData.Write("Timeout", (uint32)1000); threadService.Initialise(threadData); streamerService.Initialise(threadData); if (!tcpServer.Open()) return false; if (!tcpServer.Listen(controlPort)) return false; printf("[DebugService] TCP Server listening on port %u\n", controlPort); if (!udpSocket.Open()) return false; printf("[DebugService] UDP Streamer socket opened\n"); if (threadService.Start() != ErrorManagement::NoError) return false; if (streamerService.Start() != ErrorManagement::NoError) return false; printf("[DebugService] Worker threads started.\n"); } return true; } void DebugService::PatchRegistry() { } void DebugService::ProcessSignal(DebugSignalInfo* s, uint32 size, uint64 timestamp) { if (s == NULL_PTR(DebugSignalInfo*)) return; if (s->isForcing) CopySignal(s->memoryAddress, s->forcedValue, size); if (s->isTracing) { if (s->decimationFactor <= 1) (void)traceBuffer.Push(s->internalID, timestamp, s->memoryAddress, size); else { if (s->decimationCounter == 0) { (void)traceBuffer.Push(s->internalID, timestamp, s->memoryAddress, size); s->decimationCounter = s->decimationFactor - 1; } else s->decimationCounter--; } } } void DebugService::RegisterBroker(DebugSignalInfo** signalPointers, uint32 numSignals, MemoryMapBroker* broker, volatile bool* anyActiveFlag) { mutex.FastLock(); for (uint32 i=0; iSize(); i++) { Reference child = container->Get(i); if (child.IsValid()) { DebugBrokerI* b = dynamic_cast(child.operator->()); if (b && !b->IsLinked()) { b->SetService(service); } ReferenceContainer *inner = dynamic_cast(child.operator->()); if (inner) AdoptOrphans(inner, service); } } } void DebugService::UpdateBrokersActiveStatus() { AdoptOrphans(ObjectRegistryDatabase::Instance(), this); for (uint32 i = 0; i < numberOfBrokers; i++) { uint32 nextIdx = (brokers[i].currentSetIdx + 1) % 2; BrokerActiveSet& nextSet = brokers[i].sets[nextIdx]; uint32 forcedCount = 0; uint32 tracedCount = 0; for (uint32 j = 0; j < brokers[i].numSignals; j++) { DebugSignalInfo *s = brokers[i].signalPointers[j]; if (s != NULL_PTR(DebugSignalInfo*)) { if (s->isForcing) forcedCount++; if (s->isTracing) tracedCount++; } } SignalExecuteInfo* newForced = (forcedCount > 0) ? new SignalExecuteInfo[forcedCount] : NULL_PTR(SignalExecuteInfo*); SignalExecuteInfo* newTraced = (tracedCount > 0) ? new SignalExecuteInfo[tracedCount] : NULL_PTR(SignalExecuteInfo*); uint32 fIdx = 0; uint32 tIdx = 0; for (uint32 j = 0; j < brokers[i].numSignals; j++) { DebugSignalInfo *s = brokers[i].signalPointers[j]; if (s != NULL_PTR(DebugSignalInfo*)) { uint32 size = (brokers[i].broker != NULL_PTR(MemoryMapBroker*)) ? brokers[i].broker->GetCopyByteSize(j) : 4; if (s->isForcing) { newForced[fIdx].memoryAddress = s->memoryAddress; newForced[fIdx].forcedValue = s->forcedValue; newForced[fIdx].size = size; fIdx++; } if (s->isTracing) { newTraced[tIdx].memoryAddress = s->memoryAddress; newTraced[tIdx].internalID = s->internalID; newTraced[tIdx].size = size; tIdx++; } } } SignalExecuteInfo* oldForced = nextSet.forcedSignals; SignalExecuteInfo* oldTraced = nextSet.tracedSignals; nextSet.forcedSignals = newForced; nextSet.tracedSignals = newTraced; nextSet.numForced = forcedCount; nextSet.numTraced = tracedCount; Atomic::Exchange((int32*)&brokers[i].currentSetIdx, (int32)nextIdx); if (brokers[i].anyActiveFlag) *(brokers[i].anyActiveFlag) = (forcedCount > 0 || tracedCount > 0); if (oldForced) delete[] oldForced; if (oldTraced) delete[] oldTraced; } } DebugSignalInfo* DebugService::RegisterSignal(void* memoryAddress, TypeDescriptor type, const char8* name) { mutex.FastLock(); DebugSignalInfo* res = NULL_PTR(DebugSignalInfo*); uint32 sigIdx = 0xFFFFFFFF; for (uint32 i=0; imemoryAddress == NULL && memoryAddress != NULL) res->memoryAddress = memoryAddress; break; } } if (res == NULL_PTR(DebugSignalInfo*) && numberOfSignals < MAX_SIGNALS) { sigIdx = numberOfSignals; res = &signals[numberOfSignals]; res->memoryAddress = memoryAddress; res->type = type; res->name = name; res->isTracing = false; res->isForcing = false; res->internalID = numberOfSignals; res->decimationFactor = 1; res->decimationCounter = 0; numberOfSignals++; } if (sigIdx != 0xFFFFFFFF && numberOfAliases < MAX_ALIASES) { bool foundAlias = false; for (uint32 i=0; iSize(); i++) { Reference child = container->Get(i); if (child.IsValid()) { DataSourceI *ds = dynamic_cast(child.operator->()); if (ds) { StreamString dsPath; if (DebugService::GetFullObjectName(*ds, dsPath)) { for (uint32 j=0; jGetNumberOfSignals(); j++) { StreamString sname; (void)ds->GetSignalName(j, sname); StreamString fullName = dsPath; fullName += "."; fullName += sname; service->RegisterSignal(NULL, ds->GetSignalType(j), fullName.Buffer()); } } } ReferenceContainer *inner = dynamic_cast(child.operator->()); if (inner) RecursivePopulate(inner, service); } } } void DebugService::Discover(BasicTCPSocket *client) { if (client) { RecursivePopulate(ObjectRegistryDatabase::Instance(), this); StreamString json; json = "{\n \"Signals\": [\n"; mutex.FastLock(); for (uint32 i = 0; i < numberOfAliases; i++) { DebugSignalInfo &sig = signals[aliases[i].signalIndex]; const char8* typeName = TypeDescriptor::GetTypeNameFromTypeDescriptor(sig.type); StreamString line; line.Printf(" {\"name\": \"%s\", \"id\": %u, \"type\": \"%s\", \"ready\": %s}", aliases[i].name.Buffer(), sig.internalID, typeName ? typeName : "Unknown", (sig.memoryAddress != NULL) ? "true" : "false"); json += line; if (i < numberOfAliases - 1) json += ","; json += "\n"; } mutex.FastUnLock(); json += " ]\n}\nOK DISCOVER\n"; uint32 s = json.Size(); (void)client->Write(json.Buffer(), s); } } void DebugService::HandleCommand(StreamString cmd, BasicTCPSocket *client) { StreamString token; cmd.Seek(0); char8 term; const char8* delims = " \r\n"; if (cmd.GetToken(token, delims, term)) { if (token == "FORCE") { StreamString name, val; if (cmd.GetToken(name, delims, term) && cmd.GetToken(val, delims, term)) { uint32 count = ForceSignal(name.Buffer(), val.Buffer()); if (client) { StreamString resp; resp.Printf("OK FORCE %u\n", count); uint32 s = resp.Size(); (void)client->Write(resp.Buffer(), s); } } } else if (token == "UNFORCE") { StreamString name; if (cmd.GetToken(name, delims, term)) { uint32 count = UnforceSignal(name.Buffer()); if (client) { StreamString resp; resp.Printf("OK UNFORCE %u\n", count); uint32 s = resp.Size(); (void)client->Write(resp.Buffer(), s); } } } else if (token == "TRACE") { StreamString name, state, decim; if (cmd.GetToken(name, delims, term) && cmd.GetToken(state, delims, term)) { bool enable = (state == "1"); uint32 d = 1; if (cmd.GetToken(decim, delims, term)) { AnyType decimVal(UnsignedInteger32Bit, 0u, &d); AnyType decimStr(CharString, 0u, decim.Buffer()); (void)TypeConvert(decimVal, decimStr); } uint32 count = TraceSignal(name.Buffer(), enable, d); if (client) { StreamString resp; resp.Printf("OK TRACE %u\n", count); uint32 s = resp.Size(); (void)client->Write(resp.Buffer(), s); } } } else if (token == "DISCOVER") Discover(client); else if (token == "PAUSE") { SetPaused(true); if (client) { uint32 s = 3; (void)client->Write("OK\n", s); } } else if (token == "RESUME") { SetPaused(false); if (client) { uint32 s = 3; (void)client->Write("OK\n", s); } } else if (token == "TREE") { StreamString json; json = "{\"Name\": \"Root\", \"Class\": \"ObjectRegistryDatabase\", \"Children\": [\n"; (void)ExportTree(ObjectRegistryDatabase::Instance(), json); json += "\n]}\nOK TREE\n"; uint32 s = json.Size(); if (client) (void)client->Write(json.Buffer(), s); } else if (token == "INFO") { StreamString path; if (cmd.GetToken(path, delims, term)) InfoNode(path.Buffer(), client); } else if (token == "LS") { StreamString path; if (cmd.GetToken(path, delims, term)) ListNodes(path.Buffer(), client); else ListNodes(NULL_PTR(const char8*), client); } UpdateBrokersActiveStatus(); } } void DebugService::InfoNode(const char8* path, BasicTCPSocket *client) { if (!client) return; Reference ref = ObjectRegistryDatabase::Instance()->Find(path); StreamString json = "{"; if (ref.IsValid()) { json += "\"Name\": \""; EscapeJson(ref->GetName(), json); json += "\", \"Class\": \""; EscapeJson(ref->GetClassProperties()->GetName(), json); json += "\""; ConfigurationDatabase db; if (ref->ExportData(db)) { json += ", \"Config\": {"; db.MoveToRoot(); uint32 nChildren = db.GetNumberOfChildren(); for (uint32 i=0; iWrite(json.Buffer(), s); } uint32 DebugService::ExportTree(ReferenceContainer *container, StreamString &json) { if (container == NULL_PTR(ReferenceContainer*)) return 0; uint32 size = container->Size(); uint32 validCount = 0; for (uint32 i = 0u; i < size; i++) { Reference child = container->Get(i); if (child.IsValid()) { if (validCount > 0u) json += ",\n"; StreamString nodeJson; const char8* cname = child->GetName(); if (cname == NULL_PTR(const char8*)) cname = "unnamed"; nodeJson += "{\"Name\": \""; EscapeJson(cname, nodeJson); nodeJson += "\", \"Class\": \""; EscapeJson(child->GetClassProperties()->GetName(), nodeJson); nodeJson += "\""; ReferenceContainer *inner = dynamic_cast(child.operator->()); DataSourceI *ds = dynamic_cast(child.operator->()); GAM *gam = dynamic_cast(child.operator->()); if ((inner != NULL_PTR(ReferenceContainer*)) || (ds != NULL_PTR(DataSourceI*)) || (gam != NULL_PTR(GAM*))) { nodeJson += ", \"Children\": [\n"; uint32 subCount = 0u; if (inner != NULL_PTR(ReferenceContainer*)) subCount += ExportTree(inner, nodeJson); if (ds != NULL_PTR(DataSourceI*)) { uint32 nSignals = ds->GetNumberOfSignals(); for (uint32 j = 0u; j < nSignals; j++) { if (subCount > 0u) nodeJson += ",\n"; subCount++; StreamString sname; (void)ds->GetSignalName(j, sname); const char8* stype = TypeDescriptor::GetTypeNameFromTypeDescriptor(ds->GetSignalType(j)); uint8 dims = 0u; (void)ds->GetSignalNumberOfDimensions(j, dims); uint32 elems = 0u; (void)ds->GetSignalNumberOfElements(j, elems); nodeJson += "{\"Name\": \""; EscapeJson(sname.Buffer(), nodeJson); nodeJson += "\", \"Class\": \"Signal\", \"Type\": \""; EscapeJson(stype ? stype : "Unknown", nodeJson); nodeJson.Printf("\", \"Dimensions\": %d, \"Elements\": %u}", dims, elems); } } if (gam != NULL_PTR(GAM*)) { uint32 nIn = gam->GetNumberOfInputSignals(); for (uint32 j = 0u; j < nIn; j++) { if (subCount > 0u) nodeJson += ",\n"; subCount++; StreamString sname; (void)gam->GetSignalName(InputSignals, j, sname); const char8* stype = TypeDescriptor::GetTypeNameFromTypeDescriptor(gam->GetSignalType(InputSignals, j)); uint32 dims = 0u; (void)gam->GetSignalNumberOfDimensions(InputSignals, j, dims); uint32 elems = 0u; (void)gam->GetSignalNumberOfElements(InputSignals, j, elems); nodeJson += "{\"Name\": \"In."; EscapeJson(sname.Buffer(), nodeJson); nodeJson += "\", \"Class\": \"InputSignal\", \"Type\": \""; EscapeJson(stype ? stype : "Unknown", nodeJson); nodeJson.Printf("\", \"Dimensions\": %u, \"Elements\": %u}", dims, elems); } uint32 nOut = gam->GetNumberOfOutputSignals(); for (uint32 j = 0u; j < nOut; j++) { if (subCount > 0u) nodeJson += ",\n"; subCount++; StreamString sname; (void)gam->GetSignalName(OutputSignals, j, sname); const char8* stype = TypeDescriptor::GetTypeNameFromTypeDescriptor(gam->GetSignalType(OutputSignals, j)); uint32 dims = 0u; (void)gam->GetSignalNumberOfDimensions(OutputSignals, j, dims); uint32 elems = 0u; (void)gam->GetSignalNumberOfElements(OutputSignals, j, elems); nodeJson += "{\"Name\": \"Out."; EscapeJson(sname.Buffer(), nodeJson); nodeJson += "\", \"Class\": \"OutputSignal\", \"Type\": \""; EscapeJson(stype ? stype : "Unknown", nodeJson); nodeJson.Printf("\", \"Dimensions\": %u, \"Elements\": %u}", dims, elems); } } nodeJson += "\n]"; } nodeJson += "}"; json += nodeJson; validCount++; } } return validCount; } }