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