Commit message: ntpd: respond only to client and symmetric active packets

The busybox NTP implementation doesn't check the NTP mode of packets
received on the server port and responds to any packet with the right
size. This includes responses from another NTP server. An attacker can
send a packet with a spoofed source address in order to create an
infinite loop of responses between two busybox NTP servers. Adding
more packets to the loop increases the traffic between the servers
until one of them has a fully loaded CPU and/or network.

Signed-off-by: Miroslav Lichvar <mlichvar@redhat.com>
Signed-off-by: Denys Vlasenko <vda.linux@googlemail.com>
CVE: CVE-2016-6301
CWE: CWE-399
Vulnerability description: The recv_and_process_client_pkt function in networking/ntpd.c in busybox allows remote attackers to cause a denial of service (CPU and bandwidth consumption) via a forged NTP packet, which triggers a communication loop.

Project: busybox

static double LOG2D(int a)
{
	if (a < 0)
		return 1.0 / (1UL << -a);
	return 1UL << a;
}

Project: busybox

static ALWAYS_INLINE double MAXD(double a, double b)
{
	if (a > b)
		return a;
	return b;
}

Project: busybox

static ALWAYS_INLINE double MIND(double a, double b)
{
	if (a < b)
		return a;
	return b;
}

Project: busybox

static ALWAYS_INLINE double SQRT(double X)
{
	/* If this arch doesn't use IEEE 754 floats, fall back to using libm */
	if (sizeof(float) != 4)
		return sqrt(X);

	/* This avoids needing libm, saves about 0.5k on x86-32 */
	return my_SQRT(X);
}

Project: busybox

static ALWAYS_INLINE double SQUARE(double x)
{
	return x * x;
}

Project: busybox

add_peers(const char *s)
{
	llist_t *item;
	peer_t *p;

	p = xzalloc(sizeof(*p) + strlen(s));
	strcpy(p->p_hostname, s);
	resolve_peer_hostname(p, /*loop_on_fail=*/ 1);

	/* Names like N.<country2chars>.pool.ntp.org are randomly resolved
	 * to a pool of machines. Sometimes different N's resolve to the same IP.
	 * It is not useful to have two peers with same IP. We skip duplicates.
	 */
	for (item = G.ntp_peers; item != NULL; item = item->link) {
		peer_t *pp = (peer_t *) item->data;
		if (strcmp(p->p_dotted, pp->p_dotted) == 0) {
			bb_error_msg("duplicate peer %s (%s)", s, p->p_dotted);
			free(p->p_lsa);
			free(p->p_dotted);
			free(p);
			return;
		}
	}

	p->p_fd = -1;
	p->p_xmt_msg.m_status = MODE_CLIENT | (NTP_VERSION << 3);
	p->next_action_time = G.cur_time; /* = set_next(p, 0); */
	reset_peer_stats(p, STEP_THRESHOLD);

	llist_add_to(&G.ntp_peers, p);
	G.peer_cnt++;
}

Project: busybox

adjust_poll(int count)
{
	G.polladj_count += count;
	if (G.polladj_count > POLLADJ_LIMIT) {
		G.polladj_count = 0;
		if (G.poll_exp < MAXPOLL) {
			G.poll_exp++;
			VERB4 bb_error_msg("polladj: discipline_jitter:%f ++poll_exp=%d",
					G.discipline_jitter, G.poll_exp);
		}
	} else if (G.polladj_count < -POLLADJ_LIMIT || (count < 0 && G.poll_exp > BIGPOLL)) {
		G.polladj_count = 0;
		if (G.poll_exp > MINPOLL) {
			llist_t *item;

			G.poll_exp--;
			/* Correct p->next_action_time in each peer
			 * which waits for sending, so that they send earlier.
			 * Old pp->next_action_time are on the order
			 * of t + (1 << old_poll_exp) + small_random,
			 * we simply need to subtract ~half of that.
			 */
			for (item = G.ntp_peers; item != NULL; item = item->link) {
				peer_t *pp = (peer_t *) item->data;
				if (pp->p_fd < 0)
					pp->next_action_time -= (1 << G.poll_exp);
			}
			VERB4 bb_error_msg("polladj: discipline_jitter:%f --poll_exp=%d",
					G.discipline_jitter, G.poll_exp);
		}
	} else {
		VERB4 bb_error_msg("polladj: count:%d", G.polladj_count);
	}
}

Project: busybox

static void clamp_pollexp_and_set_MAXSTRAT(void)
{
	if (G.poll_exp < MINPOLL)
		G.poll_exp = MINPOLL;
	if (G.poll_exp > BIGPOLL)
		G.poll_exp = BIGPOLL;
	G.polladj_count = 0;
	G.stratum = MAXSTRAT;
}

Project: busybox

compare_point_edge(const void *aa, const void *bb)
{
	const point_t *a = aa;
	const point_t *b = bb;
	if (a->edge < b->edge) {
		return -1;
	}
	return (a->edge > b->edge);
}

Project: busybox

compare_survivor_metric(const void *aa, const void *bb)
{
	const survivor_t *a = aa;
	const survivor_t *b = bb;
	if (a->metric < b->metric) {
		return -1;
	}
	return (a->metric > b->metric);
}

Project: busybox

d_to_sfp(double d)
{
	s_fixedpt_t sfp;
	sfp.int_parts = (uint16_t)d;
	sfp.fractions = (uint16_t)((d - sfp.int_parts) * USHRT_MAX);
	sfp.int_parts = htons(sfp.int_parts);
	sfp.fractions = htons(sfp.fractions);
	return sfp;
}

Project: busybox

d_to_tv(double d, struct timeval *tv)
{
	tv->tv_sec = (long)d;
	tv->tv_usec = (d - tv->tv_sec) * 1000000;
}

Project: busybox

do_sendto(int fd,
		const struct sockaddr *from, const struct sockaddr *to, socklen_t addrlen,
		msg_t *msg, ssize_t len)
{
	ssize_t ret;

	errno = 0;
	if (!from) {
		ret = sendto(fd, msg, len, MSG_DONTWAIT, to, addrlen);
	} else {
		ret = send_to_from(fd, msg, len, MSG_DONTWAIT, to, from, addrlen);
	}
	if (ret != len) {
		bb_perror_msg("send failed");
		return -1;
	}
	return 0;
}

Project: busybox

