Import frequency conrrection code from dragonfly, whith some changes:

only do frequency compensation if the clock is synced, and a slightly
diffent way of computing the linear regression.
You'll need a recent kernel and libc to use this.
Testing by naddy@ and ckuethe@ and others, thanks!
ok henning@

3 files changed, 99 insertions, 4 deletions

diff --git a/usr.sbin/ntpd/ntp.c b/usr.sbin/ntpd/ntp.c
index 6f849ca17a9..b970c1f9b80 100644
--- a/usr.sbin/ntpd/ntp.c
+++ b/usr.sbin/ntpd/ntp.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: ntp.c,v 1.86 2006/06/09 07:42:08 otto Exp $ */
+/*	$OpenBSD: ntp.c,v 1.87 2006/06/17 18:40:42 otto Exp $ */
 
 /*
  * Copyright (c) 2003, 2004 Henning Brauer <henning@openbsd.org>
@@ -149,6 +149,14 @@ ntp_main(int pipe_prnt[2], struct ntpd_conf *nconf)
 		client_peer_init(p);
 
 	bzero(&conf->status, sizeof(conf->status));
+
+	conf->freq.samples = 0;
+	conf->freq.x = 0.0;
+	conf->freq.xx = 0.0;
+	conf->freq.xy = 0.0;
+	conf->freq.y = 0.0;
+	conf->freq.overall_offset = 0.0;
+
 	conf->status.synced = 0;
 	clock_getres(CLOCK_REALTIME, &tp);
 	b = 1000000000 / tp.tv_nsec;	/* convert to Hz */
@@ -428,6 +436,50 @@ peer_remove(struct ntp_peer *p)
 	peer_cnt--;
 }
 
