livoxGen1: Implement attach/detachDeviceReq by sync-bridging Proto1

We perform bridged synchronous calls into liblivoxProto1 in order
to support attach/detachDeviceReq. We'll eventually make
attachDetachDeviceReq fully asynchronous but for now we're happy
that we have this working driver for this fairly tricky device.

senseApis/livoxGen1/livoxGen1.cpp

@@ -6,11 +6,13 @@
 #include <functional>
 #include <algorithm>
 #include <dlfcn.h>
+#include <opts.h>
 #include <user/senseApiDesc.h>
 #include <user/deviceAttachmentSpec.h>
 #include <livoxProto1/livoxProto1.h>
 #include <livoxProto1/device.h>
 
+
 namespace smo {
 namespace sense_api {
 
@@ -25,7 +27,8 @@ struct LivoxProto1DllState
 	: dlopenHandle(nullptr, DlCloser),
 	  livoxProto1_main(nullptr),
 	  livoxProto1_exit(nullptr),
-	  livoxProto1_getOrCreateDevice(nullptr)
+	  livoxProto1_getOrCreateDeviceReq(nullptr),
+	  livoxProto1_destroyDeviceReq(nullptr)
 	{}
 
 	static void DlCloser(void* handle)
@@ -38,7 +41,8 @@ struct LivoxProto1DllState
 	std::unique_ptr<void, void(*)(void*)> dlopenHandle;
 	livoxProto1_mainFn *livoxProto1_main;
 	livoxProto1_exitFn *livoxProto1_exit;
-	livoxProto1_getOrCreateDeviceFn *livoxProto1_getOrCreateDevice;
+	livoxProto1_getOrCreateDeviceReqFn *livoxProto1_getOrCreateDeviceReq;
+	livoxProto1_destroyDeviceReqFn *livoxProto1_destroyDeviceReq;
 };
 
 static LivoxProto1DllState livoxProto1;
@@ -100,14 +104,20 @@ extern "C" int livoxGen1_initializeInd(void)
 		dlsym(livoxProto1.dlopenHandle.get(), "livoxProto1_main"));
 	livoxProto1.livoxProto1_exit = reinterpret_cast<livoxProto1_exitFn *>(
 		dlsym(livoxProto1.dlopenHandle.get(), "livoxProto1_exit"));
-	livoxProto1.livoxProto1_getOrCreateDevice = reinterpret_cast<
+	livoxProto1.livoxProto1_getOrCreateDeviceReq = reinterpret_cast<
-		livoxProto1_getOrCreateDeviceFn *>(
+		livoxProto1_getOrCreateDeviceReqFn *>(
 			dlsym(
 				livoxProto1.dlopenHandle.get(),
-				"livoxProto1_getOrCreateDevice"));
+				"livoxProto1_getOrCreateDeviceReq"));
+	livoxProto1.livoxProto1_destroyDeviceReq = reinterpret_cast<
+		livoxProto1_destroyDeviceReqFn *>(
+			dlsym(
+				livoxProto1.dlopenHandle.get(),
+				"livoxProto1_destroyDeviceReq"));
 
 	if (!livoxProto1.livoxProto1_main || !livoxProto1.livoxProto1_exit
-		|| !livoxProto1.livoxProto1_getOrCreateDevice)
+		|| !livoxProto1.livoxProto1_getOrCreateDeviceReq
+		|| !livoxProto1.livoxProto1_destroyDeviceReq)
 	{
 		throw std::runtime_error(
 			std::string(__func__) +
@@ -115,7 +125,8 @@ extern "C" int livoxGen1_initializeInd(void)
 	}
 
 	// Call LivoxProto1 library main function
-	(*livoxProto1.livoxProto1_main)(smoThreadingModelDesc.componentThread);
+	(*livoxProto1.livoxProto1_main)(
+		smoThreadingModelDesc.componentThread, *smoHooksPtr);
 
 	return 0; // Success
 }
@@ -144,7 +155,7 @@ extern "C" int livoxGen1_attachDeviceReq(
 	const std::shared_ptr<smo::device::DeviceAttachmentSpec>& desc
 	)
 {
-	if (!livoxProto1.livoxProto1_getOrCreateDevice)
+	if (!livoxProto1.livoxProto1_getOrCreateDeviceReq)
 	{
 		throw std::runtime_error(
 			std::string(__func__) + ": LivoxProto1 getOrCreateDevice function "
@@ -152,8 +163,15 @@ extern "C" int livoxGen1_attachDeviceReq(
 	}
 
 	// Parse integer parameters from provider params with defaults
-	int handshakeTimeoutMs = 250;	// Default: 50ms
+	/* The Livox Avia will generally respond to a handshake request within
-	int retryDelayMs = 2000;			// Default: 500ms
+	 * 50ms. So we set the handshake timeout to 300ms to be safe.
+	 */
+	int handshakeTimeoutMs = 300;	// Default: 50ms
+	/* 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: 500ms
 	uint8_t smoSubnetNbits = 24;	// Default: /24 subnet
 	uint16_t dataPort = 56000;		// Default data port
 	uint16_t cmdPort = 56001;		// Default command port
@@ -214,13 +232,36 @@ extern "C" int livoxGen1_attachDeviceReq(
 		}
 	}
 
-	// Call getOrCreateDevice with parsed parameters
+	std::atomic<bool> callbackCalled{false};
-	auto device = (*livoxProto1.livoxProto1_getOrCreateDevice)(
+	std::shared_ptr<livoxProto1::Device> device = nullptr;
+	std::shared_ptr<ComponentThread> self = smoHooksPtr->
+		ComponentThread_getSelf();
+
+	(*livoxProto1.livoxProto1_getOrCreateDeviceReq)(
 		desc->deviceSelector,  // deviceIdentifier (broadcast code)
 		smoThreadingModelDesc.componentThread,
 		handshakeTimeoutMs, retryDelayMs,
 		smoIp, smoSubnetNbits,
-		dataPort, cmdPort, imuPort);
+		dataPort, cmdPort, imuPort,
+		[&callbackCalled, &device, self](
+			bool success, std::shared_ptr<livoxProto1::Device> dev) -> void
+		{
+			callbackCalled.store(true);
+			device = ((success) ? dev : nullptr);
+			// Ensure that the bridging loop below will get awakened.
+			self->getIoService().post([]{});
+		}
+	);
+
+	/**		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)
 	{
@@ -230,8 +271,12 @@ extern "C" int livoxGen1_attachDeviceReq(
 	}
 
 	g_attachedDevices.push_back(device);
-	std::cout << __func__ << ": Successfully attached Livox device: "
+	if (1 || OptionParser::getOptions().verbose)
-		<< desc->deviceSelector << " (ID: " << desc->deviceIdentifier << ")\n";
+	{
+		std::cout << __func__ << ": Successfully attached Livox device: "
+			<< desc->deviceSelector << " (ID: " << desc->deviceIdentifier
+			<< ")\n";
+	}
 
 	return 0; // Success
 }
@@ -263,10 +308,39 @@ extern "C" int livoxGen1_detachDeviceReq(
 		return -1; // Device not found
 	}
 
+	std::atomic<bool> callbackCalled{false};
+	bool retVal = false;
+	std::shared_ptr<ComponentThread> self = smoHooksPtr->
+		ComponentThread_getSelf();
+
+	(*livoxProto1.livoxProto1_destroyDeviceReq)(
+		*it,
+		[&callbackCalled, &retVal, self](bool success)
+		{
+			callbackCalled.store(true);
+			retVal = success;
+			self->getIoService().post([]{});
+		}
+	);
+
+	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; // Success
+
+	return 0;
 }
 
 // Exported function