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
 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