Added ICMPv6 support to Linux.

2025-03-27 00:15:02 -07:00
parent d41f71f17d
commit 42b26045a6
6 changed files with 293 additions and 41 deletions
--- a/src/io/socket/BaseICMP.cpp
+++ b/src/io/socket/BaseICMP.cpp
@@ -50,7 +50,7 @@ namespace ehs
 		return 0;
 	}

-	UInt_64 BaseICMP::Receive(Str_8 &address, ICMP_Header header, Serializer<UInt_64> &data)
+	UInt_64 BaseICMP::Receive(Str_8 &address, ICMP_Header &header, Serializer<UInt_64> &data)
 	{
 		return 0;
 	}
@@ -65,7 +65,7 @@ namespace ehs
 		}

 		ICMP_Header header = {
-			8,
+			version == IP::V6 ? (UInt_8)128 : (UInt_8)8,
 			0,
 			0
 		};
@@ -85,12 +85,17 @@ namespace ehs
 	{
 	}

+	IP BaseICMP::GetVersion() const
+	{
+		return version;
+	}
+
 	bool BaseICMP::IsValid() const
 	{
 		return false;
 	}

-	UInt_16 BaseICMP::ComputeChecksum(UInt_16 *buffer, Size length)
+	UInt_16 BaseICMP::ComputeChecksumV4(UInt_16 *buffer, Size length)
 	{
 		Size sum = 0;
 		while (length > 1)
--- a/src/io/socket/ICMP_LNX.cpp
+++ b/src/io/socket/ICMP_LNX.cpp
@@ -5,18 +5,30 @@
 #include <arpa/inet.h>
 #include <netinet/in.h>
 #include <netinet/ip.h>
+#include <ifaddrs.h>
+#include <unistd.h>

 namespace ehs
 {
+	ICMP::~ICMP()
+	{
+		if (close(hdl) == -1)
+			EHS_LOG_INT(LogType::ERR, 0, "Failed to close socket.");
+	}
+
 	ICMP::ICMP()
-		: hdl(EHS_INVALID_SOCKET)
+		: hdl(EHS_INVALID_SOCKET), src{}
 	{
 	}

 	ICMP::ICMP(const IP version)
-		: BaseICMP(version)
+		: BaseICMP(version), src{}
 	{
-		hdl = socket(AF_INET, SOCK_RAW, IPPROTO_ICMP);
+		if (version == IP::V6)
+			hdl = socket(AF_INET6, SOCK_RAW, IPPROTO_ICMPV6);
+		else
+			hdl = socket(AF_INET, SOCK_RAW, IPPROTO_ICMP);
+
 		if (hdl < 0)
 		{
 			EHS_LOG_INT(LogType::ERR, 0, "Failed to create ICMP socket with error #" + Str_8::FromNum(errno) + ".");
@@ -27,13 +39,14 @@ namespace ehs
 	}

 	ICMP::ICMP(ICMP &&icmp) noexcept
-		: BaseICMP((BaseICMP &&)icmp), hdl(icmp.hdl)
+		: BaseICMP((BaseICMP &&)icmp), hdl(icmp.hdl), src(icmp.src)
 	{
 		icmp.hdl = EHS_INVALID_SOCKET;
+		icmp.src = {};
 	}

 	ICMP::ICMP(const ICMP &icmp)
-		: BaseICMP(icmp), hdl(icmp.hdl)
+		: BaseICMP(icmp), hdl(icmp.hdl), src{}
 	{
 	}

@@ -45,8 +58,10 @@ namespace ehs
 		BaseICMP::operator=((BaseICMP &&)icmp);

 		hdl = icmp.hdl;
+		src = icmp.src;

 		icmp.hdl = EHS_INVALID_SOCKET;
+		icmp.src = {};

 		return *this;
 	}
@@ -59,11 +74,138 @@ namespace ehs
 		BaseICMP::operator=(icmp);

 		hdl = icmp.hdl;
+		src = {};

 		return *this;
 	}

 	UInt_64 ICMP::Send(const Str_8 &address, ICMP_Header header, const Byte *data, const UInt_64 size)
+	{
+		if (GetVersion() == IP::V6)
+			return SendV6(address, header, data, size);
+
+		return SendV4(address, header, data, size);
+	}
+
+	UInt_64 ICMP::Receive(Str_8 &address, ICMP_Header &header, Serializer<UInt_64> &data)
+	{
+		if (GetVersion() == IP::V6)
+			return ReceiveV6(address, header, data);
+
+		return ReceiveV4(address, header, data);
+	}
+
+	void ICMP::SetReceiveTimeout(const UInt_64 timeout)
+	{
+		timeval result = {};
+		result.tv_sec = (long)timeout;
+		result.tv_usec = 0;
+
+		if (setsockopt(hdl, SOL_SOCKET, SO_RCVTIMEO, &result, sizeof(result)) < 0)
+		{
+			EHS_LOG_INT(LogType::WARN, 0, "Failed to set receive timeout with error #" + Str_8::FromNum(errno) + ".");
+
+			return;
+		}
+
+		EHS_LOG_SUCCESS();
+	}
+
+	bool ICMP::IsValid() const
+	{
+		return hdl != EHS_INVALID_SOCKET;
+	}
+
+	bool ICMP::IsLinkLocal(const in6_addr &addr)
+	{
+		return addr.s6_addr[0] == 0xfe && (addr.s6_addr[1] & 0xc0) == 0x80;
+	}
+
+	sockaddr_in6 ICMP::RetrieveSrcAddress()
+	{
+		ifaddrs *ifaddr;
+		sockaddr_in6 addr = {};
+
+		if (getifaddrs(&ifaddr) == -1)
+		{
+			EHS_LOG_INT(LogType::ERR, 0, "Failed to retrieve public address with error #" + Str_8::FromNum(errno) + ".");
+
+			return addr;
+		}
+
+		for (ifaddrs *ifa = ifaddr; ifa; ifa = ifa->ifa_next)
+		{
+			if (ifa->ifa_addr == nullptr || ifa->ifa_addr->sa_family != AF_INET6)
+				continue;
+
+			addr = *(sockaddr_in6 *)ifa->ifa_addr;
+			if (!addr.sin6_addr.s6_addr32[0] || IsLinkLocal(addr.sin6_addr))
+				continue;
+
+			break;
+		}
+
+		freeifaddrs(ifaddr);
+
+		EHS_LOG_SUCCESS();
+
+		return addr;
+	}
+
+	UInt_32 ICMP::CalculatePseudoHeaderChecksum(const PseudoICMPv6_Header &header)
+	{
+		UInt_32 checksum = 0;
+
+		for (UInt_8 i = 0; i < 16; ++i)
+			checksum += header.src.sin6_addr.s6_addr[i];
+
+		for (UInt_8 i = 0; i < 16; ++i)
+			checksum += header.dst.sin6_addr.s6_addr[i];
+
+		checksum += 58;
+
+		checksum += htons(header.length);
+
+		checksum = (checksum >> 16) + (checksum & 0xFFFF);
+		checksum += (checksum >> 16);
+
+		return checksum;
+	}
+
+	UInt_16 ICMP::ComputeChecksumV6(UInt_16 *buffer, Size length, const sockaddr_in6 &dst)
+	{
+		UInt_32 checksum = 0;
+
+		if (!src.sin6_addr.s6_addr32[0])
+		{
+			src = RetrieveSrcAddress();
+			if (!src.sin6_addr.s6_addr32[0])
+			{
+				EHS_LOG_INT(LogType::ERR, 0, "Could not retrieve a suitable global address.");
+				return checksum;
+			}
+		}
+
+		checksum += CalculatePseudoHeaderChecksum({src, dst, (UInt_32)length});
+
+		while (length > 1)
+		{
+			checksum += *buffer++;
+			length -= sizeof(UInt_16);
+		}
+
+		if (length == 1)
+			checksum += *(UInt_8 *)buffer;
+
+		// Carry over any overflow from the 16-bit result
+		checksum = (checksum >> 16) + (checksum & 0xFFFF);
+		checksum += (checksum >> 16);
+
+		// Return the 16-bit complement
+		return ~checksum;
+	}
+
+	UInt_64 ICMP::SendV6(const Str_8 &address, ICMP_Header header, const Byte *data, UInt_64 size)
 	{
 		if (!IsValid())
 		{
@@ -82,17 +224,17 @@ namespace ehs

 		payload.SetOffset(payload.GetOffset() + size);

-		header.checksum = ComputeChecksum((UInt_16 *)&payload[0], payload.Size());
+		sockaddr_in6 dst = {};
+		dst.sin6_family = AF_INET6;
+		inet_pton(AF_INET6, address, &(dst.sin6_addr));
+
+		header.checksum = ComputeChecksumV6((UInt_16 *)&payload[0], payload.Size(), dst);

 		payload.SetOffset(0);
 		payload.Write(header);
 		payload.SetOffset(payload.Size());

-		sockaddr_in dst_addr = {};
-		dst_addr.sin_family = AF_INET;
-		inet_pton(AF_INET, address, &(dst_addr.sin_addr));
-
-		SInt_64 sent = sendto(hdl, payload, payload.Size(), 0, (sockaddr *)&dst_addr, sizeof(dst_addr));
+		SInt_64 sent = sendto(hdl, payload, payload.Size(), 0, (sockaddr *)&dst, sizeof(sockaddr_in6));
 		if (sent < 0)
 		{
 			EHS_LOG_INT(LogType::ERR, 0, "Failed to send packet with error #" + Str_8::FromNum(errno) + ".");
@@ -105,7 +247,96 @@ namespace ehs
 		return sent;
 	}

-	UInt_64 ICMP::Receive(Str_8 &address, ICMP_Header header, Serializer<UInt_64> &data)
+	UInt_64 ICMP::SendV4(const Str_8 &address, ICMP_Header header, const Byte *data, UInt_64 size)
+	{
+		if (!IsValid())
+		{
+			EHS_LOG_INT(LogType::WARN, 0, "Socket is not initialized.");
+
+			return 0;
+		}
+
+		header.checksum = 0;
+
+		Serializer<UInt_64> payload(Endianness::LE);
+		payload.Write(header);
+		payload.Resize(payload.Size() + size);
+
+		Util::Copy(&payload[payload.GetOffset()], data, size);
+
+		payload.SetOffset(payload.GetOffset() + size);
+
+		header.checksum = ComputeChecksumV4((UInt_16 *)&payload[0], payload.Size());
+
+		payload.SetOffset(0);
+		payload.Write(header);
+		payload.SetOffset(payload.Size());
+
+		sockaddr_in dst = {};
+		dst.sin_family = AF_INET;
+		inet_pton(AF_INET, address, &(dst.sin_addr));
+
+		SInt_64 sent = sendto(hdl, payload, payload.Size(), 0, (sockaddr *)&dst, sizeof(sockaddr_in));
+		if (sent < 0)
+		{
+			EHS_LOG_INT(LogType::ERR, 0, "Failed to send packet with error #" + Str_8::FromNum(errno) + ".");
+
+			return 0;
+		}
+
+		EHS_LOG_SUCCESS();
+
+		return sent;
+	}
+
+	UInt_64 ICMP::ReceiveV6(Str_8 &address, ICMP_Header &header, Serializer<UInt_64> &data) const
+	{
+		if (!IsValid())
+		{
+			EHS_LOG_INT(LogType::WARN, 0, "Socket is not initialized.");
+
+			return 0;
+		}
+
+		Serializer<UInt_64> payload(Endianness::LE);
+		payload.Resize(1500);
+
+		sockaddr_in6 remote = {};
+		socklen_t from_len = sizeof(remote);
+
+		SInt_64 recv = recvfrom(hdl, payload, 1500, 0, (sockaddr *)&remote, &from_len);
+		if (recv < 0)
+		{
+			int code = errno;
+			if (code == EAGAIN)
+				EHS_LOG_SUCCESS();
+			else
+				EHS_LOG_INT(LogType::ERR, 0, "Failed to receive packet with error #" + Str_8::FromNum(code) + ".");
+
+			return 0;
+		}
+
+		payload.Resize(recv);
+
+		char tmpAddr[INET6_ADDRSTRLEN];
+
+		if (!inet_ntop(remote.sin6_family, &remote.sin6_addr, tmpAddr, INET6_ADDRSTRLEN))
+		{
+			EHS_LOG_INT(LogType::ERR, 1, "Failed to convert IPv6 address with error #" + Str_8::FromNum(errno) + ".");
+
+			return recv;
+		}
+
+		address = tmpAddr;
+		header = payload.Read<ICMP_Header>();
+		data = Serializer<UInt_64>(payload.GetEndianness(), &payload[payload.GetOffset()], payload.Size() - payload.GetOffset());
+
+		EHS_LOG_SUCCESS();
+
+		return recv;
+	}
+
+	UInt_64 ICMP::ReceiveV4(Str_8 &address, ICMP_Header &header, Serializer<UInt_64> &data) const
 	{
 		if (!IsValid())
 		{
@@ -152,25 +383,4 @@ namespace ehs

 		return recv;
 	}
-
-	void ICMP::SetReceiveTimeout(UInt_64 timeout)
-	{
-		timeval result = {};
-		result.tv_sec = (long)timeout;
-		result.tv_usec = 0;
-
-		if (setsockopt(hdl, SOL_SOCKET, SO_RCVTIMEO, &result, sizeof(result)) < 0)
-		{
-			EHS_LOG_INT(LogType::WARN, 0, "Failed to set receive timeout with error #" + Str_8::FromNum(errno) + ".");
-
-			return;
-		}
-
-		EHS_LOG_SUCCESS();
-	}
-
-	bool ICMP::IsValid() const
-	{
-		return hdl != EHS_INVALID_SOCKET;
-	}
 }