clock.cc

/*
 * Copyright (C) 2010  Miroslav Lichvar <mlichvar@redhat.com>
 * 
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "clock.h"

#define MINSEC 256
#define MAXSEC 2048
#define MAXTIMECONST 10
#define MAXMAXERROR 16000000
#define SHIFT_FLL 2
#define SCALE_FREQ 65536.0e6
#define MAXFREQ_SCALED 32768000
#define MAX_SLEWRATE 500
#define MAX_TICK(base_tick) ((base_tick) * 11 / 10)
#define MIN_TICK(base_tick) ((base_tick) * 9 / 10)

#define MIN_FREQ 0.8
#define MAX_FREQ 1.2

Clock::Clock() {
	time = 0.0;
	mono_time = 0.0;
	freq = 1.0;

	freq_generator = NULL;
	step_generator = NULL;

	base_tick = sysconf(_SC_CLK_TCK);
	assert(base_tick > 0);
	base_tick = (1000000 + base_tick / 2) / base_tick;

	memset(&ntp_timex, 0, sizeof(ntp_timex));
	ntp_timex.tick = base_tick;
	ntp_timex.tolerance = MAXFREQ_SCALED;
	ntp_timex.precision = 1;

	ntp_state = TIME_OK;

	/* in Linux kernel SHIFT_PLL is 2 since 2.6.31 */
	ntp_shift_pll = 2;
	ntp_flags = 0;
	ntp_update_interval = 0;
	ntp_offset = 0.0;
	ntp_slew = 0.0;

	ss_offset = 0;
	ss_slew = 0;
}

Clock::~Clock() {
	if (freq_generator)
		delete freq_generator;
	if (step_generator)
		delete step_generator;
}

double Clock::get_real_time() const {
	return time;
}

double Clock::get_monotonic_time() const {
	return mono_time;
}

double Clock::get_total_freq() const {
	double timex_freq, adjtime_freq;

	timex_freq = (double)ntp_timex.tick / base_tick + ntp_timex.freq / SCALE_FREQ + ntp_slew;
	adjtime_freq = ss_slew / 1e6;
	return freq * (timex_freq + adjtime_freq);
}

double Clock::get_raw_freq() const {
	double timex_freq;

	timex_freq = (double)ntp_timex.tick / base_tick + ntp_timex.freq / SCALE_FREQ;
	return freq * timex_freq;
}

double Clock::get_true_interval(double local_interval) const {
	return local_interval / get_total_freq();
}

double Clock::get_local_interval(double true_interval) const {
	return true_interval * get_total_freq();
}

void Clock::set_freq_generator(Generator *gen) {
	if (freq_generator)
		delete freq_generator;
	freq_generator = gen;
}

void Clock::set_step_generator(Generator *gen) {
	if (step_generator)
		delete step_generator;
	step_generator = gen;
}

void Clock::set_freq(double freq) {
	this->freq = freq + 1.0;
	if (!(this->freq > MIN_FREQ && this->freq < MAX_FREQ)) {
		fprintf(stderr, "frequency %e outside allowed range (%.2f, %.2f)\n", this->freq - 1.0, MIN_FREQ - 1.0, MAX_FREQ - 1.0);
		exit(1);
	}
}

void Clock::set_time(double time) {
	this->time = time;
}

void Clock::step_time(double step) {
	this->time += step;
}

void Clock::set_ntp_shift_pll(int shift) {
	ntp_shift_pll = shift;
}

void Clock::set_ntp_flag(int enable, int flag) {
	ntp_flags &= ~flag;
	if (enable)
		ntp_flags |= flag;
}

void Clock::advance(double real_interval) {
	double local_interval = get_local_interval(real_interval); 

	time += local_interval;
	mono_time += local_interval;
}

