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_send(struct rtnl_handle * rth, const char * buf, int len)
34{
35	struct sockaddr_nl nladdr;
36
37	memset(&nladdr, 0, sizeof(nladdr));
38	nladdr.nl_family = AF_NETLINK;
39
40	return sendto(rth->fd, buf, len, 0, (struct sockaddr*)&nladdr, sizeof(nladdr));
41}
42
43int rtnl_open(struct rtnl_handle * rth, unsigned subscriptions)
44{
45	int addr_len;
46
47	memset(rth, 0, sizeof(rth));
48
49	rth->fd = socket(AF_NETLINK, SOCK_RAW, NETLINK_ROUTE);
50
51	if (rth->fd < 0)
52		return -1;
53
54	memset(&rth->local, 0, sizeof(rth->local));
55	rth->local.nl_family = AF_NETLINK;
56	rth->local.nl_groups = subscriptions;
57
58	if (bind(rth->fd, (struct sockaddr*)&rth->local, sizeof(rth->local)) < 0)
59		return -1;
60
61	addr_len = sizeof(rth->local);
62	if (getsockname(rth->fd, (struct sockaddr*)&rth->local, &addr_len) < 0)
63		return -1;
64
65	if (addr_len != sizeof(rth->local))
66		return -1;
67
68	if (rth->local.nl_family != AF_NETLINK)
69		return -1;
70
71	rth->seq = time(NULL);
72
73	return 0;
74}
75
76int rtnl_request(struct rtnl_handle * rth, int family, int type)
77{
78	struct {
79		struct nlmsghdr nlh;
80		struct rtgenmsg g;
81	} req;
82	struct sockaddr_nl nladdr;
83
84	memset(&nladdr, 0, sizeof(nladdr));
85	nladdr.nl_family = AF_NETLINK;
86
87	memset(&req, 0, sizeof(req));
88	req.nlh.nlmsg_len = sizeof(req);
89	req.nlh.nlmsg_type = type;
90	req.nlh.nlmsg_flags = NLM_F_ROOT | NLM_F_MATCH |NLM_F_REQUEST;
91	req.nlh.nlmsg_pid = 0;
92	req.nlh.nlmsg_seq = rth->dump = ++rth->seq;
93	req.g.rtgen_family = family;
94
95	return sendto(rth->fd, (void*)&req, sizeof(req), 0, (struct sockaddr*)&nladdr, sizeof(nladdr));
96}
97
98int rtnl_filter(struct rtnl_handle * rth, rtnl_filter_t filter, void * arg)
99{
100	struct sockaddr_nl nladdr;
101	struct iovec iov;
102	struct msghdr msg = {
103		.msg_name = &nladdr,
104		.msg_namelen = sizeof(nladdr),
105		.msg_iov = &iov,
106		.msg_iovlen = 1,
107	};
108
109	char buf[8192];
110
111	iov.iov_base = buf;
112
113	while (1) {
114		int status;
115		struct nlmsghdr *h;
116
117		iov.iov_len = sizeof(buf);
118		status = recvmsg(rth->fd, &msg, 0);
119
120		if (status < 0)
121			continue;
122
123		if (status == 0)
124			return -1;
125
126		h = (struct nlmsghdr*)buf;
127		while (NLMSG_OK(h, status)) {
128			int err;
129
130			if (nladdr.nl_pid != 0 || h->nlmsg_pid != rth->local.nl_pid || h->nlmsg_seq != rth->dump)
131				goto skip_it;
132
133			if (h->nlmsg_type == NLMSG_DONE)
134				return 0;
135
136			if (h->nlmsg_type == NLMSG_ERROR)
137				return -1;
138
139			err = filter(&nladdr, h, arg);
140
141			if (err < 0)
142				return err;
143
144skip_it:
145			h = NLMSG_NEXT(h, status);
146		}
147
148		if (msg.msg_flags & MSG_TRUNC)
149			continue;
150
151		if (status)
152			return -1;
153	}
154}
155
156int rtnl_talk(struct rtnl_handle *rtnl, struct nlmsghdr *n, pid_t peer, unsigned groups, struct nlmsghdr *answer)
157{
158	int status;
159	unsigned seq;
160	struct nlmsghdr *h;
161	struct sockaddr_nl nladdr;
162	struct iovec iov = {
163		.iov_base = (void*) n,
164		.iov_len = n->nlmsg_len
165	};
166	struct msghdr msg = {
167		.msg_name = &nladdr,
168		.msg_namelen = sizeof(nladdr),
169		.msg_iov = &iov,
170		.msg_iovlen = 1,
171	};
172
173	char   buf[8192];
174
175	memset(&nladdr, 0, sizeof(nladdr));
176	nladdr.nl_family = AF_NETLINK;
177	nladdr.nl_pid = peer;
178	nladdr.nl_groups = groups;
179
180	n->nlmsg_seq = seq = ++rtnl->seq;
181
182	if (answer == NULL)
183		n->nlmsg_flags |= NLM_F_ACK;
184
185	status = sendmsg(rtnl->fd, &msg, 0);
186
187	if (status < 0) {
188		perror("Can't talk to rtnetlink");
189		return -1;
190	}
191
192	memset(buf,0,sizeof(buf));
193
194	iov.iov_base = buf;
195
196	while (1) {
197		iov.iov_len = sizeof(buf);
198		status = recvmsg(rtnl->fd, &msg, 0);
199
200		if (status < 0)
201			continue;
202
203		if (status == 0)
204			return -1;
205
206		if (msg.msg_namelen != sizeof(nladdr))
207			return -1;
208
209		for (h = (struct nlmsghdr*)buf; status >= sizeof(*h); ) {
210			int len = h->nlmsg_len;
211			int l = len - sizeof(*h);
212
213			if (l<0 || len>status)
214				return -1;
215
216			if (nladdr.nl_pid != peer || h->nlmsg_pid != rtnl->local.nl_pid || h->nlmsg_seq != seq)
217				continue;
218
219			if (h->nlmsg_type == NLMSG_ERROR)
220				return -1;
221
222			if (answer) {
223				memcpy(answer, h, h->nlmsg_len);
224				return 0;
225			}
226
227			fprintf(stderr, "Unexpected reply!!!\n");
228
229			status -= NLMSG_ALIGN(len);
230			h = (struct nlmsghdr*)((char*)h + NLMSG_ALIGN(len));
231		}
232
233		if (msg.msg_flags & MSG_TRUNC)
234			continue;
235
236		if (status)
237			return -1;
238	}
239}
240
241int addattr32(struct nlmsghdr *n, int maxlen, int type, unsigned int data)
242{
243	int len = RTA_LENGTH(4);
244	struct rtattr *rta;
245
246	if (NLMSG_ALIGN(n->nlmsg_len) + len > maxlen)
247		return -1;
248
249	rta = NLMSG_TAIL(n);
250	rta->rta_type = type;
251	rta->rta_len = len;
252
253	memcpy(RTA_DATA(rta), &data, 4);
254	n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + len;
255
256	return 0;
257}
258
259int addattr_l(struct nlmsghdr *n, int maxlen, int type, const void *data, int alen)
260{
261	int len = RTA_LENGTH(alen);
262	struct rtattr *rta;
263
264	if (NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len) > maxlen)
265		return -1;
266
267	rta = NLMSG_TAIL(n);
268	rta->rta_type = type;
269	rta->rta_len = len;
270
271	memcpy(RTA_DATA(rta), data, alen);
272
273	n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len);
274
275	return 0;
276}
277
278int parse_rtattr(struct rtattr * tb[], int max, struct rtattr * rta, int len)
279{
280	memset(tb, 0, sizeof(struct rtattr *) * (max + 1));
281
282	while (RTA_OK(rta, len)) {
283		if (rta->rta_type <= max)
284			tb[rta->rta_type] = rta;
285
286		rta = RTA_NEXT(rta,len);
287	}
288
289	return 0;
290}
291
292