common-fun.c 7.89 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
	/* New foreign master */
96
	if (!found) {
97 98 99
		if (ppi->number_foreign_records <
		    ppi->max_foreign_records) {
			ppi->number_foreign_records++;
100 101
		}

102 103 104 105 106 107 108 109 110
		j = ppi->foreign_record_i;

		/* 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;
111 112 113 114 115

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

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

120
		DBGV("New foreign Master added \n");
121 122 123

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


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

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

	return 0;
}
158

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

	if (len < PP_SYNC_LENGTH)
		return -1;

169
	if (ppi->is_from_self)
170 171 172 173 174 175 176 177 178 179 180 181 182 183
		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));
		*/

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

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

	MsgHeader *hdr = &ppi->msg_tmp_header;

223 224 225
	if (len < PP_FOLLOW_UP_LENGTH)
		return -1;

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

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

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

249
	add_TimeInternal(&correction_field, &correction_field,
250
		&ppi->last_sync_corr_field);
251

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

	pp_update_clock(ppi);
	return 0;
}
259 260


261 262
int st_com_handle_pdelay_req(struct pp_instance *ppi, unsigned char *buf,
			     int len, TimeInternal *time)
263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284
{
	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 {
285
		msg_copy_header(&ppi->pdelay_req_hdr, hdr);
286

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

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

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

305
	DBGV("Announce message from another foreign master\n");
306

307
	st_com_add_foreign(ppi, buf);
308 309 310 311 312 313

	ppi->record_update = TRUE;

	return 0;
}

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

	if (!ppi->is_from_self)
		return 0;

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