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