salmanoff/commonLibs/livoxProto1/livoxProto1Protocol.cpp

#include <algorithm>
#include <iostream>
#include <iomanip>
#include <cstring>
#include "livoxProto1Protocol.h"

namespace livoxProto1 {
namespace comms {

// Header methods
void Header::swapToHostEndianness()
{
	if (endian::isLittleEndian()) { return; }
	length = __builtin_bswap16(length);
	seq_num = __builtin_bswap16(seq_num);
	crc_16 = __builtin_bswap16(crc_16);
}

bool Header::sanityCheck() const
{
	return (sof == 0xAA) && (version == 1);
}

// Footer methods
void Footer::swapToHostEndianness()
{
	if (endian::isLittleEndian()) { return; }
	crc_32 = __builtin_bswap32(crc_32);
}

bool Footer::sanityCheck() const
{
	/**		FIXME:
	 * Add CRC validation here.
	 */
	return true;
}

// BroadcastMessage methods
void BroadcastMessage::swapToHostEndianness()
{
	if (endian::isLittleEndian()) { return; }
	header.swapToHostEndianness();
	reserved = __builtin_bswap16(reserved);
	footer.swapToHostEndianness();
}

bool BroadcastMessage::sanityCheck() const
{
	return header.sanityCheck() &&
		(cmd_set == 0x00) &&
		(cmd_id == 0x00) &&
		(header.cmd_type == 0x02) &&
		footer.sanityCheck();
}

// 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";
	}
}

BroadcastListener::BroadcastListener(
	const std::shared_ptr<smo::ComponentThread>& componentThread,
	uint16_t listeningPort, uint16_t connectPort
)
: componentThread(componentThread),
listeningPort(listeningPort),
connectPort(connectPort),
deviceGoneAwayCb(nullptr),
socket(componentThread->getIoService()),
listeningEndpoint(boost::asio::ip::udp::v4(), listeningPort),
isListening(false)
{
}

std::shared_ptr<DiscoveredDevice>
BroadcastListener::getDevice(const std::string &deviceIdentifier) const
{
	auto it = std::find_if(discoveredDevices.begin(), discoveredDevices.end(),
		[&deviceIdentifier](const std::shared_ptr<DiscoveredDevice>& device) {
			return device->deviceIdentifier == deviceIdentifier;
		}
	);

	return it != discoveredDevices.end() ? *it : nullptr;
}

void BroadcastListener::broadcastMsgInd(
	const boost::system::error_code& ec, std::size_t bytes_received)
{
	if (ec)
	{
		std::cerr << __func__ << ": Error receiving broadcast message: "
			<< ec.message() << std::endl;
		return;
	}

	if (bytes_received < sizeof(BroadcastMessage))
	{
		std::cerr << "Received packet too small: " << bytes_received
			<< " bytes (expected at least " << sizeof(BroadcastMessage) << ")"
			<< std::endl;
		return;
	}

	// Use placement new to construct BroadcastMessage in the buffer
	BroadcastMessage* msg = new (bcastMsgRecvBuffer) BroadcastMessage;

	if (!msg->sanityCheck())
	{
		std::cerr << "Broadcast message failed sanity check" << std::endl;
		return;
	}

	// Convert from little-endian to host endianness
	msg->swapToHostEndianness();

	// Extract device information
	std::string senderIP = senderEndpoint.address().to_string();
	std::string broadcastCode(reinterpret_cast<const char*>(msg->broadcast_code));

	// Early return if device already exists
	if (deviceExists(broadcastCode)) { return; }

	// Create new DiscoveredDevice using conversion constructor
	auto device = std::make_shared<DiscoveredDevice>(*msg, senderIP);
	discoveredDevices.push_back(device);
	std::cout << "Discovered new Livox device: " << device->stringify()
		<< std::endl;
}

void BroadcastListener::start(void)
{
	if (isListening.load()) { return; }

	try
	{
		/**		EXPLANATION:
		 * Set up a boost::asio udp listening socket on the broadcast listening
		 * port.
		 *
		 *		FIXME:
		 * We should also set up a timer to check for devices that have gone
		 * away.
		 */
		socket.open(boost::asio::ip::udp::v4());
		socket.bind(listeningEndpoint);

		isListening.store(true);
		// Start the first async receive operation
		startReceive();
		std::cout << "BroadcastListener started on port " << listeningPort
			<< std::endl;
	}
	catch (const boost::system::system_error& e)
	{
		isListening.store(false);
		std::cerr << "Failed to start BroadcastListener: " << e.what()
			<< std::endl;
		throw;
	}
}

void BroadcastListener::startReceive(void)
{
	if (!isListening.load()) { return; }

	socket.async_receive_from(
		boost::asio::buffer(bcastMsgRecvBuffer, sizeof(bcastMsgRecvBuffer)),
		senderEndpoint,
		[this](const boost::system::error_code& ec, std::size_t bytes_received)
		{
			broadcastMsgInd(ec, bytes_received);

			// Continue listening for the next packet
			if (isListening.load())
				{ startReceive(); }
		}
	);
}

void BroadcastListener::stop(void)
{
	if (!isListening.load()) { return; }

	isListening.store(false);

	try
	{
		socket.close();
		std::cout << "BroadcastListener stopped" << std::endl;
	}
	catch (const boost::system::system_error& e)
	{
		std::cerr << "Error stopping BroadcastListener: " << e.what()
			<< std::endl;
		throw;
	}
}

} // namespace comms
} // namespace livoxProto1