expand txtimestamp selftest

index 81a98a2404563..ca5fad15a8554 100644
@@ -73,6 +73,7 @@ static bool cfg_show_payload;
 static bool cfg_do_pktinfo;
 static bool cfg_loop_nodata;
 static bool cfg_no_delay;
+static bool cfg_use_cmsg;
 static uint16_t dest_port = 9000;
 
 static struct sockaddr_in daddr;

@@ -269,8 +270,13 @@ static int recv_errmsg(int fd)
 	return ret == -1;
 }
 
-static void do_test(int family, unsigned int opt)
+static void do_test(int family, unsigned int report_opt)
 {
+	char control[CMSG_SPACE(sizeof(uint32_t))];
+	unsigned int sock_opt;
+	struct cmsghdr *cmsg;
+	struct msghdr msg;
+	struct iovec iov;
 	char *buf;
 	int fd, i, val = 1, total_len;
 

@@ -321,17 +327,22 @@ static void do_test(int family, unsigned int opt)
 		}
 	}
 
-	opt |= SOF_TIMESTAMPING_SOFTWARE |
-	       SOF_TIMESTAMPING_OPT_CMSG |
-	       SOF_TIMESTAMPING_OPT_ID;
+	sock_opt = SOF_TIMESTAMPING_SOFTWARE |
+		   SOF_TIMESTAMPING_OPT_CMSG |
+		   SOF_TIMESTAMPING_OPT_ID;
+
+	if (!cfg_use_cmsg)
+		sock_opt |= report_opt;
+
 	if (cfg_loop_nodata)
-		opt |= SOF_TIMESTAMPING_OPT_TSONLY;
+		sock_opt |= SOF_TIMESTAMPING_OPT_TSONLY;
 
 	if (setsockopt(fd, SOL_SOCKET, SO_TIMESTAMPING,
-		       (char *) &opt, sizeof(opt)))
+		       (char *) &sock_opt, sizeof(sock_opt)))
 		error(1, 0, "setsockopt timestamping");
 
 	for (i = 0; i < cfg_num_pkts; i++) {
+		memset(&msg, 0, sizeof(msg));
 		memset(&ts_prev, 0, sizeof(ts_prev));
 		memset(buf, 'a' + i, total_len);
 

@@ -361,14 +372,38 @@ static void do_test(int family, unsigned int opt)
 		}
 
 		print_timestamp_usr();
+
+		iov.iov_base = buf;
+		iov.iov_len = total_len;
+
 		if (cfg_proto != SOCK_STREAM) {
-			if (family == PF_INET)
-				val = sendto(fd, buf, total_len, 0, (void *) &daddr, sizeof(daddr));
-			else
-				val = sendto(fd, buf, total_len, 0, (void *) &daddr6, sizeof(daddr6));
-		} else {
-			val = send(fd, buf, cfg_payload_len, 0);
+			if (family == PF_INET) {
+				msg.msg_name = (void *)&daddr;
+				msg.msg_namelen = sizeof(daddr);
+			} else {
+				msg.msg_name = (void *)&daddr6;
+				msg.msg_namelen = sizeof(daddr6);
+			}
+		}
+
+		msg.msg_iov = &iov;
+		msg.msg_iovlen = 1;
+
+		if (cfg_use_cmsg) {
+			memset(control, 0, sizeof(control));
+
+			msg.msg_control = control;
+			msg.msg_controllen = sizeof(control);
+
+			cmsg = CMSG_FIRSTHDR(&msg);
+			cmsg->cmsg_level = SOL_SOCKET;
+			cmsg->cmsg_type = SO_TIMESTAMPING;
+			cmsg->cmsg_len = CMSG_LEN(sizeof(uint32_t));
+
+			*((uint32_t *) CMSG_DATA(cmsg)) = report_opt;
 		}
+
+		val = sendmsg(fd, &msg, 0);
 		if (val != total_len)
 			error(1, errno, "send");
 

@@ -396,6 +431,7 @@ static void __attribute__((noreturn)) usage(const char *filepath)
 			"  -6:   only IPv6\n"
 			"  -h:   show this message\n"
 			"  -c N: number of packets for each test\n"
+			"  -C:   use cmsg to set tstamp recording options\n"
 			"  -D:   no delay between packets\n"
 			"  -F:   poll() waits forever for an event\n"
 			"  -I:   request PKTINFO\n"

