2 * SPDX-License-Identifier: BSD-2-Clause-FreeBSD
4 * Copyright (c) 2022 Alexander V. Chernikov <melifaro@FreeBSD.org>
6 * Redistribution and use in source and binary forms, with or without
7 * modification, are permitted provided that the following conditions
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.
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
27 #ifndef _NETLINK_NETLINK_SNL_H_
28 #define _NETLINK_NETLINK_SNL_H_
31 * Simple Netlink Library
43 #include <sys/types.h>
44 #include <sys/socket.h>
45 #include <netlink/netlink.h>
48 #define _roundup2(x, y) (((x)+((y)-1))&(~((y)-1)))
50 #define NETLINK_ALIGN_SIZE sizeof(uint32_t)
51 #define NETLINK_ALIGN(_len) _roundup2(_len, NETLINK_ALIGN_SIZE)
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)
59 #define NLA_TYPE(_nla) ((_nla)->nla_type & 0x3FFF)
61 #define NLA_NEXT(_attr) (struct nlattr *)(void *)((char *)_attr + NLA_ALIGN(_attr->nla_len))
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))
70 #define NL_ARRAY_LEN(_a) (sizeof(_a) / sizeof((_a)[0]))
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 */
79 lb_allocz(struct linear_buffer *lb, int len)
81 len = roundup2(len, sizeof(uint64_t));
82 if (lb->offset + len > lb->size)
84 void *data = (void *)(lb->base + lb->offset);
90 lb_clear(struct linear_buffer *lb)
92 memset(lb->base, 0, lb->offset);
104 struct linear_buffer lb;
106 #define SCRATCH_BUFFER_SIZE 1024
108 typedef void snl_parse_field_f(struct snl_state *ss, void *hdr, void *target);
109 struct snl_field_parser {
112 snl_parse_field_f *cb;
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 */
124 struct snl_hdr_parser {
125 int hdr_off; /* aligned header size */
128 const struct snl_field_parser *fp; /* array of header field parsers */
129 const struct snl_attr_parser *np; /* array of attribute parsers */
132 #define SNL_DECLARE_PARSER(_name, _t, _fp, _np) \
133 static const struct snl_hdr_parser _name = { \
134 .hdr_off = sizeof(_t), \
137 .fp_size = NL_ARRAY_LEN(_fp), \
138 .np_size = NL_ARRAY_LEN(_np), \
141 #define SNL_DECLARE_ATTR_PARSER(_name, _np) \
142 static const struct snl_hdr_parser _name = { \
144 .np_size = NL_ARRAY_LEN(_np), \
149 snl_free(struct snl_state *ss)
155 if (ss->lb.base != NULL)
161 snl_init(struct snl_state *ss, int netlink_family)
163 memset(ss, 0, sizeof(*ss));
165 ss->fd = socket(AF_NETLINK, SOCK_RAW, netlink_family);
168 ss->init_done = true;
171 socklen_t optlen = sizeof(rcvbuf);
172 if (getsockopt(ss->fd, SOL_SOCKET, SO_RCVBUF, &rcvbuf, &optlen) == -1) {
177 ss->bufsize = rcvbuf;
178 ss->buf = malloc(ss->bufsize);
179 if (ss->buf == NULL) {
184 ss->lb.size = SCRATCH_BUFFER_SIZE;
185 ss->lb.base = calloc(1, ss->lb.size);
186 if (ss->lb.base == NULL) {
195 snl_allocz(struct snl_state *ss, int len)
197 return (lb_allocz(&ss->lb, len));
201 snl_clear_lb(struct snl_state *ss)
207 snl_send(struct snl_state *ss, void *data, int sz)
209 return (send(ss->fd, data, sz, 0) == sz);
212 static inline uint32_t
213 snl_get_seq(struct snl_state *ss)
218 static inline struct nlmsghdr *
219 snl_read_message(struct snl_state *ss)
221 if (ss->off == ss->datalen) {
222 struct sockaddr_nl nladdr;
225 .iov_len = ss->bufsize,
227 struct msghdr msg = {
229 .msg_namelen = sizeof(nladdr),
236 ssize_t datalen = recvmsg(ss->fd, &msg, 0);
238 ss->datalen = datalen;
240 } else if (errno != EINTR)
244 struct nlmsghdr *hdr = (struct nlmsghdr *)(void *)&ss->buf[ss->off];
245 ss->off += NLMSG_ALIGN(hdr->nlmsg_len);
250 * Checks that attributes are sorted by attribute type.
253 snl_verify_parsers(const struct snl_hdr_parser **parser, int count)
255 for (int i = 0; i < count; i++) {
256 const struct snl_hdr_parser *p = parser[i];
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;
264 #define SNL_VERIFY_PARSERS(_p) snl_verify_parsers((_p), NL_ARRAY_LEN(_p))
266 static const struct snl_attr_parser *
267 find_parser(const struct snl_attr_parser *ps, int pslen, int key)
269 int left_i = 0, right_i = pslen - 1;
271 if (key < ps[0].type || key > ps[pslen - 1].type)
274 while (left_i + 1 < right_i) {
275 int mid_i = (left_i + right_i) / 2;
276 if (key < ps[mid_i].type)
278 else if (key > ps[mid_i].type)
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]);
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)
296 NLA_FOREACH(nla, nla_head, len) {
297 if (nla->nla_len < sizeof(struct nlattr))
299 int nla_type = nla->nla_type & NLA_TYPE_MASK;
300 const struct snl_attr_parser *s = find_parser(ps, pslen, nla_type);
302 void *ptr = (void *)((char *)target + s->off);
303 if (!s->cb(ss, nla, s->arg, ptr))
311 snl_parse_attrs(struct snl_state *ss, struct nlmsghdr *hdr, int hdrlen,
312 const struct snl_attr_parser *ps, int pslen, void *target)
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);
318 return (snl_parse_attrs_raw(ss, nla_head, len, ps, pslen, target));
322 snl_parse_header(struct snl_state *ss, void *hdr, int len,
323 const struct snl_hdr_parser *parser, void *target)
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;
331 fp->cb(ss, src, dst);
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);
342 snl_parse_nlmsg(struct snl_state *ss, struct nlmsghdr *hdr,
343 const struct snl_hdr_parser *parser, void *target)
345 return (snl_parse_header(ss, hdr + 1, hdr->nlmsg_len - sizeof(*hdr), parser, target));
349 snl_attr_get_flag(struct snl_state *ss __unused, struct nlattr *nla, void *target)
351 if (NLA_DATA_LEN(nla) == 0) {
352 *((uint8_t *)target) = 1;
359 snl_attr_get_uint16(struct snl_state *ss __unused, struct nlattr *nla,
360 const void *arg __unused, void *target)
362 if (NLA_DATA_LEN(nla) == sizeof(uint16_t)) {
363 *((uint16_t *)target) = *((const uint16_t *)NLA_DATA_CONST(nla));
370 snl_attr_get_uint32(struct snl_state *ss __unused, struct nlattr *nla,
371 const void *arg __unused, void *target)
373 if (NLA_DATA_LEN(nla) == sizeof(uint32_t)) {
374 *((uint32_t *)target) = *((const uint32_t *)NLA_DATA_CONST(nla));
381 snl_attr_get_string(struct snl_state *ss __unused, struct nlattr *nla,
382 const void *arg __unused, void *target)
384 size_t maxlen = NLA_DATA_LEN(nla);
386 if (strnlen((char *)NLA_DATA(nla), maxlen) < maxlen) {
387 *((char **)target) = (char *)NLA_DATA(nla);
394 snl_attr_get_stringn(struct snl_state *ss, struct nlattr *nla,
395 const void *arg __unused, void *target)
397 int maxlen = NLA_DATA_LEN(nla);
399 char *buf = snl_allocz(ss, maxlen + 1);
403 memcpy(buf, NLA_DATA(nla), maxlen);
405 *((char **)target) = buf;
410 snl_attr_get_nested(struct snl_state *ss, struct nlattr *nla, const void *arg, void *target)
412 const struct snl_hdr_parser *p = (const struct snl_hdr_parser *)arg;
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));
419 snl_attr_get_nla(struct snl_state *ss __unused, struct nlattr *nla, void *target)
421 *((struct nlattr **)target) = nla;
426 snl_field_get_uint8(struct snl_state *ss __unused, void *src, void *target)
428 *((uint8_t *)target) = *((uint8_t *)src);
432 snl_field_get_uint16(struct snl_state *ss __unused, void *src, void *target)
434 *((uint16_t *)target) = *((uint16_t *)src);
438 snl_field_get_uint32(struct snl_state *ss __unused, void *src, void *target)
440 *((uint32_t *)target) = *((uint32_t *)src);