| // SPDX-License-Identifier: GPL-2.0 | |
| /* Original from tools/testing/selftests/net/ipsec.c */ | |
| #include <linux/netlink.h> | |
| #include <linux/random.h> | |
| #include <linux/rtnetlink.h> | |
| #include <linux/veth.h> | |
| #include <net/if.h> | |
| #include <stdint.h> | |
| #include <string.h> | |
| #include <sys/socket.h> | |
| #include "aolib.h" | |
| #define MAX_PAYLOAD 2048 | |
| static int netlink_sock(int *sock, uint32_t *seq_nr, int proto) | |
| { | |
| if (*sock > 0) { | |
| seq_nr++; | |
| return 0; | |
| } | |
| *sock = socket(AF_NETLINK, SOCK_RAW | SOCK_CLOEXEC, proto); | |
| if (*sock < 0) { | |
| test_print("socket(AF_NETLINK)"); | |
| return -1; | |
| } | |
| randomize_buffer(seq_nr, sizeof(*seq_nr)); | |
| return 0; | |
| } | |
| static int netlink_check_answer(int sock, bool quite) | |
| { | |
| struct nlmsgerror { | |
| struct nlmsghdr hdr; | |
| int error; | |
| struct nlmsghdr orig_msg; | |
| } answer; | |
| if (recv(sock, &answer, sizeof(answer), 0) < 0) { | |
| test_print("recv()"); | |
| return -1; | |
| } else if (answer.hdr.nlmsg_type != NLMSG_ERROR) { | |
| test_print("expected NLMSG_ERROR, got %d", | |
| (int)answer.hdr.nlmsg_type); | |
| return -1; | |
| } else if (answer.error) { | |
| if (!quite) { | |
| test_print("NLMSG_ERROR: %d: %s", | |
| answer.error, strerror(-answer.error)); | |
| } | |
| return answer.error; | |
| } | |
| return 0; | |
| } | |
| static inline struct rtattr *rtattr_hdr(struct nlmsghdr *nh) | |
| { | |
| return (struct rtattr *)((char *)(nh) + RTA_ALIGN((nh)->nlmsg_len)); | |
| } | |
| static int rtattr_pack(struct nlmsghdr *nh, size_t req_sz, | |
| unsigned short rta_type, const void *payload, size_t size) | |
| { | |
| /* NLMSG_ALIGNTO == RTA_ALIGNTO, nlmsg_len already aligned */ | |
| struct rtattr *attr = rtattr_hdr(nh); | |
| size_t nl_size = RTA_ALIGN(nh->nlmsg_len) + RTA_LENGTH(size); | |
| if (req_sz < nl_size) { | |
| test_print("req buf is too small: %zu < %zu", req_sz, nl_size); | |
| return -1; | |
| } | |
| nh->nlmsg_len = nl_size; | |
| attr->rta_len = RTA_LENGTH(size); | |
| attr->rta_type = rta_type; | |
| memcpy(RTA_DATA(attr), payload, size); | |
| return 0; | |
| } | |
| static struct rtattr *_rtattr_begin(struct nlmsghdr *nh, size_t req_sz, | |
| unsigned short rta_type, const void *payload, size_t size) | |
| { | |
| struct rtattr *ret = rtattr_hdr(nh); | |
| if (rtattr_pack(nh, req_sz, rta_type, payload, size)) | |
| return 0; | |
| return ret; | |
| } | |
| static inline struct rtattr *rtattr_begin(struct nlmsghdr *nh, size_t req_sz, | |
| unsigned short rta_type) | |
| { | |
| return _rtattr_begin(nh, req_sz, rta_type, 0, 0); | |
| } | |
| static inline void rtattr_end(struct nlmsghdr *nh, struct rtattr *attr) | |
| { | |
| char *nlmsg_end = (char *)nh + nh->nlmsg_len; | |
| attr->rta_len = nlmsg_end - (char *)attr; | |
| } | |
| static int veth_pack_peerb(struct nlmsghdr *nh, size_t req_sz, | |
| const char *peer, int ns) | |
| { | |
| struct ifinfomsg pi; | |
| struct rtattr *peer_attr; | |
| memset(&pi, 0, sizeof(pi)); | |
| pi.ifi_family = AF_UNSPEC; | |
| pi.ifi_change = 0xFFFFFFFF; | |
| peer_attr = _rtattr_begin(nh, req_sz, VETH_INFO_PEER, &pi, sizeof(pi)); | |
| if (!peer_attr) | |
| return -1; | |
| if (rtattr_pack(nh, req_sz, IFLA_IFNAME, peer, strlen(peer))) | |
| return -1; | |
| if (rtattr_pack(nh, req_sz, IFLA_NET_NS_FD, &ns, sizeof(ns))) | |
| return -1; | |
| rtattr_end(nh, peer_attr); | |
| return 0; | |
| } | |
| static int __add_veth(int sock, uint32_t seq, const char *name, | |
| int ns_a, int ns_b) | |
| { | |
| uint16_t flags = NLM_F_REQUEST | NLM_F_ACK | NLM_F_EXCL | NLM_F_CREATE; | |
| struct { | |
| struct nlmsghdr nh; | |
| struct ifinfomsg info; | |
| char attrbuf[MAX_PAYLOAD]; | |
| } req; | |
| static const char veth_type[] = "veth"; | |
| struct rtattr *link_info, *info_data; | |
| memset(&req, 0, sizeof(req)); | |
| req.nh.nlmsg_len = NLMSG_LENGTH(sizeof(req.info)); | |
| req.nh.nlmsg_type = RTM_NEWLINK; | |
| req.nh.nlmsg_flags = flags; | |
| req.nh.nlmsg_seq = seq; | |
| req.info.ifi_family = AF_UNSPEC; | |
| req.info.ifi_change = 0xFFFFFFFF; | |
| if (rtattr_pack(&req.nh, sizeof(req), IFLA_IFNAME, name, strlen(name))) | |
| return -1; | |
| if (rtattr_pack(&req.nh, sizeof(req), IFLA_NET_NS_FD, &ns_a, sizeof(ns_a))) | |
| return -1; | |
| link_info = rtattr_begin(&req.nh, sizeof(req), IFLA_LINKINFO); | |
| if (!link_info) | |
| return -1; | |
| if (rtattr_pack(&req.nh, sizeof(req), IFLA_INFO_KIND, veth_type, sizeof(veth_type))) | |
| return -1; | |
| info_data = rtattr_begin(&req.nh, sizeof(req), IFLA_INFO_DATA); | |
| if (!info_data) | |
| return -1; | |
| if (veth_pack_peerb(&req.nh, sizeof(req), name, ns_b)) | |
| return -1; | |
| rtattr_end(&req.nh, info_data); | |
| rtattr_end(&req.nh, link_info); | |
| if (send(sock, &req, req.nh.nlmsg_len, 0) < 0) { | |
| test_print("send()"); | |
| return -1; | |
| } | |
| return netlink_check_answer(sock, false); | |
| } | |
| int add_veth(const char *name, int nsfda, int nsfdb) | |
| { | |
| int route_sock = -1, ret; | |
| uint32_t route_seq; | |
| if (netlink_sock(&route_sock, &route_seq, NETLINK_ROUTE)) | |
| test_error("Failed to open netlink route socket\n"); | |
| ret = __add_veth(route_sock, route_seq++, name, nsfda, nsfdb); | |
| close(route_sock); | |
| return ret; | |
| } | |
| static int __ip_addr_add(int sock, uint32_t seq, const char *intf, | |
| int family, union tcp_addr addr, uint8_t prefix) | |
| { | |
| uint16_t flags = NLM_F_REQUEST | NLM_F_ACK | NLM_F_EXCL | NLM_F_CREATE; | |
| struct { | |
| struct nlmsghdr nh; | |
| struct ifaddrmsg info; | |
| char attrbuf[MAX_PAYLOAD]; | |
| } req; | |
| size_t addr_len = (family == AF_INET) ? sizeof(struct in_addr) : | |
| sizeof(struct in6_addr); | |
| memset(&req, 0, sizeof(req)); | |
| req.nh.nlmsg_len = NLMSG_LENGTH(sizeof(req.info)); | |
| req.nh.nlmsg_type = RTM_NEWADDR; | |
| req.nh.nlmsg_flags = flags; | |
| req.nh.nlmsg_seq = seq; | |
| req.info.ifa_family = family; | |
| req.info.ifa_prefixlen = prefix; | |
| req.info.ifa_index = if_nametoindex(intf); | |
| req.info.ifa_flags = IFA_F_NODAD; | |
| if (rtattr_pack(&req.nh, sizeof(req), IFA_LOCAL, &addr, addr_len)) | |
| return -1; | |
| if (send(sock, &req, req.nh.nlmsg_len, 0) < 0) { | |
| test_print("send()"); | |
| return -1; | |
| } | |
| return netlink_check_answer(sock, true); | |
| } | |
| int ip_addr_add(const char *intf, int family, | |
| union tcp_addr addr, uint8_t prefix) | |
| { | |
| int route_sock = -1, ret; | |
| uint32_t route_seq; | |
| if (netlink_sock(&route_sock, &route_seq, NETLINK_ROUTE)) | |
| test_error("Failed to open netlink route socket\n"); | |
| ret = __ip_addr_add(route_sock, route_seq++, intf, | |
| family, addr, prefix); | |
| close(route_sock); | |
| return ret; | |
| } | |
| static int __ip_route_add(int sock, uint32_t seq, const char *intf, int family, | |
| union tcp_addr src, union tcp_addr dst, uint8_t vrf) | |
| { | |
| struct { | |
| struct nlmsghdr nh; | |
| struct rtmsg rt; | |
| char attrbuf[MAX_PAYLOAD]; | |
| } req; | |
| unsigned int index = if_nametoindex(intf); | |
| size_t addr_len = (family == AF_INET) ? sizeof(struct in_addr) : | |
| sizeof(struct in6_addr); | |
| memset(&req, 0, sizeof(req)); | |
| req.nh.nlmsg_len = NLMSG_LENGTH(sizeof(req.rt)); | |
| req.nh.nlmsg_type = RTM_NEWROUTE; | |
| req.nh.nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK | NLM_F_CREATE; | |
| req.nh.nlmsg_seq = seq; | |
| req.rt.rtm_family = family; | |
| req.rt.rtm_dst_len = (family == AF_INET) ? 32 : 128; | |
| req.rt.rtm_table = vrf; | |
| req.rt.rtm_protocol = RTPROT_BOOT; | |
| req.rt.rtm_scope = RT_SCOPE_UNIVERSE; | |
| req.rt.rtm_type = RTN_UNICAST; | |
| if (rtattr_pack(&req.nh, sizeof(req), RTA_DST, &dst, addr_len)) | |
| return -1; | |
| if (rtattr_pack(&req.nh, sizeof(req), RTA_PREFSRC, &src, addr_len)) | |
| return -1; | |
| if (rtattr_pack(&req.nh, sizeof(req), RTA_OIF, &index, sizeof(index))) | |
| return -1; | |
| if (send(sock, &req, req.nh.nlmsg_len, 0) < 0) { | |
| test_print("send()"); | |
| return -1; | |
| } | |
| return netlink_check_answer(sock, true); | |
| } | |
| int ip_route_add_vrf(const char *intf, int family, | |
| union tcp_addr src, union tcp_addr dst, uint8_t vrf) | |
| { | |
| int route_sock = -1, ret; | |
| uint32_t route_seq; | |
| if (netlink_sock(&route_sock, &route_seq, NETLINK_ROUTE)) | |
| test_error("Failed to open netlink route socket\n"); | |
| ret = __ip_route_add(route_sock, route_seq++, intf, | |
| family, src, dst, vrf); | |
| close(route_sock); | |
| return ret; | |
| } | |
| int ip_route_add(const char *intf, int family, | |
| union tcp_addr src, union tcp_addr dst) | |
| { | |
| return ip_route_add_vrf(intf, family, src, dst, RT_TABLE_MAIN); | |
| } | |
| static int __link_set_up(int sock, uint32_t seq, const char *intf) | |
| { | |
| struct { | |
| struct nlmsghdr nh; | |
| struct ifinfomsg info; | |
| char attrbuf[MAX_PAYLOAD]; | |
| } req; | |
| memset(&req, 0, sizeof(req)); | |
| req.nh.nlmsg_len = NLMSG_LENGTH(sizeof(req.info)); | |
| req.nh.nlmsg_type = RTM_NEWLINK; | |
| req.nh.nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK; | |
| req.nh.nlmsg_seq = seq; | |
| req.info.ifi_family = AF_UNSPEC; | |
| req.info.ifi_change = 0xFFFFFFFF; | |
| req.info.ifi_index = if_nametoindex(intf); | |
| req.info.ifi_flags = IFF_UP; | |
| req.info.ifi_change = IFF_UP; | |
| if (send(sock, &req, req.nh.nlmsg_len, 0) < 0) { | |
| test_print("send()"); | |
| return -1; | |
| } | |
| return netlink_check_answer(sock, false); | |
| } | |
| int link_set_up(const char *intf) | |
| { | |
| int route_sock = -1, ret; | |
| uint32_t route_seq; | |
| if (netlink_sock(&route_sock, &route_seq, NETLINK_ROUTE)) | |
| test_error("Failed to open netlink route socket\n"); | |
| ret = __link_set_up(route_sock, route_seq++, intf); | |
| close(route_sock); | |
| return ret; | |
| } | |
| static int __add_vrf(int sock, uint32_t seq, const char *name, | |
| uint32_t tabid, int ifindex, int nsfd) | |
| { | |
| uint16_t flags = NLM_F_REQUEST | NLM_F_ACK | NLM_F_EXCL | NLM_F_CREATE; | |
| struct { | |
| struct nlmsghdr nh; | |
| struct ifinfomsg info; | |
| char attrbuf[MAX_PAYLOAD]; | |
| } req; | |
| static const char vrf_type[] = "vrf"; | |
| struct rtattr *link_info, *info_data; | |
| memset(&req, 0, sizeof(req)); | |
| req.nh.nlmsg_len = NLMSG_LENGTH(sizeof(req.info)); | |
| req.nh.nlmsg_type = RTM_NEWLINK; | |
| req.nh.nlmsg_flags = flags; | |
| req.nh.nlmsg_seq = seq; | |
| req.info.ifi_family = AF_UNSPEC; | |
| req.info.ifi_change = 0xFFFFFFFF; | |
| req.info.ifi_index = ifindex; | |
| if (rtattr_pack(&req.nh, sizeof(req), IFLA_IFNAME, name, strlen(name))) | |
| return -1; | |
| if (nsfd >= 0) | |
| if (rtattr_pack(&req.nh, sizeof(req), IFLA_NET_NS_FD, | |
| &nsfd, sizeof(nsfd))) | |
| return -1; | |
| link_info = rtattr_begin(&req.nh, sizeof(req), IFLA_LINKINFO); | |
| if (!link_info) | |
| return -1; | |
| if (rtattr_pack(&req.nh, sizeof(req), IFLA_INFO_KIND, vrf_type, sizeof(vrf_type))) | |
| return -1; | |
| info_data = rtattr_begin(&req.nh, sizeof(req), IFLA_INFO_DATA); | |
| if (!info_data) | |
| return -1; | |
| if (rtattr_pack(&req.nh, sizeof(req), IFLA_VRF_TABLE, | |
| &tabid, sizeof(tabid))) | |
| return -1; | |
| rtattr_end(&req.nh, info_data); | |
| rtattr_end(&req.nh, link_info); | |
| if (send(sock, &req, req.nh.nlmsg_len, 0) < 0) { | |
| test_print("send()"); | |
| return -1; | |
| } | |
| return netlink_check_answer(sock, true); | |
| } | |
| int add_vrf(const char *name, uint32_t tabid, int ifindex, int nsfd) | |
| { | |
| int route_sock = -1, ret; | |
| uint32_t route_seq; | |
| if (netlink_sock(&route_sock, &route_seq, NETLINK_ROUTE)) | |
| test_error("Failed to open netlink route socket\n"); | |
| ret = __add_vrf(route_sock, route_seq++, name, tabid, ifindex, nsfd); | |
| close(route_sock); | |
| return ret; | |
| } |