#ifndef DEBUGBROKERWRAPPER_H #define DEBUGBROKERWRAPPER_H #include "BrokerI.h" #include "DataSourceI.h" #include "DebugService.h" #include "FastPollingMutexSem.h" #include "HighResolutionTimer.h" #include "MemoryMapBroker.h" #include "ObjectBuilder.h" #include "ObjectRegistryDatabase.h" #include "Threads.h" #include "Vec.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(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(s->memoryAddress); else if (t == Float32Bit) val = *static_cast(s->memoryAddress); else if (t == UnsignedInteger32Bit) val = *static_cast(s->memoryAddress); else if (t == SignedInteger32Bit) val = *static_cast(s->memoryAddress); else if (t == UnsignedInteger64Bit) val = static_cast(*static_cast(s->memoryAddress)); else if (t == SignedInteger64Bit) val = static_cast(*static_cast(s->memoryAddress)); else if (t == UnsignedInteger16Bit) val = *static_cast(s->memoryAddress); else if (t == SignedInteger16Bit) val = *static_cast(s->memoryAddress); else if (t == UnsignedInteger8Bit) val = *static_cast(s->memoryAddress); else if (t == SignedInteger8Bit) val = *static_cast(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(DebugService *service, const char8 *gamName) { if (service == NULL_PTR(DebugService *)) 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(DebugService *service, DebugSignalInfo **signalInfoPointers, Vec &activeIndices, Vec &activeSizes, FastPollingMutexSem &activeMutex, volatile bool *anyBreakFlag, Vec *breakIndices) { if (service == NULL_PTR(DebugService *)) 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.Size(); if (n > 0 && signalInfoPointers != NULL_PTR(DebugSignalInfo **)) { // Capture timestamp ONCE per broker cycle for lowest impact uint64 ts = (uint64)((float64)HighResolutionTimer::Counter() * HighResolutionTimer::Period() * 1000000.0); 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); } } } // Conditional break check — zero cost when anyBreakFlag is false // (single volatile read; the RT loop never pays for this when no break is set). if (*anyBreakFlag && !service->IsPaused() && breakIndices != NULL_PTR(Vec *)) { uint32 nb = breakIndices->Size(); for (uint32 i = 0; i < nb; i++) { uint32 idx = (*breakIndices)[i]; DebugSignalInfo *s = signalInfoPointers[idx]; if (s != NULL_PTR(DebugSignalInfo *) && s->breakOp != BREAK_OFF && EvaluateBreak(s)) { service->SetPaused(true); break; } } } activeMutex.FastUnLock(); } // Pass numCopies explicitly so we can mock it static void InitSignals(BrokerI *broker, DataSourceI &dataSourceIn, DebugService *&service, DebugSignalInfo **&signalInfoPointers, uint32 numCopies, MemoryMapBrokerCopyTableEntry *copyTable, const char8 *functionName, SignalDirection direction, volatile bool *anyActiveFlag, Vec *activeIndices, Vec *activeSizes, FastPollingMutexSem *activeMutex, volatile bool *anyBreakFlag, Vec *breakIndices) { if (numCopies > 0) { signalInfoPointers = new DebugSignalInfo *[numCopies]; for (uint32 i = 0; i < numCopies; i++) signalInfoPointers[i] = NULL_PTR(DebugSignalInfo *); } ReferenceContainer *root = ObjectRegistryDatabase::Instance(); Reference serviceRef = root->Find("DebugService"); if (serviceRef.IsValid()) { service = dynamic_cast(serviceRef.operator->()); } if (service && (copyTable != NULL_PTR(MemoryMapBrokerCopyTableEntry *))) { StreamString dsPath; DebugService::GetFullObjectName(dataSourceIn, dsPath); fprintf(stderr, ">> %s broker for %s [%d]\n", direction == InputSignals ? "Input" : "Output", dsPath.Buffer(), numCopies); MemoryMapBroker *mmb = dynamic_cast(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; DebugService::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 class DebugBrokerWrapper : public BaseClass { public: DebugBrokerWrapper() : BaseClass() { service = NULL_PTR(DebugService *); 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; } DebugService *service; DebugSignalInfo **signalInfoPointers; uint32 numSignals; volatile bool anyActive; volatile bool anyBreakActive; bool isOutput; char8 gamName[256]; Vec activeIndices; Vec activeSizes; Vec breakIndices; FastPollingMutexSem activeMutex; }; template class DebugBrokerWrapperNoOptim : public BaseClass { public: DebugBrokerWrapperNoOptim() : BaseClass() { service = NULL_PTR(DebugService *); 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; } DebugService *service; DebugSignalInfo **signalInfoPointers; uint32 numSignals; volatile bool anyActive; volatile bool anyBreakActive; bool isOutput; char8 gamName[256]; Vec activeIndices; Vec activeSizes; Vec breakIndices; FastPollingMutexSem activeMutex; }; class DebugMemoryMapAsyncOutputBroker : public MemoryMapAsyncOutputBroker { public: DebugMemoryMapAsyncOutputBroker() : MemoryMapAsyncOutputBroker() { service = NULL_PTR(DebugService *); 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; } DebugService *service; DebugSignalInfo **signalInfoPointers; uint32 numSignals; volatile bool anyActive; volatile bool anyBreakActive; char8 gamName[256]; Vec activeIndices; Vec activeSizes; Vec breakIndices; FastPollingMutexSem activeMutex; }; class DebugMemoryMapAsyncTriggerOutputBroker : public MemoryMapAsyncTriggerOutputBroker { public: DebugMemoryMapAsyncTriggerOutputBroker() : MemoryMapAsyncTriggerOutputBroker() { service = NULL_PTR(DebugService *); 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; } DebugService *service; DebugSignalInfo **signalInfoPointers; uint32 numSignals; volatile bool anyActive; volatile bool anyBreakActive; char8 gamName[256]; Vec activeIndices; Vec activeSizes; Vec breakIndices; FastPollingMutexSem activeMutex; }; template class DebugBrokerBuilder : public ObjectBuilder { public: virtual Object *Build(HeapI *const heap) const { return new (heap) T(); } }; typedef DebugBrokerWrapper DebugMemoryMapInputBroker; // LCOV_EXCL_START typedef DebugBrokerWrapper DebugMemoryMapOutputBroker; typedef DebugBrokerWrapper DebugMemoryMapSynchronisedInputBroker; typedef DebugBrokerWrapper DebugMemoryMapSynchronisedOutputBroker; typedef DebugBrokerWrapperNoOptim DebugMemoryMapInterpolatedInputBroker; typedef DebugBrokerWrapper DebugMemoryMapMultiBufferInputBroker; typedef DebugBrokerWrapper DebugMemoryMapMultiBufferOutputBroker; typedef DebugBrokerWrapper DebugMemoryMapSynchronisedMultiBufferInputBroker; typedef DebugBrokerWrapper DebugMemoryMapSynchronisedMultiBufferOutputBroker; // LCOV_EXCL_STOP typedef DebugBrokerBuilder DebugMemoryMapInputBrokerBuilder; // LCOV_EXCL_START typedef DebugBrokerBuilder DebugMemoryMapOutputBrokerBuilder; typedef DebugBrokerBuilder DebugMemoryMapSynchronisedInputBrokerBuilder; typedef DebugBrokerBuilder DebugMemoryMapSynchronisedOutputBrokerBuilder; typedef DebugBrokerBuilder DebugMemoryMapInterpolatedInputBrokerBuilder; typedef DebugBrokerBuilder DebugMemoryMapMultiBufferInputBrokerBuilder; typedef DebugBrokerBuilder DebugMemoryMapMultiBufferOutputBrokerBuilder; typedef DebugBrokerBuilder DebugMemoryMapSynchronisedMultiBufferInputBrokerBuilder; typedef DebugBrokerBuilder DebugMemoryMapSynchronisedMultiBufferOutputBrokerBuilder; typedef DebugBrokerBuilder DebugMemoryMapAsyncOutputBrokerBuilder; typedef DebugBrokerBuilder DebugMemoryMapAsyncTriggerOutputBrokerBuilder; // LCOV_EXCL_STOP } // namespace MARTe #endif