From: Juergen Perlinger Date: Sat, 20 Apr 2019 06:18:00 +0000 (+0200) Subject: [Bug 3576] New GPS date function API X-Git-Url: http://git.ipfire.org/cgi-bin/gitweb.cgi?a=commitdiff_plain;h=08d2b10ccba0a0552886a3cf45d87dc60c9a6632;p=thirdparty%2Fntp.git [Bug 3576] New GPS date function API bk: 5cbab998HwMaqDT3MN9qDprx6fEvOw --- diff --git a/ChangeLog b/ChangeLog index 9ff845c81..511f7aae0 100644 --- a/ChangeLog +++ b/ChangeLog @@ -1,3 +1,6 @@ +--- +* [Bug 3576] New GPS date function API + --- (4.2.8p13) 2019/03/07 Released by Harlan Stenn diff --git a/include/ntp_calendar.h b/include/ntp_calendar.h index 0b1f20d6b..85fffbedc 100644 --- a/include/ntp_calendar.h +++ b/include/ntp_calendar.h @@ -19,6 +19,8 @@ struct calendar { uint8_t second; /* second of minute */ uint8_t weekday; /* 0..7, 0=Sunday */ }; +typedef struct calendar TCivilDate; +typedef struct calendar const TcCivilDate; /* ISO week calendar date */ struct isodate { @@ -29,6 +31,8 @@ struct isodate { uint8_t minute; /* minute of hour */ uint8_t second; /* second of minute */ }; +typedef struct isodate TIsoDate; +typedef struct isodate const TcIsoDate; /* general split representation */ typedef struct { @@ -109,6 +113,7 @@ extern systime_func_ptr ntpcal_set_timefunc(systime_func_ptr); extern const char * const months[12]; extern const char * const daynames[7]; +extern char * ntpcal_iso8601std(char*, size_t, struct calendar const*); extern void caljulian (uint32_t, struct calendar *); extern uint32_t caltontp (const struct calendar *); @@ -151,6 +156,13 @@ ntpcal_ntp_to_ntp(uint32_t /* ntp */, const time_t * /* pivot */); extern ntpcal_split ntpcal_daysplit(const vint64 *); +/* + * Split a time stamp in seconds into elapsed weeks and elapsed seconds + * since start of week. + */ +extern ntpcal_split +ntpcal_weeksplit(const vint64 *); + /* * Merge a number of days and a number of seconds into seconds, * expressed in 64 bits to avoid overflow. @@ -158,6 +170,13 @@ ntpcal_daysplit(const vint64 *); extern vint64 ntpcal_dayjoin(int32_t /* days */, int32_t /* seconds */); +/* + * Merge a number of weeks and a number of seconds into seconds, + * expressed in 64 bits to avoid overflow. + */ +extern vint64 +ntpcal_weekjoin(int32_t /* weeks */, int32_t /* seconds */); + /* Get the number of leap years since epoch for the number of elapsed * full years */ @@ -431,7 +450,7 @@ basedate_expand_gpsweek(unsigned short weekno); /* * Start day of the GPS epoch. This is the Rata Die of 1980-01-06 */ -#define DAY_GPS_STARTS 722819 +#define DAY_GPS_STARTS 722820 /* * Difference between UN*X and NTP epoch (25567). diff --git a/include/ntp_calgps.h b/include/ntp_calgps.h new file mode 100644 index 000000000..a8620cc38 --- /dev/null +++ b/include/ntp_calgps.h @@ -0,0 +1,126 @@ +/* + * ntp_calgps.h - calendar for GPS/GNSS based clocks + * + * Written by Juergen Perlinger (perlinger@ntp.org) for the NTP project. + * The contents of 'html/copyright.html' apply. + * + * -------------------------------------------------------------------- + * + * This module implements stuff often used with GPS/GNSS receivers + */ +#ifndef NTP_CALGPS_H +#define NTP_CALGPS_H + +#include + +#include "ntp_types.h" +#include "ntp_fp.h" +#include "ntp_calendar.h" + +/* GPS week calendar (extended weeks) + * We use weeks based on 1899-31-12, which was the last Sunday before + * the begin of the NTP epoch. (Which is equivalent to saying 1900-01-01 + * was a Monday...) + * + * We simply pre-calculate the offsets and cycle shifts for the real GPS + * calendar, which starts at 1980-01-06, to simplyfy some expressions. + * + * This has fringe benefit that sould not be overlooked: Since week zero + * is around 1900, and we should never have to deal with dates befor + * 1970 or 1980, a week number of zero can be easily used to indicate + * an invalid week time stamp. + */ +#define GPSNTP_WSHIFT 4175 /* weeks 1899-31-12 --> 1980-01-06 */ +#define GPSNTP_WCYCLE 79 /* above, modulo 1024 */ +#define GPSNTP_DSHIFT 1 /* day number of 1900-01-01 in week */ + +struct gpsdatum { + uint32_t weeks; /* weeks since GPS epoch */ + int32_t wsecs; /* seconds since week start */ + uint32_t frac; /* fractional seconds */ +}; +typedef struct gpsdatum TGpsDatum; +typedef struct gpsdatum const TcGpsDatum; + +/* NTP date/time in split representation */ +struct ntpdatum { + uint32_t days; /* since NTP epoch */ + int32_t secs; /* since midnight, denorm is ok */ + uint32_t frac; /* fractional seconds */ +}; +typedef struct ntpdatum TNtpDatum; +typedef struct ntpdatum const TcNtpDatum; + +/* + * GPS week/sec calendar functions + * + * see the implementation for details, especially the + * 'gpscal_from_weektime{1,2}()' + */ + +extern TGpsDatum +gpscal_fix_gps_era(TcGpsDatum *); + +extern void +gpscal_add_offset(TGpsDatum *datum, l_fp offset); + +extern TGpsDatum +gpscal_from_calendar(TcCivilDate*, l_fp fofs); + +extern TGpsDatum /* see source for semantic of the 'fofs' value! */ +gpscal_from_gpsweek(uint16_t w, int32_t s, l_fp fofs); + +extern TGpsDatum +gpscal_from_weektime1(int32_t wsecs, l_fp fofs, l_fp pivot); + +extern TGpsDatum +gpscal_from_weektime2(int32_t wsecs, l_fp fofs, TcGpsDatum *pivot); + +extern void +gpscal_to_calendar(TCivilDate*, TcGpsDatum*); + +extern TGpsDatum +gpscal_from_gpsntp(TcNtpDatum*); + +extern l_fp +ntpfp_from_gpsdatum(TcGpsDatum *); + +/* + * NTP day/sec calendar functions + * + * see the implementation for details, especially the + * 'gpscal_from_daytime{1,2}()' + */ +extern TNtpDatum +gpsntp_fix_gps_era(TcNtpDatum *); + +extern void +gpsntp_add_offset(TNtpDatum *datum, l_fp offset); + +extern TNtpDatum +gpsntp_from_calendar(TcCivilDate*, l_fp fofs); + +extern TNtpDatum +gpsntp_from_daytime1(TcCivilDate *dt, l_fp fofs, l_fp pivot); + +extern TNtpDatum +gpsntp_from_daytime2(TcCivilDate *dt, l_fp fofs, TcNtpDatum *pivot); + +extern TNtpDatum +gpsntp_from_gpscal(TcGpsDatum*); + +extern void +gpsntp_to_calendar(TCivilDate*, TcNtpDatum*); + +extern l_fp +ntpfp_from_ntpdatum(TcNtpDatum*); + +/* + * Some helpers + */ + +/* apply fudge to time stamp: *SUBTRACT* the given offset from an l_fp*/ +extern l_fp +ntpfp_with_fudge(l_fp lfp, double ofs); + +#endif /*!defined(NTP_CALGPS_H)*/ diff --git a/include/ntp_refclock.h b/include/ntp_refclock.h index 4b807e5f3..4b3951d94 100644 --- a/include/ntp_refclock.h +++ b/include/ntp_refclock.h @@ -14,11 +14,6 @@ #include "recvbuff.h" -#define SAMPLE(x) pp->coderecv = (pp->coderecv + 1) % MAXSTAGE; \ - pp->filter[pp->coderecv] = (x); \ - if (pp->coderecv == pp->codeproc) \ - pp->codeproc = (pp->codeproc + 1) % MAXSTAGE; - /* * Macros to determine the clock type and unit numbers from a * 127.127.t.u address @@ -133,13 +128,12 @@ extern HANDLE WaitableIoEventHandle; * Structure interface between the reference clock support * ntp_refclock.c and the driver utility routines */ -#define MAXSTAGE 60 /* max median filter stages */ +#define MAXSTAGE 64 /* max median filter stages */ #define NSTAGE 5 /* default median filter stages */ #define BMAX 128 /* max timecode length */ #define GMT 0 /* I hope nobody sees this */ #define MAXDIAL 60 /* max length of modem dial strings */ - struct refclockproc { void * unitptr; /* pointer to unit structure */ struct refclock * conf; /* refclock_conf[type] */ @@ -162,8 +156,8 @@ struct refclockproc { int second; /* second of minute */ long nsec; /* nanosecond of second */ u_long yearstart; /* beginning of year */ - int coderecv; /* put pointer */ - int codeproc; /* get pointer */ + u_int coderecv; /* put pointer */ + u_int codeproc; /* get pointer */ l_fp lastref; /* reference timestamp */ l_fp lastrec; /* receive timestamp */ double offset; /* mean offset */ @@ -229,12 +223,29 @@ extern int refclock_process(struct refclockproc *); extern int refclock_process_f(struct refclockproc *, double); extern void refclock_process_offset(struct refclockproc *, l_fp, l_fp, double); +extern int refclock_samples_avail(struct refclockproc const *); +extern int refclock_samples_expire(struct refclockproc *, int); extern void refclock_report (struct peer *, int); extern int refclock_gtlin (struct recvbuf *, char *, int, l_fp *); extern int refclock_gtraw (struct recvbuf *, char *, int, l_fp *); extern int indicate_refclock_packet(struct refclockio *, struct recvbuf *); extern void process_refclock_packet(struct recvbuf *); + +/* save string as la_code, size==(size_t)-1 ==> ASCIIZ string */ +extern void refclock_save_lcode( + struct refclockproc *, char const *, size_t); +/* format data into la_code */ +extern void refclock_format_lcode( + struct refclockproc *, char const *, ...); +extern void refclock_vformat_lcode( + struct refclockproc *, char const *, va_list); + +struct refclock_atom; +extern int refclock_ppsaugment( + const struct refclock_atom*, l_fp *rcvtime , + double rcvfudge, double ppsfudge); + #endif /* REFCLOCK */ #endif /* NTP_REFCLOCK_H */ diff --git a/include/timespecops.h b/include/timespecops.h index fa32e42a6..17a9b6806 100644 --- a/include/timespecops.h +++ b/include/timespecops.h @@ -56,68 +56,11 @@ /* predicate: returns TRUE if the nanoseconds are out-of-bounds */ #define timespec_isdenormal(x) (!timespec_isnormal(x)) -/* conversion between l_fp fractions and nanoseconds */ -#ifdef HAVE_U_INT64 -# define FTOTVN(tsf) \ - ((int32) \ - (((u_int64)(tsf) * NANOSECONDS + 0x80000000) >> 32)) -# define TVNTOF(tvu) \ - ((u_int32) \ - ((((u_int64)(tvu) << 32) + NANOSECONDS / 2) / \ - NANOSECONDS)) -#else -# define NSECFRAC (FRAC / NANOSECONDS) -# define FTOTVN(tsf) \ - ((int32)((tsf) / NSECFRAC + 0.5)) -# define TVNTOF(tvu) \ - ((u_int32)((tvu) * NSECFRAC + 0.5)) -#endif /* make sure nanoseconds are in nominal range */ -static inline struct timespec -normalize_tspec( - struct timespec x - ) -{ -#if SIZEOF_LONG > 4 - long z; - - /* - * tv_nsec is of type 'long', and on a 64-bit machine using only - * loops becomes prohibitive once the upper 32 bits get - * involved. On the other hand, division by constant should be - * fast enough; so we do a division of the nanoseconds in that - * case. The floor adjustment step follows with the standard - * normalisation loops. And labs() is intentionally not used - * here: it has implementation-defined behaviour when applied - * to LONG_MIN. - */ - if (x.tv_nsec < -3l * NANOSECONDS || - x.tv_nsec > 3l * NANOSECONDS) { - z = x.tv_nsec / NANOSECONDS; - x.tv_nsec -= z * NANOSECONDS; - x.tv_sec += z; - } -#endif - /* since 10**9 is close to 2**32, we don't divide but do a - * normalisation in a loop; this takes 3 steps max, and should - * outperform a division even if the mul-by-inverse trick is - * employed. */ - if (x.tv_nsec < 0) - do { - x.tv_nsec += NANOSECONDS; - x.tv_sec--; - } while (x.tv_nsec < 0); - else if (x.tv_nsec >= NANOSECONDS) - do { - x.tv_nsec -= NANOSECONDS; - x.tv_sec++; - } while (x.tv_nsec >= NANOSECONDS); - - return x; -} +extern struct timespec normalize_tspec(struct timespec x); /* x = a + b */ static inline struct timespec @@ -196,45 +139,13 @@ neg_tspec( } /* x = abs(a) */ -static inline struct timespec -abs_tspec( - struct timespec a - ) -{ - struct timespec c; - - c = normalize_tspec(a); - if (c.tv_sec < 0) { - if (c.tv_nsec != 0) { - c.tv_sec = -c.tv_sec - 1; - c.tv_nsec = NANOSECONDS - c.tv_nsec; - } else { - c.tv_sec = -c.tv_sec; - } - } - - return c; -} +struct timespec abs_tspec(struct timespec a); /* * compare previously-normalised a and b * return 1 / 0 / -1 if a < / == / > b */ -static inline int -cmp_tspec( - struct timespec a, - struct timespec b - ) -{ - int r; - - r = (a.tv_sec > b.tv_sec) - (a.tv_sec < b.tv_sec); - if (0 == r) - r = (a.tv_nsec > b.tv_nsec) - - (a.tv_nsec < b.tv_nsec); - - return r; -} +extern int cmp_tspec(struct timespec a, struct timespec b); /* * compare possibly-denormal a and b @@ -253,19 +164,7 @@ cmp_tspec_denorm( * test previously-normalised a * return 1 / 0 / -1 if a < / == / > 0 */ -static inline int -test_tspec( - struct timespec a - ) -{ - int r; - - r = (a.tv_sec > 0) - (a.tv_sec < 0); - if (r == 0) - r = (a.tv_nsec > 0); - - return r; -} +extern int test_tspec(struct timespec a); /* * test possibly-denormal a @@ -293,20 +192,7 @@ tspectoa( */ /* convert from timespec duration to l_fp duration */ -static inline l_fp -tspec_intv_to_lfp( - struct timespec x - ) -{ - struct timespec v; - l_fp y; - - v = normalize_tspec(x); - y.l_uf = TVNTOF(v.tv_nsec); - y.l_i = (int32)v.tv_sec; - - return y; -} +extern l_fp tspec_intv_to_lfp(struct timespec x); /* x must be UN*X epoch, output will be in NTP epoch */ static inline l_fp @@ -323,71 +209,14 @@ tspec_stamp_to_lfp( } /* convert from l_fp type, relative signed/unsigned and absolute */ -static inline struct timespec -lfp_intv_to_tspec( - l_fp x - ) -{ - struct timespec out; - l_fp absx; - int neg; - - neg = L_ISNEG(&x); - absx = x; - if (neg) { - L_NEG(&absx); - } - out.tv_nsec = FTOTVN(absx.l_uf); - out.tv_sec = absx.l_i; - if (neg) { - out.tv_sec = -out.tv_sec; - out.tv_nsec = -out.tv_nsec; - out = normalize_tspec(out); - } - - return out; -} - -static inline struct timespec -lfp_uintv_to_tspec( - l_fp x - ) -{ - struct timespec out; - - out.tv_nsec = FTOTVN(x.l_uf); - out.tv_sec = x.l_ui; - - return out; -} +extern struct timespec lfp_intv_to_tspec(l_fp x); +extern struct timespec lfp_uintv_to_tspec(l_fp x); /* * absolute (timestamp) conversion. Input is time in NTP epoch, output * is in UN*X epoch. The NTP time stamp will be expanded around the * pivot time *p or the current time, if p is NULL. */ -static inline struct timespec -lfp_stamp_to_tspec( - l_fp x, - const time_t * p - ) -{ - struct timespec out; - vint64 sec; - - sec = ntpcal_ntp_to_time(x.l_ui, p); - out.tv_nsec = FTOTVN(x.l_uf); - - /* copying a vint64 to a time_t needs some care... */ -#if SIZEOF_TIME_T <= 4 - out.tv_sec = (time_t)sec.d_s.lo; -#elif defined(HAVE_INT64) - out.tv_sec = (time_t)sec.q_s; -#else - out.tv_sec = ((time_t)sec.d_s.hi << 32) | sec.d_s.lo; -#endif - - return out; -} +extern struct timespec lfp_stamp_to_tspec(l_fp x, const time_t *pivot); #endif /* TIMESPECOPS_H */ diff --git a/libntp/Makefile.am b/libntp/Makefile.am index 04b53b0cb..7b30afae8 100644 --- a/libntp/Makefile.am +++ b/libntp/Makefile.am @@ -81,6 +81,7 @@ libntp_a_SRCS = \ msyslog.c \ netof.c \ ntp_calendar.c \ + ntp_calgps.c \ ntp_crypto_rnd.c \ ntp_intres.c \ ntp_libopts.c \ @@ -104,6 +105,7 @@ libntp_a_SRCS = \ strdup.c \ strl_obsd.c \ syssignal.c \ + timespecops.c \ timetoa.c \ timevalops.c \ uglydate.c \ diff --git a/libntp/ntp_calendar.c b/libntp/ntp_calendar.c index 79742688a..addd50ab5 100644 --- a/libntp/ntp_calendar.c +++ b/libntp/ntp_calendar.c @@ -40,16 +40,10 @@ * complement can be easily created using XOR and a mask. * * Finally, check for overflow conditions is minimal. There are only two - * calculation steps in the whole calendar that suffer from an internal - * overflow, and these conditions are checked: errno is set to EDOM and - * the results are clamped/saturated in this case. All other functions - * do not suffer from internal overflow and simply return the result - * truncated to 32 bits. - * - * This is a sacrifice made for execution speed. Since a 32-bit day - * counter covers +/- 5,879,610 years and the clamp limits the effective - * range to +/-2.9 million years, this should not pose a problem here. - * + * calculation steps in the whole calendar that potentially suffer from + * an internal overflow, and these are coded in a way that avoids + * it. All other functions do not suffer from internal overflow and + * simply return the result truncated to 32 bits. */ #include @@ -61,6 +55,9 @@ #include "ntp_fp.h" #include "ntp_unixtime.h" +#include "ntpd.h" +#include "lib_strbuf.h" + /* For now, let's take the conservative approach: if the target property * macros are not defined, check a few well-known compiler/architecture * settings. Default is to assume that the representation of signed @@ -88,6 +85,10 @@ # define TARGET_HAS_SAR 0 #endif +#if !defined(HAVE_64BITREGS) && defined(UINT64_MAX) && (SIZE_MAX >= UINT64_MAX) +# define HAVE_64BITREGS +#endif + /* *--------------------------------------------------------------------- * replacing the 'time()' function @@ -139,47 +140,15 @@ int32_sflag( * we do this only if 'int' has at least 4 bytes. */ return (uint32_t)(v >> 31); - + # else /* This should be a rather generic approach for getting a sign * extension mask... */ return UINT32_C(0) - (uint32_t)(v < 0); - -# endif -} - -static inline uint32_t -int32_to_uint32_2cpl( - const int32_t v) -{ - uint32_t vu; - -# if TARGET_HAS_2CPL - - /* Just copy through the 32 bits from the signed value if we're - * on a two's complement target. - */ - vu = (uint32_t)v; - -# else - /* Convert from signed int to unsigned int two's complement. Do - * not make any assumptions about the representation of signed - * integers, but make sure signed integer overflow cannot happen - * here. A compiler on a two's complement target *might* find - * out that this is just a complicated cast (as above), but your - * mileage might vary. - */ - if (v < 0) - vu = ~(uint32_t)(-(v + 1)); - else - vu = (uint32_t)v; - # endif - - return vu; } static inline int32_t @@ -187,7 +156,7 @@ uint32_2cpl_to_int32( const uint32_t vu) { int32_t v; - + # if TARGET_HAS_2CPL /* Just copy through the 32 bits from the unsigned value if @@ -206,29 +175,10 @@ uint32_2cpl_to_int32( v = -(int32_t)(~vu) - 1; else v = (int32_t)vu; - + # endif - - return v; -} -/* Some of the calculations need to multiply the input by 4 before doing - * a division. This can cause overflow and strange results. Therefore we - * clamp / saturate the input operand. And since we do the calculations - * in unsigned int with an extra sign flag/mask, we only loose one bit - * of the input value range. - */ -static inline uint32_t -uint32_saturate( - uint32_t vu, - uint32_t mu) -{ - static const uint32_t limit = UINT32_MAX/4u; - if ((mu ^ vu) > limit) { - vu = mu ^ limit; - errno = EDOM; - } - return vu; + return v; } /* @@ -504,40 +454,38 @@ ntpcal_periodic_extend( int32_t cycle ) { - uint32_t diff; - char cpl = 0; /* modulo complement flag */ - char neg = 0; /* sign change flag */ - - /* make the cycle positive and adjust the flags */ - if (cycle < 0) { - cycle = - cycle; - neg ^= 1; - cpl ^= 1; + /* Implement a 4-quadrant modulus calculation by 2 2-quadrant + * branches, one for positive and one for negative dividers. + * Everything else can be handled by bit level logic and + * conditional one's complement arithmetic. By convention, we + * assume + * + * x % b == 0 if |b| < 2 + * + * that is, we don't actually divide for cycles of -1,0,1 and + * return the pivot value in that case. + */ + uint32_t uv = (uint32_t)value; + uint32_t up = (uint32_t)pivot; + uint32_t uc, sf; + + if (cycle > 1) + { + uc = (uint32_t)cycle; + sf = UINT32_C(0) - (value < pivot); + + uv = sf ^ (uv - up); + uv %= uc; + pivot += (uc & sf) + (sf ^ uv); } - /* guard against div by zero or one */ - if (cycle > 1) { - /* - * Get absolute difference as unsigned quantity and - * the complement flag. This is done by always - * subtracting the smaller value from the bigger - * one. - */ - if (value >= pivot) { - diff = int32_to_uint32_2cpl(value) - - int32_to_uint32_2cpl(pivot); - } else { - diff = int32_to_uint32_2cpl(pivot) - - int32_to_uint32_2cpl(value); - cpl ^= 1; - } - diff %= (uint32_t)cycle; - if (diff) { - if (cpl) - diff = (uint32_t)cycle - diff; - if (neg) - diff = ~diff + 1; - pivot += uint32_2cpl_to_int32(diff); - } + else if (cycle < -1) + { + uc = ~(uint32_t)cycle + 1; + sf = UINT32_C(0) - (value > pivot); + + uv = sf ^ (up - uv); + uv %= uc; + pivot -= (uc & sf) + (sf ^ uv); } return pivot; } @@ -557,7 +505,7 @@ ntpcal_periodic_extend( * standard. (Though this is admittedly not one of the most 'natural' * aspects of the 'C' language and easily to get wrong.) * - * see + * see * http://www.open-std.org/jtc1/sc22/wg14/www/docs/n1570.pdf * "ISO/IEC 9899:201x Committee Draft — April 12, 2011" * 6.4.4.1 Integer constants, clause 5 @@ -565,7 +513,7 @@ ntpcal_periodic_extend( * why there is no sign extension/overflow problem here. * * But to ease the minds of the doubtful, I added back the 'u' qualifiers - * that somehow got lost over the last years. + * that somehow got lost over the last years. */ @@ -574,7 +522,7 @@ ntpcal_periodic_extend( * Convert a timestamp in NTP scale to a 64bit seconds value in the UN*X * scale with proper epoch unfolding around a given pivot or the current * system time. This function happily accepts negative pivot values as - * timestamps befor 1970-01-01, so be aware of possible trouble on + * timestamps before 1970-01-01, so be aware of possible trouble on * platforms with 32bit 'time_t'! * * This is also a periodic extension, but since the cycle is 2^32 and @@ -690,74 +638,139 @@ ntpcal_daysplit( ) { ntpcal_split res; - uint32_t Q; + uint32_t Q, R; -# if defined(HAVE_INT64) - - /* Manual floor division by SECSPERDAY. This uses the one's - * complement trick, too, but without an extra flag value: The - * flag would be 64bit, and that's a bit of overkill on a 32bit - * target that has to use a register pair for a 64bit number. +# if defined(HAVE_64BITREGS) + + /* Assume we have 64-bit registers an can do a divison by + * constant reasonably fast using the one's complement trick.. */ + uint64_t sf64 = (uint64_t)-(ts->q_s < 0); + Q = (uint32_t)(sf64 ^ ((sf64 ^ ts->Q_s) / SECSPERDAY)); + R = (uint32_t)(ts->Q_s - Q * SECSPERDAY); + +# elif defined(UINT64_MAX) + if (ts->q_s < 0) Q = ~(uint32_t)(~ts->Q_s / SECSPERDAY); else - Q = (uint32_t)(ts->Q_s / SECSPERDAY); + Q = (uint32_t)( ts->Q_s / SECSPERDAY); + R = ts->D_s.lo - Q * SECSPERWEEK; # else - - uint32_t ah, al, sflag, A; - - /* get operand into ah/al (either ts or ts' one's complement, - * for later floor division) - */ - sflag = int32_sflag(ts->d_s.hi); - ah = sflag ^ ts->D_s.hi; - al = sflag ^ ts->D_s.lo; - - /* Since 86400 == 128*675 we can drop the least 7 bits and - * divide by 675 instead of 86400. Then the maximum remainder - * after each devision step is 674, and we need 10 bits for - * that. So in the next step we can shift in 22 bits from the - * numerator. + + /* We don't have 64bit regs. That hurts a bit. * - * Therefore we load the accu with the top 13 bits (51..63) in - * the first shot. We don't have to remember the quotient -- it - * would be shifted out anyway. - */ - A = ah >> 19; - if (A >= 675) - A = (A % 675u); - - /* Now assemble the remainder with bits 29..50 from the - * numerator and divide. This creates the upper ten bits of the - * quotient. (Well, the top 22 bits of a 44bit result. But that - * will be truncated to 32 bits anyway.) + * Here we use a mean trick to get away with just one explicit + * modulo operation and pure 32-bit ops. + * + * Remember: 86400 <--> 128 * 675 + * + * So we discard the lowest 7 bit and do an exact division by + * 675, modulo 2**32. + * + * First we shift out the lower 7 bits. + * + * Then we use a digit-wise pseudo-reduction, where a 'digit' is + * actually a 16-bit group. This is followed by a full reduction + * with a 'true' division step. This yields the modulus of the + * full 64-bit value. The sign bit gets some extra treatment. + * + * Then we decrement the lower limb by that modulus, so it is + * exactly divisible by 675. [*] + * + * Then we multiply with the modular inverse of 675 (mod 2**32) + * and voila, we have the result. + * + * Special Thanks to Henry S. Warren and his "Hacker's delight" + * for giving that idea. + * + * (Note[*]: that's not the full truth. We would have to + * subtract the modulus from the full 64 bit number to get a + * number that is divisible by 675. But since we use the + * multiplicative inverse (mod 2**32) there's no reason to carry + * the subtraction into the upper bits!) */ - A = (A << 19) | (ah & 0x0007FFFFu); - A = (A << 3) | (al >> 29); - Q = A / 675u; - A = A % 675u; + uint32_t al = ts->D_s.lo; + uint32_t ah = ts->D_s.hi; + + /* shift out the lower 7 bits, smash sign bit */ + al = (al >> 7) | (ah << 25); + ah = (ah >> 7) & 0x00FFFFFFu; + + R = (ts->d_s.hi < 0) ? 239 : 0;/* sign bit value */ + R += (al & 0xFFFF); + R += (al >> 16 ) * 61u; /* 2**16 % 675 */ + R += (ah & 0xFFFF) * 346u; /* 2**32 % 675 */ + R += (ah >> 16 ) * 181u; /* 2**48 % 675 */ + R %= 675u; /* final reduction */ + Q = (al - R) * 0x2D21C10Bu; /* modinv(675, 2**32) */ + R = (R << 7) | (ts->d_s.lo & 0x07F); + +# endif - /* Now assemble the remainder with bits 7..28 from the numerator - * and do a final division step. - */ - A = (A << 22) | ((al >> 7) & 0x003FFFFFu); - Q = (Q << 22) | (A / 675u); + res.hi = uint32_2cpl_to_int32(Q); + res.lo = R; - /* The last 7 bits get simply dropped, as they have no affect on - * the quotient when dividing by 86400. - */ + return res; +} + +/* + *--------------------------------------------------------------------- + * Split a 64bit seconds value into elapsed weeks in 'res.hi' and + * elapsed seconds since week start in 'res.lo' using explicit floor + * division. This function happily accepts negative time values as + * timestamps before the respective epoch start. + *--------------------------------------------------------------------- + */ +ntpcal_split +ntpcal_weeksplit( + const vint64 *ts + ) +{ + ntpcal_split res; + uint32_t Q, R; - /* apply sign correction and calculate the true floor - * remainder. + /* This is a very close relative to the day split function; for + * details, see there! */ - Q ^= sflag; - + +# if defined(HAVE_64BITREGS) + + uint64_t sf64 = (uint64_t)-(ts->q_s < 0); + Q = (uint32_t)(sf64 ^ ((sf64 ^ ts->Q_s) / SECSPERWEEK)); + R = (uint32_t)(ts->Q_s - Q * SECSPERWEEK); + +# elif defined(UINT64_MAX) + + if (ts->q_s < 0) + Q = ~(uint32_t)(~ts->Q_s / SECSPERWEEK); + else + Q = (uint32_t)( ts->Q_s / SECSPERWEEK); + R = ts->D_s.lo - Q * SECSPERWEEK; + +# else + + /* Remember: 7*86400 <--> 604800 <--> 128 * 4725 */ + uint32_t al = ts->D_s.lo; + uint32_t ah = ts->D_s.hi; + + al = (al >> 7) | (ah << 25); + ah = (ah >> 7) & 0x00FFFFFF; + + R = (ts->d_s.hi < 0) ? 2264 : 0;/* sign bit value */ + R += (al & 0xFFFF); + R += (al >> 16 ) * 4111u; /* 2**16 % 4725 */ + R += (ah & 0xFFFF) * 3721u; /* 2**32 % 4725 */ + R += (ah >> 16 ) * 2206u; /* 2**48 % 4725 */ + R %= 4725u; /* final reduction */ + Q = (al - R) * 0x98BBADDDu; /* modinv(4725, 2**32) */ + R = (R << 7) | (ts->d_s.lo & 0x07F); + # endif - + res.hi = uint32_2cpl_to_int32(Q); - res.lo = ts->D_s.lo - Q * SECSPERDAY; + res.lo = R; return res; } @@ -779,23 +792,23 @@ priv_timesplit( * one's complement trick and factoring out the intermediate XOR * ops to reduce the number of operations. */ - uint32_t us, um, uh, ud, sflag; - - sflag = int32_sflag(ts); - us = int32_to_uint32_2cpl(ts); + uint32_t us, um, uh, ud, sf32; - um = (sflag ^ us) / SECSPERMIN; + sf32 = int32_sflag(ts); + + us = (uint32_t)ts; + um = (sf32 ^ us) / SECSPERMIN; uh = um / MINSPERHR; ud = uh / HRSPERDAY; - um ^= sflag; - uh ^= sflag; - ud ^= sflag; + um ^= sf32; + uh ^= sf32; + ud ^= sf32; split[0] = (int32_t)(uh - ud * HRSPERDAY ); split[1] = (int32_t)(um - uh * MINSPERHR ); split[2] = (int32_t)(us - um * SECSPERMIN); - + return uint32_2cpl_to_int32(ud); } @@ -815,45 +828,80 @@ ntpcal_split_eradays( int *isleapyear ) { - /* Use the fast cyclesplit algorithm here, to calculate the + /* Use the fast cycle split algorithm here, to calculate the * centuries and years in a century with one division each. This * reduces the number of division operations to two, but is - * susceptible to internal range overflow. We make sure the - * input operands are in the safe range; this still gives us - * approx +/-2.9 million years. + * susceptible to internal range overflow. We take some extra + * steps to avoid the gap. */ ntpcal_split res; int32_t n100, n001; /* calendar year cycles */ - uint32_t uday, Q, sflag; - - /* split off centuries first */ - sflag = int32_sflag(days); - uday = uint32_saturate(int32_to_uint32_2cpl(days), sflag); - uday = (4u * uday) | 3u; - Q = sflag ^ ((sflag ^ uday) / GREGORIAN_CYCLE_DAYS); - uday = uday - Q * GREGORIAN_CYCLE_DAYS; + uint32_t uday, Q; + + /* split off centuries first + * + * We want to execute '(days * 4 + 3) /% 146097' under floor + * division rules in the first step. Well, actually we want to + * calculate 'floor((days + 0.75) / 36524.25)', but we want to + * do it in scaled integer calculation. + */ + +# if defined(HAVE_64BITREGS) + + /* not too complicated with an itermediate 64bit value */ + uint64_t ud64, sf64; + ud64 = ((uint64_t)days << 2) | 3u; + sf64 = (uint64_t)-(days < 0); + Q = (uint32_t)(sf64 ^ ((sf64 ^ ud64) / GREGORIAN_CYCLE_DAYS)); + uday = (uint32_t)(ud64 - Q * GREGORIAN_CYCLE_DAYS); n100 = uint32_2cpl_to_int32(Q); +# else + + /* 4*days+3 can suffer from range overflow. So we start with an + * approximation for the quotient; the remainder using this + * value is bound to be non-negative and fits into 32 bit, so we + * can avoid extended precision division here without limiting + * the input range. (The derivation is a bit tricky.) + * + * The extra price are a few bit ops and a multiplication by + * constant, which sounds reasonable. + */ + /* approximation, signed + * note: ceil(log2(36524.25)) --> 16 + * floor(65536 / 36524.25 * 2**12) -> 7349 + */ + uint32_t sf32; + sf32 = int32_sflag(days); + uday = (((uint32_t)days << 2) | 3); + + Q = ((((uint32_t)days ^ sf32) >> (16 + sf32)) ^ sf32) + sf32; + uday -= Q * GREGORIAN_CYCLE_DAYS; + n100 = uint32_2cpl_to_int32(Q); + + /* full unsigned division on the remainder */ + Q = uday / GREGORIAN_CYCLE_DAYS; + uday -= Q * GREGORIAN_CYCLE_DAYS; + n100 += Q; + +# endif /* Split off years in century -- days >= 0 here, and we're far * away from integer overflow trouble now. */ uday |= 3; - n001 = uday / GREGORIAN_NORMAL_LEAP_CYCLE_DAYS; - uday = uday % GREGORIAN_NORMAL_LEAP_CYCLE_DAYS; + n001 = uday / GREGORIAN_NORMAL_LEAP_CYCLE_DAYS; + uday -= n001 * GREGORIAN_NORMAL_LEAP_CYCLE_DAYS; /* Assemble the year and day in year */ res.hi = n100 * 100 + n001; res.lo = uday / 4u; - /* Eventually set the leap year flag. Note: 0 <= n001 <= 99 and - * Q is still the two's complement representation of the - * centuries: The modulo 4 ops can be done with masking here. - * We also shift the year and the century by one, so the tests - * can be done against zero instead of 3. - */ - if (isleapyear) - *isleapyear = !((n001+1) & 3) - && ((n001 != 99) || !((Q+1) & 3)); - + /* Possibly set the leap year flag */ + if (isleapyear) { + uint32_t tc = (uint32_t)n100 + 1; + uint32_t ty = (uint32_t)n001 + 1; + *isleapyear = !(ty & 3) + && ((ty != 100) || !(tc & 3)); + } return res; } @@ -881,7 +929,7 @@ ntpcal_split_yeardays( if (0 <= eyd && eyd < lt[12]) { /* get zero-based month by approximation & correction step */ res.hi = eyd >> 5; /* approx month; might be 1 too low */ - if (lt[res.hi + 1] <= eyd) /* fixup approximative month value */ + if (lt[res.hi + 1] <= eyd) /* fix approximated month value */ res.hi += 1; res.lo = eyd - lt[res.hi]; } else { @@ -1087,6 +1135,46 @@ ntpcal_time_to_date( * ==================================================================== */ +#if !defined(HAVE_INT64) +static vint64 +_dwjoin( + uint16_t mul, + int32_t hi, + int32_t lo + ) +{ + vint64 res; + uint32_t p1, p2; + int sf; + + p1 = (uint32_t)hi; + sf = (hi < 0); + p1 = (p1 + sf) ^ sf; /* absolute value if 'hi' */ + + /* assemble major units */ + res.D_s.lo = (p1 & 0xFFFF) * mul; + res.D_s.hi = 0; + p1 = (p1 >> 16) * mul; + p2 = p1 >> 16; + p1 = p1 << 16; + M_ADD(res.D_s.hi, res.D_s.lo, p2, p1); + + /* mul by 128, using shift */ + res.D_s.hi = (res.D_s.hi << 7) | (res.D_s.lo >> 25); + res.D_s.lo = (res.D_s.lo << 7); + + /* fix sign */ + if (sf) + M_NEG(res.D_s.hi, res.D_s.lo); + + /* properly add seconds */ + p1 = (uint32_t)lo; + p2 = UINT32_C(0) - (lo < 0); + M_ADD(res.D_s.hi, res.D_s.lo, p2, p1); + return res; +} +#endif + /* *--------------------------------------------------------------------- * Merge a number of days and a number of seconds into seconds, @@ -1109,42 +1197,36 @@ ntpcal_dayjoin( # else - uint32_t p1, p2; - int isneg; + res = _dwjoin(675, days, secs); - /* - * res = days *86400 + secs, using manual 16/32 bit - * multiplications and shifts. - */ - isneg = (days < 0); - if (isneg) - days = -days; +# endif - /* assemble days * 675 */ - res.D_s.lo = (days & 0xFFFF) * 675u; - res.D_s.hi = 0; - p1 = (days >> 16) * 675u; - p2 = p1 >> 16; - p1 = p1 << 16; - M_ADD(res.D_s.hi, res.D_s.lo, p2, p1); + return res; +} - /* mul by 128, using shift */ - res.D_s.hi = (res.D_s.hi << 7) | (res.D_s.lo >> 25); - res.D_s.lo = (res.D_s.lo << 7); +/* + *--------------------------------------------------------------------- + * Merge a number of weeks and a number of seconds into seconds, + * expressed in 64 bits to avoid overflow. + *--------------------------------------------------------------------- + */ +vint64 +ntpcal_weekjoin( + int32_t week, + int32_t secs + ) +{ + vint64 res; - /* fix sign */ - if (isneg) - M_NEG(res.D_s.hi, res.D_s.lo); +# if defined(HAVE_INT64) - /* properly add seconds */ - p2 = 0; - if (secs < 0) { - p1 = (uint32_t)-secs; - M_NEG(p2, p1); - } else { - p1 = (uint32_t)secs; - } - M_ADD(res.D_s.hi, res.D_s.lo, p2, p1); + res.q_s = week; + res.q_s *= SECSPERWEEK; + res.q_s += secs; + +# else + + res = _dwjoin(4725, week, secs); # endif @@ -1167,11 +1249,11 @@ ntpcal_leapyears_in_years( * get away with only one true division and doing shifts otherwise. */ - uint32_t sflag, sum, uyear; + uint32_t sf32, sum, uyear; - sflag = int32_sflag(years); - uyear = int32_to_uint32_2cpl(years); - uyear ^= sflag; + sf32 = int32_sflag(years); + uyear = (uint32_t)years; + uyear ^= sf32; sum = (uyear /= 4u); /* 4yr rule --> IN */ sum -= (uyear /= 25u); /* 100yr rule --> OUT */ @@ -1183,7 +1265,7 @@ ntpcal_leapyears_in_years( * the one's complement would have to be done when * adding/subtracting the terms. */ - return uint32_2cpl_to_int32(sflag ^ sum); + return uint32_2cpl_to_int32(sf32 ^ sum); } /* @@ -1230,14 +1312,15 @@ ntpcal_days_in_months( /* if still out of range, normalise by floor division ... */ if (res.lo < 0 || res.lo >= 12) { - uint32_t mu, Q, sflag; - sflag = int32_sflag(res.lo); - mu = int32_to_uint32_2cpl(res.lo); - Q = sflag ^ ((sflag ^ mu) / 12u); + uint32_t mu, Q, sf32; + sf32 = int32_sflag(res.lo); + mu = (uint32_t)res.lo; + Q = sf32 ^ ((sf32 ^ mu) / 12u); + res.hi += uint32_2cpl_to_int32(Q); res.lo = mu - Q * 12u; } - + /* get cummulated days in year with unshift */ res.lo = shift_month_table[res.lo] - 306; @@ -1624,7 +1707,7 @@ ntpcal_weekday_lt( * w = (y * a + b ) / k * y = (w * a' + b') / k' * - * In this implementation the values of k and k' are chosen to be + * In this implementation the values of k and k' are chosen to be the * smallest possible powers of two, so the division can be implemented * as shifts if the optimiser chooses to do so. * @@ -1640,20 +1723,20 @@ int32_t isocal_weeks_in_years( int32_t years ) -{ +{ /* * use: w = (y * 53431 + b[c]) / 1024 as interpolation */ static const uint16_t bctab[4] = { 157, 449, 597, 889 }; int32_t cs, cw; - uint32_t cc, ci, yu, sflag; + uint32_t cc, ci, yu, sf32; + + sf32 = int32_sflag(years); + yu = (uint32_t)years; - sflag = int32_sflag(years); - yu = int32_to_uint32_2cpl(years); - /* split off centuries, using floor division */ - cc = sflag ^ ((sflag ^ yu) / 100u); + cc = sf32 ^ ((sf32 ^ yu) / 100u); yu -= cc * 100u; /* calculate century cycles shift and cycle index: @@ -1666,9 +1749,9 @@ isocal_weeks_in_years( * shifting. */ ci = cc * 3u + 1; - cs = uint32_2cpl_to_int32(sflag ^ ((sflag ^ ci) / 4u)); + cs = uint32_2cpl_to_int32(sf32 ^ ((sf32 ^ ci) / 4u)); ci = ci % 4u; - + /* Get weeks in century. Can use plain division here as all ops * are >= 0, and let the compiler sort out the possible * optimisations. @@ -1697,30 +1780,58 @@ isocal_split_eraweeks( ntpcal_split res; int32_t cc, ci; - uint32_t sw, cy, Q, sflag; + uint32_t sw, cy, Q; - /* Use two fast cycle-split divisions here. This is again - * susceptible to internal overflow, so we check the range. This - * still permits more than +/-20 million years, so this is - * likely a pure academical problem. + /* Use two fast cycle-split divisions again. Herew e want to + * execute '(weeks * 4 + 2) /% 20871' under floor division rules + * in the first step. + * + * This is of course (again) susceptible to internal overflow if + * coded directly in 32 bit. And again the trick is to make a + * first approximation that guarantees the remainder to be + * non-negative and comparably small, so a full division can be + * used to polish up the result without resorting to extended + * precision division. * - * We want to execute '(weeks * 4 + 2) /% 20871' under floor - * division rules in the first step. + * And of course it's really easy if we have full 64bit regs. + */ + +# if defined(HAVE_64BITREGS) + + uint64_t sf64, sw64; + sf64 = (uint64_t)-(weeks < 0); + sw64 = ((uint64_t)weeks << 2) | 2u; + Q = (uint32_t)(sf64 ^ ((sf64 ^ sw64) / GREGORIAN_CYCLE_WEEKS)); + sw = (uint32_t)(sw64 - Q * GREGORIAN_CYCLE_WEEKS); + +# else + + uint32_t sf32; + sf32 = int32_sflag(weeks); + sw = ((uint32_t)weeks << 2) | 2; + + /* approximative division, signed + * note: ceil(log2(5217.75)) --> 13 */ - sflag = int32_sflag(weeks); - sw = uint32_saturate(int32_to_uint32_2cpl(weeks), sflag); - sw = 4u * sw + 2; - Q = sflag ^ ((sflag ^ sw) / GREGORIAN_CYCLE_WEEKS); + Q = ((((uint32_t)weeks ^ sf32) >> (13 + sf32)) ^ sf32) + sf32; sw -= Q * GREGORIAN_CYCLE_WEEKS; - ci = Q % 4u; + + /* exact division, unsigned (use 'cy' as temp buffer) */ + cy = sw / GREGORIAN_CYCLE_WEEKS; + sw -= cy * GREGORIAN_CYCLE_WEEKS; + Q += cy; + +# endif + + ci = Q & 3u; cc = uint32_2cpl_to_int32(Q); /* Split off years; sw >= 0 here! The scaled weeks in the years * are scaled up by 157 afterwards. - */ + */ sw = (sw / 4u) * 157u + bctab[ci]; - cy = sw / 8192u; /* ws >> 13 , let the compiler sort it out */ - sw = sw % 8192u; /* ws & 8191, let the compiler sort it out */ + cy = sw / 8192u; /* sw >> 13 , let the compiler sort it out */ + sw = sw % 8192u; /* sw & 8191, let the compiler sort it out */ /* assemble elapsed years and downscale the elapsed weeks in * the year. @@ -1744,7 +1855,7 @@ isocal_ntp64_to_date( { ntpcal_split ds; int32_t ts[3]; - uint32_t uw, ud, sflag; + uint32_t uw, ud, sf32; /* * Split NTP time into days and seconds, shift days into CE @@ -1760,10 +1871,11 @@ isocal_ntp64_to_date( /* split days into days and weeks, using floor division in unsigned */ ds.hi += DAY_NTP_STARTS - 1; /* shift from NTP to RDN */ - sflag = int32_sflag(ds.hi); - ud = int32_to_uint32_2cpl(ds.hi); - uw = sflag ^ ((sflag ^ ud) / DAYSPERWEEK); - ud -= uw * DAYSPERWEEK; + sf32 = int32_sflag(ds.hi); + ud = (uint32_t)ds.hi; + uw = sf32 ^ ((sf32 ^ ud) / DAYSPERWEEK); + ud -= uw * DAYSPERWEEK; + ds.hi = uint32_2cpl_to_int32(uw); ds.lo = ud; @@ -1839,7 +1951,7 @@ basedate_eval_buildstamp(void) { struct calendar jd; int32_t ed; - + if (!ntpcal_get_build_date(&jd)) return NTP_TO_UNIX_DAYS; @@ -1865,7 +1977,7 @@ basedate_eval_string( int rc, nc; size_t sl; - sl = strlen(str); + sl = strlen(str); rc = sscanf(str, "%4hu-%2hu-%2hu%n", &y, &m, &d, &nc); if (rc == 3 && (size_t)nc == sl) { if (m >= 1 && m <= 12 && d >= 1 && d <= 31) @@ -1909,7 +2021,7 @@ basedate_set_day( (unsigned long)day); day = NTP_TO_UNIX_DAYS; } - retv = s_baseday; + retv = s_baseday; s_baseday = day; ntpcal_rd_to_date(&jd, day + DAY_NTP_STARTS); msyslog(LOG_INFO, "basedate set to %04hu-%02hu-%02hu", @@ -1924,7 +2036,7 @@ basedate_set_day( ntpcal_rd_to_date(&jd, day + DAY_NTP_STARTS); msyslog(LOG_INFO, "gps base set to %04hu-%02hu-%02hu (week %d)", jd.year, (u_short)jd.month, (u_short)jd.monthday, s_gpsweek); - + return retv; } @@ -1966,10 +2078,37 @@ basedate_expand_gpsweek( #if GPSWEEKS != 1024 # error GPSWEEKS defined wrong -- should be 1024! #endif - + uint32_t diff; diff = ((uint32_t)weekno - s_gpsweek) & (GPSWEEKS - 1); return s_gpsweek + diff; } +/* + * ==================================================================== + * misc. helpers + * ==================================================================== + */ + +char * +ntpcal_iso8601std( + char * buf, + size_t len, + TcCivilDate * cdp + ) +{ + if (!buf) { + LIB_GETBUF(buf); + len = LIB_BUFLENGTH; + } + if (len) { + len = snprintf(buf, len, "%04u-%02u-%02uT%02u:%02u:%02u", + cdp->year, cdp->month, cdp->monthday, + cdp->hour, cdp->minute, cdp->second); + if (len < 0) + *buf = '\0'; + } + return buf; +} + /* -*-EOF-*- */ diff --git a/libntp/ntp_calgps.c b/libntp/ntp_calgps.c new file mode 100644 index 000000000..20e1f103a --- /dev/null +++ b/libntp/ntp_calgps.c @@ -0,0 +1,586 @@ +/* + * ntp_calgps.c - calendar for GPS/GNSS based clocks + * + * Written by Juergen Perlinger (perlinger@ntp.org) for the NTP project. + * The contents of 'html/copyright.html' apply. + * + * -------------------------------------------------------------------- + * + * This module implements stuff often used with GPS/GNSS receivers + */ + +#include +#include + +#include "ntp_types.h" +#include "ntp_calendar.h" +#include "ntp_calgps.h" +#include "ntp_stdlib.h" +#include "ntp_unixtime.h" + +#include "ntp_fp.h" +#include "ntpd.h" +#include "vint64ops.h" + +/* ==================================================================== + * misc. helpers -- might go elsewhere sometime? + * ==================================================================== + */ + +l_fp +ntpfp_with_fudge( + l_fp lfp, + double ofs + ) +{ + l_fp fpo; + /* calculate 'lfp - ofs' as '(l_fp)(-ofs) + lfp': negating a + * double is cheap, as it only flips one bit... + */ + ofs = -ofs; + DTOLFP(ofs, &fpo); + L_ADD(&fpo, &lfp); + return fpo; +} + + +/* ==================================================================== + * GPS calendar functions + * ==================================================================== + */ + +/* -------------------------------------------------------------------- + * normalization functions for day/time and week/time representations. + * Since we only use moderate offsets (leap second corrections and + * alike) it does not really pay off to do a floor-corrected division + * here. We use compare/decrement/increment loops instead. + * -------------------------------------------------------------------- + */ +static void +_norm_ntp_datum( + TNtpDatum * datum + ) +{ + static const int32_t limit = SECSPERDAY; + + if (datum->secs >= limit) { + do + ++datum->days; + while ((datum->secs -= limit) >= limit); + } else if (datum->secs < 0) { + do + --datum->days; + while ((datum->secs += limit) < 0); + } +} + +static void +_norm_gps_datum( + TGpsDatum * datum + ) +{ + static const int32_t limit = 7 * SECSPERDAY; + + if (datum->wsecs >= limit) { + do + ++datum->weeks; + while ((datum->wsecs -= limit) >= limit); + } else if (datum->wsecs < 0) { + do + --datum->weeks; + while ((datum->wsecs += limit) < 0); + } +} + +/* -------------------------------------------------------------------- + * Add an offset to a day/time and week/time representation. + * + * !!Attention!! the offset should be small, compared to the time period + * (either a day or a week). + * -------------------------------------------------------------------- + */ +void +gpsntp_add_offset( + TNtpDatum * datum, + l_fp offset + ) +{ + /* fraction can be added easily */ + datum->frac += offset.l_uf; + datum->secs += (datum->frac < offset.l_uf); + + /* avoid integer overflow on the seconds */ + if (offset.l_ui >= INT32_MAX) + datum->secs -= (int32_t)~offset.l_ui + 1; + else + datum->secs += (int32_t)offset.l_ui; + _norm_ntp_datum(datum); +} + +void +gpscal_add_offset( + TGpsDatum * datum, + l_fp offset + ) +{ + /* fraction can be added easily */ + datum->frac += offset.l_uf; + datum->wsecs += (datum->frac < offset.l_uf); + + + /* avoid integer overflow on the seconds */ + if (offset.l_ui >= INT32_MAX) + datum->wsecs -= (int32_t)~offset.l_ui + 1; + else + datum->wsecs += (int32_t)offset.l_ui; + _norm_gps_datum(datum); +} + +/* ------------------------------------------------------------------- + * API functions civil calendar and NTP datum + * ------------------------------------------------------------------- + */ + +static TNtpDatum +_gpsntp_fix_gps_era( + TcNtpDatum * in + ) +{ + /* force result in basedate era + * + * When calculating this directly in days, we have to execute a + * real modulus calculation, since we're obviously not doing a + * modulus by a power of 2. Executing this as true floor mod + * needs some care and is done under explicit usage of one's + * complement and masking to get mostly branchless code. + */ + static uint32_t const clen = 7*1024; + + uint32_t base, days, sign; + TNtpDatum out = *in; + + /* Get base in NTP day scale. No overflows here. */ + base = (basedate_get_gpsweek() + GPSNTP_WSHIFT) * 7 + - GPSNTP_DSHIFT; + days = out.days; + + sign = (uint32_t) -(days < base); + days = sign ^ (days - base); + days %= clen; + days = base + (sign & clen) + (sign ^ days); + + out.days = days; + return out; +} + +TNtpDatum +gpsntp_fix_gps_era( + TcNtpDatum * in + ) +{ + TNtpDatum out = *in; + _norm_ntp_datum(&out); + return _gpsntp_fix_gps_era(&out); +} + +/* ----------------------------------------------------------------- */ +static TNtpDatum +_gpsntp_from_daytime( + TcCivilDate * jd, + l_fp fofs, + TcNtpDatum * pivot + ) +{ + static const int32_t shift = SECSPERDAY / 2; + + TNtpDatum retv; + + /* set result based on pivot -- ops order is important here */ + ZERO(retv); + retv.secs = ntpcal_date_to_daysec(jd); + gpsntp_add_offset(&retv, fofs); /* result is normalized */ + retv.days = pivot->days; + + /* Manual periodic extension without division: */ + if (pivot->secs < shift) { + int32_t lim = pivot->secs + shift; + retv.days -= (retv.secs > lim || + (retv.secs == lim && retv.frac >= pivot->frac)); + } else { + int32_t lim = pivot->secs - shift; + retv.days += (retv.secs < lim || + (retv.secs == lim && retv.frac < pivot->frac)); + } + return _gpsntp_fix_gps_era(&retv); +} + +/* ----------------------------------------------------------------- + * Given the time-of-day part of a civil datum and an additional + * (fractional) offset, calculate a full time stamp around a given pivot + * time so that the difference between the pivot and the resulting time + * stamp is less or equal to 12 hours absolute. + */ +TNtpDatum +gpsntp_from_daytime2( + TcCivilDate * jd, + l_fp fofs, + TcNtpDatum * pivot + ) +{ + TNtpDatum dpiv = *pivot; + _norm_ntp_datum(&dpiv); + return _gpsntp_from_daytime(jd, fofs, &dpiv); +} + +/* ----------------------------------------------------------------- + * This works similar to 'gpsntp_from_daytime1()' and actually even uses + * it, but the pivot is calculated from the pivot given as 'l_fp' in NTP + * time scale. This is in turn expanded around the current system time, + * and the resulting absolute pivot is then used to calculate the full + * NTP time stamp. + */ +TNtpDatum +gpsntp_from_daytime1( + TcCivilDate * jd, + l_fp fofs, + l_fp pivot + ) +{ + vint64 pvi64; + TNtpDatum dpiv; + ntpcal_split split; + + pvi64 = ntpcal_ntp_to_ntp(pivot.l_ui, NULL); + split = ntpcal_daysplit(&pvi64); + dpiv.days = split.hi; + dpiv.secs = split.lo; + dpiv.frac = pivot.l_uf; + return _gpsntp_from_daytime(jd, fofs, &dpiv); +} + +/* ----------------------------------------------------------------- + * Given a calendar date, zap it into a GPS time format and then convert + * that one into the NTP time scale. + */ +TNtpDatum +gpsntp_from_calendar( + TcCivilDate * jd, + l_fp fofs + ) +{ + TGpsDatum gps; + gps = gpscal_from_calendar(jd, fofs); + return gpsntp_from_gpscal(&gps); +} + +/* ----------------------------------------------------------------- + * create a civil calendar datum from a NTP date representation + */ +void +gpsntp_to_calendar( + TCivilDate * cd, + TcNtpDatum * nd + ) +{ + ntpcal_rd_to_date( + cd, + nd->days + DAY_NTP_STARTS + ntpcal_daysec_to_date( + cd, nd->secs)); +} + +/* ----------------------------------------------------------------- + * get day/tod representation from week/tow datum + */ +TNtpDatum +gpsntp_from_gpscal( + TcGpsDatum * gd + ) +{ + TNtpDatum retv; + vint64 ts64; + ntpcal_split split; + + ts64 = ntpcal_weekjoin(gd->weeks, gd->wsecs); + ts64 = subv64u32(&ts64, (GPSNTP_DSHIFT * SECSPERDAY)); + split = ntpcal_daysplit(&ts64); + + retv.frac = gd->frac; + retv.secs = split.lo; + retv.days = split.hi; + return retv; +} + +/* ----------------------------------------------------------------- + * get LFP from ntp datum + */ +l_fp +ntpfp_from_ntpdatum( + TcNtpDatum * nd + ) +{ + l_fp retv; + + retv.l_uf = nd->frac; + retv.l_ui = nd->days * (uint32_t)SECSPERDAY + + nd->secs; + return retv; +} + +/* ------------------------------------------------------------------- + * API functions GPS week calendar + * + * Here we use a calendar base of 1899-12-31, so the NTP epoch has + * { 0, 86400.0 } in this representation. + * ------------------------------------------------------------------- + */ + +static TGpsDatum +_gpscal_fix_gps_era( + TcGpsDatum * in + ) +{ + /* force result in basedate era + * + * This is based on calculating the modulus to a power of two, + * so signed integer overflow does not affect the result. Which + * in turn makes for a very compact calculation... + */ + uint32_t base, week; + TGpsDatum out = *in; + + week = out.weeks; + base = basedate_get_gpsweek() + GPSNTP_WSHIFT; + week = base + ((week - base) & (GPSWEEKS - 1)); + out.weeks = week; + return out; +} + +TGpsDatum +gpscal_fix_gps_era( + TcGpsDatum * in + ) +{ + TGpsDatum out = *in; + _norm_gps_datum(&out); + return _gpscal_fix_gps_era(&out); +} + +/* ----------------------------------------------------------------- + * Given a calendar date, zap it into a GPS time format and the do a + * proper era mapping in the GPS time scale, based on the GPS base date. + * + * This function also augments the century if just a 2-digit year + * (0..99) is provided on input. + * + * This is a fail-safe against GPS receivers with an unknown starting + * point for their internal calendar calculation and therefore + * unpredictable (but reproducible!) rollover behavior. While there + * *are* receivers that create a full date in the proper way, many + * others just don't. The overall damage is minimized by simply not + * trusting the era mapping of the receiver and doing the era assignment + * with a configurable base date *inside* ntpd. + */ +TGpsDatum +gpscal_from_calendar( + TcCivilDate * jd, + l_fp fofs + ) +{ + TGpsDatum gps; + TCivilDate cal; + int32_t days, week; + + /* if needed, convert from 2-digit year to full year + * !!NOTE!! works only between 1980 and 2079! + */ + cal = *jd; + if (cal.year < 80) + cal.year += 2000; + else if (cal.year < 100) + cal.year += 1900; + + /* get RDN from date, possibly adjusting the century */ +again: if (cal.month && cal.monthday) { /* use y/m/d civil date */ + days = ntpcal_date_to_rd(&cal); + } else { /* using y/doy date */ + days = ntpcal_year_to_ystart(cal.year) + + (int32_t)cal.yearday + - 1; /* both RDN and yearday start with '1'. */ + } + if (days < DAY_GPS_STARTS) { + cal.year += 100; + goto again; + } + + /* get GPS time since start of GPS */ + days -= DAY_GPS_STARTS; + week = days / 7; + days -= week * 7; + + /* re-base on start of NTP with weeks mapped to 1024 weeks + * starting with the GPS base day set in the calendar. + */ + gps.weeks = week + GPSNTP_WSHIFT; + gps.wsecs = days * SECSPERDAY + ntpcal_date_to_daysec(&cal); + gps.frac = 0; + gpscal_add_offset(&gps, fofs); + return _gpscal_fix_gps_era(&gps); +} + +/* ----------------------------------------------------------------- + * get civil date from week/tow representation + */ +void +gpscal_to_calendar( + TCivilDate * cd, + TcGpsDatum * wd + ) +{ + TNtpDatum nd = gpsntp_from_gpscal(wd); + gpsntp_to_calendar(cd, &nd); +} + +/* ----------------------------------------------------------------- + * Given the week and seconds in week, as well as the fraction/offset + * (which should/could include the leap seconds offset), unfold the + * weeks (which are assumed to have just 10 bits) into expanded weeks + * based on the GPS base date derived from the build date (default) or + * set by the configuration. + * + * !NOTE! This function takes RAW GPS weeks, aligned to the GPS start + * (1980-01-06) on input. The output weeks will be aligned to NTPD's + * week calendar start (1899-12-31)! + */ +TGpsDatum +gpscal_from_gpsweek( + uint16_t week, + int32_t secs, + l_fp fofs + ) +{ + TGpsDatum retv; + + retv.frac = 0; + retv.wsecs = secs; + retv.weeks = week + GPSNTP_WSHIFT; + gpscal_add_offset(&retv, fofs); + return _gpscal_fix_gps_era(&retv); +} + +/* ----------------------------------------------------------------- + * internal work hores for time-of-week expansion + */ +static TGpsDatum +_gpscal_from_weektime( + int32_t wsecs, + l_fp fofs, + TcGpsDatum * pivot + ) +{ + static const int32_t shift = SECSPERWEEK / 2; + + TGpsDatum retv; + + /* set result based on pivot -- ops order is important here */ + ZERO(retv); + retv.wsecs = wsecs; + gpscal_add_offset(&retv, fofs); /* result is normalized */ + retv.weeks = pivot->weeks; + + /* Manual periodic extension without division: */ + if (pivot->wsecs < shift) { + int32_t lim = pivot->wsecs + shift; + retv.weeks -= (retv.wsecs > lim || + (retv.wsecs == lim && retv.frac >= pivot->frac)); + } else { + int32_t lim = pivot->wsecs - shift; + retv.weeks += (retv.wsecs < lim || + (retv.wsecs == lim && retv.frac < pivot->frac)); + } + return _gpscal_fix_gps_era(&retv); +} + +/* ----------------------------------------------------------------- + * expand a time-of-week around a pivot given as week datum + */ +TGpsDatum +gpscal_from_weektime2( + int32_t wsecs, + l_fp fofs, + TcGpsDatum * pivot + ) +{ + TGpsDatum wpiv = * pivot; + _norm_gps_datum(&wpiv); + return _gpscal_from_weektime(wsecs, fofs, &wpiv); +} + +/* ----------------------------------------------------------------- + * epand a time-of-week around an pivot given as LFP, which in turn + * is expanded around the current system time and then converted + * into a week datum. + */ +TGpsDatum +gpscal_from_weektime1( + int32_t wsecs, + l_fp fofs, + l_fp pivot + ) +{ + vint64 pvi64; + TGpsDatum wpiv; + ntpcal_split split; + + /* get 64-bit pivot in NTP epoch */ + pvi64 = ntpcal_ntp_to_ntp(pivot.l_ui, NULL); + + /* convert to weeks since 1899-12-31 and seconds in week */ + pvi64 = addv64u32(&pvi64, (GPSNTP_DSHIFT * SECSPERDAY)); + split = ntpcal_weeksplit(&pvi64); + + wpiv.weeks = split.hi; + wpiv.wsecs = split.lo; + wpiv.frac = pivot.l_uf; + return _gpscal_from_weektime(wsecs, fofs, &wpiv); +} + +/* ----------------------------------------------------------------- + * get week/tow representation from day/tod datum + */ +TGpsDatum +gpscal_from_gpsntp( + TcNtpDatum * gd + ) +{ + TGpsDatum retv; + vint64 ts64; + ntpcal_split split; + + ts64 = ntpcal_dayjoin(gd->days, gd->secs); + ts64 = addv64u32(&ts64, (GPSNTP_DSHIFT * SECSPERDAY)); + split = ntpcal_weeksplit(&ts64); + + retv.frac = gd->frac; + retv.wsecs = split.lo; + retv.weeks = split.hi; + return retv; +} + +/* ----------------------------------------------------------------- + * convert week/tow to LFP stamp + */ +l_fp +ntpfp_from_gpsdatum( + TcGpsDatum * gd + ) +{ + l_fp retv; + + retv.l_uf = gd->frac; + retv.l_ui = gd->weeks * (uint32_t)SECSPERWEEK + + (uint32_t)gd->wsecs + - (uint32_t)SECSPERDAY * GPSNTP_DSHIFT; + return retv; +} + +/* -*-EOF-*- */ diff --git a/libntp/timespecops.c b/libntp/timespecops.c new file mode 100644 index 000000000..7dd1c6c88 --- /dev/null +++ b/libntp/timespecops.c @@ -0,0 +1,233 @@ +/* + * timespecops.c -- calculations on 'struct timespec' values + * + * Written by Juergen Perlinger (perlinger@ntp.org) for the NTP project. + * The contents of 'html/copyright.html' apply. + * + */ + +#include "config.h" + +#include +#include +#include + +#include "ntp.h" +#include "timetoa.h" +#include "timespecops.h" + + +/* nanoseconds per second */ +#define NANOSECONDS 1000000000 + +/* conversion between l_fp fractions and nanoseconds */ +#ifdef HAVE_U_INT64 +# define FTOTVN(tsf) \ + ((int32) \ + (((u_int64)(tsf) * NANOSECONDS + 0x80000000) >> 32)) +# define TVNTOF(tvu) \ + ((u_int32) \ + ((((u_int64)(tvu) << 32) + NANOSECONDS / 2) / \ + NANOSECONDS)) +#else +# define NSECFRAC (FRAC / NANOSECONDS) +# define FTOTVN(tsf) \ + ((int32)((tsf) / NSECFRAC + 0.5)) +# define TVNTOF(tvu) \ + ((u_int32)((tvu) * NSECFRAC + 0.5)) +#endif + + + +/* make sure nanoseconds are in nominal range */ +struct timespec +normalize_tspec( + struct timespec x + ) +{ +#if SIZEOF_LONG > 4 + long z; + + /* + * tv_nsec is of type 'long', and on a 64-bit machine using only + * loops becomes prohibitive once the upper 32 bits get + * involved. On the other hand, division by constant should be + * fast enough; so we do a division of the nanoseconds in that + * case. The floor adjustment step follows with the standard + * normalisation loops. And labs() is intentionally not used + * here: it has implementation-defined behaviour when applied + * to LONG_MIN. + */ + if (x.tv_nsec < -3l * NANOSECONDS || + x.tv_nsec > 3l * NANOSECONDS) { + z = x.tv_nsec / NANOSECONDS; + x.tv_nsec -= z * NANOSECONDS; + x.tv_sec += z; + } +#endif + /* since 10**9 is close to 2**32, we don't divide but do a + * normalisation in a loop; this takes 3 steps max, and should + * outperform a division even if the mul-by-inverse trick is + * employed. */ + if (x.tv_nsec < 0) + do { + x.tv_nsec += NANOSECONDS; + x.tv_sec--; + } while (x.tv_nsec < 0); + else if (x.tv_nsec >= NANOSECONDS) + do { + x.tv_nsec -= NANOSECONDS; + x.tv_sec++; + } while (x.tv_nsec >= NANOSECONDS); + + return x; +} + +/* x = abs(a) */ +struct timespec +abs_tspec( + struct timespec a + ) +{ + struct timespec c; + + c = normalize_tspec(a); + if (c.tv_sec < 0) { + if (c.tv_nsec != 0) { + c.tv_sec = -c.tv_sec - 1; + c.tv_nsec = NANOSECONDS - c.tv_nsec; + } else { + c.tv_sec = -c.tv_sec; + } + } + + return c; +} + +/* + * compare previously-normalised a and b + * return 1 / 0 / -1 if a < / == / > b + */ +int +cmp_tspec( + struct timespec a, + struct timespec b + ) +{ + int r; + + r = (a.tv_sec > b.tv_sec) - (a.tv_sec < b.tv_sec); + if (0 == r) + r = (a.tv_nsec > b.tv_nsec) - + (a.tv_nsec < b.tv_nsec); + + return r; +} + +/* + * test previously-normalised a + * return 1 / 0 / -1 if a < / == / > 0 + */ +int +test_tspec( + struct timespec a + ) +{ + int r; + + r = (a.tv_sec > 0) - (a.tv_sec < 0); + if (r == 0) + r = (a.tv_nsec > 0); + + return r; +} + +/* + * convert to l_fp type, relative and absolute + */ + +/* convert from timespec duration to l_fp duration */ +l_fp +tspec_intv_to_lfp( + struct timespec x + ) +{ + struct timespec v; + l_fp y; + + v = normalize_tspec(x); + y.l_uf = TVNTOF(v.tv_nsec); + y.l_i = (int32)v.tv_sec; + + return y; +} + +/* convert from l_fp type, relative signed/unsigned and absolute */ +struct timespec +lfp_intv_to_tspec( + l_fp x + ) +{ + struct timespec out; + l_fp absx; + int neg; + + neg = L_ISNEG(&x); + absx = x; + if (neg) { + L_NEG(&absx); + } + out.tv_nsec = FTOTVN(absx.l_uf); + out.tv_sec = absx.l_i; + if (neg) { + out.tv_sec = -out.tv_sec; + out.tv_nsec = -out.tv_nsec; + out = normalize_tspec(out); + } + + return out; +} + +struct timespec +lfp_uintv_to_tspec( + l_fp x + ) +{ + struct timespec out; + + out.tv_nsec = FTOTVN(x.l_uf); + out.tv_sec = x.l_ui; + + return out; +} + +/* + * absolute (timestamp) conversion. Input is time in NTP epoch, output + * is in UN*X epoch. The NTP time stamp will be expanded around the + * pivot time *p or the current time, if p is NULL. + */ +struct timespec +lfp_stamp_to_tspec( + l_fp x, + const time_t * p + ) +{ + struct timespec out; + vint64 sec; + + sec = ntpcal_ntp_to_time(x.l_ui, p); + out.tv_nsec = FTOTVN(x.l_uf); + + /* copying a vint64 to a time_t needs some care... */ +#if SIZEOF_TIME_T <= 4 + out.tv_sec = (time_t)sec.d_s.lo; +#elif defined(HAVE_INT64) + out.tv_sec = (time_t)sec.q_s; +#else + out.tv_sec = ((time_t)sec.d_s.hi << 32) | sec.d_s.lo; +#endif + + return out; +} + +/* -*-EOF-*- */ diff --git a/ntpd/ntp_refclock.c b/ntpd/ntp_refclock.c index d109b7115..9dea2e7e6 100644 --- a/ntpd/ntp_refclock.c +++ b/ntpd/ntp_refclock.c @@ -12,6 +12,7 @@ #include "ntp_refclock.h" #include "ntp_stdlib.h" #include "ntp_assert.h" +#include "timespecops.h" #include @@ -70,6 +71,59 @@ static int refclock_cmpl_fp (const void *, const void *); static int refclock_sample (struct refclockproc *); static int refclock_ioctl(int, u_int); +/* circular buffer functions + * + * circular buffer management comes in two flovours: + * for powers of two, and all others. + */ + +#if MAXSTAGE & (MAXSTAGE - 1) + +static void clk_add_sample( + struct refclockproc * const pp, + double sv + ) +{ + pp->coderecv = (pp->coderecv + 1) % MAXSTAGE; + if (pp->coderecv == pp->codeproc) + pp->codeproc = (pp->codeproc + 1) % MAXSTAGE; + pp->filter[pp->coderecv] = sv; +} + +static double clk_pop_sample( + struct refclockproc * const pp + ) +{ + if (pp->coderecv == pp->codeproc) + return 0; /* Maybe a NaN would be better? */ + pp->codeproc = (pp->codeproc + 1) % MAXSTAGE; + return pp->filter[pp->codeproc]; +} + +#else + +static inline void clk_add_sample( + struct refclockproc * const pp, + double sv + ) +{ + pp->coderecv = (pp->coderecv + 1) & (MAXSTAGE - 1); + if (pp->coderecv == pp->codeproc) + pp->codeproc = (pp->codeproc + 1) & (MAXSTAGE - 1); + pp->filter[pp->coderecv] = sv; +} + +static inline double clk_pop_sample( + struct refclockproc * const pp + ) +{ + if (pp->coderecv == pp->codeproc) + return 0; /* Maybe a NaN would be better? */ + pp->codeproc = (pp->codeproc + 1) & (MAXSTAGE - 1); + return pp->filter[pp->codeproc]; +} + +#endif /* * refclock_report - note the occurance of an event @@ -353,6 +407,65 @@ refclock_cmpl_fp( return 0; } +/* + * Get number of available samples + */ +int +refclock_samples_avail( + struct refclockproc const * pp + ) +{ + u_int na; + +# if MAXSTAGE & (MAXSTAGE - 1) + + na = pp->coderecv - pp->codeproc; + if (na > MAXSTAGE) + na += MAXSTAGE; + +# else + + na = (pp->coderecv - pp->codeproc) & (MAXSTAGE - 1); + +# endif + return na; +} + +/* + * Expire (remove) samples from the tail (oldest samples removed) + * + * Returns number of samples deleted + */ +int +refclock_samples_expire( + struct refclockproc * pp, + int nd + ) +{ + u_int na; + + if (nd <= 0) + return 0; + +# if MAXSTAGE & (MAXSTAGE - 1) + + na = pp->coderecv - pp->codeproc; + if (na > MAXSTAGE) + na += MAXSTAGE; + if ((u_int)nd < na) + nd = na; + pp->codeproc = (pp->codeproc + nd) % MAXSTAGE; + +# else + + na = (pp->coderecv - pp->codeproc) & (MAXSTAGE - 1); + if ((u_int)nd > na) + nd = (int)na; + pp->codeproc = (pp->codeproc + nd) & (MAXSTAGE - 1); + +# endif + return nd; +} /* * refclock_process_offset - update median filter @@ -376,7 +489,7 @@ refclock_process_offset( lftemp = lasttim; L_SUB(&lftemp, &lastrec); LFPTOD(&lftemp, doffset); - SAMPLE(doffset + fudge); + clk_add_sample(pp, doffset + fudge); } @@ -457,11 +570,8 @@ refclock_sample( * anything if the buffer is empty. */ n = 0; - while (pp->codeproc != pp->coderecv) { - pp->codeproc = (pp->codeproc + 1) % MAXSTAGE; - off[n] = pp->filter[pp->codeproc]; - n++; - } + while (pp->codeproc != pp->coderecv) + off[n++] = clk_pop_sample(pp); if (n == 0) return (0); @@ -1367,7 +1477,7 @@ refclock_pps( */ pp->lastrec.l_ui = (u_int32)ap->ts.tv_sec + JAN_1970; pp->lastrec.l_uf = (u_int32)(dtemp * FRAC); - SAMPLE(dcorr); + clk_add_sample(pp, dcorr); #ifdef DEBUG if (debug > 1) @@ -1377,4 +1487,196 @@ refclock_pps( return (1); } #endif /* HAVE_PPSAPI */ + + +/* + * ------------------------------------------------------------------- + * refclock_ppsaugment(...) -- correlate with PPS edge + * + * This function is used to correlate a receive time stamp with a PPS + * edge time stamp. It applies the necessary fudges and then tries to + * move the receive time stamp to the corresponding edge. This can warp + * into future, if a transmission delay of more than 500ms is not + * compensated with a corresponding fudge time2 value, because then the + * next PPS edge is nearer than the last. (Similiar to what the PPS ATOM + * driver does, but we deal with full time stamps here, not just phase + * shift information.) Likewise, a negative fudge time2 value must be + * used if the reference time stamp correlates with the *following* PPS + * pulse. + * + * Note that the receive time fudge value only needs to move the receive + * stamp near a PPS edge but that close proximity is not required; + * +/-100ms precision should be enough. But since the fudge value will + * probably also be used to compensate the transmission delay when no + * PPS edge can be related to the time stamp, it's best to get it as + * close as possible. + * + * It should also be noted that the typical use case is matching to the + * preceeding edge, as most units relate their sentences to the current + * second. + * + * The function returns FALSE if there is no correlation possible, TRUE + * otherwise. Reason for failures are: + * + * - no PPS/ATOM unit given + * - PPS stamp is stale (that is, the difference between the PPS stamp + * and the corrected time stamp would exceed two seconds) + * - The phase difference is too close to 0.5, and the decision wether + * to move up or down is too sensitive to noise. + * + * On output, the receive time stamp is updated with the 'fixed' receive + * time. + * ------------------------------------------------------------------- + */ + +int/*BOOL*/ +refclock_ppsaugment( + const struct refclock_atom * ap , /* for PPS io */ + l_fp * rcvtime , + double rcvfudge, /* i/o read fudge */ + double ppsfudge /* pps fudge */ + ) +{ + l_fp delta[1]; + +#ifdef HAVE_PPSAPI + + pps_info_t pps_info; + struct timespec timeout; + l_fp stamp[1]; + uint32_t phase; + + static const uint32_t s_plim_hi = UINT32_C(1932735284); + static const uint32_t s_plim_lo = UINT32_C(2362232013); + + /* fixup receive time in case we have to bail out early */ + DTOLFP(rcvfudge, delta); + L_SUB(rcvtime, delta); + + if (NULL == ap) + return FALSE; + + ZERO(timeout); + ZERO(pps_info); + + /* fetch PPS stamp from ATOM block */ + if (time_pps_fetch(ap->handle, PPS_TSFMT_TSPEC, + &pps_info, &timeout) < 0) + return FALSE; /* can't get time stamps */ + + /* get last active PPS edge before receive */ + if (ap->pps_params.mode & PPS_CAPTUREASSERT) + timeout = pps_info.assert_timestamp; + else if (ap->pps_params.mode & PPS_CAPTURECLEAR) + timeout = pps_info.clear_timestamp; + else + return FALSE; /* WHICH edge, please?!? */ + + /* convert PPS stamp to l_fp and apply fudge */ + *stamp = tspec_stamp_to_lfp(timeout); + DTOLFP(ppsfudge, delta); + L_SUB(stamp, delta); + + /* Get difference between PPS stamp (--> yield) and receive time + * (--> base) + */ + *delta = *stamp; + L_SUB(delta, rcvtime); + + /* check if either the PPS or the STAMP is stale in relation + * to each other. Bail if it is so... + */ + phase = delta->l_ui; + if (phase >= 2 && phase < (uint32_t)-2) + return FALSE; /* PPS is stale, don't use it */ + + /* If the phase is too close to 0.5, the decision whether to + * move up or down is becoming noise sensitive. That is, we + * might amplify usec noise between samples into seconds with a + * simple threshold. This can be solved by a Schmitt Trigger + * characteristic, but that would also require additional state + * where we could remember previous decisions. Easier to play + * dead duck and wait for the conditions to become clear. + */ + phase = delta->l_uf; + if (phase > s_plim_hi && phase < s_plim_lo) + return FALSE; /* we're in the noise lock gap */ + + /* sign-extend fraction into seconds */ + delta->l_ui = UINT32_C(0) - ((phase >> 31) & 1); + /* add it up now */ + L_ADD(rcvtime, delta); + return TRUE; + +# else /* have no PPS support at all */ + + /* just fixup receive time and fail */ + UNUSED_ARG(ap); + UNUSED_ARG(ppsfudge); + + DTOLFP(rcvfudge, delta); + L_SUB(rcvtime, delta); + return FALSE; + +# endif +} + + +/* + * ------------------------------------------------------------------- + * Save the last timecode string, making sure it's properly truncated + * if necessary and NUL terminated in any case. + */ +void +refclock_save_lcode( + struct refclockproc * pp, + char const * tc, + size_t len + ) +{ + if (len == (size_t)-1) + len = strnlen(tc, sizeof(pp->a_lastcode) - 1); + else if (len >= sizeof(pp->a_lastcode)) + len = sizeof(pp->a_lastcode) - 1; + + pp->lencode = (u_short)len; + memcpy(pp->a_lastcode, tc, len); + pp->a_lastcode[len] = '\0'; +} + +/* format data into a_lastcode */ +void +refclock_vformat_lcode( + struct refclockproc * pp, + char const * fmt, + va_list va + ) +{ + long len; + + len = vsnprintf(pp->a_lastcode, sizeof(pp->a_lastcode), fmt, va); + if (len <= 0) + len = 0; + else if (len >= sizeof(pp->a_lastcode)) + len = sizeof(pp->a_lastcode) - 1; + + pp->lencode = (u_short)len; + pp->a_lastcode[len] = '\0'; + /* !note! the NUL byte is needed in case vsnprintf() really fails */ +} + +void +refclock_format_lcode( + struct refclockproc * pp, + char const * fmt, + ... + ) +{ + va_list va; + + va_start(va, fmt); + refclock_vformat_lcode(pp, fmt, va); + va_end(va); +} + #endif /* REFCLOCK */ diff --git a/ntpd/ntp_restrict.c b/ntpd/ntp_restrict.c index f3c129342..aa029cc89 100644 --- a/ntpd/ntp_restrict.c +++ b/ntpd/ntp_restrict.c @@ -154,8 +154,6 @@ dump_restrict( void dump_restricts(void) { - int defaultv4_done = 0; - int defaultv6_done = 0; restrict_u * res; restrict_u * next; diff --git a/parseutil/dcfd.c b/parseutil/dcfd.c index 969c1e386..0823f8ec9 100644 --- a/parseutil/dcfd.c +++ b/parseutil/dcfd.c @@ -121,9 +121,9 @@ #define LPRINTF if (interactive && loop_filter_debug) printf #ifdef DEBUG -#define dprintf(_x_) LPRINTF _x_ +#define DPRINTF(_x_) LPRINTF _x_ #else -#define dprintf(_x_) +#define DPRINTF(_x_) #endif #ifdef DECL_ERRNO @@ -595,7 +595,7 @@ cvt_rawdcf( /* * invalid character (no consecutive bit sequence) */ - dprintf(("parse: cvt_rawdcf: character check for 0x%x@%ld FAILED\n", + DPRINTF(("parse: cvt_rawdcf: character check for 0x%x@%ld FAILED\n", (u_int)*s, (long)(s - buffer))); *s = (unsigned char)~0; rtc = CVT_FAIL|CVT_BADFMT; @@ -616,7 +616,7 @@ cvt_rawdcf( cutoff = 4; /* doesn't really matter - it'll fail anyway, but gives error output */ } - dprintf(("parse: cvt_rawdcf: average bit count: %d\n", cutoff)); + DPRINTF(("parse: cvt_rawdcf: average bit count: %d\n", cutoff)); lowmax = 0; /* weighted sum */ highmax = 0; /* bitcount */ @@ -624,14 +624,14 @@ cvt_rawdcf( /* * collect weighted sum of lower bits (left of initial guess) */ - dprintf(("parse: cvt_rawdcf: histogram:")); + DPRINTF(("parse: cvt_rawdcf: histogram:")); for (i = 0; i <= cutoff; i++) { lowmax += histbuf[i] * i; highmax += histbuf[i]; - dprintf((" %d", histbuf[i])); + DPRINTF((" %d", histbuf[i])); } - dprintf((" ")); + DPRINTF((" ")); /* * round up @@ -662,9 +662,9 @@ cvt_rawdcf( { highmax+=histbuf[i] * i; cutoff +=histbuf[i]; - dprintf((" %d", histbuf[i])); + DPRINTF((" %d", histbuf[i])); } - dprintf(("\n")); + DPRINTF(("\n")); /* * determine upper maximum (weighted sum / bit count) @@ -716,7 +716,7 @@ cvt_rawdcf( */ cutoff = (cutoff + span) / 2; - dprintf(("parse: cvt_rawdcf: lower maximum %d, higher maximum %d, cutoff %d\n", lowmax, highmax, cutoff)); + DPRINTF(("parse: cvt_rawdcf: lower maximum %d, higher maximum %d, cutoff %d\n", lowmax, highmax, cutoff)); /* * convert the bit counts to symbolic 1/0 information for data conversion diff --git a/ports/winnt/vs2005/libntp.vcproj b/ports/winnt/vs2005/libntp.vcproj index 312ccdac0..1c87724a3 100644 --- a/ports/winnt/vs2005/libntp.vcproj +++ b/ports/winnt/vs2005/libntp.vcproj @@ -396,6 +396,10 @@ RelativePath="..\..\..\libntp\ntp_calendar.c" > + + @@ -540,6 +544,10 @@ RelativePath="..\..\..\libntp\timevalops.c" > + + @@ -693,6 +701,10 @@ RelativePath="..\..\..\include\ntp_calendar.h" > + + diff --git a/ports/winnt/vs2008/libntp/libntp.vcproj b/ports/winnt/vs2008/libntp/libntp.vcproj index 7a7b478ae..7d1193cd2 100644 --- a/ports/winnt/vs2008/libntp/libntp.vcproj +++ b/ports/winnt/vs2008/libntp/libntp.vcproj @@ -499,6 +499,10 @@ RelativePath="..\..\..\..\libntp\ntp_calendar.c" > + + @@ -659,6 +663,10 @@ RelativePath="..\..\..\..\lib\isc\tsmemcmp.c" > + + @@ -828,6 +836,10 @@ RelativePath="..\..\..\..\include\ntp_calendar.h" > + + diff --git a/ports/winnt/vs2013/libntp/libntp.vcxproj b/ports/winnt/vs2013/libntp/libntp.vcxproj index 3d9f295c7..f2b741d38 100644 --- a/ports/winnt/vs2013/libntp/libntp.vcxproj +++ b/ports/winnt/vs2013/libntp/libntp.vcxproj @@ -248,6 +248,7 @@ + @@ -273,6 +274,7 @@ + @@ -334,6 +336,7 @@ + diff --git a/ports/winnt/vs2013/libntp/libntp.vcxproj.filters b/ports/winnt/vs2013/libntp/libntp.vcxproj.filters index 1d6a7b40f..69de8cd05 100644 --- a/ports/winnt/vs2013/libntp/libntp.vcxproj.filters +++ b/ports/winnt/vs2013/libntp/libntp.vcxproj.filters @@ -182,6 +182,9 @@ Source Files + + Source Files + Source Files @@ -299,6 +302,9 @@ Source Files + + Source Files + Source Files @@ -418,6 +424,9 @@ Header Files + + Header Files + Header Files diff --git a/ports/winnt/vs2015/libntp/libntp.vcxproj b/ports/winnt/vs2015/libntp/libntp.vcxproj index 0a9433f32..8dc81b622 100644 --- a/ports/winnt/vs2015/libntp/libntp.vcxproj +++ b/ports/winnt/vs2015/libntp/libntp.vcxproj @@ -248,6 +248,7 @@ + @@ -273,6 +274,7 @@ + @@ -334,6 +336,7 @@ + diff --git a/ports/winnt/vs2015/libntp/libntp.vcxproj.filters b/ports/winnt/vs2015/libntp/libntp.vcxproj.filters index 1d6a7b40f..69de8cd05 100644 --- a/ports/winnt/vs2015/libntp/libntp.vcxproj.filters +++ b/ports/winnt/vs2015/libntp/libntp.vcxproj.filters @@ -182,6 +182,9 @@ Source Files + + Source Files + Source Files @@ -299,6 +302,9 @@ Source Files + + Source Files + Source Files @@ -418,6 +424,9 @@ Header Files + + Header Files + Header Files diff --git a/tests/libntp/calendar.c b/tests/libntp/calendar.c index b631565c0..5953b62c5 100644 --- a/tests/libntp/calendar.c +++ b/tests/libntp/calendar.c @@ -2,11 +2,15 @@ #include "ntp_stdlib.h" /* test fail without this include, for some reason */ #include "ntp_calendar.h" +#include "ntp_calgps.h" #include "ntp_unixtime.h" +#include "ntp_fp.h" #include "unity.h" #include +static char mbuf[128]; + static int leapdays(int year); void setUp(void); @@ -21,7 +25,9 @@ char * DateFromIsoToString(const struct isodate *iso); int IsEqualDateCal(const struct calendar *expected, const struct calendar *actual); int IsEqualDateIso(const struct isodate *expected, const struct isodate *actual); +void test_Constants(void); void test_DaySplitMerge(void); +void test_WeekSplitMerge(void); void test_SplitYearDays1(void); void test_SplitYearDays2(void); void test_RataDie1(void); @@ -36,7 +42,9 @@ void test_IsoCalYearsToWeeks(void); void test_IsoCalWeeksToYearStart(void); void test_IsoCalWeeksToYearEnd(void); void test_DaySecToDate(void); +void test_GpsRollOver(void); +void test_GpsNtpFixpoints(void); void test_NtpToNtp(void); void test_NtpToTime(void); @@ -230,6 +238,25 @@ static const u_short real_month_days[2][14] = { { 31, 31, 29, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31, 31 } }; +void +test_Constants(void) +{ + int32_t rdn; + struct calendar jdn; + + jdn.year = 1900; + jdn.month = 1; + jdn.monthday = 1; + rdn = ntpcal_date_to_rd(&jdn); + TEST_ASSERT_EQUAL_MESSAGE(DAY_NTP_STARTS, rdn, "(NTP EPOCH)"); + + jdn.year = 1980; + jdn.month = 1; + jdn.monthday = 6; + rdn = ntpcal_date_to_rd(&jdn); + TEST_ASSERT_EQUAL_MESSAGE(DAY_GPS_STARTS, rdn, "(GPS EPOCH)"); +} + /* test the day/sec join & split ops, making sure that 32bit * intermediate results would definitely overflow and the hi DWORD of * the 'vint64' is definitely needed. @@ -241,10 +268,10 @@ test_DaySplitMerge(void) for (day = -1000000; day <= 1000000; day += 100) { for (sec = -100000; sec <= 186400; sec += 10000) { - vint64 merge; - ntpcal_split split; - int32 eday; - int32 esec; + vint64 merge; + ntpcal_split split; + int32 eday; + int32 esec; merge = ntpcal_dayjoin(day, sec); split = ntpcal_daysplit(&merge); @@ -268,6 +295,40 @@ test_DaySplitMerge(void) return; } +void +test_WeekSplitMerge(void) +{ + int32 wno,sec; + + for (wno = -1000000; wno <= 1000000; wno += 100) { + for (sec = -100000; sec <= 2*SECSPERWEEK; sec += 10000) { + vint64 merge; + ntpcal_split split; + int32 ewno; + int32 esec; + + merge = ntpcal_weekjoin(wno, sec); + split = ntpcal_weeksplit(&merge); + ewno = wno; + esec = sec; + + while (esec >= SECSPERWEEK) { + ewno += 1; + esec -= SECSPERWEEK; + } + while (esec < 0) { + ewno -= 1; + esec += SECSPERWEEK; + } + + TEST_ASSERT_EQUAL(ewno, split.hi); + TEST_ASSERT_EQUAL(esec, split.lo); + } + } + + return; +} + void test_SplitYearDays1(void) { @@ -735,3 +796,107 @@ test_NtpToTime(void) } # endif } + +/* -------------------------------------------------------------------- + * GPS rollover + * -------------------------------------------------------------------- + */ +void +test_GpsRollOver(void) +{ + /* we test on wednesday, noon, and on the border */ + static const int32_t wsec1 = 3*SECSPERDAY + SECSPERDAY/2; + static const int32_t wsec2 = 7 * SECSPERDAY - 1; + static const int32_t week0 = GPSNTP_WSHIFT + 2047; + static const int32_t week1 = GPSNTP_WSHIFT + 2048; + TCivilDate jd; + TGpsDatum gps; + l_fp fpz; + + ZERO(fpz); + + /* test on 2nd rollover, April 2019 + * we set the base date properly one week *before the rollover, to + * check if the expansion merrily hops over the warp. + */ + basedate_set_day(2047 * 7 + NTP_TO_GPS_DAYS); + + jd.year = 19; + jd.month = 4; + jd.monthday = 3; + jd.hour = 12; + jd.minute = 0; + jd.second = 0; + + gps = gpscal_from_calendar(&jd, fpz); + TEST_ASSERT_EQUAL_MESSAGE(week0, gps.weeks, "(week test 1))"); + TEST_ASSERT_EQUAL_MESSAGE(wsec1, gps.wsecs, "(secs test 1)"); + + jd.year = 19; + jd.month = 4; + jd.monthday = 6; + jd.hour = 23; + jd.minute = 59; + jd.second = 59; + + gps = gpscal_from_calendar(&jd, fpz); + TEST_ASSERT_EQUAL_MESSAGE(week0, gps.weeks, "(week test 2)"); + TEST_ASSERT_EQUAL_MESSAGE(wsec2, gps.wsecs, "(secs test 2)"); + + jd.year = 19; + jd.month = 4; + jd.monthday = 7; + jd.hour = 0; + jd.minute = 0; + jd.second = 0; + + gps = gpscal_from_calendar(&jd, fpz); + TEST_ASSERT_EQUAL_MESSAGE(week1, gps.weeks, "(week test 3)"); + TEST_ASSERT_EQUAL_MESSAGE( 0 , gps.wsecs, "(secs test 3)"); + + jd.year = 19; + jd.month = 4; + jd.monthday = 10; + jd.hour = 12; + jd.minute = 0; + jd.second = 0; + + gps = gpscal_from_calendar(&jd, fpz); + TEST_ASSERT_EQUAL_MESSAGE(week1, gps.weeks, "(week test 4)"); + TEST_ASSERT_EQUAL_MESSAGE(wsec1, gps.wsecs, "(secs test 4)"); +} + +void +test_GpsNtpFixpoints(void) +{ + basedate_set_day(NTP_TO_GPS_DAYS); + TGpsDatum e1gps; + TNtpDatum e1ntp, r1ntp; + l_fp lfpe , lfpr; + + lfpe.l_ui = 0; + lfpe.l_uf = UINT32_C(0x80000000); + + ZERO(e1gps); + e1gps.weeks = 0; + e1gps.wsecs = SECSPERDAY; + e1gps.frac = UINT32_C(0x80000000); + + ZERO(e1ntp); + e1ntp.frac = UINT32_C(0x80000000); + + r1ntp = gpsntp_from_gpscal(&e1gps); + TEST_ASSERT_EQUAL_MESSAGE(e1ntp.days, r1ntp.days, "gps -> ntp / days"); + TEST_ASSERT_EQUAL_MESSAGE(e1ntp.secs, r1ntp.secs, "gps -> ntp / secs"); + TEST_ASSERT_EQUAL_MESSAGE(e1ntp.frac, r1ntp.frac, "gps -> ntp / frac"); + + lfpr = ntpfp_from_gpsdatum(&e1gps); + snprintf(mbuf, sizeof(mbuf), "gps -> l_fp: %s <=> %s", + lfptoa(&lfpe, 9), lfptoa(&lfpr, 9)); + TEST_ASSERT_TRUE_MESSAGE(L_ISEQU(&lfpe, &lfpr), mbuf); + + lfpr = ntpfp_from_ntpdatum(&e1ntp); + snprintf(mbuf, sizeof(mbuf), "ntp -> l_fp: %s <=> %s", + lfptoa(&lfpe, 9), lfptoa(&lfpr, 9)); + TEST_ASSERT_TRUE_MESSAGE(L_ISEQU(&lfpe, &lfpr), mbuf); +} diff --git a/tests/libntp/run-calendar.c b/tests/libntp/run-calendar.c index 393b36898..6988d1c0d 100644 --- a/tests/libntp/run-calendar.c +++ b/tests/libntp/run-calendar.c @@ -25,13 +25,17 @@ #include "config.h" #include "ntp_stdlib.h" #include "ntp_calendar.h" +#include "ntp_calgps.h" #include "ntp_unixtime.h" +#include "ntp_fp.h" #include //=======External Functions This Runner Calls===== extern void setUp(void); extern void tearDown(void); +extern void test_Constants(void); extern void test_DaySplitMerge(void); +extern void test_WeekSplitMerge(void); extern void test_SplitYearDays1(void); extern void test_SplitYearDays2(void); extern void test_RataDie1(void); @@ -46,6 +50,8 @@ extern void test_IsoCalYearsToWeeks(void); extern void test_IsoCalWeeksToYearStart(void); extern void test_IsoCalWeeksToYearEnd(void); extern void test_DaySecToDate(void); +extern void test_GpsRollOver(void); +extern void test_GpsNtpFixpoints(void); extern void test_NtpToNtp(void); extern void test_NtpToTime(void); @@ -74,23 +80,27 @@ int main(int argc, char *argv[]) progname = argv[0]; suite_setup(); UnityBegin("calendar.c"); - RUN_TEST(test_DaySplitMerge, 24); - RUN_TEST(test_SplitYearDays1, 25); - RUN_TEST(test_SplitYearDays2, 26); - RUN_TEST(test_RataDie1, 27); - RUN_TEST(test_LeapYears1, 28); - RUN_TEST(test_LeapYears2, 29); - RUN_TEST(test_RoundTripDate, 30); - RUN_TEST(test_RoundTripYearStart, 31); - RUN_TEST(test_RoundTripMonthStart, 32); - RUN_TEST(test_RoundTripWeekStart, 33); - RUN_TEST(test_RoundTripDayStart, 34); - RUN_TEST(test_IsoCalYearsToWeeks, 35); - RUN_TEST(test_IsoCalWeeksToYearStart, 36); - RUN_TEST(test_IsoCalWeeksToYearEnd, 37); - RUN_TEST(test_DaySecToDate, 38); - RUN_TEST(test_NtpToNtp, 40); - RUN_TEST(test_NtpToTime, 41); + RUN_TEST(test_Constants, 28); + RUN_TEST(test_DaySplitMerge, 29); + RUN_TEST(test_WeekSplitMerge, 30); + RUN_TEST(test_SplitYearDays1, 31); + RUN_TEST(test_SplitYearDays2, 32); + RUN_TEST(test_RataDie1, 33); + RUN_TEST(test_LeapYears1, 34); + RUN_TEST(test_LeapYears2, 35); + RUN_TEST(test_RoundTripDate, 36); + RUN_TEST(test_RoundTripYearStart, 37); + RUN_TEST(test_RoundTripMonthStart, 38); + RUN_TEST(test_RoundTripWeekStart, 39); + RUN_TEST(test_RoundTripDayStart, 40); + RUN_TEST(test_IsoCalYearsToWeeks, 41); + RUN_TEST(test_IsoCalWeeksToYearStart, 42); + RUN_TEST(test_IsoCalWeeksToYearEnd, 43); + RUN_TEST(test_DaySecToDate, 44); + RUN_TEST(test_GpsRollOver, 45); + RUN_TEST(test_GpsNtpFixpoints, 47); + RUN_TEST(test_NtpToNtp, 48); + RUN_TEST(test_NtpToTime, 49); return (UnityEnd()); }