pdelay/hook: add to the wr hooks the handler for peer req and resp

also the wr master digest directly from the master fms the peer req messages. Signed-off-by: C.Prados <c.prados@gsi.de>

pdelay/hook: add to the wr hooks the handler for peer req and resp
also the wr master digest directly from the master fms the peer req messages. Signed-off-by: C.Prados <c.prados@gsi.de>
19a746cb · Cesar Prados · Alessandro Rubini · a040e8d1 · 19a746cb
Commit 19a746cb authored Jul 01, 2015 by Cesar Prados Committed by Alessandro Rubini May 16, 2016
Hide whitespace changes
Inline Side-by-side

Showing with 63 additions and 0 deletions

hooks.c proto-ext-whiterabbit/hooks.c +63 -0

No files found.
--- a/proto-ext-whiterabbit/hooks.c
+++ b/proto-ext-whiterabbit/hooks.c
@@ -71,6 +71,20 @@ static int wr_listening(struct pp_instance *ppi, unsigned char *pkt, int plen)
 	return 0;
 }

+static int wr_handle_preq(struct pp_instance *ppi)
+{
+
+	msg_copy_header(&ppi->pdelay_req_hdr, &ppi->received_ptp_header);
+	ppi->pdelay_req_hdr.correctionfield.msb = 0;
+	ppi->pdelay_req_hdr.correctionfield.lsb =
+	    phase_to_cf_units(ppi->last_rcv_time.phase);
+
+	msg_issue_pdelay_resp(ppi, &ppi->last_rcv_time);
+	msg_issue_pdelay_resp_followup(ppi, &ppi->last_snt_time);
+
+	return 0;
+}
+
 static int wr_master_msg(struct pp_instance *ppi, unsigned char *pkt, int plen,
 			 int msgtype)
 {
@@ -91,6 +105,11 @@ static int wr_master_msg(struct pp_instance *ppi, unsigned char *pkt, int plen,
 		msgtype = PPM_NOTHING_TO_DO;
 		break;

+	case PPM_PDELAY_REQ:
+		wr_handle_preq(ppi);
+		msgtype = PPM_NOTHING_TO_DO;
+		break;
+
 	/* This is missing in the standard protocol */
 	case PPM_SIGNALING:
 		msg_unpack_wrsig(ppi, pkt, &wrsig_msg,
@@ -202,9 +221,51 @@ static int wr_handle_followup(struct pp_instance *ppi,
 	wr_servo_got_sync(ppi, precise_orig_timestamp,
 			  &ppi->t2);

+	if (GLBS(ppi)->delay_mech)
+		wr_servo_update(ppi);
+
 	return 1; /* the caller returns too */
 }

+static int wr_handle_presp(struct pp_instance *ppi)
+{
+	MsgHeader *hdr = &ppi->received_ptp_header;
+	TimeInternal correction_field;
+	struct wr_dsport *wrp = WR_DSPOR(ppi);
+	TimeInternal *ofm = &DSCUR(ppi)->offsetFromMaster;
+
+	/* FIXME: check sub-nano relevance of correction filed */
+	cField_to_TimeInternal(&correction_field, hdr->correctionfield);
+
+	/*
+	 * If no WR mode is on, run normal code, if T2/T3 are valid.
+	 * After we adjusted the pps counter, stamps are invalid, so
+	 * we'll have the Unix time instead, marked by "correct"
+	 */
+
+	if (!wrp->wrModeOn) {
+		if (!ppi->t3.correct || !ppi->t6.correct) {
+			pp_diag(ppi, servo, 1,
+				"T3 or T6 incorrect, discarding tuple\n");
+			return 0;
+		}
+		pp_servo_got_presp(ppi);
+		/*
+		 * pps always on if offset less than 1 second,
+		 * until ve have a configurable threshold */
+		if (ofm->seconds)
+			wrp->ops->enable_timing_output(ppi, 0);
+		else
+			wrp->ops->enable_timing_output(ppi, 1);
+
+		return 0;
+	}
+
+	ppi->t4_cf = hdr->correctionfield.lsb;
+	wr_servo_got_delay(ppi, ppi->t4_cf);
+	return 0;
+}
+
 static int wr_pack_announce(struct pp_instance *ppi)
 {
 	pp_diag(ppi, ext, 2, "hook: %s\n", __func__);
@@ -237,6 +298,8 @@ struct pp_ext_hooks pp_hooks = {
 	.execute_slave = wr_execute_slave,
 	.handle_announce = wr_handle_announce,
 	.handle_followup = wr_handle_followup,
+	.handle_preq = wr_handle_preq,
+	.handle_presp = wr_handle_presp,
 	.pack_announce = wr_pack_announce,
 	.unpack_announce = wr_unpack_announce,
 };