1 // SPDX-License-Identifier: GPL-2.0 2 3 #define _GNU_SOURCE 4 5 #include <arpa/inet.h> 6 #include <errno.h> 7 #include <error.h> 8 #include <fcntl.h> 9 #include <limits.h> 10 #include <linux/filter.h> 11 #include <linux/bpf.h> 12 #include <linux/if_packet.h> 13 #include <linux/if_vlan.h> 14 #include <linux/virtio_net.h> 15 #include <net/if.h> 16 #include <net/ethernet.h> 17 #include <netinet/ip.h> 18 #include <netinet/udp.h> 19 #include <poll.h> 20 #include <sched.h> 21 #include <stdbool.h> 22 #include <stdint.h> 23 #include <stdio.h> 24 #include <stdlib.h> 25 #include <string.h> 26 #include <sys/mman.h> 27 #include <sys/socket.h> 28 #include <sys/stat.h> 29 #include <sys/types.h> 30 #include <unistd.h> 31 32 #include "psock_lib.h" 33 34 static bool cfg_use_bind; 35 static bool cfg_use_csum_off; 36 static bool cfg_use_csum_off_bad; 37 static bool cfg_use_dgram; 38 static bool cfg_use_gso; 39 static bool cfg_use_qdisc_bypass; 40 static bool cfg_use_vlan; 41 static bool cfg_use_vnet; 42 static bool cfg_drop; 43 static bool cfg_aux_data; 44 45 static char *cfg_ifname = "lo"; 46 static int cfg_mtu = 1500; 47 static int cfg_payload_len = DATA_LEN; 48 static int cfg_truncate_len = INT_MAX; 49 static uint16_t cfg_port = 8000; 50 51 /* test sending up to max mtu + 1 */ 52 #define TEST_SZ (sizeof(struct virtio_net_hdr) + ETH_HLEN + ETH_MAX_MTU + 1) 53 54 #define BURST_CNT (1000) 55 56 static char tbuf[TEST_SZ], rbuf[TEST_SZ]; 57 58 static unsigned long add_csum_hword(const uint16_t *start, int num_u16) 59 { 60 unsigned long sum = 0; 61 int i; 62 63 for (i = 0; i < num_u16; i++) 64 sum += start[i]; 65 66 return sum; 67 } 68 69 static uint16_t build_ip_csum(const uint16_t *start, int num_u16, 70 unsigned long sum) 71 { 72 sum += add_csum_hword(start, num_u16); 73 74 while (sum >> 16) 75 sum = (sum & 0xffff) + (sum >> 16); 76 77 return ~sum; 78 } 79 80 static int build_vnet_header(void *header) 81 { 82 struct virtio_net_hdr *vh = header; 83 84 vh->hdr_len = ETH_HLEN + sizeof(struct iphdr) + sizeof(struct udphdr); 85 86 if (cfg_use_csum_off) { 87 vh->flags |= VIRTIO_NET_HDR_F_NEEDS_CSUM; 88 vh->csum_start = ETH_HLEN + sizeof(struct iphdr); 89 vh->csum_offset = __builtin_offsetof(struct udphdr, check); 90 91 /* position check field exactly one byte beyond end of packet */ 92 if (cfg_use_csum_off_bad) 93 vh->csum_start += sizeof(struct udphdr) + cfg_payload_len - 94 vh->csum_offset - 1; 95 } 96 97 if (cfg_use_gso) { 98 vh->gso_type = VIRTIO_NET_HDR_GSO_UDP; 99 vh->gso_size = cfg_mtu - sizeof(struct iphdr); 100 } 101 102 return sizeof(*vh); 103 } 104 105 static int build_eth_header(void *header) 106 { 107 struct ethhdr *eth = header; 108 109 if (cfg_use_vlan) { 110 uint16_t *tag = header + ETH_HLEN; 111 112 eth->h_proto = htons(ETH_P_8021Q); 113 tag[1] = htons(ETH_P_IP); 114 return ETH_HLEN + 4; 115 } 116 117 eth->h_proto = htons(ETH_P_IP); 118 return ETH_HLEN; 119 } 120 121 static int build_ipv4_header(void *header, int payload_len) 122 { 123 struct iphdr *iph = header; 124 125 iph->ihl = 5; 126 iph->version = 4; 127 iph->ttl = 8; 128 iph->tot_len = htons(sizeof(*iph) + sizeof(struct udphdr) + payload_len); 129 iph->id = htons(1337); 130 iph->protocol = IPPROTO_UDP; 131 iph->saddr = htonl((172 << 24) | (17 << 16) | 2); 132 iph->daddr = htonl((172 << 24) | (17 << 16) | 1); 133 iph->check = build_ip_csum((void *) iph, iph->ihl << 1, 0); 134 135 return iph->ihl << 2; 136 } 137 138 static int build_udp_header(void *header, int payload_len) 139 { 140 const int alen = sizeof(uint32_t); 141 struct udphdr *udph = header; 142 int len = sizeof(*udph) + payload_len; 143 144 udph->source = htons(9); 145 udph->dest = htons(cfg_port); 146 udph->len = htons(len); 147 148 if (cfg_use_csum_off) 149 udph->check = build_ip_csum(header - (2 * alen), alen, 150 htons(IPPROTO_UDP) + udph->len); 151 else 152 udph->check = 0; 153 154 return sizeof(*udph); 155 } 156 157 static int build_packet(int payload_len) 158 { 159 int off = 0; 160 161 off += build_vnet_header(tbuf); 162 off += build_eth_header(tbuf + off); 163 off += build_ipv4_header(tbuf + off, payload_len); 164 off += build_udp_header(tbuf + off, payload_len); 165 166 if (off + payload_len > sizeof(tbuf)) 167 error(1, 0, "payload length exceeds max"); 168 169 memset(tbuf + off, DATA_CHAR, payload_len); 170 171 return off + payload_len; 172 } 173 174 static void do_bind(int fd) 175 { 176 struct sockaddr_ll laddr = {0}; 177 178 laddr.sll_family = AF_PACKET; 179 laddr.sll_protocol = htons(ETH_P_IP); 180 laddr.sll_ifindex = if_nametoindex(cfg_ifname); 181 if (!laddr.sll_ifindex) 182 error(1, errno, "if_nametoindex"); 183 184 if (bind(fd, (void *)&laddr, sizeof(laddr))) 185 error(1, errno, "bind"); 186 } 187 188 static void do_send(int fd, char *buf, int len) 189 { 190 int ret; 191 192 if (!cfg_use_vnet) { 193 buf += sizeof(struct virtio_net_hdr); 194 len -= sizeof(struct virtio_net_hdr); 195 } 196 if (cfg_use_dgram) { 197 buf += ETH_HLEN; 198 len -= ETH_HLEN; 199 } 200 201 if (cfg_use_bind) { 202 ret = write(fd, buf, len); 203 } else { 204 struct sockaddr_ll laddr = {0}; 205 206 laddr.sll_protocol = htons(ETH_P_IP); 207 laddr.sll_ifindex = if_nametoindex(cfg_ifname); 208 if (!laddr.sll_ifindex) 209 error(1, errno, "if_nametoindex"); 210 211 ret = sendto(fd, buf, len, 0, (void *)&laddr, sizeof(laddr)); 212 } 213 214 if (ret == -1) 215 error(1, errno, "write"); 216 if (ret != len) 217 error(1, 0, "write: %u %u", ret, len); 218 219 if (!cfg_drop) 220 fprintf(stderr, "tx: %u\n", ret); 221 } 222 223 static int do_tx(void) 224 { 225 const int one = 1; 226 int i, fd, len; 227 228 fd = socket(PF_PACKET, cfg_use_dgram ? SOCK_DGRAM : SOCK_RAW, 0); 229 if (fd == -1) 230 error(1, errno, "socket t"); 231 232 if (cfg_use_bind) 233 do_bind(fd); 234 235 if (cfg_use_qdisc_bypass && 236 setsockopt(fd, SOL_PACKET, PACKET_QDISC_BYPASS, &one, sizeof(one))) 237 error(1, errno, "setsockopt qdisc bypass"); 238 239 if (cfg_use_vnet && 240 setsockopt(fd, SOL_PACKET, PACKET_VNET_HDR, &one, sizeof(one))) 241 error(1, errno, "setsockopt vnet"); 242 243 len = build_packet(cfg_payload_len); 244 245 if (cfg_truncate_len < len) 246 len = cfg_truncate_len; 247 248 do_send(fd, tbuf, len); 249 250 if (cfg_drop) 251 for (i = 0; i < BURST_CNT; i++) 252 do_send(fd, tbuf, len); 253 254 if (close(fd)) 255 error(1, errno, "close t"); 256 257 return len; 258 } 259 260 static int setup_rx(void) 261 { 262 struct timeval tv = { .tv_usec = 100 * 1000 }; 263 struct sockaddr_in raddr = {0}; 264 int fd; 265 266 fd = socket(PF_INET, SOCK_DGRAM, 0); 267 if (fd == -1) 268 error(1, errno, "socket r"); 269 270 if (setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv))) 271 error(1, errno, "setsockopt rcv timeout"); 272 273 raddr.sin_family = AF_INET; 274 raddr.sin_port = htons(cfg_port); 275 raddr.sin_addr.s_addr = htonl(INADDR_ANY); 276 277 if (bind(fd, (void *)&raddr, sizeof(raddr))) 278 error(1, errno, "bind r"); 279 280 return fd; 281 } 282 283 static void check_aux_data(struct cmsghdr *cmsg, int expected_len) 284 { 285 struct tpacket_auxdata *adata; 286 287 if (!cmsg) 288 error(1, 0, "auxdata null"); 289 290 if (cmsg->cmsg_level != SOL_PACKET) 291 error(1, 0, "cmsg_level != SOL_PACKET"); 292 293 if (cmsg->cmsg_type != PACKET_AUXDATA) 294 error(1, 0, "cmsg_type != PACKET_AUXDATA"); 295 296 adata = (struct tpacket_auxdata *)CMSG_DATA(cmsg); 297 298 if (adata->tp_net != ETH_HLEN) 299 error(1, 0, "cmsg tp_net != ETH_HLEN"); 300 301 if (adata->tp_len != expected_len) 302 error(1, 0, "cmsg tp_len != %u", expected_len); 303 304 if (adata->tp_snaplen != expected_len) 305 error(1, 0, "cmsg tp_snaplen != %u", expected_len); 306 } 307 308 static void do_rx(int fd, int expected_len, char *expected, bool is_psock) 309 { 310 char cmsg_buf[1024] __attribute__((aligned(8))) = {}; 311 bool aux = is_psock && cfg_aux_data; 312 struct msghdr msg = {}; 313 struct iovec iov[1]; 314 int ret; 315 316 if (aux) { 317 iov[0].iov_base = rbuf; 318 iov[0].iov_len = sizeof(rbuf); 319 320 msg.msg_iov = iov; 321 msg.msg_iovlen = 1; 322 323 msg.msg_control = cmsg_buf; 324 msg.msg_controllen = sizeof(cmsg_buf); 325 326 ret = recvmsg(fd, &msg, 0); 327 } else { 328 ret = recv(fd, rbuf, sizeof(rbuf), 0); 329 } 330 331 if (ret == -1) 332 error(1, errno, "recv"); 333 if (ret != expected_len) 334 error(1, 0, "recv: %u != %u", ret, expected_len); 335 336 if (memcmp(rbuf, expected, ret)) 337 error(1, 0, "recv: data mismatch"); 338 339 if (aux) { 340 struct cmsghdr *cmsg = CMSG_FIRSTHDR(&msg); 341 342 check_aux_data(cmsg, expected_len); 343 } 344 345 fprintf(stderr, "rx: %u\n", ret); 346 } 347 348 static int setup_sniffer(void) 349 { 350 struct timeval tv = { .tv_usec = 100 * 1000 }; 351 const int one = 1; 352 int fd; 353 354 fd = socket(PF_PACKET, SOCK_RAW, 0); 355 if (fd == -1) 356 error(1, errno, "socket p"); 357 358 if (setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv))) 359 error(1, errno, "setsockopt rcv timeout"); 360 361 if (cfg_drop) 362 if (setsockopt(fd, SOL_SOCKET, SO_RCVBUF, &one, sizeof(one))) 363 error(1, errno, "setsockopt SO_RCVBUF"); 364 365 if (cfg_aux_data) 366 if (setsockopt(fd, SOL_PACKET, PACKET_AUXDATA, &one, sizeof(one))) 367 error(1, errno, "setsockopt PACKET_AUXDATA"); 368 369 pair_udp_setfilter(fd); 370 do_bind(fd); 371 372 return fd; 373 } 374 375 static void parse_opts(int argc, char **argv) 376 { 377 int c; 378 379 while ((c = getopt(argc, argv, "abcCdDgl:qt:vV")) != -1) { 380 switch (c) { 381 case 'a': 382 cfg_aux_data = true; 383 break; 384 case 'b': 385 cfg_use_bind = true; 386 break; 387 case 'c': 388 cfg_use_csum_off = true; 389 break; 390 case 'C': 391 cfg_use_csum_off_bad = true; 392 break; 393 case 'd': 394 cfg_use_dgram = true; 395 break; 396 case 'D': 397 cfg_drop = true; 398 break; 399 case 'g': 400 cfg_use_gso = true; 401 break; 402 case 'l': 403 cfg_payload_len = strtoul(optarg, NULL, 0); 404 break; 405 case 'q': 406 cfg_use_qdisc_bypass = true; 407 break; 408 case 't': 409 cfg_truncate_len = strtoul(optarg, NULL, 0); 410 break; 411 case 'v': 412 cfg_use_vnet = true; 413 break; 414 case 'V': 415 cfg_use_vlan = true; 416 break; 417 default: 418 error(1, 0, "%s: parse error", argv[0]); 419 } 420 } 421 422 if (cfg_use_vlan && cfg_use_dgram) 423 error(1, 0, "option vlan (-V) conflicts with dgram (-d)"); 424 425 if (cfg_use_csum_off && !cfg_use_vnet) 426 error(1, 0, "option csum offload (-c) requires vnet (-v)"); 427 428 if (cfg_use_csum_off_bad && !cfg_use_csum_off) 429 error(1, 0, "option csum bad (-C) requires csum offload (-c)"); 430 431 if (cfg_use_gso && !cfg_use_csum_off) 432 error(1, 0, "option gso (-g) requires csum offload (-c)"); 433 434 if (cfg_aux_data && cfg_drop) 435 error(1, 0, "option aux data (-a) conflicts with drop (-D)"); 436 } 437 438 static void check_packet_stats(int fd) 439 { 440 struct tpacket_stats st = {}; 441 socklen_t len = sizeof(st); 442 443 if (getsockopt(fd, SOL_PACKET, PACKET_STATISTICS, &st, &len)) 444 error(1, errno, "getsockopt packet statistics"); 445 446 if (cfg_drop) { 447 /* PACKET_STATISTICS reports all packets seen (including 448 * drops) in tp_packets 449 */ 450 if (st.tp_packets < st.tp_drops) 451 error(1, 0, "stats: tp_packets %u < tp_drops %u", 452 st.tp_packets, st.tp_drops); 453 454 if (st.tp_drops == 0) 455 error(1, 0, "stats: expected drops but tp_drops == 0"); 456 } else { 457 if (st.tp_packets != 1) 458 error(1, 0, "stats: tp_packets %u != 1", st.tp_packets); 459 460 if (st.tp_drops != 0) 461 error(1, 0, "stats: tp_drops %u != 0", st.tp_drops); 462 } 463 464 /* verify clear on read */ 465 memset(&st, 0xff, sizeof(st)); 466 len = sizeof(st); 467 468 if (getsockopt(fd, SOL_PACKET, PACKET_STATISTICS, &st, &len)) 469 error(1, errno, "getsockopt packet statistics"); 470 471 if (st.tp_packets != 0) 472 error(1, 0, "stats: tp_packets %u != 0 after clear", st.tp_packets); 473 474 if (st.tp_drops != 0) 475 error(1, 0, "stats: tp_drops %u != 0 after clear", st.tp_drops); 476 } 477 478 static void run_test(void) 479 { 480 int fdr, fds, total_len; 481 482 fdr = setup_rx(); 483 fds = setup_sniffer(); 484 485 total_len = do_tx(); 486 487 if (cfg_drop) { 488 check_packet_stats(fds); 489 goto out; 490 } 491 492 /* BPF filter accepts only this length, vlan changes MAC */ 493 if (cfg_payload_len == DATA_LEN && !cfg_use_vlan) { 494 do_rx(fds, total_len - sizeof(struct virtio_net_hdr), 495 tbuf + sizeof(struct virtio_net_hdr), true); 496 check_packet_stats(fds); 497 } 498 499 do_rx(fdr, cfg_payload_len, tbuf + total_len - cfg_payload_len, false); 500 501 out: 502 if (close(fds)) 503 error(1, errno, "close s"); 504 if (close(fdr)) 505 error(1, errno, "close r"); 506 } 507 508 int main(int argc, char **argv) 509 { 510 parse_opts(argc, argv); 511 512 if (system("ip link set dev lo mtu 1500")) 513 error(1, errno, "ip link set mtu"); 514 if (system("ip addr add dev lo 172.17.0.1/24")) 515 error(1, errno, "ip addr add"); 516 if (system("sysctl -w net.ipv4.conf.lo.accept_local=1")) 517 error(1, errno, "sysctl lo.accept_local"); 518 519 run_test(); 520 521 fprintf(stderr, "OK\n\n"); 522 return 0; 523 } 524