hayodea
816a047920
Async: Use new [Non]PostedAsyncCont and callOriginalCb This new hierarchy of classes gives us a central mechanism for managing both reply-posting and lockSpec unlocking. * callOriginalCb: Now uses a modern C++ variadic template design enabling it to handle both direct calling and std::bind() re-binding of an arbitrary number of arguments from the caller. This enables us to mostly eliminate the repeated, bespoke definitions of callOriginalCb littered throughout the codebase. We've also propagated these changes throughout the codebase in this patch.
597 lines
16 KiB
C++
597 lines
16 KiB
C++
#include <iostream>
|
|
#include <stdexcept>
|
|
#include <optional>
|
|
#include <filesystem>
|
|
#include <senseApis/senseApiManager.h>
|
|
#include <senseApis/senseApiLib.h>
|
|
#include <opts.h>
|
|
#include <asynchronousBridge.h>
|
|
#include <asynchronousContinuation.h>
|
|
#include <asynchronousLoop.h>
|
|
#include <user/senseApiDesc.h>
|
|
#include <mind.h>
|
|
#include <deviceManager/deviceManager.h>
|
|
#include <marionette/marionette.h>
|
|
|
|
|
|
namespace fs = std::filesystem;
|
|
|
|
namespace smo {
|
|
namespace sense_api {
|
|
|
|
/**
|
|
* @brief Searches for a library in predefined locations
|
|
* @param libraryPath The name or path of the library to find
|
|
* @return Optional containing the full path if found in search paths, nullopt
|
|
* if not
|
|
*
|
|
* Searches for the library in the following locations in order:
|
|
* 1. Custom path specified by --sense-api-lib-path option (if provided)
|
|
* 2. Current working directory
|
|
* 3. Directory containing the executable
|
|
*
|
|
* If the library is not found in any of these locations, returns nullopt and
|
|
* falls back to system default library search paths (LD_LIBRARY_PATH, etc.)
|
|
*/
|
|
static std::optional<std::string> searchForLibInSmoSearchPaths(
|
|
const std::string& libraryPath)
|
|
{
|
|
std::vector<std::string> searchPaths = {
|
|
fs::current_path().string(),
|
|
fs::path("/proc/self/exe").parent_path().string()
|
|
};
|
|
|
|
const auto& options = OptionParser::getOptions();
|
|
if (!options.senseApiLibPath.empty())
|
|
{
|
|
// Insert all sense API library paths at the beginning of search paths
|
|
searchPaths.insert(
|
|
searchPaths.begin(),
|
|
options.senseApiLibPath.begin(),
|
|
options.senseApiLibPath.end());
|
|
}
|
|
|
|
for (const auto& path : searchPaths)
|
|
{
|
|
fs::path fullPath = fs::path(path) / libraryPath;
|
|
if (fs::exists(fullPath))
|
|
{
|
|
return fullPath.string();
|
|
}
|
|
}
|
|
|
|
std::cerr << std::string(__func__) + ": library '"
|
|
+ libraryPath + "' isn't in search bespoke search paths: ";
|
|
for (const auto& path : searchPaths) {
|
|
std::cerr << path << " ";
|
|
}
|
|
std::cerr << std::endl;
|
|
std::cerr << "Trying to load " + libraryPath + " from system default "
|
|
"search paths\n";
|
|
|
|
return std::nullopt;
|
|
}
|
|
|
|
/* Local static function to wrap ComponentThread::getSelf for SmoCallbacks */
|
|
static std::shared_ptr<ComponentThread> ComponentThread_getSelf()
|
|
{
|
|
return ComponentThread::getSelf();
|
|
}
|
|
|
|
/* Hooks to be provided to senseApiLibs, enabling them to call into Salmanoff
|
|
* code.
|
|
*/
|
|
static SmoCallbacks smoCallbacks =
|
|
{
|
|
.searchForLibInSmoSearchPaths = searchForLibInSmoSearchPaths,
|
|
.ComponentThread_getSelf = ComponentThread_getSelf
|
|
};
|
|
|
|
/* Static file-scope threading model object for senseApi libraries */
|
|
static SmoThreadingModelDesc smoThreadingModelDesc = {
|
|
.componentThread = nullptr
|
|
};
|
|
|
|
std::optional<std::string> SenseApiManager::searchForLibInSmoSearchPaths(
|
|
const std::string& libraryPath)
|
|
{
|
|
return ::smo::sense_api::searchForLibInSmoSearchPaths(libraryPath);
|
|
}
|
|
|
|
SenseApiLib& SenseApiManager::loadSenseApiLib(
|
|
const std::string& libraryPath,
|
|
const std::shared_ptr<ComponentThread>& componentThread
|
|
)
|
|
{
|
|
std::optional<std::string> fullPath = searchForLibInSmoSearchPaths(
|
|
libraryPath);
|
|
std::string resolvedPath = fullPath.value_or(libraryPath);
|
|
|
|
// Clear any existing error
|
|
dlerror();
|
|
auto dlopen_handle = std::unique_ptr<void, SenseApiLib::DlCloser>(
|
|
dlopen(resolvedPath.c_str(), RTLD_LAZY));
|
|
if (!dlopen_handle && fullPath.has_value())
|
|
{
|
|
// Fallback to using the supplied libraryPath
|
|
dlerror();
|
|
dlopen_handle.reset(dlopen(libraryPath.c_str(), RTLD_LAZY));
|
|
}
|
|
|
|
if (!dlopen_handle)
|
|
{
|
|
const char *dlerr = dlerror();
|
|
|
|
std::string error = (dlerr
|
|
? dlerr
|
|
: "Unknown error while opening shlib");
|
|
throw std::runtime_error(
|
|
std::string(__func__) + ": Cannot load library '"
|
|
+ libraryPath + "': "
|
|
+ error);
|
|
}
|
|
|
|
// Initialize getSenseApiDescriptor
|
|
auto func = reinterpret_cast<SMO_GET_SENSE_API_DESC_FN_TYPEDEF *>(
|
|
dlsym(dlopen_handle.get(), SMO_GET_SENSE_API_DESC_FN_NAME_STR));
|
|
if (!func)
|
|
{
|
|
throw std::runtime_error(
|
|
std::string(__func__) + ": dlsym('"
|
|
SMO_GET_SENSE_API_DESC_FN_NAME_STR "') failed for library '"
|
|
+ libraryPath + "'");
|
|
}
|
|
|
|
// Check if the static threading model obj is null and initialize if needed
|
|
if (!smoThreadingModelDesc.componentThread) {
|
|
smoThreadingModelDesc.componentThread = componentThread;
|
|
}
|
|
|
|
const SenseApiDesc &libApiDesc = func(
|
|
smoCallbacks, smoThreadingModelDesc);
|
|
|
|
auto lib = std::make_shared<SenseApiLib>(
|
|
libraryPath, dlopen_handle.release(), func);
|
|
lib->setSenseApiDesc(libApiDesc);
|
|
senseApiLibs.push_back(lib);
|
|
return *senseApiLibs.back();
|
|
}
|
|
|
|
std::optional<std::shared_ptr<SenseApiLib>>
|
|
SenseApiManager::getSenseApiLib(const std::string& libraryPath)
|
|
{
|
|
auto it = std::find_if(senseApiLibs.begin(), senseApiLibs.end(),
|
|
[&libPath = libraryPath](const std::shared_ptr<SenseApiLib>& lib) {
|
|
return lib->libraryPath == libPath;
|
|
}
|
|
);
|
|
|
|
if (it != senseApiLibs.end()) { return *it; }
|
|
return std::nullopt;
|
|
}
|
|
|
|
std::optional<std::shared_ptr<SenseApiLib>>
|
|
SenseApiManager::getSenseApiLibByApiName(const std::string& apiName)
|
|
{
|
|
auto it = std::find_if(senseApiLibs.begin(), senseApiLibs.end(),
|
|
[&apiName](const std::shared_ptr<SenseApiLib>& lib) {
|
|
return lib->senseApiDesc.name == apiName;
|
|
}
|
|
);
|
|
|
|
if (it != senseApiLibs.end()) { return *it; }
|
|
return std::nullopt;
|
|
}
|
|
|
|
void SenseApiManager::unloadSenseApiLib(const std::string& libraryPath)
|
|
{
|
|
auto it = std::find_if(senseApiLibs.begin(), senseApiLibs.end(),
|
|
[&lpath = libraryPath](const std::shared_ptr<SenseApiLib>& lib) {
|
|
return lib->libraryPath == lpath;
|
|
}
|
|
);
|
|
|
|
if (it != senseApiLibs.end())
|
|
{
|
|
senseApiLibs.erase(it);
|
|
return;
|
|
}
|
|
|
|
std::cerr << std::string(__func__) + ": Library not found: "
|
|
<< libraryPath << '\n';
|
|
}
|
|
|
|
void SenseApiManager::unloadAllSenseApiLibs(void)
|
|
{
|
|
senseApiLibs.clear();
|
|
}
|
|
|
|
void SenseApiManager::loadAllSenseApiLibsFromOptions(
|
|
const std::shared_ptr<ComponentThread>& componentThread
|
|
)
|
|
{
|
|
const auto& options = OptionParser::getOptions();
|
|
for (const auto& libPath : options.senseApiLibs) {
|
|
loadSenseApiLib(libPath, componentThread);
|
|
}
|
|
}
|
|
|
|
std::string SenseApiManager::stringifyLibs() const
|
|
{
|
|
std::string result;
|
|
for (const auto& lib : senseApiLibs) {
|
|
result += lib->stringify() + "\n";
|
|
}
|
|
return result;
|
|
}
|
|
|
|
void SenseApiManager::initializeSenseApiLib(SenseApiLib& lib)
|
|
{
|
|
if (!lib.senseApiDesc.sal_mgmt_libOps.initializeInd)
|
|
{
|
|
throw std::runtime_error(
|
|
std::string(__func__) + ": initializeInd() is NULL for library '"
|
|
+ lib.libraryPath + "'");
|
|
}
|
|
lib.senseApiDesc.sal_mgmt_libOps.initializeInd();
|
|
}
|
|
|
|
void SenseApiManager::finalizeSenseApiLib(SenseApiLib& lib)
|
|
{
|
|
if (!lib.senseApiDesc.sal_mgmt_libOps.finalizeInd)
|
|
{
|
|
throw std::runtime_error(
|
|
std::string(__func__) + ": finalizeInd() is NULL for library '"
|
|
+ lib.libraryPath + "'");
|
|
}
|
|
lib.senseApiDesc.sal_mgmt_libOps.finalizeInd();
|
|
}
|
|
|
|
void SenseApiManager::initializeAllSenseApiLibs(void)
|
|
{
|
|
for (auto& lib : senseApiLibs) {
|
|
initializeSenseApiLib(*lib);
|
|
}
|
|
}
|
|
|
|
void SenseApiManager::finalizeAllSenseApiLibs(void)
|
|
{
|
|
for (auto& lib : senseApiLibs) {
|
|
finalizeSenseApiLib(*lib);
|
|
}
|
|
}
|
|
|
|
class SenseApiManager::AttachSenseDeviceReq
|
|
: public PostedAsynchronousContinuation<attachSenseDeviceReqCbFn>
|
|
{
|
|
public:
|
|
AttachSenseDeviceReq(
|
|
const std::shared_ptr<device::DeviceAttachmentSpec>& spec,
|
|
const std::shared_ptr<ComponentThread> &caller,
|
|
attachSenseDeviceReqCbFn cb)
|
|
: PostedAsynchronousContinuation<attachSenseDeviceReqCbFn>(
|
|
caller, cb),
|
|
spec(spec)
|
|
{}
|
|
|
|
public:
|
|
void attachSenseDeviceReq1_posted(
|
|
[[maybe_unused]] std::shared_ptr<AttachSenseDeviceReq> context
|
|
)
|
|
{
|
|
if (caller->id != ComponentThread::MRNTT)
|
|
{
|
|
std::cerr << std::string(__func__)
|
|
<< ": executed on non-mrntt thread: "
|
|
<< caller->name << std::endl;
|
|
callOriginalCb(false, spec);
|
|
return;
|
|
}
|
|
|
|
/** 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 = SenseApiManager::getInstance().getSenseApiLibByApiName(
|
|
spec->api);
|
|
|
|
if (!libOpt)
|
|
{
|
|
std::cerr << std::string(__func__) + ": No library found for API '"
|
|
<< spec->api << "'" << std::endl;
|
|
callOriginalCb(false, spec);
|
|
return;
|
|
}
|
|
|
|
auto& lib = *libOpt.value();
|
|
if (!lib.senseApiDesc.sal_mgmt_libOps.attachDeviceReq)
|
|
{
|
|
std::cerr << std::string(__func__) + ": attachDeviceReq() is NULL "
|
|
"for library '" << lib.libraryPath << "'" << std::endl;
|
|
callOriginalCb(false, spec);
|
|
return;
|
|
}
|
|
|
|
/** EXPLANATION:
|
|
* We pass in either the body or world thread here, depending on whether
|
|
* the device is an introspector (idev) or extrospector (edev).
|
|
*
|
|
* Introspectors are attached to the body thread; extrospectors are
|
|
* attached to the world thread.
|
|
*/
|
|
std::shared_ptr<ComponentThread> threadForAttachment;
|
|
if (spec->sensorType == 'e')
|
|
{
|
|
threadForAttachment = mind::globalMind->world.thread;
|
|
std::cout << __func__ << ": Attaching edev "
|
|
<< spec->deviceIdentifier << " to world thread" << "\n";
|
|
}
|
|
else
|
|
{
|
|
threadForAttachment = mind::globalMind->body.thread;
|
|
std::cout << __func__ << ": Attaching non-edev "
|
|
<< spec->deviceIdentifier << " to body thread" << "\n";
|
|
}
|
|
|
|
lib.senseApiDesc.sal_mgmt_libOps.attachDeviceReq(
|
|
spec, threadForAttachment,
|
|
std::bind(
|
|
&AttachSenseDeviceReq::attachSenseDeviceReq2,
|
|
context.get(), context,
|
|
std::placeholders::_1, std::placeholders::_2));
|
|
}
|
|
|
|
void attachSenseDeviceReq2(
|
|
[[maybe_unused]] std::shared_ptr<AttachSenseDeviceReq> context,
|
|
bool success,
|
|
std::shared_ptr<device::DeviceAttachmentSpec> deviceSpec
|
|
)
|
|
{
|
|
callOriginalCb(success, deviceSpec);
|
|
}
|
|
|
|
void detachSenseDeviceReq1_posted(
|
|
[[maybe_unused]] std::shared_ptr<DetachSenseDeviceReq> context
|
|
)
|
|
{
|
|
if (caller->id != ComponentThread::MRNTT)
|
|
{
|
|
std::cerr << std::string(__func__)
|
|
<< ": executed on non-mrntt thread: "
|
|
<< caller->name << std::endl;
|
|
callOriginalCb(false, spec);
|
|
return;
|
|
}
|
|
|
|
/** 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 = SenseApiManager::getInstance().getSenseApiLibByApiName(
|
|
spec->api);
|
|
if (!libOpt)
|
|
{
|
|
std::cerr << std::string(__func__) + ": No library found for API '"
|
|
<< spec->api << "'" << std::endl;
|
|
callOriginalCb(false, spec);
|
|
return;
|
|
}
|
|
auto& lib = *libOpt.value();
|
|
if (!lib.senseApiDesc.sal_mgmt_libOps.detachDeviceReq)
|
|
{
|
|
std::cerr << std::string(__func__) + ": detachDeviceReq() is NULL "
|
|
"for library '" << lib.libraryPath << "'" << std::endl;
|
|
callOriginalCb(false, spec);
|
|
return;
|
|
}
|
|
lib.senseApiDesc.sal_mgmt_libOps.detachDeviceReq(
|
|
spec,
|
|
std::bind(
|
|
&DetachSenseDeviceReq::detachSenseDeviceReq2,
|
|
context.get(), context,
|
|
std::placeholders::_1, std::placeholders::_2));
|
|
}
|
|
|
|
void detachSenseDeviceReq2(
|
|
[[maybe_unused]] std::shared_ptr<DetachSenseDeviceReq> context,
|
|
bool success,
|
|
std::shared_ptr<device::DeviceAttachmentSpec> deviceSpec
|
|
)
|
|
{
|
|
callOriginalCb(success, deviceSpec);
|
|
}
|
|
|
|
public:
|
|
std::shared_ptr<device::DeviceAttachmentSpec> spec;
|
|
};
|
|
|
|
void SenseApiManager::attachSenseDeviceReq(
|
|
const std::shared_ptr<device::DeviceAttachmentSpec>& spec,
|
|
attachSenseDeviceReqCbFn cb
|
|
)
|
|
{
|
|
const auto& caller = ComponentThread::getSelf();
|
|
auto request = std::make_shared<AttachSenseDeviceReq>(
|
|
spec, caller, cb);
|
|
|
|
mrntt::mrntt.thread->getIoService().post(
|
|
std::bind(
|
|
&AttachSenseDeviceReq::attachSenseDeviceReq1_posted,
|
|
request.get(), request));
|
|
}
|
|
|
|
void SenseApiManager::detachSenseDeviceReq(
|
|
const std::shared_ptr<device::DeviceAttachmentSpec>& spec,
|
|
detachSenseDeviceReqCbFn cb
|
|
)
|
|
{
|
|
const auto& caller = ComponentThread::getSelf();
|
|
auto request = std::make_shared<DetachSenseDeviceReq>(
|
|
spec, caller, cb);
|
|
|
|
mrntt::mrntt.thread->getIoService().post(
|
|
std::bind(
|
|
&DetachSenseDeviceReq::detachSenseDeviceReq1_posted,
|
|
request.get(), request));
|
|
}
|
|
|
|
class SenseApiManager::AttachAllSenseDevicesFromSpecsReq
|
|
: public PostedAsynchronousContinuation<
|
|
attachAllSenseDevicesFromSpecsReqCbFn>
|
|
{
|
|
public:
|
|
AttachAllSenseDevicesFromSpecsReq(
|
|
const unsigned int totalNSpecs,
|
|
const std::shared_ptr<ComponentThread>& caller,
|
|
attachAllSenseDevicesFromSpecsReqCbFn cb)
|
|
: PostedAsynchronousContinuation<attachAllSenseDevicesFromSpecsReqCbFn>(
|
|
caller, cb),
|
|
loop(totalNSpecs)
|
|
{}
|
|
|
|
public:
|
|
void attachAllSenseDevicesFromSpecsReq1_posted(
|
|
[[maybe_unused]] std::shared_ptr<AttachAllSenseDevicesFromSpecsReq> context
|
|
)
|
|
{
|
|
for (const auto& spec : device::DeviceManager::deviceAttachmentSpecs)
|
|
{
|
|
SenseApiManager::getInstance().attachSenseDeviceReq(
|
|
spec,
|
|
std::bind(
|
|
&AttachAllSenseDevicesFromSpecsReq::attachAllSenseDevicesFromSpecsReq2,
|
|
context.get(), context,
|
|
std::placeholders::_1, std::placeholders::_2));
|
|
}
|
|
}
|
|
|
|
// Callback methods for the attachment sequence
|
|
void attachAllSenseDevicesFromSpecsReq2(
|
|
std::shared_ptr<AttachAllSenseDevicesFromSpecsReq> context,
|
|
bool success, std::shared_ptr<device::DeviceAttachmentSpec> spec
|
|
)
|
|
{
|
|
if (!success)
|
|
{
|
|
std::cerr << __func__ << ": Failed to attach device: "
|
|
<< spec->deviceIdentifier << "\n";
|
|
// Fallthrough.
|
|
}
|
|
|
|
if (!context->loop.incrementSuccessOrFailureAndTestForCompletionDueTo(
|
|
success))
|
|
{
|
|
return;
|
|
}
|
|
|
|
if (OptionParser::getOptions().verbose)
|
|
{
|
|
std::cout << __func__ << ": " << context->loop.nSucceeded.load()
|
|
<< " devices attached, "
|
|
<< context->loop.nFailed.load() << " devices failed\n";
|
|
}
|
|
|
|
context->callOriginalCb(loop);
|
|
}
|
|
|
|
public:
|
|
AsynchronousLoop loop;
|
|
};
|
|
|
|
void SenseApiManager::attachAllSenseDevicesFromSpecsReq(
|
|
attachAllSenseDevicesFromSpecsReqCbFn cb
|
|
)
|
|
{
|
|
if (device::DeviceManager::getInstance().deviceAttachmentSpecs.size() == 0)
|
|
{
|
|
AsynchronousLoop tmp(0);
|
|
cb(tmp);
|
|
return;
|
|
}
|
|
|
|
const auto& caller = ComponentThread::getSelf();
|
|
auto request = std::make_shared<AttachAllSenseDevicesFromSpecsReq>(
|
|
device::DeviceManager::getInstance().deviceAttachmentSpecs.size(),
|
|
caller, std::move(cb));
|
|
|
|
mrntt::mrntt.thread->getIoService().post(
|
|
std::bind(
|
|
&AttachAllSenseDevicesFromSpecsReq::attachAllSenseDevicesFromSpecsReq1_posted,
|
|
request.get(), request));
|
|
}
|
|
|
|
class SenseApiManager::DetachAllSenseDevicesReq
|
|
: public AttachAllSenseDevicesFromSpecsReq
|
|
{
|
|
public:
|
|
using AttachAllSenseDevicesFromSpecsReq::AttachAllSenseDevicesFromSpecsReq;
|
|
|
|
void detachAllSenseDevicesReq1_posted(
|
|
[[maybe_unused]] std::shared_ptr<DetachAllSenseDevicesReq> context
|
|
)
|
|
{
|
|
for (const auto& spec : device::DeviceManager::deviceAttachmentSpecs)
|
|
{
|
|
SenseApiManager::getInstance().detachSenseDeviceReq(
|
|
spec,
|
|
std::bind(
|
|
&DetachAllSenseDevicesReq::detachAllSenseDevicesReq2,
|
|
context.get(), context,
|
|
std::placeholders::_1, std::placeholders::_2));
|
|
}
|
|
}
|
|
|
|
void detachAllSenseDevicesReq2(
|
|
std::shared_ptr<DetachAllSenseDevicesReq> context,
|
|
bool success, std::shared_ptr<device::DeviceAttachmentSpec> spec
|
|
)
|
|
{
|
|
if (!success)
|
|
{
|
|
std::cerr << __func__ << ": Failed to detach device: "
|
|
<< spec->deviceIdentifier << "\n";
|
|
// Fallthrough.
|
|
}
|
|
|
|
if (!context->loop.incrementSuccessOrFailureAndTestForCompletionDueTo(
|
|
success))
|
|
{
|
|
return;
|
|
}
|
|
|
|
if (OptionParser::getOptions().verbose)
|
|
{
|
|
std::cout << __func__ << ": " << context->loop.nSucceeded.load()
|
|
<< " devices detached, "
|
|
<< context->loop.nFailed.load() << " devices failed\n";
|
|
}
|
|
|
|
context->callOriginalCb(loop);
|
|
}
|
|
};
|
|
|
|
void SenseApiManager::detachAllSenseDevicesReq(
|
|
detachAllSenseDevicesReqCbFn cb
|
|
)
|
|
{
|
|
if (device::DeviceManager::getInstance().deviceAttachmentSpecs.size() == 0)
|
|
{
|
|
AsynchronousLoop tmp(0);
|
|
cb(tmp);
|
|
return;
|
|
}
|
|
|
|
const auto& caller = ComponentThread::getSelf();
|
|
auto request = std::make_shared<DetachAllSenseDevicesReq>(
|
|
device::DeviceManager::getInstance().deviceAttachmentSpecs.size(),
|
|
caller, std::move(cb));
|
|
|
|
mrntt::mrntt.thread->getIoService().post(
|
|
std::bind(
|
|
&DetachAllSenseDevicesReq::detachAllSenseDevicesReq1_posted,
|
|
request.get(), request));
|
|
}
|
|
|
|
} // namespace sense_api
|
|
} // namespace smo
|