1
2
3#include <stdio.h>
4#include <stdlib.h>
5#include <string.h>
6#include <sys/types.h>
7#include <sys/socket.h>
8#include <netinet/in.h>
9#include <netdb.h>
10#include <arpa/inet.h>
11#include <time.h>
12#include <unistd.h>
13#include <errno.h>
14#ifdef _PRECISION_SIOCGSTAMP
15#include <sys/ioctl.h>
16#endif
17
18#define ENABLE_DEBUG
19
20extern char *optarg;
21
22#ifdef linux
23#include <sys/utsname.h>
24#include <sys/time.h>
25typedef u_int32_t __u32;
26#include <sys/timex.h>
27#else
28#define main ntpclient
29extern struct hostent *gethostbyname(const char *name);
30extern int h_errno;
31#define herror(hostname) \
32	fprintf(stderr,"Error %d looking up hostname %s\n", h_errno,hostname)
33typedef uint32_t __u32;
34#endif
35
36#define JAN_1970        0x83aa7e80      /* 2208988800 1970 - 1900 in seconds */
37#define NTP_PORT (123)
38
39/* How to multiply by 4294.967296 quickly (and not quite exactly)
40 * without using floating point or greater than 32-bit integers.
41 * If you want to fix the last 12 microseconds of error, add in
42 * (2911*(x))>>28)
43 */
44#define NTPFRAC(x) ( 4294*(x) + ( (1981*(x))>>11 ) )
45
46/* The reverse of the above, needed if we want to set our microsecond
47 * clock (via settimeofday) based on the incoming time in NTP format.
48 * Basically exact.
49 */
50#define USEC(x) ( ( (x) >> 12 ) - 759 * ( ( ( (x) >> 10 ) + 32768 ) >> 16 ) )
51
52/* Converts NTP delay and dispersion, apparently in seconds scaled
53 * by 65536, to microseconds.  RFC1305 states this time is in seconds,
54 * doesn't mention the scaling.
55 * Should somehow be the same as 1000000 * x / 65536
56 */
57#define sec2u(x) ( (x) * 15.2587890625 )
58
59struct ntptime {
60	unsigned int coarse;
61	unsigned int fine;
62};
63
64void send_packet(int usd);
65void rfc1305print(char *data, struct ntptime *arrival);
66void udp_handle(int usd, char *data, int data_len, struct sockaddr *sa_source, int sa_len);
67
68/* global variables (I know, bad form, but this is a short program) */
69char incoming[1500];
70struct timeval time_of_send;
71int live=0;
72int set_clock=0;   /* non-zero presumably needs root privs */
73
74#ifdef ENABLE_DEBUG
75int debug=0;
76#define DEBUG_OPTION "d"
77#else
78#define debug 0
79#define DEBUG_OPTION
80#endif
81
82int contemplate_data(unsigned int absolute, double skew, double errorbar, int freq);
83
84int get_current_freq()
85{
86	/* OS dependent routine to get the current value of clock frequency.
87	 */
88#ifdef linux
89	struct timex txc;
90	txc.modes=0;
91	if (__adjtimex(&txc) < 0) {
92		perror("adjtimex"); exit(1);
93	}
94	return txc.freq;
95#else
96	return 0;
97#endif
98}
99
100int set_freq(int new_freq)
101{
102	/* OS dependent routine to set a new value of clock frequency.
103	 */
104#ifdef linux
105	struct timex txc;
106	txc.modes = ADJ_FREQUENCY;
107	txc.freq = new_freq;
108	if (__adjtimex(&txc) < 0) {
109		perror("adjtimex"); exit(1);
110	}
111	return txc.freq;
112#else
113	return 0;
114#endif
115}
116
117void send_packet(int usd)
118{
119	__u32 data[12];
120	struct timeval now;
121#define LI 0
122#define VN 3
123#define MODE 3
124#define STRATUM 0
125#define POLL 4
126#define PREC -6
127
128	if (debug) fprintf(stderr,"Sending ...\n");
129	if (sizeof(data) != 48) {
130		fprintf(stderr,"size error\n");
131		return;
132	}
133	bzero((char*)data,sizeof(data));
134	data[0] = htonl (
135		( LI << 30 ) | ( VN << 27 ) | ( MODE << 24 ) |
136		( STRATUM << 16) | ( POLL << 8 ) | ( PREC & 0xff ) );
137	data[1] = htonl(1<<16);  /* Root Delay (seconds) */
138	data[2] = htonl(1<<16);  /* Root Dispersion (seconds) */
139	gettimeofday(&now,NULL);
140	data[10] = htonl(now.tv_sec + JAN_1970); /* Transmit Timestamp coarse */
141	data[11] = htonl(NTPFRAC(now.tv_usec));  /* Transmit Timestamp fine   */
142	send(usd,data,48,0);
143	time_of_send=now;
144}
145
146
147void udp_handle(int usd, char *data, int data_len, struct sockaddr *sa_source, int sa_len)
148{
149	struct timeval udp_arrival;
150	struct ntptime udp_arrival_ntp;
151
152#ifdef _PRECISION_SIOCGSTAMP
153	if ( ioctl(usd, SIOCGSTAMP, &udp_arrival) < 0 ) {
154		perror("ioctl-SIOCGSTAMP");
155		gettimeofday(&udp_arrival,NULL);
156	}
157#else
158	gettimeofday(&udp_arrival,NULL);
159#endif
160	udp_arrival_ntp.coarse = udp_arrival.tv_sec + JAN_1970;
161	udp_arrival_ntp.fine   = NTPFRAC(udp_arrival.tv_usec);
162
163	if (debug) {
164		struct sockaddr_in *sa_in=(struct sockaddr_in *)sa_source;
165		printf("packet of length %d received\n",data_len);
166		if (sa_source->sa_family==AF_INET) {
167			printf("Source: INET Port %d host %s\n",
168				ntohs(sa_in->sin_port),inet_ntoa(sa_in->sin_addr));
169		} else {
170			printf("Source: Address family %d\n",sa_source->sa_family);
171		}
172	}
173	rfc1305print(data,&udp_arrival_ntp);
174}
175
176double ntpdiff( struct ntptime *start, struct ntptime *stop)
177{
178	int a;
179	unsigned int b;
180	a = stop->coarse - start->coarse;
181	if (stop->fine >= start->fine) {
182		b = stop->fine - start->fine;
183	} else {
184		b = start->fine - stop->fine;
185		b = ~b;
186		a -= 1;
187	}
188
189	return a*1.e6 + b * (1.e6/4294967296.0);
190}
191
192void rfc1305print(char *data, struct ntptime *arrival)
193{
194/* straight out of RFC-1305 Appendix A */
195	int li, vn, mode, stratum, poll, prec;
196	int delay, disp, refid;
197	struct ntptime reftime, orgtime, rectime, xmttime;
198	double etime,stime,skew1,skew2;
199	int freq;
200
201#define Data(i) ntohl(((unsigned int *)data)[i])
202	li      = Data(0) >> 30 & 0x03;
203	vn      = Data(0) >> 27 & 0x07;
204	mode    = Data(0) >> 24 & 0x07;
205	stratum = Data(0) >> 16 & 0xff;
206	poll    = Data(0) >>  8 & 0xff;
207	prec    = Data(0)       & 0xff;
208	if (prec & 0x80) prec|=0xffffff00;
209	delay   = Data(1);
210	disp    = Data(2);
211	refid   = Data(3);
212	reftime.coarse = Data(4);
213	reftime.fine   = Data(5);
214	orgtime.coarse = Data(6);
215	orgtime.fine   = Data(7);
216	rectime.coarse = Data(8);
217	rectime.fine   = Data(9);
218	xmttime.coarse = Data(10);
219	xmttime.fine   = Data(11);
220#undef Data
221
222	if (set_clock) {   /* you'd better be root, or ntpclient will crash! */
223		struct timeval tv_set;
224		/* it would be even better to subtract half the slop */
225		tv_set.tv_sec  = xmttime.coarse - JAN_1970;
226		/* divide xmttime.fine by 4294.967296 */
227		tv_set.tv_usec = USEC(xmttime.fine);
228		if (settimeofday(&tv_set,NULL)<0) {
229			perror("settimeofday");
230			exit(1);
231		}
232		if (debug) {
233			printf("set time to %lu.%.6lu\n", tv_set.tv_sec, tv_set.tv_usec);
234		}
235	}
236
237	if (debug) {
238	printf("LI=%d  VN=%d  Mode=%d  Stratum=%d  Poll=%d  Precision=%d\n",
239		li, vn, mode, stratum, poll, prec);
240	printf("Delay=%.1f  Dispersion=%.1f  Refid=%u.%u.%u.%u\n",
241		sec2u(delay),sec2u(disp),
242		refid>>24&0xff, refid>>16&0xff, refid>>8&0xff, refid&0xff);
243	printf("Reference %u.%.10u\n", reftime.coarse, reftime.fine);
244	printf("Originate %u.%.10u\n", orgtime.coarse, orgtime.fine);
245	printf("Receive   %u.%.10u\n", rectime.coarse, rectime.fine);
246	printf("Transmit  %u.%.10u\n", xmttime.coarse, xmttime.fine);
247	printf("Our recv  %u.%.10u\n", arrival->coarse, arrival->fine);
248	}
249	etime=ntpdiff(&orgtime,arrival);
250	stime=ntpdiff(&rectime,&xmttime);
251	skew1=ntpdiff(&orgtime,&rectime);
252	skew2=ntpdiff(&xmttime,arrival);
253	freq=get_current_freq();
254	if (debug) {
255	printf("Total elapsed: %9.2f\n"
256	       "Server stall:  %9.2f\n"
257	       "Slop:          %9.2f\n",
258		etime, stime, etime-stime);
259	printf("Skew:          %9.2f\n"
260	       "Frequency:     %9d\n"
261	       " day   second     elapsed    stall     skew  dispersion  freq\n",
262		(skew1-skew2)/2, freq);
263	}
264	if (debug) {
265	printf("%d %5d.%.3d  %8.1f %8.1f  %8.1f %8.1f %9d\n",
266		arrival->coarse/86400+15020, arrival->coarse%86400,
267		arrival->fine/4294967, etime, stime,
268		(skew1-skew2)/2, sec2u(disp), freq);
269	fflush(stdout);
270	}
271	if (live) {
272		int new_freq;
273		new_freq = contemplate_data(arrival->coarse, (skew1-skew2)/2,
274			etime+sec2u(disp), freq);
275		if (!debug && new_freq != freq) set_freq(new_freq);
276	}
277}
278
279void stuff_net_addr(struct in_addr *p, char *hostname)
280{
281	struct hostent *ntpserver;
282	ntpserver=gethostbyname(hostname);
283	if (ntpserver == NULL) {
284		herror(hostname);
285		exit(1);
286	}
287	if (ntpserver->h_length != 4) {
288		fprintf(stderr,"oops %d\n",ntpserver->h_length);
289		exit(1);
290	}
291	memcpy(&(p->s_addr),ntpserver->h_addr_list[0],4);
292}
293
294void setup_receive(int usd, unsigned int interface, short port)
295{
296	struct sockaddr_in sa_rcvr;
297	bzero((char *) &sa_rcvr, sizeof(sa_rcvr));
298	sa_rcvr.sin_family=AF_INET;
299	sa_rcvr.sin_addr.s_addr=htonl(interface);
300	sa_rcvr.sin_port=htons(port);
301	if(bind(usd,(struct sockaddr *) &sa_rcvr,sizeof(sa_rcvr)) == -1) {
302		fprintf(stderr,"could not bind to udp port %d\n",port);
303		perror("bind");
304		exit(1);
305	}
306	listen(usd,3);
307}
308
309void setup_transmit(int usd, char *host, short port)
310{
311	struct sockaddr_in sa_dest;
312	bzero((char *) &sa_dest, sizeof(sa_dest));
313	sa_dest.sin_family=AF_INET;
314	stuff_net_addr(&(sa_dest.sin_addr),host);
315	sa_dest.sin_port=htons(port);
316	if (connect(usd,(struct sockaddr *)&sa_dest,sizeof(sa_dest))==-1)
317		{perror("connect");exit(1);}
318}
319
320int primary_loop(int usd, int num_probes, int cycle_time)
321{
322	fd_set fds;
323	struct sockaddr sa_xmit;
324	int i, pack_len, sa_xmit_len, probes_sent;
325	struct timeval to;
326
327	if (debug) printf("Listening...\n");
328
329	probes_sent=0;
330	sa_xmit_len=sizeof(sa_xmit);
331	to.tv_sec=0;
332	to.tv_usec=0;
333	for (;;) {
334		FD_ZERO(&fds);
335		FD_SET(usd,&fds);
336		i=select(usd+1,&fds,NULL,NULL,&to);  /* Wait on read or error */
337		if ((i!=1)||(!FD_ISSET(usd,&fds))) {
338			if (i==EINTR) continue;
339			if (i<0) perror("select");
340			if ((to.tv_sec == 0) || (to.tv_sec == cycle_time)) {
341				if (probes_sent >= num_probes &&
342					num_probes != 0) break;
343				send_packet(usd);
344				++probes_sent;
345				to.tv_sec=cycle_time;
346				to.tv_usec=0;
347			}
348			continue;
349		}
350		pack_len=recvfrom(usd,incoming,sizeof(incoming),0,
351		                  &sa_xmit,(socklen_t *)&sa_xmit_len);
352		if (pack_len<0) {
353			perror("recvfrom");
354		} else if (pack_len>0 && pack_len<sizeof(incoming)){
355			udp_handle(usd,incoming,pack_len,&sa_xmit,sa_xmit_len);
356			return 0;
357		} else {
358			printf("Ooops.  pack_len=%d\n",pack_len);
359			fflush(stdout);
360		}
361		if (probes_sent >= num_probes && num_probes != 0) break;
362	}
363	return -1;
364}
365
366void do_replay(void)
367{
368	char line[100];
369	int n, day, freq, absolute;
370	float sec, etime, stime, disp;
371	double skew, errorbar;
372	int simulated_freq = 0;
373	unsigned int last_fake_time = 0;
374	double fake_delta_time = 0.0;
375
376	while (fgets(line,sizeof(line),stdin)) {
377		n=sscanf(line,"%d %f %f %f %lf %f %d",
378			&day, &sec, &etime, &stime, &skew, &disp, &freq);
379		if (n==7) {
380			fputs(line,stdout);
381			absolute=(day-15020)*86400+(int)sec;
382			errorbar=etime+disp;
383			if (debug) printf("contemplate %u %.1f %.1f %d\n",
384				absolute,skew,errorbar,freq);
385			if (last_fake_time==0) simulated_freq=freq;
386			fake_delta_time += (absolute-last_fake_time)*((double)(freq-simulated_freq))/65536;
387			if (debug) printf("fake %f %d \n", fake_delta_time, simulated_freq);
388			skew += fake_delta_time;
389			freq = simulated_freq;
390			last_fake_time=absolute;
391			simulated_freq = contemplate_data(absolute, skew, errorbar, freq);
392		} else {
393			fprintf(stderr,"Replay input error\n");
394			exit(2);
395		}
396	}
397}
398
399void usage(char *argv0)
400{
401	fprintf(stderr,
402	"Usage: %s [-c count] [-d] -h hostname [-i interval] [-l]\n"
403	"\t[-p port] [-r] [-s] \n",
404	argv0);
405}
406
407/* Copy each token in wordlist delimited by space into word */
408#define foreach(word, wordlist, next) \
409	for (next = &wordlist[strspn(wordlist, " ")], \
410	     strncpy(word, next, sizeof(word)), \
411	     word[strcspn(word, " ")] = '\0', \
412	     word[sizeof(word) - 1] = '\0', \
413	     next = strchr(next, ' '); \
414	     strlen(word); \
415	     next = next ? &next[strspn(next, " ")] : "", \
416	     strncpy(word, next, sizeof(word)), \
417	     word[strcspn(word, " ")] = '\0', \
418	     word[sizeof(word) - 1] = '\0', \
419	     next = strchr(next, ' '))
420
421int main(int argc, char *argv[]) {
422	int usd;  /* socket */
423	int c;
424	/* These parameters are settable from the command line
425	   the initializations here provide default behavior */
426	short int udp_local_port=0;   /* default of 0 means kernel chooses */
427	int cycle_time=3;          /* request timeout in seconds */
428	int probe_count=0;            /* default of 0 means loop forever */
429	/* int debug=0; is a global above */
430	char *hostname=NULL;          /* must be set */
431	int replay=0;                 /* replay mode overrides everything */
432	char ntps[32], *next;
433
434	for (;;) {
435		c = getopt( argc, argv, "c:" DEBUG_OPTION "h:i:p:lrs");
436		if (c == EOF) break;
437		switch (c) {
438			case 'c':
439				probe_count = atoi(optarg);
440				break;
441#ifdef ENABLE_DEBUG
442			case 'd':
443				++debug;
444				break;
445#endif
446			case 'h':
447				hostname = optarg;
448				break;
449			case 'i':
450				cycle_time = atoi(optarg);
451				break;
452			case 'l':
453				live++;
454				break;
455			case 'p':
456				udp_local_port = atoi(optarg);
457				break;
458			case 'r':
459				replay++;
460				break;
461			case 's':
462				set_clock = 1;
463				probe_count = 1;
464				break;
465			default:
466				usage(argv[0]);
467				exit(1);
468		}
469	}
470
471	if (replay) {
472		do_replay();
473		exit(0);
474	}
475	if (hostname == NULL) {
476		usage(argv[0]);
477		exit(1);
478	}
479	if (debug) {
480		printf("Configuration:\n"
481		"  -c probe_count %d\n"
482		"  -d (debug)     %d\n"
483		"  -h hostname    %s\n"
484		"  -i interval    %d\n"
485		"  -l live        %d\n"
486		"  -p local_port  %d\n"
487		"  -s set_clock   %d\n",
488		probe_count, debug, hostname, cycle_time,
489		live, udp_local_port, set_clock);
490	}
491
492	foreach(ntps, hostname, next) {
493
494		/* Startup sequence */
495		if ((usd=socket(AF_INET,SOCK_DGRAM,IPPROTO_UDP))==-1) {
496			perror ("socket");
497			exit(1);
498		}
499
500		setup_receive(usd, INADDR_ANY, udp_local_port);
501
502		setup_transmit(usd, ntps, NTP_PORT);
503
504		if (!primary_loop(usd, probe_count, cycle_time)) {
505			close(usd);
506			break;
507		}
508
509		close(usd);
510	}
511
512	return 0;
513}
514