LivoxProto1: Implemented Device and getOrCreateDevice

Includes everything from sending heartbeat msgs to performing
the connection handshake. We also accept many params to
provider-params to customize and make things easier.

commonLibs/livoxProto1/device.cpp

@@ -0,0 +1,606 @@
+#include <sstream>
+#include <thread>
+#include <chrono>
+#include <string>
+#include <stdexcept>
+#include <memory>
+#include <unistd.h>
+#include <ifaddrs.h>
+#include <arpa/inet.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <boost/asio.hpp>
+#include "device.h"
+#include "protocol.h"
+#include "core.h"
+
+namespace livoxProto1 {
+namespace comms {
+
+// DiscoveredDevice constructors
+DiscoveredDevice::DiscoveredDevice(
+	const std::string &deviceIdentifier,
+	DeviceType deviceType,
+	const std::string &ipAddr)
+: deviceIdentifier(deviceIdentifier),
+  deviceType(deviceType),
+  ipAddr(ipAddr)
+{
+}
+
+DiscoveredDevice::DiscoveredDevice(
+	const BroadcastMessage &msg, const std::string &ipAddr
+)
+: DiscoveredDevice(
+	reinterpret_cast<const char*>(msg.broadcast_code),
+	static_cast<DeviceType>(msg.dev_type),
+	ipAddr)
+{
+}
+
+std::string DiscoveredDevice::stringify(void) const
+{
+	std::ostringstream oss;
+	oss << "DiscoveredDevice{"
+		<< "identifier='" << deviceIdentifier << "', "
+		<< "ipAddr='" << ipAddr << "', "
+		<< "deviceType=" << (int)deviceType << " (" << getDeviceTypeName() << ")"
+		<< "}";
+	return oss.str();
+}
+
+std::string DiscoveredDevice::getDeviceTypeName(void) const
+{
+	switch (deviceType)
+	{
+		case DeviceType::Hub: return "Hub";
+		case DeviceType::Mid40: return "Mid-40";
+		case DeviceType::Tele15: return "Tele-15";
+		case DeviceType::Horizon: return "Horizon";
+		case DeviceType::Mid70: return "Mid-70";
+		case DeviceType::Avia: return "Avia";
+		default: return "Unknown";
+	}
+}
+
+} // namespace comms
+
+// Device implementation
+Device::Device(const std::string &deviceIdentifier,
+	const std::shared_ptr<smo::ComponentThread>& componentThread,
+	int handshakeTimeoutMs, int retryDelayMs,
+	const std::string& smoIp, uint8_t smoSubnetNbits,
+	uint16_t dataPort, uint16_t cmdPort, uint16_t imuPort)
+: discoveredDevice(
+	deviceIdentifier, comms::DeviceType::Mid40,
+	// Initialize empty. IP will be set upon successful connection.
+	""),
+componentThread(componentThread),
+handshakeTimeoutMs(handshakeTimeoutMs), retryDelayMs(retryDelayMs),
+smoIp(smoIp), smoSubnetNbits(smoSubnetNbits),
+dataPort(dataPort), cmdPort(cmdPort), imuPort(imuPort),
+heartbeatActive(false)
+{
+	connect();
+}
+
+Device::~Device()
+{
+	// Stop heartbeat if active
+	if (heartbeatActive.load()) {
+		heartbeatActive.store(false);
+		if (heartbeatTimer) {
+			heartbeatTimer->cancel();
+		}
+	}
+
+	// Clean up heartbeat resources
+	heartbeatTimer.reset();
+	heartbeatSocket.reset();
+}
+
+void Device::connect()
+{
+	/**		EXPLANATION:
+	 * First check the broadcastListener to see if the device is already known.
+	 * * If it is, return the DiscoveredDevice..
+	 * If it is not, attempt to connect to the device by assuming that its IP
+	 * address is the same as the last 2 octets of the deviceIdentifier.
+	 * * If the connection is successful, return the DiscoveredDevice.
+	 * If the connection is not successful, delay by retryDelayMs and check
+	 * the broadcastListener again.
+	 * * If the connection is successful return the DiscoveredDevice.
+	 * If the connection is not successful, throw exception?
+	 *
+	 * If the connection is successful at any point, also set up the heartbeat
+	 * pulse signal to be sent periodically by us to the device over the wire.
+	 */
+	// Try connecting to known device first
+	if (connectToKnownDevice()) {
+		startHeartbeat();
+		return;
+	}
+
+	// Try direct connect by device identifier
+	if (connectByDeviceIdentifier()) {
+		startHeartbeat();
+		return;
+	}
+
+	// Wait retry delay, then try known device again
+	std::this_thread::sleep_for(std::chrono::milliseconds(retryDelayMs));
+	if (connectToKnownDevice()) {
+		startHeartbeat();
+		return;
+	}
+
+	// All connection attempts failed
+	throw std::runtime_error(
+		std::string(__func__) + ": Failed to connect to device: "
+		+ discoveredDevice.deviceIdentifier);
+}
+
+bool Device::connectToKnownDevice()
+{
+	// Get the global DeviceManager instance
+	auto& protoState = livoxProto1::getProtoState();
+	if (!protoState.deviceManager)
+	{
+		throw std::runtime_error(
+			std::string(__func__)
+			+ ": DeviceManager is not initialized in connectToKnownDevice()");
+	}
+
+	// Check if the device is known to the broadcastListener
+	if (!protoState.deviceManager->broadcastListener.deviceExists(
+		discoveredDevice.deviceIdentifier))
+	{
+		return false;
+	}
+
+	// Get the device info from broadcastListener
+	auto deviceInfo = protoState.deviceManager->broadcastListener.getDevice(
+		discoveredDevice.deviceIdentifier);
+	if (!deviceInfo)
+		{ return false; }
+
+	// Use the IP address from the broadcast message
+	std::string deviceIP = deviceInfo->ipAddr;
+	// Execute handshake with the known device
+	bool success = executeHandshake(
+		deviceIP, handshakeTimeoutMs, dataPort, cmdPort, imuPort);
+
+	// If successful, update our device's IP address with the one from broadcast
+	if (success) {
+		discoveredDevice.ipAddr = deviceInfo->ipAddr;
+	}
+
+	return success;
+}
+
+bool Device::connectByDeviceIdentifier()
+{
+	std::string deviceIP = generateClientDeviceIpFromSerialNumber(
+		discoveredDevice.deviceIdentifier);
+	bool success = executeHandshake(
+		deviceIP, handshakeTimeoutMs, dataPort, cmdPort, imuPort);
+
+	// If successful, store the calculated IP address
+	if (success) {
+		discoveredDevice.ipAddr = deviceIP;
+	}
+
+	return success;
+}
+
+bool Device::executeHandshake(
+	const std::string& deviceIP, int timeoutMs,
+	uint16_t dataPort, uint16_t cmdPort, uint16_t imuPort
+	)
+{
+	try {
+		// Create boost::asio UDP socket
+		boost::asio::io_context io_context;
+		boost::asio::ip::udp::socket socket(io_context);
+		socket.open(boost::asio::ip::udp::v4());
+
+		std::string smoIp = getSmoIp();
+		comms::HandshakeRequest handshakeReq(smoIp, dataPort, cmdPort, imuPort);
+		handshakeReq.swapContentsToProtocolEndianness();
+		handshakeReq.header.setCrc16FromRawBytes();
+		handshakeReq.header.swapCrc16ToProtocolEndianness();
+		handshakeReq.footer.crc_32 = handshakeReq.calculateCrc32();
+		handshakeReq.footer.swapCrc32ToProtocolEndianness();
+		boost::asio::ip::udp::endpoint deviceEndpoint(
+			boost::asio::ip::address::from_string(deviceIP), 65000);
+
+		socket.send_to(
+			boost::asio::buffer(&handshakeReq, sizeof(handshakeReq)),
+			deviceEndpoint);
+
+		std::cout << __func__ << ": Sent handshake request to "
+			<< deviceIP << ":65000" << std::endl;
+
+		// Wait for response with timeout using deadline_timer
+		boost::asio::deadline_timer timer(io_context);
+		timer.expires_from_now(boost::posix_time::milliseconds(timeoutMs));
+
+		uint8_t responseBuffer[1024];
+		boost::asio::ip::udp::endpoint senderEndpoint;
+		std::atomic<bool> timeoutOccurred{false};
+
+		timer.async_wait(
+			[&timeoutOccurred](const boost::system::error_code& ec) {
+			if (!ec) { timeoutOccurred.store(true); }
+			}
+		);
+
+		size_t bytesReceived = 0;
+		boost::system::error_code receiveError;
+		socket.async_receive_from(
+			boost::asio::buffer(responseBuffer, sizeof(responseBuffer)),
+			senderEndpoint,
+			[&bytesReceived, &receiveError](
+				const boost::system::error_code& ec, size_t bytes)
+			{
+				bytesReceived = bytes;
+				receiveError = ec;
+			}
+		);
+
+		while (!timeoutOccurred.load() && !receiveError && bytesReceived == 0) {
+			io_context.run_one();
+		}
+
+		timer.cancel();
+
+		if (timeoutOccurred.load())
+		{
+			std::cerr << __func__ << ": Handshake timeout with " << deviceIP
+				<< ":" << deviceEndpoint << std::endl;
+			return false;
+		}
+
+		if (receiveError)
+		{
+			std::cerr << __func__ << ": Handshake error with " << deviceIP
+				<< ": " << receiveError.message() << ":" << deviceEndpoint
+				<< std::endl;
+			return false;
+		}
+
+		if (bytesReceived < sizeof(comms::HandshakeResponse))
+		{
+			std::cerr << __func__ << ": Handshake failed - response too small from "
+				<< deviceIP << ":" << deviceEndpoint << std::endl;
+			return false;
+		}
+
+		// Parse response as complete frame
+		comms::HandshakeResponse* resp = reinterpret_cast<
+			comms::HandshakeResponse*
+			>(responseBuffer);
+
+		// Following the clean receiving flow:
+		// 1. Swap CRC32 to host endianness first
+		resp->footer.swapCrc32ToHostEndianness();
+		// 2. Validate CRC32 (on whole message excluding footer CRC32 field)
+		if (!resp->validateCrc32())
+		{
+			std::cerr << __func__ << ": Handshake failed - CRC32 validation "
+				"failed from " << deviceIP << ":" << deviceEndpoint
+				<< std::endl;
+			return false;
+		}
+		// 3. Swap CRC16 to host endianness
+		resp->header.swapCrc16ToHostEndianness();
+		// 4. Validate CRC16 (on header only)
+		if (!resp->header.validateCrc16())
+		{
+			std::cerr << __func__ << ": Handshake failed - CRC16 validation "
+				"failed from " << deviceIP << ":" << deviceEndpoint
+				<< std::endl;
+			return false;
+		}
+		// 5. Swap content to host endianness
+		resp->swapContentsToHostEndianness();
+		if (!resp->sanityCheck() || resp->ret_code != 0x00)
+		{
+			std::cerr << __func__ << ": Handshake failed - invalid response from "
+				<< deviceIP << ":" << deviceEndpoint
+				<< std::endl;
+			return false;
+		}
+
+		std::cout << __func__ << ": Handshake successful with " << deviceIP
+			<< ":" << deviceEndpoint << std::endl;
+		return true;
+	} catch (const std::exception& e) {
+		std::cerr << __func__ << ": Handshake failed with " << deviceIP << ": "
+			<< e.what() << std::endl;
+	}
+	return false;
+}
+
+std::string Device::generateClientDeviceIpFromSerialNumber(
+	const std::string& broadcastCode
+	)
+{
+	// Determine if input is serial number (14 chars) or broadcast code (15 chars)
+	if (broadcastCode.empty())
+	{
+		throw std::invalid_argument(
+			std::string(__func__) + ": Broadcast code cannot be empty");
+	}
+
+	std::string serialNumber;
+	if (broadcastCode.length() == 14)
+	{
+		// Input is a serial number
+		serialNumber = broadcastCode;
+	} else if (broadcastCode.length() == 15)
+	{
+		// Input is a broadcast code (serial + selector)
+		serialNumber = broadcastCode.substr(0, 14);
+	} else
+	{
+		// Invalid length
+		throw std::invalid_argument(
+			std::string(__func__) +
+			": Broadcast code must be 14 or 15 characters long");
+	}
+
+	// Extract last two digits of serial number
+	if (serialNumber.length() < 2)
+	{
+		throw std::invalid_argument(
+			std::string(__func__) + ": Serial number too short");
+	}
+
+	std::string lastTwoDigits = serialNumber.substr(serialNumber.length() - 2);
+	// Validate that last two characters are digits
+	if (lastTwoDigits[0] < '0' || lastTwoDigits[0] > '9' ||
+		lastTwoDigits[1] < '0' || lastTwoDigits[1] > '9')
+	{
+		throw std::invalid_argument(
+			std::string(__func__) +
+			": Last two characters of serial number must be digits");
+	}
+
+	/**		EXPLANATION:
+	 * Use the device's subnet: X.X.X.1XX where XX = last two digits of serial.
+	 * We use the smoIp and smoSubnetNbits to determine the network prefix.
+	 */
+	// Parse smoIp to extract network prefix
+	auto smoIpOctets = comms::parseIPv4Address(smoIp);
+	if (!smoIpOctets.has_value()) {
+		throw std::invalid_argument(
+			std::string(__func__) + ": Invalid smoIp format: must be X.X.X.X");
+	}
+
+	// Generate subnet mask based on nbits
+	uint32_t subnetMask = getSubnetMaskFor(smoSubnetNbits);
+
+	// Convert smoIp to uint32_t for bitwise operations
+	uint32_t smoIpAddr = (std::stoi(smoIpOctets->octet1) << 24) |
+		(std::stoi(smoIpOctets->octet2) << 16) |
+		(std::stoi(smoIpOctets->octet3) << 8) |
+		std::stoi(smoIpOctets->octet4);
+
+	// Apply subnet mask to get network prefix
+	uint32_t networkPrefix = smoIpAddr & subnetMask;
+
+	// Extract octets from network prefix
+	uint8_t octet1 = (networkPrefix >> 24) & 0xFF;
+	uint8_t octet2 = (networkPrefix >> 16) & 0xFF;
+	uint8_t octet3 = (networkPrefix >> 8) & 0xFF;
+
+	// Use the first three octets and append "1" + last two digits
+	return std::to_string(octet1) + "." + std::to_string(octet2) + "." +
+		std::to_string(octet3) + ".1" + lastTwoDigits;
+}
+
+void Device::startHeartbeat()
+{
+	if (!componentThread || discoveredDevice.ipAddr.empty()) {
+		return; // Can't start heartbeat without component thread or IP
+	}
+
+	// Create heartbeat socket using the component thread's io_service
+	heartbeatSocket = std::make_unique<boost::asio::ip::udp::socket>(
+		componentThread->getIoService());
+	heartbeatSocket->open(boost::asio::ip::udp::v4());
+
+	// Create heartbeat timer
+	heartbeatTimer = std::make_unique<boost::asio::deadline_timer>(
+		componentThread->getIoService());
+
+	heartbeatActive.store(true);
+
+	// Send first heartbeat immediately
+	sendHeartbeat();
+}
+
+void Device::sendHeartbeat()
+{
+	if (!heartbeatActive.load() || !heartbeatSocket
+		|| discoveredDevice.ipAddr.empty())
+	{
+		return;
+	}
+
+	try {
+		// Create heartbeat message using the new HeartbeatMessage type
+		comms::HeartbeatMessage heartbeatMsg;
+
+		heartbeatMsg.swapContentsToProtocolEndianness();
+		heartbeatMsg.header.setCrc16FromRawBytes();
+		heartbeatMsg.header.swapCrc16ToProtocolEndianness();
+		heartbeatMsg.footer.crc_32 = heartbeatMsg.calculateCrc32();
+		heartbeatMsg.footer.swapCrc32ToProtocolEndianness();
+		// Send the heartbeat packet
+		boost::asio::ip::udp::endpoint deviceEndpoint(
+			boost::asio::ip::address::from_string(discoveredDevice.ipAddr), cmdPort);
+
+		heartbeatSocket->send_to(
+			boost::asio::buffer(&heartbeatMsg, sizeof(heartbeatMsg)),
+			deviceEndpoint);
+
+		// Schedule next heartbeat in 1 second
+		heartbeatTimer->expires_from_now(boost::posix_time::seconds(1));
+		heartbeatTimer->async_wait(
+			[this](const boost::system::error_code& error) {
+				onHeartbeatTimer(error);
+			}
+		);
+	}
+	catch (const std::exception& e)
+	{
+		heartbeatActive.store(false);
+		std::cerr << "[" << __func__ << "] Heartbeat send failed for device "
+			<< discoveredDevice.deviceIdentifier
+			<< ": " << e.what() << std::endl;
+	}
+}
+
+void Device::onHeartbeatTimer(const boost::system::error_code& error)
+{
+	// Timer was cancelled, heartbeat stopped
+	if (error == boost::asio::error::operation_aborted) {
+		return;
+	}
+
+	if (error)
+	{
+		heartbeatActive.store(false);
+		std::cerr << "[" << __func__ << "] Heartbeat timer error for device "
+			<< discoveredDevice.deviceIdentifier
+			<< ": " << error.message() << std::endl;
+		return;
+	}
+
+	// Send next heartbeat
+	sendHeartbeat();
+}
+
+uint32_t Device::getSubnetMaskFor(uint8_t nbits)
+{
+	if (nbits > 32) {
+		throw std::invalid_argument(
+			std::string(__func__) + ": nbits must be between 0 and 32");
+	}
+
+	// Generate subnet mask: set the first nbits to 1, rest to 0
+	if (nbits == 0) {
+		return 0x00000000;
+	} else if (nbits == 32) {
+		return 0xFFFFFFFF;
+	} else {
+		// Create mask with nbits set to 1 from the left
+		return (0xFFFFFFFF << (32 - nbits));
+	}
+}
+
+std::optional<std::string> Device::detectSmoIp()
+{
+	try {
+		// Parse the smoIp to get the network prefix
+		auto smoIpOctets = comms::parseIPv4Address(smoIp);
+		if (!smoIpOctets.has_value()) {
+			return std::nullopt;
+		}
+
+		// Convert smoIp octets to integers for bitwise operations
+		uint32_t smoIpAddr = (std::stoi(smoIpOctets->octet1) << 24) |
+			(std::stoi(smoIpOctets->octet2) << 16) |
+			(std::stoi(smoIpOctets->octet3) << 8) |
+			std::stoi(smoIpOctets->octet4);
+
+		// Generate subnet mask based on nbits
+		uint32_t subnetMask = getSubnetMaskFor(smoSubnetNbits);
+
+		// Get all network interfaces using getifaddrs (Linux/Unix specific)
+		// TODO: Add Windows support using GetAdaptersAddresses when porting
+		struct ifaddrs *ifaddr;
+		if (getifaddrs(&ifaddr) == -1) {
+			return std::nullopt;
+		}
+
+		// Use unique_ptr for automatic cleanup (RAII)
+		// This ensures freeifaddrs is called even if we break out of the loop or throw an exception
+		auto ifaddr_deleter = [](struct ifaddrs* ptr) { freeifaddrs(ptr); };
+		std::unique_ptr<struct ifaddrs, decltype(ifaddr_deleter)> ifaddr_ptr(
+			ifaddr, ifaddr_deleter);
+
+		std::string found_ip;
+
+		// Iterate through all network interfaces
+		for (struct ifaddrs *ifa = ifaddr; ifa != nullptr; ifa = ifa->ifa_next)
+		{
+			if (ifa->ifa_addr == nullptr) continue;
+
+			// Check if it's IPv4
+			if (ifa->ifa_addr->sa_family != AF_INET) { continue; }
+
+			// Get the IPv4 address
+			struct sockaddr_in* addr_in = (struct sockaddr_in*)ifa->ifa_addr;
+			char ip_str[INET_ADDRSTRLEN];
+			if (inet_ntop(
+				AF_INET, &addr_in->sin_addr, ip_str, INET_ADDRSTRLEN)
+					== nullptr)
+			{
+				continue;
+			}
+
+			std::string ip = ip_str;
+
+			// Check if this IP is in the same subnet
+			auto ipOctets = comms::parseIPv4Address(ip);
+			if (!ipOctets.has_value()) { continue; }
+
+			// Convert IP octets to integer
+			uint32_t ipAddr = (std::stoi(ipOctets->octet1) << 24) |
+				(std::stoi(ipOctets->octet2) << 16) |
+				(std::stoi(ipOctets->octet3) << 8) |
+				std::stoi(ipOctets->octet4);
+
+			// Check if IP matches the subnet using the calculated mask
+			// Only compare the bits that are set in the subnet mask
+			if ((ipAddr & subnetMask) == (smoIpAddr & subnetMask)) {
+				found_ip = ip;
+				break; // Exit loop, let unique_ptr handle cleanup
+			}
+		}
+
+		// Return the found IP (empty string if none found)
+		if (!found_ip.empty()) {
+			return found_ip;
+		}
+
+		return std::nullopt;
+	} catch (const std::exception& e) {
+		std::cerr << "Error detecting SMO IP: " << e.what() << std::endl;
+		return std::nullopt;
+	}
+}
+
+std::string Device::getSmoIp()
+{
+	// If smo-ip was provided, return it
+	if (!smoIp.empty()) {
+		return smoIp;
+	}
+
+	// Otherwise, try to detect it
+	auto detectedIp = detectSmoIp();
+	if (detectedIp.has_value()) {
+		return detectedIp.value();
+	}
+
+	// If detection failed, throw an exception
+	throw std::runtime_error(
+		std::string(__func__) + ": Failed to detect SMO IP address for smoIp "
+		+ smoIp + " with subnet mask /" + std::to_string(smoSubnetNbits));
+}
+
+} // namespace livoxProto1