filter_datapoints(peer_t *p)
{
	int i, idx;
	double sum, wavg;
	datapoint_t *fdp;

#if 0
/* Simulations have shown that use of *averaged* offset for p->filter_offset
 * is in fact worse than simply using last received one: with large poll intervals
 * (>= 2048) averaging code uses offset values which are outdated by hours,
 * and time/frequency correction goes totally wrong when fed essentially bogus offsets.
 */
	int got_newest;
	double minoff, maxoff, w;
	double x = x; /* for compiler */
	double oldest_off = oldest_off;
	double oldest_age = oldest_age;
	double newest_off = newest_off;
	double newest_age = newest_age;

	fdp = p->filter_datapoint;

	minoff = maxoff = fdp[0].d_offset;
	for (i = 1; i < NUM_DATAPOINTS; i++) {
		if (minoff > fdp[i].d_offset)
			minoff = fdp[i].d_offset;
		if (maxoff < fdp[i].d_offset)
			maxoff = fdp[i].d_offset;
	}

	idx = p->datapoint_idx; /* most recent datapoint's index */
	/* Average offset:
	 * Drop two outliers and take weighted average of the rest:
	 * most_recent/2 + older1/4 + older2/8 ... + older5/32 + older6/32
	 * we use older6/32, not older6/64 since sum of weights should be 1:
	 * 1/2 + 1/4 + 1/8 + 1/16 + 1/32 + 1/32 = 1
	 */
	wavg = 0;
	w = 0.5;
	/*                     n-1
	 *                     ---    dispersion(i)
	 * filter_dispersion =  \     -------------
	 *                      /       (i+1)
	 *                     ---     2
	 *                     i=0
	 */
	got_newest = 0;
	sum = 0;
	for (i = 0; i < NUM_DATAPOINTS; i++) {
		VERB5 {
			bb_error_msg("datapoint[%d]: off:%f disp:%f(%f) age:%f%s",
				i,
				fdp[idx].d_offset,
				fdp[idx].d_dispersion, dispersion(&fdp[idx]),
				G.cur_time - fdp[idx].d_recv_time,
				(minoff == fdp[idx].d_offset || maxoff == fdp[idx].d_offset)
					? " (outlier by offset)" : ""
			);
		}

		sum += dispersion(&fdp[idx]) / (2 << i);

		if (minoff == fdp[idx].d_offset) {
			minoff -= 1; /* so that we don't match it ever again */
		} else
		if (maxoff == fdp[idx].d_offset) {
			maxoff += 1;
		} else {
			oldest_off = fdp[idx].d_offset;
			oldest_age = G.cur_time - fdp[idx].d_recv_time;
			if (!got_newest) {
				got_newest = 1;
				newest_off = oldest_off;
				newest_age = oldest_age;
			}
			x = oldest_off * w;
			wavg += x;
			w /= 2;
		}

		idx = (idx - 1) & (NUM_DATAPOINTS - 1);
	}
	p->filter_dispersion = sum;
	wavg += x; /* add another older6/64 to form older6/32 */
	/* Fix systematic underestimation with large poll intervals.
	 * Imagine that we still have a bit of uncorrected drift,
	 * and poll interval is big (say, 100 sec). Offsets form a progression:
	 * 0.0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 - 0.7 is most recent.
	 * The algorithm above drops 0.0 and 0.7 as outliers,
	 * and then we have this estimation, ~25% off from 0.7:
	 * 0.1/32 + 0.2/32 + 0.3/16 + 0.4/8 + 0.5/4 + 0.6/2 = 0.503125
	 */
	x = oldest_age - newest_age;
	if (x != 0) {
		x = newest_age / x; /* in above example, 100 / (600 - 100) */
		if (x < 1) { /* paranoia check */
			x = (newest_off - oldest_off) * x; /* 0.5 * 100/500 = 0.1 */
			wavg += x;
		}
	}
	p->filter_offset = wavg;

#else

	fdp = p->filter_datapoint;
	idx = p->datapoint_idx; /* most recent datapoint's index */

	/* filter_offset: simply use the most recent value */
	p->filter_offset = fdp[idx].d_offset;

	/*                     n-1
	 *                     ---    dispersion(i)
	 * filter_dispersion =  \     -------------
	 *                      /       (i+1)
	 *                     ---     2
	 *                     i=0
	 */
	wavg = 0;
	sum = 0;
	for (i = 0; i < NUM_DATAPOINTS; i++) {
		sum += dispersion(&fdp[idx]) / (2 << i);
		wavg += fdp[idx].d_offset;
		idx = (idx - 1) & (NUM_DATAPOINTS - 1);
	}
	wavg /= NUM_DATAPOINTS;
	p->filter_dispersion = sum;
#endif

	/*                  +-----                 -----+ ^ 1/2
	 *                  |       n-1                 |
	 *                  |       ---                 |
	 *                  |  1    \                2  |
	 * filter_jitter =  | --- * /  (avg-offset_j)   |
	 *                  |  n    ---                 |
	 *                  |       j=0                 |
	 *                  +-----                 -----+
	 * where n is the number of valid datapoints in the filter (n > 1);
	 * if filter_jitter < precision then filter_jitter = precision
	 */
	sum = 0;
	for (i = 0; i < NUM_DATAPOINTS; i++) {
		sum += SQUARE(wavg - fdp[i].d_offset);
	}
	sum = SQRT(sum / NUM_DATAPOINTS);
	p->filter_jitter = sum > G_precision_sec ? sum : G_precision_sec;

	VERB4 bb_error_msg("filter offset:%+f disp:%f jitter:%f",
			p->filter_offset,
			p->filter_dispersion,
			p->filter_jitter);
}

Project: busybox

fit(peer_t *p, double rd)
{
	if ((p->reachable_bits & (p->reachable_bits-1)) == 0) {
		/* One or zero bits in reachable_bits */
		VERB4 bb_error_msg("peer %s unfit for selection: unreachable", p->p_dotted);
		return 0;
	}
#if 0 /* we filter out such packets earlier */
	if ((p->lastpkt_status & LI_ALARM) == LI_ALARM
	 || p->lastpkt_stratum >= MAXSTRAT
	) {
		VERB4 bb_error_msg("peer %s unfit for selection: bad status/stratum", p->p_dotted);
		return 0;
	}
#endif
	/* rd is root_distance(p) */
	if (rd > MAXDIST + FREQ_TOLERANCE * (1 << G.poll_exp)) {
		VERB4 bb_error_msg("peer %s unfit for selection: root distance too high", p->p_dotted);
		return 0;
	}
	return 1;
}

Project: busybox

gettime1900d(void)
{
	struct timeval tv;
	gettimeofday(&tv, NULL); /* never fails */
	G.cur_time = tv.tv_sec + (1.0e-6 * tv.tv_usec) + OFFSET_1900_1970;
	return G.cur_time;
}

Project: busybox

static NOINLINE double my_SQRT(double X)
{
	union {
		float   f;
		int32_t i;
	} v;
	double invsqrt;
	double Xhalf = X * 0.5;

	/* Fast and good approximation to 1/sqrt(X), black magic */
	v.f = X;
	/*v.i = 0x5f3759df - (v.i >> 1);*/
	v.i = 0x5f375a86 - (v.i >> 1); /* - this constant is slightly better */
	invsqrt = v.f; /* better than 0.2% accuracy */

	/* Refining it using Newton's method: x1 = x0 - f(x0)/f'(x0)
	 * f(x) = 1/(x*x) - X  (f==0 when x = 1/sqrt(X))
	 * f'(x) = -2/(x*x*x)
	 * f(x)/f'(x) = (X - 1/(x*x)) / (2/(x*x*x)) = X*x*x*x/2 - x/2
	 * x1 = x0 - (X*x0*x0*x0/2 - x0/2) = 1.5*x0 - X*x0*x0*x0/2 = x0*(1.5 - (X/2)*x0*x0)
	 */
	invsqrt = invsqrt * (1.5 - Xhalf * invsqrt * invsqrt); /* ~0.05% accuracy */
	/* invsqrt = invsqrt * (1.5 - Xhalf * invsqrt * invsqrt); 2nd iter: ~0.0001% accuracy */
	/* With 4 iterations, more than half results will be exact,
	 * at 6th iterations result stabilizes with about 72% results exact.
	 * We are well satisfied with 0.05% accuracy.
	 */

	return X * invsqrt; /* X * 1/sqrt(X) ~= sqrt(X) */
}

Project: busybox

