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