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