/* * Copyright (c) 2000 - 2002 Ian Dowse. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * SUCH DAMAGE. * */ #include #define _KERNEL #include #undef _KERNEL #include #include #include #include #include #include #include #include #include #include #ifndef DEV #define DEV "/dev/cuaa0" #endif int verbose = 0; /* 0 = nothing, 1 = 60s events, 2 = 1s events, 3 = all */ /* * Structure for communicating with ntpd via shm. */ struct shmTime { int mode; /* 0 - if valid set * use values, * clear valid * 1 - if valid set * if count before and after read of values is equal, * use values * clear valid */ int count; time_t clockTimeStampSec; int clockTimeStampUSec; time_t receiveTimeStampSec; int receiveTimeStampUSec; int leap; int precision; int nsamples; int valid; int dummy[10]; }; /* * For counting means of measurements. */ struct mean_set { double first_sample; int n_samples; double diff_sum; }; /* * Represents the process of decoding a MSF signal from assert/clear times */ struct decoder_state { /* bit decoding info */ pps_seq_t s_assert, s_clear; int state; int bit_ok; double t_prev; /* timing info from bits */ struct mean_set means; /* bits decoded over the last minute or so */ int A[64]; int B[64]; int AB_off; int pos, count; /* name for error messages */ char *name; }; /* * Full decoder. * Gets measturement from pps and trys both normal and inverted decoding. */ struct pps_msf_decoder { pps_handle_t pps; struct decoder_state normal, inverted; }; #define DTOTS(d, ts) do { \ (ts)->tv_sec = (int)(d); \ (ts)->tv_nsec = ((d) - (double)(ts)->tv_sec) * 1e9; \ } while (0) #define TSTOD(ts) ((double)(ts)->tv_sec + (double)(ts)->tv_nsec / 1e9) #define T_MAXERROR 0.04 #define T_MIN(t) ((t) - T_MAXERROR) #define T_MAX(t) ((t) + T_MAXERROR) #define T_MATCH(t, t1) ((t) > T_MIN(t1) && (t) < T_MAX(t1)) #define STATE_NONE -1 /* Second has just begun */ #define STATE_A0BX 0 /* Bit A is 0, bit B is undecided */ #define STATE_A0B0 1 /* Bit A is 0, bit B is 0 */ #define STATE_A1B0 2 /* Bit A is 1, bit B is 1 */ #define STATE_A0B1 3 /* Bit A is 0, bit B is 1 */ #define STATE_A1B1 4 /* Bit A is 1, bit B is 1 */ #define STATE_MINUTE 5 /* 500ms gap, so at start of minute */ void mean_reset(struct mean_set *ms) { ms->n_samples = 0; ms->diff_sum = 0.0; ms->first_sample = -1.0; } void mean_sample(struct mean_set *ms, double d) { double diff, ndiff, mdiff; int secs; if (verbose >= 3) printf("mean_sample: %.09f\n", d); if (ms->n_samples == 0) { ms->first_sample = d; ms->n_samples = 1; ms->diff_sum = 0.0; return; } diff = d - ms->first_sample; secs = (int)(diff + 0.5); ndiff = diff - (double)secs; mdiff = ndiff + (ms->diff_sum / (double)ms->n_samples); if (secs > 70 || !T_MATCH(mdiff, 0.0)) { if (verbose >= 1) { printf("mean_sample: REJECT secs %d diff %.09f\n", secs, mdiff); } ms->first_sample = d; ms->n_samples = 1; ms->diff_sum = 0.0; return; } ms->diff_sum += ndiff; ms->n_samples++; } void mean_adj(struct mean_set *ms,double *dp) { double d, d1; double diff, ndiff; int secs; if (ms->n_samples == 0) return; d = *dp; diff = d - ms->first_sample; secs = (int)(diff + 0.5); ndiff = diff - (double)secs; if (!T_MATCH(ndiff, 0.0) || secs > 70) { if (verbose >= 1) { printf("mean_adj: REJECT secs %d diff %.09f\n", secs, ndiff); } mean_reset(ms); return; } d1 = ms->first_sample + (ms->diff_sum / (double)ms->n_samples) + (double)secs; if (verbose >= 1) { printf("mean_adj: adjusting by %f to %f\n", ms->diff_sum / (double)ms->n_samples, d); } *dp = d; mean_reset(ms); } const char * statename(int state) { switch (state) { case STATE_NONE: return "NONE"; case STATE_A0BX: return "A0 BX"; case STATE_A0B0: return "A0 B0"; case STATE_A1B0: return "A1 B0"; case STATE_A0B1: return "A0 B1"; case STATE_A1B1: return "A1 B1"; case STATE_MINUTE: return "MINUTE"; } return "Unknown state"; } int secondcode(struct decoder_state *ds, double *t_extp) { static const int Pstart[4] = {17, 25, 36, 39}; static const int Pend[4] = {24, 35, 38, 51}; static const int BCD[8] = {1, 2, 4, 8, 10, 20, 40, 80}; int code = ds->state; #define Abit(n) (ds->A[(ds->AB_off - 60 + (n) + 64) & 63]) #define Bbit(n) (ds->B[(ds->AB_off - 60 + (n) + 64) & 63]) switch (code) { case STATE_NONE: ds->count = 0; return 0; case STATE_MINUTE: if (verbose >= 1) printf("MINUTE: pos %d count %d\n", ds->pos, ds->count); ds->AB_off = ds->pos; if (ds->count >= 43) { struct tm tm; int i, p; int parity; time_t unixtime; if (verbose >= 1) { printf("%s A: ", ds->name); for (i = 1; i < 60; i++) printf("%d", Abit(i)); printf("\n%s B: ", ds->name); for (i = 1; i < 60; i++) printf("%d", Bbit(i)); printf("\n"); } for (p = 0; p < 4; p++) { parity = Bbit(54 + p); for (i = Pstart[p]; i <= Pend[p]; i++) parity += Abit(i); if ((parity & 1) == 0) { if (verbose >= 1) { printf("%s Parity %d failed \n", ds->name, p); } ds->count = 0; return 0; } } tm.tm_sec = 0; tm.tm_min = 0; for (i = 0; i < 7; i++) tm.tm_min += BCD[i] * Abit(51 - i); tm.tm_hour = 0; for (i = 0; i < 6; i++) tm.tm_hour += BCD[i] * Abit(44 - i); tm.tm_mday = 0; for (i = 0; i < 6; i++) tm.tm_mday += BCD[i] * Abit(35 - i); tm.tm_mon = -1; for (i = 0; i < 5; i++) tm.tm_mon += BCD[i] * Abit(29 - i); tm.tm_year = 0; for (i = 0; i < 8; i++) tm.tm_year += BCD[i] * Abit(24 - i); if (tm.tm_year < 99) tm.tm_year += 100; tm.tm_wday = 0; for (i = 0; i < 3; i++) tm.tm_wday += BCD[i] * Abit(38 - i); tm.tm_yday = -1; tm.tm_isdst = 0; tm.tm_zone = NULL; tm.tm_gmtoff = 0; unixtime = timegm(&tm) + 1; if (Bbit(58)) unixtime -= 3600; if (verbose >= 1) { printf("%s Ctime: %s\n", ds->name, ctime(&unixtime)); } *t_extp = (double)unixtime; ds->count = 0; return 1; } ds->count = 0; return 0; case STATE_A0BX: /* Shouldn't happen! */ abort(); default: ds->A[ds->pos] = (code == STATE_A0B0 || code == STATE_A0B1) ? 0 : 1; ds->B[ds->pos] = (code == STATE_A0B0 || code == STATE_A1B0) ? 0 : 1; ds->pos = (ds->pos + 1) & 63; ds->count++; return 0; } return 0; } /* * We have just got an assert after a period of time t. * Move into the correct state and say at what offset within the second we * expected the assert. Return true if things seem to match up. */ int assertgap(double t, int *state, double *t_off) { switch (*state) { case STATE_NONE: if (T_MATCH(t, 0.100)) { *t_off = 0.100; *state = STATE_A0BX; } else if (T_MATCH(t, 0.200)) { *t_off = 0.200; *state = STATE_A1B0; } else if (T_MATCH(t, 0.300)) { *t_off = 0.300; *state = STATE_A1B1; } else if (T_MATCH(t, 0.500)) { *t_off = 0.500; *state = STATE_MINUTE; } else return 0; return 1; case STATE_A0B1: if (T_MATCH(t, 0.100)) { *t_off = 0.300; } else return 0; return 1; } return 0; } /* * We have just got a clear after a period of time t. * Most of the time we just have to check that the t was of the right * length, but we may see a A0B0 turinig into a A0B1. */ int cleargap(double t, int *state, double *t_off) { *t_off = 0.0; switch (*state) { case STATE_NONE: return 0; case STATE_A0BX: if (T_MATCH(t, 0.900)) { *state = STATE_A0B0; return 1; } if (T_MATCH(t, 0.100)) { *state = STATE_A0B1; *t_off = 0.200; return 1; } return 0; case STATE_A1B0: return T_MATCH(t, 0.800); case STATE_A0B1: case STATE_A1B1: return T_MATCH(t, 0.700); case STATE_MINUTE: return T_MATCH(t, 0.500); } return 0; } int do_decode(struct decoder_state *ds, double t_assert, double t_clear, pps_seq_t s_assert, pps_seq_t s_clear, double *t_extp, double *t_intp) { int retval = 0; double t, t_off; struct mean_set *ms = &ds->means; if (t_assert < t_clear && s_assert > ds->s_assert) { ds->s_assert = s_assert; t = t_assert - ds->t_prev; ds->t_prev = t_assert; if (verbose >= 3) printf("Assert gap %s: %f\n", ds->name, t); ds->bit_ok &= assertgap(t, &ds->state, &t_off); if (ds->bit_ok) mean_sample(ms, t_assert - t_off); } if (s_clear > ds->s_clear) { ds->s_clear = s_clear; t = t_clear - ds->t_prev; ds->t_prev = t_clear; if (verbose >= 3) printf("Clear gap %s: %f\n", ds->name, t); ds->bit_ok &= cleargap(t, &ds->state, &t_off); if (ds->bit_ok) mean_sample(ms, t_clear - t_off); if (t > T_MIN(0.500)) { if (verbose >= 2) { printf("Minute marker time %s: %f\n", ds->name, t_clear); } if (ds->state == STATE_MINUTE) { *t_intp = t_clear; mean_adj(ms, t_intp); } if (ds->bit_ok && secondcode(ds, t_extp)) retval = 1; ds->state = STATE_NONE; ds->bit_ok = 1; } } if (t_assert > t_clear && s_assert > ds->s_assert) { ds->s_assert = s_assert; t = t_assert - ds->t_prev; ds->t_prev = t_assert; if (verbose >= 3) printf("Assert gap %s: %f\n", ds->name, t); ds->bit_ok &= assertgap(t, &ds->state, &t_off); if (ds->bit_ok) mean_sample(ms, t_assert - t_off); } return retval; } int getgap(struct pps_msf_decoder *pmd, double *t_extp, double *t_intp) { pps_info_t info; double t_assert, t_clear; pps_seq_t s_assert, s_clear; int retval = 0; struct timespec ts; ts.tv_sec = ts.tv_nsec = 0; if (time_pps_fetch(pmd->pps, PPS_TSFMT_TSPEC, &info, &ts) < 0) err(1, "time_pps_getparams"); t_assert = TSTOD(&info.assert_timestamp); t_clear = TSTOD(&info.clear_timestamp); s_assert = info.assert_sequence; s_clear = info.clear_sequence; if (do_decode(&pmd->normal, t_assert, t_clear, s_assert, s_clear, t_extp, t_intp)) { retval = 1; } if (do_decode(&pmd->inverted, t_clear, t_assert, s_clear, s_assert, t_extp, t_intp)) { if (retval && verbose >= 1) printf("Good grief - they both decoded!\n"); retval = 1; } return retval; } void ntp_shmset(struct timespec *ts_ext, struct timespec *ts_int, int unit) { static volatile struct shmTime *p; int shmid = 0; if (p == NULL) { shmid = shmget(0x4e545030 + unit, sizeof(struct shmTime), 0); if (shmid == -1) { warn("shmget 0x%08x", 0x4e545030 + unit); return; } p = (struct shmTime *)shmat(shmid, 0, 0); if (p == (struct shmTime *)(-1)) { warn("shmat unit %d", unit); return; } } if (verbose >= 1) printf("ntp_shmset: sending sample\n"); p->mode = 1; p->clockTimeStampSec = ts_ext->tv_sec; p->clockTimeStampUSec = ts_ext->tv_nsec / 1000; p->receiveTimeStampSec = ts_int->tv_sec; p->receiveTimeStampUSec = ts_int->tv_nsec / 1000; p->leap = 0; p->precision = -8; p->count++; p->valid = 1; } void decoder_init(struct decoder_state *ds) { bzero(ds, sizeof(*ds)); ds->state = STATE_NONE; mean_reset(&ds->means); } void usage(void) { fprintf(stderr, "usage: pps_shm [-v] [-d device] [-o offset] [-u unit]\n"); exit(1); } int main(int argc, char **argv) { char *device = DEV; /* Device to open and do PPS stuff on */ double offset = 0.0; /* How much to offset signal by in seconds */ int unit = 0; /* What unit of the ntp shm driver to talk to */ struct pps_msf_decoder pmd; /* measurement and decoding struct */ pps_params_t params; int fd, ch; double t_int, t_ext; while ((ch = getopt(argc, argv, "d:o:u:v")) != -1 ) switch (ch) { case 'd': device = optarg; break; case 'o': if (sscanf(optarg, "%lf", &offset) != 1) usage(); break; case 'u': if (sscanf(optarg, "%d", &unit) != 1) usage(); break; case 'v': verbose++; break; default: usage(); } if ((fd = open(device, O_RDWR)) < 0) err(1, device); if (time_pps_create(fd, &pmd.pps) < 0) err(1, "time_pps_create"); bzero(¶ms, sizeof(params)); params.mode = PPS_CAPTUREBOTH; if (time_pps_setparams(pmd.pps, ¶ms) < 0) err(1, "time_pps_setparams"); decoder_init(&pmd.normal); pmd.normal.name = "uninverted"; decoder_init(&pmd.inverted); pmd.inverted.name = " inverted"; for (;;) { usleep(50000); if (getgap(&pmd, &t_ext, &t_int)) { struct timespec ts_int, ts_ext; t_ext += offset; DTOTS(t_int, &ts_int); DTOTS(t_ext, &ts_ext); if (verbose >= 1) printf("Offset: %f\n", t_ext - t_int); ntp_shmset(&ts_ext, &ts_int, unit); } } return 0; }