common-fun.c 7.94 KB
Newer Older
1
/*
2 3
 * Aurelio Colosimo for CERN, 2011 -- GNU LGPL v2.1 or later
 * Based on PTPd project v. 2.1.0 (see AUTHORS for details)
4
 */
5

6
#include <pptp/pptp.h>
7
#include <pptp/diag.h>
8
#include "common-fun.h"
9

10
int st_com_execute_slave(struct pp_instance *ppi)
11
{
12
	if (pp_timer_expired(ppi->timers[PP_TIMER_ANN_RECEIPT])) {
13
		DBGV("event ANNOUNCE_RECEIPT_TIMEOUT_EXPIRES\n");
14 15 16 17 18 19
		ppi->number_foreign_records = 0;
		ppi->foreign_record_i = 0;
		if (!DSDEF(ppi)->slaveOnly &&
			DSDEF(ppi)->clockQuality.clockClass != 255) {
			m1(ppi);
			ppi->next_state = PPS_MASTER;
20
		} else {
21
			ppi->next_state = PPS_LISTENING;
22 23 24 25 26
			st_com_restart_annrec_timer(ppi);
		}
	}

	if (ppi->rt_opts->e2e_mode) {
27
		if (pp_timer_expired(ppi->timers[PP_TIMER_DELAYREQ])) {
28
			DBGV("TODO: event DELAYREQ_INTERVAL_TIMEOUT_EXPIRES\n");
29
			return msg_issue_delay_req(ppi);
30 31
		}
	} else {
32
		if (pp_timer_expired(ppi->timers[PP_TIMER_PDELAYREQ]))
33
		{
34
			DBGV("TODO: event PDELAYREQ_INTERVAL_TOUT_EXPIRES\n");
35
			return msg_issue_pdelay_req(ppi);
36 37
		}
	}
38
	return 0;
39 40 41 42 43 44 45 46 47
}

void st_com_restart_annrec_timer(struct pp_instance *ppi)
{
	/* 0 <= logAnnounceInterval <= 4, see pag. 237 of spec */
	/* FIXME: if (logAnnounceInterval < 0), error? Or handle a right
	 * shift?*/
	pp_timer_start((DSPOR(ppi)->announceReceiptTimeout) <<
			DSPOR(ppi)->logAnnounceInterval,
48
			ppi->timers[PP_TIMER_ANN_RECEIPT]);
49 50 51 52 53
}


int st_com_check_record_update(struct pp_instance *ppi)
{
54
	if (ppi->record_update) {
55
		DBGV("event STATE_DECISION_EVENT\n");
56
		ppi->record_update = FALSE;
57
		ppi->next_state = bmc(ppi, ppi->frgn_master, ppi->rt_opts);
58 59 60 61 62 63 64

		if (ppi->next_state != ppi->state)
			return 1;
	}
	return 0;
}

65
void st_com_add_foreign(struct pp_instance *ppi, unsigned char *buf)
66
{
67
	int i, j;
68
	int found = 0;
69
	MsgHeader *hdr = &ppi->msg_tmp_header;
70

71
	j = ppi->foreign_record_best;
72

73 74 75 76 77 78 79 80
	/* Check if foreign master is already known */
	for (i = 0; i < ppi->number_foreign_records; i++) {
		if (!pp_memcmp(hdr->sourcePortIdentity.clockIdentity,
			    ppi->frgn_master[j].port_identity.
			    clockIdentity,
			    PP_CLOCK_IDENTITY_LENGTH) &&
		    (hdr->sourcePortIdentity.portNumber ==
		     ppi->frgn_master[j].port_identity.portNumber))
81
		{
82 83 84
			/* Foreign Master is already in Foreign master data set
			 */
			ppi->frgn_master[j].ann_messages++;
85
			found = 1;
86
			DBGV("addForeign : AnnounceMessage incremented \n");
87

88 89
			msg_copy_header(&ppi->frgn_master[j].hdr, hdr);
			msg_unpack_announce(buf, &ppi->frgn_master[j].ann);
90 91 92
			break;
		}

93
		j = (j + 1) % ppi->number_foreign_records;
94 95
	}

96 97 98 99
	/* Old foreign master */
	if (found)
		return;

100
	/* New foreign master */
101 102 103 104
	if (ppi->number_foreign_records <
	    ppi->max_foreign_records) {
		ppi->number_foreign_records++;
	}
105

106
	j = ppi->foreign_record_i;
107

108 109 110 111 112 113 114
	/* Copy new foreign master data set from announce message */
	pp_memcpy(ppi->frgn_master[j].port_identity.clockIdentity,
		hdr->sourcePortIdentity.clockIdentity,
		PP_CLOCK_IDENTITY_LENGTH);
	ppi->frgn_master[j].port_identity.portNumber =
		hdr->sourcePortIdentity.portNumber;
	ppi->frgn_master[j].ann_messages = 0;
115

116 117 118 119 120
	/*
	 * header and announce field of each Foreign Master are
	 * usefull to run Best Master Clock Algorithm
	 */
	msg_copy_header(&ppi->frgn_master[j].hdr, hdr);
121

122
	msg_unpack_announce(buf, &ppi->frgn_master[j].ann);
123

124
	DBGV("New foreign Master added \n");
125

126 127
	ppi->foreign_record_i = (ppi->foreign_record_i+1) %
		ppi->max_foreign_records;
128
}
129 130


