1 // SPDX-License-Identifier: GPL-2.0
2 
3 #define _GNU_SOURCE
4 
5 #include <arpa/inet.h>
6 #include <error.h>
7 #include <errno.h>
8 #include <limits.h>
9 #include <linux/errqueue.h>
10 #include <linux/if_packet.h>
11 #include <linux/socket.h>
12 #include <linux/sockios.h>
13 #include <net/ethernet.h>
14 #include <net/if.h>
15 #include <netinet/ip.h>
16 #include <netinet/ip6.h>
17 #include <netinet/tcp.h>
18 #include <netinet/udp.h>
19 #include <poll.h>
20 #include <sched.h>
21 #include <stdbool.h>
22 #include <stdio.h>
23 #include <stdint.h>
24 #include <stdlib.h>
25 #include <string.h>
26 #include <sys/ioctl.h>
27 #include <sys/socket.h>
28 #include <sys/stat.h>
29 #include <sys/time.h>
30 #include <sys/types.h>
31 #include <sys/wait.h>
32 #include <unistd.h>
33 
34 #ifndef UDP_GRO
35 #define UDP_GRO		104
36 #endif
37 
38 static int  cfg_port		= 8000;
39 static bool cfg_tcp;
40 static bool cfg_verify;
41 static bool cfg_read_all;
42 static bool cfg_gro_segment;
43 static int  cfg_family		= PF_INET6;
44 static int  cfg_alen 		= sizeof(struct sockaddr_in6);
45 static int  cfg_expected_pkt_nr;
46 static int  cfg_expected_pkt_len;
47 static int  cfg_expected_gso_size;
48 static int  cfg_connect_timeout_ms;
49 static int  cfg_rcv_timeout_ms;
50 static struct sockaddr_storage cfg_bind_addr;
51 
52 static bool interrupted;
53 static unsigned long packets, bytes;
54 
55 static void sigint_handler(int signum)
56 {
57 	if (signum == SIGINT)
58 		interrupted = true;
59 }
60 
61 static void setup_sockaddr(int domain, const char *str_addr, void *sockaddr)
62 {
63 	struct sockaddr_in6 *addr6 = (void *) sockaddr;
64 	struct sockaddr_in *addr4 = (void *) sockaddr;
65 
66 	switch (domain) {
67 	case PF_INET:
68 		addr4->sin_family = AF_INET;
69 		addr4->sin_port = htons(cfg_port);
70 		if (inet_pton(AF_INET, str_addr, &(addr4->sin_addr)) != 1)
71 			error(1, 0, "ipv4 parse error: %s", str_addr);
72 		break;
73 	case PF_INET6:
74 		addr6->sin6_family = AF_INET6;
75 		addr6->sin6_port = htons(cfg_port);
76 		if (inet_pton(AF_INET6, str_addr, &(addr6->sin6_addr)) != 1)
77 			error(1, 0, "ipv6 parse error: %s", str_addr);
78 		break;
79 	default:
80 		error(1, 0, "illegal domain");
81 	}
82 }
83 
84 static unsigned long gettimeofday_ms(void)
85 {
86 	struct timeval tv;
87 
88 	gettimeofday(&tv, NULL);
89 	return (tv.tv_sec * 1000) + (tv.tv_usec / 1000);
90 }
91 
92 static void do_poll(int fd, int timeout_ms)
93 {
94 	struct pollfd pfd;
95 	int ret;
96 
97 	pfd.events = POLLIN;
98 	pfd.revents = 0;
99 	pfd.fd = fd;
100 
101 	do {
102 		ret = poll(&pfd, 1, 10);
103 		if (interrupted)
104 			break;
105 		if (ret == -1)
106 			error(1, errno, "poll");
107 		if (ret == 0) {
108 			if (!timeout_ms)
109 				continue;
110 
111 			timeout_ms -= 10;
112 			if (timeout_ms <= 0) {
113 				interrupted = true;
114 				break;
115 			}
116 
117 			/* no events and more time to wait, do poll again */
118 			continue;
119 		}
120 		if (pfd.revents != POLLIN)
121 			error(1, errno, "poll: 0x%x expected 0x%x\n",
122 					pfd.revents, POLLIN);
123 	} while (!ret);
124 }
125 
126 static int do_socket(bool do_tcp)
127 {
128 	int fd, val;
129 
130 	fd = socket(cfg_family, cfg_tcp ? SOCK_STREAM : SOCK_DGRAM, 0);
131 	if (fd == -1)
132 		error(1, errno, "socket");
133 
134 	val = 1 << 21;
135 	if (setsockopt(fd, SOL_SOCKET, SO_RCVBUF, &val, sizeof(val)))
136 		error(1, errno, "setsockopt rcvbuf");
137 	val = 1;
138 	if (setsockopt(fd, SOL_SOCKET, SO_REUSEPORT, &val, sizeof(val)))
139 		error(1, errno, "setsockopt reuseport");
140 
141 	if (bind(fd, (void *)&cfg_bind_addr, cfg_alen))
142 		error(1, errno, "bind");
143 
144 	if (do_tcp) {
145 		int accept_fd = fd;
146 
147 		if (listen(accept_fd, 1))
148 			error(1, errno, "listen");
149 
150 		do_poll(accept_fd, cfg_connect_timeout_ms);
151 		if (interrupted)
152 			exit(0);
153 
154 		fd = accept(accept_fd, NULL, NULL);
155 		if (fd == -1)
156 			error(1, errno, "accept");
157 		if (close(accept_fd))
158 			error(1, errno, "close accept fd");
159 	}
160 
161 	return fd;
162 }
163 
164 /* Flush all outstanding bytes for the tcp receive queue */
165 static void do_flush_tcp(int fd)
166 {
167 	int ret;
168 
169 	while (true) {
170 		/* MSG_TRUNC flushes up to len bytes */
171 		ret = recv(fd, NULL, 1 << 21, MSG_TRUNC | MSG_DONTWAIT);
172 		if (ret == -1 && errno == EAGAIN)
173 			return;
174 		if (ret == -1)
175 			error(1, errno, "flush");
176 		if (ret == 0) {
177 			/* client detached */
178 			exit(0);
179 		}
180 
181 		packets++;
182 		bytes += ret;
183 	}
184 
185 }
186 
187 static char sanitized_char(char val)
188 {
189 	return (val >= 'a' && val <= 'z') ? val : '.';
190 }
191 
192 static void do_verify_udp(const char *data, int len)
193 {
194 	char cur = data[0];
195 	int i;
196 
197 	/* verify contents */
198 	if (cur < 'a' || cur > 'z')
199 		error(1, 0, "data initial byte out of range");
200 
201 	for (i = 1; i < len; i++) {
202 		if (cur == 'z')
203 			cur = 'a';
204 		else
205 			cur++;
206 
207 		if (data[i] != cur)
208 			error(1, 0, "data[%d]: len %d, %c(%hhu) != %c(%hhu)\n",
209 			      i, len,
210 			      sanitized_char(data[i]), data[i],
211 			      sanitized_char(cur), cur);
212 	}
213 }
214 
215 static int recv_msg(int fd, char *buf, int len, int *gso_size)
216 {
217 	char control[CMSG_SPACE(sizeof(uint16_t))] = {0};
218 	struct msghdr msg = {0};
219 	struct iovec iov = {0};
220 	struct cmsghdr *cmsg;
221 	uint16_t *gsosizeptr;
222 	int ret;
223 
224 	iov.iov_base = buf;
225 	iov.iov_len = len;
226 
227 	msg.msg_iov = &iov;
228 	msg.msg_iovlen = 1;
229 
230 	msg.msg_control = control;
231 	msg.msg_controllen = sizeof(control);
232 
233 	*gso_size = -1;
234 	ret = recvmsg(fd, &msg, MSG_TRUNC | MSG_DONTWAIT);
235 	if (ret != -1) {
236 		for (cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL;
237 		     cmsg = CMSG_NXTHDR(&msg, cmsg)) {
238 			if (cmsg->cmsg_level == SOL_UDP
239 			    && cmsg->cmsg_type == UDP_GRO) {
240 				gsosizeptr = (uint16_t *) CMSG_DATA(cmsg);
241 				*gso_size = *gsosizeptr;
242 				break;
243 			}
244 		}
245 	}
246 	return ret;
247 }
248 
249 /* Flush all outstanding datagrams. Verify first few bytes of each. */
250 static void do_flush_udp(int fd)
251 {
252 	static char rbuf[ETH_MAX_MTU];
253 	int ret, len, gso_size, budget = 256;
254 
255 	len = cfg_read_all ? sizeof(rbuf) : 0;
256 	while (budget--) {
257 		/* MSG_TRUNC will make return value full datagram length */
258 		if (!cfg_expected_gso_size)
259 			ret = recv(fd, rbuf, len, MSG_TRUNC | MSG_DONTWAIT);
260 		else
261 			ret = recv_msg(fd, rbuf, len, &gso_size);
262 		if (ret == -1 && errno == EAGAIN)
263 			break;
264 		if (ret == -1)
265 			error(1, errno, "recv");
266 		if (cfg_expected_pkt_len && ret != cfg_expected_pkt_len)
267 			error(1, 0, "recv: bad packet len, got %d,"
268 			      " expected %d\n", ret, cfg_expected_pkt_len);
269 		if (len && cfg_verify) {
270 			if (ret == 0)
271 				error(1, errno, "recv: 0 byte datagram\n");
272 
273 			do_verify_udp(rbuf, ret);
274 		}
275 		if (cfg_expected_gso_size && cfg_expected_gso_size != gso_size)
276 			error(1, 0, "recv: bad gso size, got %d, expected %d "
277 			      "(-1 == no gso cmsg))\n", gso_size,
278 			      cfg_expected_gso_size);
279 
280 		packets++;
281 		bytes += ret;
282 		if (cfg_expected_pkt_nr && packets >= cfg_expected_pkt_nr)
283 			break;
284 	}
285 }
286 
287 static void usage(const char *filepath)
288 {
289 	error(1, 0, "Usage: %s [-C connect_timeout] [-Grtv] [-b addr] [-p port]"
290 	      " [-l pktlen] [-n packetnr] [-R rcv_timeout] [-S gsosize]",
291 	      filepath);
292 }
293 
294 static void parse_opts(int argc, char **argv)
295 {
296 	int c;
297 
298 	/* bind to any by default */
299 	setup_sockaddr(PF_INET6, "::", &cfg_bind_addr);
300 	while ((c = getopt(argc, argv, "4b:C:Gl:n:p:rR:S:tv")) != -1) {
301 		switch (c) {
302 		case '4':
303 			cfg_family = PF_INET;
304 			cfg_alen = sizeof(struct sockaddr_in);
305 			setup_sockaddr(PF_INET, "0.0.0.0", &cfg_bind_addr);
306 			break;
307 		case 'b':
308 			setup_sockaddr(cfg_family, optarg, &cfg_bind_addr);
309 			break;
310 		case 'C':
311 			cfg_connect_timeout_ms = strtoul(optarg, NULL, 0);
312 			break;
313 		case 'G':
314 			cfg_gro_segment = true;
315 			break;
316 		case 'l':
317 			cfg_expected_pkt_len = strtoul(optarg, NULL, 0);
318 			break;
319 		case 'n':
320 			cfg_expected_pkt_nr = strtoul(optarg, NULL, 0);
321 			break;
322 		case 'p':
323 			cfg_port = strtoul(optarg, NULL, 0);
324 			break;
325 		case 'r':
326 			cfg_read_all = true;
327 			break;
328 		case 'R':
329 			cfg_rcv_timeout_ms = strtoul(optarg, NULL, 0);
330 			break;
331 		case 'S':
332 			cfg_expected_gso_size = strtol(optarg, NULL, 0);
333 			break;
334 		case 't':
335 			cfg_tcp = true;
336 			break;
337 		case 'v':
338 			cfg_verify = true;
339 			cfg_read_all = true;
340 			break;
341 		}
342 	}
343 
344 	if (optind != argc)
345 		usage(argv[0]);
346 
347 	if (cfg_tcp && cfg_verify)
348 		error(1, 0, "TODO: implement verify mode for tcp");
349 }
350 
351 static void do_recv(void)
352 {
353 	int timeout_ms = cfg_tcp ? cfg_rcv_timeout_ms : cfg_connect_timeout_ms;
354 	unsigned long tnow, treport;
355 	int fd;
356 
357 	fd = do_socket(cfg_tcp);
358 
359 	if (cfg_gro_segment && !cfg_tcp) {
360 		int val = 1;
361 		if (setsockopt(fd, IPPROTO_UDP, UDP_GRO, &val, sizeof(val)))
362 			error(1, errno, "setsockopt UDP_GRO");
363 	}
364 
365 	treport = gettimeofday_ms() + 1000;
366 	do {
367 		do_poll(fd, timeout_ms);
368 
369 		if (cfg_tcp)
370 			do_flush_tcp(fd);
371 		else
372 			do_flush_udp(fd);
373 
374 		tnow = gettimeofday_ms();
375 		if (tnow > treport) {
376 			if (packets)
377 				fprintf(stderr,
378 					"%s rx: %6lu MB/s %8lu calls/s\n",
379 					cfg_tcp ? "tcp" : "udp",
380 					bytes >> 20, packets);
381 			bytes = packets = 0;
382 			treport = tnow + 1000;
383 		}
384 
385 		timeout_ms = cfg_rcv_timeout_ms;
386 
387 	} while (!interrupted);
388 
389 	if (cfg_expected_pkt_nr && (packets != cfg_expected_pkt_nr))
390 		error(1, 0, "wrong packet number! got %ld, expected %d\n",
391 		      packets, cfg_expected_pkt_nr);
392 
393 	if (close(fd))
394 		error(1, errno, "close");
395 }
396 
397 int main(int argc, char **argv)
398 {
399 	parse_opts(argc, argv);
400 
401 	signal(SIGINT, sigint_handler);
402 
403 	do_recv();
404 
405 	return 0;
406 }
407