Initial release
This commit is contained in:
Martino Ferrari
@@ -0,0 +1,590 @@
|
||||
#ifndef DEBUGBROKERWRAPPER_H
|
||||
#define DEBUGBROKERWRAPPER_H
|
||||
|
||||
#include "BrokerI.h"
|
||||
#include "DataSourceI.h"
|
||||
#include "DebugServiceI.h"
|
||||
#include "FastPollingMutexSem.h"
|
||||
#include "HighResolutionTimer.h"
|
||||
#include "MemoryMapBroker.h"
|
||||
#include "ObjectBuilder.h"
|
||||
#include "ObjectRegistryDatabase.h"
|
||||
#include "Threads.h"
|
||||
|
||||
// Original broker headers
|
||||
#include "MemoryMapAsyncOutputBroker.h"
|
||||
#include "MemoryMapAsyncTriggerOutputBroker.h"
|
||||
#include "MemoryMapInputBroker.h"
|
||||
#include "MemoryMapInterpolatedInputBroker.h"
|
||||
#include "MemoryMapMultiBufferInputBroker.h"
|
||||
#include "MemoryMapMultiBufferOutputBroker.h"
|
||||
#include "MemoryMapOutputBroker.h"
|
||||
#include "MemoryMapSynchronisedInputBroker.h"
|
||||
#include "MemoryMapSynchronisedMultiBufferInputBroker.h"
|
||||
#include "MemoryMapSynchronisedMultiBufferOutputBroker.h"
|
||||
#include "MemoryMapSynchronisedOutputBroker.h"
|
||||
|
||||
namespace MARTe {
|
||||
|
||||
// Recursive search for any object with a given name anywhere in the registry.
|
||||
// Used to find GAMs regardless of nesting level.
|
||||
static Reference FindByNameRecursive(ReferenceContainer *container,
|
||||
const char8 *name) {
|
||||
if (container == NULL_PTR(ReferenceContainer *))
|
||||
return Reference();
|
||||
uint32 n = container->Size();
|
||||
for (uint32 i = 0; i < n; i++) {
|
||||
Reference child = container->Get(i);
|
||||
if (!child.IsValid())
|
||||
continue;
|
||||
if (StringHelper::Compare(child->GetName(), name) == 0)
|
||||
return child;
|
||||
ReferenceContainer *sub =
|
||||
dynamic_cast<ReferenceContainer *>(child.operator->());
|
||||
if (sub != NULL_PTR(ReferenceContainer *)) {
|
||||
Reference found = FindByNameRecursive(sub, name);
|
||||
if (found.IsValid())
|
||||
return found;
|
||||
}
|
||||
}
|
||||
return Reference();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Helper for optimized signal processing within brokers.
|
||||
*/
|
||||
class DebugBrokerHelper {
|
||||
public:
|
||||
// Evaluate a break condition against the current live value of a signal.
|
||||
// Returns true if the condition is met and execution should be paused.
|
||||
// Only called when signal->breakOp != BREAK_OFF; reads memoryAddress directly.
|
||||
static bool EvaluateBreak(const DebugSignalInfo *s) {
|
||||
if (s->memoryAddress == NULL_PTR(void *)) return false;
|
||||
float64 val = 0.0;
|
||||
const TypeDescriptor &t = s->type;
|
||||
if (t == Float64Bit) val = *static_cast<const float64 *>(s->memoryAddress);
|
||||
else if (t == Float32Bit) val = *static_cast<const float32 *>(s->memoryAddress);
|
||||
else if (t == UnsignedInteger32Bit) val = *static_cast<const uint32 *>(s->memoryAddress);
|
||||
else if (t == SignedInteger32Bit) val = *static_cast<const int32 *>(s->memoryAddress);
|
||||
else if (t == UnsignedInteger64Bit) val = static_cast<float64>(*static_cast<const uint64 *>(s->memoryAddress));
|
||||
else if (t == SignedInteger64Bit) val = static_cast<float64>(*static_cast<const int64 *>(s->memoryAddress));
|
||||
else if (t == UnsignedInteger16Bit) val = *static_cast<const uint16 *>(s->memoryAddress);
|
||||
else if (t == SignedInteger16Bit) val = *static_cast<const int16 *>(s->memoryAddress);
|
||||
else if (t == UnsignedInteger8Bit) val = *static_cast<const uint8 *>(s->memoryAddress);
|
||||
else if (t == SignedInteger8Bit) val = *static_cast<const int8 *>(s->memoryAddress);
|
||||
else return false; // unsupported type — skip
|
||||
const float64 thr = s->breakThreshold;
|
||||
switch (s->breakOp) {
|
||||
case BREAK_GT: return val > thr;
|
||||
case BREAK_LT: return val < thr;
|
||||
case BREAK_EQ: return val == thr;
|
||||
case BREAK_GEQ: return val >= thr;
|
||||
case BREAK_LEQ: return val <= thr;
|
||||
case BREAK_NEQ: return val != thr;
|
||||
default: return false;
|
||||
}
|
||||
}
|
||||
|
||||
// Spin-wait point for output brokers — called from Execute() AFTER Process().
|
||||
// Spins while paused, then consumes one step counter tick (if stepping).
|
||||
// Input brokers must NOT call this — they complete normally to avoid blocking
|
||||
// cross-thread EventSem posts (RealTimeThreadSynchBroker would time out).
|
||||
static void OutputPauseAndStep(DebugServiceI *service, const char8 *gamName) {
|
||||
if (service == NULL_PTR(DebugServiceI *)) return;
|
||||
// Fast path: nothing to do when neither paused nor stepping.
|
||||
// Avoids Threads::Name() lookup (and its mutex) on every 1000 Hz cycle.
|
||||
if (!service->IsPaused() && !service->IsStepPending()) return;
|
||||
// Wait if already paused (manual PAUSE or breakpoint from a previous cycle)
|
||||
while (service->IsPaused()) Sleep::MSec(10);
|
||||
// Pass the OS thread name so per-thread step filtering works.
|
||||
const char8 *tName = Threads::Name(Threads::Id());
|
||||
service->ConsumeStepIfNeeded(gamName, tName);
|
||||
while (service->IsPaused()) Sleep::MSec(10);
|
||||
}
|
||||
|
||||
static void Process(DebugServiceI *service,
|
||||
DebugSignalInfo **signalInfoPointers,
|
||||
Vector<uint32> &activeIndices, Vector<uint32> &activeSizes,
|
||||
FastPollingMutexSem &activeMutex,
|
||||
volatile bool *anyBreakFlag,
|
||||
Vector<uint32> *breakIndices) {
|
||||
if (service == NULL_PTR(DebugServiceI *))
|
||||
return;
|
||||
|
||||
// NOTE: No spin here. Spinning for paused state is handled in Execute() of
|
||||
// OUTPUT brokers only (see OutputPauseAndStep). Input brokers must not block
|
||||
// because that prevents cross-thread EventSem posts from completing.
|
||||
|
||||
activeMutex.FastLock();
|
||||
uint32 n = activeIndices.GetNumberOfElements();
|
||||
if (n > 0 && signalInfoPointers != NULL_PTR(DebugSignalInfo **)) {
|
||||
// Capture timestamp ONCE per broker cycle for lowest impact
|
||||
uint64 ts = (uint64)((float64)HighResolutionTimer::Counter() *
|
||||
HighResolutionTimer::Period() * 1.0e9);
|
||||
|
||||
for (uint32 i = 0; i < n; i++) {
|
||||
uint32 idx = activeIndices[i];
|
||||
uint32 size = activeSizes[i];
|
||||
DebugSignalInfo *s = signalInfoPointers[idx];
|
||||
if (s != NULL_PTR(DebugSignalInfo *)) {
|
||||
service->ProcessSignal(s, size, ts);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// FIX #3: copy break indices under lock into a local stack array, then
|
||||
// release activeMutex BEFORE evaluating. EvaluateBreak reads signal
|
||||
// memory which can stall (cache miss); holding activeMutex during that
|
||||
// stall would block UpdateBrokersBreakStatus() in the Server thread and
|
||||
// cause unnecessary priority inversion on the RT path.
|
||||
bool shouldCheckBreak = (*anyBreakFlag && !service->IsPaused() &&
|
||||
breakIndices != NULL_PTR(Vector<uint32> *) &&
|
||||
signalInfoPointers != NULL_PTR(DebugSignalInfo **));
|
||||
static const uint32 MAX_BREAK_INDICES = 64u;
|
||||
uint32 localBreakIdx[MAX_BREAK_INDICES];
|
||||
uint32 nb = 0u;
|
||||
if (shouldCheckBreak) {
|
||||
nb = breakIndices->GetNumberOfElements();
|
||||
if (nb > MAX_BREAK_INDICES) nb = MAX_BREAK_INDICES;
|
||||
for (uint32 i = 0; i < nb; i++) {
|
||||
localBreakIdx[i] = (*breakIndices)[i];
|
||||
}
|
||||
}
|
||||
|
||||
activeMutex.FastUnLock();
|
||||
|
||||
// Evaluate break conditions outside the lock — safe because
|
||||
// EvaluateBreak only reads signalInfoPointers[idx]->memoryAddress,
|
||||
// which is RT data-bus memory and is never freed during the RT cycle.
|
||||
if (shouldCheckBreak && nb > 0u) {
|
||||
for (uint32 i = 0; i < nb; i++) {
|
||||
uint32 idx = localBreakIdx[i];
|
||||
DebugSignalInfo *s = signalInfoPointers[idx];
|
||||
if (s != NULL_PTR(DebugSignalInfo *) && s->breakOp != BREAK_OFF &&
|
||||
EvaluateBreak(s)) {
|
||||
service->SetPaused(true);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Pass numCopies explicitly so we can mock it
|
||||
static void
|
||||
InitSignals(BrokerI *broker, DataSourceI &dataSourceIn,
|
||||
DebugServiceI *&service, DebugSignalInfo **&signalInfoPointers,
|
||||
uint32 numCopies, MemoryMapBrokerCopyTableEntry *copyTable,
|
||||
const char8 *functionName, SignalDirection direction,
|
||||
volatile bool *anyActiveFlag, Vector<uint32> *activeIndices,
|
||||
Vector<uint32> *activeSizes, FastPollingMutexSem *activeMutex,
|
||||
volatile bool *anyBreakFlag, Vector<uint32> *breakIndices) {
|
||||
if (numCopies > 0) {
|
||||
signalInfoPointers = new DebugSignalInfo *[numCopies];
|
||||
for (uint32 i = 0; i < numCopies; i++)
|
||||
signalInfoPointers[i] = NULL_PTR(DebugSignalInfo *);
|
||||
}
|
||||
|
||||
// Use the singleton registered by DebugService::Initialise() — no ORD
|
||||
// search on the Init path, and no dependency on a concrete type.
|
||||
service = DebugServiceI::GetInstance();
|
||||
|
||||
if (service && (copyTable != NULL_PTR(MemoryMapBrokerCopyTableEntry *))) {
|
||||
|
||||
StreamString dsPath;
|
||||
DebugServiceI::GetFullObjectName(dataSourceIn, dsPath);
|
||||
fprintf(stderr, ">> %s broker for %s [%d]\n",
|
||||
direction == InputSignals ? "Input" : "Output", dsPath.Buffer(),
|
||||
numCopies);
|
||||
MemoryMapBroker *mmb = dynamic_cast<MemoryMapBroker *>(broker);
|
||||
if (mmb == NULL_PTR(MemoryMapBroker *)) {
|
||||
fprintf(stderr, ">> Impossible to get broker pointer!!\n");
|
||||
}
|
||||
|
||||
for (uint32 i = 0; i < numCopies; i++) {
|
||||
void *addr = copyTable[i].dataSourcePointer;
|
||||
TypeDescriptor type = copyTable[i].type;
|
||||
|
||||
uint32 dsIdx = i;
|
||||
if (mmb != NULL_PTR(MemoryMapBroker *)) {
|
||||
dsIdx = mmb->GetDSCopySignalIndex(i);
|
||||
}
|
||||
|
||||
StreamString signalName;
|
||||
if (!dataSourceIn.GetSignalName(dsIdx, signalName))
|
||||
signalName = "Unknown";
|
||||
fprintf(stderr, ">> registering %s.%s [%p]\n", dsPath.Buffer(),
|
||||
signalName.Buffer(), mmb);
|
||||
|
||||
uint8 dims = 0;
|
||||
uint32 elems = 1;
|
||||
(void)dataSourceIn.GetSignalNumberOfDimensions(dsIdx, dims);
|
||||
(void)dataSourceIn.GetSignalNumberOfElements(dsIdx, elems);
|
||||
|
||||
// Register canonical name
|
||||
StreamString dsFullName;
|
||||
dsFullName.Printf("%s.%s", dsPath.Buffer(), signalName.Buffer());
|
||||
service->RegisterSignal(addr, type, dsFullName.Buffer(), dims, elems);
|
||||
|
||||
// Register alias
|
||||
if (functionName != NULL_PTR(const char8 *)) {
|
||||
StreamString gamFullName;
|
||||
const char8 *dirStr =
|
||||
(direction == InputSignals) ? "InputSignals" : "OutputSignals";
|
||||
const char8 *dirStrShort = (direction == InputSignals) ? "In" : "Out";
|
||||
|
||||
// Search recursively through the entire registry for the GAM by name.
|
||||
// Direct Find("GAM1") only checks top-level; the GAM may be nested
|
||||
// several levels deep inside a RealTimeApplication container.
|
||||
Reference gamRef =
|
||||
FindByNameRecursive(ObjectRegistryDatabase::Instance(),
|
||||
functionName);
|
||||
fprintf(stderr, ">> GAM lookup '%s': %s\n", functionName,
|
||||
gamRef.IsValid() ? "FOUND" : "NOT FOUND");
|
||||
|
||||
if (gamRef.IsValid()) {
|
||||
StreamString absGamPath;
|
||||
DebugServiceI::GetFullObjectName(*(gamRef.operator->()), absGamPath);
|
||||
// Register short path (In/Out) for GUI compatibility
|
||||
gamFullName.Printf("%s.%s.%s", absGamPath.Buffer(), dirStrShort,
|
||||
signalName.Buffer());
|
||||
signalInfoPointers[i] =
|
||||
service->RegisterSignal(addr, type, gamFullName.Buffer(), dims, elems);
|
||||
} else {
|
||||
// Fallback to short form
|
||||
gamFullName.Printf("%s.%s.%s", functionName, dirStrShort,
|
||||
signalName.Buffer());
|
||||
signalInfoPointers[i] =
|
||||
service->RegisterSignal(addr, type, gamFullName.Buffer(), dims, elems);
|
||||
}
|
||||
} else {
|
||||
signalInfoPointers[i] =
|
||||
service->RegisterSignal(addr, type, dsFullName.Buffer(), dims, elems);
|
||||
}
|
||||
}
|
||||
|
||||
// Register broker in DebugService for optimized control
|
||||
bool isOutputBroker = (direction == OutputSignals);
|
||||
service->RegisterBroker(signalInfoPointers, numCopies, mmb, anyActiveFlag,
|
||||
activeIndices, activeSizes, activeMutex,
|
||||
anyBreakFlag, breakIndices,
|
||||
(functionName != NULL_PTR(const char8 *)) ? functionName : dsPath.Buffer(),
|
||||
isOutputBroker);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Template class to instrument any MARTe2 Broker.
|
||||
*/
|
||||
template <typename BaseClass> class DebugBrokerWrapper : public BaseClass {
|
||||
public:
|
||||
DebugBrokerWrapper() : BaseClass() {
|
||||
service = NULL_PTR(DebugServiceI *);
|
||||
signalInfoPointers = NULL_PTR(DebugSignalInfo **);
|
||||
numSignals = 0;
|
||||
anyActive = false;
|
||||
anyBreakActive = false;
|
||||
isOutput = false;
|
||||
gamName[0] = '\0';
|
||||
}
|
||||
|
||||
virtual ~DebugBrokerWrapper() {
|
||||
if (signalInfoPointers)
|
||||
delete[] signalInfoPointers;
|
||||
}
|
||||
|
||||
virtual bool Execute() {
|
||||
bool ret = BaseClass::Execute();
|
||||
if (ret && (anyActive || anyBreakActive)) {
|
||||
DebugBrokerHelper::Process(service, signalInfoPointers, activeIndices,
|
||||
activeSizes, activeMutex,
|
||||
&anyBreakActive, &breakIndices);
|
||||
}
|
||||
// Output brokers are the safe pause point: base Execute has already
|
||||
// committed data / posted any cross-thread EventSems.
|
||||
if (ret && isOutput) {
|
||||
DebugBrokerHelper::OutputPauseAndStep(service, gamName);
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
virtual bool Init(SignalDirection direction, DataSourceI &ds,
|
||||
const char8 *const name, void *gamMem) {
|
||||
bool ret = BaseClass::Init(direction, ds, name, gamMem);
|
||||
fprintf(stderr, ">> INIT BROKER %s %s\n", name,
|
||||
direction == InputSignals ? "In" : "Out");
|
||||
if (ret) {
|
||||
numSignals = this->GetNumberOfCopies();
|
||||
isOutput = (direction == OutputSignals);
|
||||
StringHelper::CopyN(gamName, name, 255u);
|
||||
DebugBrokerHelper::InitSignals(this, ds, service, signalInfoPointers,
|
||||
numSignals, this->copyTable, name,
|
||||
direction, &anyActive, &activeIndices,
|
||||
&activeSizes, &activeMutex,
|
||||
&anyBreakActive, &breakIndices);
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
virtual bool Init(SignalDirection direction, DataSourceI &ds,
|
||||
const char8 *const name, void *gamMem, const bool optim) {
|
||||
bool ret = BaseClass::Init(direction, ds, name, gamMem, false);
|
||||
fprintf(stderr, ">> INIT optimized BROKER %s %s\n", name,
|
||||
direction == InputSignals ? "In" : "Out");
|
||||
if (ret) {
|
||||
numSignals = this->GetNumberOfCopies();
|
||||
isOutput = (direction == OutputSignals);
|
||||
StringHelper::CopyN(gamName, name, 255u);
|
||||
DebugBrokerHelper::InitSignals(this, ds, service, signalInfoPointers,
|
||||
numSignals, this->copyTable, name,
|
||||
direction, &anyActive, &activeIndices,
|
||||
&activeSizes, &activeMutex,
|
||||
&anyBreakActive, &breakIndices);
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
DebugServiceI *service;
|
||||
DebugSignalInfo **signalInfoPointers;
|
||||
uint32 numSignals;
|
||||
volatile bool anyActive;
|
||||
volatile bool anyBreakActive;
|
||||
bool isOutput;
|
||||
char8 gamName[256];
|
||||
Vector<uint32> activeIndices;
|
||||
Vector<uint32> activeSizes;
|
||||
Vector<uint32> breakIndices;
|
||||
FastPollingMutexSem activeMutex;
|
||||
};
|
||||
|
||||
template <typename BaseClass>
|
||||
class DebugBrokerWrapperNoOptim : public BaseClass {
|
||||
public:
|
||||
DebugBrokerWrapperNoOptim() : BaseClass() {
|
||||
service = NULL_PTR(DebugServiceI *);
|
||||
signalInfoPointers = NULL_PTR(DebugSignalInfo **);
|
||||
numSignals = 0;
|
||||
anyActive = false;
|
||||
anyBreakActive = false;
|
||||
isOutput = false;
|
||||
gamName[0] = '\0';
|
||||
}
|
||||
|
||||
virtual ~DebugBrokerWrapperNoOptim() {
|
||||
if (signalInfoPointers)
|
||||
delete[] signalInfoPointers;
|
||||
}
|
||||
|
||||
virtual bool Execute() {
|
||||
bool ret = BaseClass::Execute();
|
||||
if (ret && (anyActive || anyBreakActive)) {
|
||||
DebugBrokerHelper::Process(service, signalInfoPointers, activeIndices,
|
||||
activeSizes, activeMutex,
|
||||
&anyBreakActive, &breakIndices);
|
||||
}
|
||||
if (ret && isOutput) {
|
||||
DebugBrokerHelper::OutputPauseAndStep(service, gamName);
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
virtual bool Init(SignalDirection direction, DataSourceI &ds,
|
||||
const char8 *const name, void *gamMem) {
|
||||
bool ret = BaseClass::Init(direction, ds, name, gamMem);
|
||||
if (ret) {
|
||||
numSignals = this->GetNumberOfCopies();
|
||||
isOutput = (direction == OutputSignals);
|
||||
StringHelper::CopyN(gamName, name, 255u);
|
||||
DebugBrokerHelper::InitSignals(this, ds, service, signalInfoPointers,
|
||||
numSignals, this->copyTable, name,
|
||||
direction, &anyActive, &activeIndices,
|
||||
&activeSizes, &activeMutex,
|
||||
&anyBreakActive, &breakIndices);
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
DebugServiceI *service;
|
||||
DebugSignalInfo **signalInfoPointers;
|
||||
uint32 numSignals;
|
||||
volatile bool anyActive;
|
||||
volatile bool anyBreakActive;
|
||||
bool isOutput;
|
||||
char8 gamName[256];
|
||||
Vector<uint32> activeIndices;
|
||||
Vector<uint32> activeSizes;
|
||||
Vector<uint32> breakIndices;
|
||||
FastPollingMutexSem activeMutex;
|
||||
};
|
||||
|
||||
class DebugMemoryMapAsyncOutputBroker : public MemoryMapAsyncOutputBroker {
|
||||
public:
|
||||
DebugMemoryMapAsyncOutputBroker() : MemoryMapAsyncOutputBroker() {
|
||||
service = NULL_PTR(DebugServiceI *);
|
||||
signalInfoPointers = NULL_PTR(DebugSignalInfo **);
|
||||
numSignals = 0;
|
||||
anyActive = false;
|
||||
anyBreakActive = false;
|
||||
gamName[0] = '\0';
|
||||
}
|
||||
virtual ~DebugMemoryMapAsyncOutputBroker() {
|
||||
if (signalInfoPointers)
|
||||
delete[] signalInfoPointers;
|
||||
}
|
||||
virtual bool Execute() {
|
||||
bool ret = MemoryMapAsyncOutputBroker::Execute();
|
||||
if (ret && (anyActive || anyBreakActive)) {
|
||||
DebugBrokerHelper::Process(service, signalInfoPointers, activeIndices,
|
||||
activeSizes, activeMutex,
|
||||
&anyBreakActive, &breakIndices);
|
||||
}
|
||||
// Async output brokers are always output direction
|
||||
if (ret) {
|
||||
DebugBrokerHelper::OutputPauseAndStep(service, gamName);
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
virtual bool InitWithBufferParameters(const SignalDirection direction,
|
||||
DataSourceI &dataSourceIn,
|
||||
const char8 *const functionName,
|
||||
void *const gamMemoryAddress,
|
||||
const uint32 numberOfBuffersIn,
|
||||
const ProcessorType &cpuMaskIn,
|
||||
const uint32 stackSizeIn) {
|
||||
bool ret = MemoryMapAsyncOutputBroker::InitWithBufferParameters(
|
||||
direction, dataSourceIn, functionName, gamMemoryAddress,
|
||||
numberOfBuffersIn, cpuMaskIn, stackSizeIn);
|
||||
if (ret) {
|
||||
numSignals = this->GetNumberOfCopies();
|
||||
StringHelper::CopyN(gamName, (functionName != NULL_PTR(const char8 *)) ? functionName : "", 255u);
|
||||
DebugBrokerHelper::InitSignals(
|
||||
this, dataSourceIn, service, signalInfoPointers, numSignals,
|
||||
this->copyTable, functionName, direction, &anyActive, &activeIndices,
|
||||
&activeSizes, &activeMutex, &anyBreakActive, &breakIndices);
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
DebugServiceI *service;
|
||||
DebugSignalInfo **signalInfoPointers;
|
||||
uint32 numSignals;
|
||||
volatile bool anyActive;
|
||||
volatile bool anyBreakActive;
|
||||
char8 gamName[256];
|
||||
Vector<uint32> activeIndices;
|
||||
Vector<uint32> activeSizes;
|
||||
Vector<uint32> breakIndices;
|
||||
FastPollingMutexSem activeMutex;
|
||||
};
|
||||
|
||||
class DebugMemoryMapAsyncTriggerOutputBroker
|
||||
: public MemoryMapAsyncTriggerOutputBroker {
|
||||
public:
|
||||
DebugMemoryMapAsyncTriggerOutputBroker()
|
||||
: MemoryMapAsyncTriggerOutputBroker() {
|
||||
service = NULL_PTR(DebugServiceI *);
|
||||
signalInfoPointers = NULL_PTR(DebugSignalInfo **);
|
||||
numSignals = 0;
|
||||
anyActive = false;
|
||||
anyBreakActive = false;
|
||||
gamName[0] = '\0';
|
||||
}
|
||||
virtual ~DebugMemoryMapAsyncTriggerOutputBroker() {
|
||||
if (signalInfoPointers)
|
||||
delete[] signalInfoPointers;
|
||||
}
|
||||
virtual bool Execute() {
|
||||
bool ret = MemoryMapAsyncTriggerOutputBroker::Execute();
|
||||
if (ret && (anyActive || anyBreakActive)) {
|
||||
DebugBrokerHelper::Process(service, signalInfoPointers, activeIndices,
|
||||
activeSizes, activeMutex,
|
||||
&anyBreakActive, &breakIndices);
|
||||
}
|
||||
if (ret) {
|
||||
DebugBrokerHelper::OutputPauseAndStep(service, gamName);
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
virtual bool InitWithTriggerParameters(
|
||||
const SignalDirection direction, DataSourceI &dataSourceIn,
|
||||
const char8 *const functionName, void *const gamMemoryAddress,
|
||||
const uint32 numberOfBuffersIn, const uint32 preTriggerBuffersIn,
|
||||
const uint32 postTriggerBuffersIn, const ProcessorType &cpuMaskIn,
|
||||
const uint32 stackSizeIn) {
|
||||
bool ret = MemoryMapAsyncTriggerOutputBroker::InitWithTriggerParameters(
|
||||
direction, dataSourceIn, functionName, gamMemoryAddress,
|
||||
numberOfBuffersIn, preTriggerBuffersIn, postTriggerBuffersIn, cpuMaskIn,
|
||||
stackSizeIn);
|
||||
if (ret) {
|
||||
numSignals = this->GetNumberOfCopies();
|
||||
StringHelper::CopyN(gamName, (functionName != NULL_PTR(const char8 *)) ? functionName : "", 255u);
|
||||
DebugBrokerHelper::InitSignals(
|
||||
this, dataSourceIn, service, signalInfoPointers, numSignals,
|
||||
this->copyTable, functionName, direction, &anyActive, &activeIndices,
|
||||
&activeSizes, &activeMutex, &anyBreakActive, &breakIndices);
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
DebugServiceI *service;
|
||||
DebugSignalInfo **signalInfoPointers;
|
||||
uint32 numSignals;
|
||||
volatile bool anyActive;
|
||||
volatile bool anyBreakActive;
|
||||
char8 gamName[256];
|
||||
Vector<uint32> activeIndices;
|
||||
Vector<uint32> activeSizes;
|
||||
Vector<uint32> breakIndices;
|
||||
FastPollingMutexSem activeMutex;
|
||||
};
|
||||
|
||||
template <typename T> class DebugBrokerBuilder : public ObjectBuilder {
|
||||
public:
|
||||
virtual Object *Build(HeapI *const heap) const { return new (heap) T(); }
|
||||
};
|
||||
|
||||
typedef DebugBrokerWrapper<MemoryMapInputBroker> DebugMemoryMapInputBroker;
|
||||
// LCOV_EXCL_START
|
||||
typedef DebugBrokerWrapper<MemoryMapOutputBroker> DebugMemoryMapOutputBroker;
|
||||
typedef DebugBrokerWrapper<MemoryMapSynchronisedInputBroker>
|
||||
DebugMemoryMapSynchronisedInputBroker;
|
||||
typedef DebugBrokerWrapper<MemoryMapSynchronisedOutputBroker>
|
||||
DebugMemoryMapSynchronisedOutputBroker;
|
||||
typedef DebugBrokerWrapperNoOptim<MemoryMapInterpolatedInputBroker>
|
||||
DebugMemoryMapInterpolatedInputBroker;
|
||||
typedef DebugBrokerWrapper<MemoryMapMultiBufferInputBroker>
|
||||
DebugMemoryMapMultiBufferInputBroker;
|
||||
typedef DebugBrokerWrapper<MemoryMapMultiBufferOutputBroker>
|
||||
DebugMemoryMapMultiBufferOutputBroker;
|
||||
typedef DebugBrokerWrapper<MemoryMapSynchronisedMultiBufferInputBroker>
|
||||
DebugMemoryMapSynchronisedMultiBufferInputBroker;
|
||||
typedef DebugBrokerWrapper<MemoryMapSynchronisedMultiBufferOutputBroker>
|
||||
DebugMemoryMapSynchronisedMultiBufferOutputBroker;
|
||||
// LCOV_EXCL_STOP
|
||||
|
||||
typedef DebugBrokerBuilder<DebugMemoryMapInputBroker>
|
||||
DebugMemoryMapInputBrokerBuilder;
|
||||
// LCOV_EXCL_START
|
||||
typedef DebugBrokerBuilder<DebugMemoryMapOutputBroker>
|
||||
DebugMemoryMapOutputBrokerBuilder;
|
||||
typedef DebugBrokerBuilder<DebugMemoryMapSynchronisedInputBroker>
|
||||
DebugMemoryMapSynchronisedInputBrokerBuilder;
|
||||
typedef DebugBrokerBuilder<DebugMemoryMapSynchronisedOutputBroker>
|
||||
DebugMemoryMapSynchronisedOutputBrokerBuilder;
|
||||
typedef DebugBrokerBuilder<DebugMemoryMapInterpolatedInputBroker>
|
||||
DebugMemoryMapInterpolatedInputBrokerBuilder;
|
||||
typedef DebugBrokerBuilder<DebugMemoryMapMultiBufferInputBroker>
|
||||
DebugMemoryMapMultiBufferInputBrokerBuilder;
|
||||
typedef DebugBrokerBuilder<DebugMemoryMapMultiBufferOutputBroker>
|
||||
DebugMemoryMapMultiBufferOutputBrokerBuilder;
|
||||
typedef DebugBrokerBuilder<DebugMemoryMapSynchronisedMultiBufferInputBroker>
|
||||
DebugMemoryMapSynchronisedMultiBufferInputBrokerBuilder;
|
||||
typedef DebugBrokerBuilder<DebugMemoryMapSynchronisedMultiBufferOutputBroker>
|
||||
DebugMemoryMapSynchronisedMultiBufferOutputBrokerBuilder;
|
||||
typedef DebugBrokerBuilder<DebugMemoryMapAsyncOutputBroker>
|
||||
DebugMemoryMapAsyncOutputBrokerBuilder;
|
||||
typedef DebugBrokerBuilder<DebugMemoryMapAsyncTriggerOutputBroker>
|
||||
DebugMemoryMapAsyncTriggerOutputBrokerBuilder;
|
||||
// LCOV_EXCL_STOP
|
||||
|
||||
} // namespace MARTe
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,188 @@
|
||||
#ifndef DEBUGCORE_H
|
||||
#define DEBUGCORE_H
|
||||
|
||||
#include "CompilerTypes.h"
|
||||
#include "TypeDescriptor.h"
|
||||
#include "StreamString.h"
|
||||
#include <string.h>
|
||||
|
||||
namespace MARTe {
|
||||
|
||||
// Break condition operators stored in DebugSignalInfo::breakOp
|
||||
enum BreakOp {
|
||||
BREAK_OFF = 0,
|
||||
BREAK_GT = 1, // >
|
||||
BREAK_LT = 2, // <
|
||||
BREAK_EQ = 3, // ==
|
||||
BREAK_GEQ = 4, // >=
|
||||
BREAK_LEQ = 5, // <=
|
||||
BREAK_NEQ = 6 // !=
|
||||
};
|
||||
|
||||
struct DebugSignalInfo {
|
||||
void* memoryAddress;
|
||||
TypeDescriptor type;
|
||||
StreamString name;
|
||||
uint8 numberOfDimensions;
|
||||
uint32 numberOfElements;
|
||||
volatile bool isTracing;
|
||||
volatile bool isForcing;
|
||||
uint8 forcedValue[1024];
|
||||
uint8 forcedMask[32]; // bit e set → element e is forced; supports up to 256 elements
|
||||
uint32 internalID;
|
||||
volatile uint32 decimationFactor;
|
||||
volatile uint32 decimationCounter;
|
||||
// Conditional break fields (zero-cost when breakOp == BREAK_OFF)
|
||||
volatile uint8 breakOp; // BreakOp enum value
|
||||
float64 breakThreshold; // comparison threshold
|
||||
};
|
||||
|
||||
#pragma pack(push, 1)
|
||||
struct TraceHeader {
|
||||
uint32 magic; // 0xDA7A57AD
|
||||
uint32 seq; // Sequence number
|
||||
uint64 timestamp; // HighRes timestamp
|
||||
uint32 count; // Number of samples in payload
|
||||
};
|
||||
#pragma pack(pop)
|
||||
|
||||
/**
|
||||
* @brief Ring buffer for high-frequency signal tracing.
|
||||
* @details New format per sample: [ID:4][Timestamp:8][Size:4][Data:N]
|
||||
*/
|
||||
class TraceRingBuffer {
|
||||
public:
|
||||
TraceRingBuffer() {
|
||||
bufferSize = 0;
|
||||
buffer = NULL_PTR(uint8*);
|
||||
readIndex = 0;
|
||||
writeIndex = 0;
|
||||
}
|
||||
|
||||
~TraceRingBuffer() {
|
||||
if (buffer != NULL_PTR(uint8*)) {
|
||||
delete[] buffer;
|
||||
}
|
||||
}
|
||||
|
||||
bool Init(uint32 size) {
|
||||
if (buffer != NULL_PTR(uint8*)) {
|
||||
delete[] buffer;
|
||||
}
|
||||
bufferSize = size;
|
||||
buffer = new uint8[bufferSize];
|
||||
readIndex = 0;
|
||||
writeIndex = 0;
|
||||
return (buffer != NULL_PTR(uint8*));
|
||||
}
|
||||
|
||||
bool Push(uint32 signalID, uint64 timestamp, void* data, uint32 size) {
|
||||
uint32 packetSize = 4 + 8 + 4 + size; // ID + TS + Size + Data
|
||||
uint32 read = readIndex;
|
||||
uint32 write = writeIndex;
|
||||
|
||||
uint32 available = 0;
|
||||
if (read <= write) {
|
||||
available = bufferSize - (write - read) - 1;
|
||||
} else {
|
||||
available = read - write - 1;
|
||||
}
|
||||
|
||||
if (available < packetSize) return false;
|
||||
|
||||
uint32 tempWrite = write;
|
||||
WriteToBuffer(&tempWrite, &signalID, 4);
|
||||
WriteToBuffer(&tempWrite, ×tamp, 8);
|
||||
WriteToBuffer(&tempWrite, &size, 4);
|
||||
WriteToBuffer(&tempWrite, data, size);
|
||||
|
||||
writeIndex = tempWrite;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Pop(uint32 &signalID, uint64 ×tamp, void* dataBuffer, uint32 &size, uint32 maxSize) {
|
||||
uint32 read = readIndex;
|
||||
uint32 write = writeIndex;
|
||||
if (read == write) return false;
|
||||
|
||||
uint32 tempRead = read;
|
||||
uint32 tempId = 0;
|
||||
uint64 tempTs = 0;
|
||||
uint32 tempSize = 0;
|
||||
|
||||
ReadFromBuffer(&tempRead, &tempId, 4);
|
||||
ReadFromBuffer(&tempRead, &tempTs, 8);
|
||||
ReadFromBuffer(&tempRead, &tempSize, 4);
|
||||
|
||||
if (tempSize > maxSize) {
|
||||
// FIX #5: Skip only the current entry rather than discarding the
|
||||
// entire ring buffer. tempRead is already past the 16-byte header;
|
||||
// advancing by tempSize lands at the start of the next entry.
|
||||
//
|
||||
// Safety fallback: if tempSize >= bufferSize the stored size field
|
||||
// is corrupt (it can never be that large). In that case we cannot
|
||||
// locate the next entry safely, so fall back to discarding everything
|
||||
// to avoid reading garbage as sample headers on future Pop() calls.
|
||||
if (tempSize >= bufferSize) {
|
||||
readIndex = write; // corrupt ring — discard all
|
||||
} else {
|
||||
readIndex = (tempRead + tempSize) % bufferSize;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
ReadFromBuffer(&tempRead, dataBuffer, tempSize);
|
||||
|
||||
signalID = tempId;
|
||||
timestamp = tempTs;
|
||||
size = tempSize;
|
||||
|
||||
readIndex = tempRead;
|
||||
return true;
|
||||
}
|
||||
|
||||
uint32 Count() {
|
||||
uint32 read = readIndex;
|
||||
uint32 write = writeIndex;
|
||||
if (write >= read) return write - read;
|
||||
return bufferSize - (read - write);
|
||||
}
|
||||
|
||||
private:
|
||||
void WriteToBuffer(uint32 *idx, void* src, uint32 count) {
|
||||
uint32 current = *idx;
|
||||
uint32 spaceToEnd = bufferSize - current;
|
||||
if (count <= spaceToEnd) {
|
||||
memcpy(&buffer[current], src, count);
|
||||
*idx = (current + count) % bufferSize;
|
||||
} else {
|
||||
memcpy(&buffer[current], src, spaceToEnd);
|
||||
uint32 remaining = count - spaceToEnd;
|
||||
memcpy(&buffer[0], (uint8*)src + spaceToEnd, remaining);
|
||||
*idx = remaining;
|
||||
}
|
||||
}
|
||||
|
||||
void ReadFromBuffer(uint32 *idx, void* dst, uint32 count) {
|
||||
uint32 current = *idx;
|
||||
uint32 spaceToEnd = bufferSize - current;
|
||||
if (count <= spaceToEnd) {
|
||||
memcpy(dst, &buffer[current], count);
|
||||
*idx = (current + count) % bufferSize;
|
||||
} else {
|
||||
memcpy(dst, &buffer[current], spaceToEnd);
|
||||
uint32 remaining = count - spaceToEnd;
|
||||
memcpy((uint8*)dst + spaceToEnd, &buffer[0], remaining);
|
||||
*idx = remaining;
|
||||
}
|
||||
}
|
||||
|
||||
volatile uint32 readIndex;
|
||||
volatile uint32 writeIndex;
|
||||
uint32 bufferSize;
|
||||
uint8 *buffer;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,618 @@
|
||||
#include "BasicTCPSocket.h"
|
||||
#include "ConfigurationDatabase.h"
|
||||
#include "DebugService.h"
|
||||
#include "GlobalObjectsDatabase.h"
|
||||
#include "HighResolutionTimer.h"
|
||||
#include "LoggerService.h"
|
||||
#include "Message.h"
|
||||
#include "ObjectRegistryDatabase.h"
|
||||
#include "ReferenceT.h"
|
||||
#include "Sleep.h"
|
||||
#include "StreamString.h"
|
||||
#include "StringHelper.h"
|
||||
#include "Threads.h"
|
||||
#include "TimeoutType.h"
|
||||
#include "UDPSProtocol.h"
|
||||
|
||||
namespace MARTe {
|
||||
|
||||
CLASS_REGISTER(DebugService, "1.0")
|
||||
|
||||
// C++98 ODR definitions for static constants
|
||||
const uint32 DebugService::CMD_RATE_LIMIT;
|
||||
const uint32 DebugService::CLIENT_IDLE_TIMEOUT_MS;
|
||||
const uint32 DebugService::INPUT_BUFFER_MAX;
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Constructor / Destructor
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
DebugService::DebugService()
|
||||
: DebugServiceBase(),
|
||||
EmbeddedServiceMethodBinderI(),
|
||||
binderServer(this, ServiceBinder::ServerType),
|
||||
binderStreamer(this, ServiceBinder::StreamerType),
|
||||
threadService(binderServer),
|
||||
streamerService(binderStreamer) {
|
||||
controlPort = 0u;
|
||||
streamPort = 8081u;
|
||||
logPort = 8082u;
|
||||
streamIP = "127.0.0.1";
|
||||
isServer = false;
|
||||
suppressTimeoutLogs = true;
|
||||
activeClient = NULL_PTR(BasicTCPSocket *);
|
||||
cmdCountInWindow = 0u;
|
||||
cmdWindowStartMs = 0u;
|
||||
lastDataTimeMs = 0u;
|
||||
inputBuffer = "";
|
||||
|
||||
// UDPS members
|
||||
udpsNumSlots = 0u;
|
||||
udpsDataPayload = NULL_PTR(uint8 *);
|
||||
udpsDataPayloadSize = 0u;
|
||||
udpsPacketCounter = 0u;
|
||||
udpsConfigPending = false;
|
||||
}
|
||||
|
||||
DebugService::~DebugService() {
|
||||
if (DebugServiceI::GetInstance() == this) {
|
||||
DebugServiceI::SetInstance(NULL_PTR(DebugServiceI *));
|
||||
}
|
||||
threadService.Stop();
|
||||
streamerService.Stop();
|
||||
tcpServer.Close();
|
||||
udpSocket.Close();
|
||||
if (activeClient != NULL_PTR(BasicTCPSocket *)) {
|
||||
activeClient->Close();
|
||||
delete activeClient;
|
||||
activeClient = NULL_PTR(BasicTCPSocket *);
|
||||
}
|
||||
if (udpsDataPayload != NULL_PTR(uint8 *)) {
|
||||
delete[] udpsDataPayload;
|
||||
udpsDataPayload = NULL_PTR(uint8 *);
|
||||
}
|
||||
// signals owned by DebugServiceBase destructor
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Initialise
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
bool DebugService::Initialise(StructuredDataI &data) {
|
||||
if (!ReferenceContainer::Initialise(data)) return false;
|
||||
|
||||
uint32 port = 0u;
|
||||
if (data.Read("ControlPort", port)) {
|
||||
controlPort = (uint16)port;
|
||||
} else {
|
||||
(void)data.Read("TcpPort", port);
|
||||
controlPort = (uint16)port;
|
||||
}
|
||||
|
||||
if (controlPort > 0u) {
|
||||
isServer = true;
|
||||
DebugServiceI::SetInstance(this);
|
||||
}
|
||||
|
||||
port = 8081u;
|
||||
if (data.Read("StreamPort", port)) {
|
||||
streamPort = (uint16)port;
|
||||
} else {
|
||||
(void)data.Read("UdpPort", port);
|
||||
streamPort = (uint16)port;
|
||||
}
|
||||
|
||||
port = 8082u;
|
||||
if (data.Read("LogPort", port)) {
|
||||
logPort = (uint16)port;
|
||||
} else {
|
||||
(void)data.Read("TcpLogPort", port);
|
||||
logPort = (uint16)port;
|
||||
}
|
||||
|
||||
StreamString tempIP;
|
||||
if (data.Read("StreamIP", tempIP)) {
|
||||
streamIP = tempIP;
|
||||
} else {
|
||||
streamIP = "127.0.0.1";
|
||||
}
|
||||
|
||||
uint32 suppress = 1u;
|
||||
if (data.Read("SuppressTimeoutLogs", suppress)) {
|
||||
suppressTimeoutLogs = (suppress == 1u);
|
||||
}
|
||||
|
||||
// Capture only the local subtree — do NOT call MoveToRoot() on the shared CDB.
|
||||
(void)data.Copy(fullConfig);
|
||||
|
||||
if (isServer) {
|
||||
if (!traceBuffer.Init(8 * 1024 * 1024)) return false;
|
||||
PatchRegistry();
|
||||
|
||||
ConfigurationDatabase threadData;
|
||||
threadData.Write("Timeout", (uint32)1000);
|
||||
threadService.Initialise(threadData);
|
||||
streamerService.Initialise(threadData);
|
||||
|
||||
if (!tcpServer.Open()) return false;
|
||||
if (!tcpServer.Listen(controlPort)) return false;
|
||||
if (!udpSocket.Open()) return false;
|
||||
// Note: do NOT bind udpSocket to streamPort here. The Go client
|
||||
// must own that port to receive streamed data. The socket sends
|
||||
// via an ephemeral source port, which is fine for UDP.
|
||||
|
||||
if (threadService.Start() != ErrorManagement::NoError) return false;
|
||||
if (streamerService.Start() != ErrorManagement::NoError) return false;
|
||||
|
||||
// Send initial (empty) CONFIG so clients know the schema version
|
||||
SendUDPSConfig();
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Transport config hook (called by RebuildConfigFromRegistry in base)
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
void DebugService::RebuildTransportConfig() {
|
||||
const char8 *myName = GetName();
|
||||
if (myName != NULL_PTR(const char8 *)) {
|
||||
if (fullConfig.MoveRelative(myName)) {
|
||||
(void)fullConfig.Write("ControlPort", static_cast<uint32>(controlPort));
|
||||
(void)fullConfig.Write("UdpPort", static_cast<uint32>(streamPort));
|
||||
(void)fullConfig.Write("LogPort", static_cast<uint32>(logPort));
|
||||
if (streamIP.Size() > 0u)
|
||||
(void)fullConfig.Write("StreamIP", streamIP.Buffer());
|
||||
(void)fullConfig.MoveToAncestor(1u);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// SERVICE_INFO hook
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
void DebugService::GetServiceInfo(StreamString &out) {
|
||||
out.Printf("OK SERVICE_INFO TCP_CTRL:%u UDP_STREAM:%u TCP_LOG:%u STATE:%s\n",
|
||||
controlPort, streamPort, logPort,
|
||||
isPaused ? "PAUSED" : "RUNNING");
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Execute / HandleMessage (framework boilerplate)
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
ErrorManagement::ErrorType DebugService::Execute(ExecutionInfo &info) {
|
||||
(void)info;
|
||||
return ErrorManagement::FatalError;
|
||||
}
|
||||
|
||||
ErrorManagement::ErrorType DebugService::HandleMessage(ReferenceT<Message> &data) {
|
||||
(void)data;
|
||||
return ErrorManagement::NoError;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// TraceSignal override — sets config-pending flag after base handling
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
uint32 DebugService::TraceSignal(const char8 *name, bool enable, uint32 decimation) {
|
||||
uint32 ret = DebugServiceBase::TraceSignal(name, enable, decimation);
|
||||
udpsConfigPending = true;
|
||||
return ret;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// InjectTcpLoggerIfNeeded
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
void DebugService::InjectTcpLoggerIfNeeded() {
|
||||
if (logPort == 0u) return;
|
||||
|
||||
Reference existing = ObjectRegistryDatabase::Instance()->Find("LoggerService");
|
||||
if (existing.IsValid()) {
|
||||
ReferenceContainer *rc =
|
||||
dynamic_cast<ReferenceContainer *>(existing.operator->());
|
||||
if (rc != NULL_PTR(ReferenceContainer *)) {
|
||||
for (uint32 i = 0u; i < rc->Size(); i++) {
|
||||
Reference child = rc->Get(i);
|
||||
// Check by class name to avoid a direct symbol dependency on TcpLogger.so
|
||||
if (child.IsValid()) {
|
||||
const ClassProperties *cp = child->GetClassProperties();
|
||||
if (cp != NULL_PTR(const ClassProperties *)) {
|
||||
if (StringHelper::Compare(cp->GetName(), "TcpLogger") == 0) {
|
||||
return; // already has a TcpLogger
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
ConfigurationDatabase lsCdb;
|
||||
(void)lsCdb.Write("Class", "LoggerService");
|
||||
uint32 cpus = 1u;
|
||||
(void)lsCdb.Write("CPUs", cpus);
|
||||
if (lsCdb.CreateRelative("+DebugConsumer")) {
|
||||
(void)lsCdb.Write("Class", "TcpLogger");
|
||||
uint32 p = static_cast<uint32>(logPort);
|
||||
(void)lsCdb.Write("Port", p);
|
||||
(void)lsCdb.MoveToAncestor(1u);
|
||||
}
|
||||
(void)lsCdb.MoveToRoot();
|
||||
|
||||
ReferenceT<LoggerService> ls(
|
||||
"LoggerService", GlobalObjectsDatabase::Instance()->GetStandardHeap());
|
||||
if (!ls.IsValid()) return;
|
||||
ls->SetName("LoggerService");
|
||||
if (!ls->Initialise(lsCdb)) return;
|
||||
(void)ObjectRegistryDatabase::Instance()->Insert(ls);
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Server thread
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
ErrorManagement::ErrorType DebugService::Server(ExecutionInfo &info) {
|
||||
if (info.GetStage() == ExecutionInfo::TerminationStage)
|
||||
return ErrorManagement::NoError;
|
||||
if (info.GetStage() == ExecutionInfo::StartupStage) {
|
||||
serverThreadId = Threads::Id();
|
||||
Sleep::MSec(500u);
|
||||
InjectTcpLoggerIfNeeded();
|
||||
return ErrorManagement::NoError;
|
||||
}
|
||||
|
||||
uint64 nowMs = (uint64)((float64)HighResolutionTimer::Counter() *
|
||||
HighResolutionTimer::Period() * 1000.0);
|
||||
|
||||
if (activeClient == NULL_PTR(BasicTCPSocket *)) {
|
||||
BasicTCPSocket *newClient = tcpServer.WaitConnection(TimeoutType(100));
|
||||
if (newClient != NULL_PTR(BasicTCPSocket *)) {
|
||||
clientMutex.FastLock();
|
||||
activeClient = newClient;
|
||||
clientMutex.FastUnLock();
|
||||
cmdCountInWindow = 0u;
|
||||
cmdWindowStartMs = nowMs;
|
||||
lastDataTimeMs = nowMs;
|
||||
}
|
||||
} else {
|
||||
if (nowMs - lastDataTimeMs > CLIENT_IDLE_TIMEOUT_MS) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Warning,
|
||||
"Server: TCP client idle for >%u ms — closing connection.",
|
||||
CLIENT_IDLE_TIMEOUT_MS);
|
||||
inputBuffer = "";
|
||||
clientMutex.FastLock();
|
||||
activeClient->Close();
|
||||
delete activeClient;
|
||||
activeClient = NULL_PTR(BasicTCPSocket *);
|
||||
clientMutex.FastUnLock();
|
||||
cmdCountInWindow = 0u;
|
||||
} else if (!activeClient->IsConnected()) {
|
||||
inputBuffer = "";
|
||||
clientMutex.FastLock();
|
||||
activeClient->Close();
|
||||
delete activeClient;
|
||||
activeClient = NULL_PTR(BasicTCPSocket *);
|
||||
clientMutex.FastUnLock();
|
||||
} else {
|
||||
char buffer[1024];
|
||||
uint32 size = 1024u;
|
||||
if (activeClient->Read(buffer, size)) {
|
||||
if (size > 0u) {
|
||||
lastDataTimeMs = nowMs;
|
||||
|
||||
if (nowMs - cmdWindowStartMs >= 1000u) {
|
||||
cmdWindowStartMs = nowMs;
|
||||
cmdCountInWindow = 0u;
|
||||
}
|
||||
|
||||
if (inputBuffer.Size() + (uint32)size > INPUT_BUFFER_MAX) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Warning,
|
||||
"Server: input buffer overflow (>%u bytes without newline) "
|
||||
"— disconnecting.", INPUT_BUFFER_MAX);
|
||||
inputBuffer = "";
|
||||
clientMutex.FastLock();
|
||||
activeClient->Close();
|
||||
delete activeClient;
|
||||
activeClient = NULL_PTR(BasicTCPSocket *);
|
||||
clientMutex.FastUnLock();
|
||||
cmdCountInWindow = 0u;
|
||||
} else {
|
||||
(void)inputBuffer.Seek(inputBuffer.Size());
|
||||
uint32 writeSize = (uint32)size;
|
||||
inputBuffer.Write(buffer, writeSize);
|
||||
|
||||
const char8 *raw = inputBuffer.Buffer();
|
||||
uint32 total = (uint32)inputBuffer.Size();
|
||||
uint32 lineStart = 0u;
|
||||
bool rateLimitExceeded = false;
|
||||
|
||||
for (uint32 pos = 0u; pos < total && !rateLimitExceeded; pos++) {
|
||||
if (raw[pos] != '\n') continue;
|
||||
uint32 len = pos - lineStart;
|
||||
if (len > 0u && raw[lineStart + len - 1u] == '\r') len--;
|
||||
if (len > 0u) {
|
||||
cmdCountInWindow++;
|
||||
if (cmdCountInWindow > CMD_RATE_LIMIT) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Warning,
|
||||
"Server: client exceeded rate limit (%u cmd/s) "
|
||||
"— disconnecting.", CMD_RATE_LIMIT);
|
||||
rateLimitExceeded = true;
|
||||
break;
|
||||
}
|
||||
StreamString command;
|
||||
uint32 cmdLen = len;
|
||||
command.Write(raw + lineStart, cmdLen);
|
||||
|
||||
// Dispatch via base HandleCommand, write response to socket.
|
||||
StreamString out;
|
||||
HandleCommand(command, out);
|
||||
if (out.Size() > 0u) {
|
||||
const char8 *wPtr = out.Buffer();
|
||||
uint32 remaining = (uint32)out.Size();
|
||||
lastDataTimeMs = (uint64)((float64)HighResolutionTimer::Counter() *
|
||||
HighResolutionTimer::Period() * 1000.0);
|
||||
while (remaining > 0u) {
|
||||
uint32 wrote = remaining;
|
||||
if (!activeClient->Write(wPtr, wrote) || wrote == 0u) {
|
||||
break;
|
||||
}
|
||||
wPtr += wrote;
|
||||
remaining -= wrote;
|
||||
lastDataTimeMs = (uint64)((float64)HighResolutionTimer::Counter() *
|
||||
HighResolutionTimer::Period() * 1000.0);
|
||||
}
|
||||
}
|
||||
}
|
||||
lineStart = pos + 1u;
|
||||
}
|
||||
|
||||
if (rateLimitExceeded) {
|
||||
inputBuffer = "";
|
||||
clientMutex.FastLock();
|
||||
activeClient->Close();
|
||||
delete activeClient;
|
||||
activeClient = NULL_PTR(BasicTCPSocket *);
|
||||
clientMutex.FastUnLock();
|
||||
cmdCountInWindow = 0u;
|
||||
} else {
|
||||
StreamString newInputBuffer;
|
||||
if (lineStart < total) {
|
||||
uint32 remLen = total - lineStart;
|
||||
newInputBuffer.Write(raw + lineStart, remLen);
|
||||
}
|
||||
inputBuffer = newInputBuffer;
|
||||
}
|
||||
}
|
||||
}
|
||||
} else {
|
||||
inputBuffer = "";
|
||||
clientMutex.FastLock();
|
||||
activeClient->Close();
|
||||
delete activeClient;
|
||||
activeClient = NULL_PTR(BasicTCPSocket *);
|
||||
clientMutex.FastUnLock();
|
||||
}
|
||||
}
|
||||
}
|
||||
return ErrorManagement::NoError;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Streamer thread — UDPS binary telemetry
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
ErrorManagement::ErrorType DebugService::Streamer(ExecutionInfo &info) {
|
||||
if (info.GetStage() == ExecutionInfo::TerminationStage) {
|
||||
if (udpsDataPayload != NULL_PTR(uint8 *)) {
|
||||
delete[] udpsDataPayload;
|
||||
udpsDataPayload = NULL_PTR(uint8 *);
|
||||
}
|
||||
return ErrorManagement::NoError;
|
||||
}
|
||||
if (info.GetStage() == ExecutionInfo::StartupStage) {
|
||||
streamerThreadId = Threads::Id();
|
||||
return ErrorManagement::NoError;
|
||||
}
|
||||
|
||||
// Set UDP destination
|
||||
InternetHost dest(streamPort, streamIP.Buffer());
|
||||
(void)udpSocket.SetDestination(dest);
|
||||
|
||||
// a) If config has changed, rebuild and send CONFIG packet
|
||||
if (udpsConfigPending) {
|
||||
SendUDPSConfig();
|
||||
udpsConfigPending = false;
|
||||
}
|
||||
|
||||
// b) Drain traceBuffer — pack each sample into udpsDataPayload
|
||||
bool anyData = false;
|
||||
uint32 id, size;
|
||||
uint64 ts;
|
||||
uint8 udpsSampleBuf[UDPS_MAX_SAMPLE_BYTES];
|
||||
|
||||
while (traceBuffer.Pop(id, ts, udpsSampleBuf, size, UDPS_MAX_SAMPLE_BYTES)) {
|
||||
// Find matching slot by internalID
|
||||
for (uint32 i = 0u; i < udpsNumSlots; i++) {
|
||||
if (udpsSlots[i].internalID == id) {
|
||||
if ((udpsDataPayload != NULL_PTR(uint8 *)) &&
|
||||
(8u + udpsSlots[i].wireOffset + udpsSlots[i].wireSize <= udpsDataPayloadSize)) {
|
||||
uint32 copySize = size;
|
||||
if (copySize > udpsSlots[i].wireSize) copySize = udpsSlots[i].wireSize;
|
||||
memcpy(udpsDataPayload + 8u + udpsSlots[i].wireOffset, udpsSampleBuf, copySize);
|
||||
udpsSlots[i].everFilled = true;
|
||||
}
|
||||
anyData = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// c) If we have data, stamp with HRT and send
|
||||
if (anyData && udpsNumSlots > 0u && udpsDataPayload != NULL_PTR(uint8 *)) {
|
||||
uint64 hrt = HighResolutionTimer::Counter();
|
||||
memcpy(udpsDataPayload, &hrt, 8u);
|
||||
SendUDPSFragmented(UDPS_TYPE_DATA, udpsDataPayload, udpsDataPayloadSize);
|
||||
udpsPacketCounter++;
|
||||
}
|
||||
|
||||
if (!anyData) {
|
||||
Sleep::MSec(1u);
|
||||
}
|
||||
|
||||
return ErrorManagement::NoError;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// SendUDPSConfig — build and send a UDPS CONFIG packet
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
bool DebugService::SendUDPSConfig() {
|
||||
// Snapshot traced signals under mutex
|
||||
mutex.FastLock();
|
||||
|
||||
// Count traced signals
|
||||
uint32 tracedCount = 0u;
|
||||
for (uint32 i = 0u; i < signals.GetNumberOfElements(); i++) {
|
||||
if (signals[i]->isTracing) {
|
||||
tracedCount++;
|
||||
}
|
||||
}
|
||||
|
||||
// Build CONFIG payload into udpsTxBuf starting at UDPS_HEADER_SIZE offset
|
||||
uint8 *payload = udpsTxBuf + UDPS_HEADER_SIZE;
|
||||
|
||||
// Write numTraced (uint32 LE)
|
||||
memcpy(payload, &tracedCount, 4u);
|
||||
uint32 payloadOffset = 4u;
|
||||
|
||||
// Build slot table in parallel
|
||||
uint32 currentOffset = 0u;
|
||||
uint32 newNumSlots = 0u;
|
||||
uint32 totalWireBytes = 0u;
|
||||
|
||||
for (uint32 i = 0u; i < signals.GetNumberOfElements(); i++) {
|
||||
if (!signals[i]->isTracing) continue;
|
||||
if (newNumSlots >= UDPS_MAX_SLOTS) break;
|
||||
|
||||
uint8 typeCode = UDPSTypeDescriptorToCode(signals[i]->type);
|
||||
if (typeCode == UDPS_TYPECODE_UNKNOWN) {
|
||||
// Skip unsupported types
|
||||
continue;
|
||||
}
|
||||
|
||||
uint32 numElements = signals[i]->numberOfElements;
|
||||
if (numElements == 0u) numElements = 1u;
|
||||
|
||||
uint32 numRows, numCols;
|
||||
if (signals[i]->numberOfDimensions >= 2u) {
|
||||
// For matrix: approximate square root
|
||||
numCols = numElements;
|
||||
numRows = 1u;
|
||||
} else if (signals[i]->numberOfDimensions == 1u) {
|
||||
numRows = numElements;
|
||||
numCols = 1u;
|
||||
} else {
|
||||
numRows = 1u;
|
||||
numCols = 1u;
|
||||
}
|
||||
|
||||
uint32 wireSize = UDPSTypeCodeByteSize(typeCode) * numElements;
|
||||
|
||||
// Write signal descriptor (136 bytes) to payload
|
||||
if (payloadOffset + UDPS_SIGNAL_DESC_SIZE <= sizeof(udpsTxBuf) - UDPS_HEADER_SIZE - 1u) {
|
||||
UDPSSignalDescriptor *desc = reinterpret_cast<UDPSSignalDescriptor *>(payload + payloadOffset);
|
||||
memset(desc, 0, UDPS_SIGNAL_DESC_SIZE);
|
||||
if (signals[i]->name.Size() > 0u) {
|
||||
strncpy(desc->name, signals[i]->name.Buffer(), UDPS_MAX_SIGNAL_NAME - 1u);
|
||||
}
|
||||
desc->typeCode = typeCode;
|
||||
desc->quantType = UDPS_QUANT_NONE;
|
||||
desc->numDimensions = signals[i]->numberOfDimensions;
|
||||
desc->numRows = numRows;
|
||||
desc->numCols = numCols;
|
||||
desc->rangeMin = 0.0;
|
||||
desc->rangeMax = 0.0;
|
||||
desc->timeMode = UDPS_TIMEMODE_PACKET;
|
||||
desc->samplingRate = 0.0;
|
||||
desc->timeSignalIdx = UDPS_NO_TIME_SIGNAL;
|
||||
// unit stays zero
|
||||
|
||||
payloadOffset += UDPS_SIGNAL_DESC_SIZE;
|
||||
}
|
||||
|
||||
// Fill slot table entry
|
||||
udpsSlots[newNumSlots].internalID = signals[i]->internalID;
|
||||
udpsSlots[newNumSlots].wireOffset = currentOffset;
|
||||
udpsSlots[newNumSlots].wireSize = wireSize;
|
||||
udpsSlots[newNumSlots].everFilled = false;
|
||||
newNumSlots++;
|
||||
|
||||
currentOffset += wireSize;
|
||||
totalWireBytes += wireSize;
|
||||
}
|
||||
|
||||
// Write publish mode byte
|
||||
if (payloadOffset < sizeof(udpsTxBuf) - UDPS_HEADER_SIZE) {
|
||||
payload[payloadOffset] = UDPS_PUBLISH_STRICT;
|
||||
payloadOffset++;
|
||||
}
|
||||
|
||||
udpsNumSlots = newNumSlots;
|
||||
|
||||
mutex.FastUnLock();
|
||||
|
||||
// Reallocate data payload buffer
|
||||
if (udpsDataPayload != NULL_PTR(uint8 *)) {
|
||||
delete[] udpsDataPayload;
|
||||
udpsDataPayload = NULL_PTR(uint8 *);
|
||||
}
|
||||
udpsDataPayloadSize = 8u + totalWireBytes; // 8 bytes HRT + signal data
|
||||
if (udpsDataPayloadSize > 0u) {
|
||||
udpsDataPayload = new uint8[udpsDataPayloadSize];
|
||||
memset(udpsDataPayload, 0, udpsDataPayloadSize);
|
||||
}
|
||||
|
||||
// Send CONFIG packet
|
||||
SendUDPSFragmented(UDPS_TYPE_CONFIG, payload, payloadOffset);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// SendUDPSFragmented — fragment and send a UDPS packet
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
bool DebugService::SendUDPSFragmented(uint8 type, const uint8 *payload, uint32 payloadSize) {
|
||||
if (payloadSize <= UDPS_MAX_PAYLOAD) {
|
||||
// Single datagram
|
||||
UDPSBuildHeader(udpsTxBuf, type, udpsPacketCounter, 0u, 1u, payloadSize);
|
||||
if (payload != (udpsTxBuf + UDPS_HEADER_SIZE)) {
|
||||
memcpy(udpsTxBuf + UDPS_HEADER_SIZE, payload, payloadSize);
|
||||
}
|
||||
uint32 toWrite = UDPS_HEADER_SIZE + payloadSize;
|
||||
(void)udpSocket.Write(reinterpret_cast<char8 *>(udpsTxBuf), toWrite);
|
||||
} else {
|
||||
// Fragmented
|
||||
uint32 numFrags = (payloadSize + UDPS_MAX_PAYLOAD - 1u) / UDPS_MAX_PAYLOAD;
|
||||
uint32 offset = 0u;
|
||||
for (uint32 i = 0u; i < numFrags; i++) {
|
||||
uint32 chunkSize = UDPS_MAX_PAYLOAD;
|
||||
if (offset + chunkSize > payloadSize) {
|
||||
chunkSize = payloadSize - offset;
|
||||
}
|
||||
UDPSBuildHeader(udpsTxBuf, type, udpsPacketCounter,
|
||||
static_cast<uint16>(i),
|
||||
static_cast<uint16>(numFrags),
|
||||
chunkSize);
|
||||
memcpy(udpsTxBuf + UDPS_HEADER_SIZE, payload + offset, chunkSize);
|
||||
uint32 toWrite = UDPS_HEADER_SIZE + chunkSize;
|
||||
(void)udpSocket.Write(reinterpret_cast<char8 *>(udpsTxBuf), toWrite);
|
||||
offset += chunkSize;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace MARTe
|
||||
@@ -0,0 +1,183 @@
|
||||
#ifndef DEBUGSERVICE_H
|
||||
#define DEBUGSERVICE_H
|
||||
|
||||
#include "BasicTCPSocket.h"
|
||||
#include "BasicUDPSocket.h"
|
||||
#include "DebugServiceBase.h"
|
||||
#include "EmbeddedServiceMethodBinderI.h"
|
||||
#include "MessageI.h"
|
||||
#include "ReferenceT.h"
|
||||
#include "SingleThreadService.h"
|
||||
#include "UDPSProtocol.h"
|
||||
|
||||
namespace MARTe {
|
||||
|
||||
/**
|
||||
* @brief TCP/UDP implementation of DebugServiceI (via DebugServiceBase).
|
||||
*
|
||||
* Signal tracing now uses the UDPS binary protocol (same wire format as
|
||||
* UDPStreamer) so that the same Go web client can consume both sources.
|
||||
*
|
||||
* Flow:
|
||||
* 1. The Go client listens on udpPort.
|
||||
* 2. DebugService binds the same port on its end (or uses the client's
|
||||
* configured IP:port as destination for the old push model).
|
||||
* 3. On startup and on every TRACE change, DebugService sends a CONFIG
|
||||
* packet listing the currently-traced signals.
|
||||
* 4. For each RT drain cycle that contains new data, DebugService sends
|
||||
* one DATA packet (Strict mode) with the most-recent value per signal.
|
||||
*/
|
||||
class DebugService : public DebugServiceBase,
|
||||
public MessageI,
|
||||
public EmbeddedServiceMethodBinderI {
|
||||
public:
|
||||
friend class DebugServiceTest;
|
||||
CLASS_REGISTER_DECLARATION()
|
||||
|
||||
DebugService();
|
||||
virtual ~DebugService();
|
||||
|
||||
virtual bool Initialise(StructuredDataI &data);
|
||||
|
||||
virtual ErrorManagement::ErrorType Execute(ExecutionInfo &info);
|
||||
virtual ErrorManagement::ErrorType HandleMessage(ReferenceT<Message> &data);
|
||||
|
||||
/**
|
||||
* @brief Override TraceSignal to trigger a CONFIG re-send after each change.
|
||||
*/
|
||||
virtual uint32 TraceSignal(const char8 *name, bool enable, uint32 decimation = 1u);
|
||||
|
||||
protected:
|
||||
virtual void GetServiceInfo(StreamString &out);
|
||||
virtual void RebuildTransportConfig();
|
||||
|
||||
private:
|
||||
void InjectTcpLoggerIfNeeded();
|
||||
|
||||
ErrorManagement::ErrorType Server (ExecutionInfo &info);
|
||||
ErrorManagement::ErrorType Streamer(ExecutionInfo &info);
|
||||
|
||||
/**
|
||||
* @brief Build and transmit a CONFIG packet for the current traced-signal set.
|
||||
* @details Iterates over all registered signals, selects those with
|
||||
* isTracing == true, builds UDPSSignalDescriptor array, and sends
|
||||
* a (possibly fragmented) UDPS CONFIG packet. Also rebuilds the
|
||||
* udpsSlots table and udpsWirePayload buffer.
|
||||
* @return true on success.
|
||||
*/
|
||||
bool SendUDPSConfig();
|
||||
|
||||
/**
|
||||
* @brief Fragment and send a payload as one or more UDPS datagrams.
|
||||
* @param type Packet type (UDPS_TYPE_CONFIG or UDPS_TYPE_DATA).
|
||||
* @param payload Pointer to fully-built payload.
|
||||
* @param payloadSize Byte count of payload.
|
||||
* @return true if all datagrams were written without error.
|
||||
*/
|
||||
bool SendUDPSFragmented(uint8 type, const uint8 *payload, uint32 payloadSize);
|
||||
|
||||
// -----------------------------------------------------------------------
|
||||
// TCP/UDP transport configuration
|
||||
// -----------------------------------------------------------------------
|
||||
uint16 controlPort;
|
||||
uint16 streamPort;
|
||||
uint16 logPort;
|
||||
StreamString streamIP;
|
||||
bool isServer;
|
||||
bool suppressTimeoutLogs;
|
||||
|
||||
BasicTCPSocket tcpServer;
|
||||
BasicUDPSocket udpSocket;
|
||||
|
||||
// -----------------------------------------------------------------------
|
||||
// Server-thread helper class
|
||||
// -----------------------------------------------------------------------
|
||||
class ServiceBinder : public EmbeddedServiceMethodBinderI {
|
||||
public:
|
||||
enum ServiceType { ServerType, StreamerType };
|
||||
ServiceBinder(DebugService *parent, ServiceType type)
|
||||
: parent(parent), type(type) {}
|
||||
virtual ErrorManagement::ErrorType Execute(ExecutionInfo &info) {
|
||||
if (type == StreamerType) return parent->Streamer(info);
|
||||
return parent->Server(info);
|
||||
}
|
||||
private:
|
||||
DebugService *parent;
|
||||
ServiceType type;
|
||||
};
|
||||
|
||||
ServiceBinder binderServer;
|
||||
ServiceBinder binderStreamer;
|
||||
SingleThreadService threadService;
|
||||
SingleThreadService streamerService;
|
||||
|
||||
ThreadIdentifier serverThreadId;
|
||||
ThreadIdentifier streamerThreadId;
|
||||
|
||||
FastPollingMutexSem clientMutex;
|
||||
BasicTCPSocket *activeClient;
|
||||
|
||||
// -----------------------------------------------------------------------
|
||||
// TCP server rate limiting and idle detection
|
||||
// -----------------------------------------------------------------------
|
||||
static const uint32 CMD_RATE_LIMIT = 100u;
|
||||
static const uint32 CLIENT_IDLE_TIMEOUT_MS = 120000u;
|
||||
static const uint32 INPUT_BUFFER_MAX = 8192u;
|
||||
|
||||
uint32 cmdCountInWindow;
|
||||
uint64 cmdWindowStartMs;
|
||||
uint64 lastDataTimeMs;
|
||||
|
||||
StreamString inputBuffer;
|
||||
|
||||
// -----------------------------------------------------------------------
|
||||
// UDPS streaming state (replaces the old custom-format streamer buffers)
|
||||
// -----------------------------------------------------------------------
|
||||
|
||||
/**
|
||||
* @brief Per-slot descriptor for DATA packet assembly.
|
||||
* One slot per currently-traced signal; index mirrors CONFIG order.
|
||||
*/
|
||||
struct UDPSSlot {
|
||||
uint32 internalID; ///< DebugSignalInfo::internalID
|
||||
uint32 wireOffset; ///< Byte offset in udpsDataPayload (after 8-byte HRT)
|
||||
uint32 wireSize; ///< Bytes occupied by this signal in the DATA payload
|
||||
bool everFilled; ///< True once at least one sample has been placed
|
||||
};
|
||||
|
||||
/** Maximum number of simultaneously traced signals. */
|
||||
static const uint32 UDPS_MAX_SLOTS = 512u;
|
||||
|
||||
/** Maximum payload per UDP datagram (signal data bytes, excluding header). */
|
||||
static const uint32 UDPS_MAX_PAYLOAD = 1400u;
|
||||
|
||||
/** Hard cap on a single signal's data: 65535 - header(17) - HRT(8). */
|
||||
static const uint32 UDPS_MAX_SAMPLE_BYTES = 65510u;
|
||||
|
||||
UDPSSlot udpsSlots[UDPS_MAX_SLOTS]; ///< Slot table (valid for [0, udpsNumSlots))
|
||||
uint32 udpsNumSlots; ///< Number of active slots
|
||||
|
||||
/** Heap-allocated DATA payload buffer: [HRT:8][sig0_data]...[sigN_data].
|
||||
* Re-allocated by SendUDPSConfig(). NULL when no signals are traced. */
|
||||
uint8 *udpsDataPayload;
|
||||
uint32 udpsDataPayloadSize;
|
||||
|
||||
/** Staging buffer: a single sample popped from traceBuffer. */
|
||||
uint8 udpsSampleBuf[65535u];
|
||||
|
||||
/** General-purpose TX buffer for CONFIG and DATA packet assembly. */
|
||||
uint8 udpsTxBuf[65535u];
|
||||
|
||||
/** Packet sequence counter (incremented per DATA/CONFIG datagram group). */
|
||||
uint32 udpsPacketCounter;
|
||||
|
||||
/**
|
||||
* @brief Set to true by TraceSignal() to request a CONFIG re-send.
|
||||
* Read and cleared by the Streamer thread.
|
||||
*/
|
||||
volatile bool udpsConfigPending;
|
||||
};
|
||||
|
||||
} // namespace MARTe
|
||||
|
||||
#endif // DEBUGSERVICE_H
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,214 @@
|
||||
#ifndef DEBUGSERVICEBASE_H
|
||||
#define DEBUGSERVICEBASE_H
|
||||
|
||||
/**
|
||||
* @file DebugServiceBase.h
|
||||
* @brief Shared base class for DebugService and WebDebugService.
|
||||
*
|
||||
* Extracts all signal-management, command-handling and config logic that is
|
||||
* common to both the TCP/UDP transport (DebugService) and the HTTP/SSE
|
||||
* transport (WebDebugService), eliminating the previous code duplication.
|
||||
*/
|
||||
|
||||
#include "ConfigurationDatabase.h"
|
||||
#include "DataSourceI.h"
|
||||
#include "DebugServiceI.h"
|
||||
#include "FastPollingMutexSem.h"
|
||||
#include "ReferenceContainer.h"
|
||||
#include "ReferenceT.h"
|
||||
#include "StreamString.h"
|
||||
|
||||
namespace MARTe {
|
||||
|
||||
/**
|
||||
* @brief Intermediate base class that holds all shared debugging logic.
|
||||
*
|
||||
* Inherits from ReferenceContainer (to be a MARTe2 Object) and DebugServiceI
|
||||
* (to expose the RT-path / control-path interface). Transport subclasses
|
||||
* (DebugService, WebDebugService) inherit from this class and add only their
|
||||
* transport-specific threading and socket logic.
|
||||
*/
|
||||
class DebugServiceBase : public ReferenceContainer, public DebugServiceI {
|
||||
public:
|
||||
friend class DebugServiceTest;
|
||||
|
||||
DebugServiceBase();
|
||||
virtual ~DebugServiceBase();
|
||||
|
||||
// =========================================================================
|
||||
// DebugServiceI RT-path overrides (shared implementation)
|
||||
// =========================================================================
|
||||
|
||||
virtual DebugSignalInfo *RegisterSignal(void *memoryAddress,
|
||||
TypeDescriptor type,
|
||||
const char8 *name,
|
||||
uint8 numberOfDimensions = 0,
|
||||
uint32 numberOfElements = 1);
|
||||
|
||||
virtual void ProcessSignal(DebugSignalInfo *signalInfo, uint32 size,
|
||||
uint64 timestamp);
|
||||
|
||||
virtual void RegisterBroker(DebugSignalInfo **signalPointers,
|
||||
uint32 numSignals,
|
||||
MemoryMapBroker *broker,
|
||||
volatile bool *anyActiveFlag,
|
||||
Vector<uint32> *activeIndices,
|
||||
Vector<uint32> *activeSizes,
|
||||
FastPollingMutexSem *activeMutex,
|
||||
volatile bool *anyBreakFlag,
|
||||
Vector<uint32> *breakIndices,
|
||||
const char8 *gamName = NULL_PTR(const char8 *),
|
||||
bool isOutput = false);
|
||||
|
||||
virtual bool IsPaused() const { return isPaused; }
|
||||
virtual void SetPaused(bool paused) { isPaused = paused; }
|
||||
virtual bool IsStepPending() const { return stepRemaining > 0u; }
|
||||
virtual void ConsumeStepIfNeeded(const char8 *gamName,
|
||||
const char8 *threadName = NULL_PTR(const char8 *));
|
||||
|
||||
// =========================================================================
|
||||
// DebugServiceI control-path overrides (shared implementation)
|
||||
// =========================================================================
|
||||
|
||||
virtual uint32 ForceSignal (const char8 *name, const char8 *valueStr);
|
||||
virtual uint32 UnforceSignal(const char8 *name);
|
||||
virtual uint32 TraceSignal (const char8 *name, bool enable, uint32 decimation = 1);
|
||||
virtual uint32 SetBreak (const char8 *name, uint8 op, float64 threshold);
|
||||
virtual uint32 ClearBreak (const char8 *name);
|
||||
virtual bool IsInstrumented(const char8 *fullPath, bool &traceable, bool &forcable);
|
||||
virtual uint32 RegisterMonitorSignal(const char8 *path, uint32 periodMs);
|
||||
virtual uint32 UnmonitorSignal (const char8 *path);
|
||||
|
||||
// =========================================================================
|
||||
// Shared control-path methods (used by transport subclasses)
|
||||
// =========================================================================
|
||||
|
||||
/**
|
||||
* @brief Parse and dispatch a debug command; write text response to @p out.
|
||||
*
|
||||
* Handles: TRACE, FORCE, UNFORCE, BREAK, PAUSE, RESUME, STEP, STEP_STATUS,
|
||||
* VALUE, DISCOVER, TREE, INFO, LS, CONFIG, MONITOR, UNMONITOR,
|
||||
* SERVICE_INFO, MSG.
|
||||
*
|
||||
* SERVICE_INFO delegates to GetServiceInfo() so each transport can fill in
|
||||
* its own port/state information.
|
||||
*/
|
||||
void HandleCommand(const StreamString &cmdIn, StreamString &out);
|
||||
|
||||
void GetStepStatus(StreamString &out);
|
||||
void GetSignalValue(const char8 *name, StreamString &out);
|
||||
void Discover (StreamString &out);
|
||||
void InfoNode (const char8 *path, StreamString &out);
|
||||
void ListNodes (const char8 *path, StreamString &out);
|
||||
void ServeConfig (StreamString &out);
|
||||
|
||||
void SetFullConfig(ConfigurationDatabase &config);
|
||||
void RebuildConfigFromRegistry();
|
||||
|
||||
// =========================================================================
|
||||
// Struct shared by both transports
|
||||
// =========================================================================
|
||||
|
||||
struct MonitoredSignal {
|
||||
ReferenceT<DataSourceI> dataSource;
|
||||
uint32 signalIdx;
|
||||
uint32 internalID;
|
||||
uint32 periodMs;
|
||||
uint64 lastPollTime;
|
||||
uint32 size;
|
||||
StreamString path;
|
||||
};
|
||||
|
||||
// =========================================================================
|
||||
// Constants
|
||||
// =========================================================================
|
||||
|
||||
static const uint32 GET_VALUE_MAX_ELEMENTS = 256u;
|
||||
|
||||
/**
|
||||
* @brief Pre-build DISCOVER and TREE response caches.
|
||||
*
|
||||
* Called automatically by SetFullConfig() once the application is fully
|
||||
* initialised. After this point, DISCOVER and TREE are served from the
|
||||
* pre-built string with no mutex contention and no JSON generation cost.
|
||||
* Both caches are invalidated whenever a new signal is registered (which
|
||||
* normally only happens during broker init, before SetFullConfig).
|
||||
*/
|
||||
void BuildDiscoverCache();
|
||||
void BuildTreeCache();
|
||||
|
||||
// Number of signals per DISCOVER_PART TCP chunk. Responses larger than
|
||||
// this are split so the Go client can start parsing immediately.
|
||||
static const uint32 DISCOVER_CHUNK_SIGNALS = 256u;
|
||||
|
||||
protected:
|
||||
// =========================================================================
|
||||
// Virtual hooks for transport subclasses
|
||||
// =========================================================================
|
||||
|
||||
/**
|
||||
* @brief Fill the SERVICE_INFO response text (transport-specific).
|
||||
*
|
||||
* Called by HandleCommand when the SERVICE_INFO command is received.
|
||||
* The base implementation writes nothing; subclasses override to append
|
||||
* e.g. "OK SERVICE_INFO TCP_CTRL:8080 UDP_STREAM:8081 STATE:RUNNING\n".
|
||||
*/
|
||||
virtual void GetServiceInfo(StreamString &out) = 0;
|
||||
|
||||
/**
|
||||
* @brief Write back transport-specific config keys after RebuildConfigFromRegistry().
|
||||
*
|
||||
* Called at the end of RebuildConfigFromRegistry() so that port numbers
|
||||
* and other transport parameters that were read in Initialise() are
|
||||
* reflected back into fullConfig (ExportData on ReferenceContainers
|
||||
* doesn't re-emit config-file parameters).
|
||||
*/
|
||||
virtual void RebuildTransportConfig() {}
|
||||
|
||||
// =========================================================================
|
||||
// Shared protected helpers
|
||||
// =========================================================================
|
||||
|
||||
void Step(uint32 n, const char8 *threadName = NULL_PTR(const char8 *));
|
||||
void UpdateBrokersActiveStatus();
|
||||
void UpdateBrokersBreakStatus();
|
||||
void PatchRegistry();
|
||||
|
||||
uint32 ExportTree(ReferenceContainer *container, StreamString &json,
|
||||
const char8 *pathPrefix);
|
||||
void ExportTreeNode(const char8 *path, StreamString &out);
|
||||
void EnrichWithConfig(const char8 *path, StreamString &json);
|
||||
static void JsonifyDatabase(ConfigurationDatabase &db, StreamString &json);
|
||||
|
||||
// =========================================================================
|
||||
// Shared data members
|
||||
// =========================================================================
|
||||
|
||||
Vector<DebugSignalInfo *> signals;
|
||||
Vector<SignalAlias> aliases;
|
||||
Vector<BrokerInfo> brokers;
|
||||
Vector<MonitoredSignal> monitoredSignals;
|
||||
|
||||
FastPollingMutexSem mutex;
|
||||
FastPollingMutexSem tracePushMutex;
|
||||
TraceRingBuffer traceBuffer;
|
||||
|
||||
volatile bool isPaused;
|
||||
volatile uint32 stepRemaining;
|
||||
StreamString pausedAtGam;
|
||||
StreamString stepThreadFilter;
|
||||
|
||||
ConfigurationDatabase fullConfig;
|
||||
bool manualConfigSet;
|
||||
|
||||
// Pre-built response caches. Guarded by mutex (brief lock for swap,
|
||||
// none needed for reads once cacheValid is true and construction is done).
|
||||
StreamString discoverCache; // full chunked DISCOVER_PART+DISCOVER payload
|
||||
StreamString treeCache; // full TREE payload including sentinel
|
||||
volatile bool discoverCacheValid;
|
||||
volatile bool treeCacheValid;
|
||||
};
|
||||
|
||||
} // namespace MARTe
|
||||
|
||||
#endif // DEBUGSERVICEBASE_H
|
||||
@@ -0,0 +1,188 @@
|
||||
#ifndef DEBUGSERVICEI_H
|
||||
#define DEBUGSERVICEI_H
|
||||
|
||||
/**
|
||||
* @file DebugServiceI.h
|
||||
* @brief Abstract interface for the MARTe2 debug/instrumentation service.
|
||||
*
|
||||
* All broker wrappers (DebugBrokerWrapper) depend solely on this interface,
|
||||
* decoupling them from the concrete TCP/UDP implementation. Alternative
|
||||
* transports — TTY, WebSocket, shared-memory ring, etc. — can be plugged in
|
||||
* by providing a new concrete implementation without touching any broker or
|
||||
* application code.
|
||||
*
|
||||
* The interface is split into two logical groups:
|
||||
*
|
||||
* RT-path API
|
||||
* Called from broker threads on every RT cycle. Implementations must
|
||||
* keep these methods as cheap as possible; the only permissible
|
||||
* synchronisation primitive is a fast spinlock (FastPollingMutexSem).
|
||||
*
|
||||
* Control-path API
|
||||
* Called from server / command-handler threads (TCP, TTY, Web …).
|
||||
* Latency here is acceptable; correctness and thread-safety matter.
|
||||
*
|
||||
* Concrete implementations register themselves during Initialise() via
|
||||
* SetInstance(). Broker wrappers retrieve the active instance via
|
||||
* GetInstance(); there is no ORD search on the hot path.
|
||||
*/
|
||||
|
||||
#include "DebugCore.h"
|
||||
#include "FastPollingMutexSem.h"
|
||||
#include "Object.h"
|
||||
#include "StreamString.h"
|
||||
#include "TypeDescriptor.h"
|
||||
|
||||
namespace MARTe {
|
||||
|
||||
// Forward declarations — concrete types are only needed in the implementation.
|
||||
class MemoryMapBroker;
|
||||
|
||||
struct SignalAlias {
|
||||
StreamString name;
|
||||
uint32 signalIndex;
|
||||
};
|
||||
|
||||
struct BrokerInfo {
|
||||
DebugSignalInfo **signalPointers;
|
||||
uint32 numSignals;
|
||||
MemoryMapBroker *broker;
|
||||
volatile bool *anyActiveFlag;
|
||||
Vector<uint32> *activeIndices;
|
||||
Vector<uint32> *activeSizes;
|
||||
FastPollingMutexSem *activeMutex;
|
||||
volatile bool *anyBreakFlag;
|
||||
Vector<uint32> *breakIndices;
|
||||
StreamString gamName;
|
||||
bool isOutput;
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Abstract debug-service interface.
|
||||
*/
|
||||
class DebugServiceI {
|
||||
public:
|
||||
// -------------------------------------------------------------------------
|
||||
// Static instance registry
|
||||
// -------------------------------------------------------------------------
|
||||
|
||||
/**
|
||||
* @brief Return the currently registered debug-service instance, or NULL.
|
||||
*
|
||||
* Called on every broker Init() path and from OutpautPauseAndStep().
|
||||
* Returns NULL when no debug service has been initialised, in which case
|
||||
* all instrumentation is a no-op.
|
||||
*/
|
||||
static DebugServiceI *GetInstance() { return instance; }
|
||||
|
||||
/**
|
||||
* @brief Register @p inst as the global debug-service.
|
||||
*
|
||||
* Concrete implementations call this from their Initialise() method.
|
||||
* Passing NULL deregisters the current instance (called from the
|
||||
* destructor so dangling pointers are never visible to broker threads).
|
||||
*/
|
||||
static void SetInstance(DebugServiceI *inst) { instance = inst; }
|
||||
|
||||
virtual ~DebugServiceI() {}
|
||||
|
||||
// =========================================================================
|
||||
// RT-path API (called from broker execute threads every cycle)
|
||||
// =========================================================================
|
||||
|
||||
/**
|
||||
* @brief Register a signal memory region with the debug service.
|
||||
*
|
||||
* Called once per signal during broker Init(). Returns a pointer to the
|
||||
* internal DebugSignalInfo that the broker caches for use on the hot path.
|
||||
* Thread-safe; must not be called after the RT loop has started.
|
||||
*/
|
||||
virtual DebugSignalInfo *RegisterSignal(void *memoryAddress,
|
||||
TypeDescriptor type,
|
||||
const char8 *name,
|
||||
uint8 numberOfDimensions = 0,
|
||||
uint32 numberOfElements = 1) = 0;
|
||||
|
||||
/**
|
||||
* @brief Process one signal on the RT path.
|
||||
*
|
||||
* Applies forced values (memcpy into signal memory) and, when tracing is
|
||||
* enabled and the decimation counter fires, pushes a sample to the trace
|
||||
* ring buffer. Called under the broker's activeMutex; implementations
|
||||
* must not acquire any lock that is also held by the Server thread.
|
||||
*/
|
||||
virtual void ProcessSignal(DebugSignalInfo *signalInfo,
|
||||
uint32 size,
|
||||
uint64 timestamp) = 0;
|
||||
|
||||
/**
|
||||
* @brief Register a broker so the service can push active/break index
|
||||
* updates to it without iterating every signal.
|
||||
*/
|
||||
virtual void RegisterBroker(DebugSignalInfo **signalPointers,
|
||||
uint32 numSignals,
|
||||
MemoryMapBroker *broker,
|
||||
volatile bool *anyActiveFlag,
|
||||
Vector<uint32> *activeIndices,
|
||||
Vector<uint32> *activeSizes,
|
||||
FastPollingMutexSem *activeMutex,
|
||||
volatile bool *anyBreakFlag,
|
||||
Vector<uint32> *breakIndices,
|
||||
const char8 *gamName = NULL_PTR(const char8 *),
|
||||
bool isOutput = false) = 0;
|
||||
|
||||
/** @brief Return true if the RT loop is currently held at a pause/breakpoint. */
|
||||
virtual bool IsPaused() const = 0;
|
||||
|
||||
/** @brief Set or clear the paused state (called by break-condition logic). */
|
||||
virtual void SetPaused(bool paused) = 0;
|
||||
|
||||
/** @brief Return true if a step count is pending (stepRemaining > 0). */
|
||||
virtual bool IsStepPending() const = 0;
|
||||
|
||||
/**
|
||||
* @brief Consume one step credit for the current output-broker cycle.
|
||||
*
|
||||
* Called by every output broker after Execute(). No-op when stepRemaining
|
||||
* is zero (the common case); only acquires a mutex when stepping is active.
|
||||
*/
|
||||
virtual void ConsumeStepIfNeeded(
|
||||
const char8 *gamName,
|
||||
const char8 *threadName = NULL_PTR(const char8 *)) = 0;
|
||||
|
||||
// =========================================================================
|
||||
// Control-path API (called from server / command-handler threads)
|
||||
// =========================================================================
|
||||
|
||||
virtual uint32 ForceSignal (const char8 *name, const char8 *valueStr) = 0;
|
||||
virtual uint32 UnforceSignal(const char8 *name) = 0;
|
||||
virtual uint32 TraceSignal (const char8 *name, bool enable, uint32 decimation = 1) = 0;
|
||||
virtual uint32 SetBreak (const char8 *name, uint8 op, float64 threshold) = 0;
|
||||
virtual uint32 ClearBreak (const char8 *name) = 0;
|
||||
virtual bool IsInstrumented(const char8 *fullPath,
|
||||
bool &traceable, bool &forcable) = 0;
|
||||
virtual uint32 RegisterMonitorSignal(const char8 *path, uint32 periodMs) = 0;
|
||||
virtual uint32 UnmonitorSignal (const char8 *path) = 0;
|
||||
|
||||
// =========================================================================
|
||||
// Utility (implementation-agnostic, defined in DebugService.cpp)
|
||||
// =========================================================================
|
||||
|
||||
/**
|
||||
* @brief Resolve the fully-qualified ORD path of @p obj into @p fullPath.
|
||||
*
|
||||
* Static so it can be called without a service instance. All concrete
|
||||
* implementations (and InitSignals in DebugBrokerWrapper.h) use this
|
||||
* to build canonical signal names. The definition lives in
|
||||
* DebugService.cpp alongside FindPathInContainer().
|
||||
*/
|
||||
static bool GetFullObjectName(const Object &obj, StreamString &fullPath);
|
||||
|
||||
protected:
|
||||
/** Pointer to the single active debug-service instance. */
|
||||
static DebugServiceI *instance;
|
||||
};
|
||||
|
||||
} // namespace MARTe
|
||||
|
||||
#endif // DEBUGSERVICEI_H
|
||||
@@ -0,0 +1 @@
|
||||
include Makefile.inc
|
||||
@@ -0,0 +1,36 @@
|
||||
#############################################################
|
||||
# MARTe2 Integrated Components — DebugService
|
||||
#############################################################
|
||||
OBJSX=DebugService.x DebugServiceBase.x
|
||||
|
||||
PACKAGE=Components/Interfaces
|
||||
|
||||
ROOT_DIR=../../../../
|
||||
MAKEDEFAULTDIR=$(MARTe2_DIR)/MakeDefaults
|
||||
include $(MAKEDEFAULTDIR)/MakeStdLibDefs.$(TARGET)
|
||||
|
||||
# Shared UDPS protocol header (from Common/)
|
||||
INCLUDES += -I$(ROOT_DIR)/Common/UDP
|
||||
INCLUDES += -I$(ROOT_DIR)/Source/Components/Interfaces/TCPLogger
|
||||
INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L0Types
|
||||
INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L1Portability
|
||||
INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L2Objects
|
||||
INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L3Streams
|
||||
INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L4Messages
|
||||
INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L4Logger
|
||||
INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L4Configuration
|
||||
INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L5GAMs
|
||||
INCLUDES += -I$(MARTe2_DIR)/Source/Core/Scheduler/L1Portability
|
||||
INCLUDES += -I$(MARTe2_DIR)/Source/Core/Scheduler/L3Services
|
||||
INCLUDES += -I$(MARTe2_DIR)/Source/Core/Scheduler/L4Messages
|
||||
INCLUDES += -I$(MARTe2_DIR)/Source/Core/Scheduler/L4LoggerService
|
||||
INCLUDES += -I$(MARTe2_DIR)/Source/Core/Scheduler/L5GAMs
|
||||
INCLUDES += -I$(MARTe2_DIR)/Source/Core/FileSystem/L1Portability
|
||||
INCLUDES += -I$(MARTe2_DIR)/Source/Core/FileSystem/L3Streams
|
||||
|
||||
all: $(OBJS) $(SUBPROJ) \
|
||||
$(BUILD_DIR)/DebugService$(LIBEXT) \
|
||||
$(BUILD_DIR)/DebugService$(DLLEXT)
|
||||
echo $(OBJS)
|
||||
|
||||
include $(MAKEDEFAULTDIR)/MakeStdLibRules.$(TARGET)
|
||||
Reference in New Issue
Block a user