diff --git a/include/user/senseApiDesc.h b/include/user/senseApiDesc.h index 6b514e8..aed913a 100644 --- a/include/user/senseApiDesc.h +++ b/include/user/senseApiDesc.h @@ -34,12 +34,17 @@ struct SmoThreadingModelDesc std::shared_ptr componentThread; }; +typedef std::function sal_mlo_attachDeviceReqCbFn; +typedef std::function sal_mlo_detachDeviceReqCbFn; + typedef int (sal_mlo_initializeIndFn)(void); typedef int (sal_mlo_finalizeIndFn)(void); -typedef int (sal_mlo_attachDeviceReqFn)( - const std::shared_ptr& desc); -typedef int (sal_mlo_detachDeviceReqFn)( - const std::shared_ptr& desc); +typedef void (sal_mlo_attachDeviceReqFn)( + const std::shared_ptr& desc, + sal_mlo_attachDeviceReqCbFn cb); +typedef void (sal_mlo_detachDeviceReqFn)( + const std::shared_ptr& desc, + sal_mlo_detachDeviceReqCbFn cb); /** * @brief Hooks provided by Salmanoff to senseApi libraries. diff --git a/senseApis/livoxGen1/livoxGen1.cpp b/senseApis/livoxGen1/livoxGen1.cpp index 3dd877f..5a7188c 100644 --- a/senseApis/livoxGen1/livoxGen1.cpp +++ b/senseApis/livoxGen1/livoxGen1.cpp @@ -53,10 +53,12 @@ static std::vector> g_attachedDevices; // Callback function declarations extern "C" int livoxGen1_initializeInd(void); extern "C" int livoxGen1_finalizeInd(void); -extern "C" int livoxGen1_attachDeviceReq( - const std::shared_ptr& desc); -extern "C" int livoxGen1_detachDeviceReq( - const std::shared_ptr& desc); +extern "C" void livoxGen1_attachDeviceReq( + const std::shared_ptr& desc, + smo::sense_api::sal_mlo_attachDeviceReqCbFn cb); +extern "C" void livoxGen1_detachDeviceReq( + const std::shared_ptr& desc, + smo::sense_api::sal_mlo_detachDeviceReqCbFn cb); // Sense API descriptor static const SenseApiDesc livoxGen1ApiDesc = { @@ -151,8 +153,9 @@ extern "C" int livoxGen1_finalizeInd(void) return 0; // Success } -extern "C" int livoxGen1_attachDeviceReq( - const std::shared_ptr& desc +extern "C" void livoxGen1_attachDeviceReq( + const std::shared_ptr& desc, + smo::sense_api::sal_mlo_attachDeviceReqCbFn cb ) { if (!livoxProto1.livoxProto1_getOrCreateDeviceReq) @@ -162,6 +165,17 @@ extern "C" int livoxGen1_attachDeviceReq( "not available"); } + /** FIXME: + * We should acquire a spinlock here to ensure that the device isn't added + * in the interim while the async op executes. + */ + + for (const auto& dev : g_attachedDevices) + { + if (dev->discoveredDevice.deviceIdentifier == desc->deviceIdentifier) + { return; } + } + // Parse integer parameters from provider params with defaults /* The Livox Avia will generally respond to a handshake request within * 50ms. So we set the handshake timeout to 300ms to be safe. @@ -233,59 +247,47 @@ extern "C" int livoxGen1_attachDeviceReq( } } - std::atomic callbackCalled{false}; - std::shared_ptr device = nullptr; - std::shared_ptr self = smoHooksPtr-> - ComponentThread_getSelf(); - (*livoxProto1.livoxProto1_getOrCreateDeviceReq)( desc->deviceSelector, // deviceIdentifier (broadcast code) smoThreadingModelDesc.componentThread, handshakeTimeoutMs, retryDelayMs, smoIp, smoSubnetNbits, dataPort, cmdPort, imuPort, - [&callbackCalled, &device, self]( + [desc, cb]( bool success, std::shared_ptr dev) -> void { - callbackCalled.store(true); - device = ((success) ? dev : nullptr); - // Ensure that the bridging loop below will get awakened. - self->getIoService().post([]{}); + if (!dev) + { + std::cerr << __func__ << ": Failed to create Livox device: " + << desc->deviceSelector << std::endl; + cb(false); + return; + } + + g_attachedDevices.push_back(dev); + if (1 || OptionParser::getOptions().verbose) + { + std::cout << __func__ << ": Successfully attached Livox " + "device: " << desc->deviceSelector << " (ID: " + << desc->deviceIdentifier << ")\n"; + } + + cb(success); } ); - - /** EXPLANATION: - * Bridge the async call by dequeueing until callbackCalled is true. - */ - for (;;) - { - self->getIoService().run_one(); - if (callbackCalled.load() || self->getIoService().stopped()) - { break; } - } - - if (!device) - { - throw std::runtime_error( - std::string(__func__) + ": Failed to create Livox device: " - + desc->deviceSelector); - } - - g_attachedDevices.push_back(device); - if (1 || OptionParser::getOptions().verbose) - { - std::cout << __func__ << ": Successfully attached Livox device: " - << desc->deviceSelector << " (ID: " << desc->deviceIdentifier - << ")\n"; - } - - return 0; // Success } -extern "C" int livoxGen1_detachDeviceReq( - const std::shared_ptr& desc +extern "C" void livoxGen1_detachDeviceReq( + const std::shared_ptr& desc, + smo::sense_api::sal_mlo_detachDeviceReqCbFn cb ) { + /** FIXME: + * We should acquire a spinlock here to ensure that iterator doesn't become + * invalid in the interim while the async op executes. In the meantime, + * we'll repeat the search in the callback. + */ + // Find and remove the device from our collection auto it = std::find_if(g_attachedDevices.begin(), g_attachedDevices.end(), [&desc](const std::shared_ptr& dev) { @@ -304,44 +306,52 @@ extern "C" int livoxGen1_detachDeviceReq( if (it == g_attachedDevices.end()) { - std::cerr << __func__ << ": Device not found for detachment: " - << desc->deviceIdentifier << "\n"; - return -1; // Device not found + throw std::runtime_error( + std::string(__func__) + + ": Device not found for detachment: " + desc->deviceIdentifier); } - std::atomic callbackCalled{false}; - bool retVal = false; - std::shared_ptr self = smoHooksPtr-> - ComponentThread_getSelf(); - (*livoxProto1.livoxProto1_destroyDeviceReq)( *it, - [&callbackCalled, &retVal, self](bool success) + [cb, desc](bool success) { - callbackCalled.store(true); - retVal = success; - self->getIoService().post([]{}); + if (!success) + { + std::cerr << __func__ << ": Failed to destroy Livox device: " + << desc->deviceIdentifier << "\n"; + cb(false); + return; + } + + // Find the device in g_attachedDevices and remove it. + auto eraseIt = std::find_if( + g_attachedDevices.begin(), g_attachedDevices.end(), + [desc](const std::shared_ptr& dev) + { + const std::string& devId = dev->discoveredDevice.deviceIdentifier; + std::string devIdPrefix = devId.substr( + 0, std::min(14, devId.size())); + return devIdPrefix == desc->deviceSelector.substr( + 0, std::min(14, desc->deviceSelector.size())); + } + ); + + if (eraseIt == g_attachedDevices.end()) + { + std::cerr << __func__ << ": Race condition: device not found " + "in g_attachedDevices for detachment: " + << desc->deviceIdentifier << "\n"; + cb(false); + return; + } + + g_attachedDevices.erase(eraseIt); + std::cout << __func__ << ": Successfully detached Livox device: " + << desc->deviceIdentifier << "\n"; + + cb(success); } ); - - for (;;) - { - self->getIoService().run_one(); - if (callbackCalled.load() || self->getIoService().stopped()) - { break; } - } - - if (!retVal) - { - std::cerr << __func__ << ": Failed to destroy Livox device: " - << desc->deviceIdentifier << std::endl; - } - - g_attachedDevices.erase(it); - std::cout << __func__ << ": Successfully detached Livox device: " - << desc->deviceIdentifier << "\n"; - - return 0; } // Exported function diff --git a/senseApis/xcbWindow/xcbWindow.cpp b/senseApis/xcbWindow/xcbWindow.cpp index 5542be5..70edde9 100644 --- a/senseApis/xcbWindow/xcbWindow.cpp +++ b/senseApis/xcbWindow/xcbWindow.cpp @@ -273,8 +273,9 @@ static int xcbWindow_finalizeInd(void) return 0; } -static int xcbWindow_attachDeviceReq( - const std::shared_ptr& desc +static void xcbWindow_attachDeviceReq( + const std::shared_ptr& desc, + smo::sense_api::sal_mlo_attachDeviceReqCbFn cb ) { g_attachedWindows.emplace_back( @@ -283,11 +284,13 @@ static int xcbWindow_attachDeviceReq( std::cout << __func__ << ": Attached X11 window:\n " << g_attachedWindows.back()->stringify() << "\n"; - return 0; + + cb(true); } -static int xcbWindow_detachDeviceReq( - const std::shared_ptr& spec +static void xcbWindow_detachDeviceReq( + const std::shared_ptr& spec, + smo::sense_api::sal_mlo_detachDeviceReqCbFn cb ) { auto it = std::find_if(g_attachedWindows.begin(), g_attachedWindows.end(), @@ -298,15 +301,18 @@ static int xcbWindow_detachDeviceReq( if (it == g_attachedWindows.end()) { - std::cerr << __func__ << ": Device not found for detachment:\n" - << spec->stringify() << "\n"; - return -1; + std::cerr << __func__ << ": Device not found for detachment:\n" + << spec->stringify() << "\n"; + + cb(false); + return; } g_attachedWindows.erase(it); std::cout << __func__ << ": Detached X11 window device:\n" << spec->stringify() << "\n"; - return 0; + + cb(true); } // SenseApi descriptor diff --git a/smocore/deviceManager/deviceManager.cpp b/smocore/deviceManager/deviceManager.cpp index 784ca13..e22bbd6 100644 --- a/smocore/deviceManager/deviceManager.cpp +++ b/smocore/deviceManager/deviceManager.cpp @@ -54,6 +54,7 @@ const std::string DeviceManager::stringifyDeviceSpecs(void) return oss.str(); } +#if 0 void DeviceManager::newDeviceAttachmentSpecInd( std::shared_ptr spec, std::function device, + std::shared_ptr deviceSpec)> + deviceAttachmentSpecIndCbFn; + void newDeviceAttachmentSpecInd( std::shared_ptr spec, - std::function< - void( - bool success, std::shared_ptr device, - std::shared_ptr deviceSpec)> - callback); + deviceAttachmentSpecIndCbFn callback); private: DeviceManager() = default; diff --git a/smocore/include/senseApis/senseApiManager.h b/smocore/include/senseApis/senseApiManager.h index eeb4309..b412589 100644 --- a/smocore/include/senseApis/senseApiManager.h +++ b/smocore/include/senseApis/senseApiManager.h @@ -43,12 +43,18 @@ public: void initializeAllSenseApiLibs(void); void finalizeAllSenseApiLibs(void); - void attachAllSenseDevicesFromSpecs(void); - void attachSenseDevice( - const std::shared_ptr& spec); - void detachSenseDevice( - const std::shared_ptr& spec); - void detachAllSenseDevices(void); + typedef sal_mlo_attachDeviceReqCbFn attachSenseDeviceReqCbFn; + typedef sal_mlo_detachDeviceReqCbFn detachSenseDeviceReqCbFn; + + void attachSenseDeviceReq( + const std::shared_ptr& spec, + attachSenseDeviceReqCbFn cb); + void detachSenseDeviceReq( + const std::shared_ptr& spec, + detachSenseDeviceReqCbFn cb); + void attachAllSenseDevicesFromSpecs(void); + void detachAllSenseDevices(void); + void detachAllSenseDevicesReq(void); std::string stringifyLibs() const; @@ -61,6 +67,9 @@ private: std::vector> senseApiLibs; + class AttachSenseDeviceReq; + class DetachSenseDeviceReq; + public: static std::optional searchForLibInSmoSearchPaths( const std::string& libraryPath); diff --git a/smocore/marionette/qualeEvent.cpp b/smocore/marionette/qualeEvent.cpp new file mode 100644 index 0000000..e69de29 diff --git a/smocore/marionette/salmanoff.cpp b/smocore/marionette/salmanoff.cpp index 1b90cc6..b47f385 100644 --- a/smocore/marionette/salmanoff.cpp +++ b/smocore/marionette/salmanoff.cpp @@ -28,7 +28,7 @@ void shutdownSalmanoff(void) { std::cout << __func__ << ": Entered." << std::endl; - sense_api::SenseApiManager::getInstance().detachAllSenseDevices(); + sense_api::SenseApiManager::getInstance().detachAllSenseDevicesReq(); sense_api::SenseApiManager::getInstance().finalizeAllSenseApiLibs(); std::cout << __func__ << ": Done." << std::endl; diff --git a/smocore/senseApis/senseApiManager.cpp b/smocore/senseApis/senseApiManager.cpp index c8f9170..e0fab75 100644 --- a/smocore/senseApis/senseApiManager.cpp +++ b/smocore/senseApis/senseApiManager.cpp @@ -255,11 +255,19 @@ void SenseApiManager::finalizeAllSenseApiLibs(void) } } -void SenseApiManager::attachSenseDevice( - const std::shared_ptr& spec + + +void SenseApiManager::attachSenseDeviceReq( + const std::shared_ptr& spec, + attachSenseDeviceReqCbFn cb ) { - auto libOpt = getSenseApiLibByApiName(spec->api); + /** FIXME: + * We should acquire a spinlock here to ensure that the device isn't added + * in the interim while the async op executes. + */ + + auto libOpt = getSenseApiLibByApiName(spec->api); if (!libOpt) { throw std::runtime_error( @@ -273,13 +281,19 @@ void SenseApiManager::attachSenseDevice( std::string(__func__) + ": attachDeviceReq() is NULL for library '" + lib.libraryPath + "'"); } - lib.senseApiDesc.sal_mgmt_libOps.attachDeviceReq(spec); + lib.senseApiDesc.sal_mgmt_libOps.attachDeviceReq(spec, cb); } -void SenseApiManager::detachSenseDevice( - const std::shared_ptr& spec +void SenseApiManager::detachSenseDeviceReq( + const std::shared_ptr& spec, + detachSenseDeviceReqCbFn cb ) { + /** FIXME: + * We should acquire a spinlock here to ensure that the device isn't removed + * in the interim while the async op executes. + */ + auto libOpt = getSenseApiLibByApiName(spec->api); if (!libOpt) { @@ -294,21 +308,141 @@ void SenseApiManager::detachSenseDevice( std::string(__func__) + ": detachDeviceReq() is NULL for library '" + lib.libraryPath + "'"); } - lib.senseApiDesc.sal_mgmt_libOps.detachDeviceReq(spec); + lib.senseApiDesc.sal_mgmt_libOps.detachDeviceReq(spec, cb); } void SenseApiManager::attachAllSenseDevicesFromSpecs(void) { - for (const auto& spec : device::DeviceManager::deviceAttachmentSpecs) { - attachSenseDevice(spec); - } + std::atomic nTotal = device::DeviceManager::deviceAttachmentSpecs + .size(); + std::atomic nSucceeded = 0, nFailed = 0; + + auto self = ComponentThread::getSelf(); + for (const auto& spec : device::DeviceManager::deviceAttachmentSpecs) + { + try { + attachSenseDeviceReq(spec, + [spec, &nTotal, &nSucceeded, &nFailed, caller = self](bool success) -> void + { + if (!success) + { + ++nFailed; + std::cerr << __func__ << ": Failed to attach device: " + << spec->deviceIdentifier << "\n"; + + caller->getIoService().post([]{}); + return; + } + + ++nSucceeded; + if (nSucceeded.load() + nFailed.load() != nTotal.load()) { + return; + } + + std::cout << __func__ << ": " << nSucceeded.load() + << " devices attached, " + << nFailed.load() << " devices failed\n"; + caller->getIoService().post([]{}); + }); + } catch (const std::exception& e) { + std::cerr << __func__ << ": Exception: " << e.what() << "\n"; + ++nFailed; + } + } + + /* Bridge the async op here. */ + for (;;) + { + self->getIoService().run_one(); + if ((nSucceeded.load() + nFailed.load() == nTotal.load()) + || self->getIoService().stopped()) + { + break; + } + } + + if (self->getIoService().stopped()) + { + /* Return early because the io_service is stopped. */ + return; + } + + if (nTotal.load() != nSucceeded.load() + nFailed.load()) + { + throw std::runtime_error( + std::string(__func__) + ": Failed to get through all devices"); + } + + std::cout << __func__ << ": " << nSucceeded.load() << "/" << nTotal.load() + << " devices attached, " + << nFailed.load() << "/" << nTotal.load() << " devices failed\n"; } -void SenseApiManager::detachAllSenseDevices(void) +void SenseApiManager::detachAllSenseDevicesReq(void) { - for (const auto& spec : device::DeviceManager::deviceAttachmentSpecs) { - detachSenseDevice(spec); - } + std::atomic nTotal = device::DeviceManager::deviceAttachmentSpecs + .size(); + std::atomic nSucceeded = 0, nFailed = 0; + + auto self = ComponentThread::getSelf(); + for (const auto& spec : device::DeviceManager::deviceAttachmentSpecs) + { + try { + detachSenseDeviceReq(spec, + [spec, &nTotal, &nSucceeded, &nFailed, caller = self](bool success) -> void + { + if (!success) + { + ++nFailed; + std::cerr << __func__ << ": Failed to detach device: " + << spec->deviceIdentifier << "\n"; + + caller->getIoService().post([]{}); + return; + } + + ++nSucceeded; + if (nSucceeded.load() + nFailed.load() != nTotal.load()) { + return; + } + + std::cout << __func__ << ": " << nSucceeded.load() + << " devices detached, " + << nFailed.load() << " devices failed\n"; + caller->getIoService().post([]{}); + }); + } catch (const std::exception& e) { + std::cerr << __func__ << ": Exception: " << e.what() << "\n"; + ++nFailed; + } + } + + /* Bridge the async op here. */ + for (;;) + { + self->getIoService().run_one(); + if ((nSucceeded.load() + nFailed.load() == nTotal.load()) + || self->getIoService().stopped()) + { + break; + } + } + + if (self->getIoService().stopped()) + { + /* Return early because the io_service is stopped. */ + return; + } + + if (nTotal.load() != nSucceeded.load() + nFailed.load()) + { + throw std::runtime_error( + std::string(__func__) + ": Failed to get through all devices"); + } + + std::cout << __func__ << ": " << nSucceeded.load() << "/" << nTotal.load() + << " devices detached, " + << nFailed.load() << "/" << nTotal.load() << " devices failed\n"; } } // namespace sense_api