xref: /freebsd/sys/netlink/netlink_snl.h (revision 81ad6265) 
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
444