salmanoff/smocore/senseApis/senseApiManager.cpp

#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 <callback.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,
		Callback<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,
			{context, 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,
			{context, 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,
    Callback<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,
    Callback<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,
		Callback<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,
				{context, 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(
	Callback<attachAllSenseDevicesFromSpecsReqCbFn> cb
	)
{
	if (device::DeviceManager::getInstance().deviceAttachmentSpecs.size() == 0)
	{
		AsynchronousLoop tmp(0);
		cb.callbackFn(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,
				{context, 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(
	Callback<detachAllSenseDevicesReqCbFn> cb
	)
{
	if (device::DeviceManager::getInstance().deviceAttachmentSpecs.size() == 0)
	{
		AsynchronousLoop tmp(0);
		cb.callbackFn(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