diff --git a/AGENTS.md b/AGENTS.md index 93c541e..9946075 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -10,3 +10,4 @@ - UI should be responsive. Always prefer to use pre-packaged UI toolkit widgets, containers and colour sets harmoniously, instead of writing custom CSS overrides. Write custom CSS only if there's no UI toolkit mechanism available. - Aggressively isolate, split off, deduplicate and reuse code which can be made into common library code. Do the same with UI elements. Do this both when implementing new features and opportunistically while refactoring or changing old code/UI elements. - Names of files, functions, classes, abstractions, database fields, etc should be aimed at disambiguating purpose and function, rather than at brevity. +- Any source or header file that includes a Boost header must include `` first (at the top of the file, or immediately after the include guard in headers), before all other includes, so Boost.Asio is used as a non-header-only library correctly. diff --git a/include/adapters/README.md b/include/adapters/README.md new file mode 100644 index 0000000..7685d59 --- /dev/null +++ b/include/adapters/README.md @@ -0,0 +1,40 @@ +# Adapter Awaiters + +This directory contains coroutine/awaitable adapters that wrap callback-driven +or event-driven APIs. + +## Placement rules + +- Put wrappers for external APIs in provider-specific subdirectories. + - Examples: `boostAsio/`, `opencl/`, `liburing/`. +- Put wrappers for SMO/internal APIs in `smo/`. +- Do not place adapter awaiters in feature folders like + `stimBuffApis/*` or `smocore/*` unless they are strictly private to one + translation unit. + +## Tree layout + +```text +include/adapters/ + README.md + boostAsio/ + + opencl/ + + smo/ + cpsCallbackAReq.h + livoxProto1CpsAwaiters.h + +``` + +## Design guidelines + +- Name adapter awaiter wrapper functions `getAReqAwaiter()`, where + `` is the wrapped CPS/API request symbol with its library prefix + removed and each `_`-delimited segment Pascal-cased (e.g. + `livoxProto1_getOrCreateDeviceReq` → `getGetOrCreateDeviceReqAReqAwaiter()`). +- Keep adapters small and single-purpose; but unify where possible to reduce + code duplication. +- Make result types explicit for multi-argument callbacks. +- Resume coroutines on a caller-specified executor/io_service. +- Avoid embedding business logic in adapters. diff --git a/include/adapters/boostAsio/deadlineTimerAReq.h b/include/adapters/boostAsio/deadlineTimerAReq.h new file mode 100644 index 0000000..de01084 --- /dev/null +++ b/include/adapters/boostAsio/deadlineTimerAReq.h @@ -0,0 +1,34 @@ +#ifndef ADAPTERS_BOOST_ASIO_DEADLINE_TIMER_AREQ_H +#define ADAPTERS_BOOST_ASIO_DEADLINE_TIMER_AREQ_H + +#include +#include +#include +#include +#include + +namespace adapters::boostAsio { + +using TimerWaitCbFn = std::function; + +inline auto deadlineTimerWaitAReq( + boost::asio::io_service &ioService, + const boost::posix_time::milliseconds delay) +{ + return smo::cpsBoundary::CpsCallbackAReq)>>( + ioService, + [&ioService, delay](sscl::cps::Callback cb) + { + auto timer = std::make_shared(ioService); + timer->expires_from_now(delay); + timer->async_wait( + [timer, cb](const boost::system::error_code &error) mutable + { + cb.callbackFn(!error); + }); + }); +} + +} // namespace adapters::boostAsio + +#endif // ADAPTERS_BOOST_ASIO_DEADLINE_TIMER_AREQ_H diff --git a/smocore/include/cpsBoundary/cpsCallbackAReq.h b/include/adapters/smo/cpsCallbackAReq.h similarity index 89% rename from smocore/include/cpsBoundary/cpsCallbackAReq.h rename to include/adapters/smo/cpsCallbackAReq.h index 0c16b95..0db47bb 100644 --- a/smocore/include/cpsBoundary/cpsCallbackAReq.h +++ b/include/adapters/smo/cpsCallbackAReq.h @@ -1,5 +1,7 @@ -#ifndef CPS_CALLBACK_AREQ_H -#define CPS_CALLBACK_AREQ_H +#ifndef ADAPTERS_SMO_CPS_CALLBACK_AREQ_H +#define ADAPTERS_SMO_CPS_CALLBACK_AREQ_H + +#include #include #include @@ -13,7 +15,7 @@ namespace smo { namespace cpsBoundary { -/** Eager-start CPS callback → coroutine adapter (mirrors +/** Eager-start CPS callback -> coroutine adapter (mirrors * PuppetThread::ViralThreadLifetimeMgmtInvoker). */ template @@ -90,4 +92,4 @@ private: } // namespace cpsBoundary } // namespace smo -#endif // CPS_CALLBACK_AREQ_H +#endif // ADAPTERS_SMO_CPS_CALLBACK_AREQ_H diff --git a/include/adapters/smo/livoxProto1CpsAwaiters.h b/include/adapters/smo/livoxProto1CpsAwaiters.h new file mode 100644 index 0000000..5b1030b --- /dev/null +++ b/include/adapters/smo/livoxProto1CpsAwaiters.h @@ -0,0 +1,121 @@ +#ifndef ADAPTERS_SMO_LIVOX_PROTO1_CPS_AWAITERS_H +#define ADAPTERS_SMO_LIVOX_PROTO1_CPS_AWAITERS_H + +#include +#include + +#include +#include +#include + +namespace adapters::smo { + +struct GetOrCreateDeviceResult +{ + bool success = false; + std::shared_ptr device; +}; + +struct GetReturnModeResult +{ + bool success = false; + uint8_t returnMode = 0; +}; + +inline auto getGetOrCreateDeviceReqAReqAwaiter( + boost::asio::io_service &resumeIoService, + livoxProto1_getOrCreateDeviceReqFn *fn, + const std::string &deviceIdentifier, + const std::shared_ptr &componentThread, + int commandTimeoutMs, + int retryDelayMs, + const std::string &smoIp, + uint8_t smoSubnetNbits, + uint16_t dataPort, + uint16_t cmdPort, + uint16_t imuPort) +{ + return ::smo::cpsBoundary::CpsCallbackAReq< + GetOrCreateDeviceResult, + livoxProto1_getOrCreateDeviceReqCbFn, + std::function)>>( + resumeIoService, + [=](sscl::cps::Callback cb) + { + (*fn)( + deviceIdentifier, + componentThread, + commandTimeoutMs, retryDelayMs, + smoIp, smoSubnetNbits, + dataPort, cmdPort, imuPort, + std::move(cb)); + }); +} + +inline auto getDeviceGetReturnModeReqAReqAwaiter( + boost::asio::io_service &resumeIoService, + livoxProto1_device_getReturnModeReqFn *fn, + std::shared_ptr device) +{ + return ::smo::cpsBoundary::CpsCallbackAReq< + GetReturnModeResult, + livoxProto1_device_getReturnModeReqCbFn, + std::function)>>( + resumeIoService, + [=](sscl::cps::Callback cb) + { + (*fn)(device, std::move(cb)); + }); +} + +inline auto getDeviceEnablePcloudDataReqAReqAwaiter( + boost::asio::io_service &resumeIoService, + livoxProto1_device_enablePcloudDataReqFn *fn, + std::shared_ptr device) +{ + return ::smo::cpsBoundary::CpsCallbackAReq< + bool, + livoxProto1_device_enablePcloudDataReqCbFn, + std::function)>>( + resumeIoService, + [=](sscl::cps::Callback cb) + { + (*fn)(device, std::move(cb)); + }); +} + +inline auto getDeviceDisablePcloudDataReqAReqAwaiter( + boost::asio::io_service &resumeIoService, + livoxProto1_device_disablePcloudDataReqFn *fn, + std::shared_ptr device) +{ + return ::smo::cpsBoundary::CpsCallbackAReq< + bool, + livoxProto1_device_disablePcloudDataReqCbFn, + std::function)>>( + resumeIoService, + [=](sscl::cps::Callback cb) + { + (*fn)(device, std::move(cb)); + }); +} + +inline auto getDestroyDeviceReqAReqAwaiter( + boost::asio::io_service &resumeIoService, + livoxProto1_destroyDeviceReqFn *fn, + std::shared_ptr device) +{ + return ::smo::cpsBoundary::CpsCallbackAReq< + bool, + livoxProto1_destroyDeviceReqCbFn, + std::function)>>( + resumeIoService, + [=](sscl::cps::Callback cb) + { + (*fn)(device, std::move(cb)); + }); +} + +} // namespace adapters::smo + +#endif // ADAPTERS_SMO_LIVOX_PROTO1_CPS_AWAITERS_H diff --git a/stimBuffApis/livoxGen1/CMakeLists.txt b/stimBuffApis/livoxGen1/CMakeLists.txt index a2819cb..70200e3 100644 --- a/stimBuffApis/livoxGen1/CMakeLists.txt +++ b/stimBuffApis/livoxGen1/CMakeLists.txt @@ -14,6 +14,7 @@ if(ENABLE_STIMBUFFAPI_livoxGen1) add_library(livoxGen1 SHARED livoxGen1.cpp + livoxGen1Proto1CpsBridge.cpp pcloudStimulusProducer.cpp livoxPcloudFrameDumper.cpp ioUringAssemblyEngine.cpp @@ -36,6 +37,8 @@ if(ENABLE_STIMBUFFAPI_livoxGen1) target_include_directories(livoxGen1 PUBLIC ${Boost_INCLUDE_DIRS} + ${CMAKE_SOURCE_DIR}/include + ${CMAKE_SOURCE_DIR}/smocore/include ${CMAKE_SOURCE_DIR}/commonLibs ${URING_INCLUDE_DIRS} ${OPENCL_INCLUDE_DIRS} @@ -46,6 +49,7 @@ if(ENABLE_STIMBUFFAPI_livoxGen1) ${URING_LIBRARIES} ${OPENCL_LIBRARIES} attachmentSupport + spinscale ) target_link_directories(livoxGen1 PUBLIC ${URING_LIBRARY_DIRS} diff --git a/stimBuffApis/livoxGen1/livoxGen1.cpp b/stimBuffApis/livoxGen1/livoxGen1.cpp index 425cddb..45cee8a 100644 --- a/stimBuffApis/livoxGen1/livoxGen1.cpp +++ b/stimBuffApis/livoxGen1/livoxGen1.cpp @@ -1,72 +1,29 @@ #include -#include -#include -#include -#include -#include -#include #include #include +#include #include -#include -#include -#include -#include -#include -#include +#include + +#include #include -#include -#include -#include "pcloudStimulusProducer.h" -#include "livoxGen1.h" +#include +#include +#include "livoxGen1Internal.h" +#include "livoxGen1Proto1CpsBridge.h" -namespace smo { -namespace stim_buff { +namespace smo::stim_buff { + +constexpr int LIVOX_GEN1_DEVICE_COMMAND_DELAY_MS = 5; // Salmanoff hooks, obtained from SMO_GET_STIM_BUFF_API_DESC_FN_NAME(). -const SmoCallbacks* smoHooksPtr = nullptr; -static SmoThreadingModelDesc smoThreadingModelDesc; +const SmoCallbacks *smoHooksPtr = nullptr; +SmoThreadingModelDesc smoThreadingModelDesc; // Local collection of stimulus producers -static std::vector> attachedStimulusProducers; +std::vector> attachedStimulusProducers; -// Get stimulus producer by device attachment spec -static std::shared_ptr -getStimulusProducer( - const std::shared_ptr& spec - ) -{ - for (const auto& stimProducer : attachedStimulusProducers) - { - // Compare device selectors to find matching buffer - if (livoxProto1::comms::deviceIdentifiersEqual( - stimProducer->deviceAttachmentSpec->deviceSelector, - spec->deviceSelector) - && stimProducer->exportsQualeIfaceApi(spec->qualeIfaceApi)) - { - return stimProducer; - } - } - - return nullptr; -} - -// Helper function to parse n-dgrams-per-frame from stim-buff-api params -static size_t parseNDgramsPerFrame( - const std::shared_ptr& spec) -{ - const std::vector nDgramsPerFrameParamNames = { - "n-dgrams-per-frame", - "num-dgrams-per-frame" - }; - - return static_cast( - smo::device::DeviceAttachmentSpec::parseOptionalParamAsIntWithSynonyms( - spec->stimBuffApiParams, nDgramsPerFrameParamNames, 84)); -} - -// LivoxProto1DllState constructor implementation LivoxProto1DllState::LivoxProto1DllState() : dlopenHandle(nullptr, DlCloser), livoxProto1_main(nullptr), @@ -79,8 +36,7 @@ LivoxProto1DllState::LivoxProto1DllState() livoxProto1_getPcloudDataFdDesc(nullptr) {} -// LivoxProto1DllState DlCloser implementation -void LivoxProto1DllState::DlCloser(void* handle) +void LivoxProto1DllState::DlCloser(void *handle) { if (handle) { dlclose(handle); @@ -89,465 +45,584 @@ void LivoxProto1DllState::DlCloser(void* handle) LivoxProto1DllState livoxProto1; -// Continuation classes for async operations -class AttachDeviceReq -: public sscl::cps::NonPostedAsynchronousContinuation +std::shared_ptr getStimulusProducer( + const std::shared_ptr &spec) { -public: - AttachDeviceReq( - const std::shared_ptr& spec, - sscl::cps::Callback cb) - : sscl::cps::NonPostedAsynchronousContinuation( - std::move(cb)), - spec(spec) - {} - -public: - const std::shared_ptr spec; - std::shared_ptr stimProducer; - std::shared_ptr deviceTmp; - -private: - std::unique_ptr delayTimer; - - // Helper method to ensure StimBuffer is attached - // Returns true if successful, false on error - bool ensureStimBufferAttached(std::shared_ptr context) + for (const auto &stimProducer : attachedStimulusProducers) { - if (!context->stimProducer) + if (livoxProto1::comms::deviceIdentifiersEqual( + stimProducer->deviceAttachmentSpec->deviceSelector, + spec->deviceSelector) + && stimProducer->exportsQualeIfaceApi(spec->qualeIfaceApi)) { - std::cerr << __func__ << ": stimProducer is null" << std::endl; - return false; + return stimProducer; } - - // Check for duplicate qualeIfaceApi - const std::string& qualeIfaceApi = context->spec->qualeIfaceApi; - if (context->stimProducer->hasBufferWithQualeIfaceApi(qualeIfaceApi)) - { - std::cerr << __func__ << ": Buffer with qualeIfaceApi '" - << qualeIfaceApi << "' already exists for this producer. " - "Each producer can only have one buffer per qualeIfaceApi." - << std::endl; - return false; - } - - // Call getOrCreateAttachedStimulusBuffer (may throw, catch and return failure) - try { - context->stimProducer->getOrCreateAttachedStimulusBuffer( - context->spec); - } catch (const std::exception& e) { - std::cerr << __func__ << ": Failed to create StimBuffer: " - << e.what() << ". Producer is committed, DeviceReattacher will retry." - << std::endl; - // Return false so caller can handle error callback - return false; - } - - return true; } -public: - void attachDeviceReq1( - std::shared_ptr context, - bool success, std::shared_ptr dev) - { - if (!success || !dev) - { - std::cerr << __func__ << ": Failed to create/find Livox device: " - << context->spec->deviceSelector << std::endl; - context->callOriginalCb(false, context->spec); - return; - } - - // Stash device pointer until after getReturnMode succeeds - context->deviceTmp = dev; - - if (1 || smoHooksPtr->OptionParser_getOptions().verbose) - { - std::cout << __func__ << ": Successfully attached/found Livox " - "device: " << context->spec->deviceSelector << " (ID: " - << context->spec->deviceIdentifier << ")\n"; - } - - /* Delay here because getOrCreate just sent HandshakeReq, so device - * may not yet be ready for another command. - */ - context->delayedGetReturnMode(context); - } - - void delayedGetReturnMode( - std::shared_ptr context) - { - // Initialize timer with LivoxGen1 metadata io_service - delayTimer = std::make_unique( - smoThreadingModelDesc.componentThread->getIoService()); - - delayTimer->expires_from_now(boost::posix_time::milliseconds(5)); - delayTimer->async_wait( - std::bind( - &AttachDeviceReq::attachDeviceReq2, - context.get(), context, - std::placeholders::_1)); - } - - void attachDeviceReq2( - std::shared_ptr context, - const boost::system::error_code& error) - { - if (error) - { - std::cerr << __func__ << ": Timer error: " << error.message() - << std::endl; - context->callOriginalCb(false, context->spec); - return; - } - - (*livoxProto1.livoxProto1_device_getReturnModeReq)( - context->deviceTmp, - {context, std::bind( - &AttachDeviceReq::attachDeviceReq3_doCreateStimProducer, - context.get(), context, - std::placeholders::_1, std::placeholders::_2)}); - } - - void attachDeviceReq3_doCreateStimProducer( - std::shared_ptr context, - bool success, uint8_t mode) - { - if (!success) - { - std::cerr << __func__ << ": Failed to get return mode for dev " - << context->spec->deviceSelector << std::endl; - - context->callOriginalCb(false, context->spec); - return; - } - - if (1 || smoHooksPtr->OptionParser_getOptions().verbose) - { - std::cout << __func__ << ": Got return mode (" << (int)mode - << ") for device: " << context->spec->deviceSelector - << std::endl; - } - - /* Check if PcloudStimulusProducer already exists - * (race condition or double-add) - */ - auto existingProducer = getStimulusProducer(context->spec); - if (existingProducer) + return nullptr; +} + +size_t parseNDgramsPerFrame( + const std::shared_ptr &spec) +{ + const std::vector nDgramsPerFrameParamNames = { + "n-dgrams-per-frame", + "num-dgrams-per-frame" + }; + + return static_cast( + device::DeviceAttachmentSpec::parseOptionalParamAsIntWithSynonyms( + spec->stimBuffApiParams, + nDgramsPerFrameParamNames, + 84)); +} + +LivoxProviderParams parseLivoxProviderParams( + const std::shared_ptr &desc) +{ + LivoxProviderParams params; + + // Parse optional integer parameters from provider params + for (const auto &providerParam : desc->providerParams) { + if (providerParam.first == "cmd-timeout-ms" + || providerParam.first == "command-timeout-ms") { + params.commandTimeoutMs = device::DeviceAttachmentSpec + ::parseRequiredParamAsInt( + desc->providerParams, + providerParam.first); + } else if (providerParam.first == "retry-delay-ms") { + params.retryDelayMs = device::DeviceAttachmentSpec + ::parseRequiredParamAsInt( + desc->providerParams, + "retry-delay-ms"); + } else if (providerParam.first == "smo-subnet-nbits") { + params.smoSubnetNbits = static_cast( + device::DeviceAttachmentSpec::parseRequiredParamAsInt( + desc->providerParams, + "smo-subnet-nbits")); + } else if (providerParam.first == "data-port") { + params.dataPort = static_cast( + device::DeviceAttachmentSpec::parseRequiredParamAsInt( + desc->providerParams, + "data-port")); + } else if (providerParam.first == "cmd-port") { + params.cmdPort = static_cast( + device::DeviceAttachmentSpec::parseRequiredParamAsInt( + desc->providerParams, + "cmd-port")); + } else if (providerParam.first == "imu-port") { + params.imuPort = static_cast( + device::DeviceAttachmentSpec::parseRequiredParamAsInt( + desc->providerParams, + "imu-port")); + } else if (providerParam.first == "smo-ip") { + if (providerParam.second.empty()) { + throw std::runtime_error( + std::string(__func__) + ": smo-ip parameter is empty"); + } + if (providerParam.second.find('.') == std::string::npos + || std::count( + providerParam.second.begin(), + providerParam.second.end(), + '.') != 3) + { + throw std::runtime_error( + std::string(__func__) + + ": smo-ip parameter is not an IPv4 address"); + } + params.smoIp = providerParam.second; + } else { throw std::runtime_error( - std::string(__func__) + ": PcloudStimulusProducer already " - "exists for device " + context->spec->deviceSelector + " " - "(race condition or double-add)"); + std::string(__func__) + ": Unknown provider parameter: " + + providerParam.first); + } + } + + return params; +} + +// Helper method to ensure StimBuffer is attached +// Returns true if successful, false on error +bool ensureStimBufferAttachedWithoutDuplicates( + const std::shared_ptr &stimProducer, + const std::shared_ptr &spec) +{ + if (!stimProducer) { + std::cerr << __func__ << ": stimProducer is null" << std::endl; + return false; + } + + // Check for duplicate qualeIfaceApi + const std::string &qualeIfaceApi = spec->qualeIfaceApi; + if (stimProducer->hasBufferWithQualeIfaceApi(qualeIfaceApi)) { + std::cerr << __func__ << ": Buffer with qualeIfaceApi '" + << qualeIfaceApi << "' already exists for this producer. " + "Each producer can only have one buffer per qualeIfaceApi." + << std::endl; + return false; + } + + // Call getOrCreateAttachedStimulusBuffer (may throw, catch and return failure) + try { + stimProducer->getOrCreateAttachedStimulusBuffer(spec); + } catch (const std::exception &e) { + std::cerr << __func__ << ": Failed to create StimBuffer: " + << e.what() << ". Producer is committed, DeviceReattacher will retry." + << std::endl; + // Return false so caller can handle error callback + return false; + } + + return true; +} + +namespace { + +constexpr size_t MAX_STIM_PRODUCERS_PER_DEVICE = 2; + +bool validateAttachRequest( + const std::shared_ptr &desc) +{ + // Validate qualeIfaceApi + const std::string &qualeIfaceApi = desc->qualeIfaceApi; + if (qualeIfaceApi == "gyro" || qualeIfaceApi == "accel") + { + // These are for ImuStimulusProducer (not yet implemented) + std::cerr << __func__ << ": qualeIfaceApi '" << qualeIfaceApi + << "' requires ImuStimulusProducer which is not yet implemented" + << std::endl; + return false; + } + + try { + smo::intrin::validateNoIntrinParamsOnQualeIface( + desc->qualeIfaceApi, + desc->qualeIfaceApiParams); + } catch (const std::runtime_error &e) { + std::cerr << __func__ << ": " << e.what() << std::endl; + return false; + } + + if (!PcloudStimulusProducer::supportsQualeIfaceApi(qualeIfaceApi)) + { + // Unknown qualeIfaceApi + std::cerr << __func__ << ": Unsupported qualeIfaceApi '" + << qualeIfaceApi << "' for LivoxGen1. " + "Supported values: mesh, pcloudIntensity, " + "pcloudLightAmbience, pcloudDarkAmbience" + << std::endl; + return false; + } + + return true; +} + +sscl::co::ViralNonPostingInvoker +enablePcloudDataForAttach( + const std::shared_ptr &desc, + const std::shared_ptr &componentThread, + const std::shared_ptr &device) +{ + /* Enable pcloud data. Don't need delay since no commands were + * sent to device prior to us reaching here (or delay already handled). + */ + const bool enabled = co_await coAwaitEnablePcloudData( + componentThread, device); + + if (!enabled) + { + std::cerr << __func__ << ": Failed to enable pcloud data for dev " + << desc->deviceSelector << std::endl; + + co_return StimBuffDeviceOpResult{false, desc}; + } + + if (smoHooksPtr->OptionParser_getOptions().verbose) + { + std::cout << __func__ << ": Enabled pcloud data for device: " + << desc->deviceSelector << std::endl; + } + + co_return StimBuffDeviceOpResult{true, desc}; +} + +sscl::co::ViralNonPostingInvoker +attachBufferAndEnablePcloud( + const std::shared_ptr &desc, + const std::shared_ptr &componentThread, + const std::shared_ptr &stimProducer) +{ + // Ensure StimBuffer is attached + if (!ensureStimBufferAttachedWithoutDuplicates(stimProducer, desc)) { + co_return StimBuffDeviceOpResult{false, desc}; + } + + // Continue to enable pcloud data if needed + co_return co_await enablePcloudDataForAttach( + desc, componentThread, stimProducer->device); +} + +sscl::co::ViralNonPostingInvoker +attachToExistingProducer( + const std::shared_ptr &desc, + const std::shared_ptr &componentThread, + const std::shared_ptr &stimProducer) +{ + // Case 1: Check if StimBuffer already exists + auto existingBuffer = stimProducer->getAttachedStimulusBuffer(desc); + if (existingBuffer) + { + // StimBuffer exists, check if pcloud data is active + if (stimProducer->device && stimProducer->device->pcloudDataActive) { + // Both StimBuffer and pcloud data are active, early return with success + co_return StimBuffDeviceOpResult{true, desc}; } - // Create & add PcloudStimulusProducer to collection since dev now ready - PcloudStimulusProducer::PcloudFormatDesc formatDesc; - formatDesc.format = PcloudStimulusProducer::PcloudFormatDesc::Format - ::XYZI; + // StimBuffer exists but pcloud data is not active, enable it + co_return co_await enablePcloudDataForAttach( + desc, componentThread, + stimProducer->device); + } - // Parse n-dgrams-per-frame from stim-buff-api params (default: 84) - size_t nDgramsPerFrame = parseNDgramsPerFrame(context->spec); + /** EXPLANATION: + * StimProducer exists, StimBuffer doesn't (DASpec doesn't match) + * Check if producer already has a buffer with the requested + * qualeIfaceApi but different DASpec - this is not allowed. + */ + if (stimProducer->hasBufferWithQualeIfaceApi(desc->qualeIfaceApi)) + { + std::cerr << __func__ << ": Producer already has a buffer with " + "qualeIfaceApi '" << desc->qualeIfaceApi + << "' but with a different DeviceAttachmentSpec. " + "A single LivoxGen1 device cannot support multiple DASpecs " + "with the same qualeIfaceApi." + << std::endl; - auto pcloudDataProducer = std::make_shared( - context->spec, context->deviceTmp, formatDesc, nDgramsPerFrame); + co_return StimBuffDeviceOpResult{false, desc}; + } - context->stimProducer = pcloudDataProducer; - context->deviceTmp->nAttachedStimulusProducers++; - if (context->deviceTmp->nAttachedStimulusProducers > 2) - { - throw std::runtime_error( - std::string(__func__) + ": Each LivoxGen1 device can only have " - "at most two StimulusProducers attached to it. Found " - + std::to_string( - context->deviceTmp->nAttachedStimulusProducers) + "."); - } + // Ensure StimBuffer is attached and enable pcloud data if needed + co_return co_await attachBufferAndEnablePcloud( + desc, componentThread, stimProducer); +} - attachedStimulusProducers.push_back(pcloudDataProducer); - if (false - /*attachedStimulusProducers.size() >= 2*nDevicesKnownToGen1Lib */) - { - /** TODO: - * It would be nice to add an nDevicesKnownToGen1Lib counter, and - * then add a check here to ensure that - * attachedStimulusProducers.size() is always less than or equal to - * 2*nDevicesKnownToGen1Lib. - * - * (2 stim producers per device). - */ +sscl::co::ViralNonPostingInvoker +attachByCreatingProducer( + const std::shared_ptr &desc, + const std::shared_ptr &componentThread) +{ + // StimProducer doesn't exist - need to create device first + + // Parse integer parameters from provider params with defaults + /** EXPLANATION: + * We may want to add a new param here called "command-delay-ms" to control + * the delay we insert between commands sent to the device. 5ms has been + * shown to be sufficient for the Livox Avia. + */ + + /* The Livox Avia will generally respond to a handshake request within + * 5ms. + */ + /* Based on testing on a Livox Avia, the device will generally resume + * sending broadcast advertisement dgrams after about 5 seconds at most. + * Generally, it will resume sending them within 1-2 seconds. + */ + const LivoxProviderParams params = parseLivoxProviderParams(desc); + adapters::smo::GetOrCreateDeviceResult deviceResult = + co_await coAwaitGetOrCreateDevice( + componentThread, desc->deviceSelector, + params); + + if (!deviceResult.success || !deviceResult.device) + { + std::cerr << __func__ << ": Failed to create/find Livox device: " + << desc->deviceSelector << std::endl; + + co_return StimBuffDeviceOpResult{false, desc}; + } + + // Stash device pointer until after getReturnMode succeeds + if (smoHooksPtr->OptionParser_getOptions().verbose) + { + std::cout << __func__ << ": Successfully attached/found Livox " + "device: " << desc->deviceSelector << " (ID: " + << desc->deviceIdentifier << ")\n"; + } + + /* Delay here because getOrCreate just sent HandshakeReq, so device + * may not yet be ready for another command. + */ + // Initialize timer with LivoxGen1 metadata io_service + const bool delayOk = co_await adapters::boostAsio::deadlineTimerWaitAReq( + componentThread->getIoService(), + boost::posix_time::milliseconds(LIVOX_GEN1_DEVICE_COMMAND_DELAY_MS)); + + if (!delayOk) + { + std::cerr << __func__ << ": Delay timer failed before getReturnMode\n"; + co_return StimBuffDeviceOpResult{false, desc}; + } + + auto returnModeResult = co_await coAwaitGetReturnMode( + componentThread, deviceResult.device); + + if (!returnModeResult.success) + { + std::cerr << __func__ << ": Failed to get return mode for dev " + << desc->deviceSelector << std::endl; + co_return StimBuffDeviceOpResult{false, desc}; + } + + if (smoHooksPtr->OptionParser_getOptions().verbose) + { + std::cout << __func__ << ": Got return mode (" + << static_cast(returnModeResult.returnMode) + << ") for device: " << desc->deviceSelector << std::endl; + } + + /* Check if PcloudStimulusProducer already exists + * (race condition or double-add) + */ + auto existingProducer = getStimulusProducer(desc); + if (existingProducer) + { + throw std::runtime_error( + std::string(__func__) + ": PcloudStimulusProducer already " + "exists for device " + desc->deviceSelector + " " + "(race condition or double-add)"); + } + + // Create & add PcloudStimulusProducer to collection since dev now ready + PcloudStimulusProducer::PcloudFormatDesc formatDesc; + formatDesc.format = PcloudStimulusProducer::PcloudFormatDesc::Format::XYZI; + + // Parse n-dgrams-per-frame from stim-buff-api params (default: 84) + const size_t nDgramsPerFrame = parseNDgramsPerFrame(desc); + + auto pcloudDataProducer = std::make_shared( + desc, deviceResult.device, + formatDesc, nDgramsPerFrame); + + deviceResult.device->nAttachedStimulusProducers++; + if (deviceResult.device->nAttachedStimulusProducers + > MAX_STIM_PRODUCERS_PER_DEVICE) + { + throw std::runtime_error( + std::string(__func__) + ": Each LivoxGen1 device can only have " + "at most two StimulusProducers attached to it. Found " + + std::to_string(deviceResult.device->nAttachedStimulusProducers) + + "."); + } + + attachedStimulusProducers.push_back(pcloudDataProducer); + if (false + /*attachedStimulusProducers.size() >= 2*nDevicesKnownToGen1Lib */) + { + /** TODO: + * It would be nice to add an nDevicesKnownToGen1Lib counter, and + * then add a check here to ensure that + * attachedStimulusProducers.size() is always less than or equal to + * 2*nDevicesKnownToGen1Lib. + * + * (2 stim producers per device). + */ #if 0 - throw std::runtime_error( - std::string(__func__) + ": Number of StimulusProducers attached " - "to LivoxGen1 devices known to the library (" - + std::to_string(attachedStimulusProducers.size()) - + ") is greater than " - "expected. Lib knows about " - + std::to_string(nDevicesKnownToGen1Lib) + " devices, " - "so there should be at most " - + std::to_string(2*nDevicesKnownToGen1Lib) - + " StimulusProducers attached to devices."); + throw std::runtime_error( + std::string(__func__) + ": Number of StimulusProducers attached " + "to LivoxGen1 devices known to the library (" + + std::to_string(attachedStimulusProducers.size()) + + ") is greater than " + "expected. Lib knows about " + + std::to_string(nDevicesKnownToGen1Lib) + " devices, " + "so there should be at most " + + std::to_string(2*nDevicesKnownToGen1Lib) + + " StimulusProducers attached to devices."); #endif - } - - pcloudDataProducer->start(); - - // Ensure StimBuffer is attached - attachDeviceReq4_doCreateStimBuff_maybeDirectlyCalled(context); } + pcloudDataProducer->start(); + // Ensure StimBuffer is attached - void attachDeviceReq4_doCreateStimBuff_maybeDirectlyCalled( - std::shared_ptr context - ) - { - // Ensure StimBuffer is attached - if (!ensureStimBufferAttached(context)) - { - context->callOriginalCb(false, context->spec); - return; - } + co_return co_await attachBufferAndEnablePcloud( + desc, componentThread, pcloudDataProducer); +} - // Continue to enable pcloud data if needed - attachDeviceReq5_doEnablePcloudData_maybeDirectlyCalled(context); - } +} // namespace - // Enable pcloud data if needed - void attachDeviceReq5_doEnablePcloudData_maybeDirectlyCalled( - std::shared_ptr context - ) - { - if (!context->stimProducer || !context->stimProducer->device) - { - std::cerr << __func__ << ": stimProducer or device is null" - << std::endl; - context->callOriginalCb(false, context->spec); - return; - } - - /* Enable pcloud data. Don't need delay since no commands were - * sent to device prior to us reaching here (or delay already handled). - */ - (*livoxProto1.livoxProto1_device_enablePcloudDataReq)( - context->stimProducer->device, - {context, std::bind( - &AttachDeviceReq::attachDeviceReq6, - context.get(), context, - std::placeholders::_1)}); - } - - void attachDeviceReq6( - std::shared_ptr context, - bool success) - { - if (!success) - { - std::cerr << __func__ << ": Failed to enable pcloud data for dev " - << context->spec->deviceSelector << std::endl; - - context->callOriginalCb(false, context->spec); - return; - } - - if (1 || smoHooksPtr->OptionParser_getOptions().verbose) - { - std::cout << __func__ << ": Enabled pcloud data for device: " - << context->spec->deviceSelector << std::endl; - } - - context->callOriginalCb(success, context->spec); - } -}; - -class DetachDeviceReq -: public sscl::cps::NonPostedAsynchronousContinuation +sscl::co::ViralNonPostingInvoker +livoxGen1_attachDeviceCReq( + const std::shared_ptr &desc, + const std::shared_ptr &componentThread) { -public: - DetachDeviceReq( - const std::shared_ptr& spec, - const std::shared_ptr& stimBuffer, - sscl::cps::Callback cb) - : sscl::cps::NonPostedAsynchronousContinuation( - std::move(cb)), - spec(spec), stimBuffer(stimBuffer) - {} - -public: - const std::shared_ptr spec; - std::shared_ptr stimBuffer; -private: - std::unique_ptr delayTimer; - -public: - void detachDeviceReq1( - std::shared_ptr context, - bool success) + if (!livoxProto1.livoxProto1_getOrCreateDeviceReq) { - if (!success) - { - std::cerr << __func__ << ": Failed to disable pcloud data for " - "stim producer " << context->spec->deviceSelector << std::endl; - // Fallthrough. - } - - // Add 5ms delay before destroying device - context->delayedDestroyDevice(context); + throw std::runtime_error( + std::string(__func__) + ": LivoxProto1 getOrCreateDevice function " + "not available"); } + if (!validateAttachRequest(desc)) { + co_return StimBuffDeviceOpResult{false, desc}; + } + + auto stimProducer = std::dynamic_pointer_cast( + getStimulusProducer(desc)); + + if (stimProducer) + { + co_return co_await attachToExistingProducer( + desc, + componentThread, + stimProducer); + } + + co_return co_await attachByCreatingProducer(desc, componentThread); +} + +sscl::co::ViralNonPostingInvoker +livoxGen1_detachDeviceCReq( + const std::shared_ptr &desc) +{ + // Case 1: Check if StimBuffer doesn't exist (early return) + auto stimProducerBase = getStimulusProducer(desc); + if (!stimProducerBase) { + // StimProducer doesn't exist, nothing to detach - success + co_return StimBuffDeviceOpResult{true, desc}; + } + + auto stimProducer = std::dynamic_pointer_cast( + stimProducerBase); + + if (!stimProducer) + { + throw std::runtime_error(std::string(__func__) + + ": Failed to cast StimulusProducer to PcloudStimulusProducer " + "for device " + desc->deviceSelector); + } + + // Check if StimBuffer exists + auto stimBuffer = stimProducer->getAttachedStimulusBuffer(desc); + if (!stimBuffer) { + // StimBuffer doesn't exist, nothing to detach - success + co_return StimBuffDeviceOpResult{true, desc}; + } + + // Case 2: StimBuffer exists - proceed with detach + auto requestComponentThread = stimProducer->device->componentThread; + + /** FIXME: + * We always disable Livox pcloud sampling here, even when other + * StimBuffers on this PcloudStimulusProducer will remain attached. + * That leaves surviving qualae without a device stream until something + * re-enables pcloud (e.g. a later attach). PcloudStimulusProducer:: + * destroyAttachedStimulusBuffer() stop()/start() only pauses the SMO + * io_uring/OpenCL frame pipeline for buffer-list mutation; it does not + * turn Livox sampling back on. Only call disable when detaching the + * last buffer, or re-enable after a partial detach when buffers remain. + */ + // Disable point cloud data first + const bool disabled = co_await coAwaitDisablePcloudData( + requestComponentThread, stimProducer->device); + + if (!disabled) + { + std::cerr << __func__ << ": Failed to disable pcloud data for " + "stim producer " << desc->deviceSelector << std::endl; + // Fallthrough. + } + + // Add 5ms delay before destroying device + // Helper method to delay and then call destroyDeviceReq - void delayedDestroyDevice( - std::shared_ptr context) - { - // Initialize timer with LivoxGen1 metadata io_service - delayTimer = std::make_unique( - smoThreadingModelDesc.componentThread->getIoService()); - - delayTimer->expires_from_now(boost::posix_time::milliseconds(5)); - delayTimer->async_wait( - std::bind( - &DetachDeviceReq::detachDeviceReq1_delayed, - context.get(), context, - std::placeholders::_1)); - } + // Initialize timer with LivoxGen1 metadata io_service + co_await adapters::boostAsio::deadlineTimerWaitAReq( + requestComponentThread->getIoService(), + boost::posix_time::milliseconds(LIVOX_GEN1_DEVICE_COMMAND_DELAY_MS)); // Callback for the delayed destroyDeviceReq - void detachDeviceReq1_delayed( - std::shared_ptr context, - const boost::system::error_code& error) + // Remove StimBuffer from collection if it exists + if (!stimBuffer) { - if (error) - { - std::cerr << __func__ << ": Timer error: " << error.message() - << std::endl; - // Fallthrough. - } - - // Remove StimBuffer from collection if it exists - if (!context->stimBuffer) - { - throw std::runtime_error(std::string(__func__) - + ": stimBuffer (API: " + context->spec->stimBuffApi + ") " - + "is missing in detachDeviceReq1_delayed " - + "for device " + context->spec->deviceSelector); - } - - // Get the producer from the buffer's parent - auto& stimProducer = dynamic_cast( - context->stimBuffer->parent); - - stimProducer.destroyAttachedStimulusBuffer(context->stimBuffer); - - // Check if StimProducer has other buffers - if (!stimProducer.attachedStimulusBuffers.empty()) - { - // Other buffers exist - just remove this buffer, done - context->callOriginalCb(true, context->spec); - return; - } - - // No other buffers - stop and remove StimProducer - stimProducer.stop(); - // Remove stimulus producer from collection before destroying device - stimProducer.device->nAttachedStimulusProducers--; - // Find and remove the producer from the collection by comparing device - auto it2 = std::find_if( - attachedStimulusProducers.begin(), attachedStimulusProducers.end(), - [&stimProducer](const std::shared_ptr& p) - { - /** FIXME: - * When we implement the ImuStimulusProducer, we need to make - * sure we handle that properly here. - */ - auto pcloudProd = std::dynamic_pointer_cast(p); - return pcloudProd && pcloudProd->device == stimProducer.device; - }); - if (it2 != attachedStimulusProducers.end()) - { attachedStimulusProducers.erase(it2); } - - (*livoxProto1.livoxProto1_destroyDeviceReq)( - stimProducer.device, - {context, std::bind( - &DetachDeviceReq::detachDeviceReq2, - context.get(), context, - std::placeholders::_1)}); + throw std::runtime_error(std::string(__func__) + + ": stimBuffer (API: " + desc->stimBuffApi + ") " + + "is missing in detachDeviceReq1_delayed " + + "for device " + desc->deviceSelector); } - void detachDeviceReq2( - std::shared_ptr context, - bool success) - { - if (!success) - { - std::cerr << __func__ << ": Failed to destroy dev " - "device " << context->spec->deviceSelector << " for stim " - "producer.\n"; + // Get the producer from the buffer's parent + stimProducer->destroyAttachedStimulusBuffer(stimBuffer); - /** NOTE: - * There's a decent argument for falling through here and still - * removing the stimulus producer from attachedStimulusProducers. + // Check if StimProducer has other buffers + if (!stimProducer->attachedStimulusBuffers.empty()) { + // Other buffers exist - just remove this buffer, done + co_return StimBuffDeviceOpResult{true, desc}; + } + + // No other buffers - stop and remove StimProducer + stimProducer->stop(); + // Remove stimulus producer from collection before destroying device + stimProducer->device->nAttachedStimulusProducers--; + + // Find and remove the producer from the collection by comparing device + auto it = std::find_if( + attachedStimulusProducers.begin(), + attachedStimulusProducers.end(), + [&stimProducer](const std::shared_ptr &producer) + { + /** FIXME: + * When we implement the ImuStimulusProducer, we need to make + * sure we handle that properly here. */ - context->callOriginalCb(false, context->spec); - return; - } - - if (1 || smoHooksPtr->OptionParser_getOptions().verbose) - { - std::cout << __func__ << ": Successfully detached pcloud stim " - "producer for device " << context->spec->deviceSelector - << " and possibly also destroyed device.\n"; - } - - context->callOriginalCb(success, context->spec); + auto pcloudProducer = + std::dynamic_pointer_cast(producer); + return pcloudProducer && pcloudProducer->device == stimProducer->device; + }); + if (it != attachedStimulusProducers.end()) { + attachedStimulusProducers.erase(it); } -}; -// Callback function declarations -extern "C" sal_mlo_initializeIndFn livoxGen1_initializeInd; -extern "C" sal_mlo_finalizeIndFn livoxGen1_finalizeInd; -extern "C" sal_mlo_attachDeviceReqFn livoxGen1_attachDeviceReq; -extern "C" sal_mlo_detachDeviceReqFn livoxGen1_detachDeviceReq; + const bool destroyed = co_await coAwaitDestroyDevice( + requestComponentThread, + stimProducer->device); + if (!destroyed) { + std::cerr << __func__ << ": Failed to destroy dev " + "device " << desc->deviceSelector << " for stim " + "producer.\n"; -// Stim Buff API descriptor -static const StimBuffApiDesc livoxGen1ApiDesc = { - .name = "livoxGen1", - .exportedQualeIfaceApis = { - {.name = "mesh"}, - {.name = "pcloudIntensity"}, - {.name = "pcloudLightAmbience"}, - {.name = "pcloudDarkAmbience"}, - {.name = "gyro"}, - {.name = "accel"} - }, - .sal_mgmt_libOps = { - .initializeInd = livoxGen1_initializeInd, - .finalizeInd = livoxGen1_finalizeInd, - .attachDeviceReq = livoxGen1_attachDeviceReq, - .detachDeviceReq = livoxGen1_detachDeviceReq + /** NOTE: + * There's a decent argument for falling through here and still + * removing the stimulus producer from attachedStimulusProducers. + */ + co_return StimBuffDeviceOpResult{false, desc}; } -}; -// Callback function implementations -extern "C" int livoxGen1_initializeInd(void) + if (smoHooksPtr->OptionParser_getOptions().verbose) { + std::cout << __func__ << ": Successfully detached pcloud stim " + "producer for device " << desc->deviceSelector + << " and possibly also destroyed device.\n"; + } + + co_return StimBuffDeviceOpResult{true, desc}; +} + +sscl::co::ViralNonPostingInvoker livoxGen1_initializeCInd() { - if (!smoHooksPtr) - { + if (!smoHooksPtr) { throw std::runtime_error(std::string(__func__) + ": SMO hooks " "pointers not filled in."); } // Load LivoxProto1 library - auto libPath = smoHooksPtr->searchForLibInSmoSearchPaths( - "liblivoxProto1.so"); - + auto libPath = smoHooksPtr->searchForLibInSmoSearchPaths("liblivoxProto1.so"); livoxProto1.dlopenHandle.reset(dlopen( - libPath.value_or("liblivoxProto1.so").c_str(), RTLD_LAZY)); - - if (!livoxProto1.dlopenHandle) - { + libPath.value_or("liblivoxProto1.so").c_str(), + RTLD_LAZY)); + if (!livoxProto1.dlopenHandle) { throw std::runtime_error( std::string(__func__) + - ": Failed to load LivoxProto1 library: " + - (dlerror() ? dlerror() : "unknown error")); + ": Failed to load LivoxProto1 library: " + + (dlerror() ? dlerror() : "unknown error")); } // Get LivoxProto1 library functions @@ -586,7 +661,8 @@ extern "C" int livoxGen1_initializeInd(void) livoxProto1.dlopenHandle.get(), "livoxProto1_getPcloudDataFdDesc")); - if (!livoxProto1.livoxProto1_main || !livoxProto1.livoxProto1_exit + if (!livoxProto1.livoxProto1_main + || !livoxProto1.livoxProto1_exit || !livoxProto1.livoxProto1_getOrCreateDeviceReq || !livoxProto1.livoxProto1_destroyDeviceReq || !livoxProto1.livoxProto1_device_enablePcloudDataReq @@ -595,293 +671,55 @@ extern "C" int livoxGen1_initializeInd(void) || !livoxProto1.livoxProto1_getPcloudDataFdDesc) { throw std::runtime_error( - std::string(__func__) + - ": Failed to get LivoxProto1 library functions"); + std::string(__func__) + ": Failed to get LivoxProto1 library functions"); } - // Call LivoxProto1 library main function (*livoxProto1.livoxProto1_main)( - smoThreadingModelDesc.componentThread, *smoHooksPtr); - - return 0; // Success + smoThreadingModelDesc.componentThread, + *smoHooksPtr); + co_return 0; } -extern "C" int livoxGen1_finalizeInd(void) +sscl::co::ViralNonPostingInvoker livoxGen1_finalizeCInd() { - attachedStimulusProducers.clear(); - - // Call LivoxProto1 library exit function if (livoxProto1.livoxProto1_exit) { (*livoxProto1.livoxProto1_exit)(); } livoxProto1.dlopenHandle.reset(nullptr); - livoxProto1 = LivoxProto1DllState(); - return 0; // Success + co_return 0; } -extern "C" void livoxGen1_attachDeviceReq( - const std::shared_ptr& desc, - const std::shared_ptr& componentThread, - sscl::cps::Callback cb - ) -{ - if (!livoxProto1.livoxProto1_getOrCreateDeviceReq) - { - throw std::runtime_error( - std::string(__func__) + ": LivoxProto1 getOrCreateDevice function " - "not available"); +static const StimBuffApiDesc livoxGen1ApiDesc = { + .name = "livoxGen1", + .exportedQualeIfaceApis = { + {.name = "mesh"}, + {.name = "pcloudIntensity"}, + {.name = "pcloudLightAmbience"}, + {.name = "pcloudDarkAmbience"}, + {.name = "gyro"}, + {.name = "accel"} + }, + .sal_mgmt_libOps = { + .initializeCInd = livoxGen1_initializeCInd, + .finalizeCInd = livoxGen1_finalizeCInd, + .attachDeviceCReq = livoxGen1_attachDeviceCReq, + .detachDeviceCReq = livoxGen1_detachDeviceCReq } +}; - // Validate qualeIfaceApi - const std::string& qualeIfaceApi = desc->qualeIfaceApi; - if (qualeIfaceApi == "gyro" || qualeIfaceApi == "accel") - { - // These are for ImuStimulusProducer (not yet implemented) - std::cerr << __func__ << ": qualeIfaceApi '" << qualeIfaceApi - << "' requires ImuStimulusProducer which is not yet implemented" - << std::endl; - cb.callbackFn(false, desc); - return; - } - - try { - smo::intrin::validateNoIntrinParamsOnQualeIface( - desc->qualeIfaceApi, desc->qualeIfaceApiParams); - } - catch (const std::runtime_error& e) - { - std::cerr << __func__ << ": " << e.what() << std::endl; - cb.callbackFn(false, desc); - return; - } - - if (!PcloudStimulusProducer::supportsQualeIfaceApi(qualeIfaceApi)) - { - // Unknown qualeIfaceApi - std::cerr << __func__ << ": Unsupported qualeIfaceApi '" - << qualeIfaceApi << "' for LivoxGen1. " - "Supported values: mesh, pcloudIntensity, " - "pcloudLightAmbience, pcloudDarkAmbience" - << std::endl; - cb.callbackFn(false, desc); - return; - } - - auto request = std::make_shared(desc, cb); - - // Case 1: Check if StimBuffer already exists - auto stimProducer = std::dynamic_pointer_cast( - getStimulusProducer(desc)); - if (stimProducer) - { - auto existingBuffer = stimProducer->getAttachedStimulusBuffer(desc); - if (existingBuffer) - { - // StimBuffer exists, check if pcloud data is active - if (stimProducer->device && stimProducer->device->pcloudDataActive) - { - // Both StimBuffer and pcloud data are active, early return with success - request->callOriginalCb(true, request->spec); - return; - } - - // StimBuffer exists but pcloud data is not active, enable it - request->stimProducer = stimProducer; - request->attachDeviceReq5_doEnablePcloudData_maybeDirectlyCalled( - request); - - return; - } - else - { - /** EXPLANATION: - * StimProducer exists, StimBuffer doesn't (DASpec doesn't match) - * Check if producer already has a buffer with the requested - * qualeIfaceApi but different DASpec - this is not allowed. - */ - if (stimProducer->hasBufferWithQualeIfaceApi(desc->qualeIfaceApi)) - { - std::cerr << __func__ << ": Producer already has a buffer with " - "qualeIfaceApi '" << desc->qualeIfaceApi - << "' but with a different DeviceAttachmentSpec. " - "A single LivoxGen1 device cannot support multiple DASpecs " - "with the same qualeIfaceApi." << std::endl; - - cb.callbackFn(false, desc); - return; - } - - request->stimProducer = stimProducer; - // Ensure StimBuffer is attached and enable pcloud data if needed - request->attachDeviceReq4_doCreateStimBuff_maybeDirectlyCalled( - request); - - return; - } - } - - // StimProducer doesn't exist - need to create device first - - // Parse integer parameters from provider params with defaults - /** EXPLANATION: - * We may want to add a new param here called "command-delay-ms" to control - * the delay we insert between commands sent to the device. 5ms has been - * shown to be sufficient for the Livox Avia. - */ - - /* The Livox Avia will generally respond to a handshake request within - * 5ms. - */ - int commandTimeoutMs = 5; // Default: 5ms - /* Based on testing on a Livox Avia, the device will generally resume - * sending broadcast advertisement dgrams after about 5 seconds at most. - * Generally, it will resume sending them within 1-2 seconds. - */ - int retryDelayMs = 5250; // Default: 5250ms - uint8_t smoSubnetNbits = 24; // Default: /24 subnet - uint16_t dataPort = 56000; // Default data port - uint16_t cmdPort = 56001; // Default command port - uint16_t imuPort = 56002; // Default IMU port - // Default: empty string (will trigger IP auto-detection) - std::string smoIp = ""; - - // Parse optional integer parameters from provider params - for (const auto& param : desc->providerParams) - { - if (param.first == "cmd-timeout-ms") - { - commandTimeoutMs = smo::device::DeviceAttachmentSpec - ::parseRequiredParamAsInt( - desc->providerParams, "cmd-timeout-ms"); - } else if (param.first == "command-timeout-ms") - { - commandTimeoutMs = smo::device::DeviceAttachmentSpec - ::parseRequiredParamAsInt( - desc->providerParams, "command-timeout-ms"); - } else if (param.first == "retry-delay-ms") - { - retryDelayMs = smo::device::DeviceAttachmentSpec - ::parseRequiredParamAsInt( - desc->providerParams, "retry-delay-ms"); - } else if (param.first == "smo-subnet-nbits") - { - smoSubnetNbits = static_cast( - smo::device::DeviceAttachmentSpec - ::parseRequiredParamAsInt( - desc->providerParams, "smo-subnet-nbits")); - } else if (param.first == "data-port") - { - dataPort = static_cast( - smo::device::DeviceAttachmentSpec - ::parseRequiredParamAsInt(desc->providerParams, "data-port")); - } else if (param.first == "cmd-port") - { - cmdPort = static_cast( - smo::device::DeviceAttachmentSpec - ::parseRequiredParamAsInt(desc->providerParams, "cmd-port")); - } else if (param.first == "imu-port") - { - imuPort = static_cast( - smo::device::DeviceAttachmentSpec - ::parseRequiredParamAsInt(desc->providerParams, "imu-port")); - } else if (param.first == "smo-ip") - { - if (param.second.empty()) - { - throw std::runtime_error( - std::string(__func__) + ": smo-ip parameter is empty"); - } - if (param.second.find('.') == std::string::npos || - std::count(param.second.begin(), param.second.end(), '.') != 3) - { - throw std::runtime_error( - std::string(__func__) + ": smo-ip parameter is not an " - "IPv4 address"); - } - smoIp = param.second; - } - else - { - throw std::runtime_error( - std::string(__func__) + ": Unknown provider parameter: " - + param.first); - } - } - - (*livoxProto1.livoxProto1_getOrCreateDeviceReq)( - desc->deviceSelector, // deviceIdentifier (broadcast code) - componentThread, - commandTimeoutMs, retryDelayMs, - smoIp, smoSubnetNbits, - dataPort, cmdPort, imuPort, - {request, std::bind( - &AttachDeviceReq::attachDeviceReq1, - request.get(), request, - std::placeholders::_1, std::placeholders::_2)}); -} - -extern "C" void livoxGen1_detachDeviceReq( - const std::shared_ptr& desc, - sscl::cps::Callback cb - ) -{ - // Case 1: Check if StimBuffer doesn't exist (early return) - auto stimProducerBase = getStimulusProducer(desc); - if (!stimProducerBase) - { - // StimProducer doesn't exist, nothing to detach - success - cb.callbackFn(true, desc); - return; - } - - auto stimProducer = std::dynamic_pointer_cast( - stimProducerBase); - - if (!stimProducer) - { - throw std::runtime_error(std::string(__func__) + - ": Failed to cast StimulusProducer to PcloudStimulusProducer " - "for device " + desc->deviceSelector); - } - - // Check if StimBuffer exists - auto stimBuffer = stimProducer->getAttachedStimulusBuffer(desc); - if (!stimBuffer) - { - // StimBuffer doesn't exist, nothing to detach - success - cb.callbackFn(true, desc); - return; - } - - // Case 2: StimBuffer exists - proceed with detach - auto request = std::make_shared( - desc, stimBuffer, cb); - - // Disable point cloud data first - (*livoxProto1.livoxProto1_device_disablePcloudDataReq)( - stimProducer->device, - {request, std::bind( - &DetachDeviceReq::detachDeviceReq1, - request.get(), request, - std::placeholders::_1)}); -} - -// Exported function -extern "C" smo::stim_buff::SMO_GET_STIM_BUFF_API_DESC_FN_TYPEDEF +extern "C" SMO_GET_STIM_BUFF_API_DESC_FN_TYPEDEF SMO_GET_STIM_BUFF_API_DESC_FN_NAME; -const smo::stim_buff::StimBuffApiDesc& SMO_GET_STIM_BUFF_API_DESC_FN_NAME( - const smo::stim_buff::SmoCallbacks& callbacks, - const smo::stim_buff::SmoThreadingModelDesc& threadingModel) +const StimBuffApiDesc& SMO_GET_STIM_BUFF_API_DESC_FN_NAME( + const SmoCallbacks& callbacks, + const SmoThreadingModelDesc& threadingModel) { smoHooksPtr = &callbacks; smoThreadingModelDesc = threadingModel; - return livoxGen1ApiDesc; } -} // namespace stim_buff -} // namespace smo +} // namespace smo::stim_buff diff --git a/stimBuffApis/livoxGen1/livoxGen1Internal.h b/stimBuffApis/livoxGen1/livoxGen1Internal.h new file mode 100644 index 0000000..19030fd --- /dev/null +++ b/stimBuffApis/livoxGen1/livoxGen1Internal.h @@ -0,0 +1,57 @@ +#ifndef LIVOX_GEN1_INTERNAL_H +#define LIVOX_GEN1_INTERNAL_H + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "livoxGen1.h" +#include "pcloudStimulusProducer.h" + +namespace smo::stim_buff { + +struct LivoxProviderParams +{ + int commandTimeoutMs = 5; // Default: 5ms + int retryDelayMs = 5250; // Default: 5250ms + uint8_t smoSubnetNbits = 24; // Default: /24 subnet + uint16_t dataPort = 56000; // Default data port + uint16_t cmdPort = 56001; // Default command port + uint16_t imuPort = 56002; // Default IMU port + std::string smoIp; // Default: empty string (will trigger IP auto-detection) +}; + +extern const SmoCallbacks *smoHooksPtr; +extern SmoThreadingModelDesc smoThreadingModelDesc; +extern std::vector> attachedStimulusProducers; + +std::shared_ptr getStimulusProducer( + const std::shared_ptr &spec); + +size_t parseNDgramsPerFrame( + const std::shared_ptr &spec); + +LivoxProviderParams parseLivoxProviderParams( + const std::shared_ptr &desc); + +bool ensureStimBufferAttachedWithoutDuplicates( + const std::shared_ptr &stimProducer, + const std::shared_ptr &spec); + +sscl::co::ViralNonPostingInvoker livoxGen1_initializeCInd(); +sscl::co::ViralNonPostingInvoker livoxGen1_finalizeCInd(); +sscl::co::ViralNonPostingInvoker livoxGen1_attachDeviceCReq( + const std::shared_ptr &desc, + const std::shared_ptr &componentThread); +sscl::co::ViralNonPostingInvoker livoxGen1_detachDeviceCReq( + const std::shared_ptr &desc); + +} // namespace smo::stim_buff + +#endif // LIVOX_GEN1_INTERNAL_H diff --git a/stimBuffApis/livoxGen1/livoxGen1Proto1CpsBridge.cpp b/stimBuffApis/livoxGen1/livoxGen1Proto1CpsBridge.cpp new file mode 100644 index 0000000..59b5d3a --- /dev/null +++ b/stimBuffApis/livoxGen1/livoxGen1Proto1CpsBridge.cpp @@ -0,0 +1,89 @@ +#include "livoxGen1Proto1CpsBridge.h" + +#include + +namespace smo::stim_buff { + +sscl::co::ViralNonPostingInvoker +coAwaitGetOrCreateDevice( + const std::shared_ptr &componentThread, + const std::string &deviceIdentifier, + const LivoxProviderParams ¶ms) +{ + if (!livoxProto1.livoxProto1_getOrCreateDeviceReq) { + throw std::runtime_error("coAwaitGetOrCreateDevice: proto1 function missing"); + } + + auto result = co_await adapters::smo::getGetOrCreateDeviceReqAReqAwaiter( + componentThread->getIoService(), + livoxProto1.livoxProto1_getOrCreateDeviceReq, + deviceIdentifier, + componentThread, + params.commandTimeoutMs, + params.retryDelayMs, + params.smoIp, + params.smoSubnetNbits, + params.dataPort, + params.cmdPort, + params.imuPort); + co_return result; +} + +sscl::co::ViralNonPostingInvoker +coAwaitGetReturnMode( + const std::shared_ptr &componentThread, + const std::shared_ptr &device) +{ + if (!livoxProto1.livoxProto1_device_getReturnModeReq) { + throw std::runtime_error("coAwaitGetReturnMode: proto1 function missing"); + } + + co_return co_await adapters::smo::getDeviceGetReturnModeReqAReqAwaiter( + componentThread->getIoService(), + livoxProto1.livoxProto1_device_getReturnModeReq, + device); +} + +sscl::co::ViralNonPostingInvoker coAwaitEnablePcloudData( + const std::shared_ptr &componentThread, + const std::shared_ptr &device) +{ + if (!livoxProto1.livoxProto1_device_enablePcloudDataReq) { + throw std::runtime_error("coAwaitEnablePcloudData: proto1 function missing"); + } + + co_return co_await adapters::smo::getDeviceEnablePcloudDataReqAReqAwaiter( + componentThread->getIoService(), + livoxProto1.livoxProto1_device_enablePcloudDataReq, + device); +} + +sscl::co::ViralNonPostingInvoker coAwaitDisablePcloudData( + const std::shared_ptr &componentThread, + const std::shared_ptr &device) +{ + if (!livoxProto1.livoxProto1_device_disablePcloudDataReq) { + throw std::runtime_error("coAwaitDisablePcloudData: proto1 function missing"); + } + + co_return co_await adapters::smo::getDeviceDisablePcloudDataReqAReqAwaiter( + componentThread->getIoService(), + livoxProto1.livoxProto1_device_disablePcloudDataReq, + device); +} + +sscl::co::ViralNonPostingInvoker coAwaitDestroyDevice( + const std::shared_ptr &componentThread, + const std::shared_ptr &device) +{ + if (!livoxProto1.livoxProto1_destroyDeviceReq) { + throw std::runtime_error("coAwaitDestroyDevice: proto1 function missing"); + } + + co_return co_await adapters::smo::getDestroyDeviceReqAReqAwaiter( + componentThread->getIoService(), + livoxProto1.livoxProto1_destroyDeviceReq, + device); +} + +} // namespace smo::stim_buff diff --git a/stimBuffApis/livoxGen1/livoxGen1Proto1CpsBridge.h b/stimBuffApis/livoxGen1/livoxGen1Proto1CpsBridge.h new file mode 100644 index 0000000..394d053 --- /dev/null +++ b/stimBuffApis/livoxGen1/livoxGen1Proto1CpsBridge.h @@ -0,0 +1,39 @@ +#ifndef LIVOX_GEN1_PROTO1_CPS_BRIDGE_H +#define LIVOX_GEN1_PROTO1_CPS_BRIDGE_H + +#include + +#include +#include +#include + +#include "livoxGen1Internal.h" + +namespace smo::stim_buff { + +sscl::co::ViralNonPostingInvoker +coAwaitGetOrCreateDevice( + const std::shared_ptr &componentThread, + const std::string &deviceIdentifier, + const LivoxProviderParams ¶ms); + +sscl::co::ViralNonPostingInvoker +coAwaitGetReturnMode( + const std::shared_ptr &componentThread, + const std::shared_ptr &device); + +sscl::co::ViralNonPostingInvoker coAwaitEnablePcloudData( + const std::shared_ptr &componentThread, + const std::shared_ptr &device); + +sscl::co::ViralNonPostingInvoker coAwaitDisablePcloudData( + const std::shared_ptr &componentThread, + const std::shared_ptr &device); + +sscl::co::ViralNonPostingInvoker coAwaitDestroyDevice( + const std::shared_ptr &componentThread, + const std::shared_ptr &device); + +} // namespace smo::stim_buff + +#endif // LIVOX_GEN1_PROTO1_CPS_BRIDGE_H