state-slave.c 3.17 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
		return 0;
	}

54
	ppi->t4 = resp.receiveTimestamp;
55 56
	pp_time_add(&ppi->t4, &hdr->cField);
	/* WARNING: should be "sub" (see README-cfield::BUG)  */
57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74

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

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

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

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

90
	pp_lib_may_issue_request(ppi);
91

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

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

110
out:
111 112 113 114
	switch(e) {
	case PP_SEND_OK: /* 0 */
		break;
	case PP_SEND_ERROR:
115
		/* ignore: a lost frame is not the end of the world */
116 117 118 119 120
		break;
	case PP_SEND_NO_STAMP:
		/* nothing, just keep the ball rolling */
		e = 0;
		break;
121
	}
122 123

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