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 = (_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
73 struct 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
80 static inline struct linear_buffer *
lb_init(uint32_t size)81 lb_init(uint32_t size)
82 {
83 struct linear_buffer *lb = 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
93 static inline void
lb_free(struct linear_buffer * lb)94 lb_free(struct linear_buffer *lb)
95 {
96 free(lb);
97 }
98
99 static inline char *
lb_allocz(struct linear_buffer * lb,int len)100 lb_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 void *data = (void *)(lb->base + lb->offset);
106 lb->offset += len;
107 return (data);
108 }
109
110 static inline void
lb_clear(struct linear_buffer * lb)111 lb_clear(struct linear_buffer *lb)
112 {
113 memset(lb->base, 0, lb->offset);
114 lb->offset = 0;
115 }
116
117 struct 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
130 typedef void snl_parse_field_f(struct snl_state *ss, void *hdr, void *target);
131 struct snl_field_parser {
132 uint16_t off_in;
133 uint16_t off_out;
134 snl_parse_field_f *cb;
135 };
136
137 typedef bool snl_parse_attr_f(struct snl_state *ss, struct nlattr *attr,
138 const void *arg, void *target);
139 struct 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
151 typedef bool snl_parse_post_f(struct snl_state *ss, void *target);
152
153 struct 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) \
164 static 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) \
178 static 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) \
190 static 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
201 static inline void *
snl_allocz(struct snl_state * ss,int len)202 snl_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
224 static inline void
snl_clear_lb(struct snl_state * ss)225 snl_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
240 static void
snl_free(struct snl_state * ss)241 snl_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
254 static inline bool
snl_init(struct snl_state * ss,int netlink_family)255 snl_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 = 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
293 static inline bool
snl_send(struct snl_state * ss,void * data,int sz)294 snl_send(struct snl_state *ss, void *data, int sz)
295 {
296 return (send(ss->fd, data, sz, 0) == sz);
297 }
298
299 static inline bool
snl_send_message(struct snl_state * ss,struct nlmsghdr * hdr)300 snl_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
307 static inline uint32_t
snl_get_seq(struct snl_state * ss)308 snl_get_seq(struct snl_state *ss)
309 {
310 return (++ss->seq);
311 }
312
313 struct 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 };
320 static inline bool parse_cmsg(struct snl_state *ss, const struct msghdr *msg,
321 struct snl_msg_info *attrs);
322
323 static inline struct nlmsghdr *
snl_read_message_dbg(struct snl_state * ss,struct snl_msg_info * cinfo)324 snl_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
362 static inline struct nlmsghdr *
snl_read_message(struct snl_state * ss)363 snl_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
393 static inline struct nlmsghdr *
snl_read_reply(struct snl_state * ss,uint32_t nlmsg_seq)394 snl_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 */
409 static inline void
snl_verify_parsers(const struct snl_hdr_parser ** parser,int count)410 snl_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
423 static const struct snl_attr_parser *
find_parser(const struct snl_attr_parser * ps,int pslen,int key)424 find_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
447 static inline bool
snl_parse_attrs_raw(struct snl_state * ss,struct nlattr * nla_head,int len,const struct snl_attr_parser * ps,int pslen,void * target)448 snl_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
467 static inline bool
snl_parse_attrs(struct snl_state * ss,struct nlmsghdr * hdr,int hdrlen,const struct snl_attr_parser * ps,int pslen,void * target)468 snl_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
478 static inline void
snl_parse_fields(struct snl_state * ss,struct nlmsghdr * hdr,int hdrlen __unused,const struct snl_field_parser * ps,int pslen,void * target)479 snl_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
491 static inline bool
snl_parse_header(struct snl_state * ss,void * hdr,int len,const struct snl_hdr_parser * parser,void * target)492 snl_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, hdr, parser->in_hdr_size, parser->fp, parser->fp_size, target);
499
500 nla_head = (struct nlattr *)(void *)((char *)hdr + parser->in_hdr_size);
501 bool result = snl_parse_attrs_raw(ss, nla_head, len - parser->in_hdr_size,
502 parser->np, parser->np_size, target);
503
504 if (result && parser->cb_post != NULL)
505 result = parser->cb_post(ss, target);
506
507 return (result);
508 }
509
510 static inline bool
snl_parse_nlmsg(struct snl_state * ss,struct nlmsghdr * hdr,const struct snl_hdr_parser * parser,void * target)511 snl_parse_nlmsg(struct snl_state *ss, struct nlmsghdr *hdr,
512 const struct snl_hdr_parser *parser, void *target)
513 {
514 return (snl_parse_header(ss, hdr + 1, hdr->nlmsg_len - sizeof(*hdr), parser, target));
515 }
516
517 static inline bool
snl_attr_get_flag(struct snl_state * ss __unused,struct nlattr * nla,const void * arg __unused,void * target)518 snl_attr_get_flag(struct snl_state *ss __unused, struct nlattr *nla, const void *arg __unused,
519 void *target)
520 {
521 if (NLA_DATA_LEN(nla) == 0) {
522 *((uint8_t *)target) = 1;
523 return (true);
524 }
525 return (false);
526 }
527
528 static inline bool
snl_attr_get_bytes(struct snl_state * ss __unused,struct nlattr * nla,const void * arg,void * target)529 snl_attr_get_bytes(struct snl_state *ss __unused, struct nlattr *nla, const void *arg,
530 void *target)
531 {
532 if ((size_t)NLA_DATA_LEN(nla) != (size_t)arg)
533 return (false);
534
535 memcpy(target, NLA_DATA_CONST(nla), (size_t)arg);
536
537 return (true);
538 }
539
540 static inline bool
snl_attr_get_bool(struct snl_state * ss __unused,struct nlattr * nla,const void * arg __unused,void * target)541 snl_attr_get_bool(struct snl_state *ss __unused, struct nlattr *nla,
542 const void *arg __unused, void *target)
543 {
544 if (NLA_DATA_LEN(nla) == sizeof(bool)) {
545 *((bool *)target) = *((const bool *)NLA_DATA_CONST(nla));
546 return (true);
547 }
548 return (false);
549 }
550
551 static inline bool
snl_attr_get_uint8(struct snl_state * ss __unused,struct nlattr * nla,const void * arg __unused,void * target)552 snl_attr_get_uint8(struct snl_state *ss __unused, struct nlattr *nla,
553 const void *arg __unused, void *target)
554 {
555 if (NLA_DATA_LEN(nla) == sizeof(uint8_t)) {
556 *((uint8_t *)target) = *((const uint8_t *)NLA_DATA_CONST(nla));
557 return (true);
558 }
559 return (false);
560 }
561
562 static inline bool
snl_attr_get_uint16(struct snl_state * ss __unused,struct nlattr * nla,const void * arg __unused,void * target)563 snl_attr_get_uint16(struct snl_state *ss __unused, struct nlattr *nla,
564 const void *arg __unused, void *target)
565 {
566 if (NLA_DATA_LEN(nla) == sizeof(uint16_t)) {
567 *((uint16_t *)target) = *((const uint16_t *)NLA_DATA_CONST(nla));
568 return (true);
569 }
570 return (false);
571 }
572
573 static inline bool
snl_attr_get_uint32(struct snl_state * ss __unused,struct nlattr * nla,const void * arg __unused,void * target)574 snl_attr_get_uint32(struct snl_state *ss __unused, struct nlattr *nla,
575 const void *arg __unused, void *target)
576 {
577 if (NLA_DATA_LEN(nla) == sizeof(uint32_t)) {
578 *((uint32_t *)target) = *((const uint32_t *)NLA_DATA_CONST(nla));
579 return (true);
580 }
581 return (false);
582 }
583
584 static inline bool
snl_attr_get_uint64(struct snl_state * ss __unused,struct nlattr * nla,const void * arg __unused,void * target)585 snl_attr_get_uint64(struct snl_state *ss __unused, struct nlattr *nla,
586 const void *arg __unused, void *target)
587 {
588 if (NLA_DATA_LEN(nla) == sizeof(uint64_t)) {
589 memcpy(target, NLA_DATA_CONST(nla), sizeof(uint64_t));
590 return (true);
591 }
592 return (false);
593 }
594
595 static inline bool
snl_attr_get_int8(struct snl_state * ss,struct nlattr * nla,const void * arg,void * target)596 snl_attr_get_int8(struct snl_state *ss, struct nlattr *nla, const void *arg,
597 void *target)
598 {
599 return (snl_attr_get_uint8(ss, nla, arg, target));
600 }
601
602 static inline bool
snl_attr_get_int16(struct snl_state * ss,struct nlattr * nla,const void * arg,void * target)603 snl_attr_get_int16(struct snl_state *ss, struct nlattr *nla, const void *arg,
604 void *target)
605 {
606 return (snl_attr_get_uint16(ss, nla, arg, target));
607 }
608
609 static inline bool
snl_attr_get_int32(struct snl_state * ss,struct nlattr * nla,const void * arg,void * target)610 snl_attr_get_int32(struct snl_state *ss, struct nlattr *nla, const void *arg,
611 void *target)
612 {
613 return (snl_attr_get_uint32(ss, nla, arg, target));
614 }
615
616 static inline bool
snl_attr_get_int64(struct snl_state * ss,struct nlattr * nla,const void * arg,void * target)617 snl_attr_get_int64(struct snl_state *ss, struct nlattr *nla, const void *arg,
618 void *target)
619 {
620 return (snl_attr_get_uint64(ss, nla, arg, target));
621 }
622
623 static inline bool
snl_attr_get_string(struct snl_state * ss __unused,struct nlattr * nla,const void * arg __unused,void * target)624 snl_attr_get_string(struct snl_state *ss __unused, struct nlattr *nla,
625 const void *arg __unused, void *target)
626 {
627 size_t maxlen = NLA_DATA_LEN(nla);
628
629 if (strnlen((char *)NLA_DATA(nla), maxlen) < maxlen) {
630 *((char **)target) = (char *)NLA_DATA(nla);
631 return (true);
632 }
633 return (false);
634 }
635
636 static inline bool
snl_attr_get_stringn(struct snl_state * ss,struct nlattr * nla,const void * arg __unused,void * target)637 snl_attr_get_stringn(struct snl_state *ss, struct nlattr *nla,
638 const void *arg __unused, void *target)
639 {
640 int maxlen = NLA_DATA_LEN(nla);
641
642 char *buf = snl_allocz(ss, maxlen + 1);
643 if (buf == NULL)
644 return (false);
645 buf[maxlen] = '\0';
646 memcpy(buf, NLA_DATA(nla), maxlen);
647
648 *((char **)target) = buf;
649 return (true);
650 }
651
652 static inline bool
snl_attr_copy_string(struct snl_state * ss,struct nlattr * nla,const void * arg,void * target)653 snl_attr_copy_string(struct snl_state *ss, struct nlattr *nla,
654 const void *arg, void *target)
655 {
656 char *tmp;
657
658 if (snl_attr_get_string(ss, nla, NULL, &tmp)) {
659 strlcpy(target, tmp, (size_t)arg);
660 return (true);
661 }
662 return (false);
663 }
664
665 static inline bool
snl_attr_dup_string(struct snl_state * ss __unused,struct nlattr * nla,const void * arg __unused,void * target)666 snl_attr_dup_string(struct snl_state *ss __unused, struct nlattr *nla,
667 const void *arg __unused, void *target)
668 {
669 size_t maxlen = NLA_DATA_LEN(nla);
670
671 if (strnlen((char *)NLA_DATA(nla), maxlen) < maxlen) {
672 char *buf = snl_allocz(ss, maxlen);
673 if (buf == NULL)
674 return (false);
675 memcpy(buf, NLA_DATA(nla), maxlen);
676 *((char **)target) = buf;
677 return (true);
678 }
679 return (false);
680 }
681
682 static inline bool
snl_attr_get_nested(struct snl_state * ss,struct nlattr * nla,const void * arg,void * target)683 snl_attr_get_nested(struct snl_state *ss, struct nlattr *nla, const void *arg, void *target)
684 {
685 const struct snl_hdr_parser *p = (const struct snl_hdr_parser *)arg;
686
687 /* Assumes target points to the beginning of the structure */
688 return (snl_parse_header(ss, NLA_DATA(nla), NLA_DATA_LEN(nla), p, target));
689 }
690
691 struct snl_parray {
692 uint32_t count;
693 void **items;
694 };
695
696 static inline bool
snl_attr_get_parray_sz(struct snl_state * ss,struct nlattr * container_nla,uint32_t start_size,const void * arg,void * target)697 snl_attr_get_parray_sz(struct snl_state *ss, struct nlattr *container_nla,
698 uint32_t start_size, const void *arg, void *target)
699 {
700 const struct snl_hdr_parser *p = (const struct snl_hdr_parser *)arg;
701 struct snl_parray *array = target;
702 struct nlattr *nla;
703 uint32_t count = 0, size = start_size;
704
705 if (p->out_size == 0)
706 return (false);
707
708 array->items = snl_allocz(ss, size * sizeof(void *));
709 if (array->items == NULL)
710 return (false);
711
712 /*
713 * If the provided parser is an attribute parser, assume that each
714 * nla in the container nla is the container nla itself and parse
715 * the contents of this nla.
716 * Otherwise, run the parser on raw data, assuming the header of this
717 * data has u16 field with total size in the beginning.
718 */
719 uint32_t data_off = 0;
720
721 if (p->in_hdr_size == 0)
722 data_off = sizeof(struct nlattr);
723
724 NLA_FOREACH(nla, NLA_DATA(container_nla), NLA_DATA_LEN(container_nla)) {
725 void *item = snl_allocz(ss, p->out_size);
726
727 if (item == NULL)
728 return (false);
729
730 void *data = (char *)(void *)nla + data_off;
731 int data_len = nla->nla_len - data_off;
732
733 if (!(snl_parse_header(ss, data, data_len, p, item)))
734 return (false);
735
736 if (count == size) {
737 uint32_t new_size = size * 2;
738 void **new_array = snl_allocz(ss, new_size *sizeof(void *));
739
740 memcpy(new_array, array->items, size * sizeof(void *));
741 array->items = new_array;
742 size = new_size;
743 }
744 array->items[count++] = item;
745 }
746 array->count = count;
747
748 return (true);
749 }
750
751 /*
752 * Parses and stores the unknown-size array.
753 * Assumes each array item is a container and the NLAs in the container are parsable
754 * by the parser provided in @arg.
755 * Assumes @target is struct snl_parray
756 */
757 static inline bool
snl_attr_get_parray(struct snl_state * ss,struct nlattr * nla,const void * arg,void * target)758 snl_attr_get_parray(struct snl_state *ss, struct nlattr *nla, const void *arg, void *target)
759 {
760 return (snl_attr_get_parray_sz(ss, nla, 8, arg, target));
761 }
762
763 static inline bool
snl_attr_get_nla(struct snl_state * ss __unused,struct nlattr * nla,const void * arg __unused,void * target)764 snl_attr_get_nla(struct snl_state *ss __unused, struct nlattr *nla,
765 const void *arg __unused, void *target)
766 {
767 *((struct nlattr **)target) = nla;
768 return (true);
769 }
770
771 static inline bool
snl_attr_dup_nla(struct snl_state * ss,struct nlattr * nla,const void * arg __unused,void * target)772 snl_attr_dup_nla(struct snl_state *ss, struct nlattr *nla,
773 const void *arg __unused, void *target)
774 {
775 void *ptr = snl_allocz(ss, nla->nla_len);
776
777 if (ptr != NULL) {
778 memcpy(ptr, nla, nla->nla_len);
779 *((void **)target) = ptr;
780 return (true);
781 }
782 return (false);
783 }
784
785 static inline bool
snl_attr_copy_struct(struct snl_state * ss,struct nlattr * nla,const void * arg __unused,void * target)786 snl_attr_copy_struct(struct snl_state *ss, struct nlattr *nla,
787 const void *arg __unused, void *target)
788 {
789 void *ptr = snl_allocz(ss, NLA_DATA_LEN(nla));
790
791 if (ptr != NULL) {
792 memcpy(ptr, NLA_DATA(nla), NLA_DATA_LEN(nla));
793 *((void **)target) = ptr;
794 return (true);
795 }
796 return (false);
797 }
798
799 static inline bool
snl_attr_dup_struct(struct snl_state * ss,struct nlattr * nla,const void * arg __unused,void * target)800 snl_attr_dup_struct(struct snl_state *ss, struct nlattr *nla,
801 const void *arg __unused, void *target)
802 {
803 void *ptr = snl_allocz(ss, NLA_DATA_LEN(nla));
804
805 if (ptr != NULL) {
806 memcpy(ptr, NLA_DATA(nla), NLA_DATA_LEN(nla));
807 *((void **)target) = ptr;
808 return (true);
809 }
810 return (false);
811 }
812
813 struct snl_attr_bit {
814 uint32_t bit_index;
815 char *bit_name;
816 int bit_value;
817 };
818
819 struct snl_attr_bits {
820 uint32_t num_bits;
821 struct snl_attr_bit **bits;
822 };
823
824 #define _OUT(_field) offsetof(struct snl_attr_bit, _field)
825 static const struct snl_attr_parser _nla_p_bit[] = {
826 { .type = NLA_BITSET_BIT_INDEX, .off = _OUT(bit_index), .cb = snl_attr_get_uint32 },
827 { .type = NLA_BITSET_BIT_NAME, .off = _OUT(bit_name), .cb = snl_attr_dup_string },
828 { .type = NLA_BITSET_BIT_VALUE, .off = _OUT(bit_value), .cb = snl_attr_get_flag },
829 };
830 #undef _OUT
831 SNL_DECLARE_ATTR_PARSER_EXT(_nla_bit_parser, sizeof(struct snl_attr_bit), _nla_p_bit, NULL);
832
833 struct snl_attr_bitset {
834 uint32_t nla_bitset_size;
835 uint32_t *nla_bitset_mask;
836 uint32_t *nla_bitset_value;
837 struct snl_attr_bits bits;
838 };
839
840 #define _OUT(_field) offsetof(struct snl_attr_bitset, _field)
841 static const struct snl_attr_parser _nla_p_bitset[] = {
842 { .type = NLA_BITSET_SIZE, .off = _OUT(nla_bitset_size), .cb = snl_attr_get_uint32 },
843 { .type = NLA_BITSET_BITS, .off = _OUT(bits), .cb = snl_attr_get_parray, .arg = &_nla_bit_parser },
844 { .type = NLA_BITSET_VALUE, .off = _OUT(nla_bitset_mask), .cb = snl_attr_dup_nla },
845 { .type = NLA_BITSET_MASK, .off = _OUT(nla_bitset_value), .cb = snl_attr_dup_nla },
846 };
847
848 static inline bool
_cb_p_bitset(struct snl_state * ss __unused,void * _target)849 _cb_p_bitset(struct snl_state *ss __unused, void *_target)
850 {
851 struct snl_attr_bitset *target = _target;
852
853 uint32_t sz_bytes = _roundup2(target->nla_bitset_size, 32) / 8;
854
855 if (target->nla_bitset_mask != NULL) {
856 struct nlattr *nla = (struct nlattr *)target->nla_bitset_mask;
857 uint32_t data_len = NLA_DATA_LEN(nla);
858
859 if (data_len != sz_bytes || _roundup2(data_len, 4) != data_len)
860 return (false);
861 target->nla_bitset_mask = (uint32_t *)NLA_DATA(nla);
862 }
863
864 if (target->nla_bitset_value != NULL) {
865 struct nlattr *nla = (struct nlattr *)target->nla_bitset_value;
866 uint32_t data_len = NLA_DATA_LEN(nla);
867
868 if (data_len != sz_bytes || _roundup2(data_len, 4) != data_len)
869 return (false);
870 target->nla_bitset_value = (uint32_t *)NLA_DATA(nla);
871 }
872 return (true);
873 }
874 #undef _OUT
875 SNL_DECLARE_ATTR_PARSER_EXT(_nla_bitset_parser,
876 sizeof(struct snl_attr_bitset),
877 _nla_p_bitset, _cb_p_bitset);
878
879 /*
880 * Parses the compact bitset representation.
881 */
882 static inline bool
snl_attr_get_bitset_c(struct snl_state * ss,struct nlattr * nla,const void * arg __unused,void * _target)883 snl_attr_get_bitset_c(struct snl_state *ss, struct nlattr *nla,
884 const void *arg __unused, void *_target)
885 {
886 const struct snl_hdr_parser *p = &_nla_bitset_parser;
887 struct snl_attr_bitset *target = _target;
888
889 /* Assumes target points to the beginning of the structure */
890 if (!snl_parse_header(ss, NLA_DATA(nla), NLA_DATA_LEN(nla), p, _target))
891 return (false);
892 if (target->nla_bitset_mask == NULL || target->nla_bitset_value == NULL)
893 return (false);
894 return (true);
895 }
896
897 static inline void
snl_field_get_uint8(struct snl_state * ss __unused,void * src,void * target)898 snl_field_get_uint8(struct snl_state *ss __unused, void *src, void *target)
899 {
900 *((uint8_t *)target) = *((uint8_t *)src);
901 }
902
903 static inline void
snl_field_get_uint16(struct snl_state * ss __unused,void * src,void * target)904 snl_field_get_uint16(struct snl_state *ss __unused, void *src, void *target)
905 {
906 *((uint16_t *)target) = *((uint16_t *)src);
907 }
908
909 static inline void
snl_field_get_uint32(struct snl_state * ss __unused,void * src,void * target)910 snl_field_get_uint32(struct snl_state *ss __unused, void *src, void *target)
911 {
912 *((uint32_t *)target) = *((uint32_t *)src);
913 }
914
915 static inline void
snl_field_get_ptr(struct snl_state * ss __unused,void * src,void * target)916 snl_field_get_ptr(struct snl_state *ss __unused, void *src, void *target)
917 {
918 *((void **)target) = src;
919 }
920
921 struct snl_errmsg_data {
922 struct nlmsghdr *orig_hdr;
923 int error;
924 uint32_t error_offs;
925 char *error_str;
926 struct nlattr *cookie;
927 };
928
929 #define _IN(_field) offsetof(struct nlmsgerr, _field)
930 #define _OUT(_field) offsetof(struct snl_errmsg_data, _field)
931 static const struct snl_attr_parser nla_p_errmsg[] = {
932 { .type = NLMSGERR_ATTR_MSG, .off = _OUT(error_str), .cb = snl_attr_get_string },
933 { .type = NLMSGERR_ATTR_OFFS, .off = _OUT(error_offs), .cb = snl_attr_get_uint32 },
934 { .type = NLMSGERR_ATTR_COOKIE, .off = _OUT(cookie), .cb = snl_attr_get_nla },
935 };
936
937 static const struct snl_field_parser nlf_p_errmsg[] = {
938 { .off_in = _IN(error), .off_out = _OUT(error), .cb = snl_field_get_uint32 },
939 { .off_in = _IN(msg), .off_out = _OUT(orig_hdr), .cb = snl_field_get_ptr },
940 };
941 #undef _IN
942 #undef _OUT
943 SNL_DECLARE_PARSER(snl_errmsg_parser, struct nlmsgerr, nlf_p_errmsg, nla_p_errmsg);
944
945 #define _IN(_field) offsetof(struct nlmsgerr, _field)
946 #define _OUT(_field) offsetof(struct snl_errmsg_data, _field)
947 static const struct snl_field_parser nlf_p_donemsg[] = {
948 { .off_in = _IN(error), .off_out = _OUT(error), .cb = snl_field_get_uint32 },
949 };
950 #undef _IN
951 #undef _OUT
952 SNL_DECLARE_FIELD_PARSER(snl_donemsg_parser, struct nlmsgerr, nlf_p_donemsg);
953
954 static inline bool
snl_parse_errmsg(struct snl_state * ss,struct nlmsghdr * hdr,struct snl_errmsg_data * e)955 snl_parse_errmsg(struct snl_state *ss, struct nlmsghdr *hdr, struct snl_errmsg_data *e)
956 {
957 if ((hdr->nlmsg_flags & NLM_F_CAPPED) != 0)
958 return (snl_parse_nlmsg(ss, hdr, &snl_errmsg_parser, e));
959
960 const struct snl_hdr_parser *ps = &snl_errmsg_parser;
961 struct nlmsgerr *errmsg = (struct nlmsgerr *)(hdr + 1);
962 int hdrlen = sizeof(int) + NLMSG_ALIGN(errmsg->msg.nlmsg_len);
963 struct nlattr *attr_head = (struct nlattr *)(void *)((char *)errmsg + hdrlen);
964 int attr_len = hdr->nlmsg_len - sizeof(struct nlmsghdr) - hdrlen;
965
966 snl_parse_fields(ss, (struct nlmsghdr *)errmsg, hdrlen, ps->fp, ps->fp_size, e);
967 return (snl_parse_attrs_raw(ss, attr_head, attr_len, ps->np, ps->np_size, e));
968 }
969
970 static inline bool
snl_read_reply_code(struct snl_state * ss,uint32_t nlmsg_seq,struct snl_errmsg_data * e)971 snl_read_reply_code(struct snl_state *ss, uint32_t nlmsg_seq, struct snl_errmsg_data *e)
972 {
973 struct nlmsghdr *hdr = snl_read_reply(ss, nlmsg_seq);
974
975 if (hdr == NULL) {
976 e->error = EINVAL;
977 } else if (hdr->nlmsg_type == NLMSG_ERROR) {
978 if (!snl_parse_errmsg(ss, hdr, e))
979 e->error = EINVAL;
980 return (e->error == 0);
981 }
982
983 return (false);
984 }
985
986 #define _OUT(_field) offsetof(struct snl_msg_info, _field)
987 static const struct snl_attr_parser _nla_p_cinfo[] = {
988 { .type = NLMSGINFO_ATTR_PROCESS_ID, .off = _OUT(process_id), .cb = snl_attr_get_uint32 },
989 { .type = NLMSGINFO_ATTR_PORT_ID, .off = _OUT(port_id), .cb = snl_attr_get_uint32 },
990 { .type = NLMSGINFO_ATTR_SEQ_ID, .off = _OUT(seq_id), .cb = snl_attr_get_uint32 },
991 };
992 #undef _OUT
993 SNL_DECLARE_ATTR_PARSER(snl_msg_info_parser, _nla_p_cinfo);
994
995 static inline bool
parse_cmsg(struct snl_state * ss,const struct msghdr * msg,struct snl_msg_info * attrs)996 parse_cmsg(struct snl_state *ss, const struct msghdr *msg, struct snl_msg_info *attrs)
997 {
998 for (struct cmsghdr *cmsg = CMSG_FIRSTHDR(msg); cmsg != NULL;
999 cmsg = CMSG_NXTHDR(msg, cmsg)) {
1000 if (cmsg->cmsg_level != SOL_NETLINK || cmsg->cmsg_type != NETLINK_MSG_INFO)
1001 continue;
1002
1003 void *data = CMSG_DATA(cmsg);
1004 int len = cmsg->cmsg_len - ((char *)data - (char *)cmsg);
1005 const struct snl_hdr_parser *ps = &snl_msg_info_parser;
1006
1007 return (snl_parse_attrs_raw(ss, data, len, ps->np, ps->np_size, attrs));
1008 }
1009
1010 return (false);
1011 }
1012
1013 /*
1014 * Assumes e is zeroed
1015 */
1016 static inline struct nlmsghdr *
snl_read_reply_multi(struct snl_state * ss,uint32_t nlmsg_seq,struct snl_errmsg_data * e)1017 snl_read_reply_multi(struct snl_state *ss, uint32_t nlmsg_seq, struct snl_errmsg_data *e)
1018 {
1019 struct nlmsghdr *hdr = snl_read_reply(ss, nlmsg_seq);
1020
1021 if (hdr == NULL) {
1022 e->error = EINVAL;
1023 } else if (hdr->nlmsg_type == NLMSG_ERROR) {
1024 if (!snl_parse_errmsg(ss, hdr, e))
1025 e->error = EINVAL;
1026 } else if (hdr->nlmsg_type == NLMSG_DONE) {
1027 snl_parse_nlmsg(ss, hdr, &snl_donemsg_parser, e);
1028 } else
1029 return (hdr);
1030
1031 return (NULL);
1032 }
1033
1034
1035 /* writer logic */
1036 struct snl_writer {
1037 char *base;
1038 uint32_t offset;
1039 uint32_t size;
1040 struct nlmsghdr *hdr;
1041 struct snl_state *ss;
1042 bool error;
1043 };
1044
1045 static inline void
snl_init_writer(struct snl_state * ss,struct snl_writer * nw)1046 snl_init_writer(struct snl_state *ss, struct snl_writer *nw)
1047 {
1048 nw->size = SNL_WRITER_BUFFER_SIZE;
1049 nw->base = snl_allocz(ss, nw->size);
1050 if (nw->base == NULL) {
1051 nw->error = true;
1052 nw->size = 0;
1053 }
1054
1055 nw->offset = 0;
1056 nw->hdr = NULL;
1057 nw->error = false;
1058 nw->ss = ss;
1059 }
1060
1061 static inline bool
snl_realloc_msg_buffer(struct snl_writer * nw,size_t sz)1062 snl_realloc_msg_buffer(struct snl_writer *nw, size_t sz)
1063 {
1064 uint32_t new_size = nw->size * 2;
1065
1066 while (new_size < nw->size + sz)
1067 new_size *= 2;
1068
1069 if (nw->error)
1070 return (false);
1071
1072 if (snl_allocz(nw->ss, new_size) == NULL) {
1073 nw->error = true;
1074 return (false);
1075 }
1076 nw->size = new_size;
1077
1078 void *new_base = nw->ss->lb->base;
1079 if (new_base != nw->base) {
1080 memcpy(new_base, nw->base, nw->offset);
1081 if (nw->hdr != NULL) {
1082 int hdr_off = (char *)(nw->hdr) - nw->base;
1083
1084 nw->hdr = (struct nlmsghdr *)
1085 (void *)((char *)new_base + hdr_off);
1086 }
1087 nw->base = new_base;
1088 }
1089
1090 return (true);
1091 }
1092
1093 static inline void *
snl_reserve_msg_data_raw(struct snl_writer * nw,size_t sz)1094 snl_reserve_msg_data_raw(struct snl_writer *nw, size_t sz)
1095 {
1096 sz = NETLINK_ALIGN(sz);
1097
1098 if (__predict_false(nw->offset + sz > nw->size)) {
1099 if (!snl_realloc_msg_buffer(nw, sz))
1100 return (NULL);
1101 }
1102
1103 void *data_ptr = &nw->base[nw->offset];
1104 nw->offset += sz;
1105
1106 return (data_ptr);
1107 }
1108 #define snl_reserve_msg_object(_ns, _t) ((_t *)snl_reserve_msg_data_raw(_ns, sizeof(_t)))
1109 #define snl_reserve_msg_data(_ns, _sz, _t) ((_t *)snl_reserve_msg_data_raw(_ns, _sz))
1110
1111 static inline void *
_snl_reserve_msg_attr(struct snl_writer * nw,uint16_t nla_type,uint16_t sz)1112 _snl_reserve_msg_attr(struct snl_writer *nw, uint16_t nla_type, uint16_t sz)
1113 {
1114 sz += sizeof(struct nlattr);
1115
1116 struct nlattr *nla = snl_reserve_msg_data(nw, sz, struct nlattr);
1117 if (__predict_false(nla == NULL))
1118 return (NULL);
1119 nla->nla_type = nla_type;
1120 nla->nla_len = sz;
1121
1122 return ((void *)(nla + 1));
1123 }
1124 #define snl_reserve_msg_attr(_ns, _at, _t) ((_t *)_snl_reserve_msg_attr(_ns, _at, sizeof(_t)))
1125
1126 static inline bool
snl_add_msg_attr(struct snl_writer * nw,int attr_type,int attr_len,const void * data)1127 snl_add_msg_attr(struct snl_writer *nw, int attr_type, int attr_len, const void *data)
1128 {
1129 int required_len = NLA_ALIGN(attr_len + sizeof(struct nlattr));
1130
1131 if (__predict_false(nw->offset + required_len > nw->size)) {
1132 if (!snl_realloc_msg_buffer(nw, required_len))
1133 return (false);
1134 }
1135
1136 struct nlattr *nla = (struct nlattr *)(void *)(&nw->base[nw->offset]);
1137
1138 nla->nla_len = attr_len + sizeof(struct nlattr);
1139 nla->nla_type = attr_type;
1140 if (attr_len > 0) {
1141 if ((attr_len % 4) != 0) {
1142 /* clear padding bytes */
1143 bzero((char *)nla + required_len - 4, 4);
1144 }
1145 memcpy((nla + 1), data, attr_len);
1146 }
1147 nw->offset += required_len;
1148 return (true);
1149 }
1150
1151 static inline bool
snl_add_msg_attr_raw(struct snl_writer * nw,const struct nlattr * nla_src)1152 snl_add_msg_attr_raw(struct snl_writer *nw, const struct nlattr *nla_src)
1153 {
1154 int attr_len = nla_src->nla_len - sizeof(struct nlattr);
1155
1156 assert(attr_len >= 0);
1157
1158 return (snl_add_msg_attr(nw, nla_src->nla_type, attr_len, (const void *)(nla_src + 1)));
1159 }
1160
1161 static inline bool
snl_add_msg_attr_bool(struct snl_writer * nw,int attrtype,bool value)1162 snl_add_msg_attr_bool(struct snl_writer *nw, int attrtype, bool value)
1163 {
1164 return (snl_add_msg_attr(nw, attrtype, sizeof(bool), &value));
1165 }
1166
1167 static inline bool
snl_add_msg_attr_u8(struct snl_writer * nw,int attrtype,uint8_t value)1168 snl_add_msg_attr_u8(struct snl_writer *nw, int attrtype, uint8_t value)
1169 {
1170 return (snl_add_msg_attr(nw, attrtype, sizeof(uint8_t), &value));
1171 }
1172
1173 static inline bool
snl_add_msg_attr_u16(struct snl_writer * nw,int attrtype,uint16_t value)1174 snl_add_msg_attr_u16(struct snl_writer *nw, int attrtype, uint16_t value)
1175 {
1176 return (snl_add_msg_attr(nw, attrtype, sizeof(uint16_t), &value));
1177 }
1178
1179 static inline bool
snl_add_msg_attr_u32(struct snl_writer * nw,int attrtype,uint32_t value)1180 snl_add_msg_attr_u32(struct snl_writer *nw, int attrtype, uint32_t value)
1181 {
1182 return (snl_add_msg_attr(nw, attrtype, sizeof(uint32_t), &value));
1183 }
1184
1185 static inline bool
snl_add_msg_attr_u64(struct snl_writer * nw,int attrtype,uint64_t value)1186 snl_add_msg_attr_u64(struct snl_writer *nw, int attrtype, uint64_t value)
1187 {
1188 return (snl_add_msg_attr(nw, attrtype, sizeof(uint64_t), &value));
1189 }
1190
1191 static inline bool
snl_add_msg_attr_s8(struct snl_writer * nw,int attrtype,int8_t value)1192 snl_add_msg_attr_s8(struct snl_writer *nw, int attrtype, int8_t value)
1193 {
1194 return (snl_add_msg_attr(nw, attrtype, sizeof(int8_t), &value));
1195 }
1196
1197 static inline bool
snl_add_msg_attr_s16(struct snl_writer * nw,int attrtype,int16_t value)1198 snl_add_msg_attr_s16(struct snl_writer *nw, int attrtype, int16_t value)
1199 {
1200 return (snl_add_msg_attr(nw, attrtype, sizeof(int16_t), &value));
1201 }
1202
1203 static inline bool
snl_add_msg_attr_s32(struct snl_writer * nw,int attrtype,int32_t value)1204 snl_add_msg_attr_s32(struct snl_writer *nw, int attrtype, int32_t value)
1205 {
1206 return (snl_add_msg_attr(nw, attrtype, sizeof(int32_t), &value));
1207 }
1208
1209 static inline bool
snl_add_msg_attr_s64(struct snl_writer * nw,int attrtype,int64_t value)1210 snl_add_msg_attr_s64(struct snl_writer *nw, int attrtype, int64_t value)
1211 {
1212 return (snl_add_msg_attr(nw, attrtype, sizeof(int64_t), &value));
1213 }
1214
1215 static inline bool
snl_add_msg_attr_flag(struct snl_writer * nw,int attrtype)1216 snl_add_msg_attr_flag(struct snl_writer *nw, int attrtype)
1217 {
1218 return (snl_add_msg_attr(nw, attrtype, 0, NULL));
1219 }
1220
1221 static inline bool
snl_add_msg_attr_string(struct snl_writer * nw,int attrtype,const char * str)1222 snl_add_msg_attr_string(struct snl_writer *nw, int attrtype, const char *str)
1223 {
1224 return (snl_add_msg_attr(nw, attrtype, strlen(str) + 1, str));
1225 }
1226
1227
1228 static inline int
snl_get_msg_offset(const struct snl_writer * nw)1229 snl_get_msg_offset(const struct snl_writer *nw)
1230 {
1231 return (nw->offset - ((char *)nw->hdr - nw->base));
1232 }
1233
1234 static inline void *
_snl_restore_msg_offset(const struct snl_writer * nw,int off)1235 _snl_restore_msg_offset(const struct snl_writer *nw, int off)
1236 {
1237 return ((void *)((char *)nw->hdr + off));
1238 }
1239 #define snl_restore_msg_offset(_ns, _off, _t) ((_t *)_snl_restore_msg_offset(_ns, _off))
1240
1241 static inline int
snl_add_msg_attr_nested(struct snl_writer * nw,int attrtype)1242 snl_add_msg_attr_nested(struct snl_writer *nw, int attrtype)
1243 {
1244 int off = snl_get_msg_offset(nw);
1245 struct nlattr *nla = snl_reserve_msg_data(nw, sizeof(struct nlattr), struct nlattr);
1246 if (__predict_false(nla == NULL))
1247 return (0);
1248 nla->nla_type = attrtype;
1249 return (off);
1250 }
1251
1252 static inline void
snl_end_attr_nested(const struct snl_writer * nw,int off)1253 snl_end_attr_nested(const struct snl_writer *nw, int off)
1254 {
1255 if (!nw->error) {
1256 struct nlattr *nla = snl_restore_msg_offset(nw, off, struct nlattr);
1257 nla->nla_len = NETLINK_ALIGN(snl_get_msg_offset(nw) - off);
1258 }
1259 }
1260
1261 static inline struct nlmsghdr *
snl_create_msg_request(struct snl_writer * nw,int nlmsg_type)1262 snl_create_msg_request(struct snl_writer *nw, int nlmsg_type)
1263 {
1264 assert(nw->hdr == NULL);
1265
1266 struct nlmsghdr *hdr = snl_reserve_msg_object(nw, struct nlmsghdr);
1267 hdr->nlmsg_type = nlmsg_type;
1268 hdr->nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK;
1269 nw->hdr = hdr;
1270
1271 return (hdr);
1272 }
1273
1274 static void
snl_abort_msg(struct snl_writer * nw)1275 snl_abort_msg(struct snl_writer *nw)
1276 {
1277 if (nw->hdr != NULL) {
1278 int offset = (char *)(&nw->base[nw->offset]) - (char *)(nw->hdr);
1279
1280 nw->offset -= offset;
1281 nw->hdr = NULL;
1282 }
1283 }
1284
1285 static inline struct nlmsghdr *
snl_finalize_msg(struct snl_writer * nw)1286 snl_finalize_msg(struct snl_writer *nw)
1287 {
1288 if (nw->error)
1289 snl_abort_msg(nw);
1290 if (nw->hdr != NULL) {
1291 struct nlmsghdr *hdr = nw->hdr;
1292
1293 int offset = (char *)(&nw->base[nw->offset]) - (char *)(nw->hdr);
1294 hdr->nlmsg_len = offset;
1295 hdr->nlmsg_seq = snl_get_seq(nw->ss);
1296 nw->hdr = NULL;
1297
1298 return (hdr);
1299 }
1300 return (NULL);
1301 }
1302
1303 static inline bool
snl_send_msgs(struct snl_writer * nw)1304 snl_send_msgs(struct snl_writer *nw)
1305 {
1306 int offset = nw->offset;
1307
1308 assert(nw->hdr == NULL);
1309 nw->offset = 0;
1310
1311 return (snl_send(nw->ss, nw->base, offset));
1312 }
1313
1314 static const struct snl_hdr_parser *snl_all_core_parsers[] = {
1315 &snl_errmsg_parser, &snl_donemsg_parser,
1316 &_nla_bit_parser, &_nla_bitset_parser,
1317 };
1318
1319 #endif
1320