/*      $NetBSD: ntp_calgps.c,v 1.2 2020/05/25 20:47:24 christos Exp $  */

/*
* ntp_calgps.c - calendar for GPS/GNSS based clocks
*
* Written by Juergen Perlinger ([email protected]) for the NTP project.
* The contents of 'html/copyright.html' apply.
*
* --------------------------------------------------------------------
*
* This module implements stuff often used with GPS/GNSS receivers
*/

#include <config.h>
#include <sys/types.h>

#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,
       int             warp
       )
{
       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 warp ? _gpsntp_fix_gps_era(&retv) : 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_ex(
       TcCivilDate *   jd,
       l_fp            fofs,
       TcNtpDatum *    pivot,
       int/*BOOL*/     warp
       )
{
       TNtpDatum       dpiv = *pivot;
       _norm_ntp_datum(&dpiv);
       return _gpsntp_from_daytime(jd, fofs, &dpiv, warp);
}

/* -----------------------------------------------------------------
* 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_ex(
       TcCivilDate *   jd,
       l_fp            fofs,
       l_fp            pivot,
       int/*BOOL*/     warp
       )
{
       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, warp);
}

/* -----------------------------------------------------------------
* 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_ex(
       TcCivilDate *   jd,
       l_fp            fofs,
       int/*BOOL*/     warp
       )
{
       TGpsDatum       gps;
       gps = gpscal_from_calendar_ex(jd, fofs, warp);
       return gpsntp_from_gpscal_ex(&gps, FALSE);
}

/* -----------------------------------------------------------------
* create a civil calendar datum from a NTP date representation
*/
void
gpsntp_to_calendar(
       TCivilDate * cd,
       TcNtpDatum * nd
       )
{
       memset(cd, 0, sizeof(*cd));
       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_ex(
       TcGpsDatum *    gd,
       int/*BOOL*/     warp
       )
{
       TNtpDatum       retv;
       vint64          ts64;
       ntpcal_split    split;
       TGpsDatum       date = *gd;

       if (warp) {
               uint32_t base = basedate_get_gpsweek() + GPSNTP_WSHIFT;
               _norm_gps_datum(&date);
               date.weeks = ((date.weeks - base) & 1023u) + base;
       }

       ts64  = ntpcal_weekjoin(date.weeks, date.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,
* if so requested.
*
* 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_ex(
       TcCivilDate *   jd,
       l_fp            fofs,
       int/*BOOL*/     warp
       )
{
       /*  (-DAY_GPS_STARTS) (mod 7*1024) -- complement of cycle shift */
       static const uint32_t s_compl_shift =
           (7 * 1024) - DAY_GPS_STARTS % (7 * 1024);

       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'. */
       }

       /* Rebase to days after the GPS epoch. 'days' is positive here,
        * but it might be less than the GPS epoch start. Depending on
        * the input, we have to do different things to get the desired
        * result. (Since we want to remap the era anyway, we only have
        * to retain congruential identities....)
        */

       if (days >= DAY_GPS_STARTS) {
               /* simply shift to days since GPS epoch */
               days -= DAY_GPS_STARTS;
       } else if (jd->year < 100) {
               /* Two-digit year on input: add another century and
                * retry.  This can happen only if the century expansion
                * yielded a date between 1980-01-01 and 1980-01-05,
                * both inclusive. We have at most one retry here.
                */
               cal.year += 100;
               goto again;
       } else {
               /* A very bad date before the GPS epoch. There's not
                * much we can do, except to add the complement of
                * DAY_GPS_STARTS % (7 * 1024) here, that is, use a
                * congruential identity: Add the complement instead of
                * subtracting the value gives a value with the same
                * modulus. But of course, now we MUST to go through a
                * cycle fix... because the date was obviously wrong!
                */
               warp  = TRUE;
               days += s_compl_shift;
       }

       /* Splitting to weeks is simple now: */
       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 warp ? _gpscal_fix_gps_era(&gps) : gps;
}

/* -----------------------------------------------------------------
* get civil date from week/tow representation
*/
void
gpscal_to_calendar(
       TCivilDate * cd,
       TcGpsDatum * wd
       )
{
       TNtpDatum nd;

       memset(cd, 0, sizeof(*cd));
       nd = gpsntp_from_gpscal_ex(wd, FALSE);
       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 horse 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-*- */

You are viewing proxied material from ftp.icm.edu.pl. The copyright of proxied material belongs to its original authors. Any comments or complaints in relation to proxied material should be directed to the original authors of the content concerned. Please see the disclaimer for more details.