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
193		iov.iov_len = sizeof(buf);
194		status = recvmsg(rth->fd, &msg, 0);
195
196		if (status < 0) {
197			if (errno == EINTR || errno == EAGAIN)
198				continue;
199			fprintf(stderr, "netlink receive error %s (%d)\n",
200				strerror(errno), errno);
201			return -1;
202		}
203
204		if (status == 0) {
205			fprintf(stderr, "EOF on netlink\n");
206			return -1;
207		}
208
209		for (a = arg; a->filter; a++) {
210			struct nlmsghdr *h = (struct nlmsghdr*)buf;
211
212			while (NLMSG_OK(h, status)) {
213				int err;
214
215				if (nladdr.nl_pid != 0 ||
216				    h->nlmsg_pid != rth->local.nl_pid ||
217				    h->nlmsg_seq != rth->dump) {
218					if (a->junk) {
219						err = a->junk(&nladdr, h,
220							      a->arg2);
221						if (err < 0)
222							return err;
223					}
224					goto skip_it;
225				}
226
227				if (h->nlmsg_type == NLMSG_DONE)
228					return 0;
229				if (h->nlmsg_type == NLMSG_ERROR) {
230					struct nlmsgerr *err = (struct nlmsgerr*)NLMSG_DATA(h);
231					if (h->nlmsg_len < NLMSG_LENGTH(sizeof(struct nlmsgerr))) {
232						fprintf(stderr,
233							"ERROR truncated\n");
234					} else {
235						errno = -err->error;
236						perror("RTNETLINK answers");
237					}
238					return -1;
239				}
240				err = a->filter(&nladdr, h, a->arg1);
241				if (err < 0)
242					return err;
243
244skip_it:
245				h = NLMSG_NEXT(h, status);
246			}
247		} while (0);
248		if (msg.msg_flags & MSG_TRUNC) {
249			fprintf(stderr, "Message truncated\n");
250			continue;
251		}
252		if (status) {
253			fprintf(stderr, "!!!Remnant of size %d\n", status);
254			exit(1);
255		}
256	}
257}
258
259int rtnl_dump_filter(struct rtnl_handle *rth,
260		     rtnl_filter_t filter,
261		     void *arg1,
262		     rtnl_filter_t junk,
263		     void *arg2)
264{
265	const struct rtnl_dump_filter_arg a[2] = {
266		{ .filter = filter, .arg1 = arg1, .junk = junk, .arg2 = arg2 },
267		{ .filter = NULL,   .arg1 = NULL, .junk = NULL, .arg2 = NULL }
268	};
269
270	return rtnl_dump_filter_l(rth, a);
271}
272
273int rtnl_talk(struct rtnl_handle *rtnl, struct nlmsghdr *n, pid_t peer,
274	      unsigned groups, struct nlmsghdr *answer,
275	      rtnl_filter_t junk,
276	      void *jarg)
277{
278	int status;
279	unsigned seq;
280	struct nlmsghdr *h;
281	struct sockaddr_nl nladdr;
282	struct iovec iov = {
283		.iov_base = (void*) n,
284		.iov_len = n->nlmsg_len
285	};
286	struct msghdr msg = {
287		.msg_name = &nladdr,
288		.msg_namelen = sizeof(nladdr),
289		.msg_iov = &iov,
290		.msg_iovlen = 1,
291	};
292	char   buf[16384];
293
294	memset(&nladdr, 0, sizeof(nladdr));
295	nladdr.nl_family = AF_NETLINK;
296	nladdr.nl_pid = peer;
297	nladdr.nl_groups = groups;
298
299	n->nlmsg_seq = seq = ++rtnl->seq;
300
301	if (answer == NULL)
302		n->nlmsg_flags |= NLM_F_ACK;
303
304	status = sendmsg(rtnl->fd, &msg, 0);
305
306	if (status < 0) {
307		perror("Cannot talk to rtnetlink");
308		return -1;
309	}
310
311	memset(buf,0,sizeof(buf));
312
313	iov.iov_base = buf;
314
315	while (1) {
316		iov.iov_len = sizeof(buf);
317		status = recvmsg(rtnl->fd, &msg, 0);
318
319		if (status < 0) {
320			if (errno == EINTR || errno == EAGAIN)
321				continue;
322			fprintf(stderr, "netlink receive error %s (%d)\n",
323				strerror(errno), errno);
324			return -1;
325		}
326		if (status == 0) {
327			fprintf(stderr, "EOF on netlink\n");
328			return -1;
329		}
330		if (msg.msg_namelen != sizeof(nladdr)) {
331			fprintf(stderr, "sender address length == %d\n", msg.msg_namelen);
332			exit(1);
333		}
334		for (h = (struct nlmsghdr*)buf; status >= sizeof(*h); ) {
335			int err;
336			int len = h->nlmsg_len;
337			int l = len - sizeof(*h);
338
339			if (l<0 || len>status) {
340				if (msg.msg_flags & MSG_TRUNC) {
341					fprintf(stderr, "Truncated message\n");
342					return -1;
343				}
344				fprintf(stderr, "!!!malformed message: len=%d\n", len);
345				exit(1);
346			}
347
348			if (nladdr.nl_pid != peer ||
349			    h->nlmsg_pid != rtnl->local.nl_pid ||
350			    h->nlmsg_seq != seq) {
351				if (junk) {
352					err = junk(&nladdr, h, jarg);
353					if (err < 0)
354						return err;
355				}
356				/* Don't forget to skip that message. */
357				status -= NLMSG_ALIGN(len);
358				h = (struct nlmsghdr*)((char*)h + NLMSG_ALIGN(len));
359				continue;
360			}
361
362			if (h->nlmsg_type == NLMSG_ERROR) {
363				struct nlmsgerr *err = (struct nlmsgerr*)NLMSG_DATA(h);
364				if (l < sizeof(struct nlmsgerr)) {
365					fprintf(stderr, "ERROR truncated\n");
366				} else {
367					errno = -err->error;
368					if (errno == 0) {
369						if (answer)
370							memcpy(answer, h, h->nlmsg_len);
371						return 0;
372					}
373					perror("RTNETLINK answers");
374				}
375				return -1;
376			}
377			if (answer) {
378				memcpy(answer, h, h->nlmsg_len);
379				return 0;
380			}
381
382			fprintf(stderr, "Unexpected reply!!!\n");
383
384			status -= NLMSG_ALIGN(len);
385			h = (struct nlmsghdr*)((char*)h + NLMSG_ALIGN(len));
386		}
387		if (msg.msg_flags & MSG_TRUNC) {
388			fprintf(stderr, "Message truncated\n");
389			continue;
390		}
391		if (status) {
392			fprintf(stderr, "!!!Remnant of size %d\n", status);
393			exit(1);
394		}
395	}
396}
397
398int rtnl_listen(struct rtnl_handle *rtnl,
399		rtnl_filter_t handler,
400		void *jarg)
401{
402	int status;
403	struct nlmsghdr *h;
404	struct sockaddr_nl nladdr;
405	struct iovec iov;
406	struct msghdr msg = {
407		.msg_name = &nladdr,
408		.msg_namelen = sizeof(nladdr),
409		.msg_iov = &iov,
410		.msg_iovlen = 1,
411	};
412	char   buf[8192];
413
414	memset(&nladdr, 0, sizeof(nladdr));
415	nladdr.nl_family = AF_NETLINK;
416	nladdr.nl_pid = 0;
417	nladdr.nl_groups = 0;
418
419	iov.iov_base = buf;
420	while (1) {
421		iov.iov_len = sizeof(buf);
422		status = recvmsg(rtnl->fd, &msg, 0);
423
424		if (status < 0) {
425			if (errno == EINTR || errno == EAGAIN)
426				continue;
427			fprintf(stderr, "netlink receive error %s (%d)\n",
428				strerror(errno), errno);
429			if (errno == ENOBUFS)
430				continue;
431			return -1;
432		}
433		if (status == 0) {
434			fprintf(stderr, "EOF on netlink\n");
435			return -1;
436		}
437		if (msg.msg_namelen != sizeof(nladdr)) {
438			fprintf(stderr, "Sender address length == %d\n", msg.msg_namelen);
439			exit(1);
440		}
441		for (h = (struct nlmsghdr*)buf; status >= sizeof(*h); ) {
442			int err;
443			int len = h->nlmsg_len;
444			int l = len - sizeof(*h);
445
446			if (l<0 || len>status) {
447				if (msg.msg_flags & MSG_TRUNC) {
448					fprintf(stderr, "Truncated message\n");
449					return -1;
450				}
451				fprintf(stderr, "!!!malformed message: len=%d\n", len);
452				exit(1);
453			}
454
455			err = handler(&nladdr, h, jarg);
456			if (err < 0)
457				return err;
458
459			status -= NLMSG_ALIGN(len);
460			h = (struct nlmsghdr*)((char*)h + NLMSG_ALIGN(len));
461		}
462		if (msg.msg_flags & MSG_TRUNC) {
463			fprintf(stderr, "Message truncated\n");
464			continue;
465		}
466		if (status) {
467			fprintf(stderr, "!!!Remnant of size %d\n", status);
468			exit(1);
469		}
470	}
471}
472
473int rtnl_from_file(FILE *rtnl, rtnl_filter_t handler,
474		   void *jarg)
475{
476	int status;
477	struct sockaddr_nl nladdr;
478	char   buf[8192];
479	struct nlmsghdr *h = (void*)buf;
480
481	memset(&nladdr, 0, sizeof(nladdr));
482	nladdr.nl_family = AF_NETLINK;
483	nladdr.nl_pid = 0;
484	nladdr.nl_groups = 0;
485
486	while (1) {
487		int err, len, type;
488		int l;
489
490		status = fread(&buf, 1, sizeof(*h), rtnl);
491
492		if (status < 0) {
493			if (errno == EINTR)
494				continue;
495			perror("rtnl_from_file: fread");
496			return -1;
497		}
498		if (status == 0)
499			return 0;
500
501		len = h->nlmsg_len;
502		type= h->nlmsg_type;
503		l = len - sizeof(*h);
504
505		if (l<0 || len>sizeof(buf)) {
506			fprintf(stderr, "!!!malformed message: len=%d @%lu\n",
507				len, ftell(rtnl));
508			return -1;
509		}
510
511		status = fread(NLMSG_DATA(h), 1, NLMSG_ALIGN(l), rtnl);
512
513		if (status < 0) {
514			perror("rtnl_from_file: fread");
515			return -1;
516		}
517		if (status < l) {
518			fprintf(stderr, "rtnl-from_file: truncated message\n");
519			return -1;
520		}
521
522		err = handler(&nladdr, h, jarg);
523		if (err < 0)
524			return err;
525	}
526}
527
528int addattr32(struct nlmsghdr *n, int maxlen, int type, __u32 data)
529{
530	int len = RTA_LENGTH(4);
531	struct rtattr *rta;
532	if (NLMSG_ALIGN(n->nlmsg_len) + len > maxlen) {
533		fprintf(stderr,"addattr32: Error! max allowed bound %d exceeded\n",maxlen);
534		return -1;
535	}
536	rta = NLMSG_TAIL(n);
537	rta->rta_type = type;
538	rta->rta_len = len;
539	memcpy(RTA_DATA(rta), &data, 4);
540	n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + len;
541	return 0;
542}
543
544int addattr_l(struct nlmsghdr *n, int maxlen, int type, const void *data,
545	      int alen)
546{
547	int len = RTA_LENGTH(alen);
548	struct rtattr *rta;
549
550	if (NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len) > maxlen) {
551		fprintf(stderr, "addattr_l ERROR: message exceeded bound of %d\n",maxlen);
552		return -1;
553	}
554	rta = NLMSG_TAIL(n);
555	rta->rta_type = type;
556	rta->rta_len = len;
557	memcpy(RTA_DATA(rta), data, alen);
558	n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len);
559	return 0;
560}
561
562int addraw_l(struct nlmsghdr *n, int maxlen, const void *data, int len)
563{
564	if (NLMSG_ALIGN(n->nlmsg_len) + NLMSG_ALIGN(len) > maxlen) {
565		fprintf(stderr, "addraw_l ERROR: message exceeded bound of %d\n",maxlen);
566		return -1;
567	}
568
569	memcpy(NLMSG_TAIL(n), data, len);
570	memset((void *) NLMSG_TAIL(n) + len, 0, NLMSG_ALIGN(len) - len);
571	n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + NLMSG_ALIGN(len);
572	return 0;
573}
574
575struct rtattr *addattr_nest(struct nlmsghdr *n, int maxlen, int type)
576{
577	struct rtattr *nest = NLMSG_TAIL(n);
578
579	addattr_l(n, maxlen, type, NULL, 0);
580	return nest;
581}
582
583int addattr_nest_end(struct nlmsghdr *n, struct rtattr *nest)
584{
585	nest->rta_len = (void *)NLMSG_TAIL(n) - (void *)nest;
586	return n->nlmsg_len;
587}
588
589struct rtattr *addattr_nest_compat(struct nlmsghdr *n, int maxlen, int type,
590				   const void *data, int len)
591{
592	struct rtattr *start = NLMSG_TAIL(n);
593
594	addattr_l(n, maxlen, type, data, len);
595	addattr_nest(n, maxlen, type);
596	return start;
597}
598
599int addattr_nest_compat_end(struct nlmsghdr *n, struct rtattr *start)
600{
601	struct rtattr *nest = (void *)start + NLMSG_ALIGN(start->rta_len);
602
603	start->rta_len = (void *)NLMSG_TAIL(n) - (void *)start;
604	addattr_nest_end(n, nest);
605	return n->nlmsg_len;
606}
607
608int rta_addattr32(struct rtattr *rta, int maxlen, int type, __u32 data)
609{
610	int len = RTA_LENGTH(4);
611	struct rtattr *subrta;
612
613	if (RTA_ALIGN(rta->rta_len) + len > maxlen) {
614		fprintf(stderr,"rta_addattr32: Error! max allowed bound %d exceeded\n",maxlen);
615		return -1;
616	}
617	subrta = (struct rtattr*)(((char*)rta) + RTA_ALIGN(rta->rta_len));
618	subrta->rta_type = type;
619	subrta->rta_len = len;
620	memcpy(RTA_DATA(subrta), &data, 4);
621	rta->rta_len = NLMSG_ALIGN(rta->rta_len) + len;
622	return 0;
623}
624
625int rta_addattr_l(struct rtattr *rta, int maxlen, int type,
626		  const void *data, int alen)
627{
628	struct rtattr *subrta;
629	int len = RTA_LENGTH(alen);
630
631	if (RTA_ALIGN(rta->rta_len) + RTA_ALIGN(len) > maxlen) {
632		fprintf(stderr,"rta_addattr_l: Error! max allowed bound %d exceeded\n",maxlen);
633		return -1;
634	}
635	subrta = (struct rtattr*)(((char*)rta) + RTA_ALIGN(rta->rta_len));
636	subrta->rta_type = type;
637	subrta->rta_len = len;
638	memcpy(RTA_DATA(subrta), data, alen);
639	rta->rta_len = NLMSG_ALIGN(rta->rta_len) + RTA_ALIGN(len);
640	return 0;
641}
642
643int parse_rtattr(struct rtattr *tb[], int max, struct rtattr *rta, int len)
644{
645	memset(tb, 0, sizeof(struct rtattr *) * (max + 1));
646	while (RTA_OK(rta, len)) {
647		if (rta->rta_type <= max)
648			tb[rta->rta_type] = rta;
649		rta = RTA_NEXT(rta,len);
650	}
651	if (len)
652		fprintf(stderr, "!!!Deficit %d, rta_len=%d\n", len, rta->rta_len);
653	return 0;
654}
655
656int parse_rtattr_byindex(struct rtattr *tb[], int max, struct rtattr *rta, int len)
657{
658	int i = 0;
659
660	memset(tb, 0, sizeof(struct rtattr *) * max);
661	while (RTA_OK(rta, len)) {
662		if (rta->rta_type <= max && i < max)
663			tb[i++] = rta;
664		rta = RTA_NEXT(rta,len);
665	}
666	if (len)
667		fprintf(stderr, "!!!Deficit %d, rta_len=%d\n", len, rta->rta_len);
668	return i;
669}
670
671int __parse_rtattr_nested_compat(struct rtattr *tb[], int max, struct rtattr *rta,
672			         int len)
673{
674	if (RTA_PAYLOAD(rta) < len)
675		return -1;
676	if (RTA_PAYLOAD(rta) >= RTA_ALIGN(len) + sizeof(struct rtattr)) {
677		rta = RTA_DATA(rta) + RTA_ALIGN(len);
678		return parse_rtattr_nested(tb, max, rta);
679	}
680	memset(tb, 0, sizeof(struct rtattr *) * (max + 1));
681	return 0;
682}
683