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