static NOINLINE void ntp_init(char **argv)
{
	unsigned opts;
	llist_t *peers;

	srand(getpid());

	if (getuid())
		bb_error_msg_and_die(bb_msg_you_must_be_root);

	/* Set some globals */
	G.discipline_jitter = G_precision_sec;
	G.stratum = MAXSTRAT;
	if (BURSTPOLL != 0)
		G.poll_exp = BURSTPOLL; /* speeds up initial sync */
	G.last_script_run = G.reftime = G.last_update_recv_time = gettime1900d(); /* sets G.cur_time too */

	/* Parse options */
	peers = NULL;
	opt_complementary = "dd:wn"  /* -d: counter; -p: list; -w implies -n */
		IF_FEATURE_NTPD_SERVER(":Il"); /* -I implies -l */
	opts = getopt32(argv,
			"nqNx" /* compat */
			"wp:*S:"IF_FEATURE_NTPD_SERVER("l") /* NOT compat */
			IF_FEATURE_NTPD_SERVER("I:") /* compat */
			"d" /* compat */
			"46aAbgL", /* compat, ignored */
			&peers,&G.script_name,
#if ENABLE_FEATURE_NTPD_SERVER
			&G.if_name,
#endif
			&G.verbose);


#if ENABLE_FEATURE_NTPD_SERVER
	G_listen_fd = -1;
	if (opts & OPT_l) {
		G_listen_fd = create_and_bind_dgram_or_die(NULL, 123);
		if (G.if_name) {
			if (setsockopt_bindtodevice(G_listen_fd, G.if_name))
				xfunc_die();
		}
		socket_want_pktinfo(G_listen_fd);
		setsockopt_int(G_listen_fd, IPPROTO_IP, IP_TOS, IPTOS_LOWDELAY);
	}
#endif
	/* I hesitate to set -20 prio. -15 should be high enough for timekeeping */
	if (opts & OPT_N)
		setpriority(PRIO_PROCESS, 0, -15);

	/* add_peers() calls can retry DNS resolution (possibly forever).
	 * Daemonize before them, or else boot can stall forever.
	 */
	if (!(opts & OPT_n)) {
		bb_daemonize_or_rexec(DAEMON_DEVNULL_STDIO, argv);
		logmode = LOGMODE_NONE;
	}

	if (peers) {
		while (peers)
			add_peers(llist_pop(&peers));
	}
#if ENABLE_FEATURE_NTPD_CONF
	else {
		parser_t *parser;
		char *token[3];

		parser = config_open("/etc/ntp.conf");
		while (config_read(parser, token, 3, 1, "# \t", PARSE_NORMAL)) {
			if (strcmp(token[0], "server") == 0 && token[1]) {
				add_peers(token[1]);
				continue;
			}
			bb_error_msg("skipping %s:%u: unimplemented command '%s'",
				"/etc/ntp.conf", parser->lineno, token[0]
			);
		}
		config_close(parser);
	}
#endif
	if (G.peer_cnt == 0) {
		if (!(opts & OPT_l))
			bb_show_usage();
		/* -l but no peers: "stratum 1 server" mode */
		G.stratum = 1;
	}
	/* If network is up, syncronization occurs in ~10 seconds.
	 * We give "ntpd -q" 10 seconds to get first reply,
	 * then another 50 seconds to finish syncing.
	 *
	 * I tested ntpd 4.2.6p1 and apparently it never exits
	 * (will try forever), but it does not feel right.
	 * The goal of -q is to act like ntpdate: set time
	 * after a reasonably small period of polling, or fail.
	 */
	if (opts & OPT_q) {
		option_mask32 |= OPT_qq;
		alarm(10);
	}

	bb_signals(0
		| (1 << SIGTERM)
		| (1 << SIGINT)
		| (1 << SIGALRM)
		, record_signo
	);
	bb_signals(0
		| (1 << SIGPIPE)
		| (1 << SIGCHLD)
		, SIG_IGN
	);
}

Project: busybox

int ntpd_main(int argc UNUSED_PARAM, char **argv)
{
#undef G
	struct globals G;
	struct pollfd *pfd;
	peer_t **idx2peer;
	unsigned cnt;

	memset(&G, 0, sizeof(G));
	SET_PTR_TO_GLOBALS(&G);

	ntp_init(argv);

	/* If ENABLE_FEATURE_NTPD_SERVER, + 1 for listen_fd: */
	cnt = G.peer_cnt + ENABLE_FEATURE_NTPD_SERVER;
	idx2peer = xzalloc(sizeof(idx2peer[0]) * cnt);
	pfd = xzalloc(sizeof(pfd[0]) * cnt);

	/* Countdown: we never sync before we sent INITIAL_SAMPLES+1
	 * packets to each peer.
	 * NB: if some peer is not responding, we may end up sending
	 * fewer packets to it and more to other peers.
	 * NB2: sync usually happens using INITIAL_SAMPLES packets,
	 * since last reply does not come back instantaneously.
	 */
	cnt = G.peer_cnt * (INITIAL_SAMPLES + 1);

	write_pidfile(CONFIG_PID_FILE_PATH "/ntpd.pid");

	while (!bb_got_signal) {
		llist_t *item;
		unsigned i, j;
		int nfds, timeout;
		double nextaction;

		/* Nothing between here and poll() blocks for any significant time */

		nextaction = G.cur_time + 3600;

		i = 0;
#if ENABLE_FEATURE_NTPD_SERVER
		if (G_listen_fd != -1) {
			pfd[0].fd = G_listen_fd;
			pfd[0].events = POLLIN;
			i++;
		}
#endif
		/* Pass over peer list, send requests, time out on receives */
		for (item = G.ntp_peers; item != NULL; item = item->link) {
			peer_t *p = (peer_t *) item->data;

			if (p->next_action_time <= G.cur_time) {
				if (p->p_fd == -1) {
					/* Time to send new req */
					if (--cnt == 0) {
						VERB4 bb_error_msg("disabling burst mode");
						G.polladj_count = 0;
						G.poll_exp = MINPOLL;
					}
					send_query_to_peer(p);
				} else {
					/* Timed out waiting for reply */
					close(p->p_fd);
					p->p_fd = -1;
					/* If poll interval is small, increase it */
					if (G.poll_exp < BIGPOLL)
						adjust_poll(MINPOLL);
					timeout = poll_interval(NOREPLY_INTERVAL);
					bb_error_msg("timed out waiting for %s, reach 0x%02x, next query in %us",
							p->p_dotted, p->reachable_bits, timeout);

					/* What if don't see it because it changed its IP? */
					if (p->reachable_bits == 0)
						resolve_peer_hostname(p, /*loop_on_fail=*/ 0);

					set_next(p, timeout);
				}
			}

			if (p->next_action_time < nextaction)
				nextaction = p->next_action_time;

			if (p->p_fd >= 0) {
				/* Wait for reply from this peer */
				pfd[i].fd = p->p_fd;
				pfd[i].events = POLLIN;
				idx2peer[i] = p;
				i++;
			}
		}

		timeout = nextaction - G.cur_time;
		if (timeout < 0)
			timeout = 0;
		timeout++; /* (nextaction - G.cur_time) rounds down, compensating */

		/* Here we may block */
		VERB2 {
			if (i > (ENABLE_FEATURE_NTPD_SERVER && G_listen_fd != -1)) {
				/* We wait for at least one reply.
				 * Poll for it, without wasting time for message.
				 * Since replies often come under 1 second, this also
				 * reduces clutter in logs.
				 */
				nfds = poll(pfd, i, 1000);
				if (nfds != 0)
					goto did_poll;
				if (--timeout <= 0)
					goto did_poll;
			}
			bb_error_msg("poll:%us sockets:%u interval:%us", timeout, i, 1 << G.poll_exp);
		}
		nfds = poll(pfd, i, timeout * 1000);
 did_poll:
		gettime1900d(); /* sets G.cur_time */
		if (nfds <= 0) {
			if (!bb_got_signal /* poll wasn't interrupted by a signal */
			 && G.cur_time - G.last_script_run > 11*60
			) {
				/* Useful for updating battery-backed RTC and such */
				run_script("periodic", G.last_update_offset);
				gettime1900d(); /* sets G.cur_time */
			}
			goto check_unsync;
		}

		/* Process any received packets */
		j = 0;
#if ENABLE_FEATURE_NTPD_SERVER
		if (G.listen_fd != -1) {
			if (pfd[0].revents /* & (POLLIN|POLLERR)*/) {
				nfds--;
				recv_and_process_client_pkt(/*G.listen_fd*/);
				gettime1900d(); /* sets G.cur_time */
			}
			j = 1;
		}
#endif
		for (; nfds != 0 && j < i; j++) {
			if (pfd[j].revents /* & (POLLIN|POLLERR)*/) {
				/*
				 * At init, alarm was set to 10 sec.
				 * Now we did get a reply.
				 * Increase timeout to 50 seconds to finish syncing.
				 */
				if (option_mask32 & OPT_qq) {
					option_mask32 &= ~OPT_qq;
					alarm(50);
				}
				nfds--;
				recv_and_process_peer_pkt(idx2peer[j]);
				gettime1900d(); /* sets G.cur_time */
			}
		}

 check_unsync:
		if (G.ntp_peers && G.stratum != MAXSTRAT) {
			for (item = G.ntp_peers; item != NULL; item = item->link) {
				peer_t *p = (peer_t *) item->data;
				if (p->reachable_bits)
					goto have_reachable_peer;
			}
			/* No peer responded for last 8 packets, panic */
			clamp_pollexp_and_set_MAXSTRAT();
			run_script("unsync", 0.0);
 have_reachable_peer: ;
		}
	} /* while (!bb_got_signal) */

	remove_pidfile(CONFIG_PID_FILE_PATH "/ntpd.pid");
	kill_myself_with_sig(bb_got_signal);
}

Project: busybox

poll_interval(int upper_bound)
{
	unsigned interval, r, mask;
	interval = 1 << G.poll_exp;
	if (interval > upper_bound)
		interval = upper_bound;
	mask = ((interval-1) >> 4) | 1;
	r = rand();
	interval += r & mask; /* ~ random(0..1) * interval/16 */
	VERB4 bb_error_msg("chose poll interval:%u (poll_exp:%d)", interval, G.poll_exp);
	return interval;
}

