linuxptp/ts2phc_nmea_master.c

228 lines
5.4 KiB
C

/**
* @file ts2phc_nmea_master.c
* @note Copyright (C) 2019 Richard Cochran <richardcochran@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;
}