void Clock::update(bool second) {
	if (freq_generator)
		set_freq(freq_generator->generate(NULL));
	if (step_generator)
		step_time(step_generator->generate(NULL));
	
	if (!second)
		return;

	if (ntp_timex.status & STA_PLL) {
		ntp_update_interval++;
		ntp_slew = ntp_offset / (1 << (ntp_shift_pll +
			ntp_timex.constant));

#if 0
		if (ntp_slew > MAX_SLEWRATE / 1e6)
			ntp_slew = MAX_SLEWRATE / 1e6;
		else if (ntp_slew < -MAX_SLEWRATE / 1e6)
			ntp_slew = -MAX_SLEWRATE / 1e6;
#endif

		ntp_offset -= ntp_slew;

		if (ntp_timex.status & STA_NANO)
			ntp_timex.offset = ntp_offset * 1e9;
		else
			ntp_timex.offset = ntp_offset * 1e6;
	}

	if (ss_offset) {
		if (ss_offset > 0) {
			if (ss_offset > MAX_SLEWRATE) {
				ss_slew = MAX_SLEWRATE;
				ss_offset -= MAX_SLEWRATE;
			} else {
				ss_slew = ss_offset;
				ss_offset = 0;
			}
		} else {
			if (ss_offset < -MAX_SLEWRATE) {
				ss_slew = -MAX_SLEWRATE;
				ss_offset -= -MAX_SLEWRATE;
			} else {
				ss_slew = ss_offset;
				ss_offset = 0;
			}
		}
	} else
		ss_slew = 0;

	switch (ntp_state) {
		case TIME_OK:
			if (ntp_timex.status & STA_INS)
				ntp_state = TIME_INS;
			else if (ntp_timex.status & STA_DEL)
				ntp_state = TIME_DEL;
			break;
		case TIME_INS:
			if ((time_t)(time + 0.5) % (24 * 3600) <= 1) {
				time -= 1.0;
				ntp_timex.tai += 1.0;
				ntp_state = TIME_OOP;
			} else if (!(ntp_timex.status & STA_INS)) {
				ntp_state = TIME_OK;
			}
			break;
		case TIME_DEL:
			if ((time_t)(time + 1.0 + 0.5) % (24 * 3600) <= 1) {
				time += 1.0;
				ntp_timex.tai -= 1.0;
				ntp_state = TIME_WAIT;
			} else if (!(ntp_timex.status & STA_DEL)) {
				ntp_state = TIME_OK;
			}
			break;
		case TIME_OOP:
			ntp_state = TIME_WAIT;
			break;
		case TIME_WAIT:
			if (!(ntp_timex.status & (STA_INS | STA_DEL)))
				ntp_state = TIME_OK;
			break;
		default:
			assert(0);
	}
}

void Clock::update_ntp_offset(long offset) {
	double fll_adj, pll_adj, new_offset, old_offset, tc, t;

	if (ntp_timex.status & STA_FREQHOLD)
		ntp_update_interval = 0;

	if (ntp_timex.status & STA_NANO)
		new_offset = offset / 1e9;
	else
		new_offset = offset / 1e6;

	tc = 1 << ntp_timex.constant;
	ntp_timex.offset = offset;
	old_offset = ntp_offset;
	ntp_offset = new_offset;

	if (!(ntp_timex.status & STA_PLL))
		return;

	if (old_offset && ntp_update_interval >= MINSEC &&
		(ntp_timex.status & STA_FLL || ntp_update_interval > MAXSEC)) {
		ntp_timex.status |= STA_MODE;
		if (ntp_flags & CLOCK_NTP_FLL_MODE2)
			fll_adj = (new_offset - old_offset) / (ntp_update_interval * (1 << SHIFT_FLL));
		else
			fll_adj = new_offset / (ntp_update_interval * (1 << SHIFT_FLL));
	} else {
		ntp_timex.status &= ~STA_MODE;
		fll_adj = 0.0;
	}

	if (ntp_flags & CLOCK_NTP_PLL_CLAMP) {
		if (ntp_update_interval > MAXSEC)
			ntp_update_interval = MAXSEC;
		if (ntp_update_interval > tc * (1 << (ntp_shift_pll + 1)))
			ntp_update_interval = tc * (1 << (ntp_shift_pll + 1));
	}

	t = 4 * (1 << ntp_shift_pll) * tc;
	pll_adj = new_offset * ntp_update_interval / (t * t);

	ntp_timex.freq += (fll_adj + pll_adj) * SCALE_FREQ;

	if (ntp_timex.freq > MAXFREQ_SCALED)
		ntp_timex.freq = MAXFREQ_SCALED;
	else if (ntp_timex.freq < -MAXFREQ_SCALED)
		ntp_timex.freq = -MAXFREQ_SCALED;

	ntp_update_interval = 0;
}

