1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
|
#include <assert.h>
#include <stdio.h>
#include <string.h>
#include <errno.h>
#include <unistd.h>
#include "nlrequest.h"
int netlink_open(void) {
struct sockaddr_nl addr;
int s;
if ((s = socket(PF_NETLINK, SOCK_DGRAM, NETLINK_ROUTE)) < 0) {
fprintf(stderr, "socket(PF_NETLINK): %s\n", strerror(errno));
return -1;
}
memset(&addr, 0, sizeof(addr));
addr.nl_family = AF_NETLINK;
addr.nl_groups = 0;
addr.nl_pid = getpid();
if (bind(s, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
fprintf(stderr, "bind(): %s\n", strerror(errno));
return -1;
}
return s;
}
int netlink_request(int s, struct nlmsghdr *n, int (*callback) (struct nlmsghdr *n, void *u), void *u) {
static int seq = 0;
pid_t pid = getpid();
assert(s >= 0 && n);
n->nlmsg_seq = seq++;
n->nlmsg_flags |= NLM_F_ACK;
if (send(s, n, n->nlmsg_len, 0) < 0) {
fprintf(stderr, "NETLINK: send(): %s\n", strerror(errno));
return -1;
}
for (;;) {
int bytes;
char replybuf[4096];
struct nlmsghdr *p = (struct nlmsghdr *) replybuf;
if ((bytes = recv(s, &replybuf, sizeof(replybuf), 0)) < 0) {
fprintf(stderr, "recv(): %s\n", strerror(errno));
return -1;
}
for (; bytes > 0; p = NLMSG_NEXT(p, bytes)) {
int ret;
if (!NLMSG_OK(p, bytes) || bytes < sizeof(struct nlmsghdr) || bytes < p->nlmsg_len) {
fprintf(stderr, "NETLINK: Packet too small or truncated! %u!=%u!=%u\n", bytes, sizeof(struct nlmsghdr), p->nlmsg_len);
return -1;
}
if (p->nlmsg_type == NLMSG_DONE)
return 0;
if (p->nlmsg_type == NLMSG_ERROR) {
struct nlmsgerr *e = (struct nlmsgerr *) NLMSG_DATA (p);
if (e->error) {
fprintf(stderr, "NETLINK: Error: %s\n", strerror(-e->error));
return -1;
} else
return 0;
}
if (p->nlmsg_pid != pid)
continue;
if (p->nlmsg_seq != n->nlmsg_seq) {
fprintf(stderr, "NETLINK: Received message with bogus sequence number!\n");
continue;
}
if (callback)
if ((ret = callback(p, u)) < 0)
return ret;
}
}
return 0;
}
/*
* Utility function comes from iproute2.
* Author: Alexey Kuznetsov, <kuznet@ms2.inr.ac.ru>
*/
int addattr_l(struct nlmsghdr *n, int maxlen, int type, void *data, int alen) {
int len;
struct rtattr *rta;
len = RTA_LENGTH(alen);
if (NLMSG_ALIGN(n->nlmsg_len) + len > maxlen)
return -1;
rta = (struct rtattr*) (((char*)n) + NLMSG_ALIGN (n->nlmsg_len));
rta->rta_type = type;
rta->rta_len = len;
memcpy (RTA_DATA(rta), data, alen);
n->nlmsg_len = NLMSG_ALIGN (n->nlmsg_len) + len;
return 0;
}
/*
* Utility function originated from iproute2.
* Author: Alexey Kuznetsov, <kuznet@ms2.inr.ac.ru>
*/
int addattr32(struct nlmsghdr *n, int maxlen, int type, int data) {
int len;
struct rtattr *rta;
len = RTA_LENGTH(4);
if (NLMSG_ALIGN (n->nlmsg_len) + len > maxlen)
return -1;
rta = (struct rtattr*) (((char*)n) + NLMSG_ALIGN (n->nlmsg_len));
rta->rta_type = type;
rta->rta_len = len;
memcpy (RTA_DATA(rta), &data, 4);
n->nlmsg_len = NLMSG_ALIGN (n->nlmsg_len) + len;
return 0;
}
|
