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