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