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
28void rtnl_close(struct rtnl_handle *rth)
29{
30	close(rth->fd);
31}
32
33int rtnl_open_byproto(struct rtnl_handle *rth, unsigned subscriptions,
34		      int protocol)
35{
36	socklen_t addr_len;
37	int sndbuf = 32768;
38	int rcvbuf = 32768;
39
40	memset(rth, 0, sizeof(rth));
41
42	rth->fd = socket(AF_NETLINK, SOCK_RAW, protocol);
43	if (rth->fd < 0) {
44		perror("Cannot open netlink socket");
45		return -1;
46	}
47
48	if (setsockopt(rth->fd,SOL_SOCKET,SO_SNDBUF,&sndbuf,sizeof(sndbuf)) < 0) {
49		perror("SO_SNDBUF");
50		return -1;
51	}
52
53	if (setsockopt(rth->fd,SOL_SOCKET,SO_RCVBUF,&rcvbuf,sizeof(rcvbuf)) < 0) {
54		perror("SO_RCVBUF");
55		return -1;
56	}
57
58	memset(&rth->local, 0, sizeof(rth->local));
59	rth->local.nl_family = AF_NETLINK;
60	rth->local.nl_groups = subscriptions;
61
62	if (bind(rth->fd, (struct sockaddr*)&rth->local, sizeof(rth->local)) < 0) {
63		perror("Cannot bind netlink socket");
64		return -1;
65	}
66	addr_len = sizeof(rth->local);
67	if (getsockname(rth->fd, (struct sockaddr*)&rth->local, &addr_len) < 0) {
68		perror("Cannot getsockname");
69		return -1;
70	}
71	if (addr_len != sizeof(rth->local)) {
72		fprintf(stderr, "Wrong address length %d\n", addr_len);
73		return -1;
74	}
75	if (rth->local.nl_family != AF_NETLINK) {
76		fprintf(stderr, "Wrong address family %d\n", rth->local.nl_family);
77		return -1;
78	}
79	rth->seq = time(NULL);
80	return 0;
81}
82
83int rtnl_open(struct rtnl_handle *rth, unsigned subscriptions)
84{
85	return rtnl_open_byproto(rth, subscriptions, NETLINK_ROUTE);
86}
87
88int rtnl_wilddump_request(struct rtnl_handle *rth, int family, int type)
89{
90	struct {
91		struct nlmsghdr nlh;
92		struct rtgenmsg g;
93	} req;
94	struct sockaddr_nl nladdr;
95
96	memset(&nladdr, 0, sizeof(nladdr));
97	nladdr.nl_family = AF_NETLINK;
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 sendto(rth->fd, (void*)&req, sizeof(req), 0,
108		      (struct sockaddr*)&nladdr, sizeof(nladdr));
109}
110
111int rtnl_send(struct rtnl_handle *rth, const char *buf, int len)
112{
113	struct sockaddr_nl nladdr;
114
115	memset(&nladdr, 0, sizeof(nladdr));
116	nladdr.nl_family = AF_NETLINK;
117
118	return sendto(rth->fd, buf, len, 0, (struct sockaddr*)&nladdr, sizeof(nladdr));
119}
120
121int rtnl_dump_request(struct rtnl_handle *rth, int type, void *req, int len)
122{
123	struct nlmsghdr nlh;
124	struct sockaddr_nl nladdr;
125	struct iovec iov[2] = {
126		{ .iov_base = &nlh, .iov_len = sizeof(nlh) },
127		{ .iov_base = req, .iov_len = len }
128	};
129	struct msghdr msg = {
130		.msg_name = &nladdr,
131		.msg_namelen = 	sizeof(nladdr),
132		.msg_iov = iov,
133		.msg_iovlen = 2,
134	};
135
136	memset(&nladdr, 0, sizeof(nladdr));
137	nladdr.nl_family = AF_NETLINK;
138
139	nlh.nlmsg_len = NLMSG_LENGTH(len);
140	nlh.nlmsg_type = type;
141	nlh.nlmsg_flags = NLM_F_ROOT|NLM_F_MATCH|NLM_F_REQUEST;
142	nlh.nlmsg_pid = 0;
143	nlh.nlmsg_seq = rth->dump = ++rth->seq;
144
145	return sendmsg(rth->fd, &msg, 0);
146}
147
148int rtnl_dump_filter(struct rtnl_handle *rth,
149		     rtnl_filter_t filter,
150		     void *arg1,
151		     rtnl_filter_t junk,
152		     void *arg2)
153{
154	struct sockaddr_nl nladdr;
155	struct iovec iov;
156	struct msghdr msg = {
157		.msg_name = &nladdr,
158		.msg_namelen = sizeof(nladdr),
159		.msg_iov = &iov,
160		.msg_iovlen = 1,
161	};
162	char buf[16384];
163
164	iov.iov_base = buf;
165	while (1) {
166		int status;
167		struct nlmsghdr *h;
168
169		iov.iov_len = sizeof(buf);
170		status = recvmsg(rth->fd, &msg, 0);
171
172		if (status < 0) {
173			if (errno == EINTR)
174				continue;
175			perror("OVERRUN");
176			continue;
177		}
178
179		if (status == 0) {
180			fprintf(stderr, "EOF on netlink\n");
181			return -1;
182		}
183
184		h = (struct nlmsghdr*)buf;
185		while (NLMSG_OK(h, status)) {
186			int err;
187
188			if (nladdr.nl_pid != 0 ||
189			    h->nlmsg_pid != rth->local.nl_pid ||
190			    h->nlmsg_seq != rth->dump) {
191				if (junk) {
192					err = junk(&nladdr, h, arg2);
193					if (err < 0)
194						return err;
195				}
196				goto skip_it;
197			}
198
199			if (h->nlmsg_type == NLMSG_DONE)
200				return 0;
201			if (h->nlmsg_type == NLMSG_ERROR) {
202				struct nlmsgerr *err = (struct nlmsgerr*)NLMSG_DATA(h);
203				if (h->nlmsg_len < NLMSG_LENGTH(sizeof(struct nlmsgerr))) {
204					fprintf(stderr, "ERROR truncated\n");
205				} else {
206					errno = -err->error;
207					perror("RTNETLINK answers");
208				}
209				return -1;
210			}
211			err = filter(&nladdr, h, arg1);
212			if (err < 0)
213				return err;
214
215skip_it:
216			h = NLMSG_NEXT(h, status);
217		}
218		if (msg.msg_flags & MSG_TRUNC) {
219			fprintf(stderr, "Message truncated\n");
220			continue;
221		}
222		if (status) {
223			fprintf(stderr, "!!!Remnant of size %d\n", status);
224			exit(1);
225		}
226	}
227}
228
229int rtnl_talk(struct rtnl_handle *rtnl, struct nlmsghdr *n, pid_t peer,
230	      unsigned groups, struct nlmsghdr *answer,
231	      rtnl_filter_t junk,
232	      void *jarg)
233{
234	int status;
235	unsigned seq;
236	struct nlmsghdr *h;
237	struct sockaddr_nl nladdr;
238	struct iovec iov = {
239		.iov_base = (void*) n,
240		.iov_len = n->nlmsg_len
241	};
242	struct msghdr msg = {
243		.msg_name = &nladdr,
244		.msg_namelen = sizeof(nladdr),
245		.msg_iov = &iov,
246		.msg_iovlen = 1,
247	};
248	char   buf[16384];
249
250	memset(&nladdr, 0, sizeof(nladdr));
251	nladdr.nl_family = AF_NETLINK;
252	nladdr.nl_pid = peer;
253	nladdr.nl_groups = groups;
254
255	n->nlmsg_seq = seq = ++rtnl->seq;
256
257	if (answer == NULL)
258		n->nlmsg_flags |= NLM_F_ACK;
259
260	status = sendmsg(rtnl->fd, &msg, 0);
261
262	if (status < 0) {
263		perror("Cannot talk to rtnetlink");
264		return -1;
265	}
266
267	memset(buf,0,sizeof(buf));
268
269	iov.iov_base = buf;
270
271	while (1) {
272		iov.iov_len = sizeof(buf);
273		status = recvmsg(rtnl->fd, &msg, 0);
274
275		if (status < 0) {
276			if (errno == EINTR)
277				continue;
278			perror("OVERRUN");
279			continue;
280		}
281		if (status == 0) {
282			fprintf(stderr, "EOF on netlink\n");
283			return -1;
284		}
285		if (msg.msg_namelen != sizeof(nladdr)) {
286			fprintf(stderr, "sender address length == %d\n", msg.msg_namelen);
287			exit(1);
288		}
289		for (h = (struct nlmsghdr*)buf; status >= sizeof(*h); ) {
290			int err;
291			int len = h->nlmsg_len;
292			int l = len - sizeof(*h);
293
294			if (l<0 || len>status) {
295				if (msg.msg_flags & MSG_TRUNC) {
296					fprintf(stderr, "Truncated message\n");
297					return -1;
298				}
299				fprintf(stderr, "!!!malformed message: len=%d\n", len);
300				exit(1);
301			}
302
303			if (nladdr.nl_pid != peer ||
304			    h->nlmsg_pid != rtnl->local.nl_pid ||
305			    h->nlmsg_seq != seq) {
306				if (junk) {
307					err = junk(&nladdr, h, jarg);
308					if (err < 0)
309						return err;
310				}
311				continue;
312			}
313
314			if (h->nlmsg_type == NLMSG_ERROR) {
315				struct nlmsgerr *err = (struct nlmsgerr*)NLMSG_DATA(h);
316				if (l < sizeof(struct nlmsgerr)) {
317					fprintf(stderr, "ERROR truncated\n");
318				} else {
319					errno = -err->error;
320					if (errno == 0) {
321						if (answer)
322							memcpy(answer, h, h->nlmsg_len);
323						return 0;
324					}
325					perror("RTNETLINK answers");
326				}
327				return -1;
328			}
329			if (answer) {
330				memcpy(answer, h, h->nlmsg_len);
331				return 0;
332			}
333
334			fprintf(stderr, "Unexpected reply!!!\n");
335
336			status -= NLMSG_ALIGN(len);
337			h = (struct nlmsghdr*)((char*)h + NLMSG_ALIGN(len));
338		}
339		if (msg.msg_flags & MSG_TRUNC) {
340			fprintf(stderr, "Message truncated\n");
341			continue;
342		}
343		if (status) {
344			fprintf(stderr, "!!!Remnant of size %d\n", status);
345			exit(1);
346		}
347	}
348}
349
350int rtnl_listen(struct rtnl_handle *rtnl,
351		rtnl_filter_t handler,
352		void *jarg)
353{
354	int status;
355	struct nlmsghdr *h;
356	struct sockaddr_nl nladdr;
357	struct iovec iov;
358	struct msghdr msg = {
359		.msg_name = &nladdr,
360		.msg_namelen = sizeof(nladdr),
361		.msg_iov = &iov,
362		.msg_iovlen = 1,
363	};
364	char   buf[8192];
365
366	memset(&nladdr, 0, sizeof(nladdr));
367	nladdr.nl_family = AF_NETLINK;
368	nladdr.nl_pid = 0;
369	nladdr.nl_groups = 0;
370
371	iov.iov_base = buf;
372	while (1) {
373		iov.iov_len = sizeof(buf);
374		status = recvmsg(rtnl->fd, &msg, 0);
375
376		if (status < 0) {
377			if (errno == EINTR)
378				continue;
379			perror("OVERRUN");
380			continue;
381		}
382		if (status == 0) {
383			fprintf(stderr, "EOF on netlink\n");
384			return -1;
385		}
386		if (msg.msg_namelen != sizeof(nladdr)) {
387			fprintf(stderr, "Sender address length == %d\n", msg.msg_namelen);
388			exit(1);
389		}
390		for (h = (struct nlmsghdr*)buf; status >= sizeof(*h); ) {
391			int err;
392			int len = h->nlmsg_len;
393			int l = len - sizeof(*h);
394
395			if (l<0 || len>status) {
396				if (msg.msg_flags & MSG_TRUNC) {
397					fprintf(stderr, "Truncated message\n");
398					return -1;
399				}
400				fprintf(stderr, "!!!malformed message: len=%d\n", len);
401				exit(1);
402			}
403
404			err = handler(&nladdr, h, jarg);
405			if (err < 0)
406				return err;
407
408			status -= NLMSG_ALIGN(len);
409			h = (struct nlmsghdr*)((char*)h + NLMSG_ALIGN(len));
410		}
411		if (msg.msg_flags & MSG_TRUNC) {
412			fprintf(stderr, "Message truncated\n");
413			continue;
414		}
415		if (status) {
416			fprintf(stderr, "!!!Remnant of size %d\n", status);
417			exit(1);
418		}
419	}
420}
421
422int rtnl_from_file(FILE *rtnl, rtnl_filter_t handler,
423		   void *jarg)
424{
425	int status;
426	struct sockaddr_nl nladdr;
427	char   buf[8192];
428	struct nlmsghdr *h = (void*)buf;
429
430	memset(&nladdr, 0, sizeof(nladdr));
431	nladdr.nl_family = AF_NETLINK;
432	nladdr.nl_pid = 0;
433	nladdr.nl_groups = 0;
434
435	while (1) {
436		int err, len, type;
437		int l;
438
439		status = fread(&buf, 1, sizeof(*h), rtnl);
440
441		if (status < 0) {
442			if (errno == EINTR)
443				continue;
444			perror("rtnl_from_file: fread");
445			return -1;
446		}
447		if (status == 0)
448			return 0;
449
450		len = h->nlmsg_len;
451		type= h->nlmsg_type;
452		l = len - sizeof(*h);
453
454		if (l<0 || len>sizeof(buf)) {
455			fprintf(stderr, "!!!malformed message: len=%d @%lu\n",
456				len, ftell(rtnl));
457			return -1;
458		}
459
460		status = fread(NLMSG_DATA(h), 1, NLMSG_ALIGN(l), rtnl);
461
462		if (status < 0) {
463			perror("rtnl_from_file: fread");
464			return -1;
465		}
466		if (status < l) {
467			fprintf(stderr, "rtnl-from_file: truncated message\n");
468			return -1;
469		}
470
471		err = handler(&nladdr, h, jarg);
472		if (err < 0)
473			return err;
474	}
475}
476
477int addattr32(struct nlmsghdr *n, int maxlen, int type, __u32 data)
478{
479	int len = RTA_LENGTH(4);
480	struct rtattr *rta;
481	if (NLMSG_ALIGN(n->nlmsg_len) + len > maxlen) {
482		fprintf(stderr,"addattr32: Error! max allowed bound %d exceeded\n",maxlen);
483		return -1;
484	}
485	rta = NLMSG_TAIL(n);
486	rta->rta_type = type;
487	rta->rta_len = len;
488	memcpy(RTA_DATA(rta), &data, 4);
489	n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + len;
490	return 0;
491}
492
493int addattr_l(struct nlmsghdr *n, int maxlen, int type, const void *data,
494	      int alen)
495{
496	int len = RTA_LENGTH(alen);
497	struct rtattr *rta;
498
499	if (NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len) > maxlen) {
500		fprintf(stderr, "addattr_l ERROR: message exceeded bound of %d\n",maxlen);
501		return -1;
502	}
503	rta = NLMSG_TAIL(n);
504	rta->rta_type = type;
505	rta->rta_len = len;
506	memcpy(RTA_DATA(rta), data, alen);
507	n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len);
508	return 0;
509}
510
511int addraw_l(struct nlmsghdr *n, int maxlen, const void *data, int len)
512{
513	if (NLMSG_ALIGN(n->nlmsg_len) + NLMSG_ALIGN(len) > maxlen) {
514		fprintf(stderr, "addraw_l ERROR: message exceeded bound of %d\n",maxlen);
515		return -1;
516	}
517
518	memcpy(NLMSG_TAIL(n), data, len);
519	memset((void *) NLMSG_TAIL(n) + len, 0, NLMSG_ALIGN(len) - len);
520	n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + NLMSG_ALIGN(len);
521	return 0;
522}
523
524int rta_addattr32(struct rtattr *rta, int maxlen, int type, __u32 data)
525{
526	int len = RTA_LENGTH(4);
527	struct rtattr *subrta;
528
529	if (RTA_ALIGN(rta->rta_len) + len > maxlen) {
530		fprintf(stderr,"rta_addattr32: Error! max allowed bound %d exceeded\n",maxlen);
531		return -1;
532	}
533	subrta = (struct rtattr*)(((char*)rta) + RTA_ALIGN(rta->rta_len));
534	subrta->rta_type = type;
535	subrta->rta_len = len;
536	memcpy(RTA_DATA(subrta), &data, 4);
537	rta->rta_len = NLMSG_ALIGN(rta->rta_len) + len;
538	return 0;
539}
540
541int rta_addattr_l(struct rtattr *rta, int maxlen, int type,
542		  const void *data, int alen)
543{
544	struct rtattr *subrta;
545	int len = RTA_LENGTH(alen);
546
547	if (RTA_ALIGN(rta->rta_len) + RTA_ALIGN(len) > maxlen) {
548		fprintf(stderr,"rta_addattr_l: Error! max allowed bound %d exceeded\n",maxlen);
549		return -1;
550	}
551	subrta = (struct rtattr*)(((char*)rta) + RTA_ALIGN(rta->rta_len));
552	subrta->rta_type = type;
553	subrta->rta_len = len;
554	memcpy(RTA_DATA(subrta), data, alen);
555	rta->rta_len = NLMSG_ALIGN(rta->rta_len) + RTA_ALIGN(len);
556	return 0;
557}
558
559int parse_rtattr(struct rtattr *tb[], int max, struct rtattr *rta, int len)
560{
561	memset(tb, 0, sizeof(struct rtattr *) * (max + 1));
562	while (RTA_OK(rta, len)) {
563		if (rta->rta_type <= max)
564			tb[rta->rta_type] = rta;
565		rta = RTA_NEXT(rta,len);
566	}
567	if (len)
568		fprintf(stderr, "!!!Deficit %d, rta_len=%d\n", len, rta->rta_len);
569	return 0;
570}
571
572int parse_rtattr_byindex(struct rtattr *tb[], int max, struct rtattr *rta, int len)
573{
574	int i = 0;
575
576	memset(tb, 0, sizeof(struct rtattr *) * max);
577	while (RTA_OK(rta, len)) {
578		if (rta->rta_type <= max && i < max)
579			tb[i++] = rta;
580		rta = RTA_NEXT(rta,len);
581	}
582	if (len)
583		fprintf(stderr, "!!!Deficit %d, rta_len=%d\n", len, rta->rta_len);
584	return i;
585}
586