Many GPS radios provide both a 1-PPS and time of day information via
NMEA sentences. This patch introduces a ts2phc master that decodes
the "recommended minimum data" sentence, RMC, which provides UTC time
and a validity flag. Together with the file based leap second table,
this sentence provides adequate time of day for determining the time
of the PPS edge.
Signed-off-by: Richard Cochran <richardcoch...@gmail.com>
---
config.c | 3 +
makefile | 6 +-
nmea.c | 202 ++++++++++++++++++++++++++++++++++++++
nmea.h | 44 +++++++++
serial.c | 96 ++++++++++++++++++
serial.h | 19 ++++
sock.c | 58 +++++++++++
sock.h | 17 ++++
ts2phc.c | 3 +
ts2phc_master.c | 2 +
ts2phc_nmea_master.c | 227 +++++++++++++++++++++++++++++++++++++++++++
ts2phc_nmea_master.h | 13 +++
12 files changed, 687 insertions(+), 3 deletions(-)
create mode 100644 nmea.c
create mode 100644 nmea.h
create mode 100644 serial.c
create mode 100644 serial.h
create mode 100644 sock.c
create mode 100644 sock.h
create mode 100644 ts2phc_nmea_master.c
create mode 100644 ts2phc_nmea_master.h
diff --git a/config.c b/config.c
index b81149c..53ad788 100644
--- a/config.c
+++ b/config.c
@@ -309,6 +309,9 @@ struct config_item config_tab[] = {
PORT_ITEM_INT("ts2phc.extts_correction", 0, INT_MIN, INT_MAX),
PORT_ITEM_ENU("ts2phc.extts_polarity", PTP_RISING_EDGE,
extts_polarity_enu),
PORT_ITEM_INT("ts2phc.master", 0, 0, 1),
+ GLOB_ITEM_STR("ts2phc.nmea_remote_host", ""),
+ GLOB_ITEM_STR("ts2phc.nmea_remote_port", ""),
+ GLOB_ITEM_STR("ts2phc.nmea_serialport", "/dev/ttyS0"),
PORT_ITEM_INT("ts2phc.pin_index", 0, 0, INT_MAX),
GLOB_ITEM_INT("ts2phc.pulsewidth", 500000000, 1000000, 999000000),
PORT_ITEM_ENU("tsproc_mode", TSPROC_FILTER, tsproc_enu),
diff --git a/makefile b/makefile
index 17f2e55..acc94f7 100644
--- a/makefile
+++ b/makefile
@@ -21,13 +21,13 @@ DEBUG =
CC = $(CROSS_COMPILE)gcc
VER = -DVER=$(version)
CFLAGS = -Wall $(VER) $(incdefs) $(DEBUG) $(EXTRA_CFLAGS)
-LDLIBS = -lm -lrt $(EXTRA_LDFLAGS)
+LDLIBS = -lm -lrt -pthread $(EXTRA_LDFLAGS)
PRG = ptp4l hwstamp_ctl nsm phc2sys phc_ctl pmc timemaster ts2phc
FILTERS = filter.o mave.o mmedian.o
SERVOS = linreg.o ntpshm.o nullf.o pi.o servo.o
TRANSP = raw.o transport.o udp.o udp6.o uds.o
-TS2PHC = ts2phc.o ts2phc_generic_master.o ts2phc_master.o ts2phc_phc_master.o \
- ts2phc_slave.o
+TS2PHC = ts2phc.o lstab.o nmea.o serial.o sock.o ts2phc_generic_master.o \
+ ts2phc_master.o ts2phc_phc_master.o ts2phc_nmea_master.o ts2phc_slave.o
OBJ = bmc.o clock.o clockadj.o clockcheck.o config.o designated_fsm.o \
e2e_tc.o fault.o $(FILTERS) fsm.o hash.o interface.o msg.o phc.o port.o \
port_signaling.o pqueue.o print.o ptp4l.o p2p_tc.o rtnl.o $(SERVOS) sk.o \
diff --git a/nmea.c b/nmea.c
new file mode 100644
index 0000000..dc865d0
--- /dev/null
+++ b/nmea.c
@@ -0,0 +1,202 @@
+/**
+ * @file nmea.c
+ * @note Copyright (C) 2020 Richard Cochran <richardcoch...@gmail.com>
+ * @note SPDX-License-Identifier: GPL-2.0+
+ */
+#include <malloc.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "nmea.h"
+#include "print.h"
+
+#define NMEA_CHAR_MIN ' '
+#define NMEA_CHAR_MAX '~'
+#define NMEA_MAX_LENGTH 256
+
+enum nmea_state {
+ NMEA_IDLE,
+ NMEA_HAVE_DOLLAR,
+ NMEA_HAVE_STARTG,
+ NMEA_HAVE_STARTX,
+ NMEA_HAVE_BODY,
+ NMEA_HAVE_CSUMA,
+ NMEA_HAVE_CSUM_MSB,
+ NMEA_HAVE_CSUM_LSB,
+ NMEA_HAVE_PENULTIMATE,
+};
+
+struct nmea_parser {
+ char sentence[NMEA_MAX_LENGTH + 1];
+ char payload_checksum[3];
+ enum nmea_state state;
+ uint8_t checksum;
+ int offset;
+};
+
+static void nmea_reset(struct nmea_parser *np);
+
+static void nmea_accumulate(struct nmea_parser *np, char c)
+{
+ if (c < NMEA_CHAR_MIN || c > NMEA_CHAR_MAX) {
+ nmea_reset(np);
+ return;
+ }
+ if (np->offset == NMEA_MAX_LENGTH) {
+ nmea_reset(np);
+ }
+ np->sentence[np->offset++] = c;
+ np->checksum ^= c;
+}
+
+static int nmea_parse_symbol(struct nmea_parser *np, char c)
+{
+ switch (np->state) {
+ case NMEA_IDLE:
+ if (c == '$') {
+ np->state = NMEA_HAVE_DOLLAR;
+ }
+ break;
+ case NMEA_HAVE_DOLLAR:
+ if (c == 'G') {
+ np->state = NMEA_HAVE_STARTG;
+ nmea_accumulate(np, c);
+ } else {
+ nmea_reset(np);
+ }
+ break;
+ case NMEA_HAVE_STARTG:
+ np->state = NMEA_HAVE_STARTX;
+ nmea_accumulate(np, c);
+ break;
+ case NMEA_HAVE_STARTX:
+ np->state = NMEA_HAVE_BODY;
+ nmea_accumulate(np, c);
+ break;
+ case NMEA_HAVE_BODY:
+ if (c == '*') {
+ np->state = NMEA_HAVE_CSUMA;
+ } else {
+ nmea_accumulate(np, c);
+ }
+ break;
+ case NMEA_HAVE_CSUMA:
+ np->state = NMEA_HAVE_CSUM_MSB;
+ np->payload_checksum[0] = c;
+ break;
+ case NMEA_HAVE_CSUM_MSB:
+ np->state = NMEA_HAVE_CSUM_LSB;
+ np->payload_checksum[1] = c;
+ break;
+ case NMEA_HAVE_CSUM_LSB:
+ if (c == '\n') {
+ /*skip the CR*/
+ return 0;
+ }
+ if (c == '\r') {
+ np->state = NMEA_HAVE_PENULTIMATE;
+ } else {
+ nmea_reset(np);
+ }
+ break;
+ case NMEA_HAVE_PENULTIMATE:
+ if (c == '\n') {
+ return 0;
+ }
+ nmea_reset(np);
+ break;
+ }
+ return -1;
+}
+
+static void nmea_reset(struct nmea_parser *np)
+{
+ memset(np, 0, sizeof(*np));
+}
+
+static int nmea_scan_rmc(struct nmea_parser *np, struct nmea_rmc *result)
+{
+ int cnt, i, msec = 0;
+ char *ptr, status;
+ uint8_t checksum;
+ struct tm tm;
+
+ pr_debug("nmea sentence: %s", np->sentence);
+ cnt = sscanf(np->payload_checksum, "%02hhx", &checksum);
+ if (cnt != 1) {
+ return -1;
+ }
+ if (checksum != np->checksum) {
+ pr_err("checksum mismatch 0x%02hhx != 0x%02hhx on %s",
+ checksum, np->checksum, np->sentence);
+ return -1;
+ }
+ cnt = sscanf(np->sentence,
+ "G%*cRMC,%2d%2d%2d.%d,%c",
+ &tm.tm_hour, &tm.tm_min, &tm.tm_sec, &msec, &status);
+ if (cnt != 5) {
+ cnt = sscanf(np->sentence,
+ "G%*cRMC,%2d%2d%2d,%c",
+ &tm.tm_hour, &tm.tm_min, &tm.tm_sec, &status);
+ if (cnt != 4) {
+ return -1;
+ }
+ }
+ ptr = np->sentence;
+ for (i = 0; i < 9; i++) {
+ ptr = strchr(ptr, ',');
+ if (!ptr) {
+ return -1;
+ }
+ ptr++;
+ }
+ cnt = sscanf(ptr, "%2d%2d%2d", &tm.tm_mday, &tm.tm_mon, &tm.tm_year);
+ if (cnt != 3) {
+ return -1;
+ }
+ tm.tm_year += 100;
+ tm.tm_mon--;
+ result->ts.tv_sec = mktime(&tm);
+ result->ts.tv_nsec = msec * 1000000UL;
+ result->fix_valid = status == 'A' ? true : false;
+ return 0;
+}
+
+int nmea_parse(struct nmea_parser *np, const char *ptr, int buflen,
+ struct nmea_rmc *result, int *parsed)
+{
+ int count = 0;
+ while (buflen) {
+ if (!nmea_parse_symbol(np, *ptr)) {
+ if (!nmea_scan_rmc(np, result)) {
+ *parsed = count + 1;
+ return 0;
+ }
+ nmea_reset(np);
+ }
+ buflen--;
+ count++;
+ ptr++;
+ }
+ *parsed = count;
+ return -1;
+}
+
+struct nmea_parser *nmea_parser_create(void)
+{
+ struct nmea_parser *np;
+ np = malloc(sizeof(*np));
+ if (!np) {
+ return NULL;
+ }
+ nmea_reset(np);
+ /* Ensure that mktime(3) returns a value in the UTC time scale. */
+ setenv("TZ", "UTC", 1);
+ return np;
+}
+
+void nmea_parser_destroy(struct nmea_parser *np)
+{
+ free(np);
+}
diff --git a/nmea.h b/nmea.h
new file mode 100644
index 0000000..4f0c152
--- /dev/null
+++ b/nmea.h
@@ -0,0 +1,44 @@
+/**
+ * @file nmea.h
+ * @note Copyright (C) 2020 Richard Cochran <richardcoch...@gmail.com>
+ * @note SPDX-License-Identifier: GPL-2.0+
+ */
+#ifndef HAVE_NMEA_H
+#define HAVE_NMEA_H
+
+#include <stdbool.h>
+#include <time.h>
+
+/** Opaque type. */
+struct nmea_parser;
+
+struct nmea_rmc {
+ struct timespec ts;
+ bool fix_valid;
+};
+
+/**
+ * Parses NMEA RMC sentences out of a given buffer.
+ * @param np Pointer obtained via nmea_parser_create().
+ * @param buf Pointer to the data to be parsed.
+ * @param buflen Length of 'buf' in bytes.
+ * @param rmc Pointer to hold the result.
+ * @param parsed Returns the number of bytes parsed, possibly less than
buflen.
+ * @return Zero on success, non-zero otherwise.
+ */
+int nmea_parse(struct nmea_parser *np, const char *buf, int buflen,
+ struct nmea_rmc *rmc, int *parsed);
+
+/**
+ * Creates an instance of an NMEA parser.
+ * @return Pointer to a new instance on success, NULL otherwise.
+ */
+struct nmea_parser *nmea_parser_create(void);
+
+/**
+ * Destroys an NMEA parser instance.
+ * @param np Pointer obtained via nmea_parser_create().
+ */
+void nmea_parser_destroy(struct nmea_parser *np);
+
+#endif
diff --git a/serial.c b/serial.c
new file mode 100644
index 0000000..29cef0c
--- /dev/null
+++ b/serial.c
@@ -0,0 +1,96 @@
+/**
+ * @file serial.c
+ * @note Copyright (C) 2020 Richard Cochran <richardcoch...@gmail.com>
+ * @note SPDX-License-Identifier: GPL-2.0+
+ */
+#include <fcntl.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <termios.h>
+
+#include "print.h"
+#include "serial.h"
+
+#define CANONICAL 1
+
+static int open_serial_baud(const char *name, tcflag_t baud, int icrnl, int
hwfc)
+{
+ struct termios nterm;
+ int fd;
+
+ fd = open(name, O_RDWR | O_NOCTTY);
+ if (fd < 0) {
+ pr_err("cannot open %s : %m", name);
+ return fd;
+ }
+ memset(&nterm, 0, sizeof(nterm));
+
+ /* Input Modes */
+ nterm.c_iflag = IGNPAR; /* Ignore framing errors and parity errors */
+ if (icrnl) {
+ /* Translate carriage return to newline on input */
+ nterm.c_iflag |= ICRNL;
+ }
+
+ /* Output Modes */
+ nterm.c_oflag = 0;
+
+ /* Control Modes */
+ nterm.c_cflag = baud;
+ nterm.c_cflag |= CS8; /* Character size */
+ nterm.c_cflag |= CLOCAL; /* Ignore modem control lines */
+ nterm.c_cflag |= CREAD; /* Enable receiver */
+ if (hwfc) {
+ /* Enable RTS/CTS (hardware) flow control */
+ nterm.c_cflag |= CRTSCTS;
+ }
+
+ /* Local Modes */
+ if (CANONICAL) {
+ nterm.c_lflag = ICANON; /* Enable canonical mode */
+ }
+
+ nterm.c_cc[VTIME] = 10; /* timeout is 10 deciseconds */
+ nterm.c_cc[VMIN] = 1; /* blocking read until N chars received */
+ tcflush(fd, TCIFLUSH);
+ tcsetattr(fd, TCSANOW, &nterm);
+ return fd;
+}
+
+int serial_open(const char *name, int bps, int icrnl, int hwfc)
+{
+ tcflag_t baud;
+
+ switch (bps) {
+ case 1200:
+ baud = B1200;
+ break;
+ case 1800:
+ baud = B1800;
+ break;
+ case 2400:
+ baud = B2400;
+ break;
+ case 4800:
+ baud = B4800;
+ break;
+ case 9600:
+ baud = B9600;
+ break;
+ case 19200:
+ baud = B19200;
+ break;
+ case 38400:
+ baud = B38400;
+ break;
+ case 57600:
+ baud = B57600;
+ break;
+ case 115200:
+ baud = B115200;
+ break;
+ default:
+ return -1;
+ }
+ return open_serial_baud(name, baud, icrnl, hwfc);
+}
diff --git a/serial.h b/serial.h
new file mode 100644
index 0000000..6a7d8a1
--- /dev/null
+++ b/serial.h
@@ -0,0 +1,19 @@
+/**
+ * @file serial.h
+ * @note Copyright (C) 2020 Richard Cochran <richardcoch...@gmail.com>
+ * @note SPDX-License-Identifier: GPL-2.0+
+ */
+#ifndef HAVE_SERIAL_H
+#define HAVE_SERIAL_H
+
+/**
+ * Opens a serial port device.
+ * @param name Serial port device to open.
+ * @param bps Baud rate in bits per second.
+ * @param icrnl Pass 1 to map CR to NL on input, zero otherwise.
+ * @param hwfc Pass 1 to enable hardware flow control, zero otherwise.
+ * @return An open file descriptor on success, -1 otherwise.
+ */
+int serial_open(const char *name, int bps, int icrnl, int hwfc);
+
+#endif
diff --git a/sock.c b/sock.c
new file mode 100644
index 0000000..89b3d71
--- /dev/null
+++ b/sock.c
@@ -0,0 +1,58 @@
+/**
+ * @file sock.c
+ * @note Copyright (C) 2020 Richard Cochran <richardcoch...@gmail.com>
+ * @note SPDX-License-Identifier: GPL-2.0+
+ */
+#include <netdb.h>
+#include <netinet/tcp.h>
+#include <string.h>
+#include <unistd.h>
+
+#include "print.h"
+#include "sock.h"
+
+typedef void *so_t;
+
+int sock_open(const char *server, const char *port)
+{
+ int i, err, family[2] = { AF_INET, AF_INET6 }, fd;
+ struct addrinfo hints, *result = NULL;
+ socklen_t addrlen;
+
+ memset(&hints, 0, sizeof(hints));
+ hints.ai_flags = AI_CANONNAME;
+ hints.ai_socktype = SOCK_STREAM;
+
+ for (i = 0; i < 2; i++) {
+ hints.ai_family = family[i];
+ err = getaddrinfo(server, port, &hints, &result);
+ if (err) {
+ pr_debug("%s: getaddrinfo failed family %d: %s",
+ __func__, hints.ai_family, gai_strerror(err));
+ result = NULL;
+ } else {
+ break;
+ }
+ }
+ if (!result) {
+ return -1;
+ }
+
+ addrlen = (socklen_t) result->ai_addrlen;
+ pr_debug("%s: connecting to server %s canonical %s",
+ __func__, server, result->ai_canonname);
+
+ fd = socket(result->ai_family, SOCK_STREAM, result->ai_protocol);
+ if (fd < 0) {
+ pr_err("%s: socket failed: %m", __func__);
+ goto failed;
+ }
+ if (connect(fd, result->ai_addr, addrlen) < 0) {
+ pr_err("%s: connect failed: %m", __func__);
+ close(fd);
+ fd = -1;
+ }
+failed:
+ freeaddrinfo(result);
+ return fd;
+}
diff --git a/sock.h b/sock.h
new file mode 100644
index 0000000..fe5aa78
--- /dev/null
+++ b/sock.h
@@ -0,0 +1,17 @@
+/**
+ * @file sock.h
+ * @note Copyright (C) 2020 Richard Cochran <richardcoch...@gmail.com>
+ * @note SPDX-License-Identifier: GPL-2.0+
+ */
+#ifndef HAVE_SOCK_H
+#define HAVE_SOCK_H
+
+/**
+ * Opens a socket connected to a given remote address.
+ * @param server Host name or IP address of the server.
+ * @param port Port on the server with which to connect.
+ * @return An open file descriptor on success, -1 otherwise.
+ */
+int sock_open(const char *server, const char *port);
+
+#endif
diff --git a/ts2phc.c b/ts2phc.c
index 107b055..ac23f7f 100644
--- a/ts2phc.c
+++ b/ts2phc.c
@@ -47,6 +47,7 @@ static void usage(char *progname)
" generic - an external 1-PPS without ToD
information\n"
" /dev/ptp0 - a local PTP Hardware Clock
(PHC)\n"
" eth0 - a local PTP Hardware Clock
(PHC)\n"
+ " nmea - a gps device connected by
serial port or network\n"
" -v prints the software version and exits\n"
"\n",
progname);
@@ -183,6 +184,8 @@ int main(int argc, char *argv[])
if (!strcasecmp(pps_source, "generic")) {
pps_type = TS2PHC_MASTER_GENERIC;
+ } else if (!strcasecmp(pps_source, "nmea")) {
+ pps_type = TS2PHC_MASTER_NMEA;
} else {
pps_type = TS2PHC_MASTER_PHC;
}
diff --git a/ts2phc_master.c b/ts2phc_master.c
index 895a0f8..9283580 100644
--- a/ts2phc_master.c
+++ b/ts2phc_master.c
@@ -5,6 +5,7 @@
*/
#include "ts2phc_generic_master.h"
#include "ts2phc_master_private.h"
+#include "ts2phc_nmea_master.h"
#include "ts2phc_phc_master.h"
struct ts2phc_master *ts2phc_master_create(struct config *cfg, const char *dev,
@@ -17,6 +18,7 @@ struct ts2phc_master *ts2phc_master_create(struct config
*cfg, const char *dev,
master = ts2phc_generic_master_create(cfg, dev);
break;
case TS2PHC_MASTER_NMEA:
+ master = ts2phc_nmea_master_create(cfg, dev);
break;
case TS2PHC_MASTER_PHC:
master = ts2phc_phc_master_create(cfg, dev);
diff --git a/ts2phc_nmea_master.c b/ts2phc_nmea_master.c
new file mode 100644
index 0000000..2b9af3b
--- /dev/null
+++ b/ts2phc_nmea_master.c
@@ -0,0 +1,227 @@
+/**
+ * @file ts2phc_nmea_master.c
+ * @note Copyright (C) 2019 Richard Cochran <richardcoch...@gmail.com>
+ * @note SPDX-License-Identifier: GPL-2.0+
+ */
+#include <poll.h>
+#include <pthread.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "config.h"
+#include "lstab.h"
+#include "missing.h"
+#include "nmea.h"
+#include "print.h"
+#include "serial.h"
+#include "sock.h"
+#include "tmv.h"
+#include "ts2phc_master_private.h"
+#include "ts2phc_nmea_master.h"
+#include "util.h"
+
+#define BAUD 9600
+#define NMEA_TMO 2000 /*milliseconds*/
+
+struct ts2phc_nmea_master {
+ struct ts2phc_master master;
+ struct config *config;
+ struct lstab *lstab;
+ pthread_t worker;
+ /* Protects anonymous struct fields, below, from concurrent access. */
+ pthread_mutex_t mutex;
+ struct {
+ struct timespec local_monotime;
+ struct timespec local_utctime;
+ struct timespec rmc_utctime;
+ bool rmc_fix_valid;
+ };
+};
+
+static int open_nmea_connection(const char *host, const char *port,
+ const char *serialport)
+{
+ int fd;
+
+ if (host[0] && port[0]) {
+ fd = sock_open(host, port);
+ if (fd == -1) {
+ pr_err("failed to open nmea source %s:%s", host, port);
+ }
+ return fd;
+ }
+ fd = serial_open(serialport, BAUD, 0, 0);
+ if (fd == -1) {
+ pr_err("failed to open nmea source %s", serialport);
+ }
+ return fd;
+}
+
+static void *monitor_nmea_status(void *arg)
+{
+ struct nmea_parser *np = nmea_parser_create();
+ struct pollfd pfd = { -1, POLLIN | POLLPRI };
+ char *host, input[256], *port, *ptr, *uart;
+ struct ts2phc_nmea_master *master = arg;
+ struct timespec rxtime, tmo = { 2, 0 };
+ int cnt, num, parsed;
+ struct nmea_rmc rmc;
+ struct timex ntx;
+
+ if (!np) {
+ pr_err("failed to create NMEA parser");
+ return NULL;
+ }
+ host = config_get_string(master->config, NULL,
"ts2phc.nmea_remote_host");
+ port = config_get_string(master->config, NULL,
"ts2phc.nmea_remote_port");
+ uart = config_get_string(master->config, NULL,
"ts2phc.nmea_serialport");
+ memset(&ntx, 0, sizeof(ntx));
+ ntx.modes = ADJ_NANO;
+
+ while (is_running()) {
+ if (pfd.fd == -1) {
+ pfd.fd = open_nmea_connection(host, port, uart);
+ if (pfd.fd == -1) {
+ clock_nanosleep(CLOCK_MONOTONIC, 0, &tmo, NULL);
+ continue;
+ }
+ }
+ num = poll(&pfd, 1, NMEA_TMO);
+ clock_gettime(CLOCK_MONOTONIC, &rxtime);
+ adjtimex(&ntx);
+ if (num < 0) {
+ pr_err("poll failed");
+ break;
+ }
+ if (!num) {
+ pr_err("nmea source timed out");
+ close(pfd.fd);
+ pfd.fd = -1;
+ continue;
+ }
+ if (pfd.revents & POLLERR) {
+ pr_err("nmea source socket error");
+ close(pfd.fd);
+ pfd.fd = -1;
+ continue;
+ }
+ if (!(pfd.revents & (POLLIN | POLLPRI))) {
+ continue;
+ }
+ cnt = read(pfd.fd, input, sizeof(input));
+ if (cnt < 0) {
+ pr_err("failed to read from nmea source");
+ close(pfd.fd);
+ pfd.fd = -1;
+ continue;
+ }
+ ptr = input;
+ do {
+ if (!nmea_parse(np, ptr, cnt, &rmc, &parsed)) {
+ pthread_mutex_lock(&master->mutex);
+ master->local_monotime = rxtime;
+ master->local_utctime.tv_sec = ntx.time.tv_sec;
+ master->local_utctime.tv_nsec =
ntx.time.tv_usec;
+ master->rmc_utctime = rmc.ts;
+ master->rmc_fix_valid = rmc.fix_valid;
+ pthread_mutex_unlock(&master->mutex);
+ }
+ cnt -= parsed;
+ ptr += parsed;
+ } while (cnt);
+ }
+
+ nmea_parser_destroy(np);
+ if (pfd.fd != -1) {
+ close(pfd.fd);
+ }
+ return NULL;
+}
+
+static void ts2phc_nmea_master_destroy(struct ts2phc_master *master)
+{
+ struct ts2phc_nmea_master *m =
+ container_of(master, struct ts2phc_nmea_master, master);
+ pthread_join(m->worker, NULL);
+ pthread_mutex_destroy(&m->mutex);
+ lstab_destroy(m->lstab);
+ free(m);
+}
+
+static int ts2phc_nmea_master_getppstime(struct ts2phc_master *master,
+ struct timespec *ts)
+{
+ struct ts2phc_nmea_master *m =
+ container_of(master, struct ts2phc_nmea_master, master);
+ tmv_t delay_t1, delay_t2, local_t1, local_t2, rmc;
+ int lstab_error = 0, tai_offset = 0;
+ enum lstab_result result;
+ struct timespec now;
+ int64_t utc_time;
+ bool fix_valid;
+
+ clock_gettime(CLOCK_MONOTONIC, &now);
+ local_t2 = timespec_to_tmv(now);
+
+ pthread_mutex_lock(&m->mutex);
+
+ local_t1 = timespec_to_tmv(m->local_monotime);
+ delay_t2 = timespec_to_tmv(m->local_utctime);
+ rmc = timespec_to_tmv(m->rmc_utctime);
+ fix_valid = m->rmc_fix_valid;
+
+ pthread_mutex_unlock(&m->mutex);
+
+ delay_t1 = rmc;
+ pr_debug("nmea delay: %" PRId64 " ns",
+ tmv_to_nanoseconds(tmv_sub(delay_t2, delay_t1)));
+
+ //
+ // TODO - check that (local_t2 - local_t1) is smaller than X.
+ //
+ rmc = tmv_add(rmc, tmv_sub(local_t2, local_t1));
+ utc_time = tmv_to_nanoseconds(rmc);
+ *ts = tmv_to_timespec(rmc);
+
+ result = lstab_utc2tai(m->lstab, utc_time, &tai_offset);
+ switch (result) {
+ case LSTAB_OK:
+ lstab_error = 0;
+ break;
+ case LSTAB_UNKNOWN:
+ case LSTAB_AMBIGUOUS:
+ lstab_error = -1;
+ break;
+ }
+ ts->tv_sec += tai_offset;
+
+ return fix_valid ? lstab_error : -1;
+}
+
+struct ts2phc_master *ts2phc_nmea_master_create(struct config *cfg, const char
*dev)
+{
+ struct ts2phc_nmea_master *master;
+ const char *leapfile = NULL; // TODO - read from config.
+ int err;
+
+ master = calloc(1, sizeof(*master));
+ if (!master) {
+ return NULL;
+ }
+ master->lstab = lstab_create(leapfile);
+ if (!master->lstab) {
+ return NULL;
+ }
+ master->master.destroy = ts2phc_nmea_master_destroy;
+ master->master.getppstime = ts2phc_nmea_master_getppstime;
+ master->config = cfg;
+ pthread_mutex_init(&master->mutex, NULL);
+ err = pthread_create(&master->worker, NULL, monitor_nmea_status,
master);
+ if (err) {
+ pr_err("failed to create worker thread: %s", strerror(err));
+ free(master);
+ return NULL;
+ }
+
+ return &master->master;
+}
diff --git a/ts2phc_nmea_master.h b/ts2phc_nmea_master.h
new file mode 100644
index 0000000..7430e20
--- /dev/null
+++ b/ts2phc_nmea_master.h
@@ -0,0 +1,13 @@
+/**
+ * @file ts2phc_nmea_master.h
+ * @note Copyright (C) 2019 Richard Cochran <richardcoch...@gmail.com>
+ * @note SPDX-License-Identifier: GPL-2.0+
+ */
+#ifndef HAVE_TS2PHC_NMEA_MASTER_H
+#define HAVE_TS2PHC_NMEA_MASTER_H
+
+#include "ts2phc_master.h"
+
+struct ts2phc_master *ts2phc_nmea_master_create(struct config *cfg,
+ const char *dev);
+#endif
--
2.20.1
_______________________________________________
Linuxptp-devel mailing list
Linuxptp-devel@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/linuxptp-devel
