414 lines · c
1// SPDX-License-Identifier: GPL-2.02/* 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 204815 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}414