kern.c revision 82498
1139804Simp/* 2885Swollman * This program simulates a first-order, type-II phase-lock loop using 3885Swollman * actual code segments from modified kernel distributions for SunOS, 4885Swollman * Ultrix and OSF/1 kernels. These segments do not use any licensed code. 5885Swollman */ 6885Swollman 7885Swollman#ifdef HAVE_CONFIG_H 8885Swollman# include <config.h> 9885Swollman#endif 10885Swollman 11885Swollman#include <stdio.h> 12885Swollman#include <ctype.h> 13885Swollman#include <math.h> 14885Swollman#include <sys/time.h> 15885Swollman 16885Swollman#ifdef HAVE_TIMEX_H 1710625Sdg# include "timex.h" 18885Swollman#endif 19885Swollman 20885Swollman/* 21885Swollman * Phase-lock loop definitions 22885Swollman */ 23885Swollman#define HZ 100 /* timer interrupt frequency (Hz) */ 24885Swollman#define MAXPHASE 512000 /* max phase error (us) */ 25885Swollman#define MAXFREQ 200 /* max frequency error (ppm) */ 26885Swollman#define TAU 2 /* time constant (shift 0 - 6) */ 27116182Sobrien#define POLL 16 /* interval between updates (s) */ 28116182Sobrien#define MAXSEC 1200 /* max interval between updates (s) */ 29116182Sobrien 301549Srgrimes/* 311549Srgrimes * Function declarations 321549Srgrimes */ 333058Sdgvoid hardupdate(); 341549Srgrimesvoid hardclock(); 3576166Smarkmvoid second_overflow(); 36103181Sbde 3776166Smarkm/* 3815494Sbde * Kernel variables 3976166Smarkm */ 4039154Sjdpint tick; /* timer interrupt period (us) */ 41103181Sbdeint fixtick; /* amortization constant (ppm) */ 422257Ssosstruct timeval timex; /* ripoff of kernel time variable */ 43103181Sbde 4415494Sbde/* 4576166Smarkm * Phase-lock loop variables 46103181Sbde */ 4739154Sjdpint time_status = TIME_BAD; /* clock synchronization status */ 48885Swollmanlong time_offset = 0; /* time adjustment (us) */ 491549Srgrimeslong time_constant = 0; /* pll time constant */ 5012662Sdglong time_tolerance = MAXFREQ; /* frequency tolerance (ppm) */ 5112662Sdglong time_precision = 1000000 / HZ; /* clock precision (us) */ 5232446Sdysonlong time_maxerror = MAXPHASE; /* maximum error (us) */ 53103181Sbdelong time_esterror = MAXPHASE; /* estimated error (us) */ 54885Swollmanlong time_phase = 0; /* phase offset (scaled us) */ 5592723Salfredlong time_freq = 0; /* frequency offset (scaled ppm) */ 56102808Sjakelong time_adj = 0; /* tick adjust (scaled 1 / HZ) */ 5712568Sbdelong time_reftime = 0; /* time at last adjustment (s) */ 5839154Sjdp 5939154Sjdp/* 6039154Sjdp * Simulation variables 6139154Sjdp */ 6239154Sjdpdouble timey = 0; /* simulation time (us) */ 63102808Sjakelong timez = 0; /* current error (us) */ 6439154Sjdplong poll_interval = 0; /* poll counter */ 65102808Sjake 66102808Sjake/* 67102808Sjake * Simulation test program 6839154Sjdp */ 6939154Sjdpint 7039154Sjdpmain( 71102808Sjake int argc, 7239154Sjdp char *argv[] 7368520Smarcel ) 74138128Sdas{ 75102808Sjake tick = 1000000 / HZ; 76102808Sjake fixtick = 1000000 % HZ; 77102808Sjake timex.tv_sec = 0; 78102808Sjake timex.tv_usec = MAXPHASE; 79102808Sjake time_freq = 0; 80102808Sjake time_constant = TAU; 81102808Sjake printf("tick %d us, fixtick %d us\n", tick, fixtick); 82102808Sjake printf(" time offset freq _offset _freq _adj\n"); 83120422Speter 84120422Speter /* 8539154Sjdp * Grind the loop until ^C 8639154Sjdp */ 8750901Sbde while (1) { 88102808Sjake timey += (double)(1000000) / HZ; 89102808Sjake if (timey >= 1000000) 90102808Sjake timey -= 1000000; 91102808Sjake hardclock(); 92102808Sjake if (timex.tv_usec >= 1000000) { 93140992Ssobomax timex.tv_usec -= 1000000; 94102808Sjake timex.tv_sec++; 95102808Sjake second_overflow(); 96102808Sjake poll_interval++; 9712130Sdg if (!(poll_interval % POLL)) { 9812130Sdg timez = (long)timey - timex.tv_usec; 99885Swollman if (timez > 500000) 10017974Sbde timez -= 1000000; 10124848Sdyson if (timez < -500000) 10232446Sdyson timez += 1000000; 10344469Salc hardupdate(timez); 10432446Sdyson printf("%10li%10li%10.2f %08lx %08lx %08lx\n", 10532446Sdyson timex.tv_sec, timez, 10614703Sbde (double)time_freq / (1 << SHIFT_KF), 10712767Sdyson time_offset, time_freq, time_adj); 108885Swollman } 1093098Sphk } 110885Swollman } 11179224Sdillon} 11279224Sdillon 113885Swollman/* 1146380Ssos * This routine simulates the ntp_adjtime() call 1158876Srgrimes * 1169202Srgrimes * For default SHIFT_UPDATE = 12, offset is limited to +-512 ms, the 11714363Speter * maximum interval between updates is 4096 s and the maximum frequency 1186380Ssos * offset is +-31.25 ms/s. 1199202Srgrimes */ 12014363Spetervoid 12114363Speterhardupdate( 1229202Srgrimes long offset 1236380Ssos ) 1246380Ssos{ 125885Swollman long ltemp, mtemp; 126885Swollman 127885Swollman time_offset = offset << SHIFT_UPDATE; 128885Swollman mtemp = timex.tv_sec - time_reftime; 129885Swollman time_reftime = timex.tv_sec; 130885Swollman if (mtemp > MAXSEC) 131885Swollman mtemp = 0; 132885Swollman 13315538Sphk /* ugly multiply should be replaced */ 134885Swollman if (offset < 0) 135885Swollman time_freq -= (-offset * mtemp) >> 136885Swollman (time_constant + time_constant); 137885Swollman else 138885Swollman time_freq += (offset * mtemp) >> 139885Swollman (time_constant + time_constant); 14015538Sphk ltemp = time_tolerance << SHIFT_KF; 141885Swollman if (time_freq > ltemp) 14245270Sjdp time_freq = ltemp; 14345270Sjdp else if (time_freq < -ltemp) 144103767Sjake time_freq = -ltemp; 145885Swollman if (time_status == TIME_BAD) 146885Swollman time_status = TIME_OK; 147885Swollman} 148885Swollman 149885Swollman/* 150885Swollman * This routine simulates the timer interrupt 15115538Sphk */ 152885Swollmanvoid 153885Swollmanhardclock(void) 154885Swollman{ 155885Swollman int ltemp, time_update; 156885Swollman 157885Swollman time_update = tick; /* computed by adjtime() */ 158885Swollman time_phase += time_adj; 15915538Sphk if (time_phase < -FINEUSEC) { 160885Swollman ltemp = -time_phase >> SHIFT_SCALE; 161885Swollman time_phase += ltemp << SHIFT_SCALE; 162885Swollman time_update -= ltemp; 163885Swollman } 164885Swollman else if (time_phase > FINEUSEC) { 165885Swollman ltemp = time_phase >> SHIFT_SCALE; 166885Swollman time_phase -= ltemp << SHIFT_SCALE; 167885Swollman time_update += ltemp; 168885Swollman } 16915538Sphk timex.tv_usec += time_update; 170885Swollman} 171885Swollman 172885Swollman/* 17312130Sdg * This routine simulates the overflow of the microsecond field 174885Swollman * 175885Swollman * With SHIFT_SCALE = 23, the maximum frequency adjustment is +-256 us 176885Swollman * per tick, or 25.6 ms/s at a clock frequency of 100 Hz. The time 177885Swollman * contribution is shifted right a minimum of two bits, while the frequency 178885Swollman * contribution is a right shift. Thus, overflow is prevented if the 179125454Sjhb * frequency contribution is limited to half the maximum or 15.625 ms/s. 180885Swollman */ 18184783Spsvoid 182885Swollmansecond_overflow(void) 183885Swollman{ 184125454Sjhb int ltemp; 185125454Sjhb 186885Swollman time_maxerror += time_tolerance; 187125454Sjhb if (time_offset < 0) { 188125454Sjhb ltemp = -time_offset >> 189885Swollman (SHIFT_KG + time_constant); 190885Swollman time_offset += ltemp; 191885Swollman time_adj = -(ltemp << 192885Swollman (SHIFT_SCALE - SHIFT_HZ - SHIFT_UPDATE)); 193103767Sjake } else { 194885Swollman ltemp = time_offset >> 195885Swollman (SHIFT_KG + time_constant); 19624848Sdyson time_offset -= ltemp; 19724848Sdyson time_adj = ltemp << 19824848Sdyson (SHIFT_SCALE - SHIFT_HZ - SHIFT_UPDATE); 19924848Sdyson } 20032446Sdyson if (time_freq < 0) 20199487Sjeff time_adj -= -time_freq >> (SHIFT_KF + SHIFT_HZ - SHIFT_SCALE); 20244469Salc else 20344469Salc time_adj += time_freq >> (SHIFT_KF + SHIFT_HZ - SHIFT_SCALE); 20432446Sdyson time_adj += fixtick << (SHIFT_SCALE - SHIFT_HZ); 20532446Sdyson 20632446Sdyson /* ugly divide should be replaced */ 20744626Salc if (timex.tv_sec % 86400 == 0) { 20832446Sdyson switch (time_status) { 20932446Sdyson 21032446Sdyson case TIME_INS: 21147258Salc timex.tv_sec--; /* !! */ 21244469Salc time_status = TIME_OOP; 21344469Salc break; 214885Swollman 21544469Salc case TIME_DEL: 21632446Sdyson timex.tv_sec++; 21732446Sdyson time_status = TIME_OK; 21832446Sdyson break; 21944626Salc 22032446Sdyson case TIME_OOP: 22132446Sdyson time_status = TIME_OK; 22232446Sdyson break; 22347258Salc } 22444469Salc } 22544469Salc} 22632446Sdyson