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