Project: busybox

recv_and_process_peer_pkt(peer_t *p)
{
	int         rc;
	ssize_t     size;
	msg_t       msg;
	double      T1, T2, T3, T4;
	double      offset;
	double      prev_delay, delay;
	unsigned    interval;
	datapoint_t *datapoint;
	peer_t      *q;

	offset = 0;

	/* We can recvfrom here and check from.IP, but some multihomed
	 * ntp servers reply from their *other IP*.
	 * TODO: maybe we should check at least what we can: from.port == 123?
	 */
 recv_again:
	size = recv(p->p_fd, &msg, sizeof(msg), MSG_DONTWAIT);
	if (size < 0) {
		if (errno == EINTR)
			/* Signal caught */
			goto recv_again;
		if (errno == EAGAIN)
			/* There was no packet after all
			 * (poll() returning POLLIN for a fd
			 * is not a ironclad guarantee that data is there)
			 */
			return;
		/*
		 * If you need a different handling for a specific
		 * errno, always explain it in comment.
		 */
		bb_perror_msg_and_die("recv(%s) error", p->p_dotted);
	}

	if (size != NTP_MSGSIZE_NOAUTH && size != NTP_MSGSIZE) {
		bb_error_msg("malformed packet received from %s", p->p_dotted);
		return;
	}

	if (msg.m_orgtime.int_partl != p->p_xmt_msg.m_xmttime.int_partl
	 || msg.m_orgtime.fractionl != p->p_xmt_msg.m_xmttime.fractionl
	) {
		/* Somebody else's packet */
		return;
	}

	/* We do not expect any more packets from this peer for now.
	 * Closing the socket informs kernel about it.
	 * We open a new socket when we send a new query.
	 */
	close(p->p_fd);
	p->p_fd = -1;

	if ((msg.m_status & LI_ALARM) == LI_ALARM
	 || msg.m_stratum == 0
	 || msg.m_stratum > NTP_MAXSTRATUM
	) {
		bb_error_msg("reply from %s: peer is unsynced", p->p_dotted);
		/*
		 * Stratum 0 responses may have commands in 32-bit m_refid field:
		 * "DENY", "RSTR" - peer does not like us at all,
		 * "RATE" - peer is overloaded, reduce polling freq.
		 * If poll interval is small, increase it.
		 */
		if (G.poll_exp < BIGPOLL)
			goto increase_interval;
		goto pick_normal_interval;
	}


	/*
	 * From RFC 2030 (with a correction to the delay math):
	 *
	 * Timestamp Name          ID   When Generated
	 * ------------------------------------------------------------
	 * Originate Timestamp     T1   time request sent by client
	 * Receive Timestamp       T2   time request received by server
	 * Transmit Timestamp      T3   time reply sent by server
	 * Destination Timestamp   T4   time reply received by client
	 *
	 * The roundtrip delay and local clock offset are defined as
	 *
	 * delay = (T4 - T1) - (T3 - T2); offset = ((T2 - T1) + (T3 - T4)) / 2
	 */
	T1 = p->p_xmttime;
	T2 = lfp_to_d(msg.m_rectime);
	T3 = lfp_to_d(msg.m_xmttime);
	T4 = G.cur_time;

	/* The delay calculation is a special case. In cases where the
	 * server and client clocks are running at different rates and
	 * with very fast networks, the delay can appear negative. In
	 * order to avoid violating the Principle of Least Astonishment,
	 * the delay is clamped not less than the system precision.
	 */
	delay = (T4 - T1) - (T3 - T2);
	if (delay < G_precision_sec)
		delay = G_precision_sec;
	/*
	 * If this packet's delay is much bigger than the last one,
	 * it's better to just ignore it than use its much less precise value.
	 */
	prev_delay = p->p_raw_delay;
	p->p_raw_delay = delay;
	if (p->reachable_bits && delay > prev_delay * BAD_DELAY_GROWTH) {
		bb_error_msg("reply from %s: delay %f is too high, ignoring", p->p_dotted, delay);
		goto pick_normal_interval;
	}

	p->lastpkt_delay = delay;
	p->lastpkt_recv_time = T4;
	VERB6 bb_error_msg("%s->lastpkt_recv_time=%f", p->p_dotted, p->lastpkt_recv_time);
	p->lastpkt_status = msg.m_status;
	p->lastpkt_stratum = msg.m_stratum;
	p->lastpkt_rootdelay = sfp_to_d(msg.m_rootdelay);
	p->lastpkt_rootdisp = sfp_to_d(msg.m_rootdisp);
	p->lastpkt_refid = msg.m_refid;

	p->datapoint_idx = p->reachable_bits ? (p->datapoint_idx + 1) % NUM_DATAPOINTS : 0;
	datapoint = &p->filter_datapoint[p->datapoint_idx];
	datapoint->d_recv_time = T4;
	datapoint->d_offset    = offset = ((T2 - T1) + (T3 - T4)) / 2;
	datapoint->d_dispersion = LOG2D(msg.m_precision_exp) + G_precision_sec;
	if (!p->reachable_bits) {
		/* 1st datapoint ever - replicate offset in every element */
		int i;
		for (i = 0; i < NUM_DATAPOINTS; i++) {
			p->filter_datapoint[i].d_offset = offset;
		}
	}

	p->reachable_bits |= 1;
	if ((MAX_VERBOSE && G.verbose) || (option_mask32 & OPT_w)) {
		bb_error_msg("reply from %s: offset:%+f delay:%f status:0x%02x strat:%d refid:0x%08x rootdelay:%f reach:0x%02x",
			p->p_dotted,
			offset,
			p->lastpkt_delay,
			p->lastpkt_status,
			p->lastpkt_stratum,
			p->lastpkt_refid,
			p->lastpkt_rootdelay,
			p->reachable_bits
			/* not shown: m_ppoll, m_precision_exp, m_rootdisp,
			 * m_reftime, m_orgtime, m_rectime, m_xmttime
			 */
		);
	}

	/* Muck with statictics and update the clock */
	filter_datapoints(p);
	q = select_and_cluster();
	rc = 0;
	if (q) {
		if (!(option_mask32 & OPT_w)) {
			rc = update_local_clock(q);
#if 0

			/* If drift is dangerously large, immediately
			 * drop poll interval one step down.
			 */
			if (fabs(q->filter_offset) >= POLLDOWN_OFFSET) {
				VERB4 bb_error_msg("offset:%+f > POLLDOWN_OFFSET", q->filter_offset);
				adjust_poll(-POLLADJ_LIMIT * 3);
				rc = 0;
			}
#endif
		}
	} else {
		/* No peer selected.
		 * If poll interval is small, increase it.
		 */
		if (G.poll_exp < BIGPOLL)
			goto increase_interval;
	}

	if (rc != 0) {
		/* Adjust the poll interval by comparing the current offset
		 * with the clock jitter. If the offset is less than
		 * the clock jitter times a constant, then the averaging interval
		 * is increased, otherwise it is decreased. A bit of hysteresis
		 * helps calm the dance. Works best using burst mode.
		 */
		if (rc > 0 && G.offset_to_jitter_ratio <= POLLADJ_GATE) {
			/* was += G.poll_exp but it is a bit
			 * too optimistic for my taste at high poll_exp's */
 increase_interval:
			adjust_poll(MINPOLL);
		} else {
			VERB3 if (rc > 0)
				bb_error_msg("want smaller interval: offset/jitter = %u",
					G.offset_to_jitter_ratio);
			adjust_poll(-G.poll_exp * 2);
		}
	}

	/* Decide when to send new query for this peer */
 pick_normal_interval:
	interval = poll_interval(INT_MAX);
	if (fabs(offset) >= BIGOFF && interval > BIGOFF_INTERVAL) {
		/* If we are synced, offsets are less than SLEW_THRESHOLD,
		 * or at the very least not much larger than it.
		 * Now we see a largish one.
		 * Either this peer is feeling bad, or packet got corrupted,
		 * or _our_ clock is wrong now and _all_ peers will show similar
		 * largish offsets too.
		 * I observed this with laptop suspend stopping clock.
		 * In any case, it makes sense to make next request soonish:
		 * cases 1 and 2: get a better datapoint,
		 * case 3: allows to resync faster.
		 */
		interval = BIGOFF_INTERVAL;
	}

	set_next(p, interval);
}

