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