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