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