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