#ifdef HAVE_CONFIG_H
# include <config.h>
#endif
#if defined(REFCLOCK) && defined(CLOCK_PARSE) && defined(CLOCK_TRIMTSIP)
#include "ntp_syslog.h"
#include "ntp_types.h"
#include "ntp_fp.h"
#include "ntp_unixtime.h"
#include "ntp_calendar.h"
#include "ntp_machine.h"
#include "ntp_stdlib.h"
#include "parse.h"
#ifndef PARSESTREAM
# include <stdio.h>
#else
# include "sys/parsestreams.h"
#endif
#include "ascii.h"
#include "binio.h"
#include "ieee754io.h"
#include "trimble.h"
static offsets_t trim_offsets = { 0, 1, 2, 3, 4, 5, 6, 7 };
struct trimble
{
u_char t_in_pkt;
u_char t_dle;
u_short t_week;
u_short t_weekleap;
u_short t_dayleap;
u_short t_gpsutc;
u_short t_gpsutcleap;
u_char t_operable;
u_char t_mode;
u_char t_leap;
u_char t_utcknown;
};
#define STATUS_BAD 0
#define STATUS_UNSAFE 1
#define STATUS_SYNC 2
static unsigned long inp_tsip P((parse_t *, unsigned int, timestamp_t *));
static unsigned long cvt_trimtsip P((unsigned char *, int, struct format *, clocktime_t *, void *));
struct clockformat clock_trimtsip =
{
inp_tsip,
cvt_trimtsip,
pps_one,
0,
"Trimble TSIP",
400,
sizeof(struct trimble)
};
#define ADDSECOND 0x01
#define DELSECOND 0x02
static unsigned long
inp_tsip(
parse_t *parseio,
unsigned int ch,
timestamp_t *tstamp
)
{
struct trimble *t = (struct trimble *)parseio->parse_pdata;
if (!t)
return PARSE_INP_SKIP;
if (!t->t_in_pkt && ch != DLE) {
return PARSE_INP_SKIP;
}
if ((parseio->parse_index >= (parseio->parse_dsize - 2)) ||
(parseio->parse_dtime.parse_msglen >= (sizeof(parseio->parse_dtime.parse_msg) - 2)))
{
t->t_in_pkt = t->t_dle = 0;
parseio->parse_index = 0;
parseio->parse_dtime.parse_msglen = 0;
return PARSE_INP_SKIP;
}
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_msglen = 0;
parseio->parse_dtime.parse_msg[parseio->parse_dtime.parse_msglen++] = ch;
parseio->parse_dtime.parse_stime = *tstamp;
} else if (t->t_dle) {
t->t_dle = 0;
parseio->parse_data[parseio->parse_index++] = DLE;
parseio->parse_dtime.parse_msg[parseio->parse_dtime.parse_msglen++] = DLE;
} else
t->t_dle = 1;
break;
case ETX:
if (t->t_dle) {
parseio->parse_data[parseio->parse_index++] = DLE;
parseio->parse_data[parseio->parse_index] = ch;
parseio->parse_ldsize = parseio->parse_index+1;
memcpy(parseio->parse_ldata, parseio->parse_data, parseio->parse_ldsize);
parseio->parse_dtime.parse_msg[parseio->parse_dtime.parse_msglen++] = DLE;
parseio->parse_dtime.parse_msg[parseio->parse_dtime.parse_msglen++] = ch;
t->t_in_pkt = t->t_dle = 0;
return PARSE_INP_TIME|PARSE_INP_DATA;
}
default:
t->t_dle = 0;
parseio->parse_data[parseio->parse_index++] = ch;
parseio->parse_dtime.parse_msg[parseio->parse_dtime.parse_msglen++] = ch;
}
return PARSE_INP_SKIP;
}
static int
getshort(
unsigned char *p
)
{
return get_msb_short(&p);
}
static unsigned long
cvt_trimtsip(
unsigned char *buffer,
int size,
struct format *format,
clocktime_t *clock_time,
void *local
)
{
register struct trimble *t = (struct trimble *)local;
#define mb(_X_) (buffer[2+(_X_)])
register u_char cmd;
clock_time->flags = 0;
if (!t) {
return CVT_NONE;
}
if ((size < 4) ||
(buffer[0] != DLE) ||
(buffer[size-1] != ETX) ||
(buffer[size-2] != DLE))
{
printf("TRIMBLE BAD packet, size %d:\n", size);
return CVT_NONE;
}
else
{
unsigned char *bp;
cmd = buffer[1];
switch(cmd)
{
case CMD_RCURTIME:
{
l_fp secs;
int week = getshort((unsigned char *)&mb(4));
l_fp utcoffset;
l_fp gpstime;
bp = &mb(0);
if (fetch_ieee754(&bp, IEEE_SINGLE, &secs, trim_offsets) != IEEE_OK)
return CVT_FAIL|CVT_BADFMT;
if ((secs.l_i <= 0) ||
(t->t_utcknown == 0))
{
clock_time->flags = PARSEB_POWERUP;
return CVT_OK;
}
if (week < 990) {
week += 1024;
}
bp = &mb(6);
if (fetch_ieee754(&bp, IEEE_SINGLE, &utcoffset, trim_offsets) != IEEE_OK)
return CVT_FAIL|CVT_BADFMT;
L_SUB(&secs, &utcoffset);
gpstolfp((unsigned short)week, (unsigned short)0,
secs.l_ui, &gpstime);
gpstime.l_uf = secs.l_uf;
clock_time->utctime = gpstime.l_ui - JAN_1970;
TSFTOTVU(gpstime.l_uf, clock_time->usecond);
if (t->t_leap == ADDSECOND)
clock_time->flags |= PARSEB_LEAPADD;
if (t->t_leap == DELSECOND)
clock_time->flags |= PARSEB_LEAPDEL;
switch (t->t_operable)
{
case STATUS_SYNC:
clock_time->flags &= ~(PARSEB_POWERUP|PARSEB_NOSYNC);
break;
case STATUS_UNSAFE:
clock_time->flags |= PARSEB_NOSYNC;
break;
case STATUS_BAD:
clock_time->flags |= PARSEB_NOSYNC|PARSEB_POWERUP;
break;
}
if (t->t_mode == 0)
clock_time->flags |= PARSEB_POSITION;
clock_time->flags |= PARSEB_S_LEAP|PARSEB_S_POSITION;
return CVT_OK;
}
case CMD_RRECVHEALTH:
{
u_char status = mb(0);
switch (status)
{
case 0x00:
t->t_operable = STATUS_SYNC;
break;
case 0x09:
case 0x0A:
case 0x0B:
t->t_operable = STATUS_UNSAFE;
break;
default:
t->t_operable = STATUS_BAD;
break;
}
t->t_mode = status;
}
break;
case CMD_RUTCPARAM:
{
l_fp t0t;
unsigned char *lbp;
int tls = t->t_gpsutc = getshort((unsigned char *)&mb(12));
int tlsf = t->t_gpsutcleap = getshort((unsigned char *)&mb(24));
t->t_weekleap = getshort((unsigned char *)&mb(20));
if (t->t_weekleap < 990)
t->t_weekleap += 1024;
t->t_dayleap = getshort((unsigned char *)&mb(22));
t->t_week = getshort((unsigned char *)&mb(18));
if (t->t_week < 990)
t->t_week += 1024;
lbp = (unsigned char *)&mb(14);
if (fetch_ieee754(&lbp, IEEE_SINGLE, &t0t, trim_offsets) != IEEE_OK)
return CVT_FAIL|CVT_BADFMT;
t->t_utcknown = t0t.l_ui != 0;
if ((t->t_utcknown) &&
(tlsf != tls) &&
((t->t_weekleap - t->t_week) < 5))
{
if (tlsf > tls)
t->t_leap = ADDSECOND;
else
t->t_leap = DELSECOND;
}
else
{
t->t_leap = 0;
}
}
break;
default:
break;
}
}
return CVT_SKIP;
}
#else
int clk_trimtsip_bs;
#endif