contrib/ntp/libntp/ntp_calgps.c

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