@@ -413,9 +449,9 @@ static void __attribute__((noreturn)) usage(const char *filepath)
 static void parse_opt(int argc, char **argv)
 {
 	int proto_count = 0;
-	char c;
+	int c;
 
-	while ((c = getopt(argc, argv, "46c:DFhIl:np:rRux")) != -1) {
+	while ((c = getopt(argc, argv, "46c:CDFhIl:np:rRux")) != -1) {
 		switch (c) {
 		case '4':
 			do_ipv6 = 0;

@@ -426,6 +462,9 @@ static void parse_opt(int argc, char **argv)
 		case 'c':
 			cfg_num_pkts = strtoul(optarg, NULL, 10);
 			break;
+		case 'C':
+			cfg_use_cmsg = true;
+			break;
 		case 'D':
 			cfg_no_delay = true;
 			break;

index ca5fad15a8554..ecc1db5dcfcad 100644
@@ -39,6 +39,7 @@
 #include <inttypes.h>
 #include <linux/errqueue.h>
 #include <linux/if_ether.h>
+#include <linux/ipv6.h>
 #include <linux/net_tstamp.h>
 #include <netdb.h>
 #include <net/if.h>

@@ -74,6 +75,7 @@ static bool cfg_do_pktinfo;
 static bool cfg_loop_nodata;
 static bool cfg_no_delay;
 static bool cfg_use_cmsg;
+static bool cfg_use_pf_packet;
 static uint16_t dest_port = 9000;
 
 static struct sockaddr_in daddr;

@@ -195,7 +197,9 @@ static void __recv_errmsg_cmsg(struct msghdr *msg, int payload_len)
 		} else if ((cm->cmsg_level == SOL_IP &&
 			    cm->cmsg_type == IP_RECVERR) ||
 			   (cm->cmsg_level == SOL_IPV6 &&
-			    cm->cmsg_type == IPV6_RECVERR)) {
+			    cm->cmsg_type == IPV6_RECVERR) ||
+			   (cm->cmsg_level = SOL_PACKET &&
+			    cm->cmsg_type == PACKET_TX_TIMESTAMP)) {
 			serr = (void *) CMSG_DATA(cm);
 			if (serr->ee_errno != ENOMSG ||
 			    serr->ee_origin != SO_EE_ORIGIN_TIMESTAMPING) {

@@ -270,9 +274,89 @@ static int recv_errmsg(int fd)
 	return ret == -1;
 }
 
+static uint16_t get_ip_csum(const uint16_t *start, int num_words,
+			    unsigned long sum)
+{
+	int i;
+
+	for (i = 0; i < num_words; i++)
+		sum += start[i];
+
+	while (sum >> 16)
+		sum = (sum & 0xFFFF) + (sum >> 16);
+
+	return ~sum;
+}
+
+static uint16_t get_udp_csum(const struct udphdr *udph, int alen)
+{
+	unsigned long pseudo_sum, csum_len;
+	const void *csum_start = udph;
+
+	pseudo_sum = htons(IPPROTO_UDP);
+	pseudo_sum += udph->len;
+
+	/* checksum ip(v6) addresses + udp header + payload */
+	csum_start -= alen * 2;
+	csum_len = ntohs(udph->len) + alen * 2;
+
+	return get_ip_csum(csum_start, csum_len >> 1, pseudo_sum);
+}
+
+static int fill_header_ipv4(void *p)
+{
+	struct iphdr *iph = p;
+
+	memset(iph, 0, sizeof(*iph));
+
+	iph->ihl	= 5;
+	iph->version	= 4;
+	iph->ttl	= 2;
+	iph->saddr	= daddr.sin_addr.s_addr;	/* set for udp csum calc */
+	iph->daddr	= daddr.sin_addr.s_addr;
+	iph->protocol	= IPPROTO_UDP;
+
+	/* kernel writes saddr, csum, len */
+
+	return sizeof(*iph);
+}
+
+static int fill_header_ipv6(void *p)
+{
+	struct ipv6hdr *ip6h = p;
+
+	memset(ip6h, 0, sizeof(*ip6h));
+
+	ip6h->version		= 6;
+	ip6h->payload_len	= htons(sizeof(struct udphdr) + cfg_payload_len);
+	ip6h->nexthdr		= IPPROTO_UDP;
+	ip6h->hop_limit		= 64;
+
+	ip6h->saddr             = daddr6.sin6_addr;
+	ip6h->daddr		= daddr6.sin6_addr;
+
+	/* kernel does not write saddr in case of ipv6 */
+
+	return sizeof(*ip6h);
+}
+
+static void fill_header_udp(void *p, bool is_ipv4)
+{
+	struct udphdr *udph = p;
+
+	udph->source = ntohs(dest_port + 1);	/* spoof */
+	udph->dest   = ntohs(dest_port);
+	udph->len    = ntohs(sizeof(*udph) + cfg_payload_len);
+	udph->check  = 0;
+
+	udph->check  = get_udp_csum(udph, is_ipv4 ? sizeof(struct in_addr) :
+						    sizeof(struct in6_addr));
+}
+
 static void do_test(int family, unsigned int report_opt)
 {
 	char control[CMSG_SPACE(sizeof(uint32_t))];
+	struct sockaddr_ll laddr;
 	unsigned int sock_opt;
 	struct cmsghdr *cmsg;
 	struct msghdr msg;

@@ -280,24 +364,28 @@ static void do_test(int family, unsigned int report_opt)
 	char *buf;
 	int fd, i, val = 1, total_len;
 
-	if (family == AF_INET6 && cfg_proto != SOCK_STREAM) {
-		/* due to lack of checksum generation code */
-		fprintf(stderr, "test: skipping datagram over IPv6\n");
-		return;
-	}
-
 	total_len = cfg_payload_len;
-	if (cfg_proto == SOCK_RAW) {
+	if (cfg_use_pf_packet || cfg_proto == SOCK_RAW) {
 		total_len += sizeof(struct udphdr);
-		if (cfg_ipproto == IPPROTO_RAW)
-			total_len += sizeof(struct iphdr);
+		if (cfg_use_pf_packet || cfg_ipproto == IPPROTO_RAW)
+			if (family == PF_INET)
+				total_len += sizeof(struct iphdr);
+			else
+				total_len += sizeof(struct ipv6hdr);
+
+		/* special case, only rawv6_sendmsg:
+		 * pass proto in sin6_port if not connected
+		 * also see ANK comment in net/ipv4/raw.c
+		 */
+		daddr6.sin6_port = htons(cfg_ipproto);
 	}
 
 	buf = malloc(total_len);
 	if (!buf)
 		error(1, 0, "malloc");
 
-	fd = socket(family, cfg_proto, cfg_ipproto);
+	fd = socket(cfg_use_pf_packet ? PF_PACKET : family,
+		    cfg_proto, cfg_ipproto);
 	if (fd < 0)
 		error(1, errno, "socket");
 

@@ -346,29 +434,17 @@ static void do_test(int family, unsigned int report_opt)
 		memset(&ts_prev, 0, sizeof(ts_prev));
 		memset(buf, 'a' + i, total_len);
 
-		if (cfg_proto == SOCK_RAW) {
-			struct udphdr *udph;
+		if (cfg_use_pf_packet || cfg_proto == SOCK_RAW) {
 			int off = 0;
 
-			if (cfg_ipproto == IPPROTO_RAW) {
-				struct iphdr *iph = (void *) buf;
-
-				memset(iph, 0, sizeof(*iph));
-				iph->ihl      = 5;
-				iph->version  = 4;
-				iph->ttl      = 2;
-				iph->daddr    = daddr.sin_addr.s_addr;
-				iph->protocol = IPPROTO_UDP;
-				/* kernel writes saddr, csum, len */
-
-				off = sizeof(*iph);
+			if (cfg_use_pf_packet || cfg_ipproto == IPPROTO_RAW) {
+				if (family == PF_INET)
+					off = fill_header_ipv4(buf);
+				else
+					off = fill_header_ipv6(buf);
 			}
 
-			udph = (void *) buf + off;
-			udph->source = ntohs(9000); 	/* random spoof */
-			udph->dest   = ntohs(dest_port);
-			udph->len    = ntohs(sizeof(*udph) + cfg_payload_len);
-			udph->check  = 0;	/* not allowed for IPv6 */
+			fill_header_udp(buf + off, family == PF_INET);
 		}
 
 		print_timestamp_usr();

@@ -377,7 +453,17 @@ static void do_test(int family, unsigned int report_opt)
 		iov.iov_len = total_len;
 
 		if (cfg_proto != SOCK_STREAM) {
-			if (family == PF_INET) {
+			if (cfg_use_pf_packet) {
+				memset(&laddr, 0, sizeof(laddr));
+
+				laddr.sll_family	= AF_PACKET;
+				laddr.sll_ifindex	= 1;
+				laddr.sll_protocol	= htons(family == AF_INET ? ETH_P_IP : ETH_P_IPV6);
+				laddr.sll_halen		= ETH_ALEN;
+
+				msg.msg_name = (void *)&laddr;
+				msg.msg_namelen = sizeof(laddr);
+			} else if (family == PF_INET) {
 				msg.msg_name = (void *)&daddr;
 				msg.msg_namelen = sizeof(daddr);
 			} else {

@@ -437,9 +523,10 @@ static void __attribute__((noreturn)) usage(const char *filepath)
 			"  -I:   request PKTINFO\n"
 			"  -l N: send N bytes at a time\n"
 			"  -n:   set no-payload option\n"
+			"  -p N: connect to port N\n"
+			"  -P:   use PF_PACKET\n"
 			"  -r:   use raw\n"
 			"  -R:   use raw (IP_HDRINCL)\n"
-			"  -p N: connect to port N\n"
 			"  -u:   use udp\n"
 			"  -x:   show payload (up to 70 bytes)\n",
 			filepath);

@@ -451,7 +538,7 @@ static void parse_opt(int argc, char **argv)
 	int proto_count = 0;
 	int c;
 
-	while ((c = getopt(argc, argv, "46c:CDFhIl:np:rRux")) != -1) {
+	while ((c = getopt(argc, argv, "46c:CDFhIl:np:PrRux")) != -1) {
 		switch (c) {
 		case '4':
 			do_ipv6 = 0;

@@ -474,9 +561,21 @@ static void parse_opt(int argc, char **argv)
 		case 'I':
 			cfg_do_pktinfo = true;
 			break;
+		case 'l':
+			cfg_payload_len = strtoul(optarg, NULL, 10);
+			break;
 		case 'n':
 			cfg_loop_nodata = true;
 			break;
+		case 'p':
+			dest_port = strtoul(optarg, NULL, 10);
+			break;
+		case 'P':
+			proto_count++;
+			cfg_use_pf_packet = true;
+			cfg_proto = SOCK_DGRAM;
+			cfg_ipproto = 0;
+			break;
 		case 'r':
 			proto_count++;
 			cfg_proto = SOCK_RAW;

@@ -492,12 +591,6 @@ static void parse_opt(int argc, char **argv)
 			cfg_proto = SOCK_DGRAM;
 			cfg_ipproto = IPPROTO_UDP;
 			break;
-		case 'l':
-			cfg_payload_len = strtoul(optarg, NULL, 10);
-			break;
-		case 'p':
-			dest_port = strtoul(optarg, NULL, 10);
-			break;
 		case 'x':
 			cfg_show_payload = true;
 			break;

@@ -514,7 +607,9 @@ static void parse_opt(int argc, char **argv)
 	if (!do_ipv4 && !do_ipv6)
 		error(1, 0, "pass -4 or -6, not both");
 	if (proto_count > 1)
-		error(1, 0, "pass -r, -R or -u, not multiple");
+		error(1, 0, "pass -P, -r, -R or -u, not multiple");
+	if (cfg_do_pktinfo && cfg_use_pf_packet)
+		error(1, 0, "cannot ask for pktinfo over pf_packet");
 
 	if (optind != argc - 1)
 		error(1, 0, "missing required hostname argument");

@@ -522,10 +617,12 @@ static void parse_opt(int argc, char **argv)
 
 static void resolve_hostname(const char *hostname)
 {
+	struct addrinfo hints = { .ai_family = do_ipv4 ? AF_INET : AF_INET6 };
 	struct addrinfo *addrs, *cur;
 	int have_ipv4 = 0, have_ipv6 = 0;
 
-	if (getaddrinfo(hostname, NULL, NULL, &addrs))
+retry:
+	if (getaddrinfo(hostname, NULL, &hints, &addrs))
 		error(1, errno, "getaddrinfo");
 
 	cur = addrs;

@@ -545,14 +642,20 @@ static void resolve_hostname(const char *hostname)
 	if (addrs)
 		freeaddrinfo(addrs);
 
+	if (do_ipv6 && hints.ai_family != AF_INET6) {
+		hints.ai_family = AF_INET6;
+		goto retry;
+	}
+
 	do_ipv4 &= have_ipv4;
 	do_ipv6 &= have_ipv6;
 }
 
 static void do_main(int family)
 {
-	fprintf(stderr, "family:       %s\n",
-			family == PF_INET ? "INET" : "INET6");
+	fprintf(stderr, "family:       %s %s\n",
+			family == PF_INET ? "INET" : "INET6",
+			cfg_use_pf_packet ? "(PF_PACKET)" : "");
 
 	fprintf(stderr, "test SND\n");
 	do_test(family, SOF_TIMESTAMPING_TX_SOFTWARE);

241 lines not on this page · raw message