XNSim/XNServices/XNUDPService/XNUDPService.cpp

#include "XNUDPService.h"
#include "XNUDPService_p.h"
#include <XNCore/XNServiceManager.h>
#include <XNCore/XNDDSManager.h>
#include <fstream>
#include <sstream>
#include <cstring>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <XNCore/XNByteArray.h>

XN_SERVICE_INITIALIZE(XNUDPService)

XNUDPService::XNUDPService() : XNServiceObject(new XNUDPServicePrivate())
{
}

XNUDPService::~XNUDPService()
{
	T_D();
	if (d->udpSocket >= 0) {
		close(d->udpSocket);
		d->udpSocket = -1;
	}
};

XNUDPService::XNUDPService(PrivateType *p) : XNServiceObject(p)
{
}

void XNUDPService::Initialize()
{
	XNServiceObject::Initialize();
	T_D();
	// 读取配置文件
	std::ifstream file(GetXmlPath());
	if (!file.is_open()) {
		LOG_WARNING("Failed to open config file:%s", GetXmlPath().c_str());
		return;
	}

	std::stringstream buffer;
	buffer << file.rdbuf();
	std::string content = buffer.str();
	file.close();

	// 简单的XML解析
	size_t udpPos = content.find("<UDP>");
	if (udpPos != std::string::npos) {
		size_t localPortPos	 = content.find("<LocalPort>", udpPos);
		size_t targetHostPos = content.find("<TargetHost>", udpPos);
		size_t targetPortPos = content.find("<TargetPort>", udpPos);

		if (localPortPos != std::string::npos) {
			size_t endPos = content.find("</LocalPort>", localPortPos);
			d->localPort = std::stoi(content.substr(localPortPos + 11, endPos - localPortPos - 11));
		}
		if (targetHostPos != std::string::npos) {
			size_t endPos = content.find("</TargetHost>", targetHostPos);
			d->targetHost = content.substr(targetHostPos + 12, endPos - targetHostPos - 12);
		}
		if (targetPortPos != std::string::npos) {
			size_t endPos = content.find("</TargetPort>", targetPortPos);
			d->targetPort =
				std::stoi(content.substr(targetPortPos + 12, endPos - targetPortPos - 12));
		}
	} else {
		LOG_WARNING("UDP configuration not found, using default values");
		d->localPort  = 12345;
		d->targetHost = "127.0.0.1";
		d->targetPort = 54321;
	}
}

void XNUDPService::PrepareForExecute()
{
	XNServiceObject::PrepareForExecute();
	T_D();
	// 初始化UDP socket
	d->udpSocket = socket(AF_INET, SOCK_DGRAM, 0);
	if (d->udpSocket < 0) {
		LOG_WARNING("Failed to create UDP socket");
		return;
	}

	// 设置非阻塞模式
	int flags = fcntl(d->udpSocket, F_GETFL, 0);
	fcntl(d->udpSocket, F_SETFL, flags | O_NONBLOCK);

	// 绑定本地端口
	struct sockaddr_in localAddr;
	memset(&localAddr, 0, sizeof(localAddr));
	localAddr.sin_family	  = AF_INET;
	localAddr.sin_addr.s_addr = INADDR_ANY;
	localAddr.sin_port		  = htons(d->localPort);

	if (bind(d->udpSocket, (struct sockaddr *)&localAddr, sizeof(localAddr)) < 0) {
		LOG_WARNING("UDP socket bind failed on port:%d", d->localPort);
		close(d->udpSocket);
		d->udpSocket = -1;
		return;
	}

	RegisterRTEventHandler("SendUDPData",
						   std::bind(&XNUDPService::SendData, this, std::placeholders::_1));
}

void XNUDPService::HandleIncomingData()
{
	T_D();
	if (d->udpSocket < 0)
		return;

	char buffer[65536];
	struct sockaddr_in senderAddr;
	socklen_t senderLen = sizeof(senderAddr);

	while (true) {
		ssize_t bytesRead = recvfrom(d->udpSocket, buffer, sizeof(buffer), 0,
									 (struct sockaddr *)&senderAddr, &senderLen);

		if (bytesRead <= 0) {
			if (errno == EAGAIN || errno == EWOULDBLOCK) {
				break; // 没有更多数据
			}
			LOG_WARNING("Error reading from UDP socket");
			break;
		}

		if (bytesRead <= 6) {
			LOG_WARNING("Invalid size of UDP datagram received, size:%zd", bytesRead);
			continue;
		}

		// 解析数据包头
		uint8_t header	 = buffer[0];
		uint8_t type	 = buffer[1];
		uint8_t subType1 = buffer[2];
		uint8_t subType2 = buffer[3];
		uint16_t size	 = (buffer[4] << 8) | buffer[5];

		if (header != 0x0b) {
			LOG_WARNING("Invalid header of UDP datagram received, header:%d", header);
			continue;
		}

		if (bytesRead < size) {
			LOG_WARNING("Invalid size of UDP datagram received, size:%zd, expected:%d", bytesRead,
						size);
			continue;
		}

		std::vector<uint8_t> datagram(buffer, buffer + bytesRead);

		if (type == 0x04) {
			if (subType1 == 0x00 && subType2 == 0x00) {
				TriggerRTEvent("ATA04AeroInput", datagram);
				continue;
			} else if (subType1 == 0x01 && subType2 == 0x00) {
				TriggerRTEvent("ATA04GhInput", datagram);
				continue;
			} else if (subType1 == 0x02 && subType2 == 0x00) {
				TriggerRTEvent("ATA04WbInput", datagram);
				continue;
			}
		}
		TriggerRTEvent("ReceiveUDPData", datagram);
	}
}

void XNUDPService::SendData(const std::any &data)
{
	T_D();
	if (d->udpSocket < 0) {
		LOG_WARNING("UDP socket not initialized");
		return;
	}

	struct sockaddr_in targetAddr;
	memset(&targetAddr, 0, sizeof(targetAddr));
	targetAddr.sin_family = AF_INET;
	targetAddr.sin_port	  = htons(d->targetPort);
	inet_pton(AF_INET, d->targetHost.c_str(), &targetAddr.sin_addr);

	XNByteArray packet = std::any_cast<XNByteArray>(data);
	ssize_t bytesSent  = sendto(d->udpSocket, packet.data(), packet.size(), 0,
								(struct sockaddr *)&targetAddr, sizeof(targetAddr));

	if (bytesSent < 0) {
		LOG_WARNING("Failed to send UDP datagram: %s", strerror(errno));
	}
}