master
1/*-
2 * SPDX-License-Identifier: BSD-2-Clause
3 *
4 * Copyright (c) 2022 Alexander V. Chernikov <melifaro@FreeBSD.org>
5 *
6 * Redistribution and use in source and binary forms, with or without
7 * modification, are permitted provided that the following conditions
8 * are met:
9 * 1. Redistributions of source code must retain the above copyright
10 * notice, this list of conditions and the following disclaimer.
11 * 2. Redistributions in binary form must reproduce the above copyright
12 * notice, this list of conditions and the following disclaimer in the
13 * documentation and/or other materials provided with the distribution.
14 *
15 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
16 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
19 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
20 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
21 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
22 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
23 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
24 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
25 * SUCH DAMAGE.
26 */
27#ifndef _NETLINK_NETLINK_SNL_H_
28#define _NETLINK_NETLINK_SNL_H_
29
30/*
31 * Simple Netlink Library
32 */
33
34#include <assert.h>
35#include <errno.h>
36#include <stdalign.h>
37#include <stddef.h>
38#include <stdbool.h>
39#include <stdint.h>
40#include <stdlib.h>
41#include <string.h>
42#include <unistd.h>
43
44#include <sys/types.h>
45#include <sys/socket.h>
46#include <netlink/netlink.h>
47#include <netlink/netlink_bitset.h>
48
49#define _roundup2(x, y) (((x)+((y)-1))&(~((y)-1)))
50
51#define NETLINK_ALIGN_SIZE sizeof(uint32_t)
52#define NETLINK_ALIGN(_len) _roundup2(_len, NETLINK_ALIGN_SIZE)
53
54#define NLA_ALIGN_SIZE sizeof(uint32_t)
55#define NLA_HDRLEN ((int)sizeof(struct nlattr))
56#define NLA_DATA_LEN(_nla) ((int)((_nla)->nla_len - NLA_HDRLEN))
57#define NLA_DATA(_nla) NL_ITEM_DATA(_nla, NLA_HDRLEN)
58#define NLA_DATA_CONST(_nla) NL_ITEM_DATA_CONST(_nla, NLA_HDRLEN)
59
60#define NLA_TYPE(_nla) ((_nla)->nla_type & 0x3FFF)
61
62#define NLA_NEXT(_attr) (struct nlattr *)(void *)((char *)_attr + NLA_ALIGN(_attr->nla_len))
63
64#define _NLA_END(_start, _len) ((char *)(_start) + (_len))
65#define NLA_FOREACH(_attr, _start, _len) \
66 for (_attr = (struct nlattr *)(_start); \
67 ((char *)_attr < _NLA_END(_start, _len)) && \
68 ((char *)NLA_NEXT(_attr) <= _NLA_END(_start, _len)); \
69 _attr = NLA_NEXT(_attr))
70
71#define NL_ARRAY_LEN(_a) (sizeof(_a) / sizeof((_a)[0]))
72
73struct linear_buffer {
74 char *base; /* Base allocated memory pointer */
75 uint32_t offset; /* Currently used offset */
76 uint32_t size; /* Total buffer size */
77 struct linear_buffer *next; /* Buffer chaining */
78} __aligned(alignof(__max_align_t));
79
80static inline struct linear_buffer *
81lb_init(uint32_t size)
82{
83 struct linear_buffer *lb = (struct linear_buffer *)calloc(1, size);
84
85 if (lb != NULL) {
86 lb->base = (char *)(lb + 1);
87 lb->size = size - sizeof(*lb);
88 }
89
90 return (lb);
91}
92
93static inline void
94lb_free(struct linear_buffer *lb)
95{
96 free(lb);
97}
98
99static inline char *
100lb_allocz(struct linear_buffer *lb, int len)
101{
102 len = roundup2(len, alignof(__max_align_t));
103 if (lb->offset + len > lb->size)
104 return (NULL);
105 char *data = (lb->base + lb->offset);
106 lb->offset += len;
107 return (data);
108}
109
110static inline void
111lb_clear(struct linear_buffer *lb)
112{
113 memset(lb->base, 0, lb->offset);
114 lb->offset = 0;
115}
116
117struct snl_state {
118 int fd;
119 char *buf;
120 size_t off;
121 size_t bufsize;
122 size_t datalen;
123 uint32_t seq;
124 bool init_done;
125 struct linear_buffer *lb;
126};
127#define SCRATCH_BUFFER_SIZE 1024
128#define SNL_WRITER_BUFFER_SIZE 256
129
130typedef void snl_parse_field_f(struct snl_state *ss, void *hdr, void *target);
131struct snl_field_parser {
132 uint16_t off_in;
133 uint16_t off_out;
134 snl_parse_field_f *cb;
135};
136
137typedef bool snl_parse_attr_f(struct snl_state *ss, struct nlattr *attr,
138 const void *arg, void *target);
139struct snl_attr_parser {
140 uint16_t type; /* Attribute type */
141 uint16_t off; /* field offset in the target structure */
142 snl_parse_attr_f *cb; /* parser function to call */
143
144 /* Optional parser argument */
145 union {
146 const void *arg;
147 const uint32_t arg_u32;
148 };
149};
150
151typedef bool snl_parse_post_f(struct snl_state *ss, void *target);
152
153struct snl_hdr_parser {
154 uint16_t in_hdr_size; /* Input header size */
155 uint16_t out_size; /* Output structure size */
156 uint16_t fp_size; /* Number of items in field parser */
157 uint16_t np_size; /* Number of items in attribute parser */
158 const struct snl_field_parser *fp; /* array of header field parsers */
159 const struct snl_attr_parser *np; /* array of attribute parsers */
160 snl_parse_post_f *cb_post; /* post-parse callback */
161};
162
163#define SNL_DECLARE_PARSER_EXT(_name, _sz_h_in, _sz_out, _fp, _np, _cb) \
164static const struct snl_hdr_parser _name = { \
165 .in_hdr_size = _sz_h_in, \
166 .out_size = _sz_out, \
167 .fp = &((_fp)[0]), \
168 .np = &((_np)[0]), \
169 .fp_size = NL_ARRAY_LEN(_fp), \
170 .np_size = NL_ARRAY_LEN(_np), \
171 .cb_post = _cb, \
172}
173
174#define SNL_DECLARE_PARSER(_name, _t, _fp, _np) \
175 SNL_DECLARE_PARSER_EXT(_name, sizeof(_t), 0, _fp, _np, NULL)
176
177#define SNL_DECLARE_FIELD_PARSER_EXT(_name, _sz_h_in, _sz_out, _fp, _cb) \
178static const struct snl_hdr_parser _name = { \
179 .in_hdr_size = _sz_h_in, \
180 .out_size = _sz_out, \
181 .fp = &((_fp)[0]), \
182 .fp_size = NL_ARRAY_LEN(_fp), \
183 .cb_post = _cb, \
184}
185
186#define SNL_DECLARE_FIELD_PARSER(_name, _t, _fp) \
187 SNL_DECLARE_FIELD_PARSER_EXT(_name, sizeof(_t), 0, _fp, NULL)
188
189#define SNL_DECLARE_ATTR_PARSER_EXT(_name, _sz_out, _np, _cb) \
190static const struct snl_hdr_parser _name = { \
191 .out_size = _sz_out, \
192 .np = &((_np)[0]), \
193 .np_size = NL_ARRAY_LEN(_np), \
194 .cb_post = _cb, \
195}
196
197#define SNL_DECLARE_ATTR_PARSER(_name, _np) \
198 SNL_DECLARE_ATTR_PARSER_EXT(_name, 0, _np, NULL)
199
200
201static inline void *
202snl_allocz(struct snl_state *ss, int len)
203{
204 void *data = lb_allocz(ss->lb, len);
205
206 if (data == NULL) {
207 uint32_t size = ss->lb->size * 2;
208
209 while (size < len + sizeof(struct linear_buffer))
210 size *= 2;
211
212 struct linear_buffer *lb = lb_init(size);
213
214 if (lb != NULL) {
215 lb->next = ss->lb;
216 ss->lb = lb;
217 data = lb_allocz(ss->lb, len);
218 }
219 }
220
221 return (data);
222}
223
224static inline void
225snl_clear_lb(struct snl_state *ss)
226{
227 struct linear_buffer *lb = ss->lb;
228
229 lb_clear(lb);
230 lb = lb->next;
231 ss->lb->next = NULL;
232 /* Remove all linear bufs except the largest one */
233 while (lb != NULL) {
234 struct linear_buffer *lb_next = lb->next;
235 lb_free(lb);
236 lb = lb_next;
237 }
238}
239
240static void
241snl_free(struct snl_state *ss)
242{
243 if (ss->init_done) {
244 close(ss->fd);
245 if (ss->buf != NULL)
246 free(ss->buf);
247 if (ss->lb != NULL) {
248 snl_clear_lb(ss);
249 lb_free(ss->lb);
250 }
251 }
252}
253
254static inline bool
255snl_init(struct snl_state *ss, int netlink_family)
256{
257 memset(ss, 0, sizeof(*ss));
258
259 ss->fd = socket(AF_NETLINK, SOCK_RAW, netlink_family);
260 if (ss->fd == -1)
261 return (false);
262 ss->init_done = true;
263
264 int val = 1;
265 socklen_t optlen = sizeof(val);
266 if (setsockopt(ss->fd, SOL_NETLINK, NETLINK_EXT_ACK, &val, optlen) == -1) {
267 snl_free(ss);
268 return (false);
269 }
270
271 int rcvbuf;
272 if (getsockopt(ss->fd, SOL_SOCKET, SO_RCVBUF, &rcvbuf, &optlen) == -1) {
273 snl_free(ss);
274 return (false);
275 }
276
277 ss->bufsize = rcvbuf;
278 ss->buf = (char *)malloc(ss->bufsize);
279 if (ss->buf == NULL) {
280 snl_free(ss);
281 return (false);
282 }
283
284 ss->lb = lb_init(SCRATCH_BUFFER_SIZE);
285 if (ss->lb == NULL) {
286 snl_free(ss);
287 return (false);
288 }
289
290 return (true);
291}
292
293static inline bool
294snl_send(struct snl_state *ss, void *data, int sz)
295{
296 return (send(ss->fd, data, sz, 0) == sz);
297}
298
299static inline bool
300snl_send_message(struct snl_state *ss, struct nlmsghdr *hdr)
301{
302 ssize_t sz = NLMSG_ALIGN(hdr->nlmsg_len);
303
304 return (send(ss->fd, hdr, sz, 0) == sz);
305}
306
307static inline uint32_t
308snl_get_seq(struct snl_state *ss)
309{
310 return (++ss->seq);
311}
312
313struct snl_msg_info {
314 int cmsg_type;
315 int cmsg_level;
316 uint32_t process_id;
317 uint8_t port_id;
318 uint8_t seq_id;
319};
320static inline bool parse_cmsg(struct snl_state *ss, const struct msghdr *msg,
321 struct snl_msg_info *attrs);
322
323static inline struct nlmsghdr *
324snl_read_message_dbg(struct snl_state *ss, struct snl_msg_info *cinfo)
325{
326 memset(cinfo, 0, sizeof(*cinfo));
327
328 if (ss->off == ss->datalen) {
329 struct sockaddr_nl nladdr;
330 char cbuf[64];
331
332 struct iovec iov = {
333 .iov_base = ss->buf,
334 .iov_len = ss->bufsize,
335 };
336 struct msghdr msg = {
337 .msg_name = &nladdr,
338 .msg_namelen = sizeof(nladdr),
339 .msg_iov = &iov,
340 .msg_iovlen = 1,
341 .msg_control = cbuf,
342 .msg_controllen = sizeof(cbuf),
343 };
344 ss->off = 0;
345 ss->datalen = 0;
346 for (;;) {
347 ssize_t datalen = recvmsg(ss->fd, &msg, 0);
348 if (datalen > 0) {
349 ss->datalen = datalen;
350 parse_cmsg(ss, &msg, cinfo);
351 break;
352 } else if (errno != EINTR)
353 return (NULL);
354 }
355 }
356 struct nlmsghdr *hdr = (struct nlmsghdr *)(void *)&ss->buf[ss->off];
357 ss->off += NLMSG_ALIGN(hdr->nlmsg_len);
358 return (hdr);
359}
360
361
362static inline struct nlmsghdr *
363snl_read_message(struct snl_state *ss)
364{
365 if (ss->off == ss->datalen) {
366 struct sockaddr_nl nladdr;
367 struct iovec iov = {
368 .iov_base = ss->buf,
369 .iov_len = ss->bufsize,
370 };
371 struct msghdr msg = {
372 .msg_name = &nladdr,
373 .msg_namelen = sizeof(nladdr),
374 .msg_iov = &iov,
375 .msg_iovlen = 1,
376 };
377 ss->off = 0;
378 ss->datalen = 0;
379 for (;;) {
380 ssize_t datalen = recvmsg(ss->fd, &msg, 0);
381 if (datalen > 0) {
382 ss->datalen = datalen;
383 break;
384 } else if (errno != EINTR)
385 return (NULL);
386 }
387 }
388 struct nlmsghdr *hdr = (struct nlmsghdr *)(void *)&ss->buf[ss->off];
389 ss->off += NLMSG_ALIGN(hdr->nlmsg_len);
390 return (hdr);
391}
392
393static inline struct nlmsghdr *
394snl_read_reply(struct snl_state *ss, uint32_t nlmsg_seq)
395{
396 struct nlmsghdr *hdr;
397
398 while ((hdr = snl_read_message(ss)) != NULL) {
399 if (hdr->nlmsg_seq == nlmsg_seq)
400 return (hdr);
401 }
402
403 return (NULL);
404}
405
406/*
407 * Checks that attributes are sorted by attribute type.
408 */
409static inline void
410snl_verify_parsers(const struct snl_hdr_parser **parser, int count)
411{
412 for (int i = 0; i < count; i++) {
413 const struct snl_hdr_parser *p = parser[i];
414 int attr_type = 0;
415 for (int j = 0; j < p->np_size; j++) {
416 assert(p->np[j].type > attr_type);
417 attr_type = p->np[j].type;
418 }
419 }
420}
421#define SNL_VERIFY_PARSERS(_p) snl_verify_parsers((_p), NL_ARRAY_LEN(_p))
422
423static const struct snl_attr_parser *
424find_parser(const struct snl_attr_parser *ps, int pslen, int key)
425{
426 int left_i = 0, right_i = pslen - 1;
427
428 if (key < ps[0].type || key > ps[pslen - 1].type)
429 return (NULL);
430
431 while (left_i + 1 < right_i) {
432 int mid_i = (left_i + right_i) / 2;
433 if (key < ps[mid_i].type)
434 right_i = mid_i;
435 else if (key > ps[mid_i].type)
436 left_i = mid_i + 1;
437 else
438 return (&ps[mid_i]);
439 }
440 if (ps[left_i].type == key)
441 return (&ps[left_i]);
442 else if (ps[right_i].type == key)
443 return (&ps[right_i]);
444 return (NULL);
445}
446
447static inline bool
448snl_parse_attrs_raw(struct snl_state *ss, struct nlattr *nla_head, int len,
449 const struct snl_attr_parser *ps, int pslen, void *target)
450{
451 struct nlattr *nla;
452
453 NLA_FOREACH(nla, nla_head, len) {
454 if (nla->nla_len < sizeof(struct nlattr))
455 return (false);
456 int nla_type = nla->nla_type & NLA_TYPE_MASK;
457 const struct snl_attr_parser *s = find_parser(ps, pslen, nla_type);
458 if (s != NULL) {
459 void *ptr = (void *)((char *)target + s->off);
460 if (!s->cb(ss, nla, s->arg, ptr))
461 return (false);
462 }
463 }
464 return (true);
465}
466
467static inline bool
468snl_parse_attrs(struct snl_state *ss, struct nlmsghdr *hdr, int hdrlen,
469 const struct snl_attr_parser *ps, int pslen, void *target)
470{
471 int off = NLMSG_HDRLEN + NETLINK_ALIGN(hdrlen);
472 int len = hdr->nlmsg_len - off;
473 struct nlattr *nla_head = (struct nlattr *)(void *)((char *)hdr + off);
474
475 return (snl_parse_attrs_raw(ss, nla_head, len, ps, pslen, target));
476}
477
478static inline void
479snl_parse_fields(struct snl_state *ss, struct nlmsghdr *hdr, int hdrlen __unused,
480 const struct snl_field_parser *ps, int pslen, void *target)
481{
482 for (int i = 0; i < pslen; i++) {
483 const struct snl_field_parser *fp = &ps[i];
484 void *src = (char *)hdr + fp->off_in;
485 void *dst = (char *)target + fp->off_out;
486
487 fp->cb(ss, src, dst);
488 }
489}
490
491static inline bool
492snl_parse_header(struct snl_state *ss, void *hdr, int len,
493 const struct snl_hdr_parser *parser, void *target)
494{
495 struct nlattr *nla_head;
496
497 /* Extract fields first (if any) */
498 snl_parse_fields(ss, (struct nlmsghdr *)hdr, parser->in_hdr_size,
499 parser->fp, parser->fp_size, target);
500
501 nla_head = (struct nlattr *)(void *)((char *)hdr + parser->in_hdr_size);
502 bool result = snl_parse_attrs_raw(ss, nla_head, len - parser->in_hdr_size,
503 parser->np, parser->np_size, target);
504
505 if (result && parser->cb_post != NULL)
506 result = parser->cb_post(ss, target);
507
508 return (result);
509}
510
511static inline bool
512snl_parse_nlmsg(struct snl_state *ss, struct nlmsghdr *hdr,
513 const struct snl_hdr_parser *parser, void *target)
514{
515 return (snl_parse_header(ss, hdr + 1, hdr->nlmsg_len - sizeof(*hdr), parser, target));
516}
517
518static inline bool
519snl_attr_get_flag(struct snl_state *ss __unused, struct nlattr *nla, const void *arg __unused,
520 void *target)
521{
522 if (NLA_DATA_LEN(nla) == 0) {
523 *((uint8_t *)target) = 1;
524 return (true);
525 }
526 return (false);
527}
528
529static inline bool
530snl_attr_get_uint8(struct snl_state *ss __unused, struct nlattr *nla,
531 const void *arg __unused, void *target)
532{
533 if (NLA_DATA_LEN(nla) == sizeof(uint8_t)) {
534 *((uint8_t *)target) = *((const uint8_t *)NLA_DATA_CONST(nla));
535 return (true);
536 }
537 return (false);
538}
539
540static inline bool
541snl_attr_get_uint16(struct snl_state *ss __unused, struct nlattr *nla,
542 const void *arg __unused, void *target)
543{
544 if (NLA_DATA_LEN(nla) == sizeof(uint16_t)) {
545 *((uint16_t *)target) = *((const uint16_t *)NLA_DATA_CONST(nla));
546 return (true);
547 }
548 return (false);
549}
550
551static inline bool
552snl_attr_get_uint32(struct snl_state *ss __unused, struct nlattr *nla,
553 const void *arg __unused, void *target)
554{
555 if (NLA_DATA_LEN(nla) == sizeof(uint32_t)) {
556 *((uint32_t *)target) = *((const uint32_t *)NLA_DATA_CONST(nla));
557 return (true);
558 }
559 return (false);
560}
561
562static inline bool
563snl_attr_get_uint64(struct snl_state *ss __unused, struct nlattr *nla,
564 const void *arg __unused, void *target)
565{
566 if (NLA_DATA_LEN(nla) == sizeof(uint64_t)) {
567 memcpy(target, NLA_DATA_CONST(nla), sizeof(uint64_t));
568 return (true);
569 }
570 return (false);
571}
572
573static inline bool
574snl_attr_get_int8(struct snl_state *ss, struct nlattr *nla, const void *arg,
575 void *target)
576{
577 return (snl_attr_get_uint8(ss, nla, arg, target));
578}
579
580static inline bool
581snl_attr_get_int16(struct snl_state *ss, struct nlattr *nla, const void *arg,
582 void *target)
583{
584 return (snl_attr_get_uint16(ss, nla, arg, target));
585}
586
587static inline bool
588snl_attr_get_int32(struct snl_state *ss, struct nlattr *nla, const void *arg,
589 void *target)
590{
591 return (snl_attr_get_uint32(ss, nla, arg, target));
592}
593
594static inline bool
595snl_attr_get_int64(struct snl_state *ss, struct nlattr *nla, const void *arg,
596 void *target)
597{
598 return (snl_attr_get_uint64(ss, nla, arg, target));
599}
600
601static inline bool
602snl_attr_get_string(struct snl_state *ss __unused, struct nlattr *nla,
603 const void *arg __unused, void *target)
604{
605 size_t maxlen = NLA_DATA_LEN(nla);
606
607 if (strnlen((char *)NLA_DATA(nla), maxlen) < maxlen) {
608 *((char **)target) = (char *)NLA_DATA(nla);
609 return (true);
610 }
611 return (false);
612}
613
614static inline bool
615snl_attr_get_stringn(struct snl_state *ss, struct nlattr *nla,
616 const void *arg __unused, void *target)
617{
618 int maxlen = NLA_DATA_LEN(nla);
619
620 char *buf = (char *)snl_allocz(ss, maxlen + 1);
621 if (buf == NULL)
622 return (false);
623 buf[maxlen] = '\0';
624 memcpy(buf, NLA_DATA(nla), maxlen);
625
626 *((char **)target) = buf;
627 return (true);
628}
629
630static inline bool
631snl_attr_copy_string(struct snl_state *ss, struct nlattr *nla,
632 const void *arg, void *target)
633{
634 char *tmp;
635
636 if (snl_attr_get_string(ss, nla, NULL, &tmp)) {
637 strlcpy((char *)target, tmp, (size_t)arg);
638 return (true);
639 }
640 return (false);
641}
642
643static inline bool
644snl_attr_dup_string(struct snl_state *ss __unused, struct nlattr *nla,
645 const void *arg __unused, void *target)
646{
647 size_t maxlen = NLA_DATA_LEN(nla);
648
649 if (strnlen((char *)NLA_DATA(nla), maxlen) < maxlen) {
650 char *buf = (char *)snl_allocz(ss, maxlen);
651 if (buf == NULL)
652 return (false);
653 memcpy(buf, NLA_DATA(nla), maxlen);
654 *((char **)target) = buf;
655 return (true);
656 }
657 return (false);
658}
659
660static inline bool
661snl_attr_get_nested(struct snl_state *ss, struct nlattr *nla, const void *arg, void *target)
662{
663 const struct snl_hdr_parser *p = (const struct snl_hdr_parser *)arg;
664
665 /* Assumes target points to the beginning of the structure */
666 return (snl_parse_header(ss, NLA_DATA(nla), NLA_DATA_LEN(nla), p, target));
667}
668
669struct snl_parray {
670 uint32_t count;
671 void **items;
672};
673
674static inline bool
675snl_attr_get_parray_sz(struct snl_state *ss, struct nlattr *container_nla,
676 uint32_t start_size, const void *arg, void *target)
677{
678 const struct snl_hdr_parser *p = (const struct snl_hdr_parser *)arg;
679 struct snl_parray *array = (struct snl_parray *)target;
680 struct nlattr *nla;
681 uint32_t count = 0, size = start_size;
682
683 if (p->out_size == 0)
684 return (false);
685
686 array->items = (void **)snl_allocz(ss, size * sizeof(void *));
687 if (array->items == NULL)
688 return (false);
689
690 /*
691 * If the provided parser is an attribute parser, assume that each
692 * nla in the container nla is the container nla itself and parse
693 * the contents of this nla.
694 * Otherwise, run the parser on raw data, assuming the header of this
695 * data has u16 field with total size in the beginning.
696 */
697 uint32_t data_off = 0;
698
699 if (p->in_hdr_size == 0)
700 data_off = sizeof(struct nlattr);
701
702 NLA_FOREACH(nla, NLA_DATA(container_nla), NLA_DATA_LEN(container_nla)) {
703 void *item = snl_allocz(ss, p->out_size);
704
705 if (item == NULL)
706 return (false);
707
708 void *data = (char *)(void *)nla + data_off;
709 int data_len = nla->nla_len - data_off;
710
711 if (!(snl_parse_header(ss, data, data_len, p, item)))
712 return (false);
713
714 if (count == size) {
715 uint32_t new_size = size * 2;
716 void **new_array = (void **)snl_allocz(ss, new_size *sizeof(void *));
717
718 memcpy(new_array, array->items, size * sizeof(void *));
719 array->items = new_array;
720 size = new_size;
721 }
722 array->items[count++] = item;
723 }
724 array->count = count;
725
726 return (true);
727}
728
729/*
730 * Parses and stores the unknown-size array.
731 * Assumes each array item is a container and the NLAs in the container are parsable
732 * by the parser provided in @arg.
733 * Assumes @target is struct snl_parray
734 */
735static inline bool
736snl_attr_get_parray(struct snl_state *ss, struct nlattr *nla, const void *arg, void *target)
737{
738 return (snl_attr_get_parray_sz(ss, nla, 8, arg, target));
739}
740
741static inline bool
742snl_attr_get_nla(struct snl_state *ss __unused, struct nlattr *nla,
743 const void *arg __unused, void *target)
744{
745 *((struct nlattr **)target) = nla;
746 return (true);
747}
748
749static inline bool
750snl_attr_dup_nla(struct snl_state *ss, struct nlattr *nla,
751 const void *arg __unused, void *target)
752{
753 void *ptr = snl_allocz(ss, nla->nla_len);
754
755 if (ptr != NULL) {
756 memcpy(ptr, nla, nla->nla_len);
757 *((void **)target) = ptr;
758 return (true);
759 }
760 return (false);
761}
762
763static inline bool
764snl_attr_copy_struct(struct snl_state *ss, struct nlattr *nla,
765 const void *arg __unused, void *target)
766{
767 void *ptr = snl_allocz(ss, NLA_DATA_LEN(nla));
768
769 if (ptr != NULL) {
770 memcpy(ptr, NLA_DATA(nla), NLA_DATA_LEN(nla));
771 *((void **)target) = ptr;
772 return (true);
773 }
774 return (false);
775}
776
777static inline bool
778snl_attr_dup_struct(struct snl_state *ss, struct nlattr *nla,
779 const void *arg __unused, void *target)
780{
781 void *ptr = snl_allocz(ss, NLA_DATA_LEN(nla));
782
783 if (ptr != NULL) {
784 memcpy(ptr, NLA_DATA(nla), NLA_DATA_LEN(nla));
785 *((void **)target) = ptr;
786 return (true);
787 }
788 return (false);
789}
790
791struct snl_attr_bit {
792 uint32_t bit_index;
793 char *bit_name;
794 int bit_value;
795};
796
797struct snl_attr_bits {
798 uint32_t num_bits;
799 struct snl_attr_bit **bits;
800};
801
802#define _OUT(_field) offsetof(struct snl_attr_bit, _field)
803static const struct snl_attr_parser _nla_p_bit[] = {
804 { .type = NLA_BITSET_BIT_INDEX, .off = _OUT(bit_index), .cb = snl_attr_get_uint32 },
805 { .type = NLA_BITSET_BIT_NAME, .off = _OUT(bit_name), .cb = snl_attr_dup_string },
806 { .type = NLA_BITSET_BIT_VALUE, .off = _OUT(bit_value), .cb = snl_attr_get_flag },
807};
808#undef _OUT
809SNL_DECLARE_ATTR_PARSER_EXT(_nla_bit_parser, sizeof(struct snl_attr_bit), _nla_p_bit, NULL);
810
811struct snl_attr_bitset {
812 uint32_t nla_bitset_size;
813 uint32_t *nla_bitset_mask;
814 uint32_t *nla_bitset_value;
815 struct snl_attr_bits bits;
816};
817
818#define _OUT(_field) offsetof(struct snl_attr_bitset, _field)
819static const struct snl_attr_parser _nla_p_bitset[] = {
820 { .type = NLA_BITSET_SIZE, .off = _OUT(nla_bitset_size), .cb = snl_attr_get_uint32 },
821 { .type = NLA_BITSET_BITS, .off = _OUT(bits), .cb = snl_attr_get_parray, .arg = &_nla_bit_parser },
822 { .type = NLA_BITSET_VALUE, .off = _OUT(nla_bitset_mask), .cb = snl_attr_dup_nla },
823 { .type = NLA_BITSET_MASK, .off = _OUT(nla_bitset_value), .cb = snl_attr_dup_nla },
824};
825
826static inline bool
827_cb_p_bitset(struct snl_state *ss __unused, void *_target)
828{
829 struct snl_attr_bitset *target = (struct snl_attr_bitset *)_target;
830
831 uint32_t sz_bytes = _roundup2(target->nla_bitset_size, 32) / 8;
832
833 if (target->nla_bitset_mask != NULL) {
834 struct nlattr *nla = (struct nlattr *)target->nla_bitset_mask;
835 uint32_t data_len = NLA_DATA_LEN(nla);
836
837 if (data_len != sz_bytes || _roundup2(data_len, 4) != data_len)
838 return (false);
839 target->nla_bitset_mask = (uint32_t *)NLA_DATA(nla);
840 }
841
842 if (target->nla_bitset_value != NULL) {
843 struct nlattr *nla = (struct nlattr *)target->nla_bitset_value;
844 uint32_t data_len = NLA_DATA_LEN(nla);
845
846 if (data_len != sz_bytes || _roundup2(data_len, 4) != data_len)
847 return (false);
848 target->nla_bitset_value = (uint32_t *)NLA_DATA(nla);
849 }
850 return (true);
851}
852#undef _OUT
853SNL_DECLARE_ATTR_PARSER_EXT(_nla_bitset_parser,
854 sizeof(struct snl_attr_bitset),
855 _nla_p_bitset, _cb_p_bitset);
856
857/*
858 * Parses the compact bitset representation.
859 */
860static inline bool
861snl_attr_get_bitset_c(struct snl_state *ss, struct nlattr *nla,
862 const void *arg __unused, void *_target)
863{
864 const struct snl_hdr_parser *p = &_nla_bitset_parser;
865 struct snl_attr_bitset *target = (struct snl_attr_bitset *)_target;
866
867 /* Assumes target points to the beginning of the structure */
868 if (!snl_parse_header(ss, NLA_DATA(nla), NLA_DATA_LEN(nla), p, _target))
869 return (false);
870 if (target->nla_bitset_mask == NULL || target->nla_bitset_value == NULL)
871 return (false);
872 return (true);
873}
874
875static inline void
876snl_field_get_uint8(struct snl_state *ss __unused, void *src, void *target)
877{
878 *((uint8_t *)target) = *((uint8_t *)src);
879}
880
881static inline void
882snl_field_get_uint16(struct snl_state *ss __unused, void *src, void *target)
883{
884 *((uint16_t *)target) = *((uint16_t *)src);
885}
886
887static inline void
888snl_field_get_uint32(struct snl_state *ss __unused, void *src, void *target)
889{
890 *((uint32_t *)target) = *((uint32_t *)src);
891}
892
893static inline void
894snl_field_get_ptr(struct snl_state *ss __unused, void *src, void *target)
895{
896 *((void **)target) = src;
897}
898
899struct snl_errmsg_data {
900 struct nlmsghdr *orig_hdr;
901 int error;
902 uint32_t error_offs;
903 char *error_str;
904 struct nlattr *cookie;
905};
906
907#define _IN(_field) offsetof(struct nlmsgerr, _field)
908#define _OUT(_field) offsetof(struct snl_errmsg_data, _field)
909static const struct snl_attr_parser nla_p_errmsg[] = {
910 { .type = NLMSGERR_ATTR_MSG, .off = _OUT(error_str), .cb = snl_attr_get_string },
911 { .type = NLMSGERR_ATTR_OFFS, .off = _OUT(error_offs), .cb = snl_attr_get_uint32 },
912 { .type = NLMSGERR_ATTR_COOKIE, .off = _OUT(cookie), .cb = snl_attr_get_nla },
913};
914
915static const struct snl_field_parser nlf_p_errmsg[] = {
916 { .off_in = _IN(error), .off_out = _OUT(error), .cb = snl_field_get_uint32 },
917 { .off_in = _IN(msg), .off_out = _OUT(orig_hdr), .cb = snl_field_get_ptr },
918};
919#undef _IN
920#undef _OUT
921SNL_DECLARE_PARSER(snl_errmsg_parser, struct nlmsgerr, nlf_p_errmsg, nla_p_errmsg);
922
923#define _IN(_field) offsetof(struct nlmsgerr, _field)
924#define _OUT(_field) offsetof(struct snl_errmsg_data, _field)
925static const struct snl_field_parser nlf_p_donemsg[] = {
926 { .off_in = _IN(error), .off_out = _OUT(error), .cb = snl_field_get_uint32 },
927};
928#undef _IN
929#undef _OUT
930SNL_DECLARE_FIELD_PARSER(snl_donemsg_parser, struct nlmsgerr, nlf_p_donemsg);
931
932static inline bool
933snl_parse_errmsg(struct snl_state *ss, struct nlmsghdr *hdr, struct snl_errmsg_data *e)
934{
935 if ((hdr->nlmsg_flags & NLM_F_CAPPED) != 0)
936 return (snl_parse_nlmsg(ss, hdr, &snl_errmsg_parser, e));
937
938 const struct snl_hdr_parser *ps = &snl_errmsg_parser;
939 struct nlmsgerr *errmsg = (struct nlmsgerr *)(hdr + 1);
940 int hdrlen = sizeof(int) + NLMSG_ALIGN(errmsg->msg.nlmsg_len);
941 struct nlattr *attr_head = (struct nlattr *)(void *)((char *)errmsg + hdrlen);
942 int attr_len = hdr->nlmsg_len - sizeof(struct nlmsghdr) - hdrlen;
943
944 snl_parse_fields(ss, (struct nlmsghdr *)errmsg, hdrlen, ps->fp, ps->fp_size, e);
945 return (snl_parse_attrs_raw(ss, attr_head, attr_len, ps->np, ps->np_size, e));
946}
947
948static inline bool
949snl_read_reply_code(struct snl_state *ss, uint32_t nlmsg_seq, struct snl_errmsg_data *e)
950{
951 struct nlmsghdr *hdr = snl_read_reply(ss, nlmsg_seq);
952
953 if (hdr == NULL) {
954 e->error = EINVAL;
955 } else if (hdr->nlmsg_type == NLMSG_ERROR) {
956 if (!snl_parse_errmsg(ss, hdr, e))
957 e->error = EINVAL;
958 return (e->error == 0);
959 }
960
961 return (false);
962}
963
964#define _OUT(_field) offsetof(struct snl_msg_info, _field)
965static const struct snl_attr_parser _nla_p_cinfo[] = {
966 { .type = NLMSGINFO_ATTR_PROCESS_ID, .off = _OUT(process_id), .cb = snl_attr_get_uint32 },
967 { .type = NLMSGINFO_ATTR_PORT_ID, .off = _OUT(port_id), .cb = snl_attr_get_uint32 },
968 { .type = NLMSGINFO_ATTR_SEQ_ID, .off = _OUT(seq_id), .cb = snl_attr_get_uint32 },
969};
970#undef _OUT
971SNL_DECLARE_ATTR_PARSER(snl_msg_info_parser, _nla_p_cinfo);
972
973static inline bool
974parse_cmsg(struct snl_state *ss, const struct msghdr *msg, struct snl_msg_info *attrs)
975{
976 for (struct cmsghdr *cmsg = CMSG_FIRSTHDR(msg); cmsg != NULL;
977 cmsg = CMSG_NXTHDR(msg, cmsg)) {
978 if (cmsg->cmsg_level != SOL_NETLINK || cmsg->cmsg_type != NETLINK_MSG_INFO)
979 continue;
980
981 void *data = CMSG_DATA(cmsg);
982 int len = cmsg->cmsg_len - ((char *)data - (char *)cmsg);
983 const struct snl_hdr_parser *ps = &snl_msg_info_parser;
984
985 return (snl_parse_attrs_raw(ss, (struct nlattr *)data, len, ps->np, ps->np_size, attrs));
986 }
987
988 return (false);
989}
990
991/*
992 * Assumes e is zeroed
993 */
994static inline struct nlmsghdr *
995snl_read_reply_multi(struct snl_state *ss, uint32_t nlmsg_seq, struct snl_errmsg_data *e)
996{
997 struct nlmsghdr *hdr = snl_read_reply(ss, nlmsg_seq);
998
999 if (hdr == NULL) {
1000 e->error = EINVAL;
1001 } else if (hdr->nlmsg_type == NLMSG_ERROR) {
1002 if (!snl_parse_errmsg(ss, hdr, e))
1003 e->error = EINVAL;
1004 } else if (hdr->nlmsg_type == NLMSG_DONE) {
1005 snl_parse_nlmsg(ss, hdr, &snl_donemsg_parser, e);
1006 } else
1007 return (hdr);
1008
1009 return (NULL);
1010}
1011
1012
1013/* writer logic */
1014struct snl_writer {
1015 char *base;
1016 uint32_t offset;
1017 uint32_t size;
1018 struct nlmsghdr *hdr;
1019 struct snl_state *ss;
1020 bool error;
1021};
1022
1023static inline void
1024snl_init_writer(struct snl_state *ss, struct snl_writer *nw)
1025{
1026 nw->size = SNL_WRITER_BUFFER_SIZE;
1027 nw->base = (char *)snl_allocz(ss, nw->size);
1028 if (nw->base == NULL) {
1029 nw->error = true;
1030 nw->size = 0;
1031 }
1032
1033 nw->offset = 0;
1034 nw->hdr = NULL;
1035 nw->error = false;
1036 nw->ss = ss;
1037}
1038
1039static inline bool
1040snl_realloc_msg_buffer(struct snl_writer *nw, size_t sz)
1041{
1042 uint32_t new_size = nw->size * 2;
1043
1044 while (new_size < nw->size + sz)
1045 new_size *= 2;
1046
1047 if (nw->error)
1048 return (false);
1049
1050 if (snl_allocz(nw->ss, new_size) == NULL) {
1051 nw->error = true;
1052 return (false);
1053 }
1054 nw->size = new_size;
1055
1056 void *new_base = nw->ss->lb->base;
1057 if (new_base != nw->base) {
1058 memcpy(new_base, nw->base, nw->offset);
1059 if (nw->hdr != NULL) {
1060 int hdr_off = (char *)(nw->hdr) - nw->base;
1061
1062 nw->hdr = (struct nlmsghdr *)
1063 (void *)((char *)new_base + hdr_off);
1064 }
1065 nw->base = (char *)new_base;
1066 }
1067
1068 return (true);
1069}
1070
1071static inline void *
1072snl_reserve_msg_data_raw(struct snl_writer *nw, size_t sz)
1073{
1074 sz = NETLINK_ALIGN(sz);
1075
1076 if (__predict_false(nw->offset + sz > nw->size)) {
1077 if (!snl_realloc_msg_buffer(nw, sz))
1078 return (NULL);
1079 }
1080
1081 void *data_ptr = &nw->base[nw->offset];
1082 nw->offset += sz;
1083
1084 return (data_ptr);
1085}
1086#define snl_reserve_msg_object(_ns, _t) ((_t *)snl_reserve_msg_data_raw(_ns, sizeof(_t)))
1087#define snl_reserve_msg_data(_ns, _sz, _t) ((_t *)snl_reserve_msg_data_raw(_ns, _sz))
1088
1089static inline void *
1090_snl_reserve_msg_attr(struct snl_writer *nw, uint16_t nla_type, uint16_t sz)
1091{
1092 sz += sizeof(struct nlattr);
1093
1094 struct nlattr *nla = snl_reserve_msg_data(nw, sz, struct nlattr);
1095 if (__predict_false(nla == NULL))
1096 return (NULL);
1097 nla->nla_type = nla_type;
1098 nla->nla_len = sz;
1099
1100 return ((void *)(nla + 1));
1101}
1102#define snl_reserve_msg_attr(_ns, _at, _t) ((_t *)_snl_reserve_msg_attr(_ns, _at, sizeof(_t)))
1103
1104static inline bool
1105snl_add_msg_attr(struct snl_writer *nw, int attr_type, int attr_len, const void *data)
1106{
1107 int required_len = NLA_ALIGN(attr_len + sizeof(struct nlattr));
1108
1109 if (__predict_false(nw->offset + required_len > nw->size)) {
1110 if (!snl_realloc_msg_buffer(nw, required_len))
1111 return (false);
1112 }
1113
1114 struct nlattr *nla = (struct nlattr *)(void *)(&nw->base[nw->offset]);
1115
1116 nla->nla_len = attr_len + sizeof(struct nlattr);
1117 nla->nla_type = attr_type;
1118 if (attr_len > 0) {
1119 if ((attr_len % 4) != 0) {
1120 /* clear padding bytes */
1121 bzero((char *)nla + required_len - 4, 4);
1122 }
1123 memcpy((nla + 1), data, attr_len);
1124 }
1125 nw->offset += required_len;
1126 return (true);
1127}
1128
1129static inline bool
1130snl_add_msg_attr_raw(struct snl_writer *nw, const struct nlattr *nla_src)
1131{
1132 int attr_len = nla_src->nla_len - sizeof(struct nlattr);
1133
1134 assert(attr_len >= 0);
1135
1136 return (snl_add_msg_attr(nw, nla_src->nla_type, attr_len, (const void *)(nla_src + 1)));
1137}
1138
1139static inline bool
1140snl_add_msg_attr_u8(struct snl_writer *nw, int attrtype, uint8_t value)
1141{
1142 return (snl_add_msg_attr(nw, attrtype, sizeof(uint8_t), &value));
1143}
1144
1145static inline bool
1146snl_add_msg_attr_u16(struct snl_writer *nw, int attrtype, uint16_t value)
1147{
1148 return (snl_add_msg_attr(nw, attrtype, sizeof(uint16_t), &value));
1149}
1150
1151static inline bool
1152snl_add_msg_attr_u32(struct snl_writer *nw, int attrtype, uint32_t value)
1153{
1154 return (snl_add_msg_attr(nw, attrtype, sizeof(uint32_t), &value));
1155}
1156
1157static inline bool
1158snl_add_msg_attr_u64(struct snl_writer *nw, int attrtype, uint64_t value)
1159{
1160 return (snl_add_msg_attr(nw, attrtype, sizeof(uint64_t), &value));
1161}
1162
1163static inline bool
1164snl_add_msg_attr_s8(struct snl_writer *nw, int attrtype, int8_t value)
1165{
1166 return (snl_add_msg_attr(nw, attrtype, sizeof(int8_t), &value));
1167}
1168
1169static inline bool
1170snl_add_msg_attr_s16(struct snl_writer *nw, int attrtype, int16_t value)
1171{
1172 return (snl_add_msg_attr(nw, attrtype, sizeof(int16_t), &value));
1173}
1174
1175static inline bool
1176snl_add_msg_attr_s32(struct snl_writer *nw, int attrtype, int32_t value)
1177{
1178 return (snl_add_msg_attr(nw, attrtype, sizeof(int32_t), &value));
1179}
1180
1181static inline bool
1182snl_add_msg_attr_s64(struct snl_writer *nw, int attrtype, int64_t value)
1183{
1184 return (snl_add_msg_attr(nw, attrtype, sizeof(int64_t), &value));
1185}
1186
1187static inline bool
1188snl_add_msg_attr_flag(struct snl_writer *nw, int attrtype)
1189{
1190 return (snl_add_msg_attr(nw, attrtype, 0, NULL));
1191}
1192
1193static inline bool
1194snl_add_msg_attr_string(struct snl_writer *nw, int attrtype, const char *str)
1195{
1196 return (snl_add_msg_attr(nw, attrtype, strlen(str) + 1, str));
1197}
1198
1199
1200static inline int
1201snl_get_msg_offset(const struct snl_writer *nw)
1202{
1203 return (nw->offset - ((char *)nw->hdr - nw->base));
1204}
1205
1206static inline void *
1207_snl_restore_msg_offset(const struct snl_writer *nw, int off)
1208{
1209 return ((void *)((char *)nw->hdr + off));
1210}
1211#define snl_restore_msg_offset(_ns, _off, _t) ((_t *)_snl_restore_msg_offset(_ns, _off))
1212
1213static inline int
1214snl_add_msg_attr_nested(struct snl_writer *nw, int attrtype)
1215{
1216 int off = snl_get_msg_offset(nw);
1217 struct nlattr *nla = snl_reserve_msg_data(nw, sizeof(struct nlattr), struct nlattr);
1218 if (__predict_false(nla == NULL))
1219 return (0);
1220 nla->nla_type = attrtype;
1221 return (off);
1222}
1223
1224static inline void
1225snl_end_attr_nested(const struct snl_writer *nw, int off)
1226{
1227 if (!nw->error) {
1228 struct nlattr *nla = snl_restore_msg_offset(nw, off, struct nlattr);
1229 nla->nla_len = NETLINK_ALIGN(snl_get_msg_offset(nw) - off);
1230 }
1231}
1232
1233static inline struct nlmsghdr *
1234snl_create_msg_request(struct snl_writer *nw, int nlmsg_type)
1235{
1236 assert(nw->hdr == NULL);
1237
1238 struct nlmsghdr *hdr = snl_reserve_msg_object(nw, struct nlmsghdr);
1239 hdr->nlmsg_type = nlmsg_type;
1240 hdr->nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK;
1241 nw->hdr = hdr;
1242
1243 return (hdr);
1244}
1245
1246static void
1247snl_abort_msg(struct snl_writer *nw)
1248{
1249 if (nw->hdr != NULL) {
1250 int offset = (char *)(&nw->base[nw->offset]) - (char *)(nw->hdr);
1251
1252 nw->offset -= offset;
1253 nw->hdr = NULL;
1254 }
1255}
1256
1257static inline struct nlmsghdr *
1258snl_finalize_msg(struct snl_writer *nw)
1259{
1260 if (nw->error)
1261 snl_abort_msg(nw);
1262 if (nw->hdr != NULL) {
1263 struct nlmsghdr *hdr = nw->hdr;
1264
1265 int offset = (char *)(&nw->base[nw->offset]) - (char *)(nw->hdr);
1266 hdr->nlmsg_len = offset;
1267 hdr->nlmsg_seq = snl_get_seq(nw->ss);
1268 nw->hdr = NULL;
1269
1270 return (hdr);
1271 }
1272 return (NULL);
1273}
1274
1275static inline bool
1276snl_send_msgs(struct snl_writer *nw)
1277{
1278 int offset = nw->offset;
1279
1280 assert(nw->hdr == NULL);
1281 nw->offset = 0;
1282
1283 return (snl_send(nw->ss, nw->base, offset));
1284}
1285
1286static const struct snl_hdr_parser *snl_all_core_parsers[] = {
1287 &snl_errmsg_parser, &snl_donemsg_parser,
1288 &_nla_bit_parser, &_nla_bitset_parser,
1289};
1290
1291#endif