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
30l_fp
31ntpfp_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 */
59static 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
77static 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 */
102void
103gpsntp_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
120void
121gpscal_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
144static 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
176TNtpDatum
177gpsntp_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/* ----------------------------------------------------------------- */
187static 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 */
224TNtpDatum
225gpsntp_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 */
244TNtpDatum
245gpsntp_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 */
268TNtpDatum
269gpsntp_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 */
283void
284gpsntp_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 */
299TNtpDatum
300gpsntp_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 */
329l_fp
330ntpfp_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
350static 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
371TGpsDatum
372gpscal_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 */
397TGpsDatum
398gpscal_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 */
422again:	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 */
478void
479gpscal_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 */
502TGpsDatum
503gpscal_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 */
521static 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 */
554TGpsDatum
555gpscal_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 */
571TGpsDatum
572gpscal_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 */
598TGpsDatum
599gpscal_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 */
620l_fp
621ntpfp_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