Project fixes and correct MARTe style makefile and source structure
This commit is contained in:
Martino Ferrari
972
Source/Components/Interfaces/DebugService/DebugService.cpp
Normal file
972
Source/Components/Interfaces/DebugService/DebugService.cpp
Normal file
@@ -0,0 +1,972 @@
|
||||
#include "BasicTCPSocket.h"
|
||||
#include "ClassRegistryItem.h"
|
||||
#include "ConfigurationDatabase.h"
|
||||
#include "DebugBrokerWrapper.h"
|
||||
#include "DebugService.h"
|
||||
#include "GAM.h"
|
||||
#include "HighResolutionTimer.h"
|
||||
#include "ObjectBuilder.h"
|
||||
#include "ObjectRegistryDatabase.h"
|
||||
#include "StreamString.h"
|
||||
#include "TimeoutType.h"
|
||||
#include "TypeConversion.h"
|
||||
|
||||
namespace MARTe {
|
||||
|
||||
DebugService *DebugService::instance = (DebugService *)0;
|
||||
|
||||
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 FindPathInContainer(ReferenceContainer *container,
|
||||
const Object *target, StreamString &path) {
|
||||
if (container == NULL_PTR(ReferenceContainer *))
|
||||
return false;
|
||||
uint32 n = container->Size();
|
||||
for (uint32 i = 0u; i < n; i++) {
|
||||
Reference ref = container->Get(i);
|
||||
if (ref.IsValid()) {
|
||||
if (ref.operator->() == target) {
|
||||
path = ref->GetName();
|
||||
return true;
|
||||
}
|
||||
ReferenceContainer *sub =
|
||||
dynamic_cast<ReferenceContainer *>(ref.operator->());
|
||||
if (sub != NULL_PTR(ReferenceContainer *)) {
|
||||
if (FindPathInContainer(sub, target, path)) {
|
||||
StreamString full;
|
||||
full.Printf("%s.%s", ref->GetName(), path.Buffer());
|
||||
path = full;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
CLASS_REGISTER(DebugService, "1.0")
|
||||
|
||||
DebugService::DebugService()
|
||||
: ReferenceContainer(), EmbeddedServiceMethodBinderI(),
|
||||
binderServer(this, ServiceBinder::ServerType),
|
||||
binderStreamer(this, ServiceBinder::StreamerType),
|
||||
threadService(binderServer), streamerService(binderStreamer) {
|
||||
controlPort = 0;
|
||||
streamPort = 8081;
|
||||
streamIP = "127.0.0.1";
|
||||
isServer = false;
|
||||
suppressTimeoutLogs = true;
|
||||
isPaused = false;
|
||||
activeClient = NULL_PTR(BasicTCPSocket *);
|
||||
}
|
||||
|
||||
DebugService::~DebugService() {
|
||||
if (instance == this) {
|
||||
instance = NULL_PTR(DebugService *);
|
||||
}
|
||||
threadService.Stop();
|
||||
streamerService.Stop();
|
||||
tcpServer.Close();
|
||||
udpSocket.Close();
|
||||
if (activeClient != NULL_PTR(BasicTCPSocket *)) {
|
||||
activeClient->Close();
|
||||
delete activeClient;
|
||||
}
|
||||
for (uint32 i = 0; i < signals.Size(); i++) {
|
||||
delete signals[i];
|
||||
}
|
||||
}
|
||||
|
||||
bool DebugService::Initialise(StructuredDataI &data) {
|
||||
if (!ReferenceContainer::Initialise(data))
|
||||
return false;
|
||||
|
||||
uint32 port = 0;
|
||||
if (data.Read("ControlPort", port)) {
|
||||
controlPort = (uint16)port;
|
||||
} else {
|
||||
(void)data.Read("TcpPort", port);
|
||||
controlPort = (uint16)port;
|
||||
}
|
||||
|
||||
if (controlPort > 0) {
|
||||
isServer = true;
|
||||
instance = this;
|
||||
}
|
||||
|
||||
port = 8081;
|
||||
if (data.Read("StreamPort", port)) {
|
||||
streamPort = (uint16)port;
|
||||
} else {
|
||||
(void)data.Read("UdpPort", port);
|
||||
streamPort = (uint16)port;
|
||||
}
|
||||
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);
|
||||
}
|
||||
|
||||
// Try to capture full configuration autonomously if data is a
|
||||
// ConfigurationDatabase
|
||||
ConfigurationDatabase *cdb = dynamic_cast<ConfigurationDatabase *>(&data);
|
||||
if (cdb != NULL_PTR(ConfigurationDatabase *)) {
|
||||
// Save current position
|
||||
StreamString currentPath;
|
||||
// In MARTe2 ConfigurationDatabase there isn't a direct GetCurrentPath,
|
||||
// but we can at least try to copy from root if we are at root.
|
||||
// For now, we rely on explicit SetFullConfig or documentary injection.
|
||||
}
|
||||
|
||||
// Copy local branch as fallback
|
||||
(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;
|
||||
if (!udpSocket.Open())
|
||||
return false;
|
||||
if (threadService.Start() != ErrorManagement::NoError)
|
||||
return false;
|
||||
if (streamerService.Start() != ErrorManagement::NoError)
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void DebugService::SetFullConfig(ConfigurationDatabase &config) {
|
||||
config.MoveToRoot();
|
||||
config.Copy(fullConfig);
|
||||
}
|
||||
|
||||
static void PatchItemInternal(const char8 *originalName,
|
||||
ObjectBuilder *debugBuilder) {
|
||||
ClassRegistryItem *item =
|
||||
ClassRegistryDatabase::Instance()->Find(originalName);
|
||||
if (item != NULL_PTR(ClassRegistryItem *)) {
|
||||
item->SetObjectBuilder(debugBuilder);
|
||||
}
|
||||
}
|
||||
|
||||
void DebugService::PatchRegistry() {
|
||||
DebugMemoryMapInputBrokerBuilder *b1 = new DebugMemoryMapInputBrokerBuilder();
|
||||
PatchItemInternal("MemoryMapInputBroker", b1);
|
||||
DebugMemoryMapOutputBrokerBuilder *b2 =
|
||||
new DebugMemoryMapOutputBrokerBuilder();
|
||||
PatchItemInternal("MemoryMapOutputBroker", b2);
|
||||
DebugMemoryMapSynchronisedInputBrokerBuilder *b3 =
|
||||
new DebugMemoryMapSynchronisedInputBrokerBuilder();
|
||||
PatchItemInternal("MemoryMapSynchronisedInputBroker", b3);
|
||||
DebugMemoryMapSynchronisedOutputBrokerBuilder *b4 =
|
||||
new DebugMemoryMapSynchronisedOutputBrokerBuilder();
|
||||
PatchItemInternal("MemoryMapSynchronisedOutputBroker", b4);
|
||||
DebugMemoryMapInterpolatedInputBrokerBuilder *b5 =
|
||||
new DebugMemoryMapInterpolatedInputBrokerBuilder();
|
||||
PatchItemInternal("MemoryMapInterpolatedInputBroker", b5);
|
||||
DebugMemoryMapMultiBufferInputBrokerBuilder *b6 =
|
||||
new DebugMemoryMapMultiBufferInputBrokerBuilder();
|
||||
PatchItemInternal("MemoryMapMultiBufferInputBroker", b6);
|
||||
DebugMemoryMapMultiBufferOutputBrokerBuilder *b7 =
|
||||
new DebugMemoryMapMultiBufferOutputBrokerBuilder();
|
||||
PatchItemInternal("MemoryMapMultiBufferOutputBroker", b7);
|
||||
DebugMemoryMapSynchronisedMultiBufferInputBrokerBuilder *b8 =
|
||||
new DebugMemoryMapSynchronisedMultiBufferInputBrokerBuilder();
|
||||
PatchItemInternal("MemoryMapSynchronisedMultiBufferInputBroker", b8);
|
||||
DebugMemoryMapSynchronisedMultiBufferOutputBrokerBuilder *b9 =
|
||||
new DebugMemoryMapSynchronisedMultiBufferOutputBrokerBuilder();
|
||||
PatchItemInternal("MemoryMapSynchronisedMultiBufferOutputBroker", b9);
|
||||
DebugMemoryMapAsyncOutputBrokerBuilder *b10 =
|
||||
new DebugMemoryMapAsyncOutputBrokerBuilder();
|
||||
PatchItemInternal("MemoryMapAsyncOutputBroker", b10);
|
||||
DebugMemoryMapAsyncTriggerOutputBrokerBuilder *b11 =
|
||||
new DebugMemoryMapAsyncTriggerOutputBrokerBuilder();
|
||||
PatchItemInternal("MemoryMapAsyncTriggerOutputBroker", b11);
|
||||
}
|
||||
|
||||
DebugSignalInfo *DebugService::RegisterSignal(void *memoryAddress,
|
||||
TypeDescriptor type,
|
||||
const char8 *name) {
|
||||
printf("<debug> registering: %s\n", name);
|
||||
mutex.FastLock();
|
||||
DebugSignalInfo *res = NULL_PTR(DebugSignalInfo *);
|
||||
uint32 sigIdx = 0xFFFFFFFF;
|
||||
for (uint32 i = 0; i < signals.Size(); i++) {
|
||||
if (signals[i]->memoryAddress == memoryAddress) {
|
||||
res = signals[i];
|
||||
sigIdx = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (res == NULL_PTR(DebugSignalInfo *)) {
|
||||
sigIdx = signals.Size();
|
||||
res = new DebugSignalInfo();
|
||||
res->memoryAddress = memoryAddress;
|
||||
res->type = type;
|
||||
res->name = name;
|
||||
res->isTracing = false;
|
||||
res->isForcing = false;
|
||||
res->internalID = sigIdx;
|
||||
res->decimationFactor = 1;
|
||||
res->decimationCounter = 0;
|
||||
signals.Push(res);
|
||||
}
|
||||
if (sigIdx != 0xFFFFFFFF) {
|
||||
bool foundAlias = false;
|
||||
for (uint32 i = 0; i < aliases.Size(); i++) {
|
||||
if (aliases[i].name == name) {
|
||||
foundAlias = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!foundAlias) {
|
||||
SignalAlias a;
|
||||
a.name = name;
|
||||
a.signalIndex = sigIdx;
|
||||
aliases.Push(a);
|
||||
}
|
||||
}
|
||||
mutex.FastUnLock();
|
||||
return res;
|
||||
}
|
||||
|
||||
void DebugService::ProcessSignal(DebugSignalInfo *signalInfo, uint32 size,
|
||||
uint64 timestamp) {
|
||||
if (signalInfo == NULL_PTR(DebugSignalInfo *))
|
||||
return;
|
||||
if (signalInfo->isForcing) {
|
||||
memcpy(signalInfo->memoryAddress, signalInfo->forcedValue, size);
|
||||
}
|
||||
if (signalInfo->isTracing) {
|
||||
if (signalInfo->decimationCounter == 0) {
|
||||
traceBuffer.Push(signalInfo->internalID, timestamp,
|
||||
(uint8 *)signalInfo->memoryAddress, size);
|
||||
}
|
||||
signalInfo->decimationCounter =
|
||||
(signalInfo->decimationCounter + 1) % signalInfo->decimationFactor;
|
||||
}
|
||||
}
|
||||
|
||||
void DebugService::RegisterBroker(DebugSignalInfo **signalPointers,
|
||||
uint32 numSignals, MemoryMapBroker *broker,
|
||||
volatile bool *anyActiveFlag,
|
||||
Vec<uint32> *activeIndices,
|
||||
Vec<uint32> *activeSizes,
|
||||
FastPollingMutexSem *activeMutex) {
|
||||
mutex.FastLock();
|
||||
BrokerInfo b;
|
||||
b.signalPointers = signalPointers;
|
||||
b.numSignals = numSignals;
|
||||
b.broker = broker;
|
||||
b.anyActiveFlag = anyActiveFlag;
|
||||
b.activeIndices = activeIndices;
|
||||
b.activeSizes = activeSizes;
|
||||
b.activeMutex = activeMutex;
|
||||
brokers.Push(b);
|
||||
mutex.FastUnLock();
|
||||
}
|
||||
|
||||
void DebugService::UpdateBrokersActiveStatus() {
|
||||
for (uint32 i = 0; i < brokers.Size(); i++) {
|
||||
uint32 count = 0;
|
||||
for (uint32 j = 0; j < brokers[i].numSignals; j++) {
|
||||
DebugSignalInfo *s = brokers[i].signalPointers[j];
|
||||
if (s != NULL_PTR(DebugSignalInfo *) && (s->isTracing || s->isForcing)) {
|
||||
count++;
|
||||
}
|
||||
}
|
||||
|
||||
Vec<uint32> tempInd;
|
||||
Vec<uint32> tempSizes;
|
||||
for (uint32 j = 0; j < brokers[i].numSignals; j++) {
|
||||
DebugSignalInfo *s = brokers[i].signalPointers[j];
|
||||
if (s != NULL_PTR(DebugSignalInfo *) && (s->isTracing || s->isForcing)) {
|
||||
tempInd.Push(j);
|
||||
tempSizes.Push((brokers[i].broker != NULL_PTR(MemoryMapBroker *))
|
||||
? brokers[i].broker->GetCopyByteSize(j)
|
||||
: 4);
|
||||
}
|
||||
}
|
||||
|
||||
if (brokers[i].activeMutex)
|
||||
brokers[i].activeMutex->FastLock();
|
||||
|
||||
if (brokers[i].activeIndices)
|
||||
*(brokers[i].activeIndices) = tempInd;
|
||||
if (brokers[i].activeSizes)
|
||||
*(brokers[i].activeSizes) = tempSizes;
|
||||
if (brokers[i].anyActiveFlag)
|
||||
*(brokers[i].anyActiveFlag) = (count > 0);
|
||||
|
||||
if (brokers[i].activeMutex)
|
||||
brokers[i].activeMutex->FastUnLock();
|
||||
}
|
||||
}
|
||||
|
||||
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) {
|
||||
while (activeClient == NULL_PTR(BasicTCPSocket *)) {
|
||||
BasicTCPSocket *newClient = tcpServer.WaitConnection(TTInfiniteWait);
|
||||
if (newClient != NULL_PTR(BasicTCPSocket *)) {
|
||||
// Single connection mode: disconnect any existing client first
|
||||
activeClient = newClient;
|
||||
}
|
||||
}
|
||||
// Single connection mode: only check client 0
|
||||
{
|
||||
if (activeClient != NULL_PTR(BasicTCPSocket *)) {
|
||||
// Check if client is still connected
|
||||
if (!activeClient->IsConnected()) {
|
||||
activeClient->Close();
|
||||
delete activeClient;
|
||||
activeClient = NULL_PTR(BasicTCPSocket *);
|
||||
|
||||
} else {
|
||||
char buffer[1024];
|
||||
uint32 size = 1024;
|
||||
if (activeClient->Read(buffer, size)) {
|
||||
if (size > 0) {
|
||||
// Process each line separately
|
||||
char *ptr = buffer;
|
||||
char *end = buffer + size;
|
||||
while (ptr < end) {
|
||||
char *newline = (char *)memchr(ptr, '\n', end - ptr);
|
||||
if (!newline) {
|
||||
break;
|
||||
}
|
||||
*newline = '\0';
|
||||
// Skip carriage return if present
|
||||
if (newline > ptr && *(newline - 1) == '\r')
|
||||
*(newline - 1) = '\0';
|
||||
StreamString command;
|
||||
uint32 len = (uint32)(newline - ptr);
|
||||
command.Write(ptr, len);
|
||||
if (command.Size() > 0) {
|
||||
HandleCommand(command, activeClient);
|
||||
}
|
||||
ptr = newline + 1;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
// // Read failed (client disconnected or error), clean up
|
||||
if (activeClient != NULL_PTR(BasicTCPSocket *)) {
|
||||
activeClient->Close();
|
||||
delete activeClient;
|
||||
activeClient = NULL_PTR(BasicTCPSocket *);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
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;
|
||||
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;
|
||||
memcpy(packetBuffer, &header, sizeof(TraceHeader));
|
||||
packetOffset = sizeof(TraceHeader);
|
||||
}
|
||||
memcpy(&packetBuffer[packetOffset], &id, 4);
|
||||
memcpy(&packetBuffer[packetOffset + 4], &ts, 8);
|
||||
memcpy(&packetBuffer[packetOffset + 12], &size, 4);
|
||||
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;
|
||||
}
|
||||
|
||||
bool DebugService::GetFullObjectName(const Object &obj,
|
||||
StreamString &fullPath) {
|
||||
fullPath = "";
|
||||
if (FindPathInContainer(ObjectRegistryDatabase::Instance(), &obj, fullPath)) {
|
||||
return true;
|
||||
}
|
||||
const char8 *name = obj.GetName();
|
||||
if (name != NULL_PTR(const char8 *))
|
||||
fullPath = name;
|
||||
return true;
|
||||
}
|
||||
|
||||
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 == "CONFIG")
|
||||
ServeConfig(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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void DebugService::EnrichWithConfig(const char8 *path, StreamString &json) {
|
||||
if (path == NULL_PTR(const char8 *))
|
||||
return;
|
||||
fullConfig.MoveToRoot();
|
||||
|
||||
const char8 *current = path;
|
||||
bool ok = true;
|
||||
while (ok) {
|
||||
const char8 *nextDot = StringHelper::SearchString(current, ".");
|
||||
StreamString part;
|
||||
if (nextDot != NULL_PTR(const char8 *)) {
|
||||
uint32 len = (uint32)(nextDot - current);
|
||||
(void)part.Write(current, len);
|
||||
current = nextDot + 1;
|
||||
} else {
|
||||
part = current;
|
||||
ok = false;
|
||||
}
|
||||
|
||||
if (fullConfig.MoveRelative(part.Buffer())) {
|
||||
// Found exact
|
||||
} else {
|
||||
bool found = false;
|
||||
if (part == "In") {
|
||||
if (fullConfig.MoveRelative("InputSignals")) {
|
||||
found = true;
|
||||
}
|
||||
} else if (part == "Out") {
|
||||
if (fullConfig.MoveRelative("OutputSignals")) {
|
||||
found = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (!found) {
|
||||
StreamString prefixed;
|
||||
prefixed.Printf("+%s", part.Buffer());
|
||||
if (fullConfig.MoveRelative(prefixed.Buffer())) {
|
||||
// Found prefixed
|
||||
} else {
|
||||
return; // Not found
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
ConfigurationDatabase db;
|
||||
fullConfig.Copy(db);
|
||||
fullConfig.MoveToRoot();
|
||||
db.MoveToRoot();
|
||||
uint32 n = db.GetNumberOfChildren();
|
||||
for (uint32 i = 0u; i < n; i++) {
|
||||
const char8 *name = db.GetChildName(i);
|
||||
AnyType at = db.GetType(name);
|
||||
if (!at.GetTypeDescriptor().isStructuredData) {
|
||||
json += ", \"";
|
||||
EscapeJson(name, json);
|
||||
json += "\": \"";
|
||||
char8 buf[1024];
|
||||
AnyType st(CharString, 0u, buf);
|
||||
st.SetNumberOfElements(0, 1024);
|
||||
if (TypeConvert(st, at)) {
|
||||
EscapeJson(buf, json);
|
||||
}
|
||||
json += "\"";
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void DebugService::JsonifyDatabase(ConfigurationDatabase &db,
|
||||
StreamString &json) {
|
||||
json += "{";
|
||||
uint32 n = db.GetNumberOfChildren();
|
||||
for (uint32 i = 0u; i < n; i++) {
|
||||
const char8 *name = db.GetChildName(i);
|
||||
json += "\"";
|
||||
EscapeJson(name, json);
|
||||
json += "\": ";
|
||||
if (db.MoveRelative(name)) {
|
||||
ConfigurationDatabase child;
|
||||
db.Copy(child);
|
||||
JsonifyDatabase(child, json);
|
||||
db.MoveToAncestor(1u);
|
||||
} else {
|
||||
AnyType at = db.GetType(name);
|
||||
char8 buf[1024];
|
||||
AnyType st(CharString, 0u, buf);
|
||||
st.SetNumberOfElements(0, 1024);
|
||||
if (TypeConvert(st, at)) {
|
||||
json += "\"";
|
||||
EscapeJson(buf, json);
|
||||
json += "\"";
|
||||
} else {
|
||||
json += "null";
|
||||
}
|
||||
}
|
||||
if (i < n - 1)
|
||||
json += ", ";
|
||||
}
|
||||
json += "}";
|
||||
}
|
||||
|
||||
void DebugService::ServeConfig(BasicTCPSocket *client) {
|
||||
if (client == NULL_PTR(BasicTCPSocket *))
|
||||
return;
|
||||
StreamString json;
|
||||
fullConfig.MoveToRoot();
|
||||
JsonifyDatabase(fullConfig, json);
|
||||
json += "\nOK CONFIG\n";
|
||||
uint32 s = json.Size();
|
||||
(void)client->Write(json.Buffer(), s);
|
||||
}
|
||||
|
||||
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; i < nChildren; i++) {
|
||||
const char8 *cname = db.GetChildName(i);
|
||||
AnyType at = db.GetType(cname);
|
||||
char8 valBuf[1024];
|
||||
AnyType strType(CharString, 0u, valBuf);
|
||||
strType.SetNumberOfElements(0, 1024);
|
||||
if (TypeConvert(strType, at)) {
|
||||
json += "\"";
|
||||
EscapeJson(cname, json);
|
||||
json += "\": \"";
|
||||
EscapeJson(valBuf, json);
|
||||
json += "\"";
|
||||
if (i < nChildren - 1)
|
||||
json += ", ";
|
||||
}
|
||||
}
|
||||
json += "}";
|
||||
}
|
||||
EnrichWithConfig(path, json);
|
||||
} else {
|
||||
mutex.FastLock();
|
||||
bool found = false;
|
||||
for (uint32 i = 0; i < aliases.Size(); i++) {
|
||||
if (aliases[i].name == path ||
|
||||
SuffixMatch(aliases[i].name.Buffer(), path)) {
|
||||
DebugSignalInfo *s = signals[aliases[i].signalIndex];
|
||||
const char8 *tname =
|
||||
TypeDescriptor::GetTypeNameFromTypeDescriptor(s->type);
|
||||
json.Printf("\"Name\": \"%s\", \"Class\": \"Signal\", \"Type\": "
|
||||
"\"%s\", \"ID\": %d",
|
||||
s->name.Buffer(), tname ? tname : "Unknown", s->internalID);
|
||||
EnrichWithConfig(aliases[i].name.Buffer(), json);
|
||||
found = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
mutex.FastUnLock();
|
||||
if (!found)
|
||||
json += "\"Error\": \"Object not found\"";
|
||||
}
|
||||
json += "}\nOK INFO\n";
|
||||
uint32 s = json.Size();
|
||||
(void)client->Write(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<ReferenceContainer *>(child.operator->());
|
||||
DataSourceI *ds = dynamic_cast<DataSourceI *>(child.operator->());
|
||||
GAM *gam = dynamic_cast<GAM *>(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;
|
||||
}
|
||||
|
||||
uint32 DebugService::ForceSignal(const char8 *name, const char8 *valueStr) {
|
||||
mutex.FastLock();
|
||||
uint32 count = 0;
|
||||
for (uint32 i = 0; i < aliases.Size(); 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++;
|
||||
}
|
||||
}
|
||||
UpdateBrokersActiveStatus();
|
||||
mutex.FastUnLock();
|
||||
return count;
|
||||
}
|
||||
|
||||
uint32 DebugService::UnforceSignal(const char8 *name) {
|
||||
mutex.FastLock();
|
||||
uint32 count = 0;
|
||||
for (uint32 i = 0; i < aliases.Size(); i++) {
|
||||
if (aliases[i].name == name ||
|
||||
SuffixMatch(aliases[i].name.Buffer(), name)) {
|
||||
signals[aliases[i].signalIndex]->isForcing = false;
|
||||
count++;
|
||||
}
|
||||
}
|
||||
UpdateBrokersActiveStatus();
|
||||
mutex.FastUnLock();
|
||||
return count;
|
||||
}
|
||||
|
||||
uint32 DebugService::TraceSignal(const char8 *name, bool enable,
|
||||
uint32 decimation) {
|
||||
mutex.FastLock();
|
||||
uint32 count = 0;
|
||||
for (uint32 i = 0; i < aliases.Size(); i++) {
|
||||
printf("<debug>%s\n", aliases[i].name.Buffer());
|
||||
if (aliases[i].name == name ||
|
||||
SuffixMatch(aliases[i].name.Buffer(), name)) {
|
||||
DebugSignalInfo *s = signals[aliases[i].signalIndex];
|
||||
s->isTracing = enable;
|
||||
s->decimationFactor = decimation;
|
||||
s->decimationCounter = 0;
|
||||
count++;
|
||||
}
|
||||
}
|
||||
if (count == 0) {
|
||||
printf("<!!> signal %s not found\n", name);
|
||||
}
|
||||
UpdateBrokersActiveStatus();
|
||||
mutex.FastUnLock();
|
||||
return count;
|
||||
}
|
||||
|
||||
void DebugService::Discover(BasicTCPSocket *client) {
|
||||
if (client) {
|
||||
StreamString header = "{\n \"Signals\": [\n";
|
||||
uint32 s = header.Size();
|
||||
(void)client->Write(header.Buffer(), s);
|
||||
mutex.FastLock();
|
||||
for (uint32 i = 0; i < aliases.Size(); i++) {
|
||||
StreamString line;
|
||||
DebugSignalInfo *sig = signals[aliases[i].signalIndex];
|
||||
const char8 *typeName =
|
||||
TypeDescriptor::GetTypeNameFromTypeDescriptor(sig->type);
|
||||
line.Printf(" {\"name\": \"%s\", \"id\": %d, \"type\": \"%s\"",
|
||||
aliases[i].name.Buffer(), sig->internalID,
|
||||
typeName ? typeName : "Unknown");
|
||||
EnrichWithConfig(aliases[i].name.Buffer(), line);
|
||||
line += "}";
|
||||
if (i < aliases.Size() - 1)
|
||||
line += ",";
|
||||
line += "\n";
|
||||
s = line.Size();
|
||||
(void)client->Write(line.Buffer(), s);
|
||||
}
|
||||
mutex.FastUnLock();
|
||||
StreamString footer = " ]\n}\nOK DISCOVER\n";
|
||||
s = footer.Size();
|
||||
(void)client->Write(footer.Buffer(), s);
|
||||
}
|
||||
}
|
||||
|
||||
void DebugService::ListNodes(const char8 *path, BasicTCPSocket *client) {
|
||||
if (!client)
|
||||
return;
|
||||
Reference ref =
|
||||
(path == NULL_PTR(const char8 *) || StringHelper::Length(path) == 0 ||
|
||||
StringHelper::Compare(path, "/") == 0)
|
||||
? ObjectRegistryDatabase::Instance()
|
||||
: ObjectRegistryDatabase::Instance()->Find(path);
|
||||
if (ref.IsValid()) {
|
||||
StreamString out;
|
||||
out.Printf("Nodes under %s:\n", path ? path : "/");
|
||||
ReferenceContainer *container =
|
||||
dynamic_cast<ReferenceContainer *>(ref.operator->());
|
||||
if (container) {
|
||||
for (uint32 i = 0; i < container->Size(); i++) {
|
||||
Reference child = container->Get(i);
|
||||
if (child.IsValid())
|
||||
out.Printf(" %s [%s]\n", child->GetName(),
|
||||
child->GetClassProperties()->GetName());
|
||||
}
|
||||
}
|
||||
const char *okMsg = "OK LS\n";
|
||||
out += okMsg;
|
||||
uint32 s = out.Size();
|
||||
(void)client->Write(out.Buffer(), s);
|
||||
} else {
|
||||
const char *msg = "ERROR: Path not found\n";
|
||||
uint32 s = StringHelper::Length(msg);
|
||||
(void)client->Write(msg, s);
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace MARTe
|
||||
Reference in New Issue
Block a user