PageRenderTime 68ms CodeModel.GetById 47ms app.highlight 17ms RepoModel.GetById 1ms app.codeStats 1ms

/contrib/ntp/libparse/clk_trimtsip.c

https://bitbucket.org/freebsd/freebsd-head/
C | 456 lines | 255 code | 59 blank | 142 comment | 42 complexity | 86bfe306fdf00b01bb572498d6bf26fc MD5 | raw file
  1/*
  2 * /src/NTP/ntp4-dev/libparse/clk_trimtsip.c,v 4.17 2005/04/16 17:32:10 kardel RELEASE_20050508_A
  3 *
  4 * clk_trimtsip.c,v 4.17 2005/04/16 17:32:10 kardel RELEASE_20050508_A
  5 *
  6 * Trimble TSIP support
  7 * Thanks to Sven Dietrich for providing test hardware
  8 *
  9 * Copyright (c) 1995-2005 by Frank Kardel <kardel <AT> ntp.org>
 10 * Copyright (c) 1989-1994 by Frank Kardel, Friedrich-Alexander Universität Erlangen-Nürnberg, Germany
 11 *
 12 * Redistribution and use in source and binary forms, with or without
 13 * modification, are permitted provided that the following conditions
 14 * are met:
 15 * 1. Redistributions of source code must retain the above copyright
 16 *    notice, this list of conditions and the following disclaimer.
 17 * 2. Redistributions in binary form must reproduce the above copyright
 18 *    notice, this list of conditions and the following disclaimer in the
 19 *    documentation and/or other materials provided with the distribution.
 20 * 3. Neither the name of the author nor the names of its contributors
 21 *    may be used to endorse or promote products derived from this software
 22 *    without specific prior written permission.
 23 *
 24 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
 25 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 26 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 27 * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
 28 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 29 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 30 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 31 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 33 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 34 * SUCH DAMAGE.
 35 *
 36 */
 37
 38#ifdef HAVE_CONFIG_H
 39# include <config.h>
 40#endif
 41
 42#if defined(REFCLOCK) && defined(CLOCK_PARSE) && defined(CLOCK_TRIMTSIP)
 43
 44#include "ntp_syslog.h"
 45#include "ntp_types.h"
 46#include "ntp_fp.h"
 47#include "ntp_unixtime.h"
 48#include "ntp_calendar.h"
 49#include "ntp_machine.h"
 50#include "ntp_stdlib.h"
 51
 52#include "parse.h"
 53
 54#ifndef PARSESTREAM
 55# include <stdio.h>
 56#else
 57# include "sys/parsestreams.h"
 58#endif
 59
 60#include "ascii.h"
 61#include "binio.h"
 62#include "ieee754io.h"
 63#include "trimble.h"
 64
 65/*
 66 * Trimble low level TSIP parser / time converter
 67 *
 68 * The receiver uses a serial message protocol called Trimble Standard
 69 * Interface Protocol (it can support others but this driver only supports
 70 * TSIP). Messages in this protocol have the following form:
 71 *
 72 * <DLE><id> ... <data> ... <DLE><ETX>
 73 *
 74 * Any bytes within the <data> portion of value 10 hex (<DLE>) are doubled
 75 * on transmission and compressed back to one on reception. Otherwise
 76 * the values of data bytes can be anything. The serial interface is RS-422
 77 * asynchronous using 9600 baud, 8 data bits with odd party (**note** 9 bits
 78 * in total!), and 1 stop bit. The protocol supports byte, integer, single,
 79 * and double datatypes. Integers are two bytes, sent most significant first.
 80 * Singles are IEEE754 single precision floating point numbers (4 byte) sent
 81 * sign & exponent first. Doubles are IEEE754 double precision floating point
 82 * numbers (8 byte) sent sign & exponent first.
 83 * The receiver supports a large set of messages, only a very small subset of
 84 * which is used here.
 85 *
 86 * From this module the following are recognised:
 87 *
 88 *  ID    Description
 89 *
 90 *  41    GPS Time
 91 *  46    Receiver health
 92 *  4F    UTC correction data (used to get leap second warnings)
 93 *
 94 * All others are accepted but ignored for time conversion - they are passed up to higher layers.
 95 *
 96 */
 97
 98static offsets_t trim_offsets = { 0, 1, 2, 3, 4, 5, 6, 7 };
 99
