state-slave: merge extension into std, using hooks

Signed-off-by: Alessandro Rubini <rubini@gnudd.com>

state-slave: merge extension into std, using hooks
Signed-off-by: Alessandro Rubini <rubini@gnudd.com>
7f9f8d69 · Alessandro Rubini · ec422bda · 7f9f8d69 · 7f9f8d69 · 7f9f8d69
Commit 7f9f8d69 authored Feb 27, 2013 by Alessandro Rubini
5 changed files
--- a/include/ppsi/ppsi.h
+++ b/include/ppsi/ppsi.h
@@ -313,6 +313,8 @@ struct pp_ext_hooks {
 	int (*listening)(struct pp_instance *ppi, unsigned char *pkt, int plen);
 	int (*master_msg)(struct pp_instance *ppi, unsigned char *pkt,
 			  int plen, int msgtype);
+	int (*new_slave)(struct pp_instance *ppi, unsigned char *pkt, int plen);
+	int (*update_delay)(struct pp_instance *ppi);
 };

 extern struct pp_ext_hooks pp_hooks; /* The one for the extension we build */

--- a/proto-ext-whiterabbit/Makefile
+++ b/proto-ext-whiterabbit/Makefile
@@ -7,7 +7,6 @@ LIBWRO := $D/libwr.o
 LIBS += $(LIBWRO)

 OBJ-libwr := $D/fsm-table.o \
-	$D/state-slave.o \
 	$D/common-fun.o \
 	$D/bmc.o \
 	$D/msg.o \

--- a/proto-ext-whiterabbit/hooks.c
+++ b/proto-ext-whiterabbit/hooks.c
@@ -69,9 +69,35 @@ static int wr_master_msg(struct pp_instance *ppi, unsigned char *pkt, int plen,
 	return msgtype;
 }

+static int wr_new_slave(struct pp_instance *ppi, unsigned char *pkt, int plen)
+{
+	wr_servo_init(ppi);
+	return 0;
+}
+
+static int wr_update_delay(struct pp_instance *ppi)
+{
+	MsgHeader *hdr = &ppi->msg_tmp_header;
+	TimeInternal correction_field;
+
+	int64_to_TimeInternal(hdr->correctionfield, &correction_field);
+
+	/* If no WR mode is on, run normal code */
+	if (!WR_DSPOR(ppi)->wrModeOn) {
+		pp_update_delay(ppi, &correction_field);
+		return 0;
+	}
+	wr_servo_got_delay(ppi, hdr->correctionfield.lsb);
+	wr_servo_update(ppi);
+	return 0;
+}
+
+
 struct pp_ext_hooks pp_hooks = {
 	.init = wr_init,
 	.open = wr_open,
 	.listening = wr_listening,
 	.master_msg = wr_master_msg,
+	.new_slave = wr_new_slave,
+	.update_delay = wr_update_delay,
 };
