1 #include <errno.h>
2 #include <error.h>
3 #include <getopt.h>
4 #include <stdbool.h>
5 #include <stdio.h>
6 #include <stdlib.h>
7 #include <string.h>
8 #include <unistd.h>
9 
10 #include <sys/time.h>
11 #include <sys/socket.h>
12 #include <sys/select.h>
13 #include <sys/ioctl.h>
14 #include <arpa/inet.h>
15 #include <net/if.h>
16 
17 #include <asm/types.h>
18 #include <linux/net_tstamp.h>
19 #include <linux/errqueue.h>
20 
21 #define ARRAY_SIZE(arr) (sizeof(arr) / sizeof((arr)[0]))
22 
23 struct options {
24 	int so_timestamp;
25 	int so_timestampns;
26 	int so_timestamping;
27 };
28 
29 struct tstamps {
30 	bool tstamp;
31 	bool tstampns;
32 	bool swtstamp;
33 	bool hwtstamp;
34 };
35 
36 struct socket_type {
37 	char *friendly_name;
38 	int type;
39 	int protocol;
40 	bool enabled;
41 };
42 
43 struct test_case {
44 	struct options sockopt;
45 	struct tstamps expected;
46 	bool enabled;
47 	bool warn_on_fail;
48 };
49 
50 struct sof_flag {
51 	int mask;
52 	char *name;
53 };
54 
55 static struct sof_flag sof_flags[] = {
56 #define SOF_FLAG(f) { f, #f }
57 	SOF_FLAG(SOF_TIMESTAMPING_SOFTWARE),
58 	SOF_FLAG(SOF_TIMESTAMPING_RX_SOFTWARE),
59 	SOF_FLAG(SOF_TIMESTAMPING_RX_HARDWARE),
60 };
61 
62 static struct socket_type socket_types[] = {
63 	{ "ip",		SOCK_RAW,	IPPROTO_EGP },
64 	{ "udp",	SOCK_DGRAM,	IPPROTO_UDP },
65 	{ "tcp",	SOCK_STREAM,	IPPROTO_TCP },
66 };
67 
68 static struct test_case test_cases[] = {
69 	{ {}, {} },
70 	{
71 		{ .so_timestamp = 1 },
72 		{ .tstamp = true }
73 	},
74 	{
75 		{ .so_timestampns = 1 },
76 		{ .tstampns = true }
77 	},
78 	{
79 		{ .so_timestamp = 1, .so_timestampns = 1 },
80 		{ .tstampns = true }
81 	},
82 	{
83 		{ .so_timestamping = SOF_TIMESTAMPING_RX_SOFTWARE },
84 		{}
85 	},
86 	{
87 		/* Loopback device does not support hw timestamps. */
88 		{ .so_timestamping = SOF_TIMESTAMPING_RX_HARDWARE },
89 		{}
90 	},
91 	{
92 		{ .so_timestamping = SOF_TIMESTAMPING_SOFTWARE },
93 		.warn_on_fail = true
94 	},
95 	{
96 		{ .so_timestamping = SOF_TIMESTAMPING_RX_SOFTWARE
97 			| SOF_TIMESTAMPING_RX_HARDWARE },
98 		{}
99 	},
100 	{
101 		{ .so_timestamping = SOF_TIMESTAMPING_SOFTWARE
102 			| SOF_TIMESTAMPING_RX_SOFTWARE },
103 		{ .swtstamp = true }
104 	},
105 	{
106 		{ .so_timestamp = 1, .so_timestamping = SOF_TIMESTAMPING_SOFTWARE
107 			| SOF_TIMESTAMPING_RX_SOFTWARE },
108 		{ .tstamp = true, .swtstamp = true }
109 	},
110 };
111 
112 static struct option long_options[] = {
113 	{ "list_tests", no_argument, 0, 'l' },
114 	{ "test_num", required_argument, 0, 'n' },
115 	{ "op_size", required_argument, 0, 's' },
116 	{ "tcp", no_argument, 0, 't' },
117 	{ "udp", no_argument, 0, 'u' },
118 	{ "ip", no_argument, 0, 'i' },
119 	{ "strict", no_argument, 0, 'S' },
120 	{ "ipv4", no_argument, 0, '4' },
121 	{ "ipv6", no_argument, 0, '6' },
122 	{ NULL, 0, NULL, 0 },
123 };
124 
125 static int next_port = 19999;
126 static int op_size = 10 * 1024;
127 
128 void print_test_case(struct test_case *t)
129 {
130 	int f = 0;
131 
132 	printf("sockopts {");
133 	if (t->sockopt.so_timestamp)
134 		printf(" SO_TIMESTAMP ");
135 	if (t->sockopt.so_timestampns)
136 		printf(" SO_TIMESTAMPNS ");
137 	if (t->sockopt.so_timestamping) {
138 		printf(" SO_TIMESTAMPING: {");
139 		for (f = 0; f < ARRAY_SIZE(sof_flags); f++)
140 			if (t->sockopt.so_timestamping & sof_flags[f].mask)
141 				printf(" %s |", sof_flags[f].name);
142 		printf("}");
143 	}
144 	printf("} expected cmsgs: {");
145 	if (t->expected.tstamp)
146 		printf(" SCM_TIMESTAMP ");
147 	if (t->expected.tstampns)
148 		printf(" SCM_TIMESTAMPNS ");
149 	if (t->expected.swtstamp || t->expected.hwtstamp) {
150 		printf(" SCM_TIMESTAMPING {");
151 		if (t->expected.swtstamp)
152 			printf("0");
153 		if (t->expected.swtstamp && t->expected.hwtstamp)
154 			printf(",");
155 		if (t->expected.hwtstamp)
156 			printf("2");
157 		printf("}");
158 	}
159 	printf("}\n");
160 }
161 
162 void do_send(int src)
163 {
164 	int r;
165 	char *buf = malloc(op_size);
166 
167 	memset(buf, 'z', op_size);
168 	r = write(src, buf, op_size);
169 	if (r < 0)
170 		error(1, errno, "Failed to sendmsg");
171 
172 	free(buf);
173 }
174 
175 bool do_recv(int rcv, int read_size, struct tstamps expected)
176 {
177 	const int CMSG_SIZE = 1024;
178 
179 	struct scm_timestamping *ts;
180 	struct tstamps actual = {};
181 	char cmsg_buf[CMSG_SIZE];
182 	struct iovec recv_iov;
183 	struct cmsghdr *cmsg;
184 	bool failed = false;
185 	struct msghdr hdr;
186 	int flags = 0;
187 	int r;
188 
189 	memset(&hdr, 0, sizeof(hdr));
190 	hdr.msg_iov = &recv_iov;
191 	hdr.msg_iovlen = 1;
192 	recv_iov.iov_base = malloc(read_size);
193 	recv_iov.iov_len = read_size;
194 
195 	hdr.msg_control = cmsg_buf;
196 	hdr.msg_controllen = sizeof(cmsg_buf);
197 
198 	r = recvmsg(rcv, &hdr, flags);
199 	if (r < 0)
200 		error(1, errno, "Failed to recvmsg");
201 	if (r != read_size)
202 		error(1, 0, "Only received %d bytes of payload.", r);
203 
204 	if (hdr.msg_flags & (MSG_TRUNC | MSG_CTRUNC))
205 		error(1, 0, "Message was truncated.");
206 
207 	for (cmsg = CMSG_FIRSTHDR(&hdr); cmsg != NULL;
208 	     cmsg = CMSG_NXTHDR(&hdr, cmsg)) {
209 		if (cmsg->cmsg_level != SOL_SOCKET)
210 			error(1, 0, "Unexpected cmsg_level %d",
211 			      cmsg->cmsg_level);
212 		switch (cmsg->cmsg_type) {
213 		case SCM_TIMESTAMP:
214 			actual.tstamp = true;
215 			break;
216 		case SCM_TIMESTAMPNS:
217 			actual.tstampns = true;
218 			break;
219 		case SCM_TIMESTAMPING:
220 			ts = (struct scm_timestamping *)CMSG_DATA(cmsg);
221 			actual.swtstamp = !!ts->ts[0].tv_sec;
222 			if (ts->ts[1].tv_sec != 0)
223 				error(0, 0, "ts[1] should not be set.");
224 			actual.hwtstamp = !!ts->ts[2].tv_sec;
225 			break;
226 		default:
227 			error(1, 0, "Unexpected cmsg_type %d", cmsg->cmsg_type);
228 		}
229 	}
230 
231 #define VALIDATE(field) \
232 	do { \
233 		if (expected.field != actual.field) { \
234 			if (expected.field) \
235 				error(0, 0, "Expected " #field " to be set."); \
236 			else \
237 				error(0, 0, \
238 				      "Expected " #field " to not be set."); \
239 			failed = true; \
240 		} \
241 	} while (0)
242 
243 	VALIDATE(tstamp);
244 	VALIDATE(tstampns);
245 	VALIDATE(swtstamp);
246 	VALIDATE(hwtstamp);
247 #undef VALIDATE
248 
249 	free(recv_iov.iov_base);
250 
251 	return failed;
252 }
253 
254 void config_so_flags(int rcv, struct options o)
255 {
256 	int on = 1;
257 
258 	if (setsockopt(rcv, SOL_SOCKET, SO_REUSEADDR, &on, sizeof(on)) < 0)
259 		error(1, errno, "Failed to enable SO_REUSEADDR");
260 
261 	if (o.so_timestamp &&
262 	    setsockopt(rcv, SOL_SOCKET, SO_TIMESTAMP,
263 		       &o.so_timestamp, sizeof(o.so_timestamp)) < 0)
264 		error(1, errno, "Failed to enable SO_TIMESTAMP");
265 
266 	if (o.so_timestampns &&
267 	    setsockopt(rcv, SOL_SOCKET, SO_TIMESTAMPNS,
268 		       &o.so_timestampns, sizeof(o.so_timestampns)) < 0)
269 		error(1, errno, "Failed to enable SO_TIMESTAMPNS");
270 
271 	if (o.so_timestamping &&
272 	    setsockopt(rcv, SOL_SOCKET, SO_TIMESTAMPING,
273 		       &o.so_timestamping, sizeof(o.so_timestamping)) < 0)
274 		error(1, errno, "Failed to set SO_TIMESTAMPING");
275 }
276 
277 bool run_test_case(struct socket_type *s, int test_num, char ip_version,
278 		   bool strict)
279 {
280 	union {
281 		struct sockaddr_in6 addr6;
282 		struct sockaddr_in addr4;
283 		struct sockaddr addr_un;
284 	} addr;
285 	int read_size = op_size;
286 	int src, dst, rcv, port;
287 	socklen_t addr_size;
288 	bool failed = false;
289 
290 	port = (s->type == SOCK_RAW) ? 0 : next_port++;
291 	memset(&addr, 0, sizeof(addr));
292 	if (ip_version == '4') {
293 		addr.addr4.sin_family = AF_INET;
294 		addr.addr4.sin_addr.s_addr = htonl(INADDR_LOOPBACK);
295 		addr.addr4.sin_port = htons(port);
296 		addr_size = sizeof(addr.addr4);
297 		if (s->type == SOCK_RAW)
298 			read_size += 20;  /* for IPv4 header */
299 	} else {
300 		addr.addr6.sin6_family = AF_INET6;
301 		addr.addr6.sin6_addr = in6addr_loopback;
302 		addr.addr6.sin6_port = htons(port);
303 		addr_size = sizeof(addr.addr6);
304 	}
305 	printf("Starting testcase %d over ipv%c...\n", test_num, ip_version);
306 	src = socket(addr.addr_un.sa_family, s->type,
307 		     s->protocol);
308 	if (src < 0)
309 		error(1, errno, "Failed to open src socket");
310 
311 	dst = socket(addr.addr_un.sa_family, s->type,
312 		     s->protocol);
313 	if (dst < 0)
314 		error(1, errno, "Failed to open dst socket");
315 
316 	if (bind(dst, &addr.addr_un, addr_size) < 0)
317 		error(1, errno, "Failed to bind to port %d", port);
318 
319 	if (s->type == SOCK_STREAM && (listen(dst, 1) < 0))
320 		error(1, errno, "Failed to listen");
321 
322 	if (connect(src, &addr.addr_un, addr_size) < 0)
323 		error(1, errno, "Failed to connect");
324 
325 	if (s->type == SOCK_STREAM) {
326 		rcv = accept(dst, NULL, NULL);
327 		if (rcv < 0)
328 			error(1, errno, "Failed to accept");
329 		close(dst);
330 	} else {
331 		rcv = dst;
332 	}
333 
334 	config_so_flags(rcv, test_cases[test_num].sockopt);
335 	usleep(20000); /* setsockopt for SO_TIMESTAMPING is asynchronous */
336 	do_send(src);
337 
338 	failed = do_recv(rcv, read_size, test_cases[test_num].expected);
339 
340 	close(rcv);
341 	close(src);
342 
343 	if (failed) {
344 		printf("FAILURE in testcase %d over ipv%c ", test_num,
345 		       ip_version);
346 		print_test_case(&test_cases[test_num]);
347 		if (!strict && test_cases[test_num].warn_on_fail)
348 			failed = false;
349 	}
350 	return failed;
351 }
352 
353 int main(int argc, char **argv)
354 {
355 	bool all_protocols = true;
356 	bool all_tests = true;
357 	bool cfg_ipv4 = false;
358 	bool cfg_ipv6 = false;
359 	bool strict = false;
360 	int arg_index = 0;
361 	int failures = 0;
362 	int s, t, opt;
363 
364 	while ((opt = getopt_long(argc, argv, "", long_options,
365 				  &arg_index)) != -1) {
366 		switch (opt) {
367 		case 'l':
368 			for (t = 0; t < ARRAY_SIZE(test_cases); t++) {
369 				printf("%d\t", t);
370 				print_test_case(&test_cases[t]);
371 			}
372 			return 0;
373 		case 'n':
374 			t = atoi(optarg);
375 			if (t >= ARRAY_SIZE(test_cases))
376 				error(1, 0, "Invalid test case: %d", t);
377 			all_tests = false;
378 			test_cases[t].enabled = true;
379 			break;
380 		case 's':
381 			op_size = atoi(optarg);
382 			break;
383 		case 't':
384 			all_protocols = false;
385 			socket_types[2].enabled = true;
386 			break;
387 		case 'u':
388 			all_protocols = false;
389 			socket_types[1].enabled = true;
390 			break;
391 		case 'i':
392 			all_protocols = false;
393 			socket_types[0].enabled = true;
394 			break;
395 		case 'S':
396 			strict = true;
397 			break;
398 		case '4':
399 			cfg_ipv4 = true;
400 			break;
401 		case '6':
402 			cfg_ipv6 = true;
403 			break;
404 		default:
405 			error(1, 0, "Failed to parse parameters.");
406 		}
407 	}
408 
409 	for (s = 0; s < ARRAY_SIZE(socket_types); s++) {
410 		if (!all_protocols && !socket_types[s].enabled)
411 			continue;
412 
413 		printf("Testing %s...\n", socket_types[s].friendly_name);
414 		for (t = 0; t < ARRAY_SIZE(test_cases); t++) {
415 			if (!all_tests && !test_cases[t].enabled)
416 				continue;
417 			if (cfg_ipv4 || !cfg_ipv6)
418 				if (run_test_case(&socket_types[s], t, '4',
419 						  strict))
420 					failures++;
421 			if (cfg_ipv6 || !cfg_ipv4)
422 				if (run_test_case(&socket_types[s], t, '6',
423 						  strict))
424 					failures++;
425 		}
426 	}
427 	if (!failures)
428 		printf("PASSED.\n");
429 	return failures;
430 }
431