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#include "../kselftest.h"
22
23struct options {
24	int so_timestamp;
25	int so_timestampns;
26	int so_timestamping;
27};
28
29struct tstamps {
30	bool tstamp;
31	bool tstampns;
32	bool swtstamp;
33	bool hwtstamp;
34};
35
36struct socket_type {
37	char *friendly_name;
38	int type;
39	int protocol;
40	bool enabled;
41};
42
43struct test_case {
44	struct options sockopt;
45	struct tstamps expected;
46	bool enabled;
47	bool warn_on_fail;
48};
49
50struct sof_flag {
51	int mask;
52	char *name;
53};
54
55static 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
62static 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
68static 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
112static 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
125static int next_port = 19999;
126static int op_size = 10 * 1024;
127
128void 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
162void 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
175bool 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
254void 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
277bool 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
353int 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