/* kern.c,v 3.1 1993/07/06 01:07:29 jbj Exp
 * This program simulates a first-order, type-II phase-lock loop using
 * actual code segments from modified SunOS 4.1.1 and Ultrix 3.2a
 * kernels. This program does not include licensed code.
 */
#include <stdio.h>
#include <ctype.h>
#include <math.h>

/*
 * phase-lock loop definitions
 */
#define HZ 100			/* timer interrupt frequency (Hz) */
#define SHIFT_HZ 7		/* log2(HZ) */
#define SHIFT_KG 8 		/* shift for phase increment */
#define SHIFT_KF 20		/* shift for frequency increment */
#define MAXTC 6			/* maximum time constant (shift) */
#define SHIFT_SCALE 24		/* shift for scale factor */
#define SHIFT_UPDATE (SHIFT_KG + MAXTC) /* shift for offset scale factor */
#define FINEUSEC (1 << SHIFT_SCALE) /* 1 us in scaled units */
#define FINETUNE (1000000 - (1000000 / HZ) * HZ) /* frequency adjustment
				 * for non-isochronous HZ (ppm) */
#define MAXPHASE 128000		/* max phase error (us) */
#define MAXFREQ 100		/* max frequency error (ppm) */
#define MINSEC 16		/* min interval between updates (s) */
#define MAXSEC 1200		/* max interval between updates (s) */
#define TIME_UNS 0		/* unspecified or unknown */
#define TIME_OK 1		/* operation succeeded */
#define TIME_INS 1		/* insert leap second at end of day */
#define TIME_DEL 2		/* delete leap second at end of day */
#define TIME_OOP 3		/* leap second in progress */
#define TIME_BAD 4		/* system clock is not synchronized */
#define TIME_ADR -1		/* operation failed: invalid address */
#define TIME_VAL -2		/* operation failed: invalid argument */
#define TIME_PRV -3		/* operation failed: priviledged */

/*
 * function declarations
 */
void hardupdate();
void hardclock();
void second_overflow();

/*
 * kernel variables
 */
long tick;			/* timer interrupt period */
struct timeval {		/* ripoff of unix timeval */
	long tv_sec;
	long tv_usec;
} timex;

/*
 * phase-lock loop variables
 */
int time_status = TIME_BAD;	/* clock synchronization status */
long time_offset = 0;		/* time adjustment (us) */
long time_constant = 0;		/* pll time constant */
long time_tolerance = MAXFREQ;	/* frequency tolerance (ppm) */
long time_precision = 1000000 / HZ; /* clock precision (us) */
struct timeval time_maxerror;	/* maximum error */
struct timeval time_esterror;	/* estimated error */
long time_phase = 0;		/* phase offset (scaled us) */
long time_freq = 0;		/* frequency offset (scaled ppm) */
long time_adj = 0;		/* tick adjust (scaled 1 / HZ) */
long time_reftime = 0;		/* time at last adjustment (s) */

/*
 * simulation variables
 */
double timey = 0;		/* simulation time (us) */
long timez = 0;			/* current error (us) */
long poll_interval = 0;		/* poll counter */

/*
 * simulation test program
 */
void main()
{
	tick = 1000000 / HZ;
	timex.tv_sec = 0;
	timex.tv_usec = MAXPHASE;
 	time_freq = time_tolerance << SHIFT_KF;
	time_constant = 0;

	/*
	 * grind the loop until ^C
	 */
	while (1) {
		timey += (double)1000000 / HZ;
		if (timey >= 1000000)
			timey -= 1000000;
		hardclock();
		if (timex.tv_usec >= 1000000) {
			timex.tv_usec -= 1000000;
			timex.tv_sec++;
			second_overflow();
			poll_interval++;
			if (poll_interval >= (1 << (time_constant + 4))) {
				poll_interval -= 1 << (time_constant + 4);
				timez = (long)timey - timex.tv_usec;
				if (timez > 500000)
					timez -= 1000000;
				if (timez < -500000)
					timez += 1000000;
				hardupdate(timez);
				printf("%10li%10li  %08lx  %08lx  %08lx\n",
				    timex.tv_sec, timez, time_offset,
				    time_freq, time_adj);
			}
		}
	}
}

/*
 * this routine simulates the adjtime() call
 */
void hardupdate(offset)
long offset;
{
	long ltemp, mtemp;

	time_offset = offset << SHIFT_UPDATE;
	mtemp = timex.tv_sec - time_reftime;
	time_reftime = timex.tv_sec;
	if (mtemp > MAXSEC)
		mtemp = 0;

	/* ugly multiply should be replaced */
	if (offset < 0)
		time_freq -= (-offset * mtemp) >>
		    (time_constant + time_constant);
	else
		time_freq += (offset * mtemp) >>
		    (time_constant + time_constant);
	ltemp = time_tolerance << SHIFT_KF;
	if (time_freq > ltemp)
		time_freq = ltemp;
	else if (time_freq < -ltemp)
		time_freq = -ltemp;
}

/*
 * this routine simulates the timer interrupt
 */
void hardclock()
{
	long ltemp;

	time_phase += time_adj;
	if (time_phase < -FINEUSEC) {
		ltemp = -time_phase >> SHIFT_SCALE;
		time_phase += ltemp << SHIFT_SCALE;
		timex.tv_usec += tick - ltemp;
	}
	else if (time_phase > FINEUSEC) {
		ltemp = time_phase >> SHIFT_SCALE;
		time_phase -= ltemp << SHIFT_SCALE;
		timex.tv_usec += tick + ltemp;
	} else
		timex.tv_usec += tick;
}

/*
 * this routine simulates the overflow of the microsecond field
 */
void second_overflow()
{
	long ltemp;

	time_maxerror.tv_usec += time_tolerance;
	if (time_maxerror.tv_usec >= 1000000) {
		time_maxerror.tv_usec -= 1000000;
		time_maxerror.tv_sec++;
	}
	if (time_offset < 0) {
		ltemp = -time_offset >>
		    (SHIFT_KG + time_constant);
		time_offset += ltemp;
		time_adj = -(ltemp <<
		    (SHIFT_SCALE - SHIFT_HZ - SHIFT_UPDATE));
	} else {
		ltemp = time_offset >>
		    (SHIFT_KG + time_constant);
		time_offset -= ltemp;
		time_adj = ltemp <<
		    (SHIFT_SCALE - SHIFT_HZ - SHIFT_UPDATE);
	}
	time_adj += (time_freq >> (SHIFT_KF + SHIFT_HZ - SHIFT_SCALE))
	    + (FINETUNE << (SHIFT_SCALE - SHIFT_HZ));

	/* ugly divide should be replaced */
	if (timex.tv_sec % 86400 == 0) {
		switch (time_status) {

			case TIME_INS:
			timex.tv_sec--; /* !! */
			time_status = TIME_OOP;
			break;

			case TIME_DEL:
			timex.tv_sec++;
			time_status = TIME_OK;
			break;

			case TIME_OOP:
			time_status = TIME_OK;
			break;
		}
	}
}
