1/*
2 * libnetlink.c	RTnetlink service routines.
3 *
4 *		This program is free software; you can redistribute it and/or
5 *		modify it under the terms of the GNU General Public License
6 *		as published by the Free Software Foundation; either version
7 *		2 of the License, or (at your option) any later version.
8 *
9 * Authors:	Alexey Kuznetsov, <kuznet@ms2.inr.ac.ru>
10 *
11 */
12
13#include <stdio.h>
14#include <stdlib.h>
15#include <unistd.h>
16#include <syslog.h>
17#include <fcntl.h>
18#include <net/if_arp.h>
19#include <sys/socket.h>
20#include <netinet/in.h>
21#include <string.h>
22#include <errno.h>
23#include <time.h>
24#include <sys/uio.h>
25
26#include "libnetlink.h"
27
28int rcvbuf = 1024 * 1024;
29
30void rtnl_close(struct rtnl_handle *rth)
31{
32	if (rth->fd >= 0) {
33		close(rth->fd);
34		rth->fd = -1;
35	}
36}
37
38int rtnl_open_byproto(struct rtnl_handle *rth, unsigned subscriptions,
39		      int protocol)
40{
41	socklen_t addr_len;
42	int sndbuf = 32768;
43
44	memset(rth, 0, sizeof(*rth));
45
46	rth->fd = socket(AF_NETLINK, SOCK_RAW, protocol);
47	if (rth->fd < 0) {
48		perror("Cannot open netlink socket");
49		return -1;
50	}
51
52	if (setsockopt(rth->fd,SOL_SOCKET,SO_SNDBUF,&sndbuf,sizeof(sndbuf)) < 0) {
53		perror("SO_SNDBUF");
54		return -1;
55	}
56
57	if (setsockopt(rth->fd,SOL_SOCKET,SO_RCVBUF,&rcvbuf,sizeof(rcvbuf)) < 0) {
58		perror("SO_RCVBUF");
59		return -1;
60	}
61
62	memset(&rth->local, 0, sizeof(rth->local));
63	rth->local.nl_family = AF_NETLINK;
64	rth->local.nl_groups = subscriptions;
65
66	if (bind(rth->fd, (struct sockaddr*)&rth->local, sizeof(rth->local)) < 0) {
67		perror("Cannot bind netlink socket");
68		return -1;
69	}
70	addr_len = sizeof(rth->local);
71	if (getsockname(rth->fd, (struct sockaddr*)&rth->local, &addr_len) < 0) {
72		perror("Cannot getsockname");
73		return -1;
74	}
75	if (addr_len != sizeof(rth->local)) {
76		fprintf(stderr, "Wrong address length %d\n", addr_len);
77		return -1;
78	}
79	if (rth->local.nl_family != AF_NETLINK) {
80		fprintf(stderr, "Wrong address family %d\n", rth->local.nl_family);
81		return -1;
82	}
83	rth->seq = time(NULL);
84	return 0;
85}
86
87int rtnl_open(struct rtnl_handle *rth, unsigned subscriptions)
88{
89	return rtnl_open_byproto(rth, subscriptions, NETLINK_ROUTE);
90}
91
92int rtnl_wilddump_request(struct rtnl_handle *rth, int family, int type)
93{
94	struct {
95		struct nlmsghdr nlh;
96		struct rtgenmsg g;
97	} req;
98
99	memset(&req, 0, sizeof(req));
100	req.nlh.nlmsg_len = sizeof(req);
101	req.nlh.nlmsg_type = type;
102	req.nlh.nlmsg_flags = NLM_F_ROOT|NLM_F_MATCH|NLM_F_REQUEST;
103	req.nlh.nlmsg_pid = 0;
104	req.nlh.nlmsg_seq = rth->dump = ++rth->seq;
105	req.g.rtgen_family = family;
106
107	return send(rth->fd, (void*)&req, sizeof(req), 0);
108}
109
110int rtnl_send(struct rtnl_handle *rth, const char *buf, int len)
111{
112	return send(rth->fd, buf, len, 0);
113}
114
115int rtnl_send_check(struct rtnl_handle *rth, const char *buf, int len)
116{
117	struct nlmsghdr *h;
118	int status;
119	char resp[1024];
120
121	status = send(rth->fd, buf, len, 0);
122	if (status < 0)
123		return status;
124
125	/* Check for immediate errors */
126	status = recv(rth->fd, resp, sizeof(resp), MSG_DONTWAIT|MSG_PEEK);
127	if (status < 0) {
128		if (errno == EAGAIN)
129			return 0;
130		return -1;
131	}
132
133	for (h = (struct nlmsghdr *)resp; NLMSG_OK(h, status);
134	     h = NLMSG_NEXT(h, status)) {
135		if (h->nlmsg_type == NLMSG_ERROR) {
136			struct nlmsgerr *err = (struct nlmsgerr*)NLMSG_DATA(h);
137			if (h->nlmsg_len < NLMSG_LENGTH(sizeof(struct nlmsgerr)))
138				fprintf(stderr, "ERROR truncated\n");
139			else
140				errno = -err->error;
141			return -1;
142		}
143	}
144
145	return 0;
146}
147
148int rtnl_dump_request(struct rtnl_handle *rth, int type, void *req, int len)
149{
150	struct nlmsghdr nlh;
151	struct sockaddr_nl nladdr;
152	struct iovec iov[2] = {
153		{ .iov_base = &nlh, .iov_len = sizeof(nlh) },
154		{ .iov_base = req, .iov_len = len }
155	};
156	struct msghdr msg = {
157		.msg_name = &nladdr,
158		.msg_namelen = 	sizeof(nladdr),
159		.msg_iov = iov,
160		.msg_iovlen = 2,
161	};
162
163	memset(&nladdr, 0, sizeof(nladdr));
164	nladdr.nl_family = AF_NETLINK;
165
166	nlh.nlmsg_len = NLMSG_LENGTH(len);
167	nlh.nlmsg_type = type;
168	nlh.nlmsg_flags = NLM_F_ROOT|NLM_F_MATCH|NLM_F_REQUEST;
169	nlh.nlmsg_pid = 0;
170	nlh.nlmsg_seq = rth->dump = ++rth->seq;
171
172	return sendmsg(rth->fd, &msg, 0);
173}
174
175int rtnl_dump_filter_l(struct rtnl_handle *rth,
176		       const struct rtnl_dump_filter_arg *arg)
177{
178	struct sockaddr_nl nladdr;
179	struct iovec iov;
180	struct msghdr msg = {
181		.msg_name = &nladdr,
182		.msg_namelen = sizeof(nladdr),
183		.msg_iov = &iov,
184		.msg_iovlen = 1,
185	};
186	char buf[16384];
187
188	iov.iov_base = buf;
189	while (1) {
190		int status;
191		const struct rtnl_dump_filter_arg *a;
192		int found_done = 0;
193		int msglen = 0;
194
195		iov.iov_len = sizeof(buf);
196		status = recvmsg(rth->fd, &msg, 0);
197
198		if (status < 0) {
199			if (errno == EINTR || errno == EAGAIN)
200				continue;
201			fprintf(stderr, "netlink receive error %s (%d)\n",
202				strerror(errno), errno);
203			return -1;
204		}
205
206		if (status == 0) {
207			fprintf(stderr, "EOF on netlink\n");
208			return -1;
209		}
210
211		for (a = arg; a->filter; a++) {
212			struct nlmsghdr *h = (struct nlmsghdr*)buf;
213			msglen = status;
214
215			while (NLMSG_OK(h, msglen)) {
216				int err;
217
218				if (nladdr.nl_pid != 0 ||
219				    h->nlmsg_pid != rth->local.nl_pid ||
220				    h->nlmsg_seq != rth->dump) {
221					if (a->junk) {
222						err = a->junk(&nladdr, h,
223							      a->arg2);
224						if (err < 0)
225							return err;
226					}
227					goto skip_it;
228				}
229
230				if (h->nlmsg_type == NLMSG_DONE) {
231					found_done = 1;
232					break; /* process next filter */
233				}
234				if (h->nlmsg_type == NLMSG_ERROR) {
235					struct nlmsgerr *err = (struct nlmsgerr*)NLMSG_DATA(h);
236					if (h->nlmsg_len < NLMSG_LENGTH(sizeof(struct nlmsgerr))) {
237						fprintf(stderr,
238							"ERROR truncated\n");
239					} else {
240						errno = -err->error;
241						perror("RTNETLINK answers");
242					}
243					return -1;
244				}
245				err = a->filter(&nladdr, h, a->arg1);
246				if (err < 0)
247					return err;
248
249skip_it:
250				h = NLMSG_NEXT(h, msglen);
251			}
252		}
253
254		if (found_done)
255			return 0;
256
257		if (msg.msg_flags & MSG_TRUNC) {
258			fprintf(stderr, "Message truncated\n");
259			continue;
260		}
261		if (msglen) {
262			fprintf(stderr, "!!!Remnant of size %d\n", msglen);
263			exit(1);
264		}
265	}
266}
267
268int rtnl_dump_filter(struct rtnl_handle *rth,
269		     rtnl_filter_t filter,
270		     void *arg1,
271		     rtnl_filter_t junk,
272		     void *arg2)
273{
274	const struct rtnl_dump_filter_arg a[2] = {
275		{ .filter = filter, .arg1 = arg1, .junk = junk, .arg2 = arg2 },
276		{ .filter = NULL,   .arg1 = NULL, .junk = NULL, .arg2 = NULL }
277	};
278
279	return rtnl_dump_filter_l(rth, a);
280}
281
282int rtnl_talk(struct rtnl_handle *rtnl, struct nlmsghdr *n, pid_t peer,
283	      unsigned groups, struct nlmsghdr *answer,
284	      rtnl_filter_t junk,
285	      void *jarg)
286{
287	int status;
288	unsigned seq;
289	struct nlmsghdr *h;
290	struct sockaddr_nl nladdr;
291	struct iovec iov = {
292		.iov_base = (void*) n,
293		.iov_len = n->nlmsg_len
294	};
295	struct msghdr msg = {
296		.msg_name = &nladdr,
297		.msg_namelen = sizeof(nladdr),
298		.msg_iov = &iov,
299		.msg_iovlen = 1,
300	};
301	char   buf[16384];
302
303	memset(&nladdr, 0, sizeof(nladdr));
304	nladdr.nl_family = AF_NETLINK;
305	nladdr.nl_pid = peer;
306	nladdr.nl_groups = groups;
307
308	n->nlmsg_seq = seq = ++rtnl->seq;
309
310	if (answer == NULL)
311		n->nlmsg_flags |= NLM_F_ACK;
312
313	status = sendmsg(rtnl->fd, &msg, 0);
314
315	if (status < 0) {
316		perror("Cannot talk to rtnetlink");
317		return -1;
318	}
319
320	memset(buf,0,sizeof(buf));
321
322	iov.iov_base = buf;
323
324	while (1) {
325		iov.iov_len = sizeof(buf);
326		status = recvmsg(rtnl->fd, &msg, 0);
327
328		if (status < 0) {
329			if (errno == EINTR || errno == EAGAIN)
330				continue;
331			fprintf(stderr, "netlink receive error %s (%d)\n",
332				strerror(errno), errno);
333			return -1;
334		}
335		if (status == 0) {
336			fprintf(stderr, "EOF on netlink\n");
337			return -1;
338		}
339		if (msg.msg_namelen != sizeof(nladdr)) {
340			fprintf(stderr, "sender address length == %d\n", msg.msg_namelen);
341			exit(1);
342		}
343		for (h = (struct nlmsghdr*)buf; status >= sizeof(*h); ) {
344			int err;
345			int len = h->nlmsg_len;
346			int l = len - sizeof(*h);
347
348			if (l<0 || len>status) {
349				if (msg.msg_flags & MSG_TRUNC) {
350					fprintf(stderr, "Truncated message\n");
351					return -1;
352				}
353				fprintf(stderr, "!!!malformed message: len=%d\n", len);
354				exit(1);
355			}
356
357			if (nladdr.nl_pid != peer ||
358			    h->nlmsg_pid != rtnl->local.nl_pid ||
359			    h->nlmsg_seq != seq) {
360				if (junk) {
361					err = junk(&nladdr, h, jarg);
362					if (err < 0)
363						return err;
364				}
365				/* Don't forget to skip that message. */
366				status -= NLMSG_ALIGN(len);
367				h = (struct nlmsghdr*)((char*)h + NLMSG_ALIGN(len));
368				continue;
369			}
370
371			if (h->nlmsg_type == NLMSG_ERROR) {
372				struct nlmsgerr *err = (struct nlmsgerr*)NLMSG_DATA(h);
373				if (l < sizeof(struct nlmsgerr)) {
374					fprintf(stderr, "ERROR truncated\n");
375				} else {
376					errno = -err->error;
377					if (errno == 0) {
378						if (answer)
379							memcpy(answer, h, h->nlmsg_len);
380						return 0;
381					}
382					perror("RTNETLINK answers");
383				}
384				return -1;
385			}
386			if (answer) {
387				memcpy(answer, h, h->nlmsg_len);
388				return 0;
389			}
390
391			fprintf(stderr, "Unexpected reply!!!\n");
392
393			status -= NLMSG_ALIGN(len);
394			h = (struct nlmsghdr*)((char*)h + NLMSG_ALIGN(len));
395		}
396		if (msg.msg_flags & MSG_TRUNC) {
397			fprintf(stderr, "Message truncated\n");
398			continue;
399		}
400		if (status) {
401			fprintf(stderr, "!!!Remnant of size %d\n", status);
402			exit(1);
403		}
404	}
405}
406
407int rtnl_listen(struct rtnl_handle *rtnl,
408		rtnl_filter_t handler,
409		void *jarg)
410{
411	int status;
412	struct nlmsghdr *h;
413	struct sockaddr_nl nladdr;
414	struct iovec iov;
415	struct msghdr msg = {
416		.msg_name = &nladdr,
417		.msg_namelen = sizeof(nladdr),
418		.msg_iov = &iov,
419		.msg_iovlen = 1,
420	};
421	char   buf[8192];
422
423	memset(&nladdr, 0, sizeof(nladdr));
424	nladdr.nl_family = AF_NETLINK;
425	nladdr.nl_pid = 0;
426	nladdr.nl_groups = 0;
427
428	iov.iov_base = buf;
429	while (1) {
430		iov.iov_len = sizeof(buf);
431		status = recvmsg(rtnl->fd, &msg, 0);
432
433		if (status < 0) {
434			if (errno == EINTR || errno == EAGAIN)
435				continue;
436			fprintf(stderr, "netlink receive error %s (%d)\n",
437				strerror(errno), errno);
438			if (errno == ENOBUFS)
439				continue;
440			return -1;
441		}
442		if (status == 0) {
443			fprintf(stderr, "EOF on netlink\n");
444			return -1;
445		}
446		if (msg.msg_namelen != sizeof(nladdr)) {
447			fprintf(stderr, "Sender address length == %d\n", msg.msg_namelen);
448			exit(1);
449		}
450		for (h = (struct nlmsghdr*)buf; status >= sizeof(*h); ) {
451			int err;
452			int len = h->nlmsg_len;
453			int l = len - sizeof(*h);
454
455			if (l<0 || len>status) {
456				if (msg.msg_flags & MSG_TRUNC) {
457					fprintf(stderr, "Truncated message\n");
458					return -1;
459				}
460				fprintf(stderr, "!!!malformed message: len=%d\n", len);
461				exit(1);
462			}
463
464			err = handler(&nladdr, h, jarg);
465			if (err < 0)
466				return err;
467
468			status -= NLMSG_ALIGN(len);
469			h = (struct nlmsghdr*)((char*)h + NLMSG_ALIGN(len));
470		}
471		if (msg.msg_flags & MSG_TRUNC) {
472			fprintf(stderr, "Message truncated\n");
473			continue;
474		}
475		if (status) {
476			fprintf(stderr, "!!!Remnant of size %d\n", status);
477			exit(1);
478		}
479	}
480}
481
482int rtnl_from_file(FILE *rtnl, rtnl_filter_t handler,
483		   void *jarg)
484{
485	int status;
486	struct sockaddr_nl nladdr;
487	char   buf[8192];
488	struct nlmsghdr *h = (void*)buf;
489
490	memset(&nladdr, 0, sizeof(nladdr));
491	nladdr.nl_family = AF_NETLINK;
492	nladdr.nl_pid = 0;
493	nladdr.nl_groups = 0;
494
495	while (1) {
496		int err, len;
497		int l;
498
499		status = fread(&buf, 1, sizeof(*h), rtnl);
500
501		if (status < 0) {
502			if (errno == EINTR)
503				continue;
504			perror("rtnl_from_file: fread");
505			return -1;
506		}
507		if (status == 0)
508			return 0;
509
510		len = h->nlmsg_len;
511		l = len - sizeof(*h);
512
513		if (l<0 || len>sizeof(buf)) {
514			fprintf(stderr, "!!!malformed message: len=%d @%lu\n",
515				len, ftell(rtnl));
516			return -1;
517		}
518
519		status = fread(NLMSG_DATA(h), 1, NLMSG_ALIGN(l), rtnl);
520
521		if (status < 0) {
522			perror("rtnl_from_file: fread");
523			return -1;
524		}
525		if (status < l) {
526			fprintf(stderr, "rtnl-from_file: truncated message\n");
527			return -1;
528		}
529
530		err = handler(&nladdr, h, jarg);
531		if (err < 0)
532			return err;
533	}
534}
535
536int addattr32(struct nlmsghdr *n, int maxlen, int type, __u32 data)
537{
538	int len = RTA_LENGTH(4);
539	struct rtattr *rta;
540	if (NLMSG_ALIGN(n->nlmsg_len) + len > maxlen) {
541		fprintf(stderr,"addattr32: Error! max allowed bound %d exceeded\n",maxlen);
542		return -1;
543	}
544	rta = NLMSG_TAIL(n);
545	rta->rta_type = type;
546	rta->rta_len = len;
547	memcpy(RTA_DATA(rta), &data, 4);
548	n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + len;
549	return 0;
550}
551
552int addattr_l(struct nlmsghdr *n, int maxlen, int type, const void *data,
553	      int alen)
554{
555	int len = RTA_LENGTH(alen);
556	struct rtattr *rta;
557
558	if (NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len) > maxlen) {
559		fprintf(stderr, "addattr_l ERROR: message exceeded bound of %d\n",maxlen);
560		return -1;
561	}
562	rta = NLMSG_TAIL(n);
563	rta->rta_type = type;
564	rta->rta_len = len;
565	memcpy(RTA_DATA(rta), data, alen);
566	n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len);
567	return 0;
568}
569
570int addraw_l(struct nlmsghdr *n, int maxlen, const void *data, int len)
571{
572	if (NLMSG_ALIGN(n->nlmsg_len) + NLMSG_ALIGN(len) > maxlen) {
573		fprintf(stderr, "addraw_l ERROR: message exceeded bound of %d\n",maxlen);
574		return -1;
575	}
576
577	memcpy(NLMSG_TAIL(n), data, len);
578	memset((void *) NLMSG_TAIL(n) + len, 0, NLMSG_ALIGN(len) - len);
579	n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + NLMSG_ALIGN(len);
580	return 0;
581}
582
583struct rtattr *addattr_nest(struct nlmsghdr *n, int maxlen, int type)
584{
585	struct rtattr *nest = NLMSG_TAIL(n);
586
587	addattr_l(n, maxlen, type, NULL, 0);
588	return nest;
589}
590
591int addattr_nest_end(struct nlmsghdr *n, struct rtattr *nest)
592{
593	nest->rta_len = (void *)NLMSG_TAIL(n) - (void *)nest;
594	return n->nlmsg_len;
595}
596
597struct rtattr *addattr_nest_compat(struct nlmsghdr *n, int maxlen, int type,
598				   const void *data, int len)
599{
600	struct rtattr *start = NLMSG_TAIL(n);
601
602	addattr_l(n, maxlen, type, data, len);
603	addattr_nest(n, maxlen, type);
604	return start;
605}
606
607int addattr_nest_compat_end(struct nlmsghdr *n, struct rtattr *start)
608{
609	struct rtattr *nest = (void *)start + NLMSG_ALIGN(start->rta_len);
610
611	start->rta_len = (void *)NLMSG_TAIL(n) - (void *)start;
612	addattr_nest_end(n, nest);
613	return n->nlmsg_len;
614}
615
616int rta_addattr32(struct rtattr *rta, int maxlen, int type, __u32 data)
617{
618	int len = RTA_LENGTH(4);
619	struct rtattr *subrta;
620
621	if (RTA_ALIGN(rta->rta_len) + len > maxlen) {
622		fprintf(stderr,"rta_addattr32: Error! max allowed bound %d exceeded\n",maxlen);
623		return -1;
624	}
625	subrta = (struct rtattr*)(((char*)rta) + RTA_ALIGN(rta->rta_len));
626	subrta->rta_type = type;
627	subrta->rta_len = len;
628	memcpy(RTA_DATA(subrta), &data, 4);
629	rta->rta_len = NLMSG_ALIGN(rta->rta_len) + len;
630	return 0;
631}
632
633int rta_addattr_l(struct rtattr *rta, int maxlen, int type,
634		  const void *data, int alen)
635{
636	struct rtattr *subrta;
637	int len = RTA_LENGTH(alen);
638
639	if (RTA_ALIGN(rta->rta_len) + RTA_ALIGN(len) > maxlen) {
640		fprintf(stderr,"rta_addattr_l: Error! max allowed bound %d exceeded\n",maxlen);
641		return -1;
642	}
643	subrta = (struct rtattr*)(((char*)rta) + RTA_ALIGN(rta->rta_len));
644	subrta->rta_type = type;
645	subrta->rta_len = len;
646	memcpy(RTA_DATA(subrta), data, alen);
647	rta->rta_len = NLMSG_ALIGN(rta->rta_len) + RTA_ALIGN(len);
648	return 0;
649}
650
651int parse_rtattr(struct rtattr *tb[], int max, struct rtattr *rta, int len)
652{
653	memset(tb, 0, sizeof(struct rtattr *) * (max + 1));
654	while (RTA_OK(rta, len)) {
655		if ((rta->rta_type <= max) && (!tb[rta->rta_type]))
656			tb[rta->rta_type] = rta;
657		rta = RTA_NEXT(rta,len);
658	}
659	if (len)
660		fprintf(stderr, "!!!Deficit %d, rta_len=%d\n", len, rta->rta_len);
661	return 0;
662}
663
664int parse_rtattr_byindex(struct rtattr *tb[], int max, struct rtattr *rta, int len)
665{
666	int i = 0;
667
668	memset(tb, 0, sizeof(struct rtattr *) * max);
669	while (RTA_OK(rta, len)) {
670		if (rta->rta_type <= max && i < max)
671			tb[i++] = rta;
672		rta = RTA_NEXT(rta,len);
673	}
674	if (len)
675		fprintf(stderr, "!!!Deficit %d, rta_len=%d\n", len, rta->rta_len);
676	return i;
677}
678
679int __parse_rtattr_nested_compat(struct rtattr *tb[], int max, struct rtattr *rta,
680			         int len)
681{
682	if (RTA_PAYLOAD(rta) < len)
683		return -1;
684	if (RTA_PAYLOAD(rta) >= RTA_ALIGN(len) + sizeof(struct rtattr)) {
685		rta = RTA_DATA(rta) + RTA_ALIGN(len);
686		return parse_rtattr_nested(tb, max, rta);
687	}
688	memset(tb, 0, sizeof(struct rtattr *) * (max + 1));
689	return 0;
690}
691