sys/netlink/netlink_snl.h

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