livoxProto1: udpCmdDemux now consults per-device handler registry

Device class objects now have a per-Device unordered_map of handlers
keyed by cmd_set+cmd_id.

commonLibs/livoxProto1/core.cpp

@@ -257,6 +257,7 @@ void main(const std::shared_ptr<smo::ComponentThread> &componentThread,
 	protoState.smoCallbacks = smoCallbacks;
 	protoState.deviceManager = std::make_unique<DeviceManager>();
 	protoState.deviceManager->broadcastListener.start();
+	protoState.deviceManager->udpCommandDemuxer.start();
 }
 
 void exit(void)
@@ -265,10 +266,11 @@ void exit(void)
 		return;
 	}
 
+	protoState.deviceManager->udpCommandDemuxer.stop();
 	protoState.deviceManager->broadcastListener.stop();
 	protoState.deviceManager.reset();
 	protoState.componentThread.reset();
 	protoState.isInitialized = false;
 }
 
-} // namespace livoxProto1
+} // namespace livoxProto1

commonLibs/livoxProto1/device.cpp

@@ -1743,11 +1743,11 @@ void Device::cleanupPcloudDataSocket()
 	pcloudDataActive.store(false);
 }
 
-void Device::handleUdpDgram(const uint8_t *data, ssize_t bytesReceived,
+void Device::handleUdpDgram(
-	const struct sockaddr_in &senderAddr)
+	const uint8_t *data, ssize_t bytesReceived,
+	const struct sockaddr_in &senderAddr
+	)
 {
-	(void)senderAddr;
-
 	// Check minimum size for any valid protocol message
 	if (bytesReceived < static_cast<ssize_t>(
 		sizeof(comms::Header) + sizeof(comms::Command)))
@@ -1760,37 +1760,44 @@ void Device::handleUdpDgram(const uint8_t *data, ssize_t bytesReceived,
 	uint8_t cmd_set = data[sizeof(comms::Header)];
 	uint8_t cmd_id = data[sizeof(comms::Header) + 1];
 
-	// Route based on command type
+	// Look for a registered handler for this command
-	if (cmd_set == 0x00 && cmd_id == 0x01)
+	auto key = std::make_pair(cmd_set, cmd_id);
+	auto it = udpCommandHandlers.find(key);
+
+	if (it != udpCommandHandlers.end())
 	{
-		// Handshake ACK - check if we have enough data for HandshakeResponse
+		// Found a registered handler, invoke it
-		if (bytesReceived >= static_cast<ssize_t>(
+		try
-			sizeof(comms::HandshakeResponse)))
 		{
-			std::cout << __func__ << ": Received handshake ACK from "
+			it->second(data, bytesReceived, senderAddr);
-			          << discoveredDevice.deviceIdentifier << std::endl;
 		}
-	}
+		catch (const std::exception& e)
-	else if (cmd_set == 0x00 && cmd_id == 0x03)
-	{
-		// Heartbeat ACK - check if we have enough data for HeartbeatMessage
-		if (bytesReceived >= static_cast<ssize_t>(
-			sizeof(comms::HeartbeatMessage)))
 		{
-			// Empty intentionally.
+			std::cerr << __func__ << ": Exception in command handler for "
-		}
+			          << discoveredDevice.deviceIdentifier
-	}
+			          << " cmd_set=" << (int)cmd_set
-	else if (cmd_set == 0x00 && cmd_id == 0x04)
+			          << " cmd_id=" << (int)cmd_id
-	{
+			          << ": " << e.what() << std::endl;
-		// Sampling response - check if we have enough data for SamplingResponse
-		if (bytesReceived >= static_cast<ssize_t>(
-			sizeof(comms::SamplingResponse)))
-		{
-			std::cout << __func__ << ": Received sampling response from "
-			          << discoveredDevice.deviceIdentifier << std::endl;
 		}
 	}
 	// Unknown command types are silently ignored
 }
 
+void Device::registerUdpCommandHandler(
+	uint8_t cmd_set, uint8_t cmd_id,
+	std::function<void(
+		const uint8_t* data, ssize_t bytesReceived,
+		const struct sockaddr_in& senderAddr)> handler
+	)
+{
+	auto key = std::make_pair(cmd_set, cmd_id);
+	udpCommandHandlers[key] = std::move(handler);
+}
+
+void Device::unregisterUdpCommandHandler(uint8_t cmd_set, uint8_t cmd_id)
+{
+	auto key = std::make_pair(cmd_set, cmd_id);
+	udpCommandHandlers.erase(key);
+}
+
 } // namespace livoxProto1

commonLibs/livoxProto1/device.h

@@ -7,6 +7,7 @@
 #include <atomic>
 #include <optional>
 #include <functional>
+#include <unordered_map>
 #include <sys/socket.h>
 #include <netinet/in.h>
 #include <arpa/inet.h>
@@ -16,6 +17,16 @@
 #include "protocol.h"
 #include <callback.h>
 
+// Custom hash function for std::pair<uint8_t, uint8_t>
+namespace std {
+	template<>
+	struct hash<std::pair<uint8_t, uint8_t>> {
+		size_t operator()(const std::pair<uint8_t, uint8_t>& p) const noexcept {
+			return (static_cast<size_t>(p.first) << 8) | static_cast<size_t>(p.second);
+		}
+	};
+}
+
 // Forward declaration
 namespace smo {
 	class ComponentThread;
@@ -146,10 +157,25 @@ public:
 		const uint8_t* data, ssize_t bytesReceived,
 		const struct sockaddr_in& senderAddr);
 
+	// Command handler registration
+	void registerUdpCommandHandler(
+		uint8_t cmd_set, uint8_t cmd_id,
+		std::function<void(
+			const uint8_t* data, ssize_t bytesReceived,
+			const struct sockaddr_in& senderAddr)> handler);
+	void unregisterUdpCommandHandler(uint8_t cmd_set, uint8_t cmd_id);
+
 private:
 	// Point cloud data setup
 	bool setupPcloudDataSocket();
 	void cleanupPcloudDataSocket();
+
+	// Command handler map
+	std::unordered_map<
+		std::pair<uint8_t, uint8_t>,
+		std::function<void(
+			const uint8_t* data, ssize_t bytesReceived,
+			const struct sockaddr_in& senderAddr)>> udpCommandHandlers;
 };
 
 } // namespace livoxProto1