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