state-slave.c 9.66 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
static int slave_handle_sync(struct pp_instance *ppi, void *buf, int len);
static int slave_handle_followup(struct pp_instance *ppi, void *buf,
Sven Meier's avatar
Sven Meier committed
16
			  int len);
17
static int slave_handle_response(struct pp_instance *ppi, void *buf,
18
			  int len);
19
static int slave_handle_announce(struct pp_instance *ppi, void *buf, int len);
20 21

static pp_action *actions[] = {
22
	[PPM_SYNC]			= slave_handle_sync,
23
	[PPM_DELAY_REQ]		= 0,
24
#if CONFIG_HAS_P2P
25 26
	[PPM_PDELAY_REQ]	= st_com_peer_handle_preq,
	[PPM_PDELAY_RESP]	= st_com_peer_handle_pres,
27 28
	[PPM_PDELAY_R_FUP]	= st_com_peer_handle_pres_followup,
#endif
Sven Meier's avatar
Sven Meier committed
29
	[PPM_FOLLOW_UP]		= slave_handle_followup,
30
	[PPM_DELAY_RESP]	= slave_handle_response,
31
	[PPM_ANNOUNCE]		= slave_handle_announce,
32 33
	[PPM_SIGNALING]     = st_com_handle_signaling,
	/* skip management, for binary size */
34 35
};

36
static int slave_handle_sync(struct pp_instance *ppi, void *buf,
Sven Meier's avatar
Sven Meier committed
37 38
			     int len)
{
baujc's avatar
baujc committed
39
	static int errcount=0;
Sven Meier's avatar
Sven Meier committed
40 41 42
	MsgHeader *hdr = &ppi->received_ptp_header;
	MsgSync sync;

baujc's avatar
baujc committed
43 44 45
	if (!msg_from_current_master(ppi)) {
		pp_error("%s: Sync message is not from current parent\n",
			__func__);
Sven Meier's avatar
Sven Meier committed
46
		return 0;
baujc's avatar
baujc committed
47
	}
Sven Meier's avatar
Sven Meier committed
48

baujc's avatar
baujc committed
49
	if ( is_delayMechanismE2E(ppi) &&  ppi->t1.scaled_nsecs==0 && ppi->t1.secs==0 ) {
50 51 52 53
		/* First time we receive the SYNC message in uncalib/slave state
		 * We set the REQUEST time-out to the minDelayReqInterval/2 value (500ms)
		 * in order to provide quickly a DelayReq message
		 */
baujc's avatar
baujc committed
54
		pp_timeout_set(ppi, PP_TO_REQUEST, (1000*(1<<PP_MIN_MIN_DELAY_REQ_INTERVAL))/2);
55
	}
Sven Meier's avatar
Sven Meier committed
56 57 58 59 60 61 62 63 64
	/* t2 may be overriden by follow-up, save it immediately */
	ppi->t2 = ppi->last_rcv_time;
	msg_unpack_sync(buf, &sync);

	if ((hdr->flagField[0] & PP_TWO_STEP_FLAG) != 0) {
		ppi->flags |= PPI_FLAG_WAITING_FOR_F_UP;
		ppi->recv_sync_sequence_id = hdr->sequenceId;
		/* for two-step, the stamp comes later */
		ppi->t1 = hdr->cField; /* most likely 0 */
65 66 67 68 69 70
	} else {
		/* one-step follows */
		ppi->flags &= ~PPI_FLAG_WAITING_FOR_F_UP;
		ppi->t1 = sync.originTimestamp;
		pp_time_add(&ppi->t1, &hdr->cField);
		ppi->syncCF = 0;
baujc's avatar
baujc committed
71 72 73 74
		/* t1 & t2 are saved in the instance. Check if they are correct */
		if ( is_timestamps_incorrect(ppi,&errcount,3 /* mask=t1&t2 */)  )
			return 0;

75
		/* Call the extension; it may do it all and ask to return */
baujc's avatar
baujc committed
76 77
		if ( is_ext_hook_available(ppi,handle_sync) ) {
			int ret = ppi->ext_hooks->handle_sync(ppi);
78 79 80 81 82
			if (ret == 1)
				return 0;
			if (ret < 0)
				return ret;
		}
baujc's avatar
baujc committed
83
		pp_servo_got_sync(ppi,1);
Sven Meier's avatar
Sven Meier committed
84 85 86 87
	}
	return 0;
}

88
static int slave_handle_followup(struct pp_instance *ppi, void *buf,
Sven Meier's avatar
Sven Meier committed
89 90
				 int len)
{
baujc's avatar
baujc committed
91
	static int errcount=0;
Sven Meier's avatar
Sven Meier committed
92 93 94 95
	MsgFollowUp follow;

	MsgHeader *hdr = &ppi->received_ptp_header;

96
	if (!msg_from_current_master(ppi)) {
Sven Meier's avatar
Sven Meier committed
97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120
		pp_error("%s: Follow up message is not from current parent\n",
			__func__);
		return 0;
	}

	if (!(ppi->flags & PPI_FLAG_WAITING_FOR_F_UP)) {
		pp_error("%s: Slave was not waiting a follow up message\n",
			__func__);
		return 0;
	}

	if (ppi->recv_sync_sequence_id != hdr->sequenceId) {
		pp_error("%s: SequenceID %d doesn't match last Sync message "
			 "%d\n", __func__,
			 hdr->sequenceId, ppi->recv_sync_sequence_id);
		return 0;
	}

	msg_unpack_follow_up(buf, &follow);
	ppi->flags &= ~PPI_FLAG_WAITING_FOR_F_UP;
	/* t1 for calculations is T1 + Csyn + Cful -- see README-cfield */
	pp_time_add(&ppi->t1, &follow.preciseOriginTimestamp);
	pp_time_add(&ppi->t1, &hdr->cField);
	ppi->syncCF = hdr->cField.scaled_nsecs; /* for diag about TC */
baujc's avatar
baujc committed
121 122
	/* t1 & t2 are saved in the instance. Check if they are correct */
	if ( is_timestamps_incorrect(ppi,&errcount,3 /* mask=t1&t2 */)  )
Sven Meier's avatar
Sven Meier committed
123
		return 0;
baujc's avatar
baujc committed
124 125 126 127 128 129 130 131 132
	/* Call the extension; it may do it all and ask to return */
	if (is_ext_hook_available(ppi,handle_followup) ){
		int ret = ppi->ext_hooks->handle_followup(ppi);
		if (ret == 1)
			return 0;
		if (ret < 0)
			return ret;
	}
	/* default servo action */
baujc's avatar
baujc committed
133
	pp_servo_got_sync(ppi,0);
Sven Meier's avatar
Sven Meier committed
134 135 136 137

	return 0;
}

138
static int slave_handle_response(struct pp_instance *ppi, void *buf,
139
				 int len)
140 141
{
	MsgHeader *hdr = &ppi->received_ptp_header;
142
	MsgDelayResp resp;	
baujc's avatar
baujc committed
143
	int ret;
144 145
	
	msg_unpack_delay_resp(buf, &resp);
146

147 148
	if ((bmc_idcmp(&DSPOR(ppi)->portIdentity.clockIdentity,
		    &resp.requestingPortIdentity.clockIdentity) != 0) ||
149 150
	    (ppi->sent_seq[PPM_DELAY_REQ] != hdr->sequenceId)   ||
	    (DSPOR(ppi)->portIdentity.portNumber != resp.requestingPortIdentity.portNumber) ||
151
	    (!msg_from_current_master(ppi))) {
152 153
		pp_diag(ppi, frames, 1, "%s : "
			"Delay Resp doesn't match Delay Req (f %x)\n",__func__,
154
			ppi->flags);
155 156 157
		return 0;
	}

158
	ppi->t4 = resp.receiveTimestamp;
159 160 161 162 163 164 165 166 167 168 169 170 171 172
	if ( is_ext_hook_available(ppi,is_correction_field_compliant) &&
			!ppi->ext_hooks->is_correction_field_compliant(ppi) ) {
		/* Not compliant with CF */
		pp_time_add(&ppi->t4, &hdr->cField);
	} else{
		/* We subtract the received CF and the current delayAsymmetry */
		struct pp_time delayAsym={
				.secs=0,
				.scaled_nsecs=ppi->portDS->delayAsymmetry
		};

		pp_time_sub(&ppi->t4, &hdr->cField);
		pp_time_sub(&ppi->t4, &delayAsym);
	}
173

baujc's avatar
baujc committed
174 175 176 177
	if (is_ext_hook_available(ppi,handle_resp)) {
		ret=ppi->ext_hooks->handle_resp(ppi);
	}
	else {
178
		ret=pp_servo_got_resp(ppi,1);
baujc's avatar
baujc committed
179 180 181 182
	}

	if ( ret &&
			DSPOR(ppi)->logMinDelayReqInterval !=hdr->logMessageInterval) {
183 184 185 186 187 188 189 190
		DSPOR(ppi)->logMinDelayReqInterval =
			hdr->logMessageInterval;
		/* new value for logMin */
		pp_timeout_init(ppi);
	}
	return 0;
}

191
static int slave_handle_announce(struct pp_instance *ppi, void *buf, int len)
192
{
baujc's avatar
baujc committed
193
	int ret;
194
	struct pp_frgn_master frgn_master;
195
	struct pp_frgn_master *reg_frgn_master;
196

baujc's avatar
baujc committed
197
	if ((ret = st_com_handle_announce(ppi, buf, len))!=0)
198
		return ret;
199 200 201 202

	/* If externalPortConfiguration option is set, we consider that all
	 * announce messages come from the current master.
	 */
baujc's avatar
baujc committed
203 204
	bmc_store_frgn_master(ppi, &frgn_master, buf, len);

baujc's avatar
baujc committed
205 206 207 208
	/*  Clause 17.6.5.3 : ExternalPortConfiguration enabled
	 *  - The Announce receipt timeout mechanism (see 9.2.6.12) shall not be active.
	 *  - The specifications of 9.5.3 shall be replaced by the specifications of 17.6.5.5
	 */
baujc's avatar
baujc committed
209 210 211 212 213 214 215 216 217 218 219 220 221 222 223
	if (!is_externalPortConfigurationEnabled(DSDEF(ppi)) )  {
		if ( !msg_from_current_master(ppi) ) {
			pp_error("%s: Announce message is not from current parent\n",
				__func__);

			/* Clause 9.2.2.2 MasterOnly PTP ports :
			 * Announce messages received on a masterOnly PTP Port shall not be considered
			 * in the operation of the best master clock algorithm or in the update of data sets.
			 */
			if ( ! DSPOR(ppi)->masterOnly) {
				bmc_add_frgn_master(ppi, &frgn_master);
			}

			return 0;
		}
224
		/* 9.2.6.12 a) reset timeout */
baujc's avatar
baujc committed
225
		pp_timeout_reset(ppi, PP_TO_ANN_RECEIPT);
baujc's avatar
baujc committed
226 227 228
	}

	/* Add foreign master: Figure 36 & 54 */
229 230 231
	if ( ( reg_frgn_master=bmc_add_frgn_master(ppi, &frgn_master))!=NULL) {
		/* 9.5.3 Figure 36 update data set if announce from current master */
		bmc_s1(ppi, reg_frgn_master);
baujc's avatar
baujc committed
232

233 234 235
		/* Save active peer MAC address */
		memcpy(ppi->activePeer,ppi->peer, sizeof(ppi->activePeer));
	}
236

237 238 239
	return 0;
}

Sven Meier's avatar
Sven Meier committed
240 241 242 243
/*
 * SLAVE and UNCALIBRATED have many things in common. This function implements
 * both states. We set "uncalibrated" internally to 0 or 1.
 */
244
int pp_slave(struct pp_instance *ppi, void *buf, int len)
245
{
baujc's avatar
baujc committed
246
	int ret = PP_SEND_OK; /* error var, to check errors in msg handling */
baujc's avatar
baujc committed
247
	Boolean uncalibrated = (ppi->state == PPS_UNCALIBRATED);
248
	MsgHeader *hdr = &ppi->received_ptp_header;
249

Sven Meier's avatar
Sven Meier committed
250 251
	/* upgrade from uncalibrated to slave or back*/
	if (uncalibrated) {
baujc's avatar
baujc committed
252
		if ( is_ext_hook_available(ppi,ready_for_slave) )  {
253 254 255 256 257 258
			if ( (*ppi->ext_hooks->ready_for_slave)(ppi) ) {
				ppi->next_state = PPS_SLAVE;
			}
		} else {
	           ppi->next_state = PPS_SLAVE;
		}
259
	} else {
baujc's avatar
baujc committed
260

261 262 263
		/* Check if the foreign master has changed */
		if ( DSPAR(ppi)->newGrandmaster ) {
			// New grandmaster detected
264

265
			DSPAR(ppi)->newGrandmaster=FALSE; // Clear it
266 267

			// State must transition from SLAVE to UNCALIBRATED
268
			ppi->next_state = PPS_UNCALIBRATED;
269

270 271 272 273
			Octet *id=DSPAR(ppi)->parentPortIdentity.clockIdentity.id;
			pp_info("New grandmaster detected: %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
						id[0],id[1],id[2],id[3],id[4],id[5],id[6],id[7]);
		}
274 275
	}

276
	/* Force to stay on desired state if externalPortConfiguration option is enabled */
baujc's avatar
baujc committed
277 278 279 280
	if (is_externalPortConfigurationEnabled(DSDEF(ppi)) &&
			ppi->next_state == PPS_SLAVE &&
			ppi->externalPortConfigurationPortDS.desiredState==PPS_UNCALIBRATED)
		ppi->next_state = PPS_UNCALIBRATED; //Force to stay in uncalibrated state
Sven Meier's avatar
Sven Meier committed
281 282

	/* when entering uncalibrated init servo */
283
	if (uncalibrated && ppi->is_new_state) {
284
		memset(&ppi->t1, 0, sizeof(ppi->t1));
285
		pp_diag(ppi, bmc, 2, "Entered to uncalibrated, reset servo\n");
286
		pp_servo_init(ppi);
287

baujc's avatar
baujc committed
288 289 290
		if (is_ext_hook_available(ppi,new_slave))
			ret = ppi->ext_hooks->new_slave(ppi, buf, len);
		if (ret!=PP_SEND_OK)
291
			goto out;
292 293
	}

294
	/* do a delay measurement either in p2p or e2e delay mode */
295
	pp_lib_may_issue_request(ppi);
296
	
297 298 299
	/*
	 * The management of messages is now table-driven
	 */
300
	if (hdr->messageType < ARRAY_SIZE(actions)
301
	    && actions[hdr->messageType]) {
baujc's avatar
baujc committed
302
		ret = actions[hdr->messageType](ppi, buf, len);
303
	} else {
304
		if (len)
305 306
			pp_diag(ppi, frames, 1, "Ignored frame %i\n",
				hdr->messageType);
307
	}
308

baujc's avatar
baujc committed
309 310 311 312 313
	/* Clause 17.6.5.3 : ExternalPortConfiguration enabled
	 *  - The Announce receipt timeout mechanism (see 9.2.6.12) shall not be active.
	 */
	if ( !is_externalPortConfigurationEnabled(DSDEF(ppi)))
		st_com_check_announce_receive_timeout(ppi);
314

315
out:
baujc's avatar
baujc committed
316 317
	if ( ret==PP_SEND_NO_STAMP ) {
		ret = PP_SEND_OK;/* nothing, just keep the ball rolling */
318
	}
319

baujc's avatar
baujc committed
320 321 322
	ppi->next_delay = is_externalPortConfigurationEnabled(DSDEF(ppi)) ?
			pp_next_delay_1(ppi,PP_TO_REQUEST) :
			pp_next_delay_2(ppi,PP_TO_ANN_RECEIPT, PP_TO_REQUEST);
baujc's avatar
baujc committed
323
	return ret;
324
}
325