1 // SPDX-License-Identifier: GPL-2.0
2 /* Original from tools/testing/selftests/net/ipsec.c */
3 #include <linux/netlink.h>
4 #include <linux/random.h>
5 #include <linux/rtnetlink.h>
6 #include <linux/veth.h>
7 #include <net/if.h>
8 #include <stdint.h>
9 #include <string.h>
10 #include <sys/socket.h>
11
12 #include "aolib.h"
13
14 #define MAX_PAYLOAD 2048
15
16 static int netlink_sock(int *sock, uint32_t *seq_nr, int proto)
17 {
18 if (*sock > 0) {
19 seq_nr++;
20 return 0;
21 }
22
23 *sock = socket(AF_NETLINK, SOCK_RAW | SOCK_CLOEXEC, proto);
24 if (*sock < 0) {
25 test_print("socket(AF_NETLINK)");
26 return -1;
27 }
28
29 randomize_buffer(seq_nr, sizeof(*seq_nr));
30
31 return 0;
32 }
33
34 static int netlink_check_answer(int sock, bool quite)
35 {
36 struct nlmsgerror {
37 struct nlmsghdr hdr;
38 int error;
39 struct nlmsghdr orig_msg;
40 } answer;
41
42 if (recv(sock, &answer, sizeof(answer), 0) < 0) {
43 test_print("recv()");
44 return -1;
45 } else if (answer.hdr.nlmsg_type != NLMSG_ERROR) {
46 test_print("expected NLMSG_ERROR, got %d",
47 (int)answer.hdr.nlmsg_type);
48 return -1;
49 } else if (answer.error) {
50 if (!quite) {
51 test_print("NLMSG_ERROR: %d: %s",
52 answer.error, strerror(-answer.error));
53 }
54 return answer.error;
55 }
56
57 return 0;
58 }
59
60 static inline struct rtattr *rtattr_hdr(struct nlmsghdr *nh)
61 {
62 return (struct rtattr *)((char *)(nh) + RTA_ALIGN((nh)->nlmsg_len));
63 }
64
65 static int rtattr_pack(struct nlmsghdr *nh, size_t req_sz,
66 unsigned short rta_type, const void *payload, size_t size)
67 {
68 /* NLMSG_ALIGNTO == RTA_ALIGNTO, nlmsg_len already aligned */
69 struct rtattr *attr = rtattr_hdr(nh);
70 size_t nl_size = RTA_ALIGN(nh->nlmsg_len) + RTA_LENGTH(size);
71
72 if (req_sz < nl_size) {
73 test_print("req buf is too small: %zu < %zu", req_sz, nl_size);
74 return -1;
75 }
76 nh->nlmsg_len = nl_size;
77
78 attr->rta_len = RTA_LENGTH(size);
79 attr->rta_type = rta_type;
80 memcpy(RTA_DATA(attr), payload, size);
81
82 return 0;
83 }
84
85 static struct rtattr *_rtattr_begin(struct nlmsghdr *nh, size_t req_sz,
86 unsigned short rta_type, const void *payload, size_t size)
87 {
88 struct rtattr *ret = rtattr_hdr(nh);
89
90 if (rtattr_pack(nh, req_sz, rta_type, payload, size))
91 return 0;
92
93 return ret;
94 }
95
96 static inline struct rtattr *rtattr_begin(struct nlmsghdr *nh, size_t req_sz,
97 unsigned short rta_type)
98 {
99 return _rtattr_begin(nh, req_sz, rta_type, 0, 0);
100 }
101
102 static inline void rtattr_end(struct nlmsghdr *nh, struct rtattr *attr)
103 {
104 char *nlmsg_end = (char *)nh + nh->nlmsg_len;
105
106 attr->rta_len = nlmsg_end - (char *)attr;
107 }
108
109 static int veth_pack_peerb(struct nlmsghdr *nh, size_t req_sz,
110 const char *peer, int ns)
111 {
112 struct ifinfomsg pi;
113 struct rtattr *peer_attr;
114
115 memset(&pi, 0, sizeof(pi));
116 pi.ifi_family = AF_UNSPEC;
117 pi.ifi_change = 0xFFFFFFFF;
118
119 peer_attr = _rtattr_begin(nh, req_sz, VETH_INFO_PEER, &pi, sizeof(pi));
120 if (!peer_attr)
121 return -1;
122
123 if (rtattr_pack(nh, req_sz, IFLA_IFNAME, peer, strlen(peer)))
124 return -1;
125
126 if (rtattr_pack(nh, req_sz, IFLA_NET_NS_FD, &ns, sizeof(ns)))
127 return -1;
128
129 rtattr_end(nh, peer_attr);
130
131 return 0;
132 }
133
134 static int __add_veth(int sock, uint32_t seq, const char *name,
135 int ns_a, int ns_b)
136 {
137 uint16_t flags = NLM_F_REQUEST | NLM_F_ACK | NLM_F_EXCL | NLM_F_CREATE;
138 struct {
139 struct nlmsghdr nh;
140 struct ifinfomsg info;
141 char attrbuf[MAX_PAYLOAD];
142 } req;
143 static const char veth_type[] = "veth";
144 struct rtattr *link_info, *info_data;
145
146 memset(&req, 0, sizeof(req));
147 req.nh.nlmsg_len = NLMSG_LENGTH(sizeof(req.info));
148 req.nh.nlmsg_type = RTM_NEWLINK;
149 req.nh.nlmsg_flags = flags;
150 req.nh.nlmsg_seq = seq;
151 req.info.ifi_family = AF_UNSPEC;
152 req.info.ifi_change = 0xFFFFFFFF;
153
154 if (rtattr_pack(&req.nh, sizeof(req), IFLA_IFNAME, name, strlen(name)))
155 return -1;
156
157 if (rtattr_pack(&req.nh, sizeof(req), IFLA_NET_NS_FD, &ns_a, sizeof(ns_a)))
158 return -1;
159
160 link_info = rtattr_begin(&req.nh, sizeof(req), IFLA_LINKINFO);
161 if (!link_info)
162 return -1;
163
164 if (rtattr_pack(&req.nh, sizeof(req), IFLA_INFO_KIND, veth_type, sizeof(veth_type)))
165 return -1;
166
167 info_data = rtattr_begin(&req.nh, sizeof(req), IFLA_INFO_DATA);
168 if (!info_data)
169 return -1;
170
171 if (veth_pack_peerb(&req.nh, sizeof(req), name, ns_b))
172 return -1;
173
174 rtattr_end(&req.nh, info_data);
175 rtattr_end(&req.nh, link_info);
176
177 if (send(sock, &req, req.nh.nlmsg_len, 0) < 0) {
178 test_print("send()");
179 return -1;
180 }
181 return netlink_check_answer(sock, false);
182 }
183
184 int add_veth(const char *name, int nsfda, int nsfdb)
185 {
186 int route_sock = -1, ret;
187 uint32_t route_seq;
188
189 if (netlink_sock(&route_sock, &route_seq, NETLINK_ROUTE))
190 test_error("Failed to open netlink route socket\n");
191
192 ret = __add_veth(route_sock, route_seq++, name, nsfda, nsfdb);
193 close(route_sock);
194 return ret;
195 }
196
197 static int __ip_addr_add(int sock, uint32_t seq, const char *intf,
198 int family, union tcp_addr addr, uint8_t prefix)
199 {
200 uint16_t flags = NLM_F_REQUEST | NLM_F_ACK | NLM_F_EXCL | NLM_F_CREATE;
201 struct {
202 struct nlmsghdr nh;
203 struct ifaddrmsg info;
204 char attrbuf[MAX_PAYLOAD];
205 } req;
206 size_t addr_len = (family == AF_INET) ? sizeof(struct in_addr) :
207 sizeof(struct in6_addr);
208
209 memset(&req, 0, sizeof(req));
210 req.nh.nlmsg_len = NLMSG_LENGTH(sizeof(req.info));
211 req.nh.nlmsg_type = RTM_NEWADDR;
212 req.nh.nlmsg_flags = flags;
213 req.nh.nlmsg_seq = seq;
214 req.info.ifa_family = family;
215 req.info.ifa_prefixlen = prefix;
216 req.info.ifa_index = if_nametoindex(intf);
217 req.info.ifa_flags = IFA_F_NODAD;
218
219 if (rtattr_pack(&req.nh, sizeof(req), IFA_LOCAL, &addr, addr_len))
220 return -1;
221
222 if (send(sock, &req, req.nh.nlmsg_len, 0) < 0) {
223 test_print("send()");
224 return -1;
225 }
226 return netlink_check_answer(sock, true);
227 }
228
229 int ip_addr_add(const char *intf, int family,
230 union tcp_addr addr, uint8_t prefix)
231 {
232 int route_sock = -1, ret;
233 uint32_t route_seq;
234
235 if (netlink_sock(&route_sock, &route_seq, NETLINK_ROUTE))
236 test_error("Failed to open netlink route socket\n");
237
238 ret = __ip_addr_add(route_sock, route_seq++, intf,
239 family, addr, prefix);
240
241 close(route_sock);
242 return ret;
243 }
244
245 static int __ip_route_add(int sock, uint32_t seq, const char *intf, int family,
246 union tcp_addr src, union tcp_addr dst, uint8_t vrf)
247 {
248 struct {
249 struct nlmsghdr nh;
250 struct rtmsg rt;
251 char attrbuf[MAX_PAYLOAD];
252 } req;
253 unsigned int index = if_nametoindex(intf);
254 size_t addr_len = (family == AF_INET) ? sizeof(struct in_addr) :
255 sizeof(struct in6_addr);
256
257 memset(&req, 0, sizeof(req));
258 req.nh.nlmsg_len = NLMSG_LENGTH(sizeof(req.rt));
259 req.nh.nlmsg_type = RTM_NEWROUTE;
260 req.nh.nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK | NLM_F_CREATE;
261 req.nh.nlmsg_seq = seq;
262 req.rt.rtm_family = family;
263 req.rt.rtm_dst_len = (family == AF_INET) ? 32 : 128;
264 req.rt.rtm_table = vrf;
265 req.rt.rtm_protocol = RTPROT_BOOT;
266 req.rt.rtm_scope = RT_SCOPE_UNIVERSE;
267 req.rt.rtm_type = RTN_UNICAST;
268
269 if (rtattr_pack(&req.nh, sizeof(req), RTA_DST, &dst, addr_len))
270 return -1;
271
272 if (rtattr_pack(&req.nh, sizeof(req), RTA_PREFSRC, &src, addr_len))
273 return -1;
274
275 if (rtattr_pack(&req.nh, sizeof(req), RTA_OIF, &index, sizeof(index)))
276 return -1;
277
278 if (send(sock, &req, req.nh.nlmsg_len, 0) < 0) {
279 test_print("send()");
280 return -1;
281 }
282
283 return netlink_check_answer(sock, true);
284 }
285
286 int ip_route_add_vrf(const char *intf, int family,
287 union tcp_addr src, union tcp_addr dst, uint8_t vrf)
288 {
289 int route_sock = -1, ret;
290 uint32_t route_seq;
291
292 if (netlink_sock(&route_sock, &route_seq, NETLINK_ROUTE))
293 test_error("Failed to open netlink route socket\n");
294
295 ret = __ip_route_add(route_sock, route_seq++, intf,
296 family, src, dst, vrf);
297
298 close(route_sock);
299 return ret;
300 }
301
302 int ip_route_add(const char *intf, int family,
303 union tcp_addr src, union tcp_addr dst)
304 {
305 return ip_route_add_vrf(intf, family, src, dst, RT_TABLE_MAIN);
306 }
307
308 static int __link_set_up(int sock, uint32_t seq, const char *intf)
309 {
310 struct {
311 struct nlmsghdr nh;
312 struct ifinfomsg info;
313 char attrbuf[MAX_PAYLOAD];
314 } req;
315
316 memset(&req, 0, sizeof(req));
317 req.nh.nlmsg_len = NLMSG_LENGTH(sizeof(req.info));
318 req.nh.nlmsg_type = RTM_NEWLINK;
319 req.nh.nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK;
320 req.nh.nlmsg_seq = seq;
321 req.info.ifi_family = AF_UNSPEC;
322 req.info.ifi_change = 0xFFFFFFFF;
323 req.info.ifi_index = if_nametoindex(intf);
324 req.info.ifi_flags = IFF_UP;
325 req.info.ifi_change = IFF_UP;
326
327 if (send(sock, &req, req.nh.nlmsg_len, 0) < 0) {
328 test_print("send()");
329 return -1;
330 }
331 return netlink_check_answer(sock, false);
332 }
333
334 int link_set_up(const char *intf)
335 {
336 int route_sock = -1, ret;
337 uint32_t route_seq;
338
339 if (netlink_sock(&route_sock, &route_seq, NETLINK_ROUTE))
340 test_error("Failed to open netlink route socket\n");
341
342 ret = __link_set_up(route_sock, route_seq++, intf);
343
344 close(route_sock);
345 return ret;
346 }
347
348 static int __add_vrf(int sock, uint32_t seq, const char *name,
349 uint32_t tabid, int ifindex, int nsfd)
350 {
351 uint16_t flags = NLM_F_REQUEST | NLM_F_ACK | NLM_F_EXCL | NLM_F_CREATE;
352 struct {
353 struct nlmsghdr nh;
354 struct ifinfomsg info;
355 char attrbuf[MAX_PAYLOAD];
356 } req;
357 static const char vrf_type[] = "vrf";
358 struct rtattr *link_info, *info_data;
359
360 memset(&req, 0, sizeof(req));
361 req.nh.nlmsg_len = NLMSG_LENGTH(sizeof(req.info));
362 req.nh.nlmsg_type = RTM_NEWLINK;
363 req.nh.nlmsg_flags = flags;
364 req.nh.nlmsg_seq = seq;
365 req.info.ifi_family = AF_UNSPEC;
366 req.info.ifi_change = 0xFFFFFFFF;
367 req.info.ifi_index = ifindex;
368
369 if (rtattr_pack(&req.nh, sizeof(req), IFLA_IFNAME, name, strlen(name)))
370 return -1;
371
372 if (nsfd >= 0)
373 if (rtattr_pack(&req.nh, sizeof(req), IFLA_NET_NS_FD,
374 &nsfd, sizeof(nsfd)))
375 return -1;
376
377 link_info = rtattr_begin(&req.nh, sizeof(req), IFLA_LINKINFO);
378 if (!link_info)
379 return -1;
380
381 if (rtattr_pack(&req.nh, sizeof(req), IFLA_INFO_KIND, vrf_type, sizeof(vrf_type)))
382 return -1;
383
384 info_data = rtattr_begin(&req.nh, sizeof(req), IFLA_INFO_DATA);
385 if (!info_data)
386 return -1;
387
388 if (rtattr_pack(&req.nh, sizeof(req), IFLA_VRF_TABLE,
389 &tabid, sizeof(tabid)))
390 return -1;
391
392 rtattr_end(&req.nh, info_data);
393 rtattr_end(&req.nh, link_info);
394
395 if (send(sock, &req, req.nh.nlmsg_len, 0) < 0) {
396 test_print("send()");
397 return -1;
398 }
399 return netlink_check_answer(sock, true);
400 }
401
402 int add_vrf(const char *name, uint32_t tabid, int ifindex, int nsfd)
403 {
404 int route_sock = -1, ret;
405 uint32_t route_seq;
406
407 if (netlink_sock(&route_sock, &route_seq, NETLINK_ROUTE))
408 test_error("Failed to open netlink route socket\n");
409
410 ret = __add_vrf(route_sock, route_seq++, name, tabid, ifindex, nsfd);
411 close(route_sock);
412 return ret;
413 }
414
Linux® is a registered trademark of Linus Torvalds in the United States and other countries.
TOMOYO® is a registered trademark of NTT DATA CORPORATION.