/*
 * Copyright 2012 Google Inc.
 * Author: Willem de Bruijn (willemb@google.com)
 *
 * A self-contained test to observe whether a device supports tx timestamping.
 *
 * If testing on a machine with multiple types of network cards, be aware
 * that the test does not exercise a particular card (say, eth0), so YMMV.
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms and conditions of the GNU General Public License,
 * version 2, as published by the Free Software Foundation.
 *
 * This program is distributed in the hope it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE. * See the GNU General Public License for
 * more details.
 *
 * You should have received a copy of the GNU General Public License along with
 * this program; if not, write to the Free Software Foundation, Inc.,
 * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
 */

#include <arpa/inet.h>
#include <errno.h>
#include <linux/errqueue.h>
#include <linux/if_ether.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/socket.h>
#include <sys/time.h>
#include <sys/types.h>
#include <net/if.h>
#include <unistd.h>

#ifdef HAVE_RECENT_HEADERS
#include <linux/net_tstamp.h>
#else
#define SOF_TIMESTAMPING_TX_SOFTWARE	(1 << 1)
#define SOF_TIMESTAMPING_SOFTWARE	(1 << 4)
#endif

#define NUM_RUNS	10
#define RUN_IVAL_USECS	10000

const char payload[] = "abcabcabcabcabc";

static int do_send(int fd)
{
	struct sockaddr_in daddr;
	struct timeval tv;
	int ret;

	/* Send to a reserved (and thus unused) multicast address */
	memset(&daddr, 0, sizeof(daddr));
	daddr.sin_family	= AF_INET;
	daddr.sin_port		= htons(8000);
	daddr.sin_addr.s_addr	= inet_addr("224.0.0.69");

	gettimeofday(&tv, NULL);
	printf("time (app):    %lus.%luus\n", tv.tv_sec, tv.tv_usec);
	ret = sendto(fd, payload, sizeof(payload), 0,
		     (void *) &daddr, sizeof(daddr));
	if (ret != sizeof(payload)) {
		perror("sendto");
		return 1;
	}

	return 0;
}

static void show_cmsg(struct msghdr *msg, char *bounced_payload)
{
	struct sock_extended_err *err;
	struct cmsghdr *cmsg;
	struct timespec *stamp;

	for (cmsg = CMSG_FIRSTHDR(msg); cmsg; cmsg = CMSG_NXTHDR(msg, cmsg)) {
		/* Timestamp */
		if (cmsg->cmsg_level == SOL_SOCKET &&
		    cmsg->cmsg_type == SCM_TIMESTAMPING) {
			stamp = (void *) CMSG_DATA(cmsg);
			printf("time (kernel): %lds.%06ldus\n",
			       stamp->tv_sec, stamp->tv_nsec / 1000);
		}
		/* Error message, which is how the timestamp is delivered */
		else if (cmsg->cmsg_level == IPPROTO_IP &&
			 cmsg->cmsg_type == IP_RECVERR) {
			err = (void *) CMSG_DATA(cmsg);
			if (err->ee_origin != SO_EE_ORIGIN_TIMESTAMPING)
				fprintf(stderr, "data: recv error [%s]\n",
					strerror(err->ee_errno));
			else if (memcmp(bounced_payload, payload,
					sizeof(payload)))
				fprintf(stderr, "data: wrong packet\n");
			else
				printf("data:          %luB bounced okay\n",
				       sizeof(payload));
			
		}
		else
			fprintf(stderr, "unexpected control message %d.%d\n",
				cmsg->cmsg_level, cmsg->cmsg_type);
	}

}

static int do_recv(int fd)
{
	char data[ETH_DATA_LEN];
	struct msghdr msg;
	struct iovec entry;
	struct {
		struct cmsghdr cm;
		char control[512];
	} control;
	int ret;

	entry.iov_base = data;
	entry.iov_len = sizeof(data);

	memset(&msg, 0, sizeof(msg));
	msg.msg_iov = &entry;
	msg.msg_iovlen = 1;
	msg.msg_name = NULL;
	msg.msg_namelen = 0;
	msg.msg_control = &control;
	msg.msg_controllen = sizeof(control);

	ret = recvmsg(fd, &msg, MSG_ERRQUEUE);
	if (ret == -1) {
		if (errno == EAGAIN)
			return 1;
		perror("recvmsg");
		exit(1);
	}

	show_cmsg(&msg, data + ret - sizeof(payload));
	return 0;
}

int main(int argc, char **argv)
{
	int fd, flags, i, ret;

	/* Setup timed socket */
	fd = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
	if (fd < 0) {
		perror("socket");
		return 1;
	}

	flags = SOF_TIMESTAMPING_TX_SOFTWARE | SOF_TIMESTAMPING_SOFTWARE;
	if (setsockopt(fd, SOL_SOCKET, SO_TIMESTAMPING,
		       &flags, sizeof(flags))) {
		perror("setsockopt timestamping");
		return 1;
	}

	/* Send a few packets */
	for (i = 0; i < NUM_RUNS; i++) {
		if (do_send(fd))
			return 1;
		ret = do_recv(fd);
		if (ret)
			break;
		usleep(RUN_IVAL_USECS);	/* Probably not very precise */
	}

	if (close(fd)) {
		perror("close");
		return 1;
	}

	if (ret)
		printf("Tx timestamping: Not supported\n");
	else
		printf("Tx timestamping: OK\n");

	return ret;
}