tools/testing/selftests/net/tcp_ao/lib/netlink.c

   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 }