100struct trimble
101{
102	u_char  t_in_pkt;	/* first DLE received */
103	u_char  t_dle;		/* subsequent DLE received */
104	u_short t_week;		/* GPS week */
105	u_short t_weekleap;	/* GPS week of next/last week */
106	u_short t_dayleap;	/* day in week */
107	u_short t_gpsutc;	/* GPS - UTC offset */
108	u_short t_gpsutcleap;	/* offset at next/last leap */
109	u_char  t_operable;	/* receiver feels OK */
110	u_char  t_mode;		/* actual operating mode */
111	u_char  t_leap;		/* possible leap warning */
112        u_char  t_utcknown;	/* utc offset known */
113};
114
115#define STATUS_BAD    0		/* BAD or UNINITIALIZED receiver status */
116#define STATUS_UNSAFE 1		/* not enough receivers for full precision */
117#define STATUS_SYNC   2		/* enough information for good operation */
118
119static unsigned long inp_tsip P((parse_t *, unsigned int, timestamp_t *));
120static unsigned long cvt_trimtsip P((unsigned char *, int, struct format *, clocktime_t *, void *));
121
122struct clockformat clock_trimtsip =
123{
124	inp_tsip,		/* Trimble TSIP input handler */
125	cvt_trimtsip,		/* Trimble TSIP conversion */
126	pps_one,		/* easy PPS monitoring */
127	0,			/* no configuration data */
128	"Trimble TSIP",
129	400,			/* input buffer */
130	sizeof(struct trimble)	/* private data */
131};
132
133#define ADDSECOND	0x01
134#define DELSECOND	0x02
135
136static unsigned long
137inp_tsip(
138	 parse_t      *parseio,
139	 unsigned int ch,
140	 timestamp_t  *tstamp
141	)
142{
143	struct trimble *t = (struct trimble *)parseio->parse_pdata;
144
145	if (!t)
146	    return PARSE_INP_SKIP;		/* local data not allocated - sigh! */
147
148	if (!t->t_in_pkt && ch != DLE) {
149		/* wait for start of packet */
150		return PARSE_INP_SKIP;
151	}
152
153	if ((parseio->parse_index >= (parseio->parse_dsize - 2)) ||
154	    (parseio->parse_dtime.parse_msglen >= (sizeof(parseio->parse_dtime.parse_msg) - 2)))
155		{		/* OVERFLOW - DROP! */
156			t->t_in_pkt = t->t_dle = 0;
157			parseio->parse_index = 0;
158			parseio->parse_dtime.parse_msglen = 0;
159		return PARSE_INP_SKIP;
160	}
161
162	switch (ch) {
163	    case DLE:
164		if (!t->t_in_pkt) {
165			t->t_dle = 0;
166			t->t_in_pkt = 1;
167			parseio->parse_index = 0;
168			parseio->parse_data[parseio->parse_index++] = ch;
169			parseio->parse_dtime.parse_msglen = 0;
170			parseio->parse_dtime.parse_msg[parseio->parse_dtime.parse_msglen++] = ch;
171			parseio->parse_dtime.parse_stime = *tstamp; /* pick up time stamp at packet start */
172		} else if (t->t_dle) {
173			/* Double DLE -> insert a DLE */
174			t->t_dle = 0;
175			parseio->parse_data[parseio->parse_index++] = DLE;
176			parseio->parse_dtime.parse_msg[parseio->parse_dtime.parse_msglen++] = DLE;
177		} else
178		    t->t_dle = 1;
179		break;
180
181	    case ETX:
182		if (t->t_dle) {
183			/* DLE,ETX -> end of packet */
184			parseio->parse_data[parseio->parse_index++] = DLE;
185			parseio->parse_data[parseio->parse_index] = ch;
186			parseio->parse_ldsize = parseio->parse_index+1;
187			memcpy(parseio->parse_ldata, parseio->parse_data, parseio->parse_ldsize);
188			parseio->parse_dtime.parse_msg[parseio->parse_dtime.parse_msglen++] = DLE;
189			parseio->parse_dtime.parse_msg[parseio->parse_dtime.parse_msglen++] = ch;
190			t->t_in_pkt = t->t_dle = 0;
191			return PARSE_INP_TIME|PARSE_INP_DATA;
192		}
193
194	    default:		/* collect data */
195		t->t_dle = 0;
196		parseio->parse_data[parseio->parse_index++] = ch;
197		parseio->parse_dtime.parse_msg[parseio->parse_dtime.parse_msglen++] = ch;
198	}
199
200  return PARSE_INP_SKIP;
201}
202     
203static int
204getshort(
205	 unsigned char *p
206	 )
207{
208	return get_msb_short(&p);
209}
210
211/*
212 * cvt_trimtsip
213 *
214 * convert TSIP type format
215 */
216static unsigned long
217cvt_trimtsip(
218	     unsigned char *buffer,
219	     int            size,
220	     struct format *format,
221	     clocktime_t   *clock_time,
222	     void          *local
223	     )
224{
225        register struct trimble *t = (struct trimble *)local; /* get local data space */
226#define mb(_X_) (buffer[2+(_X_)]) /* shortcut for buffer access */
227	register u_char cmd;
228
229	clock_time->flags = 0;
230
231	if (!t) {
232		return CVT_NONE;		/* local data not allocated - sigh! */
233	}
234
235	if ((size < 4) ||
236	    (buffer[0]      != DLE) ||
237	    (buffer[size-1] != ETX) ||
238	    (buffer[size-2] != DLE))
239	{
240		printf("TRIMBLE BAD packet, size %d:\n", size);
241		return CVT_NONE;
242	}
243	else
244	{
245		unsigned char *bp;
246		cmd = buffer[1];
247      
248		    switch(cmd)
249		    {
250		    case CMD_RCURTIME:
251			    {			/* GPS time */
252				    l_fp secs;
253				    int   week = getshort((unsigned char *)&mb(4));
254				    l_fp utcoffset;
255				    l_fp gpstime;
256
257				    bp = &mb(0);
258				    if (fetch_ieee754(&bp, IEEE_SINGLE, &secs, trim_offsets) != IEEE_OK)
259					    return CVT_FAIL|CVT_BADFMT;
260				    
261				    if ((secs.l_i <= 0) ||
262					(t->t_utcknown == 0))
263				    {
264					    clock_time->flags = PARSEB_POWERUP;
265					    return CVT_OK;
266				    }
267				    if (week < 990) {
268					    week += 1024;
269				    }
270
271				    /* time OK */
272
273				    /* fetch UTC offset */
274				    bp = &mb(6);
275				    if (fetch_ieee754(&bp, IEEE_SINGLE, &utcoffset, trim_offsets) != IEEE_OK)
276					    return CVT_FAIL|CVT_BADFMT;
277				    
278				    L_SUB(&secs, &utcoffset); /* adjust GPS time to UTC time */
279
280				    gpstolfp((unsigned short)week, (unsigned short)0,
281					     secs.l_ui, &gpstime);
282
283				    gpstime.l_uf = secs.l_uf;
284
285				    clock_time->utctime = gpstime.l_ui - JAN_1970;
286
287				    TSFTOTVU(gpstime.l_uf, clock_time->usecond);
288
289				    if (t->t_leap == ADDSECOND)
290					clock_time->flags |= PARSEB_LEAPADD;
291		      
292				    if (t->t_leap == DELSECOND)
293					clock_time->flags |= PARSEB_LEAPDEL;
294	
295				    switch (t->t_operable)
296				      {
297				      case STATUS_SYNC:
298					clock_time->flags &= ~(PARSEB_POWERUP|PARSEB_NOSYNC);
299					break;
300
301				      case STATUS_UNSAFE:
302					clock_time->flags |= PARSEB_NOSYNC;
303					break;
304
305				      case STATUS_BAD:
306					clock_time->flags |= PARSEB_NOSYNC|PARSEB_POWERUP;
307					break;
308				      }
309					
310				    if (t->t_mode == 0)
311					    clock_time->flags |= PARSEB_POSITION;
312				    
313				    clock_time->flags |= PARSEB_S_LEAP|PARSEB_S_POSITION;
314				    
315				    return CVT_OK;
316
317			    } /* case 0x41 */
318
319		    case CMD_RRECVHEALTH:
320			    {
321				    /* TRIMBLE health */
322				    u_char status = mb(0);
323
324				    switch (status)
325				    {
326				      case 0x00: /* position fixes */
327					t->t_operable = STATUS_SYNC;
328					break;
329
330				      case 0x09: /* 1 satellite */
331				      case 0x0A: /* 2 satellites */
332				      case 0x0B: /* 3 satellites */
333					t->t_operable = STATUS_UNSAFE;
334					break;
335
336				      default:
337					t->t_operable = STATUS_BAD;
338					break;
339				    }
340				    t->t_mode = status;
341			    }
342			    break;
343
344		    case CMD_RUTCPARAM:
345			    {
346			            l_fp t0t;
347				    unsigned char *lbp;
348				    
349				    /* UTC correction data - derive a leap warning */
350				    int tls   = t->t_gpsutc     = getshort((unsigned char *)&mb(12)); /* current leap correction (GPS-UTC) */
351				    int tlsf  = t->t_gpsutcleap = getshort((unsigned char *)&mb(24)); /* new leap correction */
352
353				    t->t_weekleap   = getshort((unsigned char *)&mb(20)); /* week no of leap correction */
354				    if (t->t_weekleap < 990)
355				      t->t_weekleap += 1024;
356
357				    t->t_dayleap    = getshort((unsigned char *)&mb(22)); /* day in week of leap correction */
358				    t->t_week = getshort((unsigned char *)&mb(18)); /* current week no */
359				    if (t->t_week < 990)
360				      t->t_week += 1024;
361				    
362				    lbp = (unsigned char *)&mb(14); /* last update time */
363				    if (fetch_ieee754(&lbp, IEEE_SINGLE, &t0t, trim_offsets) != IEEE_OK)
364					    return CVT_FAIL|CVT_BADFMT;
365
366				    t->t_utcknown = t0t.l_ui != 0;
367				      
368				    if ((t->t_utcknown) && /* got UTC information */
369					(tlsf != tls)   && /* something will change */
370					((t->t_weekleap - t->t_week) < 5)) /* and close in the future */
371				    {
372					    /* generate a leap warning */
373					    if (tlsf > tls)
374						t->t_leap = ADDSECOND;
375					    else
376						t->t_leap = DELSECOND;
377				    }
378				    else
379				    {
380					    t->t_leap = 0;
381				    }
382			    }
383			    break;
384
385		    default:
386			    /* it's validly formed, but we don't care about it! */
387			    break;
388		}
389	}
390	return CVT_SKIP;
391}
392
393#else /* not (REFCLOCK && CLOCK_PARSE && CLOCK_TRIMTSIP && !PARSESTREAM) */
394int clk_trimtsip_bs;
395#endif /* not (REFCLOCK && CLOCK_PARSE && CLOCK_TRIMTSIP && !PARSESTREAM) */
396
397/*
398 * History:
399 *
400 * clk_trimtsip.c,v
401 * Revision 4.17  2005/04/16 17:32:10  kardel
402 * update copyright
403 *
404 * Revision 4.16  2004/11/14 15:29:41  kardel
405 * support PPSAPI, upgrade Copyright to Berkeley style
406 *
407 * Revision 4.13  1999/11/28 09:13:51  kardel
408 * RECON_4_0_98F
409 *
410 * Revision 4.12  1999/02/28 13:00:08  kardel
411 * *** empty log message ***
412 *
413 * Revision 4.11  1999/02/28 11:47:54  kardel
414 * (struct trimble): new member t_utcknown
415 * (cvt_trimtsip): fixed status monitoring, bad receiver states are
416 * now recognized
417 *
418 * Revision 4.10  1999/02/27 15:57:15  kardel
419 * use mmemcpy instead of bcopy
420 *
421 * Revision 4.9  1999/02/21 12:17:42  kardel
422 * 4.91f reconcilation
423 *
424 * Revision 4.8  1998/11/15 20:27:58  kardel
425 * Release 4.0.73e13 reconcilation
426 *
427 * Revision 4.7  1998/08/16 18:49:20  kardel
428 * (cvt_trimtsip): initial kernel capable version (no more floats)
429 * (clock_trimtsip =): new format name
430 *
431 * Revision 4.6  1998/08/09 22:26:05  kardel
432 * Trimble TSIP support
433 *
434 * Revision 4.5  1998/08/02 10:37:05  kardel
435 * working TSIP parser
436 *
437 * Revision 4.4  1998/06/28 16:50:40  kardel
438 * (getflt): fixed ENDIAN issue
439 * (getdbl): fixed ENDIAN issue
440 * (getint): use get_msb_short()
441 * (cvt_trimtsip): use gpstolfp() for conversion
442 *
443 * Revision 4.3  1998/06/13 12:07:31  kardel
444 * fix SYSV clock name clash
445 *
446 * Revision 4.2  1998/06/12 15:22:30  kardel
447 * fix prototypes
448 *
449 * Revision 4.1  1998/05/24 09:39:54  kardel
450 * implementation of the new IO handling model
451 *
452 * Revision 4.0  1998/04/10 19:45:32  kardel
453 * Start 4.0 release version numbering
454 *
455 * from V3 1.8 loginfo deleted 1998/04/11 kardel
456 */