/contrib/ntp/libparse/data_mbg.c

https://bitbucket.org/freebsd/freebsd-head/ · C · 538 lines · 416 code · 59 blank · 63 comment · 24 complexity · 8a96b638b90290b8f35977f66b5fde89 MD5 · raw file

  1. /*
  2. * /src/NTP/REPOSITORY/ntp4-dev/libparse/data_mbg.c,v 4.8 2006/06/22 18:40:01 kardel RELEASE_20060622_A
  3. *
  4. * data_mbg.c,v 4.8 2006/06/22 18:40:01 kardel RELEASE_20060622_A
  5. *
  6. * $Created: Sun Jul 20 12:08:14 1997 $
  7. *
  8. * Copyright (c) 1997-2005 by Frank Kardel <kardel <AT> ntp.org>
  9. *
  10. * Redistribution and use in source and binary forms, with or without
  11. * modification, are permitted provided that the following conditions
  12. * are met:
  13. * 1. Redistributions of source code must retain the above copyright
  14. * notice, this list of conditions and the following disclaimer.
  15. * 2. Redistributions in binary form must reproduce the above copyright
  16. * notice, this list of conditions and the following disclaimer in the
  17. * documentation and/or other materials provided with the distribution.
  18. * 3. Neither the name of the author nor the names of its contributors
  19. * may be used to endorse or promote products derived from this software
  20. * without specific prior written permission.
  21. *
  22. * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
  23. * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  24. * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  25. * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  26. * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  27. * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  28. * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  29. * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  30. * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  31. * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  32. * SUCH DAMAGE.
  33. *
  34. */
  35. #ifdef PARSESTREAM
  36. #define NEED_BOPS
  37. #include "ntp_string.h"
  38. #else
  39. #include <stdio.h>
  40. #endif
  41. #include "ntp_types.h"
  42. #include "ntp_stdlib.h"
  43. #include "ntp_fp.h"
  44. #include "mbg_gps166.h"
  45. #include "binio.h"
  46. #include "ieee754io.h"
  47. static void get_mbg_tzname P((unsigned char **, char *));
  48. static void mbg_time_status_str P((char **, unsigned int, int));
  49. #if 0 /* no actual floats on Meinberg binary interface */
  50. static offsets_t mbg_float = { 1, 0, 3, 2, 0, 0, 0, 0 }; /* byte order for meinberg floats */
  51. #endif
  52. static offsets_t mbg_double = { 1, 0, 3, 2, 5, 4, 7, 6 }; /* byte order for meinberg doubles */
  53. static int32 rad2deg_i = 57;
  54. static u_int32 rad2deg_f = 0x4BB834C7; /* 57.2957795131 == 180/PI */
  55. void
  56. put_mbg_header(
  57. unsigned char **bufpp,
  58. GPS_MSG_HDR *headerp
  59. )
  60. {
  61. put_lsb_short(bufpp, headerp->gps_cmd);
  62. put_lsb_short(bufpp, headerp->gps_len);
  63. put_lsb_short(bufpp, headerp->gps_data_csum);
  64. put_lsb_short(bufpp, headerp->gps_hdr_csum);
  65. }
  66. void
  67. get_mbg_sw_rev(
  68. unsigned char **bufpp,
  69. SW_REV *sw_revp
  70. )
  71. {
  72. sw_revp->code = get_lsb_short(bufpp);
  73. memcpy(sw_revp->name, *bufpp, sizeof(sw_revp->name));
  74. *bufpp += sizeof(sw_revp->name);
  75. }
  76. void
  77. get_mbg_ascii_msg(
  78. unsigned char **bufpp,
  79. ASCII_MSG *ascii_msgp
  80. )
  81. {
  82. ascii_msgp->csum = get_lsb_short(bufpp);
  83. ascii_msgp->valid = get_lsb_short(bufpp);
  84. memcpy(ascii_msgp->s, *bufpp, sizeof(ascii_msgp->s));
  85. *bufpp += sizeof(ascii_msgp->s);
  86. }
  87. void
  88. get_mbg_svno(
  89. unsigned char **bufpp,
  90. SVNO *svnop
  91. )
  92. {
  93. *svnop = get_lsb_short(bufpp);
  94. }
  95. void
  96. get_mbg_health(
  97. unsigned char **bufpp,
  98. HEALTH *healthp
  99. )
  100. {
  101. *healthp = get_lsb_short(bufpp);
  102. }
  103. void
  104. get_mbg_cfg(
  105. unsigned char **bufpp,
  106. CFG *cfgp
  107. )
  108. {
  109. *cfgp = get_lsb_short(bufpp);
  110. }
  111. void
  112. get_mbg_tgps(
  113. unsigned char **bufpp,
  114. T_GPS *tgpsp
  115. )
  116. {
  117. tgpsp->wn = get_lsb_short(bufpp);
  118. tgpsp->sec = get_lsb_long(bufpp);
  119. tgpsp->tick = get_lsb_long(bufpp);
  120. }
  121. void
  122. get_mbg_tm(
  123. unsigned char **buffpp,
  124. TM *tmp
  125. )
  126. {
  127. tmp->year = get_lsb_short(buffpp);
  128. tmp->month = *(*buffpp)++;
  129. tmp->mday = *(*buffpp)++;
  130. tmp->yday = get_lsb_short(buffpp);
  131. tmp->wday = *(*buffpp)++;
  132. tmp->hour = *(*buffpp)++;
  133. tmp->minute = *(*buffpp)++;
  134. tmp->second = *(*buffpp)++;
  135. tmp->frac = get_lsb_long(buffpp);
  136. tmp->offs_from_utc = get_lsb_long(buffpp);
  137. tmp->status= get_lsb_short(buffpp);
  138. }
  139. void
  140. get_mbg_ttm(
  141. unsigned char **buffpp,
  142. TTM *ttmp
  143. )
  144. {
  145. ttmp->channel = get_lsb_short(buffpp);
  146. get_mbg_tgps(buffpp, &ttmp->t);
  147. get_mbg_tm(buffpp, &ttmp->tm);
  148. }
  149. void
  150. get_mbg_synth(
  151. unsigned char **buffpp,
  152. SYNTH *synthp
  153. )
  154. {
  155. synthp->freq = get_lsb_short(buffpp);
  156. synthp->range = get_lsb_short(buffpp);
  157. synthp->phase = get_lsb_short(buffpp);
  158. }
  159. static void
  160. get_mbg_tzname(
  161. unsigned char **buffpp,
  162. char *tznamep
  163. )
  164. {
  165. strncpy(tznamep, (char *)*buffpp, sizeof(TZ_NAME));
  166. *buffpp += sizeof(TZ_NAME);
  167. }
  168. void
  169. get_mbg_tzdl(
  170. unsigned char **buffpp,
  171. TZDL *tzdlp
  172. )
  173. {
  174. tzdlp->offs = get_lsb_long(buffpp);
  175. tzdlp->offs_dl = get_lsb_long(buffpp);
  176. get_mbg_tm(buffpp, &tzdlp->tm_on);
  177. get_mbg_tm(buffpp, &tzdlp->tm_off);
  178. get_mbg_tzname(buffpp, (char *)tzdlp->name[0]);
  179. get_mbg_tzname(buffpp, (char *)tzdlp->name[1]);
  180. }
  181. void
  182. get_mbg_antinfo(
  183. unsigned char **buffpp,
  184. ANT_INFO *antinfop
  185. )
  186. {
  187. antinfop->status = get_lsb_short(buffpp);
  188. get_mbg_tm(buffpp, &antinfop->tm_disconn);
  189. get_mbg_tm(buffpp, &antinfop->tm_reconn);
  190. antinfop->delta_t = get_lsb_long(buffpp);
  191. }
  192. static void
  193. mbg_time_status_str(
  194. char **buffpp,
  195. unsigned int status,
  196. int size
  197. )
  198. {
  199. static struct state
  200. {
  201. int flag; /* bit flag */
  202. const char *string; /* bit name */
  203. } states[] =
  204. {
  205. { TM_UTC, "UTC CORR" },
  206. { TM_LOCAL, "LOCAL TIME" },
  207. { TM_DL_ANN, "DST WARN" },
  208. { TM_DL_ENB, "DST" },
  209. { TM_LS_ANN, "LEAP WARN" },
  210. { TM_LS_ENB, "LEAP SEC" },
  211. { 0, "" }
  212. };
  213. if (status)
  214. {
  215. char *start, *p;
  216. struct state *s;
  217. start = p = *buffpp;
  218. for (s = states; s->flag; s++)
  219. {
  220. if (s->flag & status)
  221. {
  222. if (p != *buffpp)
  223. {
  224. strncpy(p, ", ", size - (p - start));
  225. p += 2;
  226. }
  227. strncpy(p, s->string, size - (p - start));
  228. p += strlen(p);
  229. }
  230. }
  231. *buffpp = p;
  232. }
  233. }
  234. void
  235. mbg_tm_str(
  236. char **buffpp,
  237. TM *tmp,
  238. int size
  239. )
  240. {
  241. char *s = *buffpp;
  242. snprintf(*buffpp, size, "%04d-%02d-%02d %02d:%02d:%02d.%07ld (%c%02d%02d) ",
  243. tmp->year, tmp->month, tmp->mday,
  244. tmp->hour, tmp->minute, tmp->second, tmp->frac,
  245. (tmp->offs_from_utc < 0) ? '-' : '+',
  246. abs(tmp->offs_from_utc) / 3600,
  247. (abs(tmp->offs_from_utc) / 60) % 60);
  248. *buffpp += strlen(*buffpp);
  249. mbg_time_status_str(buffpp, tmp->status, size - (*buffpp - s));
  250. }
  251. void
  252. mbg_tgps_str(
  253. char **buffpp,
  254. T_GPS *tgpsp,
  255. int size
  256. )
  257. {
  258. snprintf(*buffpp, size, "week %d + %ld days + %ld.%07ld sec",
  259. tgpsp->wn, tgpsp->sec / 86400,
  260. tgpsp->sec % 86400, tgpsp->tick);
  261. *buffpp += strlen(*buffpp);
  262. }
  263. void
  264. get_mbg_cfgh(
  265. unsigned char **buffpp,
  266. CFGH *cfghp
  267. )
  268. {
  269. int i;
  270. cfghp->csum = get_lsb_short(buffpp);
  271. cfghp->valid = get_lsb_short(buffpp);
  272. get_mbg_tgps(buffpp, &cfghp->tot_51);
  273. get_mbg_tgps(buffpp, &cfghp->tot_63);
  274. get_mbg_tgps(buffpp, &cfghp->t0a);
  275. for (i = MIN_SVNO; i <= MAX_SVNO; i++)
  276. {
  277. get_mbg_cfg(buffpp, &cfghp->cfg[i]);
  278. }
  279. for (i = MIN_SVNO; i <= MAX_SVNO; i++)
  280. {
  281. get_mbg_health(buffpp, &cfghp->health[i]);
  282. }
  283. }
  284. void
  285. get_mbg_utc(
  286. unsigned char **buffpp,
  287. UTC *utcp
  288. )
  289. {
  290. utcp->csum = get_lsb_short(buffpp);
  291. utcp->valid = get_lsb_short(buffpp);
  292. get_mbg_tgps(buffpp, &utcp->t0t);
  293. if (fetch_ieee754(buffpp, IEEE_DOUBLE, &utcp->A0, mbg_double) != IEEE_OK)
  294. {
  295. L_CLR(&utcp->A0);
  296. }
  297. if (fetch_ieee754(buffpp, IEEE_DOUBLE, &utcp->A1, mbg_double) != IEEE_OK)
  298. {
  299. L_CLR(&utcp->A1);
  300. }
  301. utcp->WNlsf = get_lsb_short(buffpp);
  302. utcp->DNt = get_lsb_short(buffpp);
  303. utcp->delta_tls = *(*buffpp)++;
  304. utcp->delta_tlsf = *(*buffpp)++;
  305. }
  306. void
  307. get_mbg_lla(
  308. unsigned char **buffpp,
  309. LLA lla
  310. )
  311. {
  312. int i;
  313. for (i = LAT; i <= ALT; i++)
  314. {
  315. if (fetch_ieee754(buffpp, IEEE_DOUBLE, &lla[i], mbg_double) != IEEE_OK)
  316. {
  317. L_CLR(&lla[i]);
  318. }
  319. else
  320. if (i != ALT)
  321. { /* convert to degrees (* 180/PI) */
  322. mfp_mul(&lla[i].l_i, &lla[i].l_uf, lla[i].l_i, lla[i].l_uf, rad2deg_i, rad2deg_f);
  323. }
  324. }
  325. }
  326. void
  327. get_mbg_xyz(
  328. unsigned char **buffpp,
  329. XYZ xyz
  330. )
  331. {
  332. int i;
  333. for (i = XP; i <= ZP; i++)
  334. {
  335. if (fetch_ieee754(buffpp, IEEE_DOUBLE, &xyz[i], mbg_double) != IEEE_OK)
  336. {
  337. L_CLR(&xyz[i]);
  338. }
  339. }
  340. }
  341. static void
  342. get_mbg_comparam(
  343. unsigned char **buffpp,
  344. COM_PARM *comparamp
  345. )
  346. {
  347. int i;
  348. comparamp->baud_rate = get_lsb_long(buffpp);
  349. for (i = 0; i < sizeof(comparamp->framing); i++)
  350. {
  351. comparamp->framing[i] = *(*buffpp)++;
  352. }
  353. comparamp->handshake = get_lsb_short(buffpp);
  354. }
  355. void
  356. get_mbg_portparam(
  357. unsigned char **buffpp,
  358. PORT_PARM *portparamp
  359. )
  360. {
  361. int i;
  362. for (i = 0; i < N_COM; i++)
  363. {
  364. get_mbg_comparam(buffpp, &portparamp->com[i]);
  365. }
  366. for (i = 0; i < N_COM; i++)
  367. {
  368. portparamp->mode[i] = *(*buffpp)++;
  369. }
  370. }
  371. #define FETCH_DOUBLE(src, addr) \
  372. if (fetch_ieee754(src, IEEE_DOUBLE, addr, mbg_double) != IEEE_OK) \
  373. { \
  374. L_CLR(addr); \
  375. }
  376. void
  377. get_mbg_eph(
  378. unsigned char ** buffpp,
  379. EPH *ephp
  380. )
  381. {
  382. ephp->csum = get_lsb_short(buffpp);
  383. ephp->valid = get_lsb_short(buffpp);
  384. ephp->health = get_lsb_short(buffpp);
  385. ephp->IODC = get_lsb_short(buffpp);
  386. ephp->IODE2 = get_lsb_short(buffpp);
  387. ephp->IODE3 = get_lsb_short(buffpp);
  388. get_mbg_tgps(buffpp, &ephp->tt);
  389. get_mbg_tgps(buffpp, &ephp->t0c);
  390. get_mbg_tgps(buffpp, &ephp->t0e);
  391. FETCH_DOUBLE(buffpp, &ephp->sqrt_A);
  392. FETCH_DOUBLE(buffpp, &ephp->e);
  393. FETCH_DOUBLE(buffpp, &ephp->M0);
  394. FETCH_DOUBLE(buffpp, &ephp->omega);
  395. FETCH_DOUBLE(buffpp, &ephp->OMEGA0);
  396. FETCH_DOUBLE(buffpp, &ephp->OMEGADOT);
  397. FETCH_DOUBLE(buffpp, &ephp->deltan);
  398. FETCH_DOUBLE(buffpp, &ephp->i0);
  399. FETCH_DOUBLE(buffpp, &ephp->idot);
  400. FETCH_DOUBLE(buffpp, &ephp->crc);
  401. FETCH_DOUBLE(buffpp, &ephp->crs);
  402. FETCH_DOUBLE(buffpp, &ephp->cuc);
  403. FETCH_DOUBLE(buffpp, &ephp->cus);
  404. FETCH_DOUBLE(buffpp, &ephp->cic);
  405. FETCH_DOUBLE(buffpp, &ephp->cis);
  406. FETCH_DOUBLE(buffpp, &ephp->af0);
  407. FETCH_DOUBLE(buffpp, &ephp->af1);
  408. FETCH_DOUBLE(buffpp, &ephp->af2);
  409. FETCH_DOUBLE(buffpp, &ephp->tgd);
  410. ephp->URA = get_lsb_short(buffpp);
  411. ephp->L2code = *(*buffpp)++;
  412. ephp->L2flag = *(*buffpp)++;
  413. }
  414. void
  415. get_mbg_alm(
  416. unsigned char **buffpp,
  417. ALM *almp
  418. )
  419. {
  420. almp->csum = get_lsb_short(buffpp);
  421. almp->valid = get_lsb_short(buffpp);
  422. almp->health = get_lsb_short(buffpp);
  423. get_mbg_tgps(buffpp, &almp->t0a);
  424. FETCH_DOUBLE(buffpp, &almp->sqrt_A);
  425. FETCH_DOUBLE(buffpp, &almp->e);
  426. FETCH_DOUBLE(buffpp, &almp->M0);
  427. FETCH_DOUBLE(buffpp, &almp->omega);
  428. FETCH_DOUBLE(buffpp, &almp->OMEGA0);
  429. FETCH_DOUBLE(buffpp, &almp->OMEGADOT);
  430. FETCH_DOUBLE(buffpp, &almp->deltai);
  431. FETCH_DOUBLE(buffpp, &almp->af0);
  432. FETCH_DOUBLE(buffpp, &almp->af1);
  433. }
  434. void
  435. get_mbg_iono(
  436. unsigned char **buffpp,
  437. IONO *ionop
  438. )
  439. {
  440. ionop->csum = get_lsb_short(buffpp);
  441. ionop->valid = get_lsb_short(buffpp);
  442. FETCH_DOUBLE(buffpp, &ionop->alpha_0);
  443. FETCH_DOUBLE(buffpp, &ionop->alpha_1);
  444. FETCH_DOUBLE(buffpp, &ionop->alpha_2);
  445. FETCH_DOUBLE(buffpp, &ionop->alpha_3);
  446. FETCH_DOUBLE(buffpp, &ionop->beta_0);
  447. FETCH_DOUBLE(buffpp, &ionop->beta_1);
  448. FETCH_DOUBLE(buffpp, &ionop->beta_2);
  449. FETCH_DOUBLE(buffpp, &ionop->beta_3);
  450. }
  451. /*
  452. * data_mbg.c,v
  453. * Revision 4.8 2006/06/22 18:40:01 kardel
  454. * clean up signedness (gcc 4)
  455. *
  456. * Revision 4.7 2005/10/07 22:11:10 kardel
  457. * bounded buffer implementation
  458. *
  459. * Revision 4.6.2.1 2005/09/25 10:23:06 kardel
  460. * support bounded buffers
  461. *
  462. * Revision 4.6 2005/04/16 17:32:10 kardel
  463. * update copyright
  464. *
  465. * Revision 4.5 2004/11/14 15:29:41 kardel
  466. * support PPSAPI, upgrade Copyright to Berkeley style
  467. *
  468. * Revision 4.3 1999/02/21 12:17:42 kardel
  469. * 4.91f reconcilation
  470. *
  471. * Revision 4.2 1998/06/14 21:09:39 kardel
  472. * Sun acc cleanup
  473. *
  474. * Revision 4.1 1998/05/24 08:02:06 kardel
  475. * trimmed version log
  476. *
  477. * Revision 4.0 1998/04/10 19:45:33 kardel
  478. * Start 4.0 release version numbering
  479. */