/* ntptest.c   by Michael Thorpe   2013-07-08 */

#include <arpa/inet.h>
#include <ctype.h>
#include <netdb.h>
#include <stdio.h>
#include <string.h>
#include <sys/socket.h>
#include <sys/time.h>
#include <sys/types.h>
#include <unistd.h>
/* For IPTOS_LOWDELAY */
#include <netinet/ip.h>

static int sock;
static struct sockaddr sa;
static char txbuf[48];
static char rxbuf[48];
static char rxtime[8];

static void tv2timestamp(const struct timeval *tv,char *s) {
	unsigned int i;

	i=tv->tv_sec+2208988800U; /* seconds between 1900-01-01 and 1970-01-01 */
	*(unsigned int *)s=htonl(i&0xFFFFFFFF);
	*(unsigned int *)(&s[4])=htonl(tv->tv_usec*4294.967296); /* 2^32/1e6 */
}

static double timestamp2double(const char *s) {
	double out;

/* We could use ntohl twice, but there can be problems when 4==sizeof(long) */
	out=(unsigned char)s[7]*.00390625; /* .00390625 is 2^-8 */
	out=(out+(unsigned char)s[6])*.00390625;
	out=(out+(unsigned char)s[5])*.00390625;
	out=(out+(unsigned char)s[4])*.00390625;
	out+=ntohl(*(unsigned int *)s);
	return(out);
}

static int opensock(char *hostname) {
	struct addrinfo ai,*out;

	sock=socket(AF_INET,SOCK_DGRAM,0);
	if(sock<0) {
		perror("socket");
		return(1);
	}
	memset(&ai,0,sizeof(ai));
	ai.ai_flags=0;
	ai.ai_family=AF_INET;
	ai.ai_socktype=SOCK_DGRAM;
	ai.ai_protocol=0;
	if(getaddrinfo(hostname,"ntp",&ai,&out)) {
		perror("getaddrinfo");
		return(2);
	}
	if(!out) {
		fprintf(stderr,"Couldn't lookup address for host \"%s\"\n",hostname);
		return(3);
	}
	memcpy(&sa,out->ai_addr,out->ai_addrlen);
	freeaddrinfo(out);
	return(0);
}

static int sendpacket() {
	struct timeval tv;

	memset(txbuf,0,sizeof(txbuf));
	txbuf[0]=0x13; /* 2-bit LI=0, 3-bit VN=2, 3-bit Mode=3 */
	txbuf[1]=0; /* Stratum */
	txbuf[2]=0; /* Poll */
	txbuf[3]=0; /* Precision */
/* txbuf[4..7] is Root Delay */
/* txbuf[8..11] is Root Dispersion */
/* txbuf[12..15] is Reference Identifier */
/* Then the 64-bit Reference, Originate, Receive, and Transmit Timestamps */
/* We only need to (optionally) set Transmit[40..47] */
	if(gettimeofday(&tv,0)) {
		perror("gettimeofday");
		return(1);
	}
	tv2timestamp(&tv,&txbuf[40]);
	if(-1==sendto(sock,txbuf,sizeof(txbuf),0,&sa,sizeof(sa))) {
		perror("sendto");
		return(2);
	}
	return(0);
}

static int recvpacket() {
	struct timeval tv;
	ssize_t l;
	socklen_t addrlen;

/* It would be better to use select with a timeout here */
	alarm(5);
	addrlen=sizeof(sa);
	l=recvfrom(sock,rxbuf,sizeof(rxbuf),0,&sa,&addrlen);
	if(-1==l) {
		perror("recvfrom");
		alarm(0);
		return(1);
	}
	if(48 != l) {
		alarm(0);
		fputs("Got short packet from server\n",stderr);
		return(2);
	}
	if(gettimeofday(&tv,0)) {
		perror("gettimeofday");
		return(3);
	}
	alarm(0);
	tv2timestamp(&tv,rxtime);
	return(0);
}