--- a/proto-ext-whiterabbit/state-slave.c
+++ b/proto-ext-whiterabbit/state-slave.c
-/*
- * Aurelio Colosimo for CERN, 2011 -- GNU LGPL v2.1 or later
- * Based on PTPd project v. 2.1.0 (see AUTHORS for details)
- */
-
-#include <ppsi/ppsi.h>
-#include <ppsi/diag.h>
-#include "wr-api.h"
-#include "common-fun.h"
-
-int pp_slave(struct pp_instance *ppi, unsigned char *pkt, int plen)
-{
-	int e = 0; /* error var, to check errors in msg handling */
-	TimeInternal *time;
-	TimeInternal req_rec_tstamp;
-	TimeInternal correction_field;
-	TimeInternal resp_orig_tstamp;
-	MsgHeader *hdr = &ppi->msg_tmp_header;
-
-	time = &ppi->last_rcv_time;
-
-	if (ppi->is_new_state) {
-		DSPOR(ppi)->portState = PPS_SLAVE;
-		pp_init_clock(ppi);
-		wr_servo_init(ppi);
-
-		ppi->waiting_for_follow = FALSE;
-
-		ppi->pdelay_req_send_time.seconds = 0;
-		ppi->pdelay_req_send_time.nanoseconds = 0;
-		ppi->pdelay_req_receive_time.seconds = 0;
-		ppi->pdelay_req_receive_time.nanoseconds = 0;
-		ppi->pdelay_resp_send_time.seconds = 0;
-		ppi->pdelay_resp_send_time.nanoseconds = 0;
-		ppi->pdelay_resp_receive_time.seconds = 0;
-		ppi->pdelay_resp_receive_time.nanoseconds = 0;
-
-		st_com_restart_annrec_timer(ppi);
-
-		if (OPTS(ppi)->e2e_mode)
-			pp_timer_start(
-			    (1 << DSPOR(ppi)->logMinDelayReqInterval) * 1000,
-			    ppi->timers[PP_TIMER_DELAYREQ]);
-		else
-			pp_timer_start(
-			    (1 << DSPOR(ppi)->logMinPdelayReqInterval) * 1000,
-			    ppi->timers[PP_TIMER_PDELAYREQ]);
-	}
-
-	if (st_com_check_record_update(ppi))
-		goto state_updated;
-
-	if (plen == 0)
-		goto no_incoming_msg;
-
-	switch (ppi->msg_tmp_header.messageType) {
-
-	case PPM_ANNOUNCE:
-		e = st_com_slave_handle_announce(ppi, pkt, plen);
-		break;
-
-	case PPM_SYNC:
-		e = st_com_slave_handle_sync(ppi, pkt, plen);
-		break;
-
-	case PPM_FOLLOW_UP:
-		e = st_com_slave_handle_followup(ppi, pkt, plen);
-		break;
-
-	case PPM_DELAY_REQ:
-
-		e = (plen < PP_DELAY_REQ_LENGTH);
-
-		if (!e && ppi->is_from_self) {
-			/* No more used: delay_req_send_time was taken
-			 * when sending it */
-		}
-		break;
-
-	case PPM_DELAY_RESP:
-
-		e = (plen < PP_DELAY_RESP_LENGTH);
-
-		if (e)
-			break;
-
-		if (!OPTS(ppi)->e2e_mode)
-			break;
-
-		msg_unpack_delay_resp(pkt, &ppi->msg_tmp.resp);
-
-		if ((memcmp(DSPOR(ppi)->portIdentity.clockIdentity,
-			ppi->msg_tmp.resp.requestingPortIdentity.clockIdentity,
-			PP_CLOCK_IDENTITY_LENGTH) == 0) &&
-			((ppi->sent_seq_id[PPM_DELAY_REQ] - 1) ==
-				hdr->sequenceId) &&
-			(DSPOR(ppi)->portIdentity.portNumber ==
-			ppi->msg_tmp.resp.requestingPortIdentity.portNumber)
-			&& ppi->is_from_cur_par) {
-
-			to_TimeInternal(&req_rec_tstamp,
-				       &ppi->msg_tmp.resp.receiveTimestamp);
-			ppi->delay_req_receive_time.seconds =
-				req_rec_tstamp.seconds;
-			ppi->delay_req_receive_time.nanoseconds =
-				req_rec_tstamp.nanoseconds;
-			ppi->delay_req_receive_time.phase =
-				req_rec_tstamp.phase;
-			ppi->delay_req_receive_time.correct =
-				req_rec_tstamp.correct;
-
-			int64_to_TimeInternal(
-				hdr->correctionfield,
-				&correction_field);
-
-			if (!WR_DSPOR(ppi)->wrModeOn)
-				pp_update_delay(ppi, &correction_field);
-			else {
-				wr_servo_got_delay(ppi,
-					hdr->correctionfield.lsb);
-				wr_servo_update(ppi);
-			}
-
-			ppi->log_min_delay_req_interval =
-				hdr->logMessageInterval;
-
-		} else {
-			PP_PRINTF("pp_slave : "
-			     "Delay Resp doesn't match Delay Req\n");
-		}
-
-		break;
-
-	case PPM_PDELAY_REQ:
-		e = st_com_handle_pdelay_req(ppi, pkt, plen);
-		break;
-
-	case PPM_PDELAY_RESP:
-		if (OPTS(ppi)->e2e_mode)
-			break;
-
-		e = (plen < PP_PDELAY_RESP_LENGTH);
-
-		if (e)
-			break;
-
-		if (ppi->is_from_self)	{
-			add_TimeInternal(time, time,
-					&OPTS(ppi)->outbound_latency);
-			msg_issue_pdelay_resp_follow_up(ppi, time);
-			break;
-		}
-		msg_unpack_pdelay_resp(pkt,
-					&ppi->msg_tmp.presp);
-
-		if (!((ppi->sent_seq_id[PPM_PDELAY_REQ] ==
-				hdr->sequenceId)
-			&& (!memcmp(DSPOR(ppi)->portIdentity.clockIdentity,
-				  ppi->msg_tmp.presp.requestingPortIdentity.
-				  clockIdentity, PP_CLOCK_IDENTITY_LENGTH))
-			&& (DSPOR(ppi)->portIdentity.portNumber ==
-				 ppi->msg_tmp.presp.
-					 requestingPortIdentity.portNumber))) {
-
-			if ((hdr->flagField[0] & 0x02) ==
-			    PP_TWO_STEP_FLAG) {
-				/* Two Step Clock */
-				/* Store t4 (Fig 35) */
-				ppi->pdelay_resp_receive_time.seconds =
-					time->seconds;
-				ppi->pdelay_resp_receive_time.nanoseconds =
-					time->nanoseconds;
-				/* Store t2 (Fig 35) */
-				to_TimeInternal(&req_rec_tstamp,
-					       &ppi->msg_tmp.presp.
-						       requestReceiptTimestamp);
-				ppi->pdelay_req_receive_time.seconds =
-					req_rec_tstamp.seconds;
-				ppi->pdelay_req_receive_time.nanoseconds =
-					req_rec_tstamp.nanoseconds;
-
-				int64_to_TimeInternal(hdr->correctionfield,
-					&correction_field);
-				ppi->last_pdelay_resp_corr_field.seconds =
-					correction_field.seconds;
-				ppi->last_pdelay_resp_corr_field.nanoseconds =
-					correction_field.nanoseconds;
-			} else {
-				/* One step Clock */
-				/* Store t4 (Fig 35) */
-				ppi->pdelay_resp_receive_time.seconds =
-					time->seconds;
-				ppi->pdelay_resp_receive_time.nanoseconds =
-					time->nanoseconds;
-
-				int64_to_TimeInternal(hdr->correctionfield,
-					&correction_field);
-				pp_update_peer_delay(ppi,& correction_field, 0);
-			}
-		} else {
-			PP_VPRINTF("pp_slave : PDelay Resp doesn't "
-			     "match PDelay Req.\n");
-		}
-		break;
-
-	case PPM_PDELAY_RESP_FOLLOW_UP:
-
-		if (OPTS(ppi)->e2e_mode)
-			break;
-
-		e = (plen < PP_PDELAY_RESP_FOLLOW_UP_LENGTH);
-
-		if (e)
-			break;
-
-		if (hdr->sequenceId ==
-		    ppi->sent_seq_id[PPM_PDELAY_REQ]) {
-
-			msg_unpack_pdelay_resp_followup(
-				pkt,
-				&ppi->msg_tmp.prespfollow);
-
-			to_TimeInternal(
-				&resp_orig_tstamp,
-				&ppi->msg_tmp.prespfollow.
-					responseOriginTimestamp);
-
-			ppi->pdelay_resp_send_time.seconds =
-				resp_orig_tstamp.seconds;
-			ppi->pdelay_resp_send_time.nanoseconds =
-				resp_orig_tstamp.nanoseconds;
-
-			int64_to_TimeInternal(
-				hdr->correctionfield,
-				&correction_field);
-			add_TimeInternal(&correction_field, &correction_field,
-				&ppi->last_pdelay_req_corr_field);
-
-			pp_update_peer_delay(ppi, &correction_field, 1);
-		}
-		break;
-
-	default:
-		/* disreguard, nothing to do */
-		break;
-
-	}
-
-no_incoming_msg:
-	if (e == 0)
-		e = st_com_execute_slave(ppi, 1);
-
-	if (e != 0)
-		ppi->next_state = PPS_FAULTY;
-
-state_updated:
-
-	/* Leaving this state */
-	if (ppi->next_state != ppi->state) {
-		pp_timer_stop(ppi->timers[PP_TIMER_ANN_RECEIPT]);
-
-		if (OPTS(ppi)->e2e_mode)
-			pp_timer_stop(ppi->timers[PP_TIMER_DELAYREQ]);
-		else
-			pp_timer_stop(ppi->timers[PP_TIMER_PDELAYREQ]);
-
-		pp_init_clock(ppi);
-	}
-
-	ppi->next_delay = PP_DEFAULT_NEXT_DELAY_MS;
-
-	return 0;
-}
--- a/proto-standard/state-slave.c
+++ b/proto-standard/state-slave.c
@@ -19,8 +19,14 @@ int pp_slave(struct pp_instance *ppi, unsigned char *pkt, int plen)

 	if (ppi->is_new_state) {
 		DSPOR(ppi)->portState = PPS_SLAVE;
+
 		pp_init_clock(ppi);

+		if (pp_hooks.new_slave)
+			e = pp_hooks.new_slave(ppi, pkt, plen);
+		if (e)
+			goto no_incoming_msg;
+
 		ppi->waiting_for_follow = FALSE;

 		ppi->pdelay_req_send_time.seconds = 0;
@@ -102,7 +108,12 @@ int pp_slave(struct pp_instance *ppi, unsigned char *pkt, int plen)
 				hdr->correctionfield,
 				&correction_field);

-			pp_update_delay(ppi, &correction_field);
+			if (pp_hooks.update_delay)
+				e = pp_hooks.update_delay(ppi);
+			else
+				pp_update_delay(ppi, &correction_field);
+			if (e)
+				goto no_incoming_msg;

 			ppi->log_min_delay_req_interval =
 				hdr->logMessageInterval;