diff --git a/clock.c b/clock.c index 83fbc30..b6e96cc 100644 --- a/clock.c +++ b/clock.c @@ -371,13 +371,15 @@ int clock_slave_only(struct clock *c) return c->dds.slaveOnly; } -void clock_synchronize(struct clock *c, - struct timespec ingress_ts, struct timestamp origin_ts, - Integer64 correction1, Integer64 correction2) +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; + enum servo_state state = SERVO_UNLOCKED; ingress = timespec_to_tmv(ingress_ts); origin = timestamp_to_tmv(origin_ts); @@ -395,7 +397,7 @@ void clock_synchronize(struct clock *c, tmv_add(origin, tmv_add(c->path_delay, tmv_add(c->c1, c->c2)))); if (!c->path_delay) - return; + return state; adj = servo_sample(c->servo, c->master_offset, ingress, &state); @@ -412,6 +414,7 @@ void clock_synchronize(struct clock *c, clock_ppb(c->clkid, -adj); break; } + return state; } static void handle_state_decision_event(struct clock *c) diff --git a/clock.h b/clock.h index 92b29a5..537e957 100644 --- a/clock.h +++ b/clock.h @@ -21,6 +21,7 @@ #define HAVE_CLOCK_H #include "ds.h" +#include "servo.h" #include "transport.h" #define MAX_PORTS 8 @@ -141,8 +142,12 @@ int clock_slave_only(struct clock *c); * @param correction1 The correction field of the sync message. * @param correction2 The correction field of the follow up message. * Pass zero in the case of one step operation. + * @return The state of the clock's servo. */ -void clock_synchronize(struct clock *c, - struct timespec ingress_ts, struct timestamp origin_ts, - Integer64 correction1, Integer64 correction2); +enum servo_state clock_synchronize(struct clock *c, + struct timespec ingress_ts, + struct timestamp origin_ts, + Integer64 correction1, + Integer64 correction2); + #endif diff --git a/port.c b/port.c index dbfdc42..94079fe 100644 --- a/port.c +++ b/port.c @@ -240,6 +240,26 @@ static int port_set_delay_tmo(struct port *p) return timerfd_settime(p->fda.fd[FD_DELAY_TIMER], 0, &tmo, NULL); } +static void port_synchronize(struct port *p, + struct timespec ingress_ts, + struct timestamp origin_ts, + Integer64 correction1, Integer64 correction2) +{ + enum servo_state state; + + state = clock_synchronize(p->clock, ingress_ts, origin_ts, + correction1, correction2); + switch (state) { + case SERVO_UNLOCKED: + case SERVO_JUMP: + port_dispatch(p, EV_SYNCHRONIZATION_FAULT); + break; + case SERVO_LOCKED: + port_dispatch(p, EV_MASTER_CLOCK_SELECTED); + break; + } +} + static int port_delay_request(struct port *p) { struct ptp_message *msg; @@ -495,8 +515,8 @@ static void process_follow_up(struct port *p, struct ptp_message *m) if (memcmp(pid, &m->header.sourcePortIdentity, sizeof(*pid))) return; - clock_synchronize(p->clock, syn->hwts.ts, m->ts.pdu, - syn->header.correction, m->header.correction); + port_synchronize(p, syn->hwts.ts, m->ts.pdu, + syn->header.correction, m->header.correction); } static void process_sync(struct port *p, struct ptp_message *m) @@ -525,8 +545,8 @@ static void process_sync(struct port *p, struct ptp_message *m) // TODO - add asymmetry value to correctionField. if (one_step(m)) { - clock_synchronize(p->clock, m->hwts.ts, m->ts.pdu, - m->header.correction, 0); + port_synchronize(p, m->hwts.ts, m->ts.pdu, + m->header.correction, 0); return; } /* @@ -534,8 +554,8 @@ static void process_sync(struct port *p, struct ptp_message *m) */ fup = p->last_follow_up; if (fup && fup->header.sequenceId == m->header.sequenceId) { - clock_synchronize(p->clock, m->hwts.ts, fup->ts.pdu, - m->header.correction, fup->header.correction); + port_synchronize(p, m->hwts.ts, fup->ts.pdu, + m->header.correction, fup->header.correction); return; } /*