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
 16static 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
 34static 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
 60static inline struct rtattr *rtattr_hdr(struct nlmsghdr *nh)
 61{
 62	return (struct rtattr *)((char *)(nh) + RTA_ALIGN((nh)->nlmsg_len));
 63}
 64
 65static 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
 85static 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
 96static 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
102static 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
109static 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
134static 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
184int 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
197static 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
229int 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
245static 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
286int 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
302int 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
308static 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
334int 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
348static 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
402int 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}