state-slave.c 3.18 KB
Newer Older
1
/*
2 3
 * Copyright (C) 2011 CERN (www.cern.ch)
 * Author: Aurelio Colosimo
4 5
 * Copyright (C) 2014 GSI (www.gsi.de)
 * Author: Cesar Prados
6
 * Originally based on PTPd project v. 2.1.0
7 8
 *
 * Released according to the GNU LGPL, version 2.1 or any later version.
9
 */
10

Alessandro Rubini's avatar
Alessandro Rubini committed
11
#include <ppsi/ppsi.h>
12
#include "common-fun.h"
13

14 15 16 17 18 19
static int slave_handle_response(struct pp_instance *ppi, unsigned char *pkt,
				 int plen);

static pp_action *actions[] = {
	[PPM_SYNC]		= st_com_slave_handle_sync,
	[PPM_DELAY_REQ]		= 0,
20
#if CONFIG_HAS_P2P
21 22
	[PPM_PDELAY_REQ]	= st_com_peer_handle_preq,
	[PPM_PDELAY_RESP]	= st_com_peer_handle_pres,
23 24
	[PPM_PDELAY_R_FUP]	= st_com_peer_handle_pres_followup,
#endif
25 26
	[PPM_FOLLOW_UP]		= st_com_slave_handle_followup,
	[PPM_DELAY_RESP]	= slave_handle_response,
27
	[PPM_ANNOUNCE]		= pp_lib_handle_announce,
28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47
	/* skip signaling and management, for binary size */
};

static int slave_handle_response(struct pp_instance *ppi, unsigned char *pkt,
				 int plen)
{
	int e = 0;
	MsgHeader *hdr = &ppi->received_ptp_header;
	MsgDelayResp resp;

	msg_unpack_delay_resp(pkt, &resp);

	if ((memcmp(&DSPOR(ppi)->portIdentity.clockIdentity,
		    &resp.requestingPortIdentity.clockIdentity,
		    PP_CLOCK_IDENTITY_LENGTH) != 0) ||
	    ((ppi->sent_seq[PPM_DELAY_REQ]) !=
	     hdr->sequenceId) ||
	    (DSPOR(ppi)->portIdentity.portNumber !=
	     resp.requestingPortIdentity.portNumber) ||
	    !(ppi->flags & PPI_FLAG_FROM_CURRENT_PARENT)) {
48 49 50
		pp_diag(ppi, frames, 1, "pp_slave : "
			"Delay Resp doesn't match Delay Req (f %x)\n",
			ppi->flags);
51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75
		return 0;
	}

	to_TimeInternal(&ppi->t4, &resp.receiveTimestamp);
	/* Save delay resp cf in ppi->cField */
	cField_to_TimeInternal(&ppi->cField,
			       hdr->correctionfield);

	if (pp_hooks.handle_resp)
		e = pp_hooks.handle_resp(ppi);
	else
		pp_servo_got_resp(ppi);
	if (e)
		return e;

	if (DSPOR(ppi)->logMinDelayReqInterval !=
	    hdr->logMessageInterval) {
		DSPOR(ppi)->logMinDelayReqInterval =
			hdr->logMessageInterval;
		/* new value for logMin */
		pp_timeout_init(ppi);
	}
	return 0;
}

76
int pp_slave(struct pp_instance *ppi, unsigned char *pkt, int plen)
77
{
78
	int e = 0; /* error var, to check errors in msg handling */
79
	MsgHeader *hdr = &ppi->received_ptp_header;
80

81
	if (ppi->is_new_state) {
82
		memset(&ppi->t1, 0, sizeof(ppi->t1));
83
		pp_servo_init(ppi);
84

85 86 87
		if (pp_hooks.new_slave)
			e = pp_hooks.new_slave(ppi, pkt, plen);
		if (e)
88
			goto out;
89 90
	}

91 92
	e  = pp_lib_may_issue_request(ppi);

93 94 95
	/*
	 * The management of messages is now table-driven
	 */
96
	if (hdr->messageType < ARRAY_SIZE(actions)
97 98 99
	    && actions[hdr->messageType]) {
		e = actions[hdr->messageType](ppi, pkt, plen);
	} else {
100 101
		if (plen)
			pp_diag(ppi, frames, 1, "Ignored frame\n");
102 103
	}
	if (e)
104
		goto out;
105

106 107 108 109 110
	/*
	 * This function, common to passive,listening etc,
	 * is the core of the slave: timeout ann-receipt, hook
	 */
	e = st_com_execute_slave(ppi);
111

112
out:
113 114 115 116
	switch(e) {
	case PP_SEND_OK: /* 0 */
		break;
	case PP_SEND_ERROR:
117
		ppi->next_state = PPS_FAULTY;
118 119 120 121 122
		break;
	case PP_SEND_NO_STAMP:
		/* nothing, just keep the ball rolling */
		e = 0;
		break;
123
	}
124 125

	if (ppi->next_state != ppi->state) {
126
		pp_servo_init(ppi);
127
		return e;
128
	}
129 130
	ppi->next_delay = pp_next_delay_2(ppi,
					  PP_TO_ANN_RECEIPT, PP_TO_REQUEST);
131
	return e;
132
}
133