kern.c | 270 ++++++++++++++++++++++++++++++++++++++++------------------------- 1 file changed, 169 insertions(+), 101 deletions(-) Index: kern/kern.c =================================================================== --- kern.orig/kern.c 2005-10-10 00:25:05.000000000 +0200 +++ kern/kern.c 2005-10-10 00:25:06.000000000 +0200 @@ -65,6 +65,10 @@ struct timeval { /* avoid sys/time.h na long tv_sec; long tv_usec; }; +struct timespec { + long tv_sec; + long tv_nsec; +}; #define PCCFREQ (-10) /* pcc offset frequency (ppm) */ @@ -84,6 +88,35 @@ struct timeval { /* avoid sys/time.h na #define HZ 100 /* timer interrupt frequency (Hz) */ #define MICRO /* include microtime() routine */ +static inline int fls(int x) +{ + int r = 32; + + if (!x) + return 0; + if (!(x & 0xffff0000u)) { + x <<= 16; + r -= 16; + } + if (!(x & 0xff000000u)) { + x <<= 8; + r -= 8; + } + if (!(x & 0xf0000000u)) { + x <<= 4; + r -= 4; + } + if (!(x & 0xc0000000u)) { + x <<= 2; + r -= 2; + } + if (!(x & 0x80000000u)) { + x <<= 1; + r -= 1; + } + return r; +} + /* * Multiprocessor definitions * @@ -113,7 +146,7 @@ struct timeval { /* avoid sys/time.h na * holds the divisor computed by microset() and used by microtime() to * determine the microseconds since the last timer interrupt. */ -struct timeval pcc_time[NCPUS]; /* time at last synch interrupt */ +struct timespec pcc_time[NCPUS];/* time at last synch interrupt */ long pcc_pcc[NCPUS]; /* pcc at last synch interrupt */ long pcc_denom[NCPUS]; /* divisor to convert pcc to usec */ #endif /* MICRO */ @@ -185,7 +218,10 @@ long time_esterror = MAXPHASE; /* estima */ long time_phase = 0; /* phase offset (scaled us) */ long time_freq = 0; /* frequency offset (scaled ppm) */ +long time_freq_adj; +long time_freq_phase; long time_adj = 0; /* tick adjust (scaled 1 / hz) */ +long time_adj_curr; long time_reftime = 0; /* time at last adjustment (s) */ /* @@ -195,7 +231,7 @@ void hardupdate(long); void hardclock(void); void second_overflow(void); void hardpps(struct timeval *, long); -void microtime(struct timeval *); +void microtime(struct timespec *); void microset(void); void kern(void); @@ -203,9 +239,10 @@ void kern(void); * Kernel variables */ static int hz = HZ; /* CPU oscillator frequency (Hz) */ +static int shift_hz; static int tick; /* timer interrupt period (us) */ -static int fixtick; /* amortization constant (ppm) */ -static struct timeval time; /* kernel time variable */ +static int tick_curr; +static struct timespec time; /* kernel time variable */ /* * Simulator variables @@ -232,13 +269,29 @@ static long ppsusec; /* pps microsecond */ int main() { +#ifdef MICRO int i; +#endif + long t, f, offset; while (scanf("%ld %d %d %d %ld %ld", &simtime, &time_status, - &tau, &poll, &time.tv_usec, &pllfreq) > 0) { + &tau, &poll, &offset, &pllfreq) > 0) { - tick = (int)(1000000 / hz); - fixtick = (int)(1000000 % hz); + //f = pllfreq; + f = 0; + t = f >> (SHIFT_USEC + 8); + if (t) { + f -= t << (SHIFT_USEC + 8); + t *= 1000 << 8; + } + f *= 125; + t += 1000000000 + (f >> (SHIFT_USEC - 3)); + f &= (1 << (SHIFT_USEC - 3)) - 1; + tick = tick_curr = t / hz; + f += (t % hz) << (SHIFT_USEC - 3); + f <<= 3; + shift_hz = fls(hz - 1); time.tv_sec = 0; + time.tv_nsec = offset * 1000; time_us = 0; cycles = 0; @@ -257,24 +310,28 @@ int main() time_phase = 0; time_freq = 0; - time_adj = 0; + time_adj_curr = 0; time_reftime = 0; time_offset = 0; - time_freq = 0; - time_adj = 0; + { + //hardupdate(offset); + //time_adj = f / hz; + //time_freq_adj = f % hz; + //time_freq = time_adj << shift_hz; + //printf("adj:%ld,fa:%ld\n", time_adj, time_freq_adj); + } #ifdef MICRO for (i = 0; i < NCPUS; i++) { pcc_time[i].tv_sec = 0; - pcc_time[i].tv_usec = 0; + pcc_time[i].tv_nsec = 0; pcc_pcc[i] = 0; pcc_denom[i] = 0; } #endif /* MICRO */ (void)printf("time %ld, status %04x, tc %d, phase %ld, freq %ld\n", - simtime, time_status, tau, time.tv_usec, pllfreq); - (void)printf("tick %d us, fixtick %d us\n", tick, - fixtick); - (void)printf(" time offset freq _offset _freq _adj\n"); + simtime, time_status, tau, time.tv_nsec, pllfreq); + (void)printf("tick %d ns, hz %d, shift %d\n", tick, hz, shift_hz); + (void)printf(" time offset freq _offset _freq _tick _adj\n"); kern(); } return 0; @@ -286,7 +343,7 @@ int main() void kern() { - struct timeval times; + struct timespec times; /* * Grind the loop until reaching the time limit. @@ -297,7 +354,7 @@ kern() cycles += (long)((PCC + PCCFREQ) / hz); #endif /* MICRO */ hardclock(); - if (time.tv_usec >= 1000000) { + if (time.tv_nsec >= 1000000000) { second_overflow(); #ifdef MICRO microset(); @@ -306,15 +363,16 @@ kern() if (!(poll_interval % poll)) { microtime(×); phi = (long)((time_us - - (double)times.tv_sec) * 1e6 - - (double)times.tv_usec); - hardupdate(phi); - (void)printf("%6ld%8ld%9.3lf %08lx %08lx %08lx\n", - time.tv_sec, phi, + (double)times.tv_sec) * 1e9 - + (double)times.tv_nsec); + hardupdate(phi / 1000); + (void)printf("%6ld.%09lu%11ld%9.3lf %08lx %08lx %11u %08lx\n", + time.tv_sec, time.tv_nsec, phi, (double)time_freq / (1L << SHIFT_USEC), time_offset & 0xffffffff, time_freq & 0xffffffff, + tick, time_adj & 0xffffffff); } } @@ -330,7 +388,7 @@ hardclock() { long ltemp, time_update; - time_update = tick; /* from preceding code */ + time_update = tick_curr; /* from preceding code */ /* * Beginning of precision-kernel code fragment @@ -339,19 +397,14 @@ hardclock() * (time_phase) of the update overflow, bump the high-order bits * (time_update). */ - time_phase += time_adj; - if (time_phase <= -FINEUSEC) { - ltemp = -time_phase >> SHIFT_SCALE; - time_phase += ltemp << SHIFT_SCALE; - time_update -= ltemp; - } - else if (time_phase >= FINEUSEC) { - ltemp = time_phase >> SHIFT_SCALE; - time_phase -= ltemp << SHIFT_SCALE; + time_phase += time_adj_curr; + if (time_phase >= (1 << (SHIFT_SCALE))) { + ltemp = time_phase >> (SHIFT_SCALE); + time_phase -= ltemp << (SHIFT_SCALE); time_update += ltemp; } - time.tv_usec += time_update; + time.tv_nsec += time_update; /* * End of precision-kernel code fragment @@ -381,8 +434,8 @@ second_overflow() * maximum frequency offset is a tad less than) +-512 ppm. On a * 64-bit machine, you shouldn't need to ask. */ - if (time.tv_usec >= 1000000) { - time.tv_usec -= 1000000; + if (time.tv_nsec >= 1000000000) { + time.tv_nsec -= 1000000000; time.tv_sec++; time_maxerror += time_tolerance >> SHIFT_USEC; @@ -427,6 +480,8 @@ second_overflow() time_state = TIME_OK; } + time_adj_curr = time_adj; + tick_curr = tick; /* * Compute the phase adjustment for the next second. In * PLL mode, the offset is reduced by a fixed factor @@ -440,22 +495,28 @@ second_overflow() ltemp = -time_offset; if (!(time_status & STA_FLL)) ltemp >>= SHIFT_KG + time_constant; - if (ltemp > (MAXPHASE / MINSEC) << SHIFT_UPDATE) - ltemp = (MAXPHASE / MINSEC) << - SHIFT_UPDATE; + if (ltemp > (((MAXPHASE * 1000 / hz) << shift_hz) / MINSEC)) + ltemp = ((MAXPHASE * 1000 / hz) << shift_hz) / MINSEC; time_offset += ltemp; - time_adj = -ltemp << (SHIFT_SCALE - SHIFT_HZ - - SHIFT_UPDATE); + tick_curr -= ltemp >> shift_hz; + time_adj_curr -= (ltemp << (SHIFT_SCALE - shift_hz)) & ((1 << SHIFT_SCALE) - 1); + if (time_adj_curr < 0) { + tick_curr--; + time_adj_curr += (1 << (SHIFT_SCALE)); + } } else { ltemp = time_offset; if (!(time_status & STA_FLL)) ltemp >>= SHIFT_KG + time_constant; - if (ltemp > (MAXPHASE / MINSEC) << SHIFT_UPDATE) - ltemp = (MAXPHASE / MINSEC) << - SHIFT_UPDATE; + if (ltemp > (((MAXPHASE * 1000 / hz) << shift_hz) / MINSEC)) + ltemp = ((MAXPHASE * 1000 / hz) << shift_hz) / MINSEC; time_offset -= ltemp; - time_adj = ltemp << (SHIFT_SCALE - SHIFT_HZ - - SHIFT_UPDATE); + tick_curr += ltemp >> shift_hz; + time_adj_curr += (ltemp << (SHIFT_SCALE - shift_hz)) & ((1 << SHIFT_SCALE) - 1); + if (time_adj_curr >= (1 << (SHIFT_SCALE))) { + tick_curr++; + time_adj_curr -= (1 << (SHIFT_SCALE)); + } } /* @@ -465,32 +526,15 @@ second_overflow() * watchdog counter and update the frequency computed by * the pll and the PPS signal. */ - ltemp = time_freq; - if (ltemp < 0) - time_adj -= -ltemp >> - (SHIFT_USEC + SHIFT_HZ - SHIFT_SCALE); - else - time_adj += ltemp >> - (SHIFT_USEC + SHIFT_HZ - SHIFT_SCALE); - time_adj += (long)fixtick << (SHIFT_SCALE - SHIFT_HZ); - -#if SHIFT_HZ == 7 - /* - * When the CPU clock oscillator frequency is not a - * power of 2 in Hz, the SHIFT_HZ is only an approximate - * scale factor. In the SunOS kernel, this results in a - * PLL gain factor of 1/1.28 = 0.78 what it should be. - * In the following code the overall gain is increased - * by a factor of 1.25, which results in a residual - * error less than 3 percent. - */ - if (hz == 100) { - if (time_adj < 0) - time_adj -= -time_adj >> 2; - else - time_adj += time_adj >> 2; + //time_adj_curr += time_freq >> shift_hz; + time_freq_phase += time_freq_adj; + if (time_freq_phase > hz) { + time_freq_phase -= hz; + time_adj_curr++; } -#endif /* SHIFT_HZ */ + //printf("%lu,adj:%ld,ph:%ld,fp:%ld\n", + // (time_freq << (SHIFT_SCALE - SHIFT_USEC)), + // time_adj_curr, time_phase, time_freq_phase); } /* @@ -526,19 +570,19 @@ hardupdate(offset) { long ltemp, mtemp; - if (!(time_status & STA_PLL) && !(time_status & STA_PPSTIME)) - return; + if (!(time_status & STA_PLL)) + goto skip; ltemp = offset; /* * Scale the phase adjustment and clamp to the operating range. */ if (ltemp > MAXPHASE) - time_offset = MAXPHASE << SHIFT_UPDATE; + time_offset = MAXPHASE; else if (ltemp < -MAXPHASE) - time_offset = -(MAXPHASE << SHIFT_UPDATE); + time_offset = -MAXPHASE; else - time_offset = ltemp << SHIFT_UPDATE; + time_offset = ltemp; /* * Select whether the frequency is to be controlled and in which @@ -551,8 +595,7 @@ hardupdate(offset) time_reftime = time.tv_sec; if (time_status & STA_FLL) { if (mtemp >= MINSEC) { - ltemp = ((time_offset / mtemp) << (SHIFT_USEC - - SHIFT_UPDATE)); + ltemp = (((time_offset << SHIFT_UPDATE) / mtemp) << (SHIFT_USEC - SHIFT_UPDATE)); if (ltemp < 0) time_freq -= -ltemp >> SHIFT_KH; else @@ -571,10 +614,33 @@ hardupdate(offset) SHIFT_USEC); } } + + time_offset = ((time_offset * 1000) / hz) << shift_hz; if (time_freq > time_tolerance) time_freq = time_tolerance; else if (time_freq < -time_tolerance) time_freq = -time_tolerance; + +skip: +{ + long t, f; + + f = time_freq; + t = f >> 24; + if (t) { + f -= t << 24; + t *= 1000 << (24 - SHIFT_USEC); + } + f *= 125; + t += 1000000000 + (f >> (SHIFT_USEC - 3)); + f &= (1 << (SHIFT_USEC - 3)) - 1; + tick = t / hz; + f += (t % hz) << (SHIFT_USEC - 3); + f <<= SHIFT_SCALE - (SHIFT_USEC - 3); + time_adj = f / hz; + //time_freq_adj = f % hz; + //time_freq = time_adj << shift_hz; +} } /* @@ -586,7 +652,7 @@ hardupdate(offset) * is called from the hardclock() code segment once per second for each * processor in a multiprocessor system. */ -struct timeval lasttime; /* last reported time */ +struct timespec lasttime; /* last reported time */ /* * Return the best possible estimate of the time in the timeval to which @@ -606,11 +672,13 @@ struct timeval lasttime; /* last reporte */ void microtime(tvp) - struct timeval *tvp; + struct timespec *tvp; { - struct timeval t; - long sec, usec; + struct timespec t; + long sec, nsec; +#ifdef MICRO int i; +#endif TIME_READ(t); #ifdef MICRO @@ -630,10 +698,10 @@ microtime(tvp) * following sanity checks will suppress timewarps. */ t = pcc_time[i]; - usec = (rpcc() - pcc_pcc[i]) & 0xffffffff; - t.tv_usec += (usec << PCC_SHIFT) / pcc_denom[i]; - while (t.tv_usec >= 1000000) { - t.tv_usec -= 1000000; + nsec = (rpcc() - pcc_pcc[i]) & 0xffffffff; + t.tv_nsec += (nsec << PCC_SHIFT) / pcc_denom[i]; + while (t.tv_nsec >= 1000000000) { + t.tv_nsec -= 1000000000; t.tv_sec++; } } @@ -649,15 +717,15 @@ microtime(tvp) * to set the clock backward only upon unavoidable crisis. */ sec = lasttime.tv_sec - t.tv_sec; - usec = lasttime.tv_usec - t.tv_usec; - if (usec < 0) { - usec += 1000000; + nsec = lasttime.tv_nsec - t.tv_nsec; + if (nsec < 0) { + nsec += 1000000000; sec--; } if (sec == 0) { - t.tv_usec += usec + 1; - if (t.tv_usec >= 1000000) { - t.tv_usec -= 1000000; + t.tv_nsec += nsec + 1; + if (t.tv_nsec >= 1000000000) { + t.tv_nsec -= 1000000000; t.tv_sec++; } } @@ -679,27 +747,27 @@ microtime(tvp) void microset() { - struct timeval t; - long ltemp, sec, usec; + struct timespec t; + long ltemp, sec, nsec; int i; TIME_READ(t); - usec = rpcc(); + nsec = rpcc(); i = cpu_number(); if (pcc_time[i].tv_sec == 0) { - pcc_pcc[i] = usec; + pcc_pcc[i] = nsec; pcc_time[i] = t; return; } - ltemp = (usec - pcc_pcc[i]) & 0xffffffff; - pcc_pcc[i] = usec; + ltemp = (nsec - pcc_pcc[i]) & 0xffffffff; + pcc_pcc[i] = nsec; sec = t.tv_sec - pcc_time[i].tv_sec; - usec = t.tv_usec - pcc_time[i].tv_usec; - if (usec >= 1000000) { - usec += 1000000; + nsec = t.tv_nsec - pcc_time[i].tv_nsec; + if (nsec >= 1000000000) { + nsec += 1000000000; sec--; } pcc_time[i] = t; - pcc_denom[i] = (ltemp << PCC_SHIFT) / (sec * 1000000 + usec); + pcc_denom[i] = (ltemp << PCC_SHIFT) / (sec * 1000000000 + nsec); } #endif /* MICRO */