static void showpacket() {
	double t1,t2,t3,t4;
	double delay,offset;
	unsigned long tmpl;
	double rootdelay;
	int i;

	printf("LI=%u, VN=%u, Mode=%u, Stratum=%u, Poll=2^%u, Precision=2^%d\n",rxbuf[0]>>6,(rxbuf[0]>>3)&7,rxbuf[0]&7,(unsigned int)(unsigned char)rxbuf[1],(unsigned int)(unsigned char)rxbuf[2],(signed int)(signed char)rxbuf[3]);
	tmpl=ntohl(*(unsigned int *)(&rxbuf[4]));
	rootdelay=tmpl*.0000152587890625; /* 2^-16 */
	if(tmpl&0x80000000)
		rootdelay-=0x8000;
	printf("Root Delay=%f, Root Dispersion=%f\n",rootdelay,ntohl(*(unsigned int *)(&rxbuf[8]))*.0000152587890625); /* 2^-16 */
	fputs("Reference Identifier: ",stdout);
	if(rxbuf[1]<2) {
		for(i=0;i<4;i++) {
			if('\0' != rxbuf[12+i] && !isprint(rxbuf[12+i]))
				goto REFID_NOT_PRINTABLE;
			if(0<i && '\0' != rxbuf[12+i] && '\0'==rxbuf[12+i-1])
				goto REFID_NOT_PRINTABLE;
		}
		putchar('.');
		for(i=0;i<4;i++) {
			if('\0'==rxbuf[12+i])
				break;
			putchar(rxbuf[12+i]);
		}
		putchar('.');
		if(0)
REFID_NOT_PRINTABLE:
			printf("0x%8.8X",ntohl(*(unsigned int *)(&rxbuf[12])));
	} else {
		printf("%u.%u.%u.%u",(unsigned int)(unsigned char)rxbuf[12],(unsigned int)(unsigned char)rxbuf[13],(unsigned int)(unsigned char)rxbuf[14],(unsigned int)(unsigned char)rxbuf[15]);
	}
	putchar('\n');
	printf("  Reference Timestamp: %f\n",timestamp2double(&rxbuf[16]));
	printf("  Originate Timestamp: %f\n",t1=timestamp2double(&rxbuf[24]));
	printf("    Receive Timestamp: %f\n",t2=timestamp2double(&rxbuf[32]));
	printf("   Transmit Timestamp: %f\n",t3=timestamp2double(&rxbuf[40]));
	printf("Destination Timestamp: %f\n",t4=timestamp2double(rxtime));
	delay=(t4-t1)-(t3-t2);
	offset=((t2-t1)+(t3-t4))/2;
	printf("Round trip delay: %fms\n",delay*1000);
	printf("Adjustment needed: %fms\n",offset*1000);
}

static void showoffset() {
	double t1,t2,t3,t4;
	double delay,offset;

	t1=timestamp2double(&rxbuf[24]);
	t2=timestamp2double(&rxbuf[32]);
	t3=timestamp2double(&rxbuf[40]);
	t4=timestamp2double(rxtime);
	delay=(t4-t1)-(t3-t2);
	offset=((t2-t1)+(t3-t4))/2;
	printf("%f: %fms (%fms RTT)\n",t4,1000*offset,1000*delay);
}

int main(int argc,char **argv) {
	int loopmode=0;

	if(2<=argc && '-'==argv[1][0] && 'l'==argv[1][1] && '\0'==argv[1][2]) {
		loopmode=1;
		argc--;
		argv++;
	}
	if(2 != argc) {
		fputs("usage: ntptest [-l] <servername>\n",stderr);
		return(1);
	}
	if(opensock(argv[1])) {
		fputs("unable to open connection\n",stderr);
		return(2);
	}
	if(loopmode)
		setbuf(stdout,0);
AGAIN:
	if(sendpacket()) {
		fputs("unable to send packet\n",stderr);
		return(3);
	}
	if(recvpacket()) {
		fputs("unable to receive packet\n",stderr);
		return(4);
	}
	if(loopmode) {
		showoffset();
		sleep(2);
		goto AGAIN;
	}
	showpacket();
	return(0);
}