+static void
+priv_adjfreq(double offset)
+{
+	double curtime, freq;
+
+	if (!conf->status.synced)
+		return;
+
+	conf->freq.samples++;
+
+	if (conf->freq.samples <= 0)
+		return;
+
+	conf->freq.overall_offset += offset;
+	offset = conf->freq.overall_offset;	
+
+	curtime = gettime_corrected();
+	conf->freq.xy += offset * curtime;
+	conf->freq.x += curtime;
+	conf->freq.y += offset;
+	conf->freq.xx += curtime * curtime;
+
+	if (conf->freq.samples % FREQUENCY_SAMPLES != 0)
+		return;
+
+	freq =
+	    (conf->freq.xy - conf->freq.x * conf->freq.y / conf->freq.samples)
+	    /
+	    (conf->freq.xx - conf->freq.x * conf->freq.x / conf->freq.samples);
+
+	if (freq > MAX_FREQUENCY_ADJUST)
+		freq = MAX_FREQUENCY_ADJUST;
+	else if (freq < -MAX_FREQUENCY_ADJUST)
+		freq = -MAX_FREQUENCY_ADJUST;
+
+	imsg_compose(ibuf_main, IMSG_ADJFREQ, 0, 0, &freq, sizeof(freq));
+	conf->freq.xy = 0.0;
+	conf->freq.x = 0.0;
+	conf->freq.y = 0.0;
+	conf->freq.xx = 0.0;
+	conf->freq.samples = 0;
+	conf->freq.overall_offset = 0.0;
+}
+
 int
 priv_adjtime(void)
 {
@@ -491,6 +543,8 @@ priv_adjtime(void)
 	imsg_compose(ibuf_main, IMSG_ADJTIME, 0, 0,
 	    &offset_median, sizeof(offset_median));
 
+	priv_adjfreq(offset_median);
+
 	conf->status.reftime = gettime();
 	conf->status.stratum++;	/* one more than selected peer */
 	update_scale(offset_median);
diff --git a/usr.sbin/ntpd/ntpd.c b/usr.sbin/ntpd/ntpd.c
index 17b08b617ff..b2e81ac57a1 100644
--- a/usr.sbin/ntpd/ntpd.c
+++ b/usr.sbin/ntpd/ntpd.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: ntpd.c,v 1.42 2006/06/07 06:29:03 otto Exp $ */
+/*	$OpenBSD: ntpd.c,v 1.43 2006/06/17 18:40:42 otto Exp $ */
 
 /*
  * Copyright (c) 2003, 2004 Henning Brauer <henning@openbsd.org>
@@ -38,6 +38,7 @@ int		main(int, char *[]);
 int		check_child(pid_t, const char *);
 int		dispatch_imsg(struct ntpd_conf *);
 int		ntpd_adjtime(double);
+void		ntpd_adjfreq(double);
 void		ntpd_settime(double);
 
 volatile sig_atomic_t	 quit = 0;
@@ -270,6 +271,12 @@ dispatch_imsg(struct ntpd_conf *conf)
 			n = ntpd_adjtime(d);
 			imsg_compose(ibuf, IMSG_ADJTIME, 0, 0, &n, sizeof(n));
 			break;
+		case IMSG_ADJFREQ:
+			if (imsg.hdr.len != IMSG_HEADER_SIZE + sizeof(d))
+				fatalx("invalid IMSG_ADJFREQ received");
+			memcpy(&d, imsg.data, sizeof(d));
+			ntpd_adjfreq(d);
+			break;
 		case IMSG_SETTIME:
 			if (imsg.hdr.len != IMSG_HEADER_SIZE + sizeof(d))
 				fatalx("invalid IMSG_SETTIME received");
@@ -336,6 +343,28 @@ ntpd_adjtime(double d)
 }
 
 void
+ntpd_adjfreq(double relfreq)
+{
+	int64_t curfreq;
+	
+	if (adjfreq(NULL, &curfreq) == -1) {
+		log_warn("adjfreq failed");
+		return;
+	}
+
+	/*
+	 * adjfreq's unit is ns/s shifted left 32; convert relfreq to
+	 * that unit before adding. We log values in part per million.
+	 */
+	curfreq += relfreq * 1e9 * (1LL << 32);
+	log_info("adjusting clock frequency by %f to %fppm", relfreq * 1e6,
+	    curfreq / 1e3 / (1LL << 32));
+
+	if (adjfreq(&curfreq, NULL) == -1) 
+		log_warn("adjfreq failed");
+}
+
+void
 ntpd_settime(double d)
 {
 	struct timeval	tv, curtime;
diff --git a/usr.sbin/ntpd/ntpd.h b/usr.sbin/ntpd/ntpd.h
index 76aa6d098fa..3565102c7dc 100644
--- a/usr.sbin/ntpd/ntpd.h
+++ b/usr.sbin/ntpd/ntpd.h
@@ -1,4 +1,4 @@
-/*	$OpenBSD: ntpd.h,v 1.71 2006/06/07 06:29:03 otto Exp $ */
+/*	$OpenBSD: ntpd.h,v 1.72 2006/06/17 18:40:42 otto Exp $ */
 
 /*
  * Copyright (c) 2003, 2004 Henning Brauer <henning@openbsd.org>
@@ -56,7 +56,10 @@
 #define	OFFSET_ARRAY_SIZE	8
 #define	SETTIME_MIN_OFFSET	180	/* min offset for settime at start */
 #define	SETTIME_TIMEOUT		15	/* max seconds to wait with -s */
-#define	LOG_NEGLIGEE		128	/* negligible drift to not log (ms) */
+#define	LOG_NEGLIGEE		32	/* negligible drift to not log (ms) */
+#define	FREQUENCY_SAMPLES	8	/* samples for est. of permanent drift */
+#define	MAX_FREQUENCY_ADJUST	128e-5	/* max correction per iteration */
+
 
 #define	SENSOR_DATA_MAXAGE	15*60
 #define	SENSOR_QUERY_INTERVAL	30
@@ -143,12 +146,20 @@ struct ntp_conf_sensor {
 	u_int8_t				 weight;
 };
 
+struct ntp_freq {
+	double				overall_offset;
+	double				x, y;
+	double				xx, xy;
+	int				samples;
+};
+
 struct ntpd_conf {
 	TAILQ_HEAD(listen_addrs, listen_addr)		listen_addrs;
 	TAILQ_HEAD(ntp_peers, ntp_peer)			ntp_peers;
 	TAILQ_HEAD(ntp_sensors, ntp_sensor)		ntp_sensors;
 	TAILQ_HEAD(ntp_conf_sensors, ntp_conf_sensor)	ntp_conf_sensors;	
 	struct ntp_status				status;
+	struct ntp_freq					freq;
 	u_int8_t					listen_all;
 	u_int8_t					settime;
 	u_int8_t					debug;
@@ -190,6 +201,7 @@ struct imsgbuf {
 enum imsg_type {
 	IMSG_NONE,
 	IMSG_ADJTIME,
+	IMSG_ADJFREQ,
 	IMSG_SETTIME,
 	IMSG_HOST_DNS
 };