From 320ae462b1315564e0ab97f1d1b13e6e05b489af Mon Sep 17 00:00:00 2001 From: Yu Watanabe Date: Tue, 21 Apr 2026 05:05:32 +0900 Subject: [PATCH] tlv-util: introduce tlv-util that handles Tag-Length-Value data format In many network protocols e.g. DHCP, the TLV format is used. Let's introduce a simple parser and builder of the data format. --- src/libsystemd-network/meson.build | 4 + src/libsystemd-network/test-tlv-util.c | 207 ++++++++++ src/libsystemd-network/tlv-util.c | 505 +++++++++++++++++++++++++ src/libsystemd-network/tlv-util.h | 82 ++++ 4 files changed, 798 insertions(+) create mode 100644 src/libsystemd-network/test-tlv-util.c create mode 100644 src/libsystemd-network/tlv-util.c create mode 100644 src/libsystemd-network/tlv-util.h diff --git a/src/libsystemd-network/meson.build b/src/libsystemd-network/meson.build index b0443c36952..6239056e3b4 100644 --- a/src/libsystemd-network/meson.build +++ b/src/libsystemd-network/meson.build @@ -37,6 +37,7 @@ libsystemd_network_sources = files( 'sd-ndisc-router.c', 'sd-ndisc-router-solicit.c', 'sd-radv.c', + 'tlv-util.c', ) sources += libsystemd_network_sources @@ -113,6 +114,9 @@ executables += [ network_test_template + { 'sources' : files('test-sd-dhcp-lease.c'), }, + network_test_template + { + 'sources' : files('test-tlv-util.c'), + }, network_fuzz_template + { 'sources' : files('fuzz-dhcp-client.c'), }, diff --git a/src/libsystemd-network/test-tlv-util.c b/src/libsystemd-network/test-tlv-util.c new file mode 100644 index 00000000000..4747afa98df --- /dev/null +++ b/src/libsystemd-network/test-tlv-util.c @@ -0,0 +1,207 @@ +/* SPDX-License-Identifier: LGPL-2.1-or-later */ + +#include "sd-dhcp-protocol.h" + +#include "hashmap.h" +#include "iovec-util.h" +#include "iovec-wrapper.h" +#include "random-util.h" +#include "tests.h" +#include "tlv-util.h" + +TEST(tlv_constant) { + ASSERT_EQ(TLV_TAG_PAD, (uint32_t) SD_DHCP_OPTION_PAD); + ASSERT_EQ(TLV_TAG_END, (uint32_t) SD_DHCP_OPTION_END); +} + +TEST(tlv) { + _cleanup_(tlv_done) TLV tlv = TLV_INIT(TLV_DHCP4); + + _cleanup_(iovec_done) struct iovec data0 = {}, data1 = {}, data2a = {}, data2b = {}, data3 = {}, data4 = {}; + ASSERT_OK(random_bytes_allocate_iovec(0, &data0)); + ASSERT_OK(random_bytes_allocate_iovec(111, &data1)); + ASSERT_OK(random_bytes_allocate_iovec(123, &data2a)); + ASSERT_OK(random_bytes_allocate_iovec(321, &data2b)); + ASSERT_OK(random_bytes_allocate_iovec(333, &data3)); + ASSERT_OK(random_bytes_allocate_iovec(444, &data4)); + + /* tlv_append() */ + ASSERT_OK(tlv_append(&tlv, 10, data0.iov_len, data0.iov_base)); + ASSERT_OK(tlv_append(&tlv, 11, data1.iov_len, data1.iov_base)); + ASSERT_OK(tlv_append(&tlv, 22, data2a.iov_len, data2a.iov_base)); + ASSERT_OK(tlv_append(&tlv, 22, data2b.iov_len, data2b.iov_base)); + ASSERT_OK(tlv_append(&tlv, 33, data3.iov_len, data3.iov_base)); + ASSERT_OK(tlv_append(&tlv, 44, data4.iov_len, data4.iov_base)); + ASSERT_ERROR(tlv_append(&tlv, 0x00, data4.iov_len, data4.iov_base), EINVAL); + ASSERT_ERROR(tlv_append(&tlv, 0xFF, data4.iov_len, data4.iov_base), EINVAL); + ASSERT_EQ(hashmap_size(tlv.entries), 5u); + + /* tlv_remove() */ + tlv_remove(&tlv, 44); + ASSERT_EQ(hashmap_size(tlv.entries), 4u); + tlv_remove(&tlv, 55); + ASSERT_EQ(hashmap_size(tlv.entries), 4u); + + /* tlv_append_tlv() */ + _cleanup_(tlv_done) TLV tlv_copy = TLV_INIT(TLV_DHCP4); + ASSERT_ERROR(tlv_append_tlv(&tlv_copy, &tlv_copy), EINVAL); + ASSERT_OK(tlv_append_tlv(&tlv_copy, NULL)); + ASSERT_OK(tlv_append_tlv(&tlv_copy, &tlv)); + ASSERT_EQ(hashmap_size(tlv_copy.entries), hashmap_size(tlv.entries)); + + /* tlv_isempty() */ + ASSERT_TRUE(tlv_isempty(NULL)); + ASSERT_TRUE(tlv_isempty(&TLV_INIT(TLV_DHCP4))); + ASSERT_FALSE(tlv_isempty(&tlv)); + + /* tlv_contains() */ + ASSERT_TRUE(tlv_contains(&tlv, 10)); + ASSERT_TRUE(tlv_contains(&tlv, 11)); + ASSERT_TRUE(tlv_contains(&tlv, 22)); + ASSERT_TRUE(tlv_contains(&tlv, 33)); + ASSERT_FALSE(tlv_contains(&tlv, 44)); + + /* tlv_get_all() */ + struct iovec_wrapper *iovw; + + iovw = ASSERT_NOT_NULL(tlv_get_all(&tlv, 10)); + ASSERT_EQ(iovw->count, 1u); + ASSERT_TRUE(iovec_equal(&iovw->iovec[0], &data0)); + + iovw = ASSERT_NOT_NULL(tlv_get_all(&tlv, 11)); + ASSERT_EQ(iovw->count, 1u); + ASSERT_TRUE(iovec_equal(&iovw->iovec[0], &data1)); + + iovw = ASSERT_NOT_NULL(tlv_get_all(&tlv, 22)); + ASSERT_EQ(iovw->count, 3u); + ASSERT_TRUE(iovec_equal(&iovw->iovec[0], &data2a)); + ASSERT_TRUE(iovec_equal(&iovw->iovec[1], &IOVEC_MAKE(data2b.iov_base, UINT8_MAX))); + ASSERT_TRUE(iovec_equal(&iovw->iovec[2], &IOVEC_SHIFT(&data2b, UINT8_MAX))); + + iovw = ASSERT_NOT_NULL(tlv_get_all(&tlv, 33)); + ASSERT_EQ(iovw->count, 2u); + ASSERT_TRUE(iovec_equal(&iovw->iovec[0], &IOVEC_MAKE(data3.iov_base, UINT8_MAX))); + ASSERT_TRUE(iovec_equal(&iovw->iovec[1], &IOVEC_SHIFT(&data3, UINT8_MAX))); + + ASSERT_NULL(tlv_get_all(&tlv, 44)); + + /* tlv_get_full() */ + struct iovec iov; + + ASSERT_OK(tlv_get(&tlv, 10, &iov)); + ASSERT_TRUE(iovec_equal(&iov, &data0)); + ASSERT_OK(tlv_get_full(&tlv, 10, data0.iov_len, &iov)); + ASSERT_TRUE(iovec_equal(&iov, &data0)); + ASSERT_ERROR(tlv_get_full(&tlv, 10, 123, &iov), ENODATA); + + ASSERT_OK(tlv_get(&tlv, 11, &iov)); + ASSERT_TRUE(iovec_equal(&iov, &data1)); + ASSERT_OK(tlv_get_full(&tlv, 11, data1.iov_len, &iov)); + ASSERT_TRUE(iovec_equal(&iov, &data1)); + ASSERT_ERROR(tlv_get_full(&tlv, 11, 123, &iov), ENODATA); + + ASSERT_OK(tlv_get(&tlv, 22, &iov)); + ASSERT_TRUE(iovec_equal(&iov, &data2a)); + ASSERT_OK(tlv_get_full(&tlv, 22, data2a.iov_len, &iov)); + ASSERT_TRUE(iovec_equal(&iov, &data2a)); + ASSERT_ERROR(tlv_get_full(&tlv, 22, data2b.iov_len, &iov), ENODATA); + ASSERT_OK(tlv_get_full(&tlv, 22, UINT8_MAX, &iov)); + ASSERT_TRUE(iovec_equal(&iov, &IOVEC_MAKE(data2b.iov_base, UINT8_MAX))); + ASSERT_OK(tlv_get_full(&tlv, 22, data2b.iov_len - UINT8_MAX, &iov)); + ASSERT_TRUE(iovec_equal(&iov, &IOVEC_SHIFT(&data2b, UINT8_MAX))); + + ASSERT_OK(tlv_get(&tlv, 33, &iov)); + ASSERT_TRUE(iovec_equal(&iov, &IOVEC_MAKE(data3.iov_base, UINT8_MAX))); + ASSERT_ERROR(tlv_get_full(&tlv, 33, data3.iov_len, &iov), ENODATA); + ASSERT_OK(tlv_get_full(&tlv, 33, UINT8_MAX, &iov)); + ASSERT_TRUE(iovec_equal(&iov, &IOVEC_MAKE(data3.iov_base, UINT8_MAX))); + ASSERT_OK(tlv_get_full(&tlv, 33, data3.iov_len - UINT8_MAX, &iov)); + ASSERT_TRUE(iovec_equal(&iov, &IOVEC_SHIFT(&data3, UINT8_MAX))); + + ASSERT_ERROR(tlv_get(&tlv, 44, NULL), ENODATA); + + /* tlv_get_alloc() */ + _cleanup_(iovec_done) struct iovec v = {}; + + ASSERT_OK(tlv_get_alloc(&tlv, 10, &v)); + ASSERT_TRUE(iovec_equal(&v, &data0)); + iovec_done(&v); + + ASSERT_OK(tlv_get_alloc(&tlv, 11, &v)); + ASSERT_TRUE(iovec_equal(&v, &data1)); + iovec_done(&v); + + ASSERT_OK(tlv_get_alloc(&tlv, 22, &v)); + ASSERT_EQ(v.iov_len, data2a.iov_len + data2b.iov_len); + ASSERT_EQ(memcmp(v.iov_base, data2a.iov_base, data2a.iov_len), 0); + ASSERT_EQ(memcmp((uint8_t*) v.iov_base + data2a.iov_len, data2b.iov_base, data2b.iov_len), 0); + iovec_done(&v); + + ASSERT_OK(tlv_get_alloc(&tlv, 33, &v)); + ASSERT_TRUE(iovec_equal(&v, &data3)); + iovec_done(&v); + + ASSERT_ERROR(tlv_get_alloc(&tlv, 44, NULL), ENODATA); + + /* tlv_size() */ + size_t sz = tlv_size(&tlv); + /* The tlv contains the 7 entries with a 2-byte header: + * tag 10: 1 entry, tag 11: 1 entry, tag 22: 3 entries, tag 33: 2 entries = 7 entries total. */ + ASSERT_EQ(sz, 7 * 2 + data0.iov_len + data1.iov_len + data2a.iov_len + data2b.iov_len + data3.iov_len + 1); + + /* tlv_build() */ + ASSERT_OK(tlv_build(&tlv, &v)); + ASSERT_EQ(v.iov_len, sz); + uint8_t *p = v.iov_base; + ASSERT_EQ(*p++, 10u); + ASSERT_EQ(*p++, data0.iov_len); + + ASSERT_EQ(*p++, 11u); + ASSERT_EQ(*p++, data1.iov_len); + ASSERT_EQ(memcmp(p, data1.iov_base, data1.iov_len), 0); + p += data1.iov_len; + + ASSERT_EQ(*p++, 22u); + ASSERT_EQ(*p++, data2a.iov_len); + ASSERT_EQ(memcmp(p, data2a.iov_base, data2a.iov_len), 0); + p += data2a.iov_len; + + ASSERT_EQ(*p++, 22u); + ASSERT_EQ(*p++, UINT8_MAX); + ASSERT_EQ(memcmp(p, data2b.iov_base, UINT8_MAX), 0); + p += UINT8_MAX; + + ASSERT_EQ(*p++, 22u); + ASSERT_EQ(*p++, data2b.iov_len - UINT8_MAX); + ASSERT_EQ(memcmp(p, (uint8_t*) data2b.iov_base + UINT8_MAX, data2b.iov_len - UINT8_MAX), 0); + p += data2b.iov_len - UINT8_MAX; + + ASSERT_EQ(*p++, 33u); + ASSERT_EQ(*p++, UINT8_MAX); + ASSERT_EQ(memcmp(p, data3.iov_base, UINT8_MAX), 0); + p += UINT8_MAX; + + ASSERT_EQ(*p++, 33u); + ASSERT_EQ(*p++, data3.iov_len - UINT8_MAX); + ASSERT_EQ(memcmp(p, (uint8_t*) data3.iov_base + UINT8_MAX, data3.iov_len - UINT8_MAX), 0); + p += data3.iov_len - UINT8_MAX; + + ASSERT_EQ(*p, 255u); + + /* tlv_new() and tlv_parse() */ + _cleanup_(tlv_unrefp) TLV *tlv2 = ASSERT_NOT_NULL(tlv_new(TLV_DHCP4 | TLV_TEMPORARY)); + ASSERT_OK(tlv_parse(tlv2, &v)); + ASSERT_EQ(hashmap_size(tlv.entries), hashmap_size(tlv2->entries)); + void *tagp; + HASHMAP_FOREACH_KEY(iovw, tagp, tlv.entries) { + struct iovec_wrapper *iovw2 = ASSERT_PTR(hashmap_get(tlv2->entries, tagp)); + ASSERT_TRUE(iovw_equal(iovw, iovw2)); + } + + /* tlv_build() again, and check the reproducibility. */ + _cleanup_(iovec_done) struct iovec v2 = {}; + ASSERT_OK(tlv_build(tlv2, &v2)); + ASSERT_TRUE(iovec_equal(&v, &v2)); +} + +DEFINE_TEST_MAIN(LOG_DEBUG); diff --git a/src/libsystemd-network/tlv-util.c b/src/libsystemd-network/tlv-util.c new file mode 100644 index 00000000000..68574e718c7 --- /dev/null +++ b/src/libsystemd-network/tlv-util.c @@ -0,0 +1,505 @@ +/* SPDX-License-Identifier: LGPL-2.1-or-later */ + +#include "alloc-util.h" +#include "hashmap.h" +#include "iovec-util.h" +#include "iovec-wrapper.h" +#include "tlv-util.h" +#include "unaligned.h" + +#define TLV_MAX_ENTRIES 4096u + +TLVFlag tlv_flags_verify(TLVFlag flags) { + assert(IN_SET(flags & _TLV_TAG_MASK, TLV_TAG_U8, TLV_TAG_U16, TLV_TAG_U32)); + assert(IN_SET(flags & _TLV_LENGTH_MASK, TLV_LENGTH_U8, TLV_LENGTH_U16, TLV_LENGTH_U32)); + + /* TLV_PAD and TLV_END are for DHCPv4 options, hence here we assume TLV_TAG_U8 is set. */ + assert(!FLAGS_SET(flags, TLV_PAD) || FLAGS_SET(flags, TLV_TAG_U8)); + assert(!FLAGS_SET(flags, TLV_END) || FLAGS_SET(flags, TLV_TAG_U8)); + + /* When we requested to append the END tag, then we should understand the END tag on parse. */ + assert(!FLAGS_SET(flags, TLV_APPEND_END) || FLAGS_SET(flags, TLV_END)); + + return flags; +} + +void tlv_done(TLV *tlv) { + assert(tlv); + + tlv->entries = hashmap_free(tlv->entries); + tlv->n_entries = 0; +} + +static TLV* tlv_free(TLV *tlv) { + if (!tlv) + return NULL; + + tlv_done(tlv); + return mfree(tlv); +} + +DEFINE_TRIVIAL_REF_UNREF_FUNC(TLV, tlv, tlv_free); + +TLV* tlv_new(TLVFlag flags) { + TLV *tlv = new(TLV, 1); + if (!tlv) + return NULL; + + *tlv = TLV_INIT(flags); + return tlv; +} + +bool tlv_isempty(const TLV *tlv) { + return !tlv || hashmap_isempty(tlv->entries); +} + +struct iovec_wrapper* tlv_get_all(const TLV *tlv, uint32_t tag) { + assert(tlv); + return hashmap_get(tlv->entries, UINT32_TO_PTR(tag)); +} + +int tlv_get_full(const TLV *tlv, uint32_t tag, size_t length, struct iovec *ret) { + assert(tlv); + + /* Do not free the result iovec, the data is still owned by TLV (or the original input data when + * TLV_TEMPORARY is set). */ + + struct iovec_wrapper *iovw = tlv_get_all(tlv, tag); + if (iovw_isempty(iovw)) + return -ENODATA; + + /* When multiple entries exist, use the first one matching the length. */ + FOREACH_ARRAY(iov, iovw->iovec, iovw->count) { + if (length != SIZE_MAX && iov->iov_len != length) + continue; + + if (ret) + *ret = *iov; + return 0; + } + + return -ENODATA; +} + +int tlv_get_alloc(const TLV *tlv, uint32_t tag, struct iovec *ret) { + assert(tlv); + + /* Free the result iovec. */ + + struct iovec_wrapper *iovw = tlv_get_all(tlv, tag); + if (iovw_isempty(iovw)) + return -ENODATA; + + if (!ret) + return 0; + + if (FLAGS_SET(tlv->flags, TLV_MERGE)) + return iovw_concat(iovw, ret); + + /* When TLV_MERGE is unset, provides the first entry. */ + if (!iovec_memdup(&iovw->iovec[0], ret)) + return -ENOMEM; + + return 0; +} + +void tlv_remove(TLV *tlv, uint32_t tag) { + assert(tlv); + + struct iovec_wrapper *iovw = hashmap_remove(tlv->entries, UINT32_TO_PTR(tag)); + if (!iovw) + return; + + assert(tlv->n_entries >= iovw->count); + tlv->n_entries -= iovw->count; + + if (FLAGS_SET(tlv->flags, TLV_TEMPORARY)) + iovw_free(iovw); + else + iovw_free_free(iovw); +} + +DEFINE_PRIVATE_HASH_OPS_WITH_VALUE_DESTRUCTOR( + tlv_hash_ops, + void, + trivial_hash_func, + trivial_compare_func, + struct iovec_wrapper, + iovw_free); + +DEFINE_PRIVATE_HASH_OPS_WITH_VALUE_DESTRUCTOR( + tlv_hash_ops_free, + void, + trivial_hash_func, + trivial_compare_func, + struct iovec_wrapper, + iovw_free_free); + +static int tlv_append_impl(TLV *tlv, uint32_t tag, size_t length, const void *data) { + int r; + + assert(tlv); + assert(length == 0 || data); + + if (tlv->n_entries >= TLV_MAX_ENTRIES) + return -E2BIG; + + if (FLAGS_SET(tlv->flags, TLV_TEMPORARY)) { + struct iovec_wrapper *e = tlv_get_all(tlv, tag); + if (e) { + r = iovw_put_full(e, /* accept_zero= */ true, (void*) data, length); + if (r < 0) + return r; + } else { + _cleanup_(iovw_freep) struct iovec_wrapper *v = new0(struct iovec_wrapper, 1); + if (!v) + return -ENOMEM; + + r = iovw_put_full(v, /* accept_zero= */ true, (void*) data, length); + if (r < 0) + return r; + + r = hashmap_ensure_put(&tlv->entries, &tlv_hash_ops, UINT32_TO_PTR(tag), v); + if (r < 0) + return r; + + TAKE_PTR(v); + } + } else { + struct iovec_wrapper *e = tlv_get_all(tlv, tag); + if (e) { + r = iovw_extend_full(e, /* accept_zero= */ true, data, length); + if (r < 0) + return r; + } else { + _cleanup_(iovw_free_freep) struct iovec_wrapper *v = new0(struct iovec_wrapper, 1); + if (!v) + return -ENOMEM; + + r = iovw_extend_full(v, /* accept_zero= */ true, data, length); + if (r < 0) + return r; + + r = hashmap_ensure_put(&tlv->entries, &tlv_hash_ops_free, UINT32_TO_PTR(tag), v); + if (r < 0) + return r; + + TAKE_PTR(v); + } + } + + tlv->n_entries++; + return 0; +} + +int tlv_append(TLV *tlv, uint32_t tag, size_t length, const void *data) { + int r; + + assert(tlv); + assert(length == 0 || data); + + switch (tlv->flags & _TLV_TAG_MASK) { + case TLV_TAG_U8: + if (tag > UINT8_MAX) + return -EINVAL; + break; + case TLV_TAG_U16: + if (tag > UINT16_MAX) + return -EINVAL; + break; + case TLV_TAG_U32: + break; + default: + assert_not_reached(); + } + + if ((FLAGS_SET(tlv->flags, TLV_PAD) && tag == TLV_TAG_PAD) || + (FLAGS_SET(tlv->flags, TLV_END) && tag == TLV_TAG_END)) + return -EINVAL; + + size_t max_length; + switch (tlv->flags & _TLV_LENGTH_MASK) { + case TLV_LENGTH_U8: + max_length = UINT8_MAX; + break; + case TLV_LENGTH_U16: + max_length = UINT16_MAX; + break; + case TLV_LENGTH_U32: + max_length = UINT32_MAX; + break; + default: + assert_not_reached(); + } + + if (FLAGS_SET(tlv->flags, TLV_MERGE)) { + /* If TLV_MERGE is set and the length is larger than the allowed maximum, then split the data + * and store them in multiple entries. + * + * Note, if tlv_append_impl() fails below, we do not rollback the entries, hence the caller + * of this function needs to discard the entire data in that case. */ + const uint8_t *p = data; + while (length > max_length) { + r = tlv_append_impl(tlv, tag, max_length, p); + if (r < 0) + return r; + + p += max_length; + length -= max_length; + } + + return tlv_append_impl(tlv, tag, length, p); + } + + /* Otherwise, refuse too long data. */ + if (length > max_length) + return -EINVAL; + + return tlv_append_impl(tlv, tag, length, data); +} + +int tlv_append_iov(TLV *tlv, uint32_t tag, const struct iovec *iov) { + assert(tlv); + assert(iovec_is_valid(iov)); + + return tlv_append(tlv, tag, iov ? iov->iov_len : 0, iov ? iov->iov_base : NULL); +} + +int tlv_append_tlv(TLV *tlv, const TLV *source) { + int r; + + assert(tlv); + + /* Note, this does not rollback entries on failure, hence the caller of this function needs to + * discard the entire data in that case. */ + + if (!source) + return 0; + + if (source == tlv) + return -EINVAL; + + void *tagp; + struct iovec_wrapper *iovw; + HASHMAP_FOREACH_KEY(iovw, tagp, source->entries) { + uint32_t tag = PTR_TO_UINT32(tagp); + + FOREACH_ARRAY(iov, iovw->iovec, iovw->count) { + r = tlv_append(tlv, tag, iov->iov_len, iov->iov_base); + if (r < 0) + return r; + } + } + + return 0; +} + +int tlv_parse(TLV *tlv, const struct iovec *iov) { + int r; + + assert(tlv); + assert(iovec_is_valid(iov)); + + /* Note, this does not rollback entries on failure, hence the caller of this function needs to + * discard the entire data in that case. */ + + if (!iovec_is_set(iov)) + return 0; + + for (struct iovec i = *iov; iovec_is_set(&i); ) { + uint32_t tag; + switch (tlv->flags & _TLV_TAG_MASK) { + case TLV_TAG_U8: + if (i.iov_len < sizeof(uint8_t)) + return -EBADMSG; + tag = *(uint8_t*) i.iov_base; + iovec_inc(&i, sizeof(uint8_t)); + break; + case TLV_TAG_U16: + if (i.iov_len < sizeof(uint16_t)) + return -EBADMSG; + tag = unaligned_read_be16(i.iov_base); + iovec_inc(&i, sizeof(uint16_t)); + break; + case TLV_TAG_U32: + if (i.iov_len < sizeof(uint32_t)) + return -EBADMSG; + tag = unaligned_read_be32(i.iov_base); + iovec_inc(&i, sizeof(uint32_t)); + break; + default: + assert_not_reached(); + } + + if (FLAGS_SET(tlv->flags, TLV_PAD) && tag == TLV_TAG_PAD) + continue; + if (FLAGS_SET(tlv->flags, TLV_END) && tag == TLV_TAG_END) + break; + + size_t len; + switch (tlv->flags & _TLV_LENGTH_MASK) { + case TLV_LENGTH_U8: + if (i.iov_len < sizeof(uint8_t)) + return -EBADMSG; + len = *(uint8_t*) i.iov_base; + iovec_inc(&i, sizeof(uint8_t)); + break; + case TLV_LENGTH_U16: + if (i.iov_len < sizeof(uint16_t)) + return -EBADMSG; + len = unaligned_read_be16(i.iov_base); + iovec_inc(&i, sizeof(uint16_t)); + break; + case TLV_LENGTH_U32: + if (i.iov_len < sizeof(uint32_t)) + return -EBADMSG; + len = unaligned_read_be32(i.iov_base); + iovec_inc(&i, sizeof(uint32_t)); + break; + default: + assert_not_reached(); + } + + if (i.iov_len < len) + return -EBADMSG; + + r = tlv_append_impl(tlv, tag, len, i.iov_base); + if (r < 0) + return r; + + iovec_inc(&i, len); + } + + return 0; +} + +size_t tlv_size(const TLV *tlv) { + assert(tlv); + + size_t header_sz; + switch (tlv->flags & _TLV_TAG_MASK) { + case TLV_TAG_U8: + header_sz = sizeof(uint8_t); + break; + case TLV_TAG_U16: + header_sz = sizeof(uint16_t); + break; + case TLV_TAG_U32: + header_sz = sizeof(uint32_t); + break; + default: + assert_not_reached(); + } + + switch (tlv->flags & _TLV_LENGTH_MASK) { + case TLV_LENGTH_U8: + header_sz += sizeof(uint8_t); + break; + case TLV_LENGTH_U16: + header_sz += sizeof(uint16_t); + break; + case TLV_LENGTH_U32: + header_sz += sizeof(uint32_t); + break; + default: + assert_not_reached(); + } + + size_t sz = FLAGS_SET(tlv->flags, TLV_APPEND_END); + + struct iovec_wrapper *iovw; + HASHMAP_FOREACH(iovw, tlv->entries) { + if (size_multiply_overflow(header_sz, iovw->count)) + return SIZE_MAX; + + sz = size_add(sz, size_add(header_sz * iovw->count, iovw_size(iovw))); + } + + return sz; +} + +int tlv_build(const TLV *tlv, struct iovec *ret) { + int r; + + assert(tlv); + assert(ret); + + size_t sz = tlv_size(tlv); + if (sz == SIZE_MAX) + return -ENOBUFS; + + _cleanup_free_ uint8_t *buf = new(uint8_t, sz); + if (!buf) + return -ENOMEM; + + /* Sort by tags, for reproducibility. */ + _cleanup_free_ void **sorted = NULL; + size_t n; + r = hashmap_dump_keys_sorted(tlv->entries, &sorted, &n); + if (r < 0) + return r; + + uint8_t *p = buf; + FOREACH_ARRAY(tagp, sorted, n) { + uint32_t tag = PTR_TO_UINT32(*tagp); + struct iovec_wrapper *iovw = ASSERT_PTR(tlv_get_all(tlv, tag)); + + if ((FLAGS_SET(tlv->flags, TLV_PAD) && tag == TLV_TAG_PAD) || + (FLAGS_SET(tlv->flags, TLV_END) && tag == TLV_TAG_END)) + return -EINVAL; + + FOREACH_ARRAY(iov, iovw->iovec, iovw->count) { + switch (tlv->flags & _TLV_TAG_MASK) { + case TLV_TAG_U8: + if (tag > UINT8_MAX) + return -EINVAL; + *p++ = tag; + break; + case TLV_TAG_U16: + if (tag > UINT16_MAX) + return -EINVAL; + unaligned_write_be16(p, tag); + p += sizeof(uint16_t); + break; + case TLV_TAG_U32: + unaligned_write_be32(p, tag); + p += sizeof(uint32_t); + break; + default: + assert_not_reached(); + } + + switch (tlv->flags & _TLV_LENGTH_MASK) { + case TLV_LENGTH_U8: + if (iov->iov_len > UINT8_MAX) + return -EINVAL; + *p++ = iov->iov_len; + break; + case TLV_LENGTH_U16: + if (iov->iov_len > UINT16_MAX) + return -EINVAL; + unaligned_write_be16(p, iov->iov_len); + p += sizeof(uint16_t); + break; + case TLV_LENGTH_U32: + if (iov->iov_len > UINT32_MAX) + return -EINVAL; + unaligned_write_be32(p, iov->iov_len); + p += sizeof(uint32_t); + break; + default: + assert_not_reached(); + } + + p = mempcpy_safe(p, iov->iov_base, iov->iov_len); + } + } + + if (FLAGS_SET(tlv->flags, TLV_APPEND_END)) + *p++ = TLV_TAG_END; + + assert(sz == (size_t) (p - buf)); + + *ret = IOVEC_MAKE(TAKE_PTR(buf), sz); + return 0; +} diff --git a/src/libsystemd-network/tlv-util.h b/src/libsystemd-network/tlv-util.h new file mode 100644 index 00000000000..5344c287032 --- /dev/null +++ b/src/libsystemd-network/tlv-util.h @@ -0,0 +1,82 @@ +/* SPDX-License-Identifier: LGPL-2.1-or-later */ +#pragma once + +#include "sd-forward.h" + +#define TLV_TAG_PAD UINT32_C(0) +#define TLV_TAG_END UINT32_C(0xFF) + +typedef enum TLVFlag { + TLV_TAG_U8 = 1 << 0, + TLV_TAG_U16 = 1 << 1, + TLV_TAG_U32 = 1 << 2, + _TLV_TAG_MASK = TLV_TAG_U8 | TLV_TAG_U16 | TLV_TAG_U32, + TLV_LENGTH_U8 = 1 << 3, + TLV_LENGTH_U16 = 1 << 4, + TLV_LENGTH_U32 = 1 << 5, + _TLV_LENGTH_MASK = TLV_LENGTH_U8 | TLV_LENGTH_U16 | TLV_LENGTH_U32, + TLV_PAD = 1 << 6, /* If set, tag == 0 is a pad, and does not have the length field. */ + TLV_END = 1 << 7, /* If set, tag == 0xFF is a sign of the end of the sequence. */ + TLV_APPEND_END = 1 << 8, /* If set, append the END tag at the end of the sequence on build. */ + TLV_MERGE = 1 << 9, /* If set, tlv_get_alloc() merges them, and tlv_append() split long data. */ + TLV_TEMPORARY = 1 << 10, /* If set, tlv_append() and tlv_parse() do not copy the data. */ + + /* DHCPv4 options. */ + TLV_DHCP4 = TLV_TAG_U8 | TLV_LENGTH_U8 | TLV_PAD | TLV_END | TLV_APPEND_END | TLV_MERGE, + /* Used for DHCPv4 sub-options, e.g. + * DHCPv4 Vendor Specific Information sub-option (43), + * DHCPv4 Relay Agent Information sub-option (82), or + * DHCPv4 Vendor-Identifying Vendor Specific Information sub-sub-option (125). + * Note that the PAD is not mentioned in RFC, but some implementations use it, hence let's gracefully + * handle it. Also note that the END tag is prohibited in most options, but we also gracefully handle + * it on parse, but of course do not append it on build. */ + TLV_DHCP4_SUBOPTION + = TLV_TAG_U8 | TLV_LENGTH_U8 | TLV_PAD | TLV_END, + /* DHCPv4 Vendor-Identifying Vendor Class sub-option (124) and + * DHCPv4 Vendor-Identifying Vendor Specific Information sub-option (125). + * The tag is called 'enterprise-number', and in uint32. */ + TLV_DHCP4_VENDOR_IDENTIFYING_OPTION + = TLV_TAG_U32 | TLV_LENGTH_U8 | TLV_MERGE, +} TLVFlag; + +typedef struct TLV { + unsigned n_ref; + TLVFlag flags; + unsigned n_entries; + Hashmap *entries; +} TLV; + +#define TLV_INIT(f) \ + (TLV) { \ + .n_ref = 1, \ + .flags = tlv_flags_verify(f), \ + } + +TLVFlag tlv_flags_verify(TLVFlag flags); + +void tlv_done(TLV *tlv); +TLV* tlv_ref(TLV *p); +TLV* tlv_unref(TLV *p); +DEFINE_TRIVIAL_CLEANUP_FUNC(TLV*, tlv_unref); +TLV* tlv_new(TLVFlag flags); + +bool tlv_isempty(const TLV *tlv); + +struct iovec_wrapper* tlv_get_all(const TLV *tlv, uint32_t tag); +static inline bool tlv_contains(const TLV *tlv, uint32_t tag) { + return tlv_get_all(tlv, tag); +} +int tlv_get_full(const TLV *tlv, uint32_t tag, size_t length, struct iovec *ret); +static inline int tlv_get(const TLV *tlv, uint32_t tag, struct iovec *ret) { + return tlv_get_full(tlv, tag, SIZE_MAX, ret); +} +int tlv_get_alloc(const TLV *tlv, uint32_t tag, struct iovec *ret); + +void tlv_remove(TLV *tlv, uint32_t tag); +int tlv_append(TLV *tlv, uint32_t tag, size_t length, const void *data); +int tlv_append_iov(TLV *tlv, uint32_t tag, const struct iovec *iov); +int tlv_append_tlv(TLV *tlv, const TLV *source); + +int tlv_parse(TLV *tlv, const struct iovec *iov); +size_t tlv_size(const TLV *tlv); +int tlv_build(const TLV *tlv, struct iovec *ret); -- 2.47.3