diff options
Diffstat (limited to 'sys/netlink/netlink_snl.h')
| -rw-r--r-- | sys/netlink/netlink_snl.h | 1330 |
1 files changed, 1330 insertions, 0 deletions
diff --git a/sys/netlink/netlink_snl.h b/sys/netlink/netlink_snl.h new file mode 100644 index 000000000000..586716776bc5 --- /dev/null +++ b/sys/netlink/netlink_snl.h @@ -0,0 +1,1330 @@ +/*- + * SPDX-License-Identifier: BSD-2-Clause + * + * Copyright (c) 2022 Alexander V. Chernikov <melifaro@FreeBSD.org> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF + * SUCH DAMAGE. + */ +#ifndef _NETLINK_NETLINK_SNL_H_ +#define _NETLINK_NETLINK_SNL_H_ + +/* + * Simple Netlink Library + */ + +#include <sys/param.h> +#include <sys/socket.h> + +#include <netlink/netlink.h> +#include <netlink/netlink_bitset.h> + +#include <assert.h> +#include <errno.h> +#include <stdalign.h> +#include <stddef.h> +#include <stdbool.h> +#include <stdint.h> +#include <stdlib.h> +#include <string.h> +#include <unistd.h> + +#define _roundup2(x, y) (((x)+((y)-1))&(~((y)-1))) + +#define NETLINK_ALIGN_SIZE sizeof(uint32_t) +#define NETLINK_ALIGN(_len) _roundup2(_len, NETLINK_ALIGN_SIZE) + +#define NLA_ALIGN_SIZE sizeof(uint32_t) +#define NLA_HDRLEN ((int)sizeof(struct nlattr)) +#define NLA_DATA_LEN(_nla) ((int)((_nla)->nla_len - NLA_HDRLEN)) +#define NLA_DATA(_nla) NL_ITEM_DATA(_nla, NLA_HDRLEN) +#define NLA_DATA_CONST(_nla) NL_ITEM_DATA_CONST(_nla, NLA_HDRLEN) + +#define NLA_TYPE(_nla) ((_nla)->nla_type & 0x3FFF) + +#define NLA_NEXT(_attr) (struct nlattr *)(void *)((char *)_attr + NLA_ALIGN(_attr->nla_len)) + +#define _NLA_END(_start, _len) ((char *)(_start) + (_len)) +#define NLA_FOREACH(_attr, _start, _len) \ + for (_attr = (struct nlattr *)(_start); \ + ((char *)_attr < _NLA_END(_start, _len)) && \ + ((char *)NLA_NEXT(_attr) <= _NLA_END(_start, _len)); \ + _attr = NLA_NEXT(_attr)) + +struct linear_buffer { + char *base; /* Base allocated memory pointer */ + uint32_t offset; /* Currently used offset */ + uint32_t size; /* Total buffer size */ + struct linear_buffer *next; /* Buffer chaining */ +} __aligned(alignof(__max_align_t)); + +static inline struct linear_buffer * +lb_init(uint32_t size) +{ + struct linear_buffer *lb = (struct linear_buffer *)calloc(1, size); + + if (lb != NULL) { + lb->base = (char *)(lb + 1); + lb->size = size - sizeof(*lb); + } + + return (lb); +} + +static inline void +lb_free(struct linear_buffer *lb) +{ + free(lb); +} + +static inline char * +lb_allocz(struct linear_buffer *lb, int len) +{ + len = roundup2(len, alignof(__max_align_t)); + if (lb->offset + len > lb->size) + return (NULL); + char *data = (lb->base + lb->offset); + lb->offset += len; + return (data); +} + +static inline void +lb_clear(struct linear_buffer *lb) +{ + memset(lb->base, 0, lb->offset); + lb->offset = 0; +} + +struct snl_state { + int fd; + char *buf; + size_t off; + size_t bufsize; + size_t datalen; + uint32_t seq; + bool init_done; + struct linear_buffer *lb; +}; +#define SCRATCH_BUFFER_SIZE 1024 +#define SNL_WRITER_BUFFER_SIZE 256 + +typedef void snl_parse_field_f(struct snl_state *ss, void *hdr, void *target); +struct snl_field_parser { + uint16_t off_in; + uint16_t off_out; + snl_parse_field_f *cb; +}; +static const struct snl_field_parser snl_f_p_empty[] = {}; + +typedef bool snl_parse_attr_f(struct snl_state *ss, struct nlattr *attr, + const void *arg, void *target); +struct snl_attr_parser { + uint16_t type; /* Attribute type */ + uint16_t off; /* field offset in the target structure */ + snl_parse_attr_f *cb; /* parser function to call */ + + /* Optional parser argument */ + union { + const void *arg; + const uint32_t arg_u32; + }; +}; + +typedef bool snl_parse_post_f(struct snl_state *ss, void *target); + +struct snl_hdr_parser { + uint16_t in_hdr_size; /* Input header size */ + uint16_t out_size; /* Output structure size */ + uint16_t fp_size; /* Number of items in field parser */ + uint16_t np_size; /* Number of items in attribute parser */ + const struct snl_field_parser *fp; /* array of header field parsers */ + const struct snl_attr_parser *np; /* array of attribute parsers */ + snl_parse_post_f *cb_post; /* post-parse callback */ +}; + +#define SNL_DECLARE_PARSER_EXT(_name, _sz_h_in, _sz_out, _fp, _np, _cb) \ +static const struct snl_hdr_parser _name = { \ + .in_hdr_size = _sz_h_in, \ + .out_size = _sz_out, \ + .fp = &((_fp)[0]), \ + .np = &((_np)[0]), \ + .fp_size = nitems(_fp), \ + .np_size = nitems(_np), \ + .cb_post = _cb, \ +} + +#define SNL_DECLARE_PARSER(_name, _t, _fp, _np) \ + SNL_DECLARE_PARSER_EXT(_name, sizeof(_t), 0, _fp, _np, NULL) + +#define SNL_DECLARE_FIELD_PARSER_EXT(_name, _sz_h_in, _sz_out, _fp, _cb) \ +static const struct snl_hdr_parser _name = { \ + .in_hdr_size = _sz_h_in, \ + .out_size = _sz_out, \ + .fp = &((_fp)[0]), \ + .fp_size = nitems(_fp), \ + .cb_post = _cb, \ +} + +#define SNL_DECLARE_FIELD_PARSER(_name, _t, _fp) \ + SNL_DECLARE_FIELD_PARSER_EXT(_name, sizeof(_t), 0, _fp, NULL) + +#define SNL_DECLARE_ATTR_PARSER_EXT(_name, _sz_out, _np, _cb) \ +static const struct snl_hdr_parser _name = { \ + .out_size = _sz_out, \ + .np = &((_np)[0]), \ + .np_size = nitems(_np), \ + .cb_post = _cb, \ +} + +#define SNL_DECLARE_ATTR_PARSER(_name, _np) \ + SNL_DECLARE_ATTR_PARSER_EXT(_name, 0, _np, NULL) + + +static inline void * +snl_allocz(struct snl_state *ss, int len) +{ + void *data = lb_allocz(ss->lb, len); + + if (data == NULL) { + uint32_t size = ss->lb->size * 2; + + while (size < len + sizeof(struct linear_buffer)) + size *= 2; + + struct linear_buffer *lb = lb_init(size); + + if (lb != NULL) { + lb->next = ss->lb; + ss->lb = lb; + data = lb_allocz(ss->lb, len); + } + } + + return (data); +} + +static inline void +snl_clear_lb(struct snl_state *ss) +{ + struct linear_buffer *lb = ss->lb; + + lb_clear(lb); + lb = lb->next; + ss->lb->next = NULL; + /* Remove all linear bufs except the largest one */ + while (lb != NULL) { + struct linear_buffer *lb_next = lb->next; + lb_free(lb); + lb = lb_next; + } +} + +static void +snl_free(struct snl_state *ss) +{ + if (ss->init_done) + close(ss->fd); + if (ss->buf != NULL) + free(ss->buf); + if (ss->lb != NULL) { + snl_clear_lb(ss); + lb_free(ss->lb); + } +} + +static inline bool +snl_init(struct snl_state *ss, int netlink_family) +{ + memset(ss, 0, sizeof(*ss)); + + ss->fd = socket(AF_NETLINK, SOCK_RAW, netlink_family); + if (ss->fd == -1) + return (false); + ss->init_done = true; + + int val = 1; + socklen_t optlen = sizeof(val); + if (setsockopt(ss->fd, SOL_NETLINK, NETLINK_EXT_ACK, &val, optlen) == -1) { + snl_free(ss); + return (false); + } + + int rcvbuf; + if (getsockopt(ss->fd, SOL_SOCKET, SO_RCVBUF, &rcvbuf, &optlen) == -1) { + snl_free(ss); + return (false); + } + + ss->bufsize = rcvbuf; + ss->buf = (char *)malloc(ss->bufsize); + if (ss->buf == NULL) { + snl_free(ss); + return (false); + } + + ss->lb = lb_init(SCRATCH_BUFFER_SIZE); + if (ss->lb == NULL) { + snl_free(ss); + return (false); + } + + return (true); +} + +static inline bool +snl_clone(struct snl_state *ss, const struct snl_state *orig) +{ + *ss = (struct snl_state){ + .fd = orig->fd, + .init_done = false, + }; + return ((ss->lb = lb_init(SCRATCH_BUFFER_SIZE)) != NULL); +} + +static inline bool +snl_send(struct snl_state *ss, void *data, int sz) +{ + return (send(ss->fd, data, sz, 0) == sz); +} + +static inline bool +snl_send_message(struct snl_state *ss, struct nlmsghdr *hdr) +{ + ssize_t sz = NLMSG_ALIGN(hdr->nlmsg_len); + + return (send(ss->fd, hdr, sz, 0) == sz); +} + +static inline uint32_t +snl_get_seq(struct snl_state *ss) +{ + return (++ss->seq); +} + +struct snl_msg_info { + int cmsg_type; + int cmsg_level; + uint32_t process_id; + uint8_t port_id; + uint8_t seq_id; +}; +static inline bool parse_cmsg(struct snl_state *ss, const struct msghdr *msg, + struct snl_msg_info *attrs); + +static inline struct nlmsghdr * +snl_read_message_dbg(struct snl_state *ss, struct snl_msg_info *cinfo) +{ + memset(cinfo, 0, sizeof(*cinfo)); + + if (ss->off == ss->datalen) { + struct sockaddr_nl nladdr; + char cbuf[64]; + + struct iovec iov = { + .iov_base = ss->buf, + .iov_len = ss->bufsize, + }; + struct msghdr msg = { + .msg_name = &nladdr, + .msg_namelen = sizeof(nladdr), + .msg_iov = &iov, + .msg_iovlen = 1, + .msg_control = cbuf, + .msg_controllen = sizeof(cbuf), + }; + ss->off = 0; + ss->datalen = 0; + for (;;) { + ssize_t datalen = recvmsg(ss->fd, &msg, 0); + if (datalen > 0) { + ss->datalen = datalen; + parse_cmsg(ss, &msg, cinfo); + break; + } else if (errno != EINTR) + return (NULL); + } + } + struct nlmsghdr *hdr = (struct nlmsghdr *)(void *)&ss->buf[ss->off]; + ss->off += NLMSG_ALIGN(hdr->nlmsg_len); + return (hdr); +} + + +static inline struct nlmsghdr * +snl_read_message(struct snl_state *ss) +{ + if (ss->off == ss->datalen) { + struct sockaddr_nl nladdr; + struct iovec iov = { + .iov_base = ss->buf, + .iov_len = ss->bufsize, + }; + struct msghdr msg = { + .msg_name = &nladdr, + .msg_namelen = sizeof(nladdr), + .msg_iov = &iov, + .msg_iovlen = 1, + }; + ss->off = 0; + ss->datalen = 0; + for (;;) { + ssize_t datalen = recvmsg(ss->fd, &msg, 0); + if (datalen > 0) { + ss->datalen = datalen; + break; + } else if (errno != EINTR) + return (NULL); + } + } + struct nlmsghdr *hdr = (struct nlmsghdr *)(void *)&ss->buf[ss->off]; + ss->off += NLMSG_ALIGN(hdr->nlmsg_len); + return (hdr); +} + +static inline struct nlmsghdr * +snl_read_reply(struct snl_state *ss, uint32_t nlmsg_seq) +{ + struct nlmsghdr *hdr; + + while ((hdr = snl_read_message(ss)) != NULL) { + if (hdr->nlmsg_seq == nlmsg_seq) + return (hdr); + } + + return (NULL); +} + +/* + * Checks that attributes are sorted by attribute type. + */ +static inline void +snl_verify_parsers(const struct snl_hdr_parser **parser, int count) +{ + for (int i = 0; i < count; i++) { + const struct snl_hdr_parser *p = parser[i]; + int attr_type = 0; + for (int j = 0; j < p->np_size; j++) { + assert(p->np[j].type > attr_type); + attr_type = p->np[j].type; + } + } +} +#define SNL_VERIFY_PARSERS(_p) snl_verify_parsers((_p), nitems(_p)) + +static const struct snl_attr_parser * +find_parser(const struct snl_attr_parser *ps, int pslen, int key) +{ + int left_i = 0, right_i = pslen - 1; + + if (key < ps[0].type || key > ps[pslen - 1].type) + return (NULL); + + while (left_i + 1 < right_i) { + int mid_i = (left_i + right_i) / 2; + if (key < ps[mid_i].type) + right_i = mid_i; + else if (key > ps[mid_i].type) + left_i = mid_i + 1; + else + return (&ps[mid_i]); + } + if (ps[left_i].type == key) + return (&ps[left_i]); + else if (ps[right_i].type == key) + return (&ps[right_i]); + return (NULL); +} + +static inline bool +snl_parse_attrs_raw(struct snl_state *ss, struct nlattr *nla_head, int len, + const struct snl_attr_parser *ps, int pslen, void *target) +{ + struct nlattr *nla; + + NLA_FOREACH(nla, nla_head, len) { + if (nla->nla_len < sizeof(struct nlattr)) + return (false); + int nla_type = nla->nla_type & NLA_TYPE_MASK; + const struct snl_attr_parser *s = find_parser(ps, pslen, nla_type); + if (s != NULL) { + void *ptr = (void *)((char *)target + s->off); + if (!s->cb(ss, nla, s->arg, ptr)) + return (false); + } + } + return (true); +} + +static inline bool +snl_parse_attrs(struct snl_state *ss, struct nlmsghdr *hdr, int hdrlen, + const struct snl_attr_parser *ps, int pslen, void *target) +{ + int off = NLMSG_HDRLEN + NETLINK_ALIGN(hdrlen); + int len = hdr->nlmsg_len - off; + struct nlattr *nla_head = (struct nlattr *)(void *)((char *)hdr + off); + + return (snl_parse_attrs_raw(ss, nla_head, len, ps, pslen, target)); +} + +static inline void +snl_parse_fields(struct snl_state *ss, struct nlmsghdr *hdr, int hdrlen __unused, + const struct snl_field_parser *ps, int pslen, void *target) +{ + for (int i = 0; i < pslen; i++) { + const struct snl_field_parser *fp = &ps[i]; + void *src = (char *)hdr + fp->off_in; + void *dst = (char *)target + fp->off_out; + + fp->cb(ss, src, dst); + } +} + +static inline bool +snl_parse_header(struct snl_state *ss, void *hdr, int len, + const struct snl_hdr_parser *parser, void *target) +{ + struct nlattr *nla_head; + + /* Extract fields first (if any) */ + snl_parse_fields(ss, (struct nlmsghdr *)hdr, parser->in_hdr_size, + parser->fp, parser->fp_size, target); + + nla_head = (struct nlattr *)(void *)((char *)hdr + parser->in_hdr_size); + bool result = snl_parse_attrs_raw(ss, nla_head, len - parser->in_hdr_size, + parser->np, parser->np_size, target); + + if (result && parser->cb_post != NULL) + result = parser->cb_post(ss, target); + + return (result); +} + +static inline bool +snl_parse_nlmsg(struct snl_state *ss, struct nlmsghdr *hdr, + const struct snl_hdr_parser *parser, void *target) +{ + return (snl_parse_header(ss, hdr + 1, hdr->nlmsg_len - sizeof(*hdr), parser, target)); +} + +static inline bool +snl_attr_get_flag(struct snl_state *ss __unused, struct nlattr *nla, const void *arg __unused, + void *target) +{ + if (NLA_DATA_LEN(nla) == 0) { + *((uint8_t *)target) = 1; + return (true); + } + return (false); +} + +static inline bool +snl_attr_get_bytes(struct snl_state *ss __unused, struct nlattr *nla, const void *arg, + void *target) +{ + if ((size_t)NLA_DATA_LEN(nla) != (size_t)arg) + return (false); + + memcpy(target, NLA_DATA_CONST(nla), (size_t)arg); + + return (true); +} + +static inline bool +snl_attr_get_bool(struct snl_state *ss __unused, struct nlattr *nla, + const void *arg __unused, void *target) +{ + if (NLA_DATA_LEN(nla) == sizeof(bool)) { + *((bool *)target) = *((const bool *)NLA_DATA_CONST(nla)); + return (true); + } + return (false); +} + +static inline bool +snl_attr_get_uint8(struct snl_state *ss __unused, struct nlattr *nla, + const void *arg __unused, void *target) +{ + if (NLA_DATA_LEN(nla) == sizeof(uint8_t)) { + *((uint8_t *)target) = *((const uint8_t *)NLA_DATA_CONST(nla)); + return (true); + } + return (false); +} + +static inline bool +snl_attr_get_uint16(struct snl_state *ss __unused, struct nlattr *nla, + const void *arg __unused, void *target) +{ + if (NLA_DATA_LEN(nla) == sizeof(uint16_t)) { + *((uint16_t *)target) = *((const uint16_t *)NLA_DATA_CONST(nla)); + return (true); + } + return (false); +} + +static inline bool +snl_attr_get_uint32(struct snl_state *ss __unused, struct nlattr *nla, + const void *arg __unused, void *target) +{ + if (NLA_DATA_LEN(nla) == sizeof(uint32_t)) { + *((uint32_t *)target) = *((const uint32_t *)NLA_DATA_CONST(nla)); + return (true); + } + return (false); +} + +static inline bool +snl_attr_get_uint64(struct snl_state *ss __unused, struct nlattr *nla, + const void *arg __unused, void *target) +{ + if (NLA_DATA_LEN(nla) == sizeof(uint64_t)) { + memcpy(target, NLA_DATA_CONST(nla), sizeof(uint64_t)); + return (true); + } + return (false); +} + +static inline bool +snl_attr_get_int8(struct snl_state *ss, struct nlattr *nla, const void *arg, + void *target) +{ + return (snl_attr_get_uint8(ss, nla, arg, target)); +} + +static inline bool +snl_attr_get_int16(struct snl_state *ss, struct nlattr *nla, const void *arg, + void *target) +{ + return (snl_attr_get_uint16(ss, nla, arg, target)); +} + +static inline bool +snl_attr_get_int32(struct snl_state *ss, struct nlattr *nla, const void *arg, + void *target) +{ + return (snl_attr_get_uint32(ss, nla, arg, target)); +} + +static inline bool +snl_attr_get_int64(struct snl_state *ss, struct nlattr *nla, const void *arg, + void *target) +{ + return (snl_attr_get_uint64(ss, nla, arg, target)); +} + +static inline bool +snl_attr_get_string(struct snl_state *ss __unused, struct nlattr *nla, + const void *arg __unused, void *target) +{ + size_t maxlen = NLA_DATA_LEN(nla); + + if (strnlen((char *)NLA_DATA(nla), maxlen) < maxlen) { + *((char **)target) = (char *)NLA_DATA(nla); + return (true); + } + return (false); +} + +static inline bool +snl_attr_get_stringn(struct snl_state *ss, struct nlattr *nla, + const void *arg __unused, void *target) +{ + int maxlen = NLA_DATA_LEN(nla); + + char *buf = (char *)snl_allocz(ss, maxlen + 1); + if (buf == NULL) + return (false); + buf[maxlen] = '\0'; + memcpy(buf, NLA_DATA(nla), maxlen); + + *((char **)target) = buf; + return (true); +} + +static inline bool +snl_attr_copy_string(struct snl_state *ss, struct nlattr *nla, + const void *arg, void *target) +{ + char *tmp; + + if (snl_attr_get_string(ss, nla, NULL, &tmp)) { + strlcpy((char *)target, tmp, (size_t)arg); + return (true); + } + return (false); +} + +static inline bool +snl_attr_dup_string(struct snl_state *ss __unused, struct nlattr *nla, + const void *arg __unused, void *target) +{ + size_t maxlen = NLA_DATA_LEN(nla); + + if (strnlen((char *)NLA_DATA(nla), maxlen) < maxlen) { + char *buf = (char *)snl_allocz(ss, maxlen); + if (buf == NULL) + return (false); + memcpy(buf, NLA_DATA(nla), maxlen); + *((char **)target) = buf; + return (true); + } + return (false); +} + +static inline bool +snl_attr_get_nested(struct snl_state *ss, struct nlattr *nla, const void *arg, void *target) +{ + const struct snl_hdr_parser *p = (const struct snl_hdr_parser *)arg; + + /* Assumes target points to the beginning of the structure */ + return (snl_parse_header(ss, NLA_DATA(nla), NLA_DATA_LEN(nla), p, target)); +} + +struct snl_parray { + uint32_t count; + void **items; +}; + +static inline bool +snl_attr_get_parray_sz(struct snl_state *ss, struct nlattr *container_nla, + uint32_t start_size, const void *arg, void *target) +{ + const struct snl_hdr_parser *p = (const struct snl_hdr_parser *)arg; + struct snl_parray *array = (struct snl_parray *)target; + struct nlattr *nla; + uint32_t count = 0, size = start_size; + + if (p->out_size == 0) + return (false); + + array->items = (void **)snl_allocz(ss, size * sizeof(void *)); + if (array->items == NULL) + return (false); + + /* + * If the provided parser is an attribute parser, assume that each + * nla in the container nla is the container nla itself and parse + * the contents of this nla. + * Otherwise, run the parser on raw data, assuming the header of this + * data has u16 field with total size in the beginning. + */ + uint32_t data_off = 0; + + if (p->in_hdr_size == 0) + data_off = sizeof(struct nlattr); + + NLA_FOREACH(nla, NLA_DATA(container_nla), NLA_DATA_LEN(container_nla)) { + void *item = snl_allocz(ss, p->out_size); + + if (item == NULL) + return (false); + + void *data = (char *)(void *)nla + data_off; + int data_len = nla->nla_len - data_off; + + if (!(snl_parse_header(ss, data, data_len, p, item))) + return (false); + + if (count == size) { + uint32_t new_size = size * 2; + void **new_array = (void **)snl_allocz(ss, new_size *sizeof(void *)); + + memcpy(new_array, array->items, size * sizeof(void *)); + array->items = new_array; + size = new_size; + } + array->items[count++] = item; + } + array->count = count; + + return (true); +} + +/* + * Parses and stores the unknown-size array. + * Assumes each array item is a container and the NLAs in the container are parsable + * by the parser provided in @arg. + * Assumes @target is struct snl_parray + */ +static inline bool +snl_attr_get_parray(struct snl_state *ss, struct nlattr *nla, const void *arg, void *target) +{ + return (snl_attr_get_parray_sz(ss, nla, 8, arg, target)); +} + +static inline bool +snl_attr_get_nla(struct snl_state *ss __unused, struct nlattr *nla, + const void *arg __unused, void *target) +{ + *((struct nlattr **)target) = nla; + return (true); +} + +static inline bool +snl_attr_dup_nla(struct snl_state *ss, struct nlattr *nla, + const void *arg __unused, void *target) +{ + void *ptr = snl_allocz(ss, nla->nla_len); + + if (ptr != NULL) { + memcpy(ptr, nla, nla->nla_len); + *((void **)target) = ptr; + return (true); + } + return (false); +} + +static inline bool +snl_attr_copy_struct(struct snl_state *ss, struct nlattr *nla, + const void *arg __unused, void *target) +{ + void *ptr = snl_allocz(ss, NLA_DATA_LEN(nla)); + + if (ptr != NULL) { + memcpy(ptr, NLA_DATA(nla), NLA_DATA_LEN(nla)); + *((void **)target) = ptr; + return (true); + } + return (false); +} + +static inline bool +snl_attr_dup_struct(struct snl_state *ss, struct nlattr *nla, + const void *arg __unused, void *target) +{ + void *ptr = snl_allocz(ss, NLA_DATA_LEN(nla)); + + if (ptr != NULL) { + memcpy(ptr, NLA_DATA(nla), NLA_DATA_LEN(nla)); + *((void **)target) = ptr; + return (true); + } + return (false); +} + +struct snl_attr_bit { + uint32_t bit_index; + char *bit_name; + int bit_value; +}; + +struct snl_attr_bits { + uint32_t num_bits; + struct snl_attr_bit **bits; +}; + +#define _OUT(_field) offsetof(struct snl_attr_bit, _field) +static const struct snl_attr_parser _nla_p_bit[] = { + { .type = NLA_BITSET_BIT_INDEX, .off = _OUT(bit_index), .cb = snl_attr_get_uint32 }, + { .type = NLA_BITSET_BIT_NAME, .off = _OUT(bit_name), .cb = snl_attr_dup_string }, + { .type = NLA_BITSET_BIT_VALUE, .off = _OUT(bit_value), .cb = snl_attr_get_flag }, +}; +#undef _OUT +SNL_DECLARE_ATTR_PARSER_EXT(_nla_bit_parser, sizeof(struct snl_attr_bit), _nla_p_bit, NULL); + +struct snl_attr_bitset { + uint32_t nla_bitset_size; + uint32_t *nla_bitset_mask; + uint32_t *nla_bitset_value; + struct snl_attr_bits bits; +}; + +#define _OUT(_field) offsetof(struct snl_attr_bitset, _field) +static const struct snl_attr_parser _nla_p_bitset[] = { + { .type = NLA_BITSET_SIZE, .off = _OUT(nla_bitset_size), .cb = snl_attr_get_uint32 }, + { .type = NLA_BITSET_BITS, .off = _OUT(bits), .cb = snl_attr_get_parray, .arg = &_nla_bit_parser }, + { .type = NLA_BITSET_VALUE, .off = _OUT(nla_bitset_mask), .cb = snl_attr_dup_nla }, + { .type = NLA_BITSET_MASK, .off = _OUT(nla_bitset_value), .cb = snl_attr_dup_nla }, +}; + +static inline bool +_cb_p_bitset(struct snl_state *ss __unused, void *_target) +{ + struct snl_attr_bitset *target = (struct snl_attr_bitset *)_target; + + uint32_t sz_bytes = _roundup2(target->nla_bitset_size, 32) / 8; + + if (target->nla_bitset_mask != NULL) { + struct nlattr *nla = (struct nlattr *)target->nla_bitset_mask; + uint32_t data_len = NLA_DATA_LEN(nla); + + if (data_len != sz_bytes || _roundup2(data_len, 4) != data_len) + return (false); + target->nla_bitset_mask = (uint32_t *)NLA_DATA(nla); + } + + if (target->nla_bitset_value != NULL) { + struct nlattr *nla = (struct nlattr *)target->nla_bitset_value; + uint32_t data_len = NLA_DATA_LEN(nla); + + if (data_len != sz_bytes || _roundup2(data_len, 4) != data_len) + return (false); + target->nla_bitset_value = (uint32_t *)NLA_DATA(nla); + } + return (true); +} +#undef _OUT +SNL_DECLARE_ATTR_PARSER_EXT(_nla_bitset_parser, + sizeof(struct snl_attr_bitset), + _nla_p_bitset, _cb_p_bitset); + +/* + * Parses the compact bitset representation. + */ +static inline bool +snl_attr_get_bitset_c(struct snl_state *ss, struct nlattr *nla, + const void *arg __unused, void *_target) +{ + const struct snl_hdr_parser *p = &_nla_bitset_parser; + struct snl_attr_bitset *target = (struct snl_attr_bitset *)_target; + + /* Assumes target points to the beginning of the structure */ + if (!snl_parse_header(ss, NLA_DATA(nla), NLA_DATA_LEN(nla), p, _target)) + return (false); + if (target->nla_bitset_mask == NULL || target->nla_bitset_value == NULL) + return (false); + return (true); +} + +static inline void +snl_field_get_uint8(struct snl_state *ss __unused, void *src, void *target) +{ + *((uint8_t *)target) = *((uint8_t *)src); +} + +static inline void +snl_field_get_uint16(struct snl_state *ss __unused, void *src, void *target) +{ + *((uint16_t *)target) = *((uint16_t *)src); +} + +static inline void +snl_field_get_uint32(struct snl_state *ss __unused, void *src, void *target) +{ + *((uint32_t *)target) = *((uint32_t *)src); +} + +static inline void +snl_field_get_ptr(struct snl_state *ss __unused, void *src, void *target) +{ + *((void **)target) = src; +} + +struct snl_errmsg_data { + struct nlmsghdr *orig_hdr; + int error; + uint32_t error_offs; + char *error_str; + struct nlattr *cookie; +}; + +#define _IN(_field) offsetof(struct nlmsgerr, _field) +#define _OUT(_field) offsetof(struct snl_errmsg_data, _field) +static const struct snl_attr_parser nla_p_errmsg[] = { + { .type = NLMSGERR_ATTR_MSG, .off = _OUT(error_str), .cb = snl_attr_get_string }, + { .type = NLMSGERR_ATTR_OFFS, .off = _OUT(error_offs), .cb = snl_attr_get_uint32 }, + { .type = NLMSGERR_ATTR_COOKIE, .off = _OUT(cookie), .cb = snl_attr_get_nla }, +}; + +static const struct snl_field_parser nlf_p_errmsg[] = { + { .off_in = _IN(error), .off_out = _OUT(error), .cb = snl_field_get_uint32 }, + { .off_in = _IN(msg), .off_out = _OUT(orig_hdr), .cb = snl_field_get_ptr }, +}; +#undef _IN +#undef _OUT +SNL_DECLARE_PARSER(snl_errmsg_parser, struct nlmsgerr, nlf_p_errmsg, nla_p_errmsg); + +#define _IN(_field) offsetof(struct nlmsgerr, _field) +#define _OUT(_field) offsetof(struct snl_errmsg_data, _field) +static const struct snl_field_parser nlf_p_donemsg[] = { + { .off_in = _IN(error), .off_out = _OUT(error), .cb = snl_field_get_uint32 }, +}; +#undef _IN +#undef _OUT +SNL_DECLARE_FIELD_PARSER(snl_donemsg_parser, struct nlmsgerr, nlf_p_donemsg); + +static inline bool +snl_parse_errmsg(struct snl_state *ss, struct nlmsghdr *hdr, struct snl_errmsg_data *e) +{ + if ((hdr->nlmsg_flags & NLM_F_CAPPED) != 0) + return (snl_parse_nlmsg(ss, hdr, &snl_errmsg_parser, e)); + + const struct snl_hdr_parser *ps = &snl_errmsg_parser; + struct nlmsgerr *errmsg = (struct nlmsgerr *)(hdr + 1); + int hdrlen = sizeof(int) + NLMSG_ALIGN(errmsg->msg.nlmsg_len); + struct nlattr *attr_head = (struct nlattr *)(void *)((char *)errmsg + hdrlen); + int attr_len = hdr->nlmsg_len - sizeof(struct nlmsghdr) - hdrlen; + + snl_parse_fields(ss, (struct nlmsghdr *)errmsg, hdrlen, ps->fp, ps->fp_size, e); + return (snl_parse_attrs_raw(ss, attr_head, attr_len, ps->np, ps->np_size, e)); +} + +static inline bool +snl_read_reply_code(struct snl_state *ss, uint32_t nlmsg_seq, struct snl_errmsg_data *e) +{ + struct nlmsghdr *hdr = snl_read_reply(ss, nlmsg_seq); + + if (hdr == NULL) { + e->error = EINVAL; + } else if (hdr->nlmsg_type == NLMSG_ERROR) { + if (!snl_parse_errmsg(ss, hdr, e)) + e->error = EINVAL; + return (e->error == 0); + } + + return (false); +} + +#define _OUT(_field) offsetof(struct snl_msg_info, _field) +static const struct snl_attr_parser _nla_p_cinfo[] = { + { .type = NLMSGINFO_ATTR_PROCESS_ID, .off = _OUT(process_id), .cb = snl_attr_get_uint32 }, + { .type = NLMSGINFO_ATTR_PORT_ID, .off = _OUT(port_id), .cb = snl_attr_get_uint32 }, + { .type = NLMSGINFO_ATTR_SEQ_ID, .off = _OUT(seq_id), .cb = snl_attr_get_uint32 }, +}; +#undef _OUT +SNL_DECLARE_ATTR_PARSER(snl_msg_info_parser, _nla_p_cinfo); + +static inline bool +parse_cmsg(struct snl_state *ss, const struct msghdr *msg, struct snl_msg_info *attrs) +{ + for (struct cmsghdr *cmsg = CMSG_FIRSTHDR(msg); cmsg != NULL; + cmsg = CMSG_NXTHDR(msg, cmsg)) { + if (cmsg->cmsg_level != SOL_NETLINK || cmsg->cmsg_type != NETLINK_MSG_INFO) + continue; + + void *data = CMSG_DATA(cmsg); + int len = cmsg->cmsg_len - ((char *)data - (char *)cmsg); + const struct snl_hdr_parser *ps = &snl_msg_info_parser; + + return (snl_parse_attrs_raw(ss, (struct nlattr *)data, len, ps->np, ps->np_size, attrs)); + } + + return (false); +} + +/* + * Assumes e is zeroed + */ +static inline struct nlmsghdr * +snl_read_reply_multi(struct snl_state *ss, uint32_t nlmsg_seq, struct snl_errmsg_data *e) +{ + struct nlmsghdr *hdr = snl_read_reply(ss, nlmsg_seq); + + if (hdr == NULL) { + e->error = EINVAL; + } else if (hdr->nlmsg_type == NLMSG_ERROR) { + if (!snl_parse_errmsg(ss, hdr, e)) + e->error = EINVAL; + } else if (hdr->nlmsg_type == NLMSG_DONE) { + snl_parse_nlmsg(ss, hdr, &snl_donemsg_parser, e); + } else + return (hdr); + + return (NULL); +} + + +/* writer logic */ +struct snl_writer { + char *base; + uint32_t offset; + uint32_t size; + struct nlmsghdr *hdr; + struct snl_state *ss; + bool error; +}; + +static inline void +snl_init_writer(struct snl_state *ss, struct snl_writer *nw) +{ + nw->size = SNL_WRITER_BUFFER_SIZE; + nw->base = (char *)snl_allocz(ss, nw->size); + if (nw->base == NULL) { + nw->error = true; + nw->size = 0; + } + + nw->offset = 0; + nw->hdr = NULL; + nw->error = false; + nw->ss = ss; +} + +static inline bool +snl_realloc_msg_buffer(struct snl_writer *nw, size_t sz) +{ + uint32_t new_size = nw->size * 2; + + while (new_size < nw->size + sz) + new_size *= 2; + + if (nw->error) + return (false); + + if (snl_allocz(nw->ss, new_size) == NULL) { + nw->error = true; + return (false); + } + nw->size = new_size; + + void *new_base = nw->ss->lb->base; + if (new_base != nw->base) { + memcpy(new_base, nw->base, nw->offset); + if (nw->hdr != NULL) { + int hdr_off = (char *)(nw->hdr) - nw->base; + + nw->hdr = (struct nlmsghdr *) + (void *)((char *)new_base + hdr_off); + } + nw->base = (char *)new_base; + } + + return (true); +} + +static inline void * +snl_reserve_msg_data_raw(struct snl_writer *nw, size_t sz) +{ + sz = NETLINK_ALIGN(sz); + + if (__predict_false(nw->offset + sz > nw->size)) { + if (!snl_realloc_msg_buffer(nw, sz)) + return (NULL); + } + + void *data_ptr = &nw->base[nw->offset]; + nw->offset += sz; + + return (data_ptr); +} +#define snl_reserve_msg_object(_ns, _t) ((_t *)snl_reserve_msg_data_raw(_ns, sizeof(_t))) +#define snl_reserve_msg_data(_ns, _sz, _t) ((_t *)snl_reserve_msg_data_raw(_ns, _sz)) + +static inline struct nlattr * +snl_reserve_msg_attr_raw(struct snl_writer *nw, uint16_t nla_type, uint16_t sz) +{ + struct nlattr *nla; + + sz += sizeof(struct nlattr); + nla = snl_reserve_msg_data(nw, sz, struct nlattr); + if (__predict_false(nla == NULL)) + return (NULL); + nla->nla_type = nla_type; + nla->nla_len = sz; + + return (nla); +} +#define snl_reserve_msg_attr(_ns, _at, _t) \ + ((_t *)(snl_reserve_msg_attr_raw(_ns, _at, sizeof(_t)) + 1)) + +static inline bool +snl_add_msg_attr(struct snl_writer *nw, int attr_type, int attr_len, const void *data) +{ + int required_len = NLA_ALIGN(attr_len + sizeof(struct nlattr)); + + if (__predict_false(nw->offset + required_len > nw->size)) { + if (!snl_realloc_msg_buffer(nw, required_len)) + return (false); + } + + struct nlattr *nla = (struct nlattr *)(void *)(&nw->base[nw->offset]); + + nla->nla_len = attr_len + sizeof(struct nlattr); + nla->nla_type = attr_type; + if (attr_len > 0) { + if ((attr_len % 4) != 0) { + /* clear padding bytes */ + bzero((char *)nla + required_len - 4, 4); + } + memcpy((nla + 1), data, attr_len); + } + nw->offset += required_len; + return (true); +} + +static inline bool +snl_add_msg_attr_raw(struct snl_writer *nw, const struct nlattr *nla_src) +{ + int attr_len = nla_src->nla_len - sizeof(struct nlattr); + + assert(attr_len >= 0); + + return (snl_add_msg_attr(nw, nla_src->nla_type, attr_len, (const void *)(nla_src + 1))); +} + +static inline bool +snl_add_msg_attr_bool(struct snl_writer *nw, int attrtype, bool value) +{ + return (snl_add_msg_attr(nw, attrtype, sizeof(bool), &value)); +} + +static inline bool +snl_add_msg_attr_u8(struct snl_writer *nw, int attrtype, uint8_t value) +{ + return (snl_add_msg_attr(nw, attrtype, sizeof(uint8_t), &value)); +} + +static inline bool +snl_add_msg_attr_u16(struct snl_writer *nw, int attrtype, uint16_t value) +{ + return (snl_add_msg_attr(nw, attrtype, sizeof(uint16_t), &value)); +} + +static inline bool +snl_add_msg_attr_u32(struct snl_writer *nw, int attrtype, uint32_t value) +{ + return (snl_add_msg_attr(nw, attrtype, sizeof(uint32_t), &value)); +} + +static inline bool +snl_add_msg_attr_u64(struct snl_writer *nw, int attrtype, uint64_t value) +{ + return (snl_add_msg_attr(nw, attrtype, sizeof(uint64_t), &value)); +} + +static inline bool +snl_add_msg_attr_s8(struct snl_writer *nw, int attrtype, int8_t value) +{ + return (snl_add_msg_attr(nw, attrtype, sizeof(int8_t), &value)); +} + +static inline bool +snl_add_msg_attr_s16(struct snl_writer *nw, int attrtype, int16_t value) +{ + return (snl_add_msg_attr(nw, attrtype, sizeof(int16_t), &value)); +} + +static inline bool +snl_add_msg_attr_s32(struct snl_writer *nw, int attrtype, int32_t value) +{ + return (snl_add_msg_attr(nw, attrtype, sizeof(int32_t), &value)); +} + +static inline bool +snl_add_msg_attr_s64(struct snl_writer *nw, int attrtype, int64_t value) +{ + return (snl_add_msg_attr(nw, attrtype, sizeof(int64_t), &value)); +} + +static inline bool +snl_add_msg_attr_flag(struct snl_writer *nw, int attrtype) +{ + return (snl_add_msg_attr(nw, attrtype, 0, NULL)); +} + +static inline bool +snl_add_msg_attr_string(struct snl_writer *nw, int attrtype, const char *str) +{ + return (snl_add_msg_attr(nw, attrtype, strlen(str) + 1, str)); +} + + +static inline int +snl_get_msg_offset(const struct snl_writer *nw) +{ + return (nw->offset - ((char *)nw->hdr - nw->base)); +} + +static inline void * +_snl_restore_msg_offset(const struct snl_writer *nw, int off) +{ + return ((void *)((char *)nw->hdr + off)); +} +#define snl_restore_msg_offset(_ns, _off, _t) ((_t *)_snl_restore_msg_offset(_ns, _off)) + +static inline int +snl_add_msg_attr_nested(struct snl_writer *nw, int attrtype) +{ + int off = snl_get_msg_offset(nw); + struct nlattr *nla = snl_reserve_msg_data(nw, sizeof(struct nlattr), struct nlattr); + if (__predict_false(nla == NULL)) + return (0); + nla->nla_type = attrtype; + return (off); +} + +static inline void +snl_end_attr_nested(const struct snl_writer *nw, int off) +{ + if (!nw->error) { + struct nlattr *nla = snl_restore_msg_offset(nw, off, struct nlattr); + nla->nla_len = NETLINK_ALIGN(snl_get_msg_offset(nw) - off); + } +} + +static inline struct nlmsghdr * +snl_create_msg_request(struct snl_writer *nw, int nlmsg_type) +{ + struct nlmsghdr *hdr; + + assert(nw->hdr == NULL); + + if (__predict_false((hdr = + snl_reserve_msg_object(nw, struct nlmsghdr)) == NULL)) + return (NULL); + hdr->nlmsg_type = nlmsg_type; + hdr->nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK; + nw->hdr = hdr; + + return (hdr); +} + +static void +snl_abort_msg(struct snl_writer *nw) +{ + if (nw->hdr != NULL) { + int offset = (char *)(&nw->base[nw->offset]) - (char *)(nw->hdr); + + nw->offset -= offset; + nw->hdr = NULL; + } +} + +static inline struct nlmsghdr * +snl_finalize_msg(struct snl_writer *nw) +{ + if (nw->error) + snl_abort_msg(nw); + if (nw->hdr != NULL) { + struct nlmsghdr *hdr = nw->hdr; + + int offset = (char *)(&nw->base[nw->offset]) - (char *)(nw->hdr); + hdr->nlmsg_len = offset; + hdr->nlmsg_seq = snl_get_seq(nw->ss); + nw->hdr = NULL; + + return (hdr); + } + return (NULL); +} + +static inline bool +snl_send_msgs(struct snl_writer *nw) +{ + int offset = nw->offset; + + assert(nw->hdr == NULL); + nw->offset = 0; + + return (snl_send(nw->ss, nw->base, offset)); +} + +#endif |