common-fun.c 7.9 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 11

void st_com_execute_slave(struct pp_instance *ppi)
{
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 30 31
			/* TODO issueDelayReq(rtOpts,ptpClock); */
		}
	} else {
32
		if (pp_timer_expired(ppi->timers[PP_TIMER_PDELAYREQ]))
33
		{
34
			DBGV("TODO: event PDELAYREQ_INTERVAL_TOUT_EXPIRES\n");
35 36 37 38 39 40 41 42 43 44 45 46
			/* TODO issuePDelayReq(rtOpts,ptpClock); */
		}
	}
}

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,
47
			ppi->timers[PP_TIMER_ANN_RECEIPT]);
48 49 50 51 52
}


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

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

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

70
	j = ppi->foreign_record_best;
71

72 73 74 75 76 77 78 79
	/* 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))
80
		{
81 82 83
			/* Foreign Master is already in Foreign master data set
			 */
			ppi->frgn_master[j].ann_messages++;
84
			found = 1;
85
			DBGV("addForeign : AnnounceMessage incremented \n");
86

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

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

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

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

105
	j = ppi->foreign_record_i;
106

107 108 109 110 111 112 113
	/* 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;
114

115 116 117 118 119
	/*
	 * 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);
120

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

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

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


130 131
int st_com_slave_handle_announce(struct pp_instance *ppi, unsigned char *buf,
				 int len)
132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148
{
	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);
149
		s1(ppi, hdr, &ppi->msg_tmp.announce);
150
	} else {
151
		/* st_com_add_foreign takes care of announce unpacking */
152
		st_com_add_foreign(ppi, buf);
153 154 155 156 157 158 159
	}

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

	return 0;
}
160

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

	if (len < PP_SYNC_LENGTH)
		return -1;

171
	if (ppi->is_from_self)
172 173 174 175 176 177 178 179 180 181 182 183 184 185
		return 0;

	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));
		*/

186
		if ((hdr->flagField[0] & PP_TWO_STEP_FLAG) != 0) {
187 188 189 190 191 192
			ppi->waiting_for_follow = TRUE;
			ppi->recv_sync_sequence_id = hdr->sequenceId;
			/* Save correctionField of Sync message */
			int64_to_TimeInternal(
				hdr->correctionfield,
				&correction_field);
193
			ppi->last_sync_corr_field.seconds =
194
				correction_field.seconds;
195
			ppi->last_sync_corr_field.nanoseconds =
196 197 198 199 200 201 202 203 204 205 206 207
				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);
208
			pp_update_offset(ppi, &origin_tstamp,
209
					&ppi->sync_receive_time,
210
					&correction_field);
211 212 213 214 215 216
			pp_update_clock(ppi);
		}
	}
	return 0;
}

217 218
int st_com_slave_handle_followup(struct pp_instance *ppi, unsigned char *buf,
				 int len)
219 220 221 222 223 224
{
	TimeInternal precise_orig_timestamp;
	TimeInternal correction_field;

	MsgHeader *hdr = &ppi->msg_tmp_header;

225 226 227
	if (len < PP_FOLLOW_UP_LENGTH)
		return -1;

228
	if (!ppi->is_from_cur_par) {
229
		DBGV("SequenceID doesn't match last Sync message\n");
230 231 232 233
		return 0;
	}

	if (!ppi->waiting_for_follow) {
234
		DBGV("Slave was not waiting a follow up message\n");
235 236 237 238
		return 0;
	}

	if (ppi->recv_sync_sequence_id != hdr->sequenceId) {
239
		DBGV("Follow up message is not from current parent\n");
240 241 242 243 244 245 246 247 248 249 250
		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);

251
	add_TimeInternal(&correction_field, &correction_field,
252
		&ppi->last_sync_corr_field);
253

254
	pp_update_offset(ppi, &precise_orig_timestamp,
255
			&ppi->sync_receive_time,
256
			&correction_field);
257 258 259 260

	pp_update_clock(ppi);
	return 0;
}
261 262


263 264
int st_com_handle_pdelay_req(struct pp_instance *ppi, unsigned char *buf,
			     int len, TimeInternal *time)
265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286
{
	MsgHeader *hdr = &ppi->msg_tmp_header;

	if (len < PP_PDELAY_REQ_LENGTH)
		return -1;

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

	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 {
287
		msg_copy_header(&ppi->pdelay_req_hdr, hdr);
288

289
		/* TODO issuePDelayResp(time, header, rtOpts,
290 291 292 293 294
				ptpClock);
		*/
	}
	return 0;
}
295

296 297
int st_com_master_handle_announce(struct pp_instance *ppi, unsigned char *buf,
				  int len)
298 299 300 301 302
{
	if (len < PP_ANNOUNCE_LENGTH)
		return -1;

	if (ppi->is_from_self) {
303
		DBGV("HandleAnnounce : Ignore message from self\n");
304 305 306
		return 0;
	}

307
	DBGV("Announce message from another foreign master\n");
308

309
	st_com_add_foreign(ppi, buf);
310 311 312 313 314 315

	ppi->record_update = TRUE;

	return 0;
}

316 317
int st_com_master_handle_sync(struct pp_instance *ppi, unsigned char *buf,
			      int len, TimeInternal *time)
318 319 320 321 322 323 324 325
{
	if (len < PP_SYNC_LENGTH)
		return -1;

	if (!ppi->is_from_self)
		return 0;

	/* Add latency */
326
	add_TimeInternal(time, time, &ppi->rt_opts->outbound_latency);
327 328 329
	/* TODO issueFollowup(time,rtOpts,ptpClock);*/
	return 0;
}