1c0b746e5SOllivier Robert /*
2c0b746e5SOllivier Robert * This program simulates a first-order, type-II phase-lock loop using
3c0b746e5SOllivier Robert * actual code segments from modified kernel distributions for SunOS,
4c0b746e5SOllivier Robert * Ultrix and OSF/1 kernels. These segments do not use any licensed code.
5c0b746e5SOllivier Robert */
6224ba2bdSOllivier Robert
7c0b746e5SOllivier Robert #ifdef HAVE_CONFIG_H
8c0b746e5SOllivier Robert # include <config.h>
9c0b746e5SOllivier Robert #endif
10c0b746e5SOllivier Robert
11c0b746e5SOllivier Robert #include <stdio.h>
12c0b746e5SOllivier Robert #include <ctype.h>
13c0b746e5SOllivier Robert #include <math.h>
14c0b746e5SOllivier Robert #include <sys/time.h>
15c0b746e5SOllivier Robert
16c0b746e5SOllivier Robert #ifdef HAVE_TIMEX_H
17c0b746e5SOllivier Robert # include "timex.h"
18c0b746e5SOllivier Robert #endif
19c0b746e5SOllivier Robert
20c0b746e5SOllivier Robert /*
21c0b746e5SOllivier Robert * Phase-lock loop definitions
22c0b746e5SOllivier Robert */
23c0b746e5SOllivier Robert #define HZ 100 /* timer interrupt frequency (Hz) */
24c0b746e5SOllivier Robert #define MAXPHASE 512000 /* max phase error (us) */
25c0b746e5SOllivier Robert #define MAXFREQ 200 /* max frequency error (ppm) */
26c0b746e5SOllivier Robert #define TAU 2 /* time constant (shift 0 - 6) */
27c0b746e5SOllivier Robert #define POLL 16 /* interval between updates (s) */
28c0b746e5SOllivier Robert #define MAXSEC 1200 /* max interval between updates (s) */
29c0b746e5SOllivier Robert
30c0b746e5SOllivier Robert /*
31c0b746e5SOllivier Robert * Function declarations
32c0b746e5SOllivier Robert */
33c0b746e5SOllivier Robert void hardupdate();
34c0b746e5SOllivier Robert void hardclock();
35c0b746e5SOllivier Robert void second_overflow();
36c0b746e5SOllivier Robert
37c0b746e5SOllivier Robert /*
38c0b746e5SOllivier Robert * Kernel variables
39c0b746e5SOllivier Robert */
40c0b746e5SOllivier Robert int tick; /* timer interrupt period (us) */
41c0b746e5SOllivier Robert int fixtick; /* amortization constant (ppm) */
42c0b746e5SOllivier Robert struct timeval timex; /* ripoff of kernel time variable */
43c0b746e5SOllivier Robert
44c0b746e5SOllivier Robert /*
45c0b746e5SOllivier Robert * Phase-lock loop variables
46c0b746e5SOllivier Robert */
47c0b746e5SOllivier Robert int time_status = TIME_BAD; /* clock synchronization status */
48c0b746e5SOllivier Robert long time_offset = 0; /* time adjustment (us) */
49c0b746e5SOllivier Robert long time_constant = 0; /* pll time constant */
50c0b746e5SOllivier Robert long time_tolerance = MAXFREQ; /* frequency tolerance (ppm) */
51c0b746e5SOllivier Robert long time_precision = 1000000 / HZ; /* clock precision (us) */
52c0b746e5SOllivier Robert long time_maxerror = MAXPHASE; /* maximum error (us) */
53c0b746e5SOllivier Robert long time_esterror = MAXPHASE; /* estimated error (us) */
54c0b746e5SOllivier Robert long time_phase = 0; /* phase offset (scaled us) */
55c0b746e5SOllivier Robert long time_freq = 0; /* frequency offset (scaled ppm) */
56c0b746e5SOllivier Robert long time_adj = 0; /* tick adjust (scaled 1 / HZ) */
57c0b746e5SOllivier Robert long time_reftime = 0; /* time at last adjustment (s) */
58c0b746e5SOllivier Robert
59c0b746e5SOllivier Robert /*
60c0b746e5SOllivier Robert * Simulation variables
61c0b746e5SOllivier Robert */
62c0b746e5SOllivier Robert double timey = 0; /* simulation time (us) */
63c0b746e5SOllivier Robert long timez = 0; /* current error (us) */
64c0b746e5SOllivier Robert long poll_interval = 0; /* poll counter */
65c0b746e5SOllivier Robert
66c0b746e5SOllivier Robert /*
67c0b746e5SOllivier Robert * Simulation test program
68c0b746e5SOllivier Robert */
69c0b746e5SOllivier Robert int
main(int argc,char * argv[])70c0b746e5SOllivier Robert main(
71c0b746e5SOllivier Robert int argc,
72c0b746e5SOllivier Robert char *argv[]
73c0b746e5SOllivier Robert )
74c0b746e5SOllivier Robert {
75c0b746e5SOllivier Robert tick = 1000000 / HZ;
76c0b746e5SOllivier Robert fixtick = 1000000 % HZ;
77c0b746e5SOllivier Robert timex.tv_sec = 0;
78c0b746e5SOllivier Robert timex.tv_usec = MAXPHASE;
79c0b746e5SOllivier Robert time_freq = 0;
80c0b746e5SOllivier Robert time_constant = TAU;
81c0b746e5SOllivier Robert printf("tick %d us, fixtick %d us\n", tick, fixtick);
82c0b746e5SOllivier Robert printf(" time offset freq _offset _freq _adj\n");
83c0b746e5SOllivier Robert
84c0b746e5SOllivier Robert /*
85c0b746e5SOllivier Robert * Grind the loop until ^C
86c0b746e5SOllivier Robert */
87c0b746e5SOllivier Robert while (1) {
88c0b746e5SOllivier Robert timey += (double)(1000000) / HZ;
89c0b746e5SOllivier Robert if (timey >= 1000000)
90c0b746e5SOllivier Robert timey -= 1000000;
91c0b746e5SOllivier Robert hardclock();
92c0b746e5SOllivier Robert if (timex.tv_usec >= 1000000) {
93c0b746e5SOllivier Robert timex.tv_usec -= 1000000;
94c0b746e5SOllivier Robert timex.tv_sec++;
95c0b746e5SOllivier Robert second_overflow();
96c0b746e5SOllivier Robert poll_interval++;
97c0b746e5SOllivier Robert if (!(poll_interval % POLL)) {
98c0b746e5SOllivier Robert timez = (long)timey - timex.tv_usec;
99c0b746e5SOllivier Robert if (timez > 500000)
100c0b746e5SOllivier Robert timez -= 1000000;
101c0b746e5SOllivier Robert if (timez < -500000)
102c0b746e5SOllivier Robert timez += 1000000;
103c0b746e5SOllivier Robert hardupdate(timez);
104c0b746e5SOllivier Robert printf("%10li%10li%10.2f %08lx %08lx %08lx\n",
105c0b746e5SOllivier Robert timex.tv_sec, timez,
106c0b746e5SOllivier Robert (double)time_freq / (1 << SHIFT_KF),
107c0b746e5SOllivier Robert time_offset, time_freq, time_adj);
108c0b746e5SOllivier Robert }
109c0b746e5SOllivier Robert }
110c0b746e5SOllivier Robert }
111c0b746e5SOllivier Robert }
112c0b746e5SOllivier Robert
113c0b746e5SOllivier Robert /*
114c0b746e5SOllivier Robert * This routine simulates the ntp_adjtime() call
115c0b746e5SOllivier Robert *
116c0b746e5SOllivier Robert * For default SHIFT_UPDATE = 12, offset is limited to +-512 ms, the
117c0b746e5SOllivier Robert * maximum interval between updates is 4096 s and the maximum frequency
118c0b746e5SOllivier Robert * offset is +-31.25 ms/s.
119c0b746e5SOllivier Robert */
120c0b746e5SOllivier Robert void
hardupdate(long offset)121c0b746e5SOllivier Robert hardupdate(
122c0b746e5SOllivier Robert long offset
123c0b746e5SOllivier Robert )
124c0b746e5SOllivier Robert {
125c0b746e5SOllivier Robert long ltemp, mtemp;
126c0b746e5SOllivier Robert
127c0b746e5SOllivier Robert time_offset = offset << SHIFT_UPDATE;
128c0b746e5SOllivier Robert mtemp = timex.tv_sec - time_reftime;
129c0b746e5SOllivier Robert time_reftime = timex.tv_sec;
130c0b746e5SOllivier Robert if (mtemp > MAXSEC)
131c0b746e5SOllivier Robert mtemp = 0;
132c0b746e5SOllivier Robert
133c0b746e5SOllivier Robert /* ugly multiply should be replaced */
134c0b746e5SOllivier Robert if (offset < 0)
135c0b746e5SOllivier Robert time_freq -= (-offset * mtemp) >>
136c0b746e5SOllivier Robert (time_constant + time_constant);
137c0b746e5SOllivier Robert else
138c0b746e5SOllivier Robert time_freq += (offset * mtemp) >>
139c0b746e5SOllivier Robert (time_constant + time_constant);
140c0b746e5SOllivier Robert ltemp = time_tolerance << SHIFT_KF;
141c0b746e5SOllivier Robert if (time_freq > ltemp)
142c0b746e5SOllivier Robert time_freq = ltemp;
143c0b746e5SOllivier Robert else if (time_freq < -ltemp)
144c0b746e5SOllivier Robert time_freq = -ltemp;
145c0b746e5SOllivier Robert if (time_status == TIME_BAD)
146c0b746e5SOllivier Robert time_status = TIME_OK;
147c0b746e5SOllivier Robert }
148c0b746e5SOllivier Robert
149c0b746e5SOllivier Robert /*
150c0b746e5SOllivier Robert * This routine simulates the timer interrupt
151c0b746e5SOllivier Robert */
152c0b746e5SOllivier Robert void
hardclock(void)153c0b746e5SOllivier Robert hardclock(void)
154c0b746e5SOllivier Robert {
155c0b746e5SOllivier Robert int ltemp, time_update;
156c0b746e5SOllivier Robert
157c0b746e5SOllivier Robert time_update = tick; /* computed by adjtime() */
158c0b746e5SOllivier Robert time_phase += time_adj;
159c0b746e5SOllivier Robert if (time_phase < -FINEUSEC) {
160c0b746e5SOllivier Robert ltemp = -time_phase >> SHIFT_SCALE;
161c0b746e5SOllivier Robert time_phase += ltemp << SHIFT_SCALE;
162c0b746e5SOllivier Robert time_update -= ltemp;
163c0b746e5SOllivier Robert }
164c0b746e5SOllivier Robert else if (time_phase > FINEUSEC) {
165c0b746e5SOllivier Robert ltemp = time_phase >> SHIFT_SCALE;
166c0b746e5SOllivier Robert time_phase -= ltemp << SHIFT_SCALE;
167c0b746e5SOllivier Robert time_update += ltemp;
168c0b746e5SOllivier Robert }
169c0b746e5SOllivier Robert timex.tv_usec += time_update;
170c0b746e5SOllivier Robert }
171c0b746e5SOllivier Robert
172c0b746e5SOllivier Robert /*
173c0b746e5SOllivier Robert * This routine simulates the overflow of the microsecond field
174c0b746e5SOllivier Robert *
175c0b746e5SOllivier Robert * With SHIFT_SCALE = 23, the maximum frequency adjustment is +-256 us
176c0b746e5SOllivier Robert * per tick, or 25.6 ms/s at a clock frequency of 100 Hz. The time
177c0b746e5SOllivier Robert * contribution is shifted right a minimum of two bits, while the frequency
178c0b746e5SOllivier Robert * contribution is a right shift. Thus, overflow is prevented if the
179c0b746e5SOllivier Robert * frequency contribution is limited to half the maximum or 15.625 ms/s.
180c0b746e5SOllivier Robert */
181c0b746e5SOllivier Robert void
second_overflow(void)182c0b746e5SOllivier Robert second_overflow(void)
183c0b746e5SOllivier Robert {
184c0b746e5SOllivier Robert int ltemp;
185c0b746e5SOllivier Robert
186c0b746e5SOllivier Robert time_maxerror += time_tolerance;
187c0b746e5SOllivier Robert if (time_offset < 0) {
188c0b746e5SOllivier Robert ltemp = -time_offset >>
189c0b746e5SOllivier Robert (SHIFT_KG + time_constant);
190c0b746e5SOllivier Robert time_offset += ltemp;
191c0b746e5SOllivier Robert time_adj = -(ltemp <<
192c0b746e5SOllivier Robert (SHIFT_SCALE - SHIFT_HZ - SHIFT_UPDATE));
193c0b746e5SOllivier Robert } else {
194c0b746e5SOllivier Robert ltemp = time_offset >>
195c0b746e5SOllivier Robert (SHIFT_KG + time_constant);
196c0b746e5SOllivier Robert time_offset -= ltemp;
197c0b746e5SOllivier Robert time_adj = ltemp <<
198c0b746e5SOllivier Robert (SHIFT_SCALE - SHIFT_HZ - SHIFT_UPDATE);
199c0b746e5SOllivier Robert }
200c0b746e5SOllivier Robert if (time_freq < 0)
201c0b746e5SOllivier Robert time_adj -= -time_freq >> (SHIFT_KF + SHIFT_HZ - SHIFT_SCALE);
202c0b746e5SOllivier Robert else
203c0b746e5SOllivier Robert time_adj += time_freq >> (SHIFT_KF + SHIFT_HZ - SHIFT_SCALE);
204c0b746e5SOllivier Robert time_adj += fixtick << (SHIFT_SCALE - SHIFT_HZ);
205c0b746e5SOllivier Robert
206c0b746e5SOllivier Robert /* ugly divide should be replaced */
207c0b746e5SOllivier Robert if (timex.tv_sec % 86400 == 0) {
208c0b746e5SOllivier Robert switch (time_status) {
209c0b746e5SOllivier Robert
210c0b746e5SOllivier Robert case TIME_INS:
211c0b746e5SOllivier Robert timex.tv_sec--; /* !! */
212c0b746e5SOllivier Robert time_status = TIME_OOP;
213c0b746e5SOllivier Robert break;
214c0b746e5SOllivier Robert
215c0b746e5SOllivier Robert case TIME_DEL:
216c0b746e5SOllivier Robert timex.tv_sec++;
217c0b746e5SOllivier Robert time_status = TIME_OK;
218c0b746e5SOllivier Robert break;
219c0b746e5SOllivier Robert
220c0b746e5SOllivier Robert case TIME_OOP:
221c0b746e5SOllivier Robert time_status = TIME_OK;
222c0b746e5SOllivier Robert break;
223c0b746e5SOllivier Robert }
224c0b746e5SOllivier Robert }
225c0b746e5SOllivier Robert }
226