int Clock::adjtimex(struct timex *buf) {
	int r = ntp_state;
	struct timex t;

	if (buf->modes & ADJ_FREQUENCY) {
		ntp_timex.freq = buf->freq;
		if (ntp_timex.freq > MAXFREQ_SCALED)
			ntp_timex.freq = MAXFREQ_SCALED;
		else if (ntp_timex.freq < -MAXFREQ_SCALED)
			ntp_timex.freq = -MAXFREQ_SCALED;
	}
	if (buf->modes & ADJ_MAXERROR)
		ntp_timex.maxerror = buf->maxerror;
	if (buf->modes & ADJ_STATUS) {
		if ((buf->status & STA_PLL) && !(ntp_timex.status & STA_PLL))
			ntp_update_interval = 0;
		ntp_timex.status = buf->status & 0xff;
	}
	if (buf->modes & ADJ_MICRO)
		ntp_timex.status &= ~STA_NANO;
	if (buf->modes & ADJ_NANO)
		ntp_timex.status |= STA_NANO;
	if (buf->modes & ADJ_TIMECONST) {
		ntp_timex.constant = buf->constant;
		if (!(ntp_timex.status & STA_NANO))
			ntp_timex.constant += 4;
		if (ntp_timex.constant > MAXTIMECONST)
			ntp_timex.constant = MAXTIMECONST;
		if (ntp_timex.constant < 0)
			ntp_timex.constant = 0;
	}
	if (buf->modes & ADJ_TICK) {
		if (buf->tick > MAX_TICK(base_tick) || buf->tick < MIN_TICK(base_tick)) {
			r = -1;
		} else
			ntp_timex.tick = buf->tick;
	}
	if ((buf->modes & ADJ_OFFSET_SINGLESHOT) != ADJ_OFFSET_SINGLESHOT) {
		if (buf->modes & ADJ_OFFSET) {
			update_ntp_offset(buf->offset);
		}
	}
	if (buf->modes & ADJ_SETOFFSET) {
		if (ntp_timex.status & STA_NANO)
			time += buf->time.tv_sec + buf->time.tv_usec * 1e-9;
		else
			time += buf->time.tv_sec + buf->time.tv_usec * 1e-6;
		ntp_timex.maxerror = MAXMAXERROR;
	}
	if (buf->modes & ADJ_TAI) {
		ntp_timex.tai = buf->constant;
	}

	t = ntp_timex;

	if ((buf->modes & ADJ_OFFSET_SINGLESHOT) == ADJ_OFFSET_SINGLESHOT) {
		if ((buf->modes & ADJ_OFFSET_SS_READ) == ADJ_OFFSET_SINGLESHOT) {
			t.offset = ss_offset;
			ss_offset = buf->offset;
		} else {
			t.offset = ss_offset;
		}
	}

	*buf = t;

	return r;
}

int Clock::adjtime(const struct timeval *delta, struct timeval *olddelta) {
	if (olddelta) {
		olddelta->tv_sec = ss_offset / 1000000;
		olddelta->tv_usec = ss_offset % 1000000;
	}
	if (delta)
		ss_offset = delta->tv_sec * 1000000 + delta->tv_usec;
	return 0;
}

Refclock::Refclock() {
	time = 0.0;
	offset = 0.0;
	generate = false;
	valid = false;
	offset_generator = NULL;
}

Refclock::~Refclock() {
	if (offset_generator)
		delete offset_generator;
}

void Refclock::set_offset_generator(Generator *gen) {
	if (offset_generator)
		delete offset_generator;
	offset_generator = gen;
}

void Refclock::set_generation(bool enable) {
	generate = enable;
}

void Refclock::update(double time, const Clock *clock) {
	if (!generate || !offset_generator)
		return;

	this->time = clock->get_real_time();
	offset = this->time - time + offset_generator->generate(NULL);
	valid = true;
}

bool Refclock::get_sample(double *time, double *offset) const {
	*time = this->time;
	*offset = this->offset;
	return valid;
}

void Refclock::get_offsets(double *offsets, int size) {
	int i;

	for (i = 0; i < size; i++)
		offsets[i] = offset_generator ? offset_generator->generate(NULL) : 0.0;
}