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