martino/marte-debug
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
e2f47d7410c45da6be7d1a5448d8d7a322f176fd
marte-debug/Source/Components/Interfaces/DebugService/DebugService.cpp
T
Martino Ferrari e2f47d7410 Working on experimental features
2026-03-24 21:16:24 +01:00

1692 lines
54 KiB
C++
Raw Blame History

#include "AdvancedErrorManagement.h"
#include "AnyType.h"
#include "BasicTCPSocket.h"
#include "ClassRegistryItem.h"
#include "ConfigurationDatabase.h"
#include "DataSourceI.h"
#include "DebugBrokerWrapper.h"
#include "DebugService.h"
#include "ErrorType.h"
#include "GAM.h"
#include "GlobalObjectsDatabase.h"
#include "HighResolutionTimer.h"
#include "Matrix.h"
#include "Message.h"
#include "ObjectBuilder.h"
#include "ObjectRegistryDatabase.h"
#include "RealTimeApplication.h"
#include "ReferenceT.h"
#include "StateMachine.h"
#include "StreamString.h"
#include "TimeoutType.h"
#include "TypeConversion.h"
#include "TypeDescriptor.h"
namespace MARTe {
// Proxy to access private members of RealTimeApplication
class RealTimeApplicationProxy : public ReferenceContainer {
public:
// ReferenceContainer members
LinkedListHolderT<ReferenceContainerNode> list;
FastPollingMutexSem mux;
TimeoutType muxTimeout;
// MessageI members (approximate alignment)
void *messageI_vtable;
uint8 messageFilters_padding[sizeof(
ReferenceContainer)]; // MessageFilterPool is a ReferenceContainer
// RealTimeApplication members
StreamString stateNameHolder[2];
uint32 index;
};
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;
}
static void JsonifyScalar(const AnyType &at, StreamString &json) {
if (at.GetDataPointer() == NULL_PTR(void *)) {
json += "null";
return;
}
char8 buf[1024];
AnyType st(CharString, 0u, buf);
st.SetNumberOfElements(0, 1024);
if (TypeConvert(st, at)) {
TypeDescriptor td = at.GetTypeDescriptor();
if (td.IsNumericType() || td == BooleanType) {
if (buf[0] == '\0')
json += "0";
else
json += buf;
} else {
json += "\"";
EscapeJson(buf, json);
json += "\"";
}
} else {
json += "null";
}
}
static void JsonifyDatabaseInternal(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);
child.MoveToRoot();
JsonifyDatabaseInternal(child, json);
db.MoveToAncestor(1u);
} else {
AnyType at = db.GetType(name);
uint32 nDims = at.GetNumberOfDimensions();
if (nDims == 0u) {
JsonifyScalar(at, json);
} else if (nDims == 1u) {
uint32 nElements = at.GetNumberOfElements(0u);
json += "[";
for (uint32 j = 0u; j < nElements; j++) {
JsonifyScalar(at[j], json);
if (j < nElements - 1u)
json += ", ";
}
json += "]";
} else if (nDims == 2u) {
uint32 nRows = at.GetNumberOfElements(0u);
uint32 nCols = at.GetNumberOfElements(1u);
json += "[";
for (uint32 r = 0u; r < nRows; r++) {
json += "[";
for (uint32 c = 0u; c < nCols; c++) {
JsonifyScalar(at[r][c], json);
if (c < nCols - 1u)
json += ", ";
}
json += "]";
if (r < nRows - 1u)
json += ", ";
}
json += "]";
} else {
json += "\"Unsupported dimensions\"";
}
}
if (i < n - 1)
json += ", ";
}
json += "}";
}
static void JsonifyObjectSafe(Reference ref, StreamString &json, uint32 depth,
Vec<Object *> &visited) {
if (!ref.IsValid() || depth > 8) {
json += "null";
return;
}
for (uint32 i = 0u; i < visited.Size(); i++) {
if (visited[i] == ref.operator->()) {
json += "\"<circular>\"";
return;
}
}
visited.Push(ref.operator->());
json += "{\n";
const char8 *className = "Unknown";
const ClassProperties *cp = ref->GetClassProperties();
if (cp != NULL_PTR(const ClassProperties *)) {
className = cp->GetName();
}
json += "\"Class\": \"";
EscapeJson(className, json);
json += "\"";
ConfigurationDatabase db;
bool exported = false;
// CRITICAL: Avoid objects that are known to cause real-time interference
// or crashes when introspected live.
bool safeToExport = true;
if (StringHelper::Compare(className, "RealTimeThreadSynchronisation") == 0 ||
StringHelper::Compare(className, "SyncDB") == 0) {
safeToExport = false;
}
if (safeToExport) {
ClassRegistryItem *cri = ClassRegistryDatabase::Instance()->Find(className);
if (cri != NULL_PTR(ClassRegistryItem *)) {
if (cri->GetIntrospection() != NULL_PTR(const Introspection *)) {
exported = ref->ExportMetadata(db);
}
}
if (!exported) {
exported = ref->ExportData(db);
}
}
if (exported) {
db.MoveToRoot();
uint32 n = db.GetNumberOfChildren();
for (uint32 i = 0u; i < n; i++) {
const char8 *name = db.GetChildName(i);
json += ",\n\"";
EscapeJson(name, json);
json += "\": ";
if (db.MoveRelative(name)) {
ConfigurationDatabase sub;
db.Copy(sub);
sub.MoveToRoot();
JsonifyDatabaseInternal(sub, json);
db.MoveToAncestor(1u);
} else {
JsonifyScalar(db.GetType(name), json);
}
}
}
ReferenceContainer *container =
dynamic_cast<ReferenceContainer *>(ref.operator->());
if (container) {
uint32 n = container->Size();
if (n > 25)
n = 25; // Even tighter breadth limit
for (uint32 i = 0u; i < n; i++) {
Reference child = container->Get(i);
if (child.IsValid()) {
json += ",\n\"+";
EscapeJson(child->GetName(), json);
json += "\": ";
JsonifyObjectSafe(child, json, depth + 1, visited);
}
}
}
json += "\n}";
(void)visited.Remove(visited.Size() - 1u);
}
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;
logPort = 8082;
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];
}
}
static void
DiscoverStatesRecursive(ReferenceContainer *container,
Vec<DebugService::MonitoredState> &monitoredStates) {
if (container == NULL_PTR(ReferenceContainer *)) {
return;
}
uint32 n = container->Size();
for (uint32 i = 0u; i < n; i++) {
Reference ref = container->Get(i);
if (ref.IsValid()) {
bool alreadyMonitored = false;
for (uint32 j = 0u; j < monitoredStates.Size(); j++) {
if (monitoredStates[j].obj == ref) {
alreadyMonitored = true;
break;
}
}
if (!alreadyMonitored) {
bool isStateful = false;
if (dynamic_cast<RealTimeApplication *>(ref.operator->()) !=
NULL_PTR(RealTimeApplication *)) {
isStateful = true;
} else if (dynamic_cast<StateMachine *>(ref.operator->()) !=
NULL_PTR(StateMachine *)) {
isStateful = true;
}
if (isStateful) {
DebugService::MonitoredState ms;
ms.obj = ref;
ms.internalID = 0x90000000 | monitoredStates.Size();
DebugService::GetFullObjectName(*(ref.operator->()), ms.path);
ms.lastState = "";
monitoredStates.Push(ms);
}
}
ReferenceContainer *sub =
dynamic_cast<ReferenceContainer *>(ref.operator->());
if (sub != NULL_PTR(ReferenceContainer *)) {
DiscoverStatesRecursive(sub, monitoredStates);
}
}
}
}
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;
}
port = 8082;
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 = 1;
if (data.Read("SuppressTimeoutLogs", suppress)) {
suppressTimeoutLogs = (suppress == 1);
}
// Copy local branch as fallback
const char *name = data.GetName();
data.MoveToRoot();
(void)data.Copy(fullConfig);
if (!data.MoveRelative(name)) {
REPORT_ERROR(ErrorManagement::FatalError, "Impossible to move back...");
return false;
}
if (isServer) {
monitoredStates.Clear();
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("MemoryMapMultiBufferInputBroker", b8);
DebugMemoryMapSynchronisedMultiBufferOutputBrokerBuilder *b9 =
new DebugMemoryMapSynchronisedMultiBufferOutputBrokerBuilder();
PatchItemInternal("MemoryMapSynchronisedMultiBufferOutputBroker", b9);
DebugMemoryMapAsyncOutputBrokerBuilder *b10 =
new DebugMemoryMapAsyncOutputBrokerBuilder();
PatchItemInternal("MemoryMapAsyncOutputBroker", b10);
DebugMemoryMapAsyncTriggerOutputBrokerBuilder *b11 =
new DebugMemoryMapAsyncTriggerOutputBrokerBuilder();
PatchItemInternal("MemoryMapAsyncTriggerOutputBroker", b11);
DebugRealTimeThreadSyncBrokerBuilder *b12 =
new DebugRealTimeThreadSyncBrokerBuilder();
PatchItemInternal("RealTimeThreadSyncBroker", b12);
}
DebugSignalInfo *DebugService::RegisterSignal(void *memoryAddress,
TypeDescriptor type,
const char8 *name,
uint8 numberOfDimensions,
uint32 numberOfElements) {
REPORT_ERROR(ErrorManagement::Debug, "registering: %s", 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->numberOfDimensions = numberOfDimensions;
res->numberOfElements = numberOfElements;
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::HandleMessage(ReferenceT<Message> &data) {
REPORT_ERROR(ErrorManagement::Debug,
"DebugService received custom message: Function=%s",
(const char8 *)data->GetFunction());
return ErrorManagement::NoError;
}
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;
uint64 lastStatePollTime = 0;
while (info.GetStage() == ExecutionInfo::MainStage) {
uint64 currentTimeMs = (uint64)((float64)HighResolutionTimer::Counter() *
HighResolutionTimer::Period() * 1000.0);
// Poll states every 100ms
if (currentTimeMs >= (lastStatePollTime + 100u)) {
lastStatePollTime = currentTimeMs;
mutex.FastLock();
for (uint32 i = 0; i < monitoredStates.Size(); i++) {
StreamString currentState;
RealTimeApplication *app = dynamic_cast<RealTimeApplication *>(
monitoredStates[i].obj.operator->());
StateMachine *sm =
dynamic_cast<StateMachine *>(monitoredStates[i].obj.operator->());
if (app != NULL_PTR(RealTimeApplication *)) {
// Look for child RealTimeState objects and check if their GetIndex()
// matches app's But wait, RealTimeState is not easily accessible
// here. Let's use the proxy but with more padding to be safe, or
// better: use the public GetIndex() and then we need a way to get the
// state names.
// Re-attempting proxy with what is most likely the layout:
// [ReferenceContainer][MessageI][RealTimeApplication]
// MessageI has virtual destructor -> vtable (8 bytes)
// MessageI has MessageFilterPool -> ReferenceContainer (40 bytes)
struct RTAP {
uint8 padding[sizeof(ReferenceContainer) + 8 +
sizeof(ReferenceContainer)];
StreamString stateNameHolder[2];
uint32 index;
};
RTAP *proxy = reinterpret_cast<RTAP *>(app);
uint32 idx = app->GetIndex();
if (idx < 2u) {
currentState = proxy->stateNameHolder[idx];
}
if (currentState.Size() == 0) {
// Maybe index is 1 but we only have 1 state? Or alignment still
// off. Try the other index if it's empty
currentState = proxy->stateNameHolder[1u - idx];
}
} else if (sm != NULL_PTR(StateMachine *)) {
Reference s = sm->GetCurrentState();
if (s.IsValid()) {
currentState = s->GetName();
}
}
if (currentState.Size() > 0u &&
currentState != monitoredStates[i].lastState) {
monitoredStates[i].lastState = currentState;
uint64 ts = (uint64)((float64)HighResolutionTimer::Counter() *
HighResolutionTimer::Period() * 1000000.0);
char8 stateBuf[64];
memset(stateBuf, 0, 64);
strncpy(stateBuf, currentState.Buffer(), 63);
traceBuffer.Push(monitoredStates[i].internalID, ts, (uint8 *)stateBuf,
64);
}
}
mutex.FastUnLock();
}
// Poll monitored signals
mutex.FastLock();
for (uint32 i = 0; i < monitoredSignals.Size(); i++) {
if (currentTimeMs >=
(monitoredSignals[i].lastPollTime + monitoredSignals[i].periodMs)) {
monitoredSignals[i].lastPollTime = currentTimeMs;
uint64 ts = (uint64)((float64)HighResolutionTimer::Counter() *
HighResolutionTimer::Period() * 1000000.0);
void *address = NULL_PTR(void *);
if (monitoredSignals[i].dataSource->GetSignalMemoryBuffer(
monitoredSignals[i].signalIdx, 0, address)) {
traceBuffer.Push(monitoredSignals[i].internalID, ts, (uint8 *)address,
monitoredSignals[i].size);
}
}
}
mutex.FastUnLock();
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 == "MSG") {
StreamString dest, func, waitStr;
if (cmd.GetToken(dest, delims, term) &&
cmd.GetToken(func, delims, term) &&
cmd.GetToken(waitStr, delims, term)) {
bool wait = (waitStr == "1");
const char8 *pStart = cmd.Buffer() + cmd.Position();
StreamString rawPayload = pStart;
// Decode escaped newlines (\n)
StreamString payload;
rawPayload.Seek(0u);
char8 c;
while (rawPayload.Size() > rawPayload.Position()) {
uint32 readS = 1;
if (rawPayload.Read(&c, readS)) {
if (c == '\\') {
char8 next;
if (rawPayload.Read(&next, readS)) {
if (next == 'n') {
payload += '\n';
} else {
payload += c;
payload += next;
}
} else {
payload += c;
}
} else {
payload += c;
}
}
}
ReferenceT<Message> msg(
"Message", GlobalObjectsDatabase::Instance()->GetStandardHeap());
ConfigurationDatabase msgConfig;
msgConfig.Write("Destination", dest.Buffer());
msgConfig.Write("Function", func.Buffer());
if (wait) {
msgConfig.Write("Mode", "ExpectsReply");
}
if (payload.Size() > 0u) {
payload.Seek(0u);
StreamString line;
while (payload.GetToken(line, "\n", term)) {
if (line.Size() > 0u) {
const char8 *eq = StringHelper::SearchChar(line.Buffer(), '=');
if (eq != NULL_PTR(const char8 *)) {
StreamString key, val;
uint32 eqPos = (uint32)(eq - line.Buffer());
(void)line.Seek(0u);
char8 *keyBuf = new char8[eqPos + 1];
uint32 keyReadSize = eqPos;
if (line.Read(keyBuf, keyReadSize)) {
keyBuf[eqPos] = '\0';
key = keyBuf;
}
delete[] keyBuf;
(void)line.Seek(eqPos + 1u);
uint32 valLen = line.Size() - eqPos - 1u;
char8 *valBuf = new char8[valLen + 1];
uint32 valReadSize = valLen;
if (line.Read(valBuf, valReadSize)) {
valBuf[valLen] = '\0';
val = valBuf;
}
delete[] valBuf;
if (key.Size() > 0u) {
if (msgConfig.CreateRelative("Payload")) {
(void)msgConfig.Write(key.Buffer(), val.Buffer());
(void)msgConfig.MoveToAncestor(1u);
}
}
}
}
line = "";
}
}
ErrorManagement::ErrorType err = ErrorManagement::ParametersError;
if (msg->Initialise(msgConfig)) {
// Find destination object in the global database
Reference destObj =
ObjectRegistryDatabase::Instance()->Find(dest.Buffer());
if (destObj.IsValid()) {
Object *sender = this;
// Double check if we are in the registry to be a valid sender
StreamString myPath;
if (!GetFullObjectName(*this, myPath)) {
sender = NULL_PTR(Object *);
}
if (wait) {
err = MessageI::WaitForReply(msg, TTInfiniteWait);
} else {
err = MessageI::SendMessage(msg, sender);
}
} else {
REPORT_ERROR(ErrorManagement::Warning,
"MSG: Destination object %s not found in ORD",
dest.Buffer());
}
if (err != ErrorManagement::NoError) {
REPORT_ERROR(ErrorManagement::Warning,
"MSG: MessageI dispatch failed.");
}
} else {
REPORT_ERROR(ErrorManagement::Warning,
"MSG: Message initialization failed");
}
if (client) {
if (err == ErrorManagement::NoError) {
uint32 okSize = 7;
(void)client->Write("OK MSG\n", okSize);
} else {
uint32 errSize = 10;
(void)client->Write("ERROR MSG\n", errSize);
}
}
}
} else if (token == "SERVICE_INFO") {
if (client) {
StreamString resp;
resp.Printf(
"OK SERVICE_INFO TCP_CTRL:%u UDP_STREAM:%u TCP_LOG:%u STATE:%s\n",
controlPort, streamPort, logPort, isPaused ? "PAUSED" : "RUNNING");
uint32 s = resp.Size();
(void)client->Write(resp.Buffer(), s);
}
} else if (token == "MONITOR") {
StreamString subToken;
if (cmd.GetToken(subToken, delims, term) && subToken == "SIGNAL") {
StreamString name, period;
if (cmd.GetToken(name, delims, term) &&
cmd.GetToken(period, delims, term)) {
uint32 p = 100;
AnyType pVal(UnsignedInteger32Bit, 0u, &p);
AnyType pStr(CharString, 0u, period.Buffer());
(void)TypeConvert(pVal, pStr);
uint32 count = RegisterMonitorSignal(name.Buffer(), p);
if (client) {
StreamString resp;
resp.Printf("OK MONITOR %u\n", count);
uint32 s = resp.Size();
(void)client->Write(resp.Buffer(), s);
}
}
}
} else if (token == "UNMONITOR") {
StreamString subToken;
if (cmd.GetToken(subToken, delims, term) && subToken == "SIGNAL") {
StreamString name;
if (cmd.GetToken(name, delims, term)) {
uint32 count = UnmonitorSignal(name.Buffer());
if (client) {
StreamString resp;
resp.Printf("OK UNMONITOR %u\n", count);
uint32 s = resp.Size();
(void)client->Write(resp.Buffer(), s);
}
}
}
} 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,
NULL_PTR(const char8 *));
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,
StreamString indent) {
JsonifyDatabaseInternal(db, json);
}
void DebugService::ServeConfig(BasicTCPSocket *client) {
if (client == NULL_PTR(BasicTCPSocket *))
return;
StreamString json;
Reference ref = ObjectRegistryDatabase::Instance();
Vec<Object *> visited;
JsonifyObjectSafe(ref, json, 0, visited);
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, const char8 *pathPrefix) {
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";
StreamString currentPath;
if (pathPrefix != NULL_PTR(const char8 *)) {
currentPath.Printf("%s.%s", pathPrefix, cname);
} else {
currentPath = cname;
}
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, currentPath.Buffer());
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);
StreamString signalFullPath;
signalFullPath.Printf("%s.%s", currentPath.Buffer(),
sname.Buffer());
bool traceable = false;
bool forcable = false;
(void)IsInstrumented(signalFullPath.Buffer(), traceable, forcable);
nodeJson += "{\"Name\": \"";
EscapeJson(sname.Buffer(), nodeJson);
nodeJson += "\", \"Class\": \"Signal\", \"Type\": \"";
EscapeJson(stype ? stype : "Unknown", nodeJson);
nodeJson.Printf("\", \"Dimensions\": %d, \"Elements\": %u", dims,
elems);
nodeJson.Printf(", \"IsTraceable\": %s, \"IsForcable\": %s}",
traceable ? "true" : "false",
forcable ? "true" : "false");
}
}
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);
StreamString signalFullPath;
signalFullPath.Printf("%s.In.%s", currentPath.Buffer(),
sname.Buffer());
bool traceable = false;
bool forcable = false;
(void)IsInstrumented(signalFullPath.Buffer(), traceable, forcable);
nodeJson += "{\"Name\": \"In.";
EscapeJson(sname.Buffer(), nodeJson);
nodeJson += "\", \"Class\": \"InputSignal\", \"Type\": \"";
EscapeJson(stype ? stype : "Unknown", nodeJson);
nodeJson.Printf("\", \"Dimensions\": %u, \"Elements\": %u", dims,
elems);
nodeJson.Printf(", \"IsTraceable\": %s, \"IsForcable\": %s}",
traceable ? "true" : "false",
forcable ? "true" : "false");
}
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);
StreamString signalFullPath;
signalFullPath.Printf("%s.Out.%s", currentPath.Buffer(),
sname.Buffer());
bool traceable = false;
bool forcable = false;
(void)IsInstrumented(signalFullPath.Buffer(), traceable, forcable);
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.Printf(", \"IsTraceable\": %s, \"IsForcable\": %s}",
traceable ? "true" : "false",
forcable ? "true" : "false");
}
}
nodeJson += "\n]";
}
nodeJson += "}";
json += nodeJson;
validCount++;
}
}
return validCount;
}
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);
}
}
void DebugService::Discover(BasicTCPSocket *client) {
if (client) {
mutex.FastLock();
// Automatically add all state machines and realtime application states if
// not already there
DiscoverStatesRecursive(ObjectRegistryDatabase::Instance(),
monitoredStates);
// Force immediate state push on next poll cycle
for (uint32 i = 0u; i < monitoredStates.Size(); i++) {
monitoredStates[i].lastState = "";
}
StreamString header = "{\n \"Signals\": [\n";
uint32 s = header.Size();
(void)client->Write(header.Buffer(), s);
uint32 total = 0;
for (uint32 i = 0; i < aliases.Size(); i++) {
if (total > 0) {
uint32 commaSize = 2;
(void)client->Write(",\n", commaSize);
}
StreamString line;
DebugSignalInfo *sig = signals[aliases[i].signalIndex];
const char8 *typeName =
TypeDescriptor::GetTypeNameFromTypeDescriptor(sig->type);
line.Printf(" {\"name\": \"%s\", \"id\": %d, \"type\": \"%s\", "
"\"dimensions\": %u, \"elements\": %u",
aliases[i].name.Buffer(), sig->internalID,
typeName ? typeName : "Unknown", sig->numberOfDimensions,
sig->numberOfElements);
EnrichWithConfig(aliases[i].name.Buffer(), line);
line += "}";
s = line.Size();
(void)client->Write(line.Buffer(), s);
total++;
}
// Export monitored signals not already in aliases
for (uint32 i = 0; i < monitoredSignals.Size(); i++) {
bool found = false;
for (uint32 j = 0; j < aliases.Size(); j++) {
if (aliases[j].name == monitoredSignals[i].path) {
found = true;
break;
}
}
if (!found) {
if (total > 0) {
uint32 commaSize = 2;
(void)client->Write(",\n", commaSize);
}
StreamString line;
const char8 *typeName = TypeDescriptor::GetTypeNameFromTypeDescriptor(
monitoredSignals[i].dataSource->GetSignalType(
monitoredSignals[i].signalIdx));
uint8 dims = 0;
uint32 elems = 1;
(void)monitoredSignals[i].dataSource->GetSignalNumberOfDimensions(
monitoredSignals[i].signalIdx, dims);
(void)monitoredSignals[i].dataSource->GetSignalNumberOfElements(
monitoredSignals[i].signalIdx, elems);
line.Printf(" {\"name\": \"%s\", \"id\": %u, \"type\": \"%s\", "
"\"dimensions\": %u, \"elements\": %u",
monitoredSignals[i].path.Buffer(),
monitoredSignals[i].internalID,
typeName ? typeName : "Unknown", dims, elems);
EnrichWithConfig(monitoredSignals[i].path.Buffer(), line);
line += "}";
s = line.Size();
(void)client->Write(line.Buffer(), s);
total++;
}
}
// Export discovered states
for (uint32 i = 0; i < monitoredStates.Size(); i++) {
if (total > 0) {
uint32 commaSize = 2;
(void)client->Write(",\n", commaSize);
}
StreamString line;
line.Printf(" {\"name\": \"%s\", \"id\": %u, \"type\": \"string\", "
"\"is_state\": true}",
monitoredStates[i].path.Buffer(),
monitoredStates[i].internalID);
s = line.Size();
(void)client->Write(line.Buffer(), s);
total++;
}
mutex.FastUnLock();
StreamString footer = " ]\n}\nOK DISCOVER\n";
s = footer.Size();
(void)client->Write(footer.Buffer(), s);
}
}
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++) {
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) {
REPORT_ERROR(ErrorManagement::Warning, "<!!> signal %s not found");
}
UpdateBrokersActiveStatus();
mutex.FastUnLock();
return count;
}
bool DebugService::IsInstrumented(const char8 *fullPath, bool &traceable,
bool &forcable) {
mutex.FastLock();
bool found = false;
for (uint32 i = 0; i < aliases.Size(); i++) {
if (aliases[i].name == fullPath ||
SuffixMatch(aliases[i].name.Buffer(), fullPath)) {
found = true;
break;
}
}
mutex.FastUnLock();
traceable = found;
forcable = found;
return found;
}
uint32 DebugService::RegisterMonitorSignal(const char8 *path, uint32 periodMs) {
mutex.FastLock();
uint32 count = 0;
// Check if already monitored
for (uint32 j = 0; j < monitoredSignals.Size(); j++) {
if (monitoredSignals[j].path == path) {
monitoredSignals[j].periodMs = periodMs;
mutex.FastUnLock();
return 1;
}
}
// Path resolution: find the DataSource object
StreamString fullPath = path;
fullPath.Seek(0);
char8 term;
Vec<StreamString> parts;
StreamString token;
while (fullPath.GetToken(token, ".", term)) {
parts.Push(token);
token = "";
}
if (parts.Size() >= 2) {
StreamString signalName = parts[parts.Size() - 1u];
StreamString dsPath;
for (uint32 i = 0; i < parts.Size() - 1u; i++) {
dsPath += parts[i];
if (i < parts.Size() - 2u)
dsPath += ".";
}
ReferenceT<DataSourceI> ds =
ObjectRegistryDatabase::Instance()->Find(dsPath.Buffer());
if (ds.IsValid()) {
uint32 idx = 0;
if (ds->GetSignalIndex(idx, signalName.Buffer())) {
MonitoredSignal m;
m.dataSource = ds;
m.signalIdx = idx;
m.path = path;
m.periodMs = periodMs;
m.lastPollTime = 0;
m.size = 0;
(void)ds->GetSignalByteSize(idx, m.size);
if (m.size == 0)
m.size = 4;
// Use high-bit for polled signals to avoid conflict with brokered ones
m.internalID = 0x80000000 | monitoredSignals.Size();
// Re-use existing ID if signal is also instrumented via broker
for (uint32 i = 0; i < aliases.Size(); i++) {
if (aliases[i].name == path ||
SuffixMatch(aliases[i].name.Buffer(), path)) {
m.internalID = signals[aliases[i].signalIndex]->internalID;
break;
}
}
monitoredSignals.Push(m);
count = 1;
}
}
}
mutex.FastUnLock();
return count;
}
uint32 DebugService::UnmonitorSignal(const char8 *path) {
mutex.FastLock();
uint32 count = 0;
for (uint32 i = 0; i < monitoredSignals.Size(); i++) {
if (monitoredSignals[i].path == path ||
SuffixMatch(monitoredSignals[i].path.Buffer(), path)) {
(void)monitoredSignals.Remove(i);
i--;
count++;
}
}
mutex.FastUnLock();
return count;
}
} // namespace MARTe
Reference in New Issue View Git Blame Copy Permalink