1 | // SPDX-License-Identifier: GPL-2.0-or-later |
2 | /* Taken & modified from iproute2's libnetlink.c |
3 | * Authors: Alexey Kuznetsov, <kuznet@ms2.inr.ac.ru> |
4 | */ |
5 | #include <stdio.h> |
6 | #include <stdlib.h> |
7 | #include <unistd.h> |
8 | #include <errno.h> |
9 | #include <time.h> |
10 | #include <sys/socket.h> |
11 | |
12 | #include "netlink_helpers.h" |
13 | |
14 | static int rcvbuf = 1024 * 1024; |
15 | |
16 | void rtnl_close(struct rtnl_handle *rth) |
17 | { |
18 | if (rth->fd >= 0) { |
19 | close(rth->fd); |
20 | rth->fd = -1; |
21 | } |
22 | } |
23 | |
24 | int rtnl_open_byproto(struct rtnl_handle *rth, unsigned int subscriptions, |
25 | int protocol) |
26 | { |
27 | socklen_t addr_len; |
28 | int sndbuf = 32768; |
29 | int one = 1; |
30 | |
31 | memset(rth, 0, sizeof(*rth)); |
32 | rth->proto = protocol; |
33 | rth->fd = socket(AF_NETLINK, SOCK_RAW | SOCK_CLOEXEC, protocol); |
34 | if (rth->fd < 0) { |
35 | perror("Cannot open netlink socket" ); |
36 | return -1; |
37 | } |
38 | if (setsockopt(rth->fd, SOL_SOCKET, SO_SNDBUF, |
39 | &sndbuf, sizeof(sndbuf)) < 0) { |
40 | perror("SO_SNDBUF" ); |
41 | goto err; |
42 | } |
43 | if (setsockopt(rth->fd, SOL_SOCKET, SO_RCVBUF, |
44 | &rcvbuf, sizeof(rcvbuf)) < 0) { |
45 | perror("SO_RCVBUF" ); |
46 | goto err; |
47 | } |
48 | |
49 | /* Older kernels may no support extended ACK reporting */ |
50 | setsockopt(rth->fd, SOL_NETLINK, NETLINK_EXT_ACK, |
51 | &one, sizeof(one)); |
52 | |
53 | memset(&rth->local, 0, sizeof(rth->local)); |
54 | rth->local.nl_family = AF_NETLINK; |
55 | rth->local.nl_groups = subscriptions; |
56 | |
57 | if (bind(rth->fd, (struct sockaddr *)&rth->local, |
58 | sizeof(rth->local)) < 0) { |
59 | perror("Cannot bind netlink socket" ); |
60 | goto err; |
61 | } |
62 | addr_len = sizeof(rth->local); |
63 | if (getsockname(rth->fd, (struct sockaddr *)&rth->local, |
64 | &addr_len) < 0) { |
65 | perror("Cannot getsockname" ); |
66 | goto err; |
67 | } |
68 | if (addr_len != sizeof(rth->local)) { |
69 | fprintf(stderr, "Wrong address length %d\n" , addr_len); |
70 | goto err; |
71 | } |
72 | if (rth->local.nl_family != AF_NETLINK) { |
73 | fprintf(stderr, "Wrong address family %d\n" , |
74 | rth->local.nl_family); |
75 | goto err; |
76 | } |
77 | rth->seq = time(NULL); |
78 | return 0; |
79 | err: |
80 | rtnl_close(rth); |
81 | return -1; |
82 | } |
83 | |
84 | int rtnl_open(struct rtnl_handle *rth, unsigned int subscriptions) |
85 | { |
86 | return rtnl_open_byproto(rth, subscriptions, NETLINK_ROUTE); |
87 | } |
88 | |
89 | static int __rtnl_recvmsg(int fd, struct msghdr *msg, int flags) |
90 | { |
91 | int len; |
92 | |
93 | do { |
94 | len = recvmsg(fd, msg, flags); |
95 | } while (len < 0 && (errno == EINTR || errno == EAGAIN)); |
96 | if (len < 0) { |
97 | fprintf(stderr, "netlink receive error %s (%d)\n" , |
98 | strerror(errno), errno); |
99 | return -errno; |
100 | } |
101 | if (len == 0) { |
102 | fprintf(stderr, "EOF on netlink\n" ); |
103 | return -ENODATA; |
104 | } |
105 | return len; |
106 | } |
107 | |
108 | static int rtnl_recvmsg(int fd, struct msghdr *msg, char **answer) |
109 | { |
110 | struct iovec *iov = msg->msg_iov; |
111 | char *buf; |
112 | int len; |
113 | |
114 | iov->iov_base = NULL; |
115 | iov->iov_len = 0; |
116 | |
117 | len = __rtnl_recvmsg(fd, msg, MSG_PEEK | MSG_TRUNC); |
118 | if (len < 0) |
119 | return len; |
120 | if (len < 32768) |
121 | len = 32768; |
122 | buf = malloc(len); |
123 | if (!buf) { |
124 | fprintf(stderr, "malloc error: not enough buffer\n" ); |
125 | return -ENOMEM; |
126 | } |
127 | iov->iov_base = buf; |
128 | iov->iov_len = len; |
129 | len = __rtnl_recvmsg(fd, msg, flags: 0); |
130 | if (len < 0) { |
131 | free(buf); |
132 | return len; |
133 | } |
134 | if (answer) |
135 | *answer = buf; |
136 | else |
137 | free(buf); |
138 | return len; |
139 | } |
140 | |
141 | static void rtnl_talk_error(struct nlmsghdr *h, struct nlmsgerr *err, |
142 | nl_ext_ack_fn_t errfn) |
143 | { |
144 | fprintf(stderr, "RTNETLINK answers: %s\n" , |
145 | strerror(-err->error)); |
146 | } |
147 | |
148 | static int __rtnl_talk_iov(struct rtnl_handle *rtnl, struct iovec *iov, |
149 | size_t iovlen, struct nlmsghdr **answer, |
150 | bool show_rtnl_err, nl_ext_ack_fn_t errfn) |
151 | { |
152 | struct sockaddr_nl nladdr = { .nl_family = AF_NETLINK }; |
153 | struct iovec riov; |
154 | struct msghdr msg = { |
155 | .msg_name = &nladdr, |
156 | .msg_namelen = sizeof(nladdr), |
157 | .msg_iov = iov, |
158 | .msg_iovlen = iovlen, |
159 | }; |
160 | unsigned int seq = 0; |
161 | struct nlmsghdr *h; |
162 | int i, status; |
163 | char *buf; |
164 | |
165 | for (i = 0; i < iovlen; i++) { |
166 | h = iov[i].iov_base; |
167 | h->nlmsg_seq = seq = ++rtnl->seq; |
168 | if (answer == NULL) |
169 | h->nlmsg_flags |= NLM_F_ACK; |
170 | } |
171 | status = sendmsg(rtnl->fd, &msg, 0); |
172 | if (status < 0) { |
173 | perror("Cannot talk to rtnetlink" ); |
174 | return -1; |
175 | } |
176 | /* change msg to use the response iov */ |
177 | msg.msg_iov = &riov; |
178 | msg.msg_iovlen = 1; |
179 | i = 0; |
180 | while (1) { |
181 | next: |
182 | status = rtnl_recvmsg(fd: rtnl->fd, msg: &msg, answer: &buf); |
183 | ++i; |
184 | if (status < 0) |
185 | return status; |
186 | if (msg.msg_namelen != sizeof(nladdr)) { |
187 | fprintf(stderr, |
188 | "Sender address length == %d!\n" , |
189 | msg.msg_namelen); |
190 | exit(1); |
191 | } |
192 | for (h = (struct nlmsghdr *)buf; status >= sizeof(*h); ) { |
193 | int len = h->nlmsg_len; |
194 | int l = len - sizeof(*h); |
195 | |
196 | if (l < 0 || len > status) { |
197 | if (msg.msg_flags & MSG_TRUNC) { |
198 | fprintf(stderr, "Truncated message!\n" ); |
199 | free(buf); |
200 | return -1; |
201 | } |
202 | fprintf(stderr, |
203 | "Malformed message: len=%d!\n" , |
204 | len); |
205 | exit(1); |
206 | } |
207 | if (nladdr.nl_pid != 0 || |
208 | h->nlmsg_pid != rtnl->local.nl_pid || |
209 | h->nlmsg_seq > seq || h->nlmsg_seq < seq - iovlen) { |
210 | /* Don't forget to skip that message. */ |
211 | status -= NLMSG_ALIGN(len); |
212 | h = (struct nlmsghdr *)((char *)h + NLMSG_ALIGN(len)); |
213 | continue; |
214 | } |
215 | if (h->nlmsg_type == NLMSG_ERROR) { |
216 | struct nlmsgerr *err = (struct nlmsgerr *)NLMSG_DATA(h); |
217 | int error = err->error; |
218 | |
219 | if (l < sizeof(struct nlmsgerr)) { |
220 | fprintf(stderr, "ERROR truncated\n" ); |
221 | free(buf); |
222 | return -1; |
223 | } |
224 | if (error) { |
225 | errno = -error; |
226 | if (rtnl->proto != NETLINK_SOCK_DIAG && |
227 | show_rtnl_err) |
228 | rtnl_talk_error(h, err, errfn); |
229 | } |
230 | if (i < iovlen) { |
231 | free(buf); |
232 | goto next; |
233 | } |
234 | if (error) { |
235 | free(buf); |
236 | return -i; |
237 | } |
238 | if (answer) |
239 | *answer = (struct nlmsghdr *)buf; |
240 | else |
241 | free(buf); |
242 | return 0; |
243 | } |
244 | if (answer) { |
245 | *answer = (struct nlmsghdr *)buf; |
246 | return 0; |
247 | } |
248 | fprintf(stderr, "Unexpected reply!\n" ); |
249 | status -= NLMSG_ALIGN(len); |
250 | h = (struct nlmsghdr *)((char *)h + NLMSG_ALIGN(len)); |
251 | } |
252 | free(buf); |
253 | if (msg.msg_flags & MSG_TRUNC) { |
254 | fprintf(stderr, "Message truncated!\n" ); |
255 | continue; |
256 | } |
257 | if (status) { |
258 | fprintf(stderr, "Remnant of size %d!\n" , status); |
259 | exit(1); |
260 | } |
261 | } |
262 | } |
263 | |
264 | static int __rtnl_talk(struct rtnl_handle *rtnl, struct nlmsghdr *n, |
265 | struct nlmsghdr **answer, bool show_rtnl_err, |
266 | nl_ext_ack_fn_t errfn) |
267 | { |
268 | struct iovec iov = { |
269 | .iov_base = n, |
270 | .iov_len = n->nlmsg_len, |
271 | }; |
272 | |
273 | return __rtnl_talk_iov(rtnl, iov: &iov, iovlen: 1, answer, show_rtnl_err, errfn); |
274 | } |
275 | |
276 | int rtnl_talk(struct rtnl_handle *rtnl, struct nlmsghdr *n, |
277 | struct nlmsghdr **answer) |
278 | { |
279 | return __rtnl_talk(rtnl, n, answer, show_rtnl_err: true, NULL); |
280 | } |
281 | |
282 | int addattr(struct nlmsghdr *n, int maxlen, int type) |
283 | { |
284 | return addattr_l(n, maxlen, type, NULL, alen: 0); |
285 | } |
286 | |
287 | int addattr8(struct nlmsghdr *n, int maxlen, int type, __u8 data) |
288 | { |
289 | return addattr_l(n, maxlen, type, data: &data, alen: sizeof(__u8)); |
290 | } |
291 | |
292 | int addattr16(struct nlmsghdr *n, int maxlen, int type, __u16 data) |
293 | { |
294 | return addattr_l(n, maxlen, type, data: &data, alen: sizeof(__u16)); |
295 | } |
296 | |
297 | int addattr32(struct nlmsghdr *n, int maxlen, int type, __u32 data) |
298 | { |
299 | return addattr_l(n, maxlen, type, data: &data, alen: sizeof(__u32)); |
300 | } |
301 | |
302 | int addattr64(struct nlmsghdr *n, int maxlen, int type, __u64 data) |
303 | { |
304 | return addattr_l(n, maxlen, type, data: &data, alen: sizeof(__u64)); |
305 | } |
306 | |
307 | int addattrstrz(struct nlmsghdr *n, int maxlen, int type, const char *str) |
308 | { |
309 | return addattr_l(n, maxlen, type, data: str, strlen(str)+1); |
310 | } |
311 | |
312 | int addattr_l(struct nlmsghdr *n, int maxlen, int type, const void *data, |
313 | int alen) |
314 | { |
315 | int len = RTA_LENGTH(alen); |
316 | struct rtattr *rta; |
317 | |
318 | if (NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len) > maxlen) { |
319 | fprintf(stderr, "%s: Message exceeded bound of %d\n" , |
320 | __func__, maxlen); |
321 | return -1; |
322 | } |
323 | rta = NLMSG_TAIL(n); |
324 | rta->rta_type = type; |
325 | rta->rta_len = len; |
326 | if (alen) |
327 | memcpy(RTA_DATA(rta), data, alen); |
328 | n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len); |
329 | return 0; |
330 | } |
331 | |
332 | int addraw_l(struct nlmsghdr *n, int maxlen, const void *data, int len) |
333 | { |
334 | if (NLMSG_ALIGN(n->nlmsg_len) + NLMSG_ALIGN(len) > maxlen) { |
335 | fprintf(stderr, "%s: Message exceeded bound of %d\n" , |
336 | __func__, maxlen); |
337 | return -1; |
338 | } |
339 | |
340 | memcpy(NLMSG_TAIL(n), data, len); |
341 | memset((void *) NLMSG_TAIL(n) + len, 0, NLMSG_ALIGN(len) - len); |
342 | n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + NLMSG_ALIGN(len); |
343 | return 0; |
344 | } |
345 | |
346 | struct rtattr *addattr_nest(struct nlmsghdr *n, int maxlen, int type) |
347 | { |
348 | struct rtattr *nest = NLMSG_TAIL(n); |
349 | |
350 | addattr_l(n, maxlen, type, NULL, alen: 0); |
351 | return nest; |
352 | } |
353 | |
354 | int addattr_nest_end(struct nlmsghdr *n, struct rtattr *nest) |
355 | { |
356 | nest->rta_len = (void *)NLMSG_TAIL(n) - (void *)nest; |
357 | return n->nlmsg_len; |
358 | } |
359 | |