// *************************************************************************** // *************************************************************************** // *************************************************************************** // *** *** // *** Copyright 2000-3 by Jack Bates *** // *** TractorBeam -at- floatingdoghead -dot- net *** // *** *** // *** LEGAL DISCLAIMER AND RIGHTS POSTING: *** // *** *** // *** THIS IS A FREE TOOL OF HACK BY JACK. *** // *** *** // *** This program is free software; you can redistribute it and/or *** // *** modify it under the terms of the GNU General Public License as *** // *** published by the Free Software Foundation; either version 2 of the *** // *** License, or (at your option) any later version. *** // *** *** // *** This program is distributed in the hope that it will be useful, but *** // *** but WITHOUT ANY WARRANTY; without even the implied warranty of *** // *** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU *** // *** General Public License for more details. *** // *** *** // *** You should have received a copy of the GNU General Public License *** // *** along with this program; if not, write to: *** // *** *** // *** Free Software Foundation, Inc. *** // *** 59 Temple Place - Suite 330 *** // *** Boston, MA 02111-1307, USA. *** // *** *** // *** THE FULL GPL LICENSE TEXT IS ALSO AVAILABLE AT http://www.fsf.org *** // *** *** // *** IN ADDITION: *** // *** *** // *** IF YOU FEEL THE NEED TO INCORPORATE THIS CODE (OR DERIVATIVE) INTO *** // *** ANY PRODUCT THAT DOES NOT USE THE GPL AS ITS COPYRIGHT ENFORCEMENT *** // *** MECHANISM, YOU ARE OBLIGATED TO ARRANGE LICENSING WITH ME. *** // *** *** // *** FURTHERMORE: *** // *** *** // *** I ACCEPT NO LIABILITY WHATSOEVER IN REGARDS TO YOUR USE OF THIS *** // *** SOURCE CODE. MY RELEASE OF THIS FILE IS A FREE PUBLIC SERVICE. I *** // *** DIDN'T WRITE THIS TO GET SUED, IT IS INTENDED FOR EDUCATIONAL USE *** // *** IN THE STUDY OF THE COMPUTING SCIENCES AND HOW THE INTERNET WORKS. *** // *** I DO NOT PROVIDE ANY FORM OF FREE SUPPORT FOR THIS SOURCE CODE. *** // *** *** // *************************************************************************** // *************************************************************************** // *************************************************************************** // #define VERSION "0.64-SIT" // // Date: 2003/01/28 // // Comment: FORCED kernel timestamp reception - will not work without // the SERIAL_INTR_TIMING kernel patch by Jack Bates (me). // // Rockwell GPS binary protocol message type 1000 is parsed. // // Tested under: Linux 2.4.20 kernel w/ SERIAL_INTR_TIMING serial.c module // HP Vectra 5/133 // 16550 UART // Tested while running as a packet filter and 802.11AP // // This configuration keeps time to within approximately 2ms // // Compilation: Standard compile: gcc -Wall -o TractorBeam TractorBeam.c // // Define the following macros to produce interesting effects: // // #define JAB_SAMPLE // print info on every aggregated sample // // LOTS of output to stdout // #define JAB_POINT // print info on every summarized point // #define JAB_MSG // print info on every type 1000 msg // // TONS of output to stdout // #define JAB_DEBUG // other interesting information // #define JAB_NO_OUT // suppress LOCKED and ADJUST messages // // essentially silencing the program // // with the exception of hard error // // *************************************************************************** // *************************************************************************** // *************************************************************************** // // Details of this program: // // You can connect a Delorme EarthMate GPS unit to a serial port of your // Linux machine and this program, in conjunction with a kernel patch, // will sync your kernel clock to within a couple of mS of GPS time. // // The Delorme EarthMate is an inexpensive GPS unit without a display. It // is intended to be connected directly to a computer via a 9-pin RS-232 // port. I have mounted mine on top of my house in a weatherproof // enclosure with a clear view of the sky. It very rarely loses lock. // The times reported from the unit are in units of nanoseconds. Even // with a kernel ISR that records interrupt timestamps, variability in // the observed delta is obvious. TractorBeam algorithm attempts to // lend a hysteresis effect to the adjustments of the clock over simple // slope calculation methods. // // In my opinion the EarthMate is a quality instrument, considering its // low cost. These things can be had on eBay for dirt cheap. Mine has // survived outdoors for three years. // // ntpd has a reference clock for the Rockwell Jupiter GPS chip. The // Delorme EarthMate is based upon the same chip, but is brain-dead, in // that it seemingly cannot be compelled operate in the various // interesting modes supported by the Jupiter (i.e. the GPS time-sync // Pulse Per Second mode). It seems to ignore everything sent to it other // than the initialization response of "EARTHA" (without the // quotes), after which it spews Rockwell binary data of just a few types. // // The type 1000 message contains time, gps position and velocity // readings and is sent at the rate appx. 1 per 1.000010 seconds. // Measurements in regards to the receipt of this type of message are what // drives the clock adjustment algorightms implemented here. // // The object was to keep the code readable by even beginner C programmers // while providing lock within a couple of mS. For all intents and // purposes, I consider this synchronized. // // NOTE: The baud rate that the EarthMate uses is 9600, mandating the // use of a 16550 UART, at least on slower processors. I have not // experimented with 16450 UARTs on faster CPUs. // // NOTE: The Earthmate may be successfully powered by the regulated // +5v supply that is on virtually every power connector in a PC // computer. I fashioned a cable that runs serial and power on one // 6-conductor piece of phone cable. The ground and +5v were tapped // from a disk drive power connector and connected to the battery // contacts of the receiver. // // *************************************************************************** // *************************************************************************** // *************************************************************************** //================================================================================================================================= // // COLUMNS 132 TABSTOP 4 // //================================================================================================================================= //------------------------------=-----------------------=-------------------------------------------------------------------------- // #include #include #include #include #include #include #include #include #include #include #include #include #include #include //------------------------------=-----------------------=-------------------------------------------------------------------------- // missing definitions for RedHat-8.0 glibc - also observed in various other glibc implementations extern int64_t llabs(int64_t); //------------------------------=-----------------------=-------------------------------------------------------------------------- // for debugging and/or analysis of output, uncomment the following line #define JAB_DEBUG #ifdef JAB_DEBUG #undef JAB_NO_OUT #define JAB_SAMPLE #define JAB_POINT #define JAB_MSG #endif //------------------------------=-----------------------=-------------------------------------------------------------------------- // manifest constants #define LL_ONE_MILLION (1000000LL) #define LL_SECS_PER_HOUR (3600LL) #define D_SECS_PER_HOUR ((double) (3600.0)) #define D_ONE_MILLION (1000000.0) // maximum uS of delta to allow without "hard setting" the system clock by that delta upon collection initial delta. #define MAX_KERPLUNK (LL_ONE_MILLION / 2LL) // minimum and maximum lengths of a sample. #define MIN_SAMPLE (5) #define MAX_SAMPLE (5) // 2003-01-28 go to uniform sample size // constraints upon statistical analysis #define MIN_POINT (2) #define MAX_POINT (128) #define MAX_TRAIL (128) //------------------------------=-----------------------=-------------------------------------------------------------------------- // how the trail is stored typedef struct { int64_t time_observed; int64_t delta; int32_t weight; } history_t; //------------------------------=-----------------------=-------------------------------------------------------------------------- // static int open_serial(char *filename) { struct termios ti; int fd; int flags = 0; // open w/ O_NDELAY to avoid hardware flow control blocking the open // sounds weird, but it can happen... (but not with the EarthMate) fd = open(filename, O_RDWR | O_NDELAY); if (fd < 0) { fprintf(stderr, "%s: open errno %d", filename, errno); exit(1); } // get flags if ((flags = fcntl(fd, F_GETFL, 0)) == -1) { fprintf(stderr, "%s: get flags errno %d", filename, errno); close(fd); exit(1); } // return us to the state of blocking being allowed flags &= ~O_NDELAY; // set flags if (fcntl(fd, F_SETFL, flags) == -1) { fprintf(stderr, "%s: set flags errno %d", filename, errno); close(fd); exit(1); } // get tty params if (ioctl(fd, TCGETS, &ti)) { fprintf(stderr, "%d: TCGETS errno %d", fd, errno); close(fd); exit(1); } // MAGIC NUMBER that the kernel mod understands ti.c_iflag = 020000000000; ti.c_oflag = 0; ti.c_lflag = 0; // 9600/N/8/1 ti.c_cflag &= (~CBAUD); ti.c_cflag |= B9600; ti.c_cflag &= (~CSIZE); ti.c_cflag |= CS8; ti.c_cflag &= ~PARENB; ti.c_cflag &= ~PARODD; // HUP closes ti.c_cflag |= HUPCL; // min 1 char, block until you get it ti.c_cc[VMIN] = 1; ti.c_cc[VTIME] = 0; // set tty params if (ioctl(fd, TCSETS, &ti)) { fprintf(stderr, "%d: TCSETS errno %d", fd, errno); close(fd); exit(1); } return fd; } //------------------------------=-----------------------=-------------------------------------------------------------------------- // static int read_chars(int fd, uint8_t *buf, int n) { int rc; int32_t count; count = 0; while (count < n) { // this should be the only RAW read() in the program... rc = read(fd, buf + count, n - count); if (rc == -1) return -1; count += rc; } if (count != n) { fprintf(stderr, "read_chars: n %d count %d\n", n, count); exit(1); } return n; } //------------------------------=-----------------------=-------------------------------------------------------------------------- // read until a 0xFF is received and then read the timestamp // this is dependent upon the Linux kernel module supplied in this distribution being installed static int read_until_FF_stamp(int fd, struct timeval *tv) { uint8_t c; while (1) { if (read_chars(fd, &c, 1) == -1) return -1; //fprintf(stdout, "%c(%02X) ", c, c); //fprintf(stdout, " %02X ", c); //fflush(stdout); if (c == 0xFF) break; } //fprintf(stdout, "\ngetting stamp\n"); // this gets the timestamp that the kernel mod provides when it sees the 0xFF // see the ioctl() in open_serial() read_chars(fd, (char *) tv, sizeof(struct timeval)); //fprintf(stdout, "\nTIME: %d.%06d\n", tv->tv_sec, tv->tv_usec); //fflush(stdout); return 0; } //------------------------------=-----------------------=-------------------------------------------------------------------------- // #define FIXED_BUF_SIZE (1024) static int read_bytes_discarding_stamp(int fd, uint8_t *buf, int n) { int32_t src; int32_t dst; int32_t skips; uint8_t fixedbuf[FIXED_BUF_SIZE]; if (n > FIXED_BUF_SIZE) { fprintf(stderr, "oversized request %d > %d", n, FIXED_BUF_SIZE); exit(1); } read_chars(fd, fixedbuf, n); skips = 0; for (src = 0, dst = 0; src < n + (skips * sizeof(struct timeval)); src++, dst++) { buf[dst] = fixedbuf[src]; // do we need to discard a stamp that appears inline to a message? if (fixedbuf[src] == 0xFF) { read_chars(fd, fixedbuf + n + (skips * sizeof(struct timeval)), sizeof(struct timeval)); skips++; src += sizeof(struct timeval); } } return n; } //------------------------------=-----------------------=-------------------------------------------------------------------------- // attempt to match some input // returns 0 on success, -1 on failure static int match(int fd, uint8_t *str) { uint8_t* cursor; uint8_t c; cursor = str; while (read_chars(fd, &c, 1) > 0) { if (c == *cursor) { cursor++; if (*cursor == '\0') return 0; } else { cursor = str; } } return -1; } //------------------------------=-----------------------=-------------------------------------------------------------------------- // simple method of computing a checksum over a header or body static unsigned short em_checksum(unsigned short *w, int n) { unsigned short cs = 0; while (n--) cs += *(w++); return -cs; } //------------------------------=-----------------------=-------------------------------------------------------------------------- // initialize the GPS unit so that it starts spewing Rockwell binary messages #define EARTHA "EARTHA\r\n\0" static int eartha(int fd) { // icky alarm alarm(30); if (match(fd, EARTHA) == -1) { alarm(0); fprintf(stderr, "EARTHA not found\n"); exit(1); } alarm(0); // answer with same and then Rockwell messages should start write(fd, EARTHA, strlen(EARTHA)); fprintf(stdout, "EARTHA\n"); fflush(stdout); return 0; } //------------------------------=-----------------------=-------------------------------------------------------------------------- // futility functions static char i32csign(int32_t l) { if (l > 0) return '+'; if (l < 0) return '-'; return ' '; } static char i64csign(int64_t ll) { if (ll > 0) return '+'; if (ll < 0) return '-'; return ' '; } static char dcsign(double f) { if (f > 0.000000000001) return '+'; if (f < -0.000000000001) return '-'; return ' '; } //------------------------------=-----------------------=-------------------------------------------------------------------------- // legend for raw Rockwell 1000 vs. kernel time dump void legend() { #ifdef JAB_MSG fprintf(stdout, "\nns kt ktdelta gt gtdelta kt - gt drift driftdelta win\n"); #endif } //------------------------------=-----------------------=-------------------------------------------------------------------------- // if tick is way out of whack, adjust it to be sane with defaults. // if it's not out of whack, just leave it alone. static void timex_sanity() { struct timex stmx; stmx.modes = 0; adjtimex(&stmx); if (9900 < stmx.tick && stmx.tick < 10100) { #ifdef JAB_DEBUG fprintf(stdout, "TICKINIT sane tick %ld freq %ld\n", stmx.tick, stmx.freq); fflush(stdout); #endif return; } fprintf(stdout, "TICKINIT insane tick %ld freq %ld - set to default\n", stmx.tick, stmx.freq); fflush(stdout); stmx.tick = 10000; stmx.freq = 0; stmx.modes = ADJ_FREQUENCY | ADJ_TICK; adjtimex(&stmx); } //------------------------------=-----------------------=-------------------------------------------------------------------------- // some manifest constants used in calculations below // units of the freq that are equal to one tick #define D_FREQ_PER_PPM (65536.0) #define L_FREQ_PER_TICK_UNIT (6553600L) #define LL_FREQ_PER_TICK_UNIT (6553600LL) #define D_PPM_SCALING_FACTOR ( ((double) LL_FREQ_PER_TICK_UNIT) / 100.0 ) //------------------------------=-----------------------=-------------------------------------------------------------------------- // returns a 64-bit count of the number of microseconds since the beginning of the Unix epoch (00:00:00 Jan 1, 1970 UTC) static int64_t gettime64() { int64_t rv; struct timeval tv; gettimeofday(&tv, NULL); rv = ((int64_t) tv.tv_sec) * LL_ONE_MILLION; rv += ((int64_t) tv.tv_usec); return rv; } //------------------------------=-----------------------=-------------------------------------------------------------------------- // cause the kernel to slew the specified offset // the i386 with kernel 2.2.16 slews at the rate of 500uS/S - 50uS every scheduler tick (100 times/S) static void adj_offset(int32_t offset) { static int64_t count = 0; int rc; struct timex stmx; if (offset == 0L) return; // get kernel clock parameters stmx.modes = 0; adjtimex(&stmx); // make the adjustment stmx.modes = ADJ_OFFSET_SINGLESHOT; stmx.offset = offset; rc = adjtimex(&stmx); #ifndef JAB_NO_OUT fprintf(stdout, "ADJOFST %06lld " "offset %d " "rc %d\n", count, offset, rc); fflush(stdout); #endif count++; } //------------------------------=-----------------------=-------------------------------------------------------------------------- // adjust the kernel timer frequency by the specified parts per million static void adj_freq(double freqadj_ppm) { int64_t freqadj; int32_t tickadj; static int64_t count = 0; int rc; struct timex stmx; // get kernel clock parameters stmx.modes = 0; adjtimex(&stmx); // divide the frequency adjustment into tick units tickadj = (int32_t) ((freqadj_ppm * D_PPM_SCALING_FACTOR) / LL_FREQ_PER_TICK_UNIT); freqadj = (int32_t) (((int64_t) (freqadj_ppm * D_PPM_SCALING_FACTOR)) % LL_FREQ_PER_TICK_UNIT); // adjust what we got earlier stmx.tick += tickadj; stmx.freq += (int32_t) freqadj; // bound freq range while (stmx.freq > L_FREQ_PER_TICK_UNIT) { stmx.tick++; stmx.freq -= L_FREQ_PER_TICK_UNIT; } while (stmx.freq < -L_FREQ_PER_TICK_UNIT) { stmx.tick--; stmx.freq += L_FREQ_PER_TICK_UNIT; } // make the adjustment stmx.modes = ADJ_FREQUENCY | ADJ_TICK; rc = adjtimex(&stmx); #ifndef JAB_NO_OUT fprintf(stdout, "ADJFREQ %06lld " "freqadj_ppm %c%f " "tck %05ld " "frq %c%07ld " "rc %d\n", count, dcsign(freqadj_ppm), fabs(freqadj_ppm), stmx.tick, i32csign(stmx.freq), labs(stmx.freq), rc); fflush(stdout); #endif count++; } //------------------------------=-----------------------=-------------------------------------------------------------------------- // delta(s) are in uS // time(s)_observed is in uS since 00:00:00 Jan 1, 1970 UTC, aka the Unix epoch - see gettime64(), above. // adj_delta controls whether the correction for a delta component is added in // slope is always corrected for // this is a MAGIC number // this is set to what looks the the max-ish amplitude of oscillations about the trail once the tractor beam is on #define LL_DELTA_EXTEND_SLEW (750LL) // initial/minimum, maximum and increment to the multiplying factor for determination of slew interval #define L_SLEW_MULTIPLIER (20L) static void adjdrift(int64_t time_observed, int64_t last_time_observed, int64_t delta, int64_t last_delta, int adj_delta) { // take note of statics... static int64_t call_counter = 0; double fa_slope_ppm = 0.0; double fa_delta_ppm = 0.0; int32_t slew_interval = 0L; double fa_slope_usec_hour; // this calculates the observed slope in uS/hr // use parens to maintain precision... fa_slope_usec_hour = - (((double) ((delta - last_delta) * LL_SECS_PER_HOUR)) / ((double) ((time_observed - last_time_observed) / LL_ONE_MILLION))); // calculate ppm change due to slope fa_slope_ppm = fa_slope_usec_hour / D_SECS_PER_HOUR; if (adj_delta) { // slew interval calculated via time delta and a constant multiplier slew_interval = ((int32_t) ((time_observed - last_time_observed) / LL_ONE_MILLION)) * L_SLEW_MULTIPLIER; // slew-interval is the the "half-life" of the current delta on the "curve" of convergence if (slew_interval != 0L) fa_delta_ppm = -(((double) delta) / ((double) slew_interval)); } #ifndef JAB_NO_OUT fprintf(stdout, "ADJDRIFT %06lld " "dlt %c%lld.%06lld " "slm %03ld " "sli %05d " "fad %c%.6f (ppm) " "%c%.6f (uS/hr) " "fas %c%.6f (ppm) " "%c%.6f (uS/hr)\n", call_counter, i64csign(delta), llabs(delta / LL_ONE_MILLION), llabs(delta % LL_ONE_MILLION), L_SLEW_MULTIPLIER, slew_interval, dcsign(fa_delta_ppm), fabs(fa_delta_ppm), dcsign(fa_delta_ppm), fabs(fa_delta_ppm * D_SECS_PER_HOUR), dcsign(fa_slope_ppm), fabs(fa_slope_ppm), dcsign(fa_slope_usec_hour), fabs(fa_slope_usec_hour)); fflush(stdout); #endif adj_freq(fa_delta_ppm + fa_slope_ppm); call_counter++; } //------------------------------=-----------------------=-------------------------------------------------------------------------- // number of trail points calculated (rolls through MAX_TRAIL) static int32_t trail_count = 0; static int32_t trail_rollover = 0; //------------------------------=-----------------------=-------------------------------------------------------------------------- // static int64_t set_time_by_delta(int64_t delta64) { struct timeval stv; struct timezone stz; int64_t now64; stz.tz_minuteswest = 0; stz.tz_dsttime = 0; now64 = gettime64() - delta64; stv.tv_sec = (int32_t) (now64 / LL_ONE_MILLION); stv.tv_usec = (int32_t) (now64 % LL_ONE_MILLION); settimeofday(&stv, &stz); #ifdef JAB_DEBUG fprintf(stdout, "KERPLUNK time was set by delta to %ld.%06ld\n", stv.tv_sec, stv.tv_usec); fflush(stdout); #endif // so the first trail elements are not polluted by a massive offset trail_count = 0; return now64; } //------------------------------=-----------------------=-------------------------------------------------------------------------- // static int64_t last_point_delta = 0LL; // sample trail static int64_t trail_delta = 0LL; static int32_t trail_virgin = 1; static history_t trail[MAX_TRAIL]; //------------------------------=-----------------------=-------------------------------------------------------------------------- // static void fix_trail(int64_t delta) { int32_t i; for (i = 0; i < MAX_TRAIL; i++) { trail[i].delta -= delta; } } //------------------------------=-----------------------=-------------------------------------------------------------------------- // this is the mode selector static int32_t tractor_beam_on = 0; // number of samples in the current point, counting up to point_len static int32_t point_count = 0; // track the offsets while the tractor beam is on so as to ajust slope every time the trail rolls over its capacity static int64_t offset_accum = 0LL; static int64_t offset_accum_start = 0LL; //------------------------------=-----------------------=-------------------------------------------------------------------------- // this only gets called before the tractor beam is on static void handle_point(history_t *point, int point_len) { // inter-point state static int64_t last_point_time_observed; static int64_t points_given = 0LL; // statistics int64_t min_time_observed; int64_t max_time_observed; int64_t min_delta; int64_t max_delta; int64_t point_time_observed; int64_t point_delta; int32_t point_weight; int32_t i; if (tractor_beam_on) return; point_time_observed = 0; point_delta = 0; point_weight = 0; min_time_observed = point[0].time_observed; max_time_observed = point[0].time_observed; min_delta = point[0].delta; max_delta = point[0].delta; for (i = 0; i < point_len; i++) { point_time_observed += point[i].time_observed; point_delta += point[i].delta; point_weight += point[i].weight; if (point[i].time_observed < min_time_observed) min_time_observed = point[i].time_observed; if (point[i].time_observed > max_time_observed) max_time_observed = point[i].time_observed; if (point[i].delta < min_delta) min_delta = point[i].delta; if (point[i].delta > max_delta) max_delta = point[i].delta; } point_time_observed /= point_len; point_delta /= point_len; #ifdef JAB_POINT fprintf(stdout, "POINT %06lld N %05d " "time %c%ld.%06ld " "%c%ld.%06ld " "%c%ld.%06ld " "delta %c%ld.%06ld " "%c%ld.%06ld " "%c%ld.%06ld\n", points_given, point_weight, i64csign(min_time_observed), labs((int32_t) (min_time_observed / LL_ONE_MILLION)), labs((int32_t) (min_time_observed % LL_ONE_MILLION)), i64csign(point_time_observed), labs((int32_t) (point_time_observed / LL_ONE_MILLION)), labs((int32_t) (point_time_observed % LL_ONE_MILLION)), i64csign(max_time_observed), labs((int32_t) (max_time_observed / LL_ONE_MILLION)), labs((int32_t) (max_time_observed % LL_ONE_MILLION)), i64csign(min_delta), labs((int32_t) (min_delta / LL_ONE_MILLION)), labs((int32_t) (min_delta % LL_ONE_MILLION)), i64csign(point_delta), labs((int32_t) (point_delta / LL_ONE_MILLION)), labs((int32_t) (point_delta % LL_ONE_MILLION)), i64csign(max_delta), labs((int32_t) (max_delta / LL_ONE_MILLION)), labs((int32_t) (max_delta % LL_ONE_MILLION))); fflush(stdout); #endif // outcome at this point must set points_given to an appropriate value if (llabs(point_delta) > MAX_KERPLUNK) { // KER-PLUNK - hard-set the clock!!! set_time_by_delta(point_delta); // reset the adjustment algorithm points_given = 0; } else { if (points_given % 2) { // long list of conditions for mode change insures less chaos if ( trail_virgin == 0 && trail_delta < 2 * LL_DELTA_EXTEND_SLEW && last_point_delta < 2 * LL_DELTA_EXTEND_SLEW && point_delta < 2 * LL_DELTA_EXTEND_SLEW && llabs(last_point_delta - point_delta) < LL_DELTA_EXTEND_SLEW && llabs(last_point_delta - trail_delta) < LL_DELTA_EXTEND_SLEW && llabs(point_delta - trail_delta) < LL_DELTA_EXTEND_SLEW ) { fprintf(stdout, "MODCHG TRACTOR BEAM ON\n"); fflush(stdout); tractor_beam_on = 1; trail_rollover = trail_count; offset_accum = 0; offset_accum_start = gettime64(); // this is done so that we start tractor beam mode with a zero'd trail_delta fix_trail(trail_delta); // adjust for SLOPE ONLY!!! adjdrift(point_time_observed, last_point_time_observed, point_delta, last_point_delta, 0); } else { #ifdef JAB_DEBUG fprintf (stdout, "NOMODCHG " "trail_virgin %d " "trail_delta %lld < %lld " "last_point_delta %lld < %lld " "point_delta %lld < %lld " "last_point_delta-point_delta |%lld| < %lld " "last_point_delta-trail_delta |%lld| < %lld " "point_delta-trail_delta |%lld| < %lld\n", trail_virgin, trail_delta, 2 * LL_DELTA_EXTEND_SLEW, last_point_delta, 2 * LL_DELTA_EXTEND_SLEW, point_delta, 2 * LL_DELTA_EXTEND_SLEW, last_point_delta - point_delta, LL_DELTA_EXTEND_SLEW, last_point_delta - trail_delta, LL_DELTA_EXTEND_SLEW, point_delta - trail_delta, LL_DELTA_EXTEND_SLEW ); fflush(stdout); #endif // adjust for SLOPE AND DELTA!!! adjdrift(point_time_observed, last_point_time_observed, point_delta, last_point_delta, 1); } } // store goodies for next time through last_point_time_observed = point_time_observed; last_point_delta = point_delta; points_given++; } // start a new point point_count = 0; point_len = 0; } //------------------------------=-----------------------=-------------------------------------------------------------------------- // if we just asked the kernel to adjust by offset, ignore until we think the kernel slew is over static int32_t ignore1000 = 0L; //------------------------------=-----------------------=-------------------------------------------------------------------------- // #define KERNEL_2_2_16_i386_SLEW_USEC_SEC (500LL) static void handle_trail() { static int64_t count = 0; int64_t time_observed = 0; int32_t i; // calc next point on the trail by averaging the contents of the trail shift register for (i = 0; i < MAX_TRAIL; i++) { time_observed += trail[i].time_observed; trail_delta += trail[i].delta; } time_observed /= MAX_TRAIL; trail_delta /= MAX_TRAIL; // in tractor_beam_on mode? if (tractor_beam_on) { // since we are always rounding down fractions, we can never catch a steady offset // this kludge fixes that at the expense of introducing up to a single microsecond of jitter if (trail_delta < 0LL) trail_delta--; else if (trail_delta > 0LL) trail_delta++; // this should be a rather small number adj_offset(-trail_delta); // this adjusts every element of the trail by trail_delta fix_trail(trail_delta); // ignore the next ignore1000 Rockwell 1000 messages (so that the slew has been fully slain) // note: this uses a define that is architecture (and, of course, possibly kernel version dependent) ignore1000 = (int32_t) (llabs(trail_delta) / KERNEL_2_2_16_i386_SLEW_USEC_SEC) + 1; // keep track of the offset we've accumulated for an eventual frequency adjustment offset_accum += trail_delta; } #ifdef JAB_DEBUG fprintf(stdout, "TRAIL %06lld " "time_observed %c%lld.%06lld " "delta %c%lld.%06lld " "offset_accum %c%lld.%06lld " "ignore1000 %d " "tractor_beam_on %d\n", count, i64csign(time_observed), llabs(time_observed) / LL_ONE_MILLION, llabs(time_observed) % LL_ONE_MILLION, i64csign(trail_delta), llabs(trail_delta) / LL_ONE_MILLION, llabs(trail_delta) % LL_ONE_MILLION, i64csign(offset_accum), llabs(offset_accum) / LL_ONE_MILLION, llabs(offset_accum) % LL_ONE_MILLION, ignore1000, tractor_beam_on); fflush(stdout); #endif count++; } //------------------------------=-----------------------=-------------------------------------------------------------------------- // length required before calling handle_point(), calculated below static int32_t point_len = 0; static int64_t trailadj_count = 0LL; #define MAX_MULT (5) static int64_t multipliers[MAX_MULT] = { 70LL, 60LL, 50LL, 40LL, 30LL }; //------------------------------=-----------------------=-------------------------------------------------------------------------- // static void handle_sample(history_t *sample, int sample_count) { // the samples of the current point, held between invocations of this function static history_t point[MAX_POINT]; // calculated statistics int64_t sample_time_observed; int64_t sample_delta; int64_t t_sample_delta; int64_t min_time_observed; int64_t max_time_observed; int64_t min_delta; int64_t max_delta; int64_t multiplier; int64_t adjusted_offset; int32_t t_point_len; int32_t i; sample_time_observed = 0; sample_delta = 0; min_time_observed = sample[0].time_observed; max_time_observed = sample[0].time_observed; min_delta = sample[0].delta; max_delta = sample[0].delta; for (i = 0; i < sample_count; i++) { sample_delta += sample[i].delta; sample_time_observed += sample[i].time_observed; if (sample[i].delta < min_delta) min_delta = sample[i].delta; if (sample[i].delta > max_delta) max_delta = sample[i].delta; if (sample[i].time_observed < min_time_observed) min_time_observed = sample[i].time_observed; if (sample[i].time_observed > max_time_observed) max_time_observed = sample[i].time_observed; } sample_delta /= sample_count; sample_time_observed /= sample_count; #ifdef JAB_SAMPLE #ifdef JAB_MSG fprintf(stdout, "\n"); #endif fprintf(stdout, "SAMPLE %06d N %05d time %c%ld.%06ld %c%ld.%06ld %c%ld.%06ld " "delta %c%ld.%06ld %c%ld.%06ld %c%ld.%06ld " "ptlen %d\n", point_count, sample_count, i64csign(min_time_observed), labs((int32_t) (min_time_observed / LL_ONE_MILLION)), labs((int32_t) (min_time_observed % LL_ONE_MILLION)), i64csign(sample_time_observed), labs((int32_t) (sample_time_observed / LL_ONE_MILLION)), labs((int32_t) (sample_time_observed % LL_ONE_MILLION)), i64csign(max_time_observed), labs((int32_t) (max_time_observed / LL_ONE_MILLION)), labs((int32_t) (max_time_observed % LL_ONE_MILLION)), i64csign(min_delta), labs((int32_t) (min_delta / LL_ONE_MILLION)), labs((int32_t) (min_delta % LL_ONE_MILLION)), i64csign(sample_delta), labs((int32_t) (sample_delta / LL_ONE_MILLION)), labs((int32_t) (sample_delta % LL_ONE_MILLION)), i64csign(max_delta), labs((int32_t) (max_delta / LL_ONE_MILLION)), labs((int32_t) (max_delta % LL_ONE_MILLION)), point_len); fflush(stdout); #endif if (tractor_beam_on && llabs(sample_delta) > 2 * LL_DELTA_EXTEND_SLEW) { fprintf(stdout, "MODCHG TRACTOR BEAM OFF sample_delta |%lld| > %lld\n", sample_delta, 2 * LL_DELTA_EXTEND_SLEW); fflush(stdout); } trail[trail_count].delta = sample_delta; trail[trail_count].time_observed = sample_time_observed; trail_count++; if (trail_count >= MAX_TRAIL) { trail_virgin = 0; trail_count = 0; } if (!trail_virgin) handle_trail(); // if tractor beam is on if (tractor_beam_on) { // if we've drifted far, adjust now... if (llabs(offset_accum) > LL_DELTA_EXTEND_SLEW / 2) { #ifdef JAB_DEBUG fprintf(stdout, "LIMIT offset_accum |%lld| > %lld\n", llabs(offset_accum), LL_DELTA_EXTEND_SLEW / 2); fflush(stdout); #endif trail_rollover = trail_count; } // time to adjust? if (trail_count == trail_rollover) { // don't make any frequency adjustment for the first trail rollover if (trailadj_count > 0) { if (trailadj_count > MAX_MULT) multiplier = multipliers[MAX_MULT - 1]; else multiplier = multipliers[trailadj_count - 1]; adjusted_offset = (offset_accum * multiplier) / 100LL; #ifdef JAB_DEBUG fprintf(stdout, "TRLADJ %06lld " "offset_accum %lld " "multiplier %lld " "adjusted_offset %lld " "offset_accum_start %lld " "sample_time_observed %lld\n", trailadj_count, offset_accum, multiplier, adjusted_offset, offset_accum_start, sample_time_observed); fflush(stdout); #endif adjdrift(sample_time_observed, offset_accum_start, offset_accum / 2, 0, 0); } // keep track of the time _now_ and reset accumulator for future slope computation - don't have access to sample_delta offset_accum_start = gettime64(); offset_accum = 0LL; // count in SAMPLE line out should be reset for every trail adjustment point_count = -1; trailadj_count++; } // ugly, given below... point_count++; return; } // tractor beam's not on, continue calculating our current point point[point_count].delta = sample_delta; point[point_count].time_observed = sample_time_observed; point[point_count].weight = sample_count; point_count++; // if we're within one LL_DELTA_EXTEND SLEW, we calculate the real sample_delta of // interest: the max of the raw sample delta _or_ the difference from the last adjustment // this helps us catch when we've crossed zero, hopefully before we go too far. t_sample_delta = sample_delta; if ( last_point_delta != 0 && llabs(last_point_delta) < LL_DELTA_EXTEND_SLEW && llabs(last_point_delta - sample_delta) > llabs(sample_delta) ) { #ifdef JAB_DEBUG fprintf(stdout, "LIMIT sample_delta %lld adjusted by last_point_delta %lld\n", sample_delta, last_point_delta); fflush(stdout); #endif t_sample_delta = last_point_delta - sample_delta; } // first sample of a point determines the length of the sample // so that the linear progression towards zero generally won't // outrun the progression defined below in the cases - but we // check if point_len has shrunk over time and min it below. // divide by MAGIC number LL_DELTA_EXTEND_SLEW to determine point_len switch ( llabs(t_sample_delta) / LL_DELTA_EXTEND_SLEW ) { case 0 : t_point_len = MAX_POINT; break; case 1 : t_point_len = MAX_POINT / 2; break; case 2 : t_point_len = MAX_POINT / 4; break; case 3 : t_point_len = MAX_POINT / 8; break; case 4 : t_point_len = MAX_POINT / 16; break; case 5 : t_point_len = MAX_POINT / 32; break; case 6 : t_point_len = MAX_POINT / 64; break; default : t_point_len = MIN_POINT; break; } // lower bound if (t_point_len < MIN_POINT) t_point_len = MIN_POINT; // is this the first time since reset, or did it shrink? if (point_len == 0 || t_point_len < point_len) { point_len = t_point_len; #ifdef JAB_DEBUG fprintf(stdout, "STATE sample_delta %lld point_len %d\n", sample_delta, point_len); fflush(stdout); #endif } if ( llabs(last_point_delta) < 4 * LL_DELTA_EXTEND_SLEW && llabs(last_point_delta - sample_delta) > 2 * LL_DELTA_EXTEND_SLEW ) { point_len = point_count; #ifdef JAB_DEBUG fprintf(stdout, "LIMIT close, but moving fast, point NOW\n"); fflush(stdout); #endif } // time to do something pointwise? if (point_count >= point_len) { handle_point(point, point_count); point_count = 0; point_len = 0; } } //------------------------------=-----------------------=-------------------------------------------------------------------------- // handle the Rockwell 1000 message // contains time, gps position, velocity vectors, etc // we only care about the time // maximum drift window in uS between the timings of consecutively-received type 1000 messages to allow their inclusion in a sample #define LEGEND_INTERVAL (20) static void handle1000(struct timeval *tv, unsigned short *msg) { // counts lines of MSGs for the MSG legend static int32_t legend_count = LEGEND_INTERVAL; static int32_t msg_count = 0; // inter-sample state static int64_t lastdelta = 0; static int64_t lastdrift = 0; static int64_t lastgtime = 0; static int64_t lastktime = 0; static int32_t sample_count = 0; static int32_t locked = 1; // the current sample static history_t sample[MAX_SAMPLE]; int32_t sample_cursor; int32_t qualifies; struct tm stm; // observations for this sample int64_t ktime; int64_t gtime; int32_t gsecs; int32_t gusecs; // printable statistics for this sample int64_t delta; int64_t drift; int64_t driftdelta; int64_t gtimedelta; int64_t ktimedelta; // qualification test variables int64_t min_delta; int64_t max_delta; int64_t this_delta; if (ignore1000 > 0) { ignore1000--; return; } // is the data invalid according to Jupiter chipset? if ((msg[4] & 0x0004) != 0) { if (locked != 0) { // transitioned to not locked #ifndef JAB_NO_MSG // msg[6] is the number of satellites that we're locked on to fprintf(stdout, "UNLOCKED %ld.%06ld %02u", tv->tv_sec, tv->tv_usec, msg[6]); fflush(stdout); #endif // if we become unlocked after having been locked, simply die // i.e. something should restart us... if (msg_count != 0) { fprintf(stdout, "\nEXITING\n"); fflush(stdout); exit(1); } locked = 0; } else { // until we're locked, drop a period a per message (appx. one second) to stdout #ifndef JAB_NO_MSG fprintf(stdout, "."); fflush(stdout); #endif } return; } // we have valid data, so we are locked if (!locked) { // transitioned to locked #ifndef JAB_NO_MSG fprintf(stdout, "\nLOCKED %ld.%06ld %02u\n", tv->tv_sec, tv->tv_usec, msg[6]); fflush(stdout); #endif locked = 1; } // we only count valid messages; msg_count++; // parse Rockwell 1000 message into a struct tm for conversion to Unix-style Julian date stm.tm_mday = msg[13]; stm.tm_mon = msg[14] - 1; stm.tm_year = msg[15] - 1900; stm.tm_hour = msg[16]; stm.tm_min = msg[17]; stm.tm_sec = msg[18]; //stm.tm_gmtoff = 0; stm.tm_isdst = 0; gsecs = mktime(&stm); // msg[19] is number of nanoseconds... gusecs = (*((int32_t *) &msg[19])) / 1000; legend_count++; if (legend_count > LEGEND_INTERVAL) { legend_count = 0; legend(); } // constant is the _FUDGE_FACTOR_ that I've found brings me into the neighborhood of NTP time // this may be different on different hardware than my setup - who knows? - grain of salt... gtime = ((int64_t) gsecs) * LL_ONE_MILLION + ((int64_t) gusecs) + 993000LL; ktime = ((int64_t) tv->tv_sec) * LL_ONE_MILLION + ((int64_t) tv->tv_usec); delta = ktime - gtime; drift = delta - lastdelta; driftdelta = drift - lastdrift; gtimedelta = gtime - lastgtime; ktimedelta = ktime - lastktime; // validate that all points collected so far fit within a window of delta qualifies = 1; min_delta = (int64_t) 0x7FFFFFFFFFFFFFFFLL; max_delta = (int64_t) 0x8000000000000000LL; // this actually rolls out to be -1 sample[sample_count].time_observed = gtime; sample[sample_count].delta = delta; for (sample_cursor = 0; sample_cursor <= sample_count; sample_cursor++) { this_delta = sample[sample_cursor].delta; if (this_delta < min_delta) min_delta = this_delta; if (this_delta > max_delta) max_delta = this_delta; if (max_delta - min_delta > (LL_DELTA_EXTEND_SLEW / 2)) { qualifies = 0; break; } } #ifdef JAB_MSG // we skip the first message so that the differential stats won't be skewed to the point of ugliness if (lastgtime != 0) { fprintf(stdout, "%02u " "%ld.%06ld " "%c%d.%06ld " "%d.%06d " "%c%ld.%06ld " "%c%ld.%06ld " "%c%ld.%06ld " "%c%ld.%06ld " "%03lld " "%s\n", msg[6], tv->tv_sec, tv->tv_usec, i64csign(ktimedelta), ((int32_t) (ktimedelta / LL_ONE_MILLION)), labs((int32_t) (ktimedelta % LL_ONE_MILLION)), gsecs, gusecs, i64csign(gtimedelta), labs((int32_t) (gtimedelta / LL_ONE_MILLION)), labs((int32_t) (gtimedelta % LL_ONE_MILLION)), i64csign(delta), labs(((int32_t) (delta / LL_ONE_MILLION))), labs((int32_t) (delta % LL_ONE_MILLION)), i64csign(drift), labs(((int32_t) (drift / LL_ONE_MILLION))), labs((int32_t) (drift % LL_ONE_MILLION)), i64csign(driftdelta), labs(((int32_t) (driftdelta / LL_ONE_MILLION))), labs((int32_t) (driftdelta % LL_ONE_MILLION)), (sample_count == 0) ? 0LL : max_delta - min_delta, (qualifies == 1) ? "*" : ""); fflush(stdout); } #endif if (qualifies == 1 && sample_count < MAX_SAMPLE) { // we shall retain the latest msg in the sample sample_count++; } else { // note that the msg that "broke" the sample is simply discarded. // more often than not, this point is an outlier anyway if (sample_count >= MIN_SAMPLE) handle_sample(sample, sample_count); // we shall toss out the sample and the latest msg sample_count = 0; legend(); legend_count = 0; } // store stats on the last message received lastktime = ktime; lastgtime = gtime; lastdrift = drift; lastdelta = delta; } //------------------------------=-----------------------=-------------------------------------------------------------------------- // struct rockwell_header { unsigned short sync; unsigned short id; unsigned short ndata; unsigned short flags; unsigned short csum; }; //------------------------------=-----------------------=-------------------------------------------------------------------------- // #define MAX_BUF (1024) static void rockwell_framer(int fd) { uint8_t c; int32_t bytes; struct rockwell_header hdr; struct timeval itv; unsigned short buf[MAX_BUF]; unsigned short cksum; int rc; while (1) { rc = read_until_FF_stamp(fd, &itv); if (rc != 0 || read_chars(fd, &c, 1) != 1) { fprintf(stderr, "read returns unhappy\n"); exit(1); } // 0x81 is always the second byte of a header if (c != 0x81) { fprintf(stdout, "\nfalse header 0x%02X != 0x81 - do you have Jack Bates' serial.c kernel module?\n", c); fflush(stdout); continue; } // we are now sync'd up // note that the following assumes LITTLE-ENDIAN hdr.sync = 0x81FF; // get the rest of the header if (read_bytes_discarding_stamp(fd, ((char *) (&hdr)) + 2, 8) != 8) { fprintf(stderr, "rockwell_framer: read_bytes_discarding_stamp invalid return (rest of header)\n"); exit(1); } cksum = em_checksum((unsigned short *) &hdr, 4); if (cksum != hdr.csum) { fprintf(stdout, "rockwell_framer: header checksum failure %04X != %04X\n", cksum, hdr.csum); fflush(stdout); continue; } // read the rest of the message bytes = hdr.ndata * sizeof(unsigned short); if (read_bytes_discarding_stamp(fd, (char *) buf, bytes) != bytes) { fprintf(stderr, "rockwell_framer: read_bytes_discarding_stamp invalid return (rest of message)\n"); exit(1); } if (read_bytes_discarding_stamp(fd, (char *) &cksum, sizeof(unsigned short)) != sizeof(unsigned short)) { fprintf(stderr, "rockwell_framer: read_bytes_discarding_stamp invalid return (checksum)\n"); exit(1); } if (cksum != em_checksum(buf, hdr.ndata)) { fprintf(stdout, "rockwell_framer: data checksum error\n"); fflush(stdout); continue; } if (hdr.id == 1000) handle1000(&itv, buf); } } //------------------------------=-----------------------=-------------------------------------------------------------------------- // int main(int argc, char *argv[]) { int fd; char* dev = argv[1]; fprintf(stdout, "TractorBeam-%s requires Jack Bates' SERIAL_INTR_TIMING kernel module (serial.c)\n", VERSION); fflush(stdout); if (argc != 2) dev = "/dev/ttyS0"; // close stdin - we don't need it close(0); // act like the daemon we are and auto-background if (fork()) exit(0); // disconnect from controlling tty fd = open("/dev/tty", O_RDWR|O_NOCTTY); if (fd >= 0) { ioctl(fd, TIOCNOTTY, NULL); close(fd); } setpgrp(); // default the kernel timex in case of INSANITY timex_sanity(); // open serial port fd = open_serial(dev); // handle EarthMate initialization eartha(fd); // handle EarthMate data stream rockwell_framer(fd); // should never get here close(fd); return 0; }