/contrib/ntp/libparse/clk_trimtsip.c

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