Project: busybox

reset_peer_stats(peer_t *p, double offset)
{
	int i;
	bool small_ofs = fabs(offset) < STEP_THRESHOLD;

	/* Used to set p->filter_datapoint[i].d_dispersion = MAXDISP
	 * and clear reachable bits, but this proved to be too agressive:
	 * after step (tested with suspending laptop for ~30 secs),
	 * this caused all previous data to be considered invalid,
	 * making us needing to collect full ~8 datapoins per peer
	 * after step in order to start trusting them.
	 * In turn, this was making poll interval decrease even after
	 * step was done. (Poll interval decreases already before step
	 * in this scenario, because we see large offsets and end up with
	 * no good peer to select).
	 */

	for (i = 0; i < NUM_DATAPOINTS; i++) {
		if (small_ofs) {
			p->filter_datapoint[i].d_recv_time += offset;
			if (p->filter_datapoint[i].d_offset != 0) {
				p->filter_datapoint[i].d_offset -= offset;
			}
		} else {
			p->filter_datapoint[i].d_recv_time  = G.cur_time;
			p->filter_datapoint[i].d_offset     = 0;
			/*p->filter_datapoint[i].d_dispersion = MAXDISP;*/
		}
	}
	if (small_ofs) {
		p->lastpkt_recv_time += offset;
	} else {
		/*p->reachable_bits = 0;*/
		p->lastpkt_recv_time = G.cur_time;
	}
	filter_datapoints(p); /* recalc p->filter_xxx */
	VERB6 bb_error_msg("%s->lastpkt_recv_time=%f", p->p_dotted, p->lastpkt_recv_time);
}

Project: busybox

resolve_peer_hostname(peer_t *p, int loop_on_fail)
{
	len_and_sockaddr *lsa;

 again:
	lsa = host2sockaddr(p->p_hostname, 123);
	if (!lsa) {
		/* error message already emitted by host2sockaddr() */
		if (!loop_on_fail)
			return;
		sleep(5);
		goto again;
	}
	free(p->p_lsa);
	free(p->p_dotted);
	p->p_lsa = lsa;
	p->p_dotted = xmalloc_sockaddr2dotted_noport(&lsa->u.sa);
}

Project: busybox

root_distance(peer_t *p)
{
	/* The root synchronization distance is the maximum error due to
	 * all causes of the local clock relative to the primary server.
	 * It is defined as half the total delay plus total dispersion
	 * plus peer jitter.
	 */
	return MAXD(MINDISP, p->lastpkt_rootdelay + p->lastpkt_delay) / 2
		+ p->lastpkt_rootdisp
		+ p->filter_dispersion
		+ FREQ_TOLERANCE * (G.cur_time - p->lastpkt_recv_time)
		+ p->filter_jitter;
}

Project: busybox

static void run_script(const char *action, double offset)
{
	char *argv[3];
	char *env1, *env2, *env3, *env4;

	G.last_script_run = G.cur_time;

	if (!G.script_name)
		return;

	argv[0] = (char*) G.script_name;
	argv[1] = (char*) action;
	argv[2] = NULL;

	VERB1 bb_error_msg("executing '%s %s'", G.script_name, action);

	env1 = xasprintf("%s=%u", "stratum", G.stratum);
	putenv(env1);
	env2 = xasprintf("%s=%ld", "freq_drift_ppm", G.kernel_freq_drift);
	putenv(env2);
	env3 = xasprintf("%s=%u", "poll_interval", 1 << G.poll_exp);
	putenv(env3);
	env4 = xasprintf("%s=%f", "offset", offset);
	putenv(env4);
	/* Other items of potential interest: selected peer,
	 * rootdelay, reftime, rootdisp, refid, ntp_status,
	 * last_update_offset, last_update_recv_time, discipline_jitter,
	 * how many peers have reachable_bits = 0?
	 */

	/* Don't want to wait: it may run hwclock --systohc, and that
	 * may take some time (seconds): */
	/*spawn_and_wait(argv);*/
	spawn(argv);

	unsetenv("stratum");
	unsetenv("freq_drift_ppm");
	unsetenv("poll_interval");
	unsetenv("offset");
	free(env1);
	free(env2);
	free(env3);
	free(env4);
}

Project: busybox