131 132
int st_com_slave_handle_announce(struct pp_instance *ppi, unsigned char *buf,
				 int len)
133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149
{
	MsgHeader *hdr = &ppi->msg_tmp_header;

	if (len < PP_ANNOUNCE_LENGTH)
		return -1;

	if (ppi->is_from_self)
		return 0;

	/*
	 * Valid announce message is received : BMC algorithm
	 * will be executed
	 */
	ppi->record_update = TRUE;

	if (!ppi->is_from_cur_par) {
		msg_unpack_announce(buf, &ppi->msg_tmp.announce);
150
		s1(ppi, hdr, &ppi->msg_tmp.announce);
151
	} else {
152
		/* st_com_add_foreign takes care of announce unpacking */
153
		st_com_add_foreign(ppi, buf);
154 155 156 157 158 159 160
	}

	/*Reset Timer handling Announce receipt timeout*/
	st_com_restart_annrec_timer(ppi);

	return 0;
}
161

162
int st_com_slave_handle_sync(struct pp_instance *ppi, unsigned char *buf,
163
			     int len)
164
{
165
	TimeInternal *time;
166 167 168 169 170 171 172
	TimeInternal origin_tstamp;
	TimeInternal correction_field;
	MsgHeader *hdr = &ppi->msg_tmp_header;

	if (len < PP_SYNC_LENGTH)
		return -1;

173
	if (ppi->is_from_self)
174 175
		return 0;

176 177
	time = &ppi->last_rcv_time;

178 179 180 181 182 183 184 185 186 187 188 189
	if (ppi->is_from_cur_par) {
		ppi->sync_receive_time.seconds = time->seconds;
		ppi->sync_receive_time.nanoseconds = time->nanoseconds;

		/* FIXME diag check. Delete it?
		if (ppi->rt_opts->recordFP)
			fprintf(rtOpts->recordFP, "%d %llu\n",
				header->sequenceId,
				((time->seconds * 1000000000ULL) +
					time->nanoseconds));
		*/

190
		if ((hdr->flagField[0] & PP_TWO_STEP_FLAG) != 0) {
191 192 193 194 195 196
			ppi->waiting_for_follow = TRUE;
			ppi->recv_sync_sequence_id = hdr->sequenceId;
			/* Save correctionField of Sync message */
			int64_to_TimeInternal(
				hdr->correctionfield,
				&correction_field);
197
			ppi->last_sync_corr_field.seconds =
198
				correction_field.seconds;
199
			ppi->last_sync_corr_field.nanoseconds =
200 201 202 203 204 205 206 207 208 209 210 211
				correction_field.nanoseconds;
		} else {
			msg_unpack_sync(buf, &ppi->msg_tmp.sync);
			int64_to_TimeInternal(
				ppi->msg_tmp_header.correctionfield,
				&correction_field);
			/* FIXME diag check
			 * timeInternal_display(&correctionfield);
			 */
			ppi->waiting_for_follow = FALSE;
			to_TimeInternal(&origin_tstamp,
					&ppi->msg_tmp.sync.originTimestamp);
212
			pp_update_offset(ppi, &origin_tstamp,
213
					&ppi->sync_receive_time,
214
					&correction_field);
215 216 217 218 219 220
			pp_update_clock(ppi);
		}
	}
	return 0;
}

