sd-ndisc: introduce ndisc_option_parse() helper function

2 files changed, 46 insertions, 0 deletions

diff --git a/src/libsystemd-network/ndisc-protocol.c b/src/libsystemd-network/ndisc-protocol.c
index fae4a583ad..39dcdcdfd8 100644
--- a/src/libsystemd-network/ndisc-protocol.c
+++ b/src/libsystemd-network/ndisc-protocol.c
@@ -1,7 +1,45 @@
 /* SPDX-License-Identifier: LGPL-2.1-or-later */
 
+#include <netinet/icmp6.h>
+
 #include "ndisc-protocol.h"
 
+int ndisc_option_parse(
+                ICMP6Packet *p,
+                size_t offset,
+                uint8_t *ret_type,
+                size_t *ret_len,
+                const uint8_t **ret_opt) {
+
+        assert(p);
+
+        if (offset == p->raw_size)
+                return -ESPIPE; /* end of the packet */
+
+        if (offset > p->raw_size)
+                return -EBADMSG;
+
+        if (p->raw_size - offset < sizeof(struct nd_opt_hdr))
+                return -EBADMSG;
+
+        const struct nd_opt_hdr *hdr = (const struct nd_opt_hdr*) (p->raw_packet + offset);
+        if (hdr->nd_opt_len == 0)
+                return -EBADMSG;
+
+        size_t len = hdr->nd_opt_len * 8;
+        if (p->raw_size - offset < len)
+                return -EBADMSG;
+
+        if (ret_type)
+                *ret_type = hdr->nd_opt_type;
+        if (ret_len)
+                *ret_len = len;
+        if (ret_opt)
+                *ret_opt = p->raw_packet + offset;
+
+        return 0;
+}
+
 static const uint8_t prefix_length_code_to_prefix_length[_PREFIX_LENGTH_CODE_MAX] = {
         [PREFIX_LENGTH_CODE_96] = 96,
         [PREFIX_LENGTH_CODE_64] = 64,
diff --git a/src/libsystemd-network/ndisc-protocol.h b/src/libsystemd-network/ndisc-protocol.h
index 8e403e3425..94842524ee 100644
--- a/src/libsystemd-network/ndisc-protocol.h
+++ b/src/libsystemd-network/ndisc-protocol.h
@@ -1,6 +1,7 @@
 /* SPDX-License-Identifier: LGPL-2.1-or-later */
 #pragma once
 
+#include "icmp6-packet.h"
 #include "time-util.h"
 
 /* RFC 8781: PREF64 or (NAT64 prefix) */
@@ -29,3 +30,10 @@ struct nd_opt_prefix64_info {
 
 int pref64_plc_to_prefix_length(uint16_t plc, uint8_t *ret);
 int pref64_prefix_length_to_plc(uint8_t prefixlen, uint8_t *ret);
+
+int ndisc_option_parse(
+                ICMP6Packet *p,
+                size_t offset,
+                uint8_t *ret_type,
+                size_t *ret_len,
+                const uint8_t **ret_opt);