select_and_cluster(void)
{
	peer_t     *p;
	llist_t    *item;
	int        i, j;
	int        size = 3 * G.peer_cnt;
	/* for selection algorithm */
	point_t    point[size];
	unsigned   num_points, num_candidates;
	double     low, high;
	unsigned   num_falsetickers;
	/* for cluster algorithm */
	survivor_t survivor[size];
	unsigned   num_survivors;

	/* Selection */

	num_points = 0;
	item = G.ntp_peers;
	while (item != NULL) {
		double rd, offset;

		p = (peer_t *) item->data;
		rd = root_distance(p);
		offset = p->filter_offset;
		if (!fit(p, rd)) {
			item = item->link;
			continue;
		}

		VERB5 bb_error_msg("interval: [%f %f %f] %s",
				offset - rd,
				offset,
				offset + rd,
				p->p_dotted
		);
		point[num_points].p = p;
		point[num_points].type = -1;
		point[num_points].edge = offset - rd;
		point[num_points].opt_rd = rd;
		num_points++;
		point[num_points].p = p;
		point[num_points].type = 0;
		point[num_points].edge = offset;
		point[num_points].opt_rd = rd;
		num_points++;
		point[num_points].p = p;
		point[num_points].type = 1;
		point[num_points].edge = offset + rd;
		point[num_points].opt_rd = rd;
		num_points++;
		item = item->link;
	}
	num_candidates = num_points / 3;
	if (num_candidates == 0) {
		VERB3 bb_error_msg("no valid datapoints%s", ", no peer selected");
		return NULL;
	}
	qsort(point, num_points, sizeof(point[0]), compare_point_edge);

	/* Start with the assumption that there are no falsetickers.
	 * Attempt to find a nonempty intersection interval containing
	 * the midpoints of all truechimers.
	 * If a nonempty interval cannot be found, increase the number
	 * of assumed falsetickers by one and try again.
	 * If a nonempty interval is found and the number of falsetickers
	 * is less than the number of truechimers, a majority has been found
	 * and the midpoint of each truechimer represents
	 * the candidates available to the cluster algorithm.
	 */
	num_falsetickers = 0;
	while (1) {
		int c;
		unsigned num_midpoints = 0;

		low = 1 << 9;
		high = - (1 << 9);
		c = 0;
		for (i = 0; i < num_points; i++) {
			/* We want to do:
			 * if (point[i].type == -1) c++;
			 * if (point[i].type == 1) c--;
			 * and it's simpler to do it this way:
			 */
			c -= point[i].type;
			if (c >= num_candidates - num_falsetickers) {
				/* If it was c++ and it got big enough... */
				low = point[i].edge;
				break;
			}
			if (point[i].type == 0)
				num_midpoints++;
		}
		c = 0;
		for (i = num_points-1; i >= 0; i--) {
			c += point[i].type;
			if (c >= num_candidates - num_falsetickers) {
				high = point[i].edge;
				break;
			}
			if (point[i].type == 0)
				num_midpoints++;
		}
		/* If the number of midpoints is greater than the number
		 * of allowed falsetickers, the intersection contains at
		 * least one truechimer with no midpoint - bad.
		 * Also, interval should be nonempty.
		 */
		if (num_midpoints <= num_falsetickers && low < high)
			break;
		num_falsetickers++;
		if (num_falsetickers * 2 >= num_candidates) {
			VERB3 bb_error_msg("falsetickers:%d, candidates:%d%s",
					num_falsetickers, num_candidates,
					", no peer selected");
			return NULL;
		}
	}
	VERB4 bb_error_msg("selected interval: [%f, %f]; candidates:%d falsetickers:%d",
			low, high, num_candidates, num_falsetickers);

	/* Clustering */

	/* Construct a list of survivors (p, metric)
	 * from the chime list, where metric is dominated
	 * first by stratum and then by root distance.
	 * All other things being equal, this is the order of preference.
	 */
	num_survivors = 0;
	for (i = 0; i < num_points; i++) {
		if (point[i].edge < low || point[i].edge > high)
			continue;
		p = point[i].p;
		survivor[num_survivors].p = p;
		/* x.opt_rd == root_distance(p); */
		survivor[num_survivors].metric = MAXDIST * p->lastpkt_stratum + point[i].opt_rd;
		VERB5 bb_error_msg("survivor[%d] metric:%f peer:%s",
			num_survivors, survivor[num_survivors].metric, p->p_dotted);
		num_survivors++;
	}
	/* There must be at least MIN_SELECTED survivors to satisfy the
	 * correctness assertions. Ordinarily, the Byzantine criteria
	 * require four survivors, but for the demonstration here, one
	 * is acceptable.
	 */
	if (num_survivors < MIN_SELECTED) {
		VERB3 bb_error_msg("survivors:%d%s",
				num_survivors,
				", no peer selected");
		return NULL;
	}

	qsort(survivor, num_survivors, sizeof(survivor[0]), compare_survivor_metric);

	/* For each association p in turn, calculate the selection
	 * jitter p->sjitter as the square root of the sum of squares
	 * (p->offset - q->offset) over all q associations. The idea is
	 * to repeatedly discard the survivor with maximum selection
	 * jitter until a termination condition is met.
	 */
	while (1) {
		unsigned max_idx = max_idx;
		double max_selection_jitter = max_selection_jitter;
		double min_jitter = min_jitter;

		if (num_survivors <= MIN_CLUSTERED) {
			VERB4 bb_error_msg("num_survivors %d <= %d, not discarding more",
					num_survivors, MIN_CLUSTERED);
			break;
		}

		/* To make sure a few survivors are left
		 * for the clustering algorithm to chew on,
		 * we stop if the number of survivors
		 * is less than or equal to MIN_CLUSTERED (3).
		 */
		for (i = 0; i < num_survivors; i++) {
			double selection_jitter_sq;

			p = survivor[i].p;
			if (i == 0 || p->filter_jitter < min_jitter)
				min_jitter = p->filter_jitter;

			selection_jitter_sq = 0;
			for (j = 0; j < num_survivors; j++) {
				peer_t *q = survivor[j].p;
				selection_jitter_sq += SQUARE(p->filter_offset - q->filter_offset);
			}
			if (i == 0 || selection_jitter_sq > max_selection_jitter) {
				max_selection_jitter = selection_jitter_sq;
				max_idx = i;
			}
			VERB6 bb_error_msg("survivor %d selection_jitter^2:%f",
					i, selection_jitter_sq);
		}
		max_selection_jitter = SQRT(max_selection_jitter / num_survivors);
		VERB5 bb_error_msg("max_selection_jitter (at %d):%f min_jitter:%f",
				max_idx, max_selection_jitter, min_jitter);

		/* If the maximum selection jitter is less than the
		 * minimum peer jitter, then tossing out more survivors
		 * will not lower the minimum peer jitter, so we might
		 * as well stop.
		 */
		if (max_selection_jitter < min_jitter) {
			VERB4 bb_error_msg("max_selection_jitter:%f < min_jitter:%f, num_survivors:%d, not discarding more",
					max_selection_jitter, min_jitter, num_survivors);
			break;
		}

		/* Delete survivor[max_idx] from the list
		 * and go around again.
		 */
		VERB6 bb_error_msg("dropping survivor %d", max_idx);
		num_survivors--;
		while (max_idx < num_survivors) {
			survivor[max_idx] = survivor[max_idx + 1];
			max_idx++;
		}
	}

	if (0) {
		/* Combine the offsets of the clustering algorithm survivors
		 * using a weighted average with weight determined by the root
		 * distance. Compute the selection jitter as the weighted RMS
		 * difference between the first survivor and the remaining
		 * survivors. In some cases the inherent clock jitter can be
		 * reduced by not using this algorithm, especially when frequent
		 * clockhopping is involved. bbox: thus we don't do it.
		 */
		double x, y, z, w;
		y = z = w = 0;
		for (i = 0; i < num_survivors; i++) {
			p = survivor[i].p;
			x = root_distance(p);
			y += 1 / x;
			z += p->filter_offset / x;
			w += SQUARE(p->filter_offset - survivor[0].p->filter_offset) / x;
		}
	}

	/* Pick the best clock. If the old system peer is on the list
	 * and at the same stratum as the first survivor on the list,
	 * then don't do a clock hop. Otherwise, select the first
	 * survivor on the list as the new system peer.
	 */
	p = survivor[0].p;
	if (G.last_update_peer
	 && G.last_update_peer->lastpkt_stratum <= p->lastpkt_stratum
	) {
		/* Starting from 1 is ok here */
		for (i = 1; i < num_survivors; i++) {
			if (G.last_update_peer == survivor[i].p) {
				VERB5 bb_error_msg("keeping old synced peer");
				p = G.last_update_peer;
				goto keep_old;
			}
		}
	}
	G.last_update_peer = p;
 keep_old:
	VERB4 bb_error_msg("selected peer %s filter_offset:%+f age:%f",
			p->p_dotted,
			p->filter_offset,
			G.cur_time - p->lastpkt_recv_time
	);
	return p;
}

Project: busybox

send_query_to_peer(peer_t *p)
{
	/* Why do we need to bind()?
	 * See what happens when we don't bind:
	 *
	 * socket(PF_INET, SOCK_DGRAM, IPPROTO_IP) = 3
	 * setsockopt(3, SOL_IP, IP_TOS, [16], 4) = 0
	 * gettimeofday({1259071266, 327885}, NULL) = 0
	 * sendto(3, "xxx", 48, MSG_DONTWAIT, {sa_family=AF_INET, sin_port=htons(123), sin_addr=inet_addr("10.34.32.125")}, 16) = 48
	 * ^^^ we sent it from some source port picked by kernel.
	 * time(NULL)              = 1259071266
	 * write(2, "ntpd: entering poll 15 secs\n", 28) = 28
	 * poll([{fd=3, events=POLLIN}], 1, 15000) = 1 ([{fd=3, revents=POLLIN}])
	 * recv(3, "yyy", 68, MSG_DONTWAIT) = 48
	 * ^^^ this recv will receive packets to any local port!
	 *
	 * Uncomment this and use strace to see it in action:
	 */
#define PROBE_LOCAL_ADDR /* { len_and_sockaddr lsa; lsa.len = LSA_SIZEOF_SA; getsockname(p->query.fd, &lsa.u.sa, &lsa.len); } */

	if (p->p_fd == -1) {
		int fd, family;
		len_and_sockaddr *local_lsa;

		family = p->p_lsa->u.sa.sa_family;
		p->p_fd = fd = xsocket_type(&local_lsa, family, SOCK_DGRAM);
		/* local_lsa has "null" address and port 0 now.
		 * bind() ensures we have a *particular port* selected by kernel
		 * and remembered in p->p_fd, thus later recv(p->p_fd)
		 * receives only packets sent to this port.
		 */
		PROBE_LOCAL_ADDR
		xbind(fd, &local_lsa->u.sa, local_lsa->len);
		PROBE_LOCAL_ADDR
#if ENABLE_FEATURE_IPV6
		if (family == AF_INET)
#endif
			setsockopt_int(fd, IPPROTO_IP, IP_TOS, IPTOS_LOWDELAY);
		free(local_lsa);
	}

	/* Emit message _before_ attempted send. Think of a very short
	 * roundtrip networks: we need to go back to recv loop ASAP,
	 * to reduce delay. Printing messages after send works against that.
	 */
	VERB1 bb_error_msg("sending query to %s", p->p_dotted);

	/*
	 * Send out a random 64-bit number as our transmit time.  The NTP
	 * server will copy said number into the originate field on the
	 * response that it sends us.  This is totally legal per the SNTP spec.
	 *
	 * The impact of this is two fold: we no longer send out the current
	 * system time for the world to see (which may aid an attacker), and
	 * it gives us a (not very secure) way of knowing that we're not
	 * getting spoofed by an attacker that can't capture our traffic
	 * but can spoof packets from the NTP server we're communicating with.
	 *
	 * Save the real transmit timestamp locally.
	 */
	p->p_xmt_msg.m_xmttime.int_partl = rand();
	p->p_xmt_msg.m_xmttime.fractionl = rand();
	p->p_xmttime = gettime1900d();

	/* Were doing it only if sendto worked, but
	 * loss of sync detection needs reachable_bits updated
	 * even if sending fails *locally*:
	 * "network is unreachable" because cable was pulled?
	 * We still need to declare "unsync" if this condition persists.
	 */
	p->reachable_bits <<= 1;

	if (do_sendto(p->p_fd, /*from:*/ NULL, /*to:*/ &p->p_lsa->u.sa, /*addrlen:*/ p->p_lsa->len,
			&p->p_xmt_msg, NTP_MSGSIZE_NOAUTH) == -1
	) {
		close(p->p_fd);
		p->p_fd = -1;
		/*
		 * We know that we sent nothing.
		 * We can retry *soon* without fearing
		 * that we are flooding the peer.
		 */
		set_next(p, RETRY_INTERVAL);
		return;
	}

	set_next(p, RESPONSE_INTERVAL);
}

Project: busybox

set_new_values(int disc_state, double offset, double recv_time)
{
	/* Enter new state and set state variables. Note we use the time
	 * of the last clock filter sample, which must be earlier than
	 * the current time.
	 */
	VERB4 bb_error_msg("disc_state=%d last update offset=%f recv_time=%f",
			disc_state, offset, recv_time);
	G.discipline_state = disc_state;
	G.last_update_offset = offset;
	G.last_update_recv_time = recv_time;
}

Project: busybox

set_next(peer_t *p, unsigned t)
{
	p->next_action_time = G.cur_time + t;
}

Project: busybox

sfp_to_d(s_fixedpt_t sfp)
{
	double ret;
	sfp.int_parts = ntohs(sfp.int_parts);
	sfp.fractions = ntohs(sfp.fractions);
	ret = (double)sfp.int_parts + ((double)sfp.fractions / USHRT_MAX);
	return ret;
}

Project: busybox

step_time(double offset)
{
	llist_t *item;
	double dtime;
	struct timeval tvc, tvn;
	char buf[sizeof("yyyy-mm-dd hh:mm:ss") + /*paranoia:*/ 4];
	time_t tval;

	gettimeofday(&tvc, NULL); /* never fails */
	dtime = tvc.tv_sec + (1.0e-6 * tvc.tv_usec) + offset;
	d_to_tv(dtime, &tvn);
	if (settimeofday(&tvn, NULL) == -1)
		bb_perror_msg_and_die("settimeofday");

	VERB2 {
		tval = tvc.tv_sec;
		strftime_YYYYMMDDHHMMSS(buf, sizeof(buf), &tval);
		bb_error_msg("current time is %s.%06u", buf, (unsigned)tvc.tv_usec);
	}
	tval = tvn.tv_sec;
	strftime_YYYYMMDDHHMMSS(buf, sizeof(buf), &tval);
	bb_error_msg("setting time to %s.%06u (offset %+fs)", buf, (unsigned)tvn.tv_usec, offset);

	/* Correct various fields which contain time-relative values: */

	/* Globals: */
	G.cur_time += offset;
	G.last_update_recv_time += offset;
	G.last_script_run += offset;

	/* p->lastpkt_recv_time, p->next_action_time and such: */
	for (item = G.ntp_peers; item != NULL; item = item->link) {
		peer_t *pp = (peer_t *) item->data;
		reset_peer_stats(pp, offset);
		pp->next_action_time += offset;
		if (pp->p_fd >= 0) {
			/* We wait for reply from this peer too.
			 * But due to step we are doing, reply's data is no longer
			 * useful (in fact, it'll be bogus). Stop waiting for it.
			 */
			close(pp->p_fd);
			pp->p_fd = -1;
			set_next(pp, RETRY_INTERVAL);
		}
	}
}

Project: busybox

update_local_clock(peer_t *p)
{
	int rc;
	struct timex tmx;
	/* Note: can use G.cluster_offset instead: */
	double offset = p->filter_offset;
	double recv_time = p->lastpkt_recv_time;
	double abs_offset;
#if !USING_KERNEL_PLL_LOOP
	double freq_drift;
#endif
#if !USING_KERNEL_PLL_LOOP || USING_INITIAL_FREQ_ESTIMATION
	double since_last_update;
#endif
	double etemp, dtemp;

	abs_offset = fabs(offset);

#if 0
	/* If needed, -S script can do it by looking at $offset
	 * env var and killing parent */
	/* If the offset is too large, give up and go home */
	if (abs_offset > PANIC_THRESHOLD) {
		bb_error_msg_and_die("offset %f far too big, exiting", offset);
	}
#endif

	/* If this is an old update, for instance as the result
	 * of a system peer change, avoid it. We never use
	 * an old sample or the same sample twice.
	 */
	if (recv_time <= G.last_update_recv_time) {
		VERB3 bb_error_msg("update from %s: same or older datapoint, not using it",
			p->p_dotted);
		return 0; /* "leave poll interval as is" */
	}

	/* Clock state machine transition function. This is where the
	 * action is and defines how the system reacts to large time
	 * and frequency errors.
	 */
#if !USING_KERNEL_PLL_LOOP || USING_INITIAL_FREQ_ESTIMATION
	since_last_update = recv_time - G.reftime;
#endif
#if !USING_KERNEL_PLL_LOOP
	freq_drift = 0;
#endif
#if USING_INITIAL_FREQ_ESTIMATION
	if (G.discipline_state == STATE_FREQ) {
		/* Ignore updates until the stepout threshold */
		if (since_last_update < WATCH_THRESHOLD) {
			VERB4 bb_error_msg("measuring drift, datapoint ignored, %f sec remains",
					WATCH_THRESHOLD - since_last_update);
			return 0; /* "leave poll interval as is" */
		}
# if !USING_KERNEL_PLL_LOOP
		freq_drift = (offset - G.last_update_offset) / since_last_update;
# endif
	}
#endif

	/* There are two main regimes: when the
	 * offset exceeds the step threshold and when it does not.
	 */
	if (abs_offset > STEP_THRESHOLD) {
#if 0
		double remains;

		switch (G.discipline_state) {
		case STATE_SYNC:
			/* The first outlyer: ignore it, switch to SPIK state */
			VERB3 bb_error_msg("update from %s: offset:%+f, spike%s",
				p->p_dotted, offset,
				"");
			G.discipline_state = STATE_SPIK;
			return -1; /* "decrease poll interval" */

		case STATE_SPIK:
			/* Ignore succeeding outlyers until either an inlyer
			 * is found or the stepout threshold is exceeded.
			 */
			remains = WATCH_THRESHOLD - since_last_update;
			if (remains > 0) {
				VERB3 bb_error_msg("update from %s: offset:%+f, spike%s",
					p->p_dotted, offset,
					", datapoint ignored");
				return -1; /* "decrease poll interval" */
			}
			/* fall through: we need to step */
		} /* switch */
#endif

		/* Step the time and clamp down the poll interval.
		 *
		 * In NSET state an initial frequency correction is
		 * not available, usually because the frequency file has
		 * not yet been written. Since the time is outside the
		 * capture range, the clock is stepped. The frequency
		 * will be set directly following the stepout interval.
		 *
		 * In FSET state the initial frequency has been set
		 * from the frequency file. Since the time is outside
		 * the capture range, the clock is stepped immediately,
		 * rather than after the stepout interval. Guys get
		 * nervous if it takes 17 minutes to set the clock for
		 * the first time.
		 *
		 * In SPIK state the stepout threshold has expired and
		 * the phase is still above the step threshold. Note
		 * that a single spike greater than the step threshold
		 * is always suppressed, even at the longer poll
		 * intervals.
		 */
		VERB4 bb_error_msg("stepping time by %+f; poll_exp=MINPOLL", offset);
		step_time(offset);
		if (option_mask32 & OPT_q) {
			/* We were only asked to set time once. Done. */
			exit(0);
		}

		clamp_pollexp_and_set_MAXSTRAT();

		run_script("step", offset);

		recv_time += offset;

#if USING_INITIAL_FREQ_ESTIMATION
		if (G.discipline_state == STATE_NSET) {
			set_new_values(STATE_FREQ, /*offset:*/ 0, recv_time);
			return 1; /* "ok to increase poll interval" */
		}
#endif
		abs_offset = offset = 0;
		set_new_values(STATE_SYNC, offset, recv_time);
	} else { /* abs_offset <= STEP_THRESHOLD */

		/* The ratio is calculated before jitter is updated to make
		 * poll adjust code more sensitive to large offsets.
		 */
		G.offset_to_jitter_ratio = abs_offset / G.discipline_jitter;

		/* Compute the clock jitter as the RMS of exponentially
		 * weighted offset differences. Used by the poll adjust code.
		 */
		etemp = SQUARE(G.discipline_jitter);
		dtemp = SQUARE(offset - G.last_update_offset);
		G.discipline_jitter = SQRT(etemp + (dtemp - etemp) / AVG);
		if (G.discipline_jitter < G_precision_sec)
			G.discipline_jitter = G_precision_sec;

		switch (G.discipline_state) {
		case STATE_NSET:
			if (option_mask32 & OPT_q) {
				/* We were only asked to set time once.
				 * The clock is precise enough, no need to step.
				 */
				exit(0);
			}
#if USING_INITIAL_FREQ_ESTIMATION
			/* This is the first update received and the frequency
			 * has not been initialized. The first thing to do
			 * is directly measure the oscillator frequency.
			 */
			set_new_values(STATE_FREQ, offset, recv_time);
#else
			set_new_values(STATE_SYNC, offset, recv_time);
#endif
			VERB4 bb_error_msg("transitioning to FREQ, datapoint ignored");
			return 0; /* "leave poll interval as is" */

#if 0 /* this is dead code for now */
		case STATE_FSET:
			/* This is the first update and the frequency
			 * has been initialized. Adjust the phase, but
			 * don't adjust the frequency until the next update.
			 */
			set_new_values(STATE_SYNC, offset, recv_time);
			/* freq_drift remains 0 */
			break;
#endif

#if USING_INITIAL_FREQ_ESTIMATION
		case STATE_FREQ:
			/* since_last_update >= WATCH_THRESHOLD, we waited enough.
			 * Correct the phase and frequency and switch to SYNC state.
			 * freq_drift was already estimated (see code above)
			 */
			set_new_values(STATE_SYNC, offset, recv_time);
			break;
#endif

		default:
#if !USING_KERNEL_PLL_LOOP
			/* Compute freq_drift due to PLL and FLL contributions.
			 *
			 * The FLL and PLL frequency gain constants
			 * depend on the poll interval and Allan
			 * intercept. The FLL is not used below one-half
			 * the Allan intercept. Above that the loop gain
			 * increases in steps to 1 / AVG.
			 */
			if ((1 << G.poll_exp) > ALLAN / 2) {
				etemp = FLL - G.poll_exp;
				if (etemp < AVG)
					etemp = AVG;
				freq_drift += (offset - G.last_update_offset) / (MAXD(since_last_update, ALLAN) * etemp);
			}
			/* For the PLL the integration interval
			 * (numerator) is the minimum of the update
			 * interval and poll interval. This allows
			 * oversampling, but not undersampling.
			 */
			etemp = MIND(since_last_update, (1 << G.poll_exp));
			dtemp = (4 * PLL) << G.poll_exp;
			freq_drift += offset * etemp / SQUARE(dtemp);
#endif
			set_new_values(STATE_SYNC, offset, recv_time);
			break;
		}
		if (G.stratum != p->lastpkt_stratum + 1) {
			G.stratum = p->lastpkt_stratum + 1;
			run_script("stratum", offset);
		}
	}

	G.reftime = G.cur_time;
	G.ntp_status = p->lastpkt_status;
	G.refid = p->lastpkt_refid;
	G.rootdelay = p->lastpkt_rootdelay + p->lastpkt_delay;
	dtemp = p->filter_jitter; // SQRT(SQUARE(p->filter_jitter) + SQUARE(G.cluster_jitter));
	dtemp += MAXD(p->filter_dispersion + FREQ_TOLERANCE * (G.cur_time - p->lastpkt_recv_time) + abs_offset, MINDISP);
	G.rootdisp = p->lastpkt_rootdisp + dtemp;
	VERB4 bb_error_msg("updating leap/refid/reftime/rootdisp from peer %s", p->p_dotted);

	/* We are in STATE_SYNC now, but did not do adjtimex yet.
	 * (Any other state does not reach this, they all return earlier)
	 * By this time, freq_drift and offset are set
	 * to values suitable for adjtimex.
	 */
#if !USING_KERNEL_PLL_LOOP
	/* Calculate the new frequency drift and frequency stability (wander).
	 * Compute the clock wander as the RMS of exponentially weighted
	 * frequency differences. This is not used directly, but can,
	 * along with the jitter, be a highly useful monitoring and
	 * debugging tool.
	 */
	dtemp = G.discipline_freq_drift + freq_drift;
	G.discipline_freq_drift = MAXD(MIND(MAXDRIFT, dtemp), -MAXDRIFT);
	etemp = SQUARE(G.discipline_wander);
	dtemp = SQUARE(dtemp);
	G.discipline_wander = SQRT(etemp + (dtemp - etemp) / AVG);

	VERB4 bb_error_msg("discipline freq_drift=%.9f(int:%ld corr:%e) wander=%f",
			G.discipline_freq_drift,
			(long)(G.discipline_freq_drift * 65536e6),
			freq_drift,
			G.discipline_wander);
#endif
	VERB4 {
		memset(&tmx, 0, sizeof(tmx));
		if (adjtimex(&tmx) < 0)
			bb_perror_msg_and_die("adjtimex");
		bb_error_msg("p adjtimex freq:%ld offset:%+ld status:0x%x tc:%ld",
				tmx.freq, tmx.offset, tmx.status, tmx.constant);
	}

	memset(&tmx, 0, sizeof(tmx));
#if 0
	tmx.modes = ADJ_FREQUENCY | ADJ_OFFSET;
	/* 65536 is one ppm */
	tmx.freq = G.discipline_freq_drift * 65536e6;
#endif
	tmx.modes = ADJ_OFFSET | ADJ_STATUS | ADJ_TIMECONST;// | ADJ_MAXERROR | ADJ_ESTERROR;
	tmx.constant = (int)G.poll_exp - 4;
	/* EXPERIMENTAL.
	 * The below if statement should be unnecessary, but...
	 * It looks like Linux kernel's PLL is far too gentle in changing
	 * tmx.freq in response to clock offset. Offset keeps growing
	 * and eventually we fall back to smaller poll intervals.
	 * We can make correction more agressive (about x2) by supplying
	 * PLL time constant which is one less than the real one.
	 * To be on a safe side, let's do it only if offset is significantly
	 * larger than jitter.
	 */
	if (G.offset_to_jitter_ratio >= TIMECONST_HACK_GATE)
		tmx.constant--;
	tmx.offset = (long)(offset * 1000000); /* usec */
	if (SLEW_THRESHOLD < STEP_THRESHOLD) {
		if (tmx.offset > (long)(SLEW_THRESHOLD * 1000000)) {
			tmx.offset = (long)(SLEW_THRESHOLD * 1000000);
			tmx.constant--;
		}
		if (tmx.offset < -(long)(SLEW_THRESHOLD * 1000000)) {
			tmx.offset = -(long)(SLEW_THRESHOLD * 1000000);
			tmx.constant--;
		}
	}
	if (tmx.constant < 0)
		tmx.constant = 0;

	tmx.status = STA_PLL;
	if (G.ntp_status & LI_PLUSSEC)
		tmx.status |= STA_INS;
	if (G.ntp_status & LI_MINUSSEC)
		tmx.status |= STA_DEL;

	rc = adjtimex(&tmx);
	if (rc < 0)
		bb_perror_msg_and_die("adjtimex");
	/* NB: here kernel returns constant == G.poll_exp, not == G.poll_exp - 4.
	 * Not sure why. Perhaps it is normal.
	 */
	VERB4 bb_error_msg("adjtimex:%d freq:%ld offset:%+ld status:0x%x",
				rc, tmx.freq, tmx.offset, tmx.status);
	G.kernel_freq_drift = tmx.freq / 65536;
	VERB2 bb_error_msg("update from:%s offset:%+f delay:%f jitter:%f clock drift:%+.3fppm tc:%d",
			p->p_dotted,
			offset,
			p->lastpkt_delay,
			G.discipline_jitter,
			(double)tmx.freq / 65536,
			(int)tmx.constant
	);

	return 1; /* "ok to increase poll interval" */
}