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
  48 #define _roundup2(x, y)         (((x)+((y)-1))&(~((y)-1)))
  49
  50 #define NETLINK_ALIGN_SIZE      sizeof(uint32_t)
  51 #define NETLINK_ALIGN(_len)     _roundup2(_len, NETLINK_ALIGN_SIZE)
  52
  53 #define NLA_ALIGN_SIZE          sizeof(uint32_t)
  54 #define NLA_HDRLEN              ((int)sizeof(struct nlattr))
  55 #define NLA_DATA_LEN(_nla)      ((int)((_nla)->nla_len - NLA_HDRLEN))
  56 #define NLA_DATA(_nla)          NL_ITEM_DATA(_nla, NLA_HDRLEN)
  57 #define NLA_DATA_CONST(_nla)    NL_ITEM_DATA_CONST(_nla, NLA_HDRLEN)
  58
  59 #define NLA_TYPE(_nla)          ((_nla)->nla_type & 0x3FFF)
  60
  61 #define NLA_NEXT(_attr) (struct nlattr *)(void *)((char *)_attr + NLA_ALIGN(_attr->nla_len))
  62
  63 #define _NLA_END(_start, _len)  ((char *)(_start) + (_len))
  64 #define NLA_FOREACH(_attr, _start, _len)      \
  65         for (_attr = (_start);          \
  66                 ((char *)_attr < _NLA_END(_start, _len)) && \
  67                 ((char *)NLA_NEXT(_attr) <= _NLA_END(_start, _len));    \
  68                 _attr =  NLA_NEXT(_attr))
  69
  70 #define NL_ARRAY_LEN(_a)        (sizeof(_a) / sizeof((_a)[0]))
  71
  72 struct linear_buffer {
  73         char            *base;  /* Base allocated memory pointer */
  74         uint32_t        offset; /* Currently used offset */
  75         uint32_t        size;   /* Total buffer size */
  76 };
  77
  78 static inline char *
  79 lb_allocz(struct linear_buffer *lb, int len)
  80 {
  81         len = roundup2(len, sizeof(uint64_t));
  82         if (lb->offset + len > lb->size)
  83                 return (NULL);
  84         void *data = (void *)(lb->base + lb->offset);
  85         lb->offset += len;
  86         return (data);
  87 }
  88
  89 static inline void
  90 lb_clear(struct linear_buffer *lb)
  91 {
  92         memset(lb->base, 0, lb->offset);
  93         lb->offset = 0;
  94 }
  95
  96 struct snl_state {
  97         int fd;
  98         char *buf;
  99         size_t off;
 100         size_t bufsize;
 101         size_t datalen;
 102         uint32_t seq;
 103         bool init_done;
 104         struct linear_buffer lb;
 105 };
 106 #define SCRATCH_BUFFER_SIZE     1024
 107
 108 typedef void snl_parse_field_f(struct snl_state *ss, void *hdr, void *target);
 109 struct snl_field_parser {
 110         uint16_t                off_in;
 111         uint16_t                off_out;
 112         snl_parse_field_f       *cb;
 113 };
 114
 115 typedef bool snl_parse_attr_f(struct snl_state *ss, struct nlattr *attr,
 116     const void *arg, void *target);
 117 struct snl_attr_parser {
 118         uint16_t                type;   /* Attribute type */
 119         uint16_t                off;    /* field offset in the target structure */
 120         snl_parse_attr_f        *cb;    /* parser function to call */
 121         const void              *arg;   /* Optional argument parser */
 122 };
 123
 124 struct snl_hdr_parser {
 125         int                     hdr_off; /* aligned header size */
 126         int                     fp_size;
 127         int                     np_size;
 128         const struct snl_field_parser   *fp; /* array of header field parsers */
 129         const struct snl_attr_parser    *np; /* array of attribute parsers */
 130 };
 131
 132 #define SNL_DECLARE_PARSER(_name, _t, _fp, _np)         \
 133 static const struct snl_hdr_parser _name = {            \
 134         .hdr_off = sizeof(_t),                          \
 135         .fp = &((_fp)[0]),                              \
 136         .np = &((_np)[0]),                              \
 137         .fp_size = NL_ARRAY_LEN(_fp),                   \
 138         .np_size = NL_ARRAY_LEN(_np),                   \
 139 }
 140
 141 #define SNL_DECLARE_ATTR_PARSER(_name, _np)             \
 142 static const struct snl_hdr_parser _name = {            \
 143         .np = &((_np)[0]),                              \
 144         .np_size = NL_ARRAY_LEN(_np),                   \
 145 }
 146
 147
 148 static void
 149 snl_free(struct snl_state *ss)
 150 {
 151         if (ss->init_done) {
 152                 close(ss->fd);
 153                 if (ss->buf != NULL)
 154                         free(ss->buf);
 155                 if (ss->lb.base != NULL)
 156                         free(ss->lb.base);
 157         }
 158 }
 159
 160 static inline bool
 161 snl_init(struct snl_state *ss, int netlink_family)
 162 {
 163         memset(ss, 0, sizeof(*ss));
 164
 165         ss->fd = socket(AF_NETLINK, SOCK_RAW, netlink_family);
 166         if (ss->fd == -1)
 167                 return (false);
 168         ss->init_done = true;
 169
 170         int rcvbuf;
 171         socklen_t optlen = sizeof(rcvbuf);
 172         if (getsockopt(ss->fd, SOL_SOCKET, SO_RCVBUF, &rcvbuf, &optlen) == -1) {
 173                 snl_free(ss);
 174                 return (false);
 175         }
 176
 177         ss->bufsize = rcvbuf;
 178         ss->buf = malloc(ss->bufsize);
 179         if (ss->buf == NULL) {
 180                 snl_free(ss);
 181                 return (false);
 182         }
 183
 184         ss->lb.size = SCRATCH_BUFFER_SIZE;
 185         ss->lb.base = calloc(1, ss->lb.size);
 186         if (ss->lb.base == NULL) {
 187                 snl_free(ss);
 188                 return (false);
 189         }
 190
 191         return (true);
 192 }
 193
 194 static inline void *
 195 snl_allocz(struct snl_state *ss, int len)
 196 {
 197         return (lb_allocz(&ss->lb, len));
 198 }
 199
 200 static inline void
 201 snl_clear_lb(struct snl_state *ss)
 202 {
 203         lb_clear(&ss->lb);
 204 }
 205
 206 static inline bool
 207 snl_send(struct snl_state *ss, void *data, int sz)
 208 {
 209         return (send(ss->fd, data, sz, 0) == sz);
 210 }
 211
 212 static inline uint32_t
 213 snl_get_seq(struct snl_state *ss)
 214 {
 215         return (++ss->seq);
 216 }
 217
 218 static inline struct nlmsghdr *
 219 snl_read_message(struct snl_state *ss)
 220 {
 221         if (ss->off == ss->datalen) {
 222                 struct sockaddr_nl nladdr;
 223                 struct iovec iov = {
 224                         .iov_base = ss->buf,
 225                         .iov_len = ss->bufsize,
 226                 };
 227                 struct msghdr msg = {
 228                         .msg_name = &nladdr,
 229                         .msg_namelen = sizeof(nladdr),
 230                         .msg_iov = &iov,
 231                         .msg_iovlen = 1,
 232                 };
 233                 ss->off = 0;
 234                 ss->datalen = 0;
 235                 for (;;) {
 236                         ssize_t datalen = recvmsg(ss->fd, &msg, 0);
 237                         if (datalen > 0) {
 238                                 ss->datalen = datalen;
 239                                 break;
 240                         } else if (errno != EINTR)
 241                                 return (NULL);
 242                 }
 243         }
 244         struct nlmsghdr *hdr = (struct nlmsghdr *)(void *)&ss->buf[ss->off];
 245         ss->off += NLMSG_ALIGN(hdr->nlmsg_len);
 246         return (hdr);
 247 }
 248
 249 /*
 250  * Checks that attributes are sorted by attribute type.
 251  */
 252 static inline void
 253 snl_verify_parsers(const struct snl_hdr_parser **parser, int count)
 254 {
 255         for (int i = 0; i < count; i++) {
 256                 const struct snl_hdr_parser *p = parser[i];
 257                 int attr_type = 0;
 258                 for (int j = 0; j < p->np_size; j++) {
 259                         assert(p->np[j].type > attr_type);
 260                         attr_type = p->np[j].type;
 261                 }
 262         }
 263 }
 264 #define SNL_VERIFY_PARSERS(_p)  snl_verify_parsers((_p), NL_ARRAY_LEN(_p))
 265
 266 static const struct snl_attr_parser *
 267 find_parser(const struct snl_attr_parser *ps, int pslen, int key)
 268 {
 269         int left_i = 0, right_i = pslen - 1;
 270
 271         if (key < ps[0].type || key > ps[pslen - 1].type)
 272                 return (NULL);
 273
 274         while (left_i + 1 < right_i) {
 275                 int mid_i = (left_i + right_i) / 2;
 276                 if (key < ps[mid_i].type)
 277                         right_i = mid_i;
 278                 else if (key > ps[mid_i].type)
 279                         left_i = mid_i + 1;
 280                 else
 281                         return (&ps[mid_i]);
 282         }
 283         if (ps[left_i].type == key)
 284                 return (&ps[left_i]);
 285         else if (ps[right_i].type == key)
 286                 return (&ps[right_i]);
 287         return (NULL);
 288 }
 289
 290 static inline bool
 291 snl_parse_attrs_raw(struct snl_state *ss, struct nlattr *nla_head, int len,
 292     const struct snl_attr_parser *ps, int pslen, void *target)
 293 {
 294         struct nlattr *nla;
 295
 296         NLA_FOREACH(nla, nla_head, len) {
 297                 if (nla->nla_len < sizeof(struct nlattr))
 298                         return (false);
 299                 int nla_type = nla->nla_type & NLA_TYPE_MASK;
 300                 const struct snl_attr_parser *s = find_parser(ps, pslen, nla_type);
 301                 if (s != NULL) {
 302                         void *ptr = (void *)((char *)target + s->off);
 303                         if (!s->cb(ss, nla, s->arg, ptr))
 304                                 return (false);
 305                 }
 306         }
 307         return (true);
 308 }
 309
 310 static inline bool
 311 snl_parse_attrs(struct snl_state *ss, struct nlmsghdr *hdr, int hdrlen,
 312     const struct snl_attr_parser *ps, int pslen, void *target)
 313 {
 314         int off = NLMSG_HDRLEN + NETLINK_ALIGN(hdrlen);
 315         int len = hdr->nlmsg_len - off;
 316         struct nlattr *nla_head = (struct nlattr *)(void *)((char *)hdr + off);
 317
 318         return (snl_parse_attrs_raw(ss, nla_head, len, ps, pslen, target));
 319 }
 320
 321 static inline bool
 322 snl_parse_header(struct snl_state *ss, void *hdr, int len,
 323     const struct snl_hdr_parser *parser, void *target)
 324 {
 325         /* Extract fields first (if any) */
 326         for (int i = 0; i < parser->fp_size; i++) {
 327                 const struct snl_field_parser *fp = &parser->fp[i];
 328                 void *src = (char *)hdr + fp->off_in;
 329                 void *dst = (char *)target + fp->off_out;
 330
 331                 fp->cb(ss, src, dst);
 332         }
 333
 334         struct nlattr *nla_head = (struct nlattr *)(void *)((char *)hdr + parser->hdr_off);
 335         bool result = snl_parse_attrs_raw(ss, nla_head, len - parser->hdr_off,
 336             parser->np, parser->np_size, target);
 337
 338         return (result);
 339 }
 340
 341 static inline bool
 342 snl_parse_nlmsg(struct snl_state *ss, struct nlmsghdr *hdr,
 343     const struct snl_hdr_parser *parser, void *target)
 344 {
 345         return (snl_parse_header(ss, hdr + 1, hdr->nlmsg_len - sizeof(*hdr), parser, target));
 346 }
 347
 348 static inline bool
 349 snl_attr_get_flag(struct snl_state *ss __unused, struct nlattr *nla, void *target)
 350 {
 351         if (NLA_DATA_LEN(nla) == 0) {
 352                 *((uint8_t *)target) = 1;
 353                 return (true);
 354         }
 355         return (false);
 356 }
 357
 358 static inline bool
 359 snl_attr_get_uint16(struct snl_state *ss __unused, struct nlattr *nla,
 360     const void *arg __unused, void *target)
 361 {
 362         if (NLA_DATA_LEN(nla) == sizeof(uint16_t)) {
 363                 *((uint16_t *)target) = *((const uint16_t *)NLA_DATA_CONST(nla));
 364                 return (true);
 365         }
 366         return (false);
 367 }
 368
 369 static inline bool
 370 snl_attr_get_uint32(struct snl_state *ss __unused, struct nlattr *nla,
 371     const void *arg __unused, void *target)
 372 {
 373         if (NLA_DATA_LEN(nla) == sizeof(uint32_t)) {
 374                 *((uint32_t *)target) = *((const uint32_t *)NLA_DATA_CONST(nla));
 375                 return (true);
 376         }
 377         return (false);
 378 }
 379
 380 static inline bool
 381 snl_attr_get_string(struct snl_state *ss __unused, struct nlattr *nla,
 382     const void *arg __unused, void *target)
 383 {
 384         size_t maxlen = NLA_DATA_LEN(nla);
 385
 386         if (strnlen((char *)NLA_DATA(nla), maxlen) < maxlen) {
 387                 *((char **)target) = (char *)NLA_DATA(nla);
 388                 return (true);
 389         }
 390         return (false);
 391 }
 392
 393 static inline bool
 394 snl_attr_get_stringn(struct snl_state *ss, struct nlattr *nla,
 395     const void *arg __unused, void *target)
 396 {
 397         int maxlen = NLA_DATA_LEN(nla);
 398
 399         char *buf = snl_allocz(ss, maxlen + 1);
 400         if (buf == NULL)
 401                 return (false);
 402         buf[maxlen] = '\0';
 403         memcpy(buf, NLA_DATA(nla), maxlen);
 404
 405         *((char **)target) = buf;
 406         return (true);
 407 }
 408
 409 static inline bool
 410 snl_attr_get_nested(struct snl_state *ss, struct nlattr *nla, const void *arg, void *target)
 411 {
 412         const struct snl_hdr_parser *p = (const struct snl_hdr_parser *)arg;
 413
 414         /* Assumes target points to the beginning of the structure */
 415         return (snl_parse_header(ss, NLA_DATA(nla), NLA_DATA_LEN(nla), p, target));
 416 }
 417
 418 static inline bool
 419 snl_attr_get_nla(struct snl_state *ss __unused, struct nlattr *nla, void *target)
 420 {
 421         *((struct nlattr **)target) = nla;
 422         return (true);
 423 }
 424
 425 static inline void
 426 snl_field_get_uint8(struct snl_state *ss __unused, void *src, void *target)
 427 {
 428         *((uint8_t *)target) = *((uint8_t *)src);
 429 }
 430
 431 static inline void
 432 snl_field_get_uint16(struct snl_state *ss __unused, void *src, void *target)
 433 {
 434         *((uint16_t *)target) = *((uint16_t *)src);
 435 }
 436
 437 static inline void
 438 snl_field_get_uint32(struct snl_state *ss __unused, void *src, void *target)
 439 {
 440         *((uint32_t *)target) = *((uint32_t *)src);
 441 }
 442
 443 #endif