154359Sroberto/* 254359Sroberto * This program simulates a first-order, type-II phase-lock loop using 354359Sroberto * actual code segments from modified kernel distributions for SunOS, 454359Sroberto * Ultrix and OSF/1 kernels. These segments do not use any licensed code. 554359Sroberto */ 682498Sroberto 754359Sroberto#ifdef HAVE_CONFIG_H 854359Sroberto# include <config.h> 954359Sroberto#endif 1054359Sroberto 1154359Sroberto#include <stdio.h> 1254359Sroberto#include <ctype.h> 1354359Sroberto#include <math.h> 1454359Sroberto#include <sys/time.h> 1554359Sroberto 1654359Sroberto#ifdef HAVE_TIMEX_H 1754359Sroberto# include "timex.h" 1854359Sroberto#endif 1954359Sroberto 2054359Sroberto/* 2154359Sroberto * Phase-lock loop definitions 2254359Sroberto */ 2354359Sroberto#define HZ 100 /* timer interrupt frequency (Hz) */ 2454359Sroberto#define MAXPHASE 512000 /* max phase error (us) */ 2554359Sroberto#define MAXFREQ 200 /* max frequency error (ppm) */ 2654359Sroberto#define TAU 2 /* time constant (shift 0 - 6) */ 2754359Sroberto#define POLL 16 /* interval between updates (s) */ 2854359Sroberto#define MAXSEC 1200 /* max interval between updates (s) */ 2954359Sroberto 3054359Sroberto/* 3154359Sroberto * Function declarations 3254359Sroberto */ 3354359Srobertovoid hardupdate(); 3454359Srobertovoid hardclock(); 3554359Srobertovoid second_overflow(); 3654359Sroberto 3754359Sroberto/* 3854359Sroberto * Kernel variables 3954359Sroberto */ 4054359Srobertoint tick; /* timer interrupt period (us) */ 4154359Srobertoint fixtick; /* amortization constant (ppm) */ 4254359Srobertostruct timeval timex; /* ripoff of kernel time variable */ 4354359Sroberto 4454359Sroberto/* 4554359Sroberto * Phase-lock loop variables 4654359Sroberto */ 4754359Srobertoint time_status = TIME_BAD; /* clock synchronization status */ 4854359Srobertolong time_offset = 0; /* time adjustment (us) */ 4954359Srobertolong time_constant = 0; /* pll time constant */ 5054359Srobertolong time_tolerance = MAXFREQ; /* frequency tolerance (ppm) */ 5154359Srobertolong time_precision = 1000000 / HZ; /* clock precision (us) */ 5254359Srobertolong time_maxerror = MAXPHASE; /* maximum error (us) */ 5354359Srobertolong time_esterror = MAXPHASE; /* estimated error (us) */ 5454359Srobertolong time_phase = 0; /* phase offset (scaled us) */ 5554359Srobertolong time_freq = 0; /* frequency offset (scaled ppm) */ 5654359Srobertolong time_adj = 0; /* tick adjust (scaled 1 / HZ) */ 5754359Srobertolong time_reftime = 0; /* time at last adjustment (s) */ 5854359Sroberto 5954359Sroberto/* 6054359Sroberto * Simulation variables 6154359Sroberto */ 6254359Srobertodouble timey = 0; /* simulation time (us) */ 6354359Srobertolong timez = 0; /* current error (us) */ 6454359Srobertolong poll_interval = 0; /* poll counter */ 6554359Sroberto 6654359Sroberto/* 6754359Sroberto * Simulation test program 6854359Sroberto */ 6954359Srobertoint 7054359Srobertomain( 7154359Sroberto int argc, 7254359Sroberto char *argv[] 7354359Sroberto ) 7454359Sroberto{ 7554359Sroberto tick = 1000000 / HZ; 7654359Sroberto fixtick = 1000000 % HZ; 7754359Sroberto timex.tv_sec = 0; 7854359Sroberto timex.tv_usec = MAXPHASE; 7954359Sroberto time_freq = 0; 8054359Sroberto time_constant = TAU; 8154359Sroberto printf("tick %d us, fixtick %d us\n", tick, fixtick); 8254359Sroberto printf(" time offset freq _offset _freq _adj\n"); 8354359Sroberto 8454359Sroberto /* 8554359Sroberto * Grind the loop until ^C 8654359Sroberto */ 8754359Sroberto while (1) { 8854359Sroberto timey += (double)(1000000) / HZ; 8954359Sroberto if (timey >= 1000000) 9054359Sroberto timey -= 1000000; 9154359Sroberto hardclock(); 9254359Sroberto if (timex.tv_usec >= 1000000) { 9354359Sroberto timex.tv_usec -= 1000000; 9454359Sroberto timex.tv_sec++; 9554359Sroberto second_overflow(); 9654359Sroberto poll_interval++; 9754359Sroberto if (!(poll_interval % POLL)) { 9854359Sroberto timez = (long)timey - timex.tv_usec; 9954359Sroberto if (timez > 500000) 10054359Sroberto timez -= 1000000; 10154359Sroberto if (timez < -500000) 10254359Sroberto timez += 1000000; 10354359Sroberto hardupdate(timez); 10454359Sroberto printf("%10li%10li%10.2f %08lx %08lx %08lx\n", 10554359Sroberto timex.tv_sec, timez, 10654359Sroberto (double)time_freq / (1 << SHIFT_KF), 10754359Sroberto time_offset, time_freq, time_adj); 10854359Sroberto } 10954359Sroberto } 11054359Sroberto } 11154359Sroberto} 11254359Sroberto 11354359Sroberto/* 11454359Sroberto * This routine simulates the ntp_adjtime() call 11554359Sroberto * 11654359Sroberto * For default SHIFT_UPDATE = 12, offset is limited to +-512 ms, the 11754359Sroberto * maximum interval between updates is 4096 s and the maximum frequency 11854359Sroberto * offset is +-31.25 ms/s. 11954359Sroberto */ 12054359Srobertovoid 12154359Srobertohardupdate( 12254359Sroberto long offset 12354359Sroberto ) 12454359Sroberto{ 12554359Sroberto long ltemp, mtemp; 12654359Sroberto 12754359Sroberto time_offset = offset << SHIFT_UPDATE; 12854359Sroberto mtemp = timex.tv_sec - time_reftime; 12954359Sroberto time_reftime = timex.tv_sec; 13054359Sroberto if (mtemp > MAXSEC) 13154359Sroberto mtemp = 0; 13254359Sroberto 13354359Sroberto /* ugly multiply should be replaced */ 13454359Sroberto if (offset < 0) 13554359Sroberto time_freq -= (-offset * mtemp) >> 13654359Sroberto (time_constant + time_constant); 13754359Sroberto else 13854359Sroberto time_freq += (offset * mtemp) >> 13954359Sroberto (time_constant + time_constant); 14054359Sroberto ltemp = time_tolerance << SHIFT_KF; 14154359Sroberto if (time_freq > ltemp) 14254359Sroberto time_freq = ltemp; 14354359Sroberto else if (time_freq < -ltemp) 14454359Sroberto time_freq = -ltemp; 14554359Sroberto if (time_status == TIME_BAD) 14654359Sroberto time_status = TIME_OK; 14754359Sroberto} 14854359Sroberto 14954359Sroberto/* 15054359Sroberto * This routine simulates the timer interrupt 15154359Sroberto */ 15254359Srobertovoid 15354359Srobertohardclock(void) 15454359Sroberto{ 15554359Sroberto int ltemp, time_update; 15654359Sroberto 15754359Sroberto time_update = tick; /* computed by adjtime() */ 15854359Sroberto time_phase += time_adj; 15954359Sroberto if (time_phase < -FINEUSEC) { 16054359Sroberto ltemp = -time_phase >> SHIFT_SCALE; 16154359Sroberto time_phase += ltemp << SHIFT_SCALE; 16254359Sroberto time_update -= ltemp; 16354359Sroberto } 16454359Sroberto else if (time_phase > FINEUSEC) { 16554359Sroberto ltemp = time_phase >> SHIFT_SCALE; 16654359Sroberto time_phase -= ltemp << SHIFT_SCALE; 16754359Sroberto time_update += ltemp; 16854359Sroberto } 16954359Sroberto timex.tv_usec += time_update; 17054359Sroberto} 17154359Sroberto 17254359Sroberto/* 17354359Sroberto * This routine simulates the overflow of the microsecond field 17454359Sroberto * 17554359Sroberto * With SHIFT_SCALE = 23, the maximum frequency adjustment is +-256 us 17654359Sroberto * per tick, or 25.6 ms/s at a clock frequency of 100 Hz. The time 17754359Sroberto * contribution is shifted right a minimum of two bits, while the frequency 17854359Sroberto * contribution is a right shift. Thus, overflow is prevented if the 17954359Sroberto * frequency contribution is limited to half the maximum or 15.625 ms/s. 18054359Sroberto */ 18154359Srobertovoid 18254359Srobertosecond_overflow(void) 18354359Sroberto{ 18454359Sroberto int ltemp; 18554359Sroberto 18654359Sroberto time_maxerror += time_tolerance; 18754359Sroberto if (time_offset < 0) { 18854359Sroberto ltemp = -time_offset >> 18954359Sroberto (SHIFT_KG + time_constant); 19054359Sroberto time_offset += ltemp; 19154359Sroberto time_adj = -(ltemp << 19254359Sroberto (SHIFT_SCALE - SHIFT_HZ - SHIFT_UPDATE)); 19354359Sroberto } else { 19454359Sroberto ltemp = time_offset >> 19554359Sroberto (SHIFT_KG + time_constant); 19654359Sroberto time_offset -= ltemp; 19754359Sroberto time_adj = ltemp << 19854359Sroberto (SHIFT_SCALE - SHIFT_HZ - SHIFT_UPDATE); 19954359Sroberto } 20054359Sroberto if (time_freq < 0) 20154359Sroberto time_adj -= -time_freq >> (SHIFT_KF + SHIFT_HZ - SHIFT_SCALE); 20254359Sroberto else 20354359Sroberto time_adj += time_freq >> (SHIFT_KF + SHIFT_HZ - SHIFT_SCALE); 20454359Sroberto time_adj += fixtick << (SHIFT_SCALE - SHIFT_HZ); 20554359Sroberto 20654359Sroberto /* ugly divide should be replaced */ 20754359Sroberto if (timex.tv_sec % 86400 == 0) { 20854359Sroberto switch (time_status) { 20954359Sroberto 21054359Sroberto case TIME_INS: 21154359Sroberto timex.tv_sec--; /* !! */ 21254359Sroberto time_status = TIME_OOP; 21354359Sroberto break; 21454359Sroberto 21554359Sroberto case TIME_DEL: 21654359Sroberto timex.tv_sec++; 21754359Sroberto time_status = TIME_OK; 21854359Sroberto break; 21954359Sroberto 22054359Sroberto case TIME_OOP: 22154359Sroberto time_status = TIME_OK; 22254359Sroberto break; 22354359Sroberto } 22454359Sroberto } 22554359Sroberto} 226