Debian Codegraph @

src/libsystemd-network/tlv-util.c

raw
1 /* SPDX-License-Identifier: LGPL-2.1-or-later */
2
3 #include "alloc-util.h"
4 #include "hashmap.h"
5 #include "iovec-util.h"
6 #include "iovec-wrapper.h"
7 #include "json-util.h"
8 #include "tlv-util.h"
9 #include "unaligned.h"
10
11 #define TLV_MAX_ENTRIES 4096u
12
13 TLVFlag tlv_flags_verify(TLVFlag flags) {
14 assert(IN_SET(flags & _TLV_TAG_MASK, TLV_TAG_U8, TLV_TAG_U16, TLV_TAG_U32));
15 assert(IN_SET(flags & _TLV_LENGTH_MASK, TLV_LENGTH_U8, TLV_LENGTH_U16, TLV_LENGTH_U32));
16
17 /* TLV_PAD and TLV_END are for DHCPv4 options, hence here we assume TLV_TAG_U8 is set. */
18 assert(!FLAGS_SET(flags, TLV_PAD) || FLAGS_SET(flags, TLV_TAG_U8));
19 assert(!FLAGS_SET(flags, TLV_END) || FLAGS_SET(flags, TLV_TAG_U8));
20
21 /* When we requested to append the END tag, then we should understand the END tag on parse. */
22 assert(!FLAGS_SET(flags, TLV_APPEND_END) || FLAGS_SET(flags, TLV_END));
23
24 return flags;
25 }
26
27 void tlv_done(TLV *tlv) {
28 assert(tlv);
29
30 tlv->entries = hashmap_free(tlv->entries);
31 tlv->n_entries = 0;
32 }
33
34 static TLV* tlv_free(TLV *tlv) {
35 if (!tlv)
36 return NULL;
37
38 tlv_done(tlv);
39 return mfree(tlv);
40 }
41
42 DEFINE_TRIVIAL_REF_UNREF_FUNC(TLV, tlv, tlv_free);
43
44 TLV* tlv_new(TLVFlag flags) {
45 TLV *tlv = new(TLV, 1);
46 if (!tlv)
47 return NULL;
48
49 *tlv = TLV_INIT(flags);
50 return tlv;
51 }
52
53 bool tlv_isempty(const TLV *tlv) {
54 return !tlv || hashmap_isempty(tlv->entries);
55 }
56
57 struct iovec_wrapper* tlv_get_all(const TLV *tlv, uint32_t tag) {
58 assert(tlv);
59 return hashmap_get(tlv->entries, UINT32_TO_PTR(tag));
60 }
61
62 int tlv_get_full(const TLV *tlv, uint32_t tag, size_t length, struct iovec *ret) {
63 assert(tlv);
64
65 /* Do not free the result iovec, the data is still owned by TLV (or the original input data when
66 * TLV_TEMPORARY is set). */
67
68 struct iovec_wrapper *iovw = tlv_get_all(tlv, tag);
69 if (iovw_isempty(iovw))
70 return -ENODATA;
71
72 /* When multiple entries exist, use the first one matching the length. */
73 FOREACH_ARRAY(iov, iovw->iovec, iovw->count) {
74 if (length != SIZE_MAX && iov->iov_len != length)
75 continue;
76
77 if (ret)
78 *ret = *iov;
79 return 0;
80 }
81
82 return -ENODATA;
83 }
84
85 int tlv_get_alloc(const TLV *tlv, uint32_t tag, struct iovec *ret) {
86 assert(tlv);
87
88 /* Free the result iovec. */
89
90 struct iovec_wrapper *iovw = tlv_get_all(tlv, tag);
91 if (iovw_isempty(iovw))
92 return -ENODATA;
93
94 if (!ret)
95 return 0;
96
97 if (FLAGS_SET(tlv->flags, TLV_MERGE))
98 return iovw_concat(iovw, ret);
99
100 /* When TLV_MERGE is unset, provides the first entry. */
101 if (!iovec_memdup(&iovw->iovec[0], ret))
102 return -ENOMEM;
103
104 return 0;
105 }
106
107 void tlv_remove(TLV *tlv, uint32_t tag) {
108 assert(tlv);
109
110 struct iovec_wrapper *iovw = hashmap_remove(tlv->entries, UINT32_TO_PTR(tag));
111 if (!iovw)
112 return;
113
114 assert(tlv->n_entries >= iovw->count);
115 tlv->n_entries -= iovw->count;
116
117 if (FLAGS_SET(tlv->flags, TLV_TEMPORARY))
118 iovw_free(iovw);
119 else
120 iovw_free_free(iovw);
121 }
122
123 DEFINE_PRIVATE_HASH_OPS_WITH_VALUE_DESTRUCTOR(
124 tlv_hash_ops,
125 void,
126 trivial_hash_func,
127 trivial_compare_func,
128 struct iovec_wrapper,
129 iovw_free);
130
131 DEFINE_PRIVATE_HASH_OPS_WITH_VALUE_DESTRUCTOR(
132 tlv_hash_ops_free,
133 void,
134 trivial_hash_func,
135 trivial_compare_func,
136 struct iovec_wrapper,
137 iovw_free_free);
138
139 static int tlv_append_impl(TLV *tlv, uint32_t tag, size_t length, const void *data) {
140 int r;
141
142 assert(tlv);
143 assert(length == 0 || data);
144
145 if (tlv->n_entries >= TLV_MAX_ENTRIES)
146 return -E2BIG;
147
148 if (FLAGS_SET(tlv->flags, TLV_TEMPORARY)) {
149 struct iovec_wrapper *e = tlv_get_all(tlv, tag);
150 if (e) {
151 r = iovw_put_full(e, /* accept_zero= */ true, (void*) data, length);
152 if (r < 0)
153 return r;
154 } else {
155 _cleanup_(iovw_freep) struct iovec_wrapper *v = new0(struct iovec_wrapper, 1);
156 if (!v)
157 return -ENOMEM;
158
159 r = iovw_put_full(v, /* accept_zero= */ true, (void*) data, length);
160 if (r < 0)
161 return r;
162
163 r = hashmap_ensure_put(&tlv->entries, &tlv_hash_ops, UINT32_TO_PTR(tag), v);
164 if (r < 0)
165 return r;
166
167 TAKE_PTR(v);
168 }
169 } else {
170 struct iovec_wrapper *e = tlv_get_all(tlv, tag);
171 if (e) {
172 r = iovw_extend_full(e, /* accept_zero= */ true, data, length);
173 if (r < 0)
174 return r;
175 } else {
176 _cleanup_(iovw_free_freep) struct iovec_wrapper *v = new0(struct iovec_wrapper, 1);
177 if (!v)
178 return -ENOMEM;
179
180 r = iovw_extend_full(v, /* accept_zero= */ true, data, length);
181 if (r < 0)
182 return r;
183
184 r = hashmap_ensure_put(&tlv->entries, &tlv_hash_ops_free, UINT32_TO_PTR(tag), v);
185 if (r < 0)
186 return r;
187
188 TAKE_PTR(v);
189 }
190 }
191
192 tlv->n_entries++;
193 return 0;
194 }
195
196 int tlv_append(TLV *tlv, uint32_t tag, size_t length, const void *data) {
197 int r;
198
199 assert(tlv);
200 assert(length == 0 || data);
201
202 switch (tlv->flags & _TLV_TAG_MASK) {
203 case TLV_TAG_U8:
204 if (tag > UINT8_MAX)
205 return -EINVAL;
206 break;
207 case TLV_TAG_U16:
208 if (tag > UINT16_MAX)
209 return -EINVAL;
210 break;
211 case TLV_TAG_U32:
212 break;
213 default:
214 assert_not_reached();
215 }
216
217 if ((FLAGS_SET(tlv->flags, TLV_PAD) && tag == TLV_TAG_PAD) ||
218 (FLAGS_SET(tlv->flags, TLV_END) && tag == TLV_TAG_END))
219 return -EINVAL;
220
221 size_t max_length;
222 switch (tlv->flags & _TLV_LENGTH_MASK) {
223 case TLV_LENGTH_U8:
224 max_length = UINT8_MAX;
225 break;
226 case TLV_LENGTH_U16:
227 max_length = UINT16_MAX;
228 break;
229 case TLV_LENGTH_U32:
230 max_length = UINT32_MAX;
231 break;
232 default:
233 assert_not_reached();
234 }
235
236 if (FLAGS_SET(tlv->flags, TLV_MERGE)) {
237 /* If TLV_MERGE is set and the length is larger than the allowed maximum, then split the data
238 * and store them in multiple entries.
239 *
240 * Note, if tlv_append_impl() fails below, we do not rollback the entries, hence the caller
241 * of this function needs to discard the entire data in that case. */
242 const uint8_t *p = data;
243 while (length > max_length) {
244 r = tlv_append_impl(tlv, tag, max_length, p);
245 if (r < 0)
246 return r;
247
248 p += max_length;
249 length -= max_length;
250 }
251
252 return tlv_append_impl(tlv, tag, length, p);
253 }
254
255 /* Otherwise, refuse too long data. */
256 if (length > max_length)
257 return -EINVAL;
258
259 return tlv_append_impl(tlv, tag, length, data);
260 }
261
262 int tlv_append_iov(TLV *tlv, uint32_t tag, const struct iovec *iov) {
263 assert(tlv);
264 assert(iovec_is_valid(iov));
265
266 return tlv_append(tlv, tag, iov ? iov->iov_len : 0, iov ? iov->iov_base : NULL);
267 }
268
269 int tlv_append_tlv(TLV *tlv, const TLV *source) {
270 int r;
271
272 assert(tlv);
273
274 /* Note, this does not rollback entries on failure, hence the caller of this function needs to
275 * discard the entire data in that case. */
276
277 if (!source)
278 return 0;
279
280 if (source == tlv)
281 return -EINVAL;
282
283 void *tagp;
284 struct iovec_wrapper *iovw;
285 HASHMAP_FOREACH_KEY(iovw, tagp, source->entries) {
286 uint32_t tag = PTR_TO_UINT32(tagp);
287
288 FOREACH_ARRAY(iov, iovw->iovec, iovw->count) {
289 r = tlv_append(tlv, tag, iov->iov_len, iov->iov_base);
290 if (r < 0)
291 return r;
292 }
293 }
294
295 return 0;
296 }
297
298 int tlv_parse(TLV *tlv, const struct iovec *iov) {
299 int r;
300
301 assert(tlv);
302 assert(iovec_is_valid(iov));
303
304 /* Note, this does not rollback entries on failure, hence the caller of this function needs to
305 * discard the entire data in that case. */
306
307 if (!iovec_is_set(iov))
308 return 0;
309
310 for (struct iovec i = *iov; iovec_is_set(&i); ) {
311 uint32_t tag;
312 switch (tlv->flags & _TLV_TAG_MASK) {
313 case TLV_TAG_U8:
314 if (i.iov_len < sizeof(uint8_t))
315 return -EBADMSG;
316 tag = *(uint8_t*) i.iov_base;
317 iovec_inc(&i, sizeof(uint8_t));
318 break;
319 case TLV_TAG_U16:
320 if (i.iov_len < sizeof(uint16_t))
321 return -EBADMSG;
322 tag = unaligned_read_be16(i.iov_base);
323 iovec_inc(&i, sizeof(uint16_t));
324 break;
325 case TLV_TAG_U32:
326 if (i.iov_len < sizeof(uint32_t))
327 return -EBADMSG;
328 tag = unaligned_read_be32(i.iov_base);
329 iovec_inc(&i, sizeof(uint32_t));
330 break;
331 default:
332 assert_not_reached();
333 }
334
335 if (FLAGS_SET(tlv->flags, TLV_PAD) && tag == TLV_TAG_PAD)
336 continue;
337 if (FLAGS_SET(tlv->flags, TLV_END) && tag == TLV_TAG_END)
338 break;
339
340 size_t len;
341 switch (tlv->flags & _TLV_LENGTH_MASK) {
342 case TLV_LENGTH_U8:
343 if (i.iov_len < sizeof(uint8_t))
344 return -EBADMSG;
345 len = *(uint8_t*) i.iov_base;
346 iovec_inc(&i, sizeof(uint8_t));
347 break;
348 case TLV_LENGTH_U16:
349 if (i.iov_len < sizeof(uint16_t))
350 return -EBADMSG;
351 len = unaligned_read_be16(i.iov_base);
352 iovec_inc(&i, sizeof(uint16_t));
353 break;
354 case TLV_LENGTH_U32:
355 if (i.iov_len < sizeof(uint32_t))
356 return -EBADMSG;
357 len = unaligned_read_be32(i.iov_base);
358 iovec_inc(&i, sizeof(uint32_t));
359 break;
360 default:
361 assert_not_reached();
362 }
363
364 if (i.iov_len < len)
365 return -EBADMSG;
366
367 r = tlv_append_impl(tlv, tag, len, i.iov_base);
368 if (r < 0)
369 return r;
370
371 iovec_inc(&i, len);
372 }
373
374 return 0;
375 }
376
377 size_t tlv_size(const TLV *tlv) {
378 assert(tlv);
379
380 size_t header_sz;
381 switch (tlv->flags & _TLV_TAG_MASK) {
382 case TLV_TAG_U8:
383 header_sz = sizeof(uint8_t);
384 break;
385 case TLV_TAG_U16:
386 header_sz = sizeof(uint16_t);
387 break;
388 case TLV_TAG_U32:
389 header_sz = sizeof(uint32_t);
390 break;
391 default:
392 assert_not_reached();
393 }
394
395 switch (tlv->flags & _TLV_LENGTH_MASK) {
396 case TLV_LENGTH_U8:
397 header_sz += sizeof(uint8_t);
398 break;
399 case TLV_LENGTH_U16:
400 header_sz += sizeof(uint16_t);
401 break;
402 case TLV_LENGTH_U32:
403 header_sz += sizeof(uint32_t);
404 break;
405 default:
406 assert_not_reached();
407 }
408
409 size_t sz = FLAGS_SET(tlv->flags, TLV_APPEND_END);
410
411 struct iovec_wrapper *iovw;
412 HASHMAP_FOREACH(iovw, tlv->entries) {
413 if (size_multiply_overflow(header_sz, iovw->count))
414 return SIZE_MAX;
415
416 sz = size_add(sz, size_add(header_sz * iovw->count, iovw_size(iovw)));
417 }
418
419 return sz;
420 }
421
422 int tlv_build(const TLV *tlv, struct iovec *ret) {
423 int r;
424
425 assert(tlv);
426 assert(ret);
427
428 size_t sz = tlv_size(tlv);
429 if (sz == SIZE_MAX)
430 return -ENOBUFS;
431
432 _cleanup_free_ uint8_t *buf = new(uint8_t, sz);
433 if (!buf)
434 return -ENOMEM;
435
436 /* Sort by tags, for reproducibility. */
437 _cleanup_free_ void **sorted = NULL;
438 size_t n;
439 r = hashmap_dump_keys_sorted(tlv->entries, &sorted, &n);
440 if (r < 0)
441 return r;
442
443 uint8_t *p = buf;
444 FOREACH_ARRAY(tagp, sorted, n) {
445 uint32_t tag = PTR_TO_UINT32(*tagp);
446 struct iovec_wrapper *iovw = ASSERT_PTR(tlv_get_all(tlv, tag));
447
448 if ((FLAGS_SET(tlv->flags, TLV_PAD) && tag == TLV_TAG_PAD) ||
449 (FLAGS_SET(tlv->flags, TLV_END) && tag == TLV_TAG_END))
450 return -EINVAL;
451
452 FOREACH_ARRAY(iov, iovw->iovec, iovw->count) {
453 switch (tlv->flags & _TLV_TAG_MASK) {
454 case TLV_TAG_U8:
455 if (tag > UINT8_MAX)
456 return -EINVAL;
457 *p++ = tag;
458 break;
459 case TLV_TAG_U16:
460 if (tag > UINT16_MAX)
461 return -EINVAL;
462 unaligned_write_be16(p, tag);
463 p += sizeof(uint16_t);
464 break;
465 case TLV_TAG_U32:
466 unaligned_write_be32(p, tag);
467 p += sizeof(uint32_t);
468 break;
469 default:
470 assert_not_reached();
471 }
472
473 switch (tlv->flags & _TLV_LENGTH_MASK) {
474 case TLV_LENGTH_U8:
475 if (iov->iov_len > UINT8_MAX)
476 return -EINVAL;
477 *p++ = iov->iov_len;
478 break;
479 case TLV_LENGTH_U16:
480 if (iov->iov_len > UINT16_MAX)
481 return -EINVAL;
482 unaligned_write_be16(p, iov->iov_len);
483 p += sizeof(uint16_t);
484 break;
485 case TLV_LENGTH_U32:
486 if (iov->iov_len > UINT32_MAX)
487 return -EINVAL;
488 unaligned_write_be32(p, iov->iov_len);
489 p += sizeof(uint32_t);
490 break;
491 default:
492 assert_not_reached();
493 }
494
495 p = mempcpy_safe(p, iov->iov_base, iov->iov_len);
496 }
497 }
498
499 if (FLAGS_SET(tlv->flags, TLV_APPEND_END))
500 *p++ = TLV_TAG_END;
501
502 assert(sz == (size_t) (p - buf));
503
504 *ret = IOVEC_MAKE(TAKE_PTR(buf), sz);
505 return 0;
506 }
507
508 int tlv_build_json(const TLV *tlv, sd_json_variant **ret) {
509 int r;
510
511 assert(tlv);
512 assert(ret);
513
514 /* Sort by tags, for reproducibility. */
515 _cleanup_free_ void **sorted = NULL;
516 size_t n;
517 r = hashmap_dump_keys_sorted(tlv->entries, &sorted, &n);
518 if (r < 0)
519 return r;
520
521 _cleanup_(sd_json_variant_unrefp) sd_json_variant *v = NULL;
522 FOREACH_ARRAY(tagp, sorted, n) {
523 uint32_t tag = PTR_TO_UINT32(*tagp);
524 struct iovec_wrapper *iovw = ASSERT_PTR(tlv_get_all(tlv, tag));
525
526 FOREACH_ARRAY(iov, iovw->iovec, iovw->count) {
527 r = sd_json_variant_append_arraybo(
528 &v,
529 SD_JSON_BUILD_PAIR_UNSIGNED("tag", tag),
530 JSON_BUILD_PAIR_IOVEC_HEX("data", iov));
531 if (r < 0)
532 return r;
533 }
534 }
535
536 *ret = TAKE_PTR(v);
537 return 0;
538 }
539
540 typedef struct TLVParam {
541 uint32_t tag;
542 struct iovec data;
543 } TLVParam;
544
545 static void tlv_param_done(TLVParam *p) {
546 iovec_done(&p->data);
547 }
548
549 int tlv_parse_json(TLV *tlv, sd_json_variant *v) {
550 static const sd_json_dispatch_field dispatch_table[] = {
551 { "tag", _SD_JSON_VARIANT_TYPE_INVALID, sd_json_dispatch_uint32, offsetof(TLVParam, tag), SD_JSON_MANDATORY },
552 { "data", SD_JSON_VARIANT_STRING, json_dispatch_unhex_iovec, offsetof(TLVParam, data), SD_JSON_MANDATORY },
553 {},
554 };
555
556 int r;
557
558 assert(tlv);
559 assert(v);
560
561 sd_json_variant *e;
562 JSON_VARIANT_ARRAY_FOREACH(e, v) {
563 _cleanup_(tlv_param_done) TLVParam p = {};
564 r = sd_json_dispatch(e, dispatch_table, SD_JSON_ALLOW_EXTENSIONS, &p);
565 if (r < 0)
566 return r;
567
568 r = tlv_append(tlv, p.tag, p.data.iov_len, p.data.iov_base);
569 if (r < 0)
570 return r;
571 }
572
573 return 0;
574 }