1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312131313141315131613171318131913201321 |
- /*-
- * 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 <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>
- #include <sys/types.h>
- #include <sys/socket.h>
- #include <netlink/netlink.h>
- #include <netlink/netlink_bitset.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))
- #define NL_ARRAY_LEN(_a) (sizeof(_a) / sizeof((_a)[0]))
- 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;
- };
- 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 = NL_ARRAY_LEN(_fp), \
- .np_size = NL_ARRAY_LEN(_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 = NL_ARRAY_LEN(_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 = NL_ARRAY_LEN(_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_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), NL_ARRAY_LEN(_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 void *
- _snl_reserve_msg_attr(struct snl_writer *nw, uint16_t nla_type, uint16_t sz)
- {
- sz += sizeof(struct nlattr);
- 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 ((void *)(nla + 1));
- }
- #define snl_reserve_msg_attr(_ns, _at, _t) ((_t *)_snl_reserve_msg_attr(_ns, _at, sizeof(_t)))
- 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)
- {
- assert(nw->hdr == NULL);
- struct nlmsghdr *hdr = snl_reserve_msg_object(nw, struct nlmsghdr);
- 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));
- }
- static const struct snl_hdr_parser *snl_all_core_parsers[] = {
- &snl_errmsg_parser, &snl_donemsg_parser,
- &_nla_bit_parser, &_nla_bitset_parser,
- };
- #endif
|