From 07db3ac66c86a641bcd073d4a4f59b935c165100 Mon Sep 17 00:00:00 2001
From: Yu Watanabe <watanabe.yu+github@gmail.com>
Date: Thu, 22 Feb 2024 16:43:04 +0900
Subject: [PATCH] icmp6-packet: introduce ICMP6Packet and several relevant
 functions

---
 src/libsystemd-network/icmp6-packet.c | 121 ++++++++++++++++++++++++++
 src/libsystemd-network/icmp6-packet.h |  28 ++++++
 src/libsystemd-network/meson.build    |   1 +
 3 files changed, 150 insertions(+)
 create mode 100644 src/libsystemd-network/icmp6-packet.c
 create mode 100644 src/libsystemd-network/icmp6-packet.h

diff --git a/src/libsystemd-network/icmp6-packet.c b/src/libsystemd-network/icmp6-packet.c
new file mode 100644
index 00000000000..a0b0e84f549
--- /dev/null
+++ b/src/libsystemd-network/icmp6-packet.c
@@ -0,0 +1,121 @@
+/* SPDX-License-Identifier: LGPL-2.1-or-later */
+
+#include <netinet/icmp6.h>
+
+#include "icmp6-packet.h"
+#include "icmp6-util.h"
+#include "in-addr-util.h"
+#include "iovec-util.h"
+#include "network-common.h"
+#include "socket-util.h"
+
+DEFINE_TRIVIAL_REF_UNREF_FUNC(ICMP6Packet, icmp6_packet, mfree);
+
+static ICMP6Packet* icmp6_packet_new(size_t size) {
+        ICMP6Packet *p;
+
+        if (size > SIZE_MAX - offsetof(ICMP6Packet, raw_packet))
+                return NULL;
+
+        p = malloc0(offsetof(ICMP6Packet, raw_packet) + size);
+        if (!p)
+                return NULL;
+
+        p->raw_size = size;
+        p->n_ref = 1;
+
+        return p;
+}
+
+int icmp6_packet_get_sender_address(ICMP6Packet *p, struct in6_addr *ret) {
+        assert(p);
+
+        if (in6_addr_is_null(&p->sender_address))
+                return -ENODATA;
+
+        if (ret)
+                *ret = p->sender_address;
+        return 0;
+}
+
+int icmp6_packet_get_timestamp(ICMP6Packet *p, clockid_t clock, usec_t *ret) {
+        assert(p);
+        assert(ret);
+
+        if (!TRIPLE_TIMESTAMP_HAS_CLOCK(clock) || !clock_supported(clock))
+                return -EOPNOTSUPP;
+
+        if (!triple_timestamp_is_set(&p->timestamp))
+                return -ENODATA;
+
+        *ret = triple_timestamp_by_clock(&p->timestamp, clock);
+        return 0;
+}
+
+static const struct icmp6_hdr* icmp6_packet_get_header(ICMP6Packet *p) {
+        assert(p);
+
+        if (p->raw_size < sizeof(struct icmp6_hdr))
+                return NULL;
+
+        return (const struct icmp6_hdr*) p->raw_packet;
+}
+
+int icmp6_packet_get_type(ICMP6Packet *p) {
+        const struct icmp6_hdr *hdr = icmp6_packet_get_header(p);
+        if (!hdr)
+                return -EBADMSG;
+
+        return hdr->icmp6_type;
+}
+
+static int icmp6_packet_verify(ICMP6Packet *p) {
+        const struct icmp6_hdr *hdr = icmp6_packet_get_header(p);
+        if (!hdr)
+                return -EBADMSG;
+
+        if (hdr->icmp6_code != 0)
+                return -EBADMSG;
+
+        return 0;
+}
+
+int icmp6_packet_receive(int fd, ICMP6Packet **ret) {
+        _cleanup_(icmp6_packet_unrefp) ICMP6Packet *p = NULL;
+        ssize_t len;
+        int r;
+
+        assert(fd >= 0);
+        assert(ret);
+
+        len = next_datagram_size_fd(fd);
+        if (len < 0)
+                return (int) len;
+
+        p = icmp6_packet_new(len);
+        if (!p)
+                return -ENOMEM;
+
+        r = icmp6_receive(fd, p->raw_packet, p->raw_size, &p->sender_address, &p->timestamp);
+        if (r < 0)
+                switch (r) {
+                case -EADDRNOTAVAIL:
+                        return log_debug_errno(r, "ICMPv6: Received a packet from neither link-local nor null address.");
+
+                case -EMULTIHOP:
+                        return log_debug_errno(r, "ICMPv6: Received a packet with an invalid hop limit.");
+
+                case -EPFNOSUPPORT:
+                        return log_debug_errno(r, "ICMPv6: Received a packet with an invalid source address.");
+
+                default:
+                        return log_debug_errno(r, "ICMPv6: Unexpected error while receiving a packet: %m");
+                }
+
+        r = icmp6_packet_verify(p);
+        if (r < 0)
+                return r;
+
+        *ret = TAKE_PTR(p);
+        return 0;
+}
diff --git a/src/libsystemd-network/icmp6-packet.h b/src/libsystemd-network/icmp6-packet.h
new file mode 100644
index 00000000000..d77d2e8537a
--- /dev/null
+++ b/src/libsystemd-network/icmp6-packet.h
@@ -0,0 +1,28 @@
+/* SPDX-License-Identifier: LGPL-2.1-or-later */
+#pragma once
+
+#include <inttypes.h>
+#include <netinet/in.h>
+
+#include "macro.h"
+#include "time-util.h"
+
+typedef struct ICMP6Pakcet {
+        unsigned n_ref;
+
+        struct in6_addr sender_address;
+        struct triple_timestamp timestamp;
+
+        size_t raw_size;
+        uint8_t raw_packet[];
+} ICMP6Packet;
+
+ICMP6Packet* icmp6_packet_ref(ICMP6Packet *p);
+ICMP6Packet* icmp6_packet_unref(ICMP6Packet *p);
+DEFINE_TRIVIAL_CLEANUP_FUNC(ICMP6Packet*, icmp6_packet_unref);
+
+int icmp6_packet_get_sender_address(ICMP6Packet *p, struct in6_addr *ret);
+int icmp6_packet_get_timestamp(ICMP6Packet *p, clockid_t clock, usec_t *ret);
+int icmp6_packet_get_type(ICMP6Packet *p);
+
+int icmp6_packet_receive(int fd, ICMP6Packet **ret);
diff --git a/src/libsystemd-network/meson.build b/src/libsystemd-network/meson.build
index 301a4d5c5cb..a2e5106b42d 100644
--- a/src/libsystemd-network/meson.build
+++ b/src/libsystemd-network/meson.build
@@ -8,6 +8,7 @@ sources = files(
         'dhcp6-network.c',
         'dhcp6-option.c',
         'dhcp6-protocol.c',
+        'icmp6-packet.c',
         'icmp6-util.c',
         'lldp-neighbor.c',
         'lldp-network.c',
-- 
2.47.3