Aren
/
NetBSD
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
NetBSD/usr.sbin/xntp/xntpd/clk_trimtsip.c

546 lines
14 KiB
C
Raw Blame History

/* $NetBSD: clk_trimtsip.c,v 1.4 1998/04/01 15:01:21 christos Exp $ */
/*
* /src/NTP/REPOSITORY/v4/libparse/clk_trimtsip.c,v 1.8 1997/04/13 10:05:23 kardel Exp
*
* Trimble TSIP support - CURRENTLY VERY MUCH UNDER CONSTRUCTION
*/
#ifdef HAVE_CONFIG_H
#include <config.h>
#endif
#if defined(REFCLOCK) && defined(PARSE) && defined(CLOCK_TRIMTSIP) && !defined(PARSESTREAM)
#include <sys/types.h>
#include <sys/time.h>
#include <sys/errno.h>
#include "ntp_syslog.h"
#include "ntp_types.h"
#include "ntp_fp.h"
#include "ntp_unixtime.h"
#include "ntp_calendar.h"
#if defined(PARSESTREAM) && (defined(SYS_SUNOS4) || defined(SYS_SOLARIS)) && defined(STREAM)
/*
* Sorry, but in SunOS 4.x AND Solaris 2.x kernels there are no
* mem* operations. I don't want them - bcopy, bzero
* are fine in the kernel
*/
# ifndef NTP_NEED_BOPS
# define NTP_NEED_BOPS
# endif
#else
# ifndef NTP_NEED_BOPS
# ifndef bzero
# define bzero(_X_, _Y_) memset(_X_, 0, _Y_)
# define bcopy(_X_, _Y_, _Z_) memmove(_Y_, _X_, _Z_)
# endif
# endif
#endif
#include "parse.h"
#include "ntp_stdlib.h"
/*
* Trimble TSIP parser
*
* The receiver uses a serial message protocol called Trimble Standard
* Interface Protocol (it can support others but this driver only supports
* TSIP). Messages in this protocol have the following form:
*
* <DLE><id> ... <data> ... <DLE><ETX>
*
* Any bytes within the <data> portion of value 10 hex (<DLE>) are doubled
* on transmission and compressed back to one on reception. Otherwise
* the values of data bytes can be anything. The serial interface is RS-422
* asynchronous using 9600 baud, 8 data bits with odd party (**note** 9 bits
* in total!), and 1 stop bit. The protocol supports byte, integer, single,
* and double datatypes. Integers are two bytes, sent most significant first.
* Singles are IEEE754 single precision floating point numbers (4 byte) sent
* sign & exponent first. Doubles are IEEE754 double precision floating point
* numbers (8 byte) sent sign & exponent first.
* The receiver supports a large set of messages, only a small subset of
* which are used here. From driver to receiver the following are used:
*
* ID Description
*
* 21 Request current time
* 22 Mode Select
* 2C Set/Request operating parameters
* 2F Request UTC info
* 35 Set/Request I/O options
*
* From receiver to driver the following are recognised:
*
* ID Description
*
* 41 GPS Time
* 44 Satellite selection, PDOP, mode
* 46 Receiver health
* 4B Machine code/status
* 4C Report operating parameters (debug only)
* 4F UTC correction data (used to get leap second warnings)
* 55 I/O options (debug only)
*
* All others are accepted but ignored.
*
*/
extern int debug;
struct trimble
{
u_char t_in_pkt; /* first DLE received */
u_char t_dle; /* subsequent DLE received */
u_char t_status; /* last status */
u_char t_error; /* last error */
u_short t_week; /* GPS week */
u_short t_weekleap; /* GPS week of next/last week */
u_short t_dayleap; /* day in week */
u_short t_gpsutc; /* GPS - UTC offset */
u_short t_gpsutcleap; /* offset at next/last leap */
u_char t_operable; /* receiver feels OK */
u_char t_leap; /* possible leap warning */
};
static u_long inp_tsip P((parse_t *, unsigned int, timestamp_t *));
static u_long cvt_trimtsip P((char *, unsigned int, void *, clocktime_t *, void *));
static float getflt P((u_char *));
static double getdbl P((u_char *));
static int getint P((u_char *));
struct clockformat clock_trimtsip =
{
inp_tsip, /* Trimble TSIP input handler */
cvt_trimtsip, /* Trimble TSIP conversion */
syn_simple, /* easy time stamps for RS232 (fallback) */
pps_simple, /* easy PPS monitoring */
NULL, /* no time code synthesizer monitoring */
NULL, /* no configuration data */
"Trimble SV6/TSIP",
128, /* input buffer */
CVT_FIXEDONLY, /* we do our own input handling */
sizeof(struct trimble), /* no private data */
{ 0, 0},
'\0',
'\0',
'\0'
};
#define ADDSECOND 0x01
#define DELSECOND 0x02
#define DLE 0x10
#define ETX 0x03
static unsigned long inp_tsip(parseio, ch, ctime)
register parse_t *parseio;
register unsigned int ch;
register timestamp_t *ctime;
{
register struct trimble *t = (struct trimble *)parseio->parse_pdata;
if (!t)
return 0; /* local data not allocated - sigh! */
if (!t->t_in_pkt && ch != DLE) {
/* wait for start of packet */
#ifdef DEBUG
if (debug > 2)
printf("sv6+ discarding %2.2x\n", ch);
#endif
return 0;
}
switch (ch) {
case DLE:
if (!t->t_in_pkt) {
t->t_dle = 0;
t->t_in_pkt = 1;
parseio->parse_index = 0;
parseio->parse_data[parseio->parse_index++] = ch;
parseio->parse_dtime.parse_stime = *ctime; /* pick up time stamp at packet start */
} else if (t->t_dle) {
/* Double DLE -> insert a DLE */
t->t_dle = 0;
parseio->parse_data[parseio->parse_index++] = DLE;
} else
t->t_dle = 1;
break;
case ETX:
if (t->t_dle) {
/* DLE,ETX -> end of packet */
parseio->parse_data[parseio->parse_index++] = DLE;
parseio->parse_data[parseio->parse_index++] = ch;
parseio->parse_data[parseio->parse_index] = '\0';
parseio->parse_ldsize = parseio->parse_index+1;
bcopy(parseio->parse_data, parseio->parse_ldata, parseio->parse_ldsize);
t->t_in_pkt = t->t_dle = 0;
return 1;
}
/* fall into ... */
default:
t->t_dle = 0;
parseio->parse_data[parseio->parse_index++] = ch;
}
return (parseio->parse_index == parseio->parse_dsize-1); /* buffer full - attempt to parse (likely to fail) */
}
#define GPSORIGIN 2524953600U /* NTP origin - GPS origin in seconds */
#define SECSPERWEEK 604800 /* seconds per week - GPS tells us about weeks */
#define L_UF_SCALE 4294967296.0 /* scale a float fraction to l_uf units */
/*
* mapping union for ints, floats, doubles for both input & output to the
* receiver
*
* CAVEAT: must disappear - non portable
*/
union {
u_char bd[8];
int iv;
float fv;
double dv;
} uval;
static float getflt P((u_char *));
static double getdbl P((u_char *));
static int getint P((u_char *));
/*
* cvt_trimtsip
*
* convert TSIP type format
*/
static unsigned long
cvt_trimtsip(buffer, size, vf, clock, vt)
register char *buffer;
register unsigned int size;
register void *vf;
register clocktime_t *clock;
register void *vt;
{
register struct trimble *t = vt;
#define mb(_X_) (buffer[2+(_X_)]) /* shortcut for buffer access */
register u_char cmd;
clock->flags = 0;
if (!t) {
#ifdef DEBUG
if (debug) printf("sv6+ BAD call (t=0)\n");
#endif
return CVT_NONE; /* local data not allocated - sigh! */
}
if ((size < 4) ||
(buffer[0] != DLE) ||
(buffer[size-1] != ETX) ||
(buffer[size-2] != DLE))
{
#ifdef DEBUG
if (debug > 2) {
int i;
printf("sv6+ BAD packet, size %d:\n ", size);
for (i = 0; i < size; i++) {
printf ("%2.2x, ", buffer[i]&0xff);
if (i%16 == 15) printf("\n\t");
}
printf("\n");
}
#endif
return CVT_NONE;
}
else
{
cmd = buffer[1];
#ifdef DEBUG
if (debug > 1)
switch(cmd)
{
case 0x41:
printf("sv6+ gps time: %f, %d, %f\n",
getflt((unsigned char *)&mb(0)), getint((unsigned char *)&mb(4)), getflt((unsigned char *)&mb(6)));
break;
case 0x44:
printf("sv6+ sats: %2x, %2d %2d %2d %2d, %.2f\n",
mb(0), mb(1), mb(2), mb(3), mb(4), getflt((unsigned char *)&mb(5)));
break;
case 0x45:
printf("sv6+ software: %d.%d (19%d/%d/%d)\n",
mb(0)&0xff, mb(1)&0xff, mb(4)&0xff, mb(2)&0xff, mb(3)&0xff);
break;
case 0x46:
printf("sv6+ health: %2x %2x\n",
mb(0), mb(1));
break;
case 0x48:
printf("sv6+ gps message: '%.22s'\n", &mb(0));
break;
case 0x4b:
printf("sv6+ status: %2d %2x\n",
mb(0), mb(1));
break;
case 0x4c:
printf("sv6+ op params: %2x %.1f %.1f %.1f %.1f\n",
mb(0), getflt((unsigned char *)&mb(1)), getflt((unsigned char *)&mb(5)),
getflt((unsigned char *)&mb(9)), getflt((unsigned char *)&mb(13)));
break;
case 0x4f:
printf("sv6+ utc data: %.3e %.3e %d %d %d %d %d\n",
getdbl((unsigned char *)&mb(0)), getflt((unsigned char *)&mb(8)),
getint((unsigned char *)&mb(18)),
getint((unsigned char *)&mb(12)),
getint((unsigned char *)&mb(20)), getint((unsigned char *)&mb(22)), getint((unsigned char *)&mb(24)));
break;
case 0x54:
/*printf("sv6+ bias and rate: %.1fm %.2fm/s at %.1fs\n",
getflt(&mb(0)), getflt(&mb(4)), getflt(&mb(8))); ignore it*/
break;
case 0x55:
printf("sv6+ io opts: %2x %2x %2x %2x\n",
mb(0), mb(1), mb(2), mb(3));
break;
case 0x8f:
{
#define RTOD (180.0 / 3.1415926535898)
double lat = getdbl((unsigned char *)&mb(2));
double lng = getdbl((unsigned char *)&mb(10));
printf("sv6+ last fix: %2.2x %d lat %f %c, long %f %c, alt %.2fm\n",
mb(1)&0xff, mb(40)&0xff,
((lat < 0) ? (-lat) : (lat))*RTOD, (lat < 0 ? 'S' : 'N'),
((lng < 0) ? (-lng) : (lng))*RTOD, (lng < 0 ? 'W' : 'E'),
getdbl((unsigned char *)&mb(18)));
}
break;
case 0x40:
case 0x5b:
case 0x6d:
/* Ignore */
break;
default:
printf("sv6+ cmd ignored: %2x, length: %d\n",
cmd, size);
break;
}
#endif /* DEBUG */
switch(cmd)
{
case 0x41:
{ /* GPS time */
float secs = getflt((unsigned char *)&mb(0));
int week = getint((unsigned char *)&mb(4));
int secint;
float secfrac;
l_fp gpstime;
if (secs <= 0)
{
#ifdef DEBUG
if (debug)
printf("sv6+ seconds <= 0 (%e), setting POWERUP\n", secs);
#endif
clock->flags = PARSEB_POWERUP;
return CVT_OK;
}
/* time OK */
secint = secs; /* integer part, hopefully */
secfrac = secs - secint; /* 0.0 <= secfrac < 1.0 */
secint -= getflt((unsigned char *)&mb(6)); /* UTC offset */
gpstime.l_ui = week*SECSPERWEEK + secint + GPSORIGIN; /* convert to NTP time */
gpstime.l_uf = secfrac*L_UF_SCALE;
clock->utctime = gpstime.l_ui - JAN_1970;
TSFTOTVU(gpstime.l_uf, clock->usecond);
if (t->t_leap == ADDSECOND)
clock->flags |= PARSEB_LEAPADD;
if (t->t_leap == DELSECOND)
clock->flags |= PARSEB_LEAPDEL;
if (t->t_operable)
clock->flags &= ~(PARSEB_NOSYNC|PARSEB_POWERUP);
else
clock->flags |= PARSEB_NOSYNC;
return CVT_OK;
} /* case 0x41 */
break;
case 0x46:
{
/* sv6+ health */
u_char status = t->t_status = mb(0);
t->t_error = mb(1);
if (status == 0 || status == 9 || status == 10 || status == 11)
{
t->t_operable = 1;
}
else
{
t->t_operable = 0;
}
}
break;
case 0x4f:
{
/* UTC correction data - derive a leap warning */
int tls = t->t_gpsutc = getint((unsigned char *)&mb(12)); /* current leap correction (GPS-UTC) */
int wnlsf = t->t_weekleap = getint((unsigned char *)&mb(20)); /* week no of leap correction */
int dn = t->t_dayleap = getint((unsigned char *)&mb(22)); /* day in week of leap correction */
int tlsf = t->t_gpsutcleap = getint((unsigned char *)&mb(24)); /* new leap correction */
u_long now, leaptime;
t->t_week = getint((unsigned char *)&mb(18)); /* current week no */
/* this stuff hasn't been tested yet... */
now = clock->utctime + JAN_1970; /* now in GPS seconds */
leaptime = (wnlsf*7 + dn)*86400; /* time of leap in GPS seconds */
if ((leaptime > now) && ((leaptime-now) < 86400*28))
{
/* generate a leap warning */
if (tlsf > tls)
t->t_leap = ADDSECOND;
else
t->t_leap = DELSECOND;
}
else
{
t->t_leap = 0;
}
}
break;
default:
/* it's validly formed, but we don't care about it! */
break;
}
}
return CVT_SKIP;
}
/*
* getflt, getdbl, getint convert fields in the incoming data into the
* appropriate type of item
*
* CAVEAT: these routines are currently definitely byte order dependent
* and assume Representation(float) == IEEE754
* These functions MUST be converted to portable versions (especially
* converting the float representation into ntp_fp formats in order
* to avoid floating point operations at all!
*/
static float
getflt(bp)
u_char *bp;
{
#ifdef XNTP_BIG_ENDIAN
uval.bd[0] = *bp++;
uval.bd[1] = *bp++;
uval.bd[2] = *bp++;
uval.bd[3] = *bp;
#else /* ! XNTP_BIG_ENDIAN */
uval.bd[3] = *bp++;
uval.bd[2] = *bp++;
uval.bd[1] = *bp++;
uval.bd[0] = *bp;
#endif /* ! XNTP_BIG_ENDIAN */
return uval.fv;
}
static double
getdbl(bp)
u_char *bp;
{
#ifdef XNTP_BIG_ENDIAN
uval.bd[0] = *bp++;
uval.bd[1] = *bp++;
uval.bd[2] = *bp++;
uval.bd[3] = *bp++;
uval.bd[4] = *bp++;
uval.bd[5] = *bp++;
uval.bd[6] = *bp++;
uval.bd[7] = *bp;
#else /* ! XNTP_BIG_ENDIAN */
uval.bd[0] = *bp++;
uval.bd[1] = *bp++;
uval.bd[2] = *bp++;
uval.bd[3] = *bp++;
uval.bd[4] = *bp++;
uval.bd[5] = *bp++;
uval.bd[6] = *bp++;
uval.bd[7] = *bp;
#endif /* ! XNTP_BIG_ENDIAN */
return uval.dv;
}
static int
getint(bp)
u_char *bp;
{
if (*bp & 0x80)
return ((~0xffff) | (*bp<<8) | *(bp+1));
else
return ((*bp<<8) | *(bp+1));
}
#else /* not (REFCLOCK && PARSE && CLOCK_TRIMTSIP && !PARSESTREAM) */
int clk_trimtsip_bs;
#endif /* not (REFCLOCK && PARSE && CLOCK_TRIMTSIP && !PARSESTREAM) */
/*
* History:
*
* clk_trimtsip.c,v
* Revision 1.8 1997/04/13 10:05:23 kardel
* 3.5.90 reconcilation
*
* Revision 1.7 1997/01/19 12:44:42 kardel
* 3-5.88.1 reconcilation
*
* Revision 1.6 1996/12/01 12:59:10 kardel
* appropriate casts for conversion routine parameters
*
* Revision 1.5 1996/11/30 20:45:16 kardel
* initial compilable SunOS 4 version of parse autoconfigure
*
* Revision 1.4 1996/11/24 20:09:48 kardel
* RELEASE_5_86_12_2 reconcilation
*
* Revision 1.3 1994/10/03 21:59:30 kardel
* 3.4e cleanup/integration
*
* Revision 1.2 1994/10/03 10:04:11 kardel
* 3.4e reconcilation
*
* Revision 1.1.1.2 1994/08/28 15:58:08 kardel
* Release 4d, Aug 20th, 1994
*
*/
Reference in New Issue View Git Blame Copy Permalink