linuxptp/clock.c

488 lines
12 KiB
C
Raw Normal View History

/**
* @file clock.c
* @note Copyright (C) 2011 Richard Cochran <richardcochran@gmail.com>
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
*/
#include <errno.h>
#include <poll.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include "bmc.h"
#include "clock.h"
#include "foreign.h"
#include "mave.h"
#include "missing.h"
#include "msg.h"
#include "phc.h"
#include "port.h"
#include "servo.h"
#include "print.h"
#include "tmv.h"
#include "util.h"
#define MAVE_LENGTH 10
#define ARRAY_SIZE(x) (sizeof(x) / sizeof((x)[0]))
struct clock {
clockid_t clkid;
struct servo *servo;
struct defaultDS dds;
struct dataset default_dataset;
struct currentDS cur;
struct parentDS dad;
struct timePropertiesDS tds;
struct foreign_clock *best;
struct port *port[MAX_PORTS];
struct pollfd pollfd[MAX_PORTS*N_POLLFD];
int nports;
tmv_t master_offset;
tmv_t path_delay;
struct mave *avg_delay;
tmv_t c1;
tmv_t c2;
tmv_t t1;
tmv_t t2;
};
struct clock the_clock;
static void handle_state_decision_event(struct clock *c);
static void clock_destroy(struct clock *c)
{
int i;
for (i = 0; i < c->nports; i++) {
port_close(c->port[i]);
}
if (c->clkid != CLOCK_REALTIME) {
phc_close(c->clkid);
}
memset(c, 0, sizeof(*c));
}
static void clock_ppb(clockid_t clkid, double ppb)
{
struct timex tx;
memset(&tx, 0, sizeof(tx));
tx.modes = ADJ_FREQUENCY;
tx.freq = (long) (ppb * 65.536);
if (clock_adjtime(clkid, &tx) < 0)
pr_err("failed to adjust the clock: %m");
}
static void clock_step(clockid_t clkid, int64_t ns)
{
struct timex tx;
int sign = 1;
if (ns < 0) {
sign = -1;
ns *= -1;
}
memset(&tx, 0, sizeof(tx));
tx.modes = ADJ_SETOFFSET | ADJ_NANO;
tx.time.tv_sec = sign * (ns / NS_PER_SEC);
tx.time.tv_usec = sign * (ns % NS_PER_SEC);
/*
* The value of a timeval is the sum of its fields, but the
* field tv_usec must always be non-negative.
*/
if (tx.time.tv_usec < 0) {
tx.time.tv_sec -= 1;
tx.time.tv_usec += 1000000000;
}
if (clock_adjtime(clkid, &tx) < 0)
pr_err("failed to step clock: %m");
}
static void clock_update_grandmaster(struct clock *c)
{
memset(&c->cur, 0, sizeof(c->cur));
c->dad.parentPortIdentity.clockIdentity = c->dds.clockIdentity;
c->dad.parentPortIdentity.portNumber = 0;
c->dad.grandmasterIdentity = c->dds.clockIdentity;
c->dad.grandmasterClockQuality = c->dds.clockQuality;
c->dad.grandmasterPriority1 = c->dds.priority1;
c->dad.grandmasterPriority2 = c->dds.priority2;
c->tds.currentUtcOffset = CURRENT_UTC_OFFSET;
c->tds.currentUtcOffsetValid = FALSE;
c->tds.leap61 = FALSE;
c->tds.leap59 = FALSE;
c->tds.timeTraceable = FALSE;
c->tds.frequencyTraceable = FALSE;
c->tds.ptpTimescale = TRUE;
c->tds.timeSource = INTERNAL_OSCILLATOR;
}
static void clock_update_slave(struct clock *c)
{
struct ptp_message *msg = TAILQ_FIRST(&c->best->messages);
c->cur.stepsRemoved = 1 + c->best->dataset.stepsRemoved;
c->dad.parentPortIdentity = c->best->dataset.sender;
c->dad.grandmasterIdentity = msg->announce.grandmasterIdentity;
c->dad.grandmasterClockQuality = msg->announce.grandmasterClockQuality;
c->dad.grandmasterPriority1 = msg->announce.grandmasterPriority1;
c->dad.grandmasterPriority2 = msg->announce.grandmasterPriority2;
c->tds.currentUtcOffset = msg->announce.currentUtcOffset;
c->tds.currentUtcOffsetValid = field_is_set(msg, 1, UTC_OFF_VALID);
c->tds.leap61 = field_is_set(msg, 1, LEAP_61);
c->tds.leap59 = field_is_set(msg, 1, LEAP_59);
c->tds.timeTraceable = field_is_set(msg, 1, TIME_TRACEABLE);
c->tds.frequencyTraceable = field_is_set(msg, 1, FREQ_TRACEABLE);
c->tds.ptpTimescale = field_is_set(msg, 1, PTP_TIMESCALE);
c->tds.timeSource = msg->announce.timeSource;
}
/* public methods */
UInteger8 clock_class(struct clock *c)
{
return c->dds.clockQuality.clockClass;
}
struct clock *clock_create(char *phc, struct interface *iface, int count,
struct defaultDS *ds, struct port_defaults *pod)
{
int i, max_adj, sw_ts = 0;
struct clock *c = &the_clock;
srandom(time(NULL));
if (c->nports)
clock_destroy(c);
if (phc) {
c->clkid = phc_open(phc);
if (c->clkid == CLOCK_INVALID) {
pr_err("Failed to open %s: %m", phc);
return NULL;
}
max_adj = phc_max_adj(c->clkid);
if (!max_adj) {
pr_err("clock is not adjustable");
return NULL;
}
} else {
c->clkid = CLOCK_REALTIME;
max_adj = 512000;
}
for (i = 0; i < count; i++) {
if (iface[i].timestamping == TS_SOFTWARE) {
sw_ts = 1;
break;
}
}
c->servo = servo_create("pi", max_adj, sw_ts);
if (!c->servo) {
pr_err("Failed to create clock servo");
return NULL;
}
c->avg_delay = mave_create(MAVE_LENGTH);
if (!c->avg_delay) {
pr_err("Failed to create moving average");
return NULL;
}
c->dds = *ds;
/* Initialize the parentDS. */
c->dad.parentPortIdentity.clockIdentity = c->dds.clockIdentity;
c->dad.parentPortIdentity.portNumber = 0;
c->dad.parentStats = 0;
c->dad.observedParentOffsetScaledLogVariance = 0xffff;
c->dad.observedParentClockPhaseChangeRate = 0x7fffffff;
c->dad.grandmasterPriority1 = c->dds.priority1;
c->dad.grandmasterClockQuality = c->dds.clockQuality;
c->dad.grandmasterPriority2 = c->dds.priority2;
c->dad.grandmasterIdentity = c->dds.clockIdentity;
for (i = 0; i < ARRAY_SIZE(c->pollfd); i++) {
c->pollfd[i].fd = -1;
c->pollfd[i].events = 0;
}
for (i = 0; i < count; i++) {
c->port[i] = port_open(pod, iface[i].name, iface[i].transport,
iface[i].timestamping, 1+i, DM_E2E, c);
if (!c->port[i]) {
pr_err("failed to open port %s", iface[i].name);
return NULL;
}
}
c->dds.numberPorts = c->nports = count;
for (i = 0; i < c->nports; i++)
port_dispatch(c->port[i], EV_INITIALIZE);
return c;
}
struct dataset *clock_best_foreign(struct clock *c)
{
return c->best ? &c->best->dataset : NULL;
}
struct port *clock_best_port(struct clock *c)
{
return c->best ? c->best->port : NULL;
}
struct dataset *clock_default_ds(struct clock *c)
{
struct dataset *out = &c->default_dataset;
struct defaultDS *in = &c->dds;
out->priority1 = in->priority1;
out->identity = in->clockIdentity;
out->quality = in->clockQuality;
out->priority2 = in->priority2;
out->stepsRemoved = 0;
out->sender.clockIdentity = in->clockIdentity;
out->sender.portNumber = 0;
out->receiver.clockIdentity = in->clockIdentity;
out->receiver.portNumber = 0;
return out;
}
UInteger8 clock_domain_number(struct clock *c)
{
return c->dds.domainNumber;
}
struct ClockIdentity clock_identity(struct clock *c)
{
return c->dds.clockIdentity;
}
void clock_install_fda(struct clock *c, struct port *p, struct fdarray fda)
{
int i, j, k;
for (i = 0; i < c->nports; i++) {
if (p == c->port[i])
break;
}
for (j = 0; j < fda.cnt; j++) {
k = N_POLLFD * i + j;
c->pollfd[k].fd = fda.fd[j];
c->pollfd[k].events = POLLIN|POLLPRI;
}
}
struct parentDS *clock_parent_ds(struct clock *c)
{
return &c->dad;
}
struct PortIdentity clock_parent_identity(struct clock *c)
{
return c->dad.parentPortIdentity;
}
int clock_poll(struct clock *c)
{
int cnt, i, j, k, sde = 0;
enum fsm_event event;
cnt = poll(c->pollfd, ARRAY_SIZE(c->pollfd), -1);
if (cnt < 0) {
if (EINTR == errno) {
return 0;
} else {
pr_emerg("poll failed");
return -1;
}
} else if (!cnt) {
return 0;
}
for (i = 0; i < c->nports; i++) {
for (j = 0; j < N_POLLFD; j++) {
k = N_POLLFD * i + j;
if (c->pollfd[k].revents & (POLLIN|POLLPRI)) {
event = port_event(c->port[i], j);
if (EV_STATE_DECISION_EVENT == event)
sde = 1;
else
port_dispatch(c->port[i], event);
}
}
}
if (sde)
handle_state_decision_event(c);
return 0;
}
void clock_path_delay(struct clock *c, struct timespec req, struct timestamp rx,
Integer64 correction)
{
tmv_t c1, c2, c3, pd, t1, t2, t3, t4;
c1 = c->c1;
c2 = c->c2;
c3 = correction_to_tmv(correction);
t1 = c->t1;
t2 = c->t2;
t3 = timespec_to_tmv(req);
t4 = timestamp_to_tmv(rx);
/*
* c->path_delay = (t2 - t3) + (t4 - t1);
* c->path_delay -= c_sync + c_fup + c_delay_resp;
* c->path_delay /= 2.0;
*/
pd = tmv_add(tmv_sub(t2, t3), tmv_sub(t4, t1));
pd = tmv_sub(pd, tmv_add(c1, tmv_add(c2, c3)));
pd = tmv_div(pd, 2);
if (pd < 0) {
pr_warning("negative path delay %10lld", pd);
pr_warning("path_delay = (t2 - t3) + (t4 - t1)");
pr_warning("t2 - t3 = %+10lld", t2 - t3);
pr_warning("t4 - t1 = %+10lld", t4 - t1);
pr_warning("c1 %10lld", c1);
pr_warning("c2 %10lld", c2);
pr_warning("c3 %10lld", c3);
return;
}
c->path_delay = mave_accumulate(c->avg_delay, pd);
pr_debug("path delay %10lld %10lld", c->path_delay, pd);
}
int clock_slave_only(struct clock *c)
{
return c->dds.slaveOnly;
}
UInteger16 clock_steps_removed(struct clock *c)
{
return c->cur.stepsRemoved;
}
enum servo_state clock_synchronize(struct clock *c,
struct timespec ingress_ts,
struct timestamp origin_ts,
Integer64 correction1,
Integer64 correction2)
{
double adj;
tmv_t ingress, origin;
enum servo_state state = SERVO_UNLOCKED;
ingress = timespec_to_tmv(ingress_ts);
origin = timestamp_to_tmv(origin_ts);
c->t1 = origin;
c->t2 = ingress;
c->c1 = correction_to_tmv(correction1);
c->c2 = correction_to_tmv(correction2);
/*
* c->master_offset = ingress - origin - c->path_delay - c->c1 - c->c2;
*/
c->master_offset = tmv_sub(ingress,
tmv_add(origin, tmv_add(c->path_delay, tmv_add(c->c1, c->c2))));
if (!c->path_delay)
return state;
adj = servo_sample(c->servo, c->master_offset, ingress, &state);
pr_info("master offset %10lld s%d adj %+7.0f path delay %10lld",
c->master_offset, state, adj, c->path_delay);
switch (state) {
case SERVO_UNLOCKED:
break;
case SERVO_JUMP:
clock_step(c->clkid, -c->master_offset);
break;
case SERVO_LOCKED:
clock_ppb(c->clkid, -adj);
break;
}
return state;
}
struct timePropertiesDS *clock_time_properties(struct clock *c)
{
return &c->tds;
}
static void handle_state_decision_event(struct clock *c)
{
struct foreign_clock *best = NULL, *fc;
int i;
for (i = 0; i < c->nports; i++) {
fc = port_compute_best(c->port[i]);
if (!fc)
continue;
if (!best || dscmp(&fc->dataset, &best->dataset) > 0)
best = fc;
}
if (!best)
return;
pr_notice("selected best master clock %s",
cid2str(&best->dataset.identity));
if (c->best != best)
mave_reset(c->avg_delay);
c->best = best;
for (i = 0; i < c->nports; i++) {
enum port_state ps;
enum fsm_event event;
ps = bmc_state_decision(c, c->port[i]);
switch (ps) {
case PS_LISTENING:
event = EV_NONE;
break;
case PS_GRAND_MASTER:
clock_update_grandmaster(c);
event = EV_RS_GRAND_MASTER;
break;
case PS_MASTER:
event = EV_RS_MASTER;
break;
case PS_PASSIVE:
event = EV_RS_PASSIVE;
break;
case PS_SLAVE:
clock_update_slave(c);
event = EV_RS_SLAVE;
break;
default:
event = EV_INITIALIZE;
break;
}
port_dispatch(c->port[i], event);
}
}