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