From 10753faccfc7e2aca995264de0b7840fe77ae42f Mon Sep 17 00:00:00 2001
From: Richard Cochran <richardcochran@gmail.com>
Date: Sun, 18 Dec 2011 11:02:26 +0100
Subject: [PATCH] Implement the synchronization events.

This will allow a port to get from the uncalibrated into the slave state.

Signed-off-by: Richard Cochran <richardcochran@gmail.com>
---
 clock.c | 13 ++++++++-----
 clock.h | 11 ++++++++---
 port.c  | 32 ++++++++++++++++++++++++++------
 3 files changed, 42 insertions(+), 14 deletions(-)

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;
 	}
 	/*