221 222
int st_com_slave_handle_followup(struct pp_instance *ppi, unsigned char *buf,
				 int len)
223 224 225 226 227 228
{
	TimeInternal precise_orig_timestamp;
	TimeInternal correction_field;

	MsgHeader *hdr = &ppi->msg_tmp_header;

229 230 231
	if (len < PP_FOLLOW_UP_LENGTH)
		return -1;

232
	if (!ppi->is_from_cur_par) {
233
		DBGV("SequenceID doesn't match last Sync message\n");
234 235 236 237
		return 0;
	}

	if (!ppi->waiting_for_follow) {
238
		DBGV("Slave was not waiting a follow up message\n");
239 240 241 242
		return 0;
	}

	if (ppi->recv_sync_sequence_id != hdr->sequenceId) {
243
		DBGV("Follow up message is not from current parent\n");
244 245 246 247 248 249 250 251 252 253 254
		return 0;
	}

	msg_unpack_follow_up(buf, &ppi->msg_tmp.follow);
	ppi->waiting_for_follow = FALSE;
	to_TimeInternal(&precise_orig_timestamp,
			&ppi->msg_tmp.follow.preciseOriginTimestamp);

	int64_to_TimeInternal(ppi->msg_tmp_header.correctionfield,
					&correction_field);

255
	add_TimeInternal(&correction_field, &correction_field,
256
		&ppi->last_sync_corr_field);
257

258
	pp_update_offset(ppi, &precise_orig_timestamp,
259
			&ppi->sync_receive_time,
260
			&correction_field);
261 262 263 264

	pp_update_clock(ppi);
	return 0;
}
265 266


267
int st_com_handle_pdelay_req(struct pp_instance *ppi, unsigned char *buf,
268
			     int len)
269
{
270
	TimeInternal *time;
271 272 273 274 275 276 277 278
	MsgHeader *hdr = &ppi->msg_tmp_header;

	if (len < PP_PDELAY_REQ_LENGTH)
		return -1;

	if (ppi->rt_opts->e2e_mode)
		return 0;

279 280
	time = &ppi->last_rcv_time;

281 282 283 284 285 286 287 288 289 290 291 292 293
	if (ppi->is_from_self) {
		/* Get sending timestamp from IP stack
		 * with So_TIMESTAMP */
		ppi->pdelay_req_send_time.seconds =
			time->seconds;
		ppi->pdelay_req_send_time.nanoseconds =
			time->nanoseconds;

		/*Add latency*/
		add_TimeInternal(&ppi->pdelay_req_send_time,
			&ppi->pdelay_req_send_time,
			&ppi->rt_opts->outbound_latency);
	} else {
294
		msg_copy_header(&ppi->pdelay_req_hdr, hdr);
295

296
		return msg_issue_pdelay_resp(ppi, time, hdr);
297 298 299
	}
	return 0;
}
300

301 302
int st_com_master_handle_announce(struct pp_instance *ppi, unsigned char *buf,
				  int len)
303 304 305 306 307
{
	if (len < PP_ANNOUNCE_LENGTH)
		return -1;

	if (ppi->is_from_self) {
308
		DBGV("HandleAnnounce : Ignore message from self\n");
309 310 311
		return 0;
	}

312
	DBGV("Announce message from another foreign master\n");
313

314
	st_com_add_foreign(ppi, buf);
315 316 317 318 319 320

	ppi->record_update = TRUE;

	return 0;
}

321
int st_com_master_handle_sync(struct pp_instance *ppi, unsigned char *buf,
322
			      int len)
323
{
324
	TimeInternal *time;
325 326 327 328 329 330
	if (len < PP_SYNC_LENGTH)
		return -1;

	if (!ppi->is_from_self)
		return 0;

331 332
	time = &ppi->last_rcv_time;

333
	/* Add latency */
334
	add_TimeInternal(time, time, &ppi->rt_opts->outbound_latency);
335
	msg_issue_followup(ppi, time);
336 337
	return 0;
}