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