xref: /freebsd/contrib/ntp/libntp/ntp_calgps.c (revision c66ec88fed842fbaad62c30d510644ceb7bd2d71)
1 /*
2  * ntp_calgps.c - calendar for GPS/GNSS based clocks
3  *
4  * Written by Juergen Perlinger (perlinger@ntp.org) for the NTP project.
5  * The contents of 'html/copyright.html' apply.
6  *
7  * --------------------------------------------------------------------
8  *
9  * This module implements stuff often used with GPS/GNSS receivers
10  */
11 
12 #include <config.h>
13 #include <sys/types.h>
14 
15 #include "ntp_types.h"
16 #include "ntp_calendar.h"
17 #include "ntp_calgps.h"
18 #include "ntp_stdlib.h"
19 #include "ntp_unixtime.h"
20 
21 #include "ntp_fp.h"
22 #include "ntpd.h"
23 #include "vint64ops.h"
24 
25 /* ====================================================================
26  * misc. helpers -- might go elsewhere sometime?
27  * ====================================================================
28  */
29 
30 l_fp
31 ntpfp_with_fudge(
32 	l_fp	lfp,
33 	double	ofs
34 	)
35 {
36 	l_fp	fpo;
37 	/* calculate 'lfp - ofs' as '(l_fp)(-ofs) + lfp': negating a
38 	 * double is cheap, as it only flips one bit...
39 	 */
40 	ofs = -ofs;
41 	DTOLFP(ofs, &fpo);
42 	L_ADD(&fpo, &lfp);
43 	return fpo;
44 }
45 
46 
47 /* ====================================================================
48  * GPS calendar functions
49  * ====================================================================
50  */
51 
52 /* --------------------------------------------------------------------
53  * normalization functions for day/time and week/time representations.
54  * Since we only use moderate offsets (leap second corrections and
55  * alike) it does not really pay off to do a floor-corrected division
56  * here.  We use compare/decrement/increment loops instead.
57  * --------------------------------------------------------------------
58  */
59 static void
60 _norm_ntp_datum(
61 	TNtpDatum *	datum
62 	)
63 {
64 	static const int32_t limit = SECSPERDAY;
65 
66 	if (datum->secs >= limit) {
67 		do
68 			++datum->days;
69 		while ((datum->secs -= limit) >= limit);
70 	} else if (datum->secs < 0) {
71 		do
72 			--datum->days;
73 		while ((datum->secs += limit) < 0);
74 	}
75 }
76 
77 static void
78 _norm_gps_datum(
79 	TGpsDatum *	datum
80 	)
81 {
82 	static const int32_t limit = 7 * SECSPERDAY;
83 
84 	if (datum->wsecs >= limit) {
85 		do
86 			++datum->weeks;
87 		while ((datum->wsecs -= limit) >= limit);
88 	} else if (datum->wsecs < 0) {
89 		do
90 			--datum->weeks;
91 		while ((datum->wsecs += limit) < 0);
92 	}
93 }
94 
95 /* --------------------------------------------------------------------
96  * Add an offset to a day/time and week/time representation.
97  *
98  * !!Attention!! the offset should be small, compared to the time period
99  * (either a day or a week).
100  * --------------------------------------------------------------------
101  */
102 void
103 gpsntp_add_offset(
104 	TNtpDatum *	datum,
105 	l_fp		offset
106 	)
107 {
108 	/* fraction can be added easily */
109 	datum->frac += offset.l_uf;
110 	datum->secs += (datum->frac < offset.l_uf);
111 
112 	/* avoid integer overflow on the seconds */
113 	if (offset.l_ui >= INT32_MAX)
114 		datum->secs -= (int32_t)~offset.l_ui + 1;
115 	else
116 		datum->secs += (int32_t)offset.l_ui;
117 	_norm_ntp_datum(datum);
118 }
119 
120 void
121 gpscal_add_offset(
122 	TGpsDatum *	datum,
123 	l_fp		offset
124 	)
125 {
126 	/* fraction can be added easily */
127 	datum->frac  += offset.l_uf;
128 	datum->wsecs += (datum->frac < offset.l_uf);
129 
130 
131 	/* avoid integer overflow on the seconds */
132 	if (offset.l_ui >= INT32_MAX)
133 		datum->wsecs -= (int32_t)~offset.l_ui + 1;
134 	else
135 		datum->wsecs += (int32_t)offset.l_ui;
136 	_norm_gps_datum(datum);
137 }
138 
139 /* -------------------------------------------------------------------
140  *	API functions civil calendar and NTP datum
141  * -------------------------------------------------------------------
142  */
143 
144 static TNtpDatum
145 _gpsntp_fix_gps_era(
146 	TcNtpDatum * in
147 	)
148 {
149 	/* force result in basedate era
150 	 *
151 	 * When calculating this directly in days, we have to execute a
152 	 * real modulus calculation, since we're obviously not doing a
153 	 * modulus by a power of 2. Executing this as true floor mod
154 	 * needs some care and is done under explicit usage of one's
155 	 * complement and masking to get mostly branchless code.
156 	 */
157 	static uint32_t const	clen = 7*1024;
158 
159 	uint32_t	base, days, sign;
160 	TNtpDatum	out = *in;
161 
162 	/* Get base in NTP day scale. No overflows here. */
163 	base = (basedate_get_gpsweek() + GPSNTP_WSHIFT) * 7
164 	     - GPSNTP_DSHIFT;
165 	days = out.days;
166 
167 	sign = (uint32_t)-(days < base);
168 	days = sign ^ (days - base);
169 	days %= clen;
170 	days = base + (sign & clen) + (sign ^ days);
171 
172 	out.days = days;
173 	return out;
174 }
175 
176 TNtpDatum
177 gpsntp_fix_gps_era(
178 	TcNtpDatum * in
179 	)
180 {
181 	TNtpDatum out = *in;
182 	_norm_ntp_datum(&out);
183 	return _gpsntp_fix_gps_era(&out);
184 }
185 
186 /* ----------------------------------------------------------------- */
187 static TNtpDatum
188 _gpsntp_from_daytime(
189 	TcCivilDate *	jd,
190 	l_fp		fofs,
191 	TcNtpDatum *	pivot,
192 	int		warp
193 	)
194 {
195 	static const int32_t shift = SECSPERDAY / 2;
196 
197 	TNtpDatum	retv;
198 
199 	/* set result based on pivot -- ops order is important here */
200 	ZERO(retv);
201 	retv.secs = ntpcal_date_to_daysec(jd);
202 	gpsntp_add_offset(&retv, fofs);	/* result is normalized */
203 	retv.days = pivot->days;
204 
205 	/* Manual periodic extension without division: */
206 	if (pivot->secs < shift) {
207 		int32_t lim = pivot->secs + shift;
208 		retv.days -= (retv.secs > lim ||
209 			      (retv.secs == lim && retv.frac >= pivot->frac));
210 	} else {
211 		int32_t lim = pivot->secs - shift;
212 		retv.days += (retv.secs < lim ||
213 			      (retv.secs == lim && retv.frac < pivot->frac));
214 	}
215 	return warp ? _gpsntp_fix_gps_era(&retv) : retv;
216 }
217 
218 /* -----------------------------------------------------------------
219  * Given the time-of-day part of a civil datum and an additional
220  * (fractional) offset, calculate a full time stamp around a given pivot
221  * time so that the difference between the pivot and the resulting time
222  * stamp is less or equal to 12 hours absolute.
223  */
224 TNtpDatum
225 gpsntp_from_daytime2_ex(
226 	TcCivilDate *	jd,
227 	l_fp		fofs,
228 	TcNtpDatum *	pivot,
229 	int/*BOOL*/	warp
230 	)
231 {
232 	TNtpDatum	dpiv = *pivot;
233 	_norm_ntp_datum(&dpiv);
234 	return _gpsntp_from_daytime(jd, fofs, &dpiv, warp);
235 }
236 
237 /* -----------------------------------------------------------------
238  * This works similar to 'gpsntp_from_daytime1()' and actually even uses
239  * it, but the pivot is calculated from the pivot given as 'l_fp' in NTP
240  * time scale. This is in turn expanded around the current system time,
241  * and the resulting absolute pivot is then used to calculate the full
242  * NTP time stamp.
243  */
244 TNtpDatum
245 gpsntp_from_daytime1_ex(
246 	TcCivilDate *	jd,
247 	l_fp		fofs,
248 	l_fp		pivot,
249 	int/*BOOL*/	warp
250 	)
251 {
252 	vint64		pvi64;
253 	TNtpDatum	dpiv;
254 	ntpcal_split	split;
255 
256 	pvi64 = ntpcal_ntp_to_ntp(pivot.l_ui, NULL);
257 	split = ntpcal_daysplit(&pvi64);
258 	dpiv.days = split.hi;
259 	dpiv.secs = split.lo;
260 	dpiv.frac = pivot.l_uf;
261 	return _gpsntp_from_daytime(jd, fofs, &dpiv, warp);
262 }
263 
264 /* -----------------------------------------------------------------
265  * Given a calendar date, zap it into a GPS time format and then convert
266  * that one into the NTP time scale.
267  */
268 TNtpDatum
269 gpsntp_from_calendar_ex(
270 	TcCivilDate *	jd,
271 	l_fp		fofs,
272 	int/*BOOL*/	warp
273 	)
274 {
275 	TGpsDatum	gps;
276 	gps = gpscal_from_calendar_ex(jd, fofs, warp);
277 	return gpsntp_from_gpscal_ex(&gps, FALSE);
278 }
279 
280 /* -----------------------------------------------------------------
281  * create a civil calendar datum from a NTP date representation
282  */
283 void
284 gpsntp_to_calendar(
285 	TCivilDate * cd,
286 	TcNtpDatum * nd
287 	)
288 {
289 	memset(cd, 0, sizeof(*cd));
290 	ntpcal_rd_to_date(
291 		cd,
292 		nd->days + DAY_NTP_STARTS + ntpcal_daysec_to_date(
293 			cd, nd->secs));
294 }
295 
296 /* -----------------------------------------------------------------
297  * get day/tod representation from week/tow datum
298  */
299 TNtpDatum
300 gpsntp_from_gpscal_ex(
301 	TcGpsDatum *	gd,
302     	int/*BOOL*/	warp
303 	)
304 {
305 	TNtpDatum	retv;
306 	vint64		ts64;
307 	ntpcal_split	split;
308 	TGpsDatum	date = *gd;
309 
310 	if (warp) {
311 		uint32_t base = basedate_get_gpsweek() + GPSNTP_WSHIFT;
312 		_norm_gps_datum(&date);
313 		date.weeks = ((date.weeks - base) & 1023u) + base;
314 	}
315 
316 	ts64  = ntpcal_weekjoin(date.weeks, date.wsecs);
317 	ts64  = subv64u32(&ts64, (GPSNTP_DSHIFT * SECSPERDAY));
318 	split = ntpcal_daysplit(&ts64);
319 
320 	retv.frac = gd->frac;
321 	retv.secs = split.lo;
322 	retv.days = split.hi;
323 	return retv;
324 }
325 
326 /* -----------------------------------------------------------------
327  * get LFP from ntp datum
328  */
329 l_fp
330 ntpfp_from_ntpdatum(
331 	TcNtpDatum *	nd
332 	)
333 {
334 	l_fp retv;
335 
336 	retv.l_uf = nd->frac;
337 	retv.l_ui = nd->days * (uint32_t)SECSPERDAY
338 	          + nd->secs;
339 	return retv;
340 }
341 
342 /* -------------------------------------------------------------------
343  *	API functions GPS week calendar
344  *
345  * Here we use a calendar base of 1899-12-31, so the NTP epoch has
346  * { 0, 86400.0 } in this representation.
347  * -------------------------------------------------------------------
348  */
349 
350 static TGpsDatum
351 _gpscal_fix_gps_era(
352 	TcGpsDatum * in
353 	)
354 {
355 	/* force result in basedate era
356 	 *
357 	 * This is based on calculating the modulus to a power of two,
358 	 * so signed integer overflow does not affect the result. Which
359 	 * in turn makes for a very compact calculation...
360 	 */
361 	uint32_t	base, week;
362 	TGpsDatum	out = *in;
363 
364 	week = out.weeks;
365 	base = basedate_get_gpsweek() + GPSNTP_WSHIFT;
366 	week = base + ((week - base) & (GPSWEEKS - 1));
367 	out.weeks = week;
368 	return out;
369 }
370 
371 TGpsDatum
372 gpscal_fix_gps_era(
373 	TcGpsDatum * in
374 	)
375 {
376 	TGpsDatum out = *in;
377 	_norm_gps_datum(&out);
378 	return _gpscal_fix_gps_era(&out);
379 }
380 
381 /* -----------------------------------------------------------------
382  * Given a calendar date, zap it into a GPS time format and the do a
383  * proper era mapping in the GPS time scale, based on the GPS base date,
384  * if so requested.
385  *
386  * This function also augments the century if just a 2-digit year
387  * (0..99) is provided on input.
388  *
389  * This is a fail-safe against GPS receivers with an unknown starting
390  * point for their internal calendar calculation and therefore
391  * unpredictable (but reproducible!) rollover behavior. While there
392  * *are* receivers that create a full date in the proper way, many
393  * others just don't.  The overall damage is minimized by simply not
394  * trusting the era mapping of the receiver and doing the era assignment
395  * with a configurable base date *inside* ntpd.
396  */
397 TGpsDatum
398 gpscal_from_calendar_ex(
399 	TcCivilDate *	jd,
400 	l_fp		fofs,
401 	int/*BOOL*/	warp
402 	)
403 {
404 	/*  (-DAY_GPS_STARTS) (mod 7*1024) -- complement of cycle shift */
405 	static const uint32_t s_compl_shift =
406 	    (7 * 1024) - DAY_GPS_STARTS % (7 * 1024);
407 
408 	TGpsDatum	gps;
409 	TCivilDate	cal;
410 	int32_t		days, week;
411 
412 	/* if needed, convert from 2-digit year to full year
413 	 * !!NOTE!! works only between 1980 and 2079!
414 	 */
415 	cal = *jd;
416 	if (cal.year < 80)
417 		cal.year += 2000;
418 	else if (cal.year < 100)
419 		cal.year += 1900;
420 
421 	/* get RDN from date, possibly adjusting the century */
422 again:	if (cal.month && cal.monthday) {	/* use Y/M/D civil date */
423 		days = ntpcal_date_to_rd(&cal);
424 	} else {				/* using Y/DoY date */
425 		days = ntpcal_year_to_ystart(cal.year)
426 		     + (int32_t)cal.yearday
427 		     - 1; /* both RDN and yearday start with '1'. */
428 	}
429 
430 	/* Rebase to days after the GPS epoch. 'days' is positive here,
431 	 * but it might be less than the GPS epoch start. Depending on
432 	 * the input, we have to do different things to get the desired
433 	 * result. (Since we want to remap the era anyway, we only have
434 	 * to retain congruential identities....)
435 	 */
436 
437 	if (days >= DAY_GPS_STARTS) {
438 		/* simply shift to days since GPS epoch */
439 		days -= DAY_GPS_STARTS;
440 	} else if (jd->year < 100) {
441 		/* Two-digit year on input: add another century and
442 		 * retry.  This can happen only if the century expansion
443 		 * yielded a date between 1980-01-01 and 1980-01-05,
444 		 * both inclusive. We have at most one retry here.
445 		 */
446 		cal.year += 100;
447 		goto again;
448 	} else {
449 		/* A very bad date before the GPS epoch. There's not
450 		 * much we can do, except to add the complement of
451 		 * DAY_GPS_STARTS % (7 * 1024) here, that is, use a
452 		 * congruential identity: Add the complement instead of
453 		 * subtracting the value gives a value with the same
454 		 * modulus. But of course, now we MUST to go through a
455 		 * cycle fix... because the date was obviously wrong!
456 		 */
457 		warp  = TRUE;
458 		days += s_compl_shift;
459 	}
460 
461 	/* Splitting to weeks is simple now: */
462 	week  = days / 7;
463 	days -= week * 7;
464 
465 	/* re-base on start of NTP with weeks mapped to 1024 weeks
466 	 * starting with the GPS base day set in the calendar.
467 	 */
468 	gps.weeks = week + GPSNTP_WSHIFT;
469 	gps.wsecs = days * SECSPERDAY + ntpcal_date_to_daysec(&cal);
470 	gps.frac  = 0;
471 	gpscal_add_offset(&gps, fofs);
472 	return warp ? _gpscal_fix_gps_era(&gps) : gps;
473 }
474 
475 /* -----------------------------------------------------------------
476  * get civil date from week/tow representation
477  */
478 void
479 gpscal_to_calendar(
480 	TCivilDate * cd,
481 	TcGpsDatum * wd
482 	)
483 {
484 	TNtpDatum nd;
485 
486 	memset(cd, 0, sizeof(*cd));
487 	nd = gpsntp_from_gpscal_ex(wd, FALSE);
488 	gpsntp_to_calendar(cd, &nd);
489 }
490 
491 /* -----------------------------------------------------------------
492  * Given the week and seconds in week, as well as the fraction/offset
493  * (which should/could include the leap seconds offset), unfold the
494  * weeks (which are assumed to have just 10 bits) into expanded weeks
495  * based on the GPS base date derived from the build date (default) or
496  * set by the configuration.
497  *
498  * !NOTE! This function takes RAW GPS weeks, aligned to the GPS start
499  * (1980-01-06) on input. The output weeks will be aligned to NTPD's
500  * week calendar start (1899-12-31)!
501  */
502 TGpsDatum
503 gpscal_from_gpsweek(
504 	uint16_t	week,
505 	int32_t		secs,
506 	l_fp		fofs
507 	)
508 {
509 	TGpsDatum retv;
510 
511 	retv.frac  = 0;
512 	retv.wsecs = secs;
513 	retv.weeks = week + GPSNTP_WSHIFT;
514 	gpscal_add_offset(&retv, fofs);
515 	return _gpscal_fix_gps_era(&retv);
516 }
517 
518 /* -----------------------------------------------------------------
519  * internal work horse for time-of-week expansion
520  */
521 static TGpsDatum
522 _gpscal_from_weektime(
523 	int32_t		wsecs,
524 	l_fp    	fofs,
525 	TcGpsDatum *	pivot
526 	)
527 {
528 	static const int32_t shift = SECSPERWEEK / 2;
529 
530 	TGpsDatum	retv;
531 
532 	/* set result based on pivot -- ops order is important here */
533 	ZERO(retv);
534 	retv.wsecs = wsecs;
535 	gpscal_add_offset(&retv, fofs);	/* result is normalized */
536 	retv.weeks = pivot->weeks;
537 
538 	/* Manual periodic extension without division: */
539 	if (pivot->wsecs < shift) {
540 		int32_t lim = pivot->wsecs + shift;
541 		retv.weeks -= (retv.wsecs > lim ||
542 			       (retv.wsecs == lim && retv.frac >= pivot->frac));
543 	} else {
544 		int32_t lim = pivot->wsecs - shift;
545 		retv.weeks += (retv.wsecs < lim ||
546 			       (retv.wsecs == lim && retv.frac < pivot->frac));
547 	}
548 	return _gpscal_fix_gps_era(&retv);
549 }
550 
551 /* -----------------------------------------------------------------
552  * expand a time-of-week around a pivot given as week datum
553  */
554 TGpsDatum
555 gpscal_from_weektime2(
556 	int32_t		wsecs,
557 	l_fp    	fofs,
558 	TcGpsDatum *	pivot
559 	)
560 {
561 	TGpsDatum wpiv = * pivot;
562 	_norm_gps_datum(&wpiv);
563 	return _gpscal_from_weektime(wsecs, fofs, &wpiv);
564 }
565 
566 /* -----------------------------------------------------------------
567  * epand a time-of-week around an pivot given as LFP, which in turn
568  * is expanded around the current system time and then converted
569  * into a week datum.
570  */
571 TGpsDatum
572 gpscal_from_weektime1(
573 	int32_t	wsecs,
574 	l_fp    fofs,
575 	l_fp    pivot
576 	)
577 {
578 	vint64		pvi64;
579 	TGpsDatum	wpiv;
580 	ntpcal_split	split;
581 
582 	/* get 64-bit pivot in NTP epoch */
583 	pvi64 = ntpcal_ntp_to_ntp(pivot.l_ui, NULL);
584 
585 	/* convert to weeks since 1899-12-31 and seconds in week */
586 	pvi64 = addv64u32(&pvi64, (GPSNTP_DSHIFT * SECSPERDAY));
587 	split = ntpcal_weeksplit(&pvi64);
588 
589 	wpiv.weeks = split.hi;
590 	wpiv.wsecs = split.lo;
591 	wpiv.frac  = pivot.l_uf;
592 	return _gpscal_from_weektime(wsecs, fofs, &wpiv);
593 }
594 
595 /* -----------------------------------------------------------------
596  * get week/tow representation from day/tod datum
597  */
598 TGpsDatum
599 gpscal_from_gpsntp(
600 	TcNtpDatum *	gd
601 	)
602 {
603 	TGpsDatum	retv;
604 	vint64		ts64;
605 	ntpcal_split	split;
606 
607 	ts64  = ntpcal_dayjoin(gd->days, gd->secs);
608 	ts64  = addv64u32(&ts64, (GPSNTP_DSHIFT * SECSPERDAY));
609 	split = ntpcal_weeksplit(&ts64);
610 
611 	retv.frac  = gd->frac;
612 	retv.wsecs = split.lo;
613 	retv.weeks = split.hi;
614 	return retv;
615 }
616 
617 /* -----------------------------------------------------------------
618  * convert week/tow to LFP stamp
619  */
620 l_fp
621 ntpfp_from_gpsdatum(
622 	TcGpsDatum *	gd
623 	)
624 {
625 	l_fp retv;
626 
627 	retv.l_uf = gd->frac;
628 	retv.l_ui = gd->weeks * (uint32_t)SECSPERWEEK
629 	          + (uint32_t)gd->wsecs
630 	          - (uint32_t)SECSPERDAY * GPSNTP_DSHIFT;
631 	return retv;
632 }
633 
634 /* -*-EOF-*- */
635