common-fun.c 7.79 KB
Newer Older
1 2 3
/*
 * FIXME: header
 */
4
#include <pptp/pptp.h>
5
#include <pptp/diag.h>
6
#include "common-fun.h"
7 8 9

void st_com_execute_slave(struct pp_instance *ppi)
{
10
	if (pp_timer_expired(ppi->timers[PP_TIMER_ANN_RECEIPT])) {
11
		DBGV("event ANNOUNCE_RECEIPT_TIMEOUT_EXPIRES\n");
12 13 14 15 16 17
		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;
18
		} else {
19
			ppi->next_state = PPS_LISTENING;
20 21 22 23 24
			st_com_restart_annrec_timer(ppi);
		}
	}

	if (ppi->rt_opts->e2e_mode) {
25
		if (pp_timer_expired(ppi->timers[PP_TIMER_DELAYREQ])) {
26
			DBGV("TODO: event DELAYREQ_INTERVAL_TIMEOUT_EXPIRES\n");
27 28 29
			/* TODO issueDelayReq(rtOpts,ptpClock); */
		}
	} else {
30
		if (pp_timer_expired(ppi->timers[PP_TIMER_PDELAYREQ]))
31
		{
32
			DBGV("TODO: event PDELAYREQ_INTERVAL_TOUT_EXPIRES\n");
33 34 35 36 37 38 39 40 41 42 43 44
			/* 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,
45
			ppi->timers[PP_TIMER_ANN_RECEIPT]);
46 47 48 49 50
}


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

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

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

68
	j = ppi->foreign_record_best;
69

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

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

90
		j = (j + 1) % ppi->number_foreign_records;
91 92
	}

93
	/* New foreign master */
94
	if (!found) {
95 96 97
		if (ppi->number_foreign_records <
		    ppi->max_foreign_records) {
			ppi->number_foreign_records++;
98 99
		}

100 101 102 103 104 105 106 107 108
		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;
109 110 111 112 113

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

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

118
		DBGV("New foreign Master added \n");
119 120 121

		ppi->foreign_record_i = (ppi->foreign_record_i+1) %
			ppi->max_foreign_records;
122 123
	}
}
124 125


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

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

	return 0;
}
156

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

	if (len < PP_SYNC_LENGTH)
		return -1;

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

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

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

	MsgHeader *hdr = &ppi->msg_tmp_header;

221 222 223
	if (len < PP_FOLLOW_UP_LENGTH)
		return -1;

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

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

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

247
	add_TimeInternal(&correction_field, &correction_field,
248
		&ppi->last_sync_corr_field);
249

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

	pp_update_clock(ppi);
	return 0;
}
257 258


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

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

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

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

303
	DBGV("Announce message from another foreign master\n");
304

305
	st_com_add_foreign(ppi, buf);
306 307 308 309 310 311

	ppi->record_update = TRUE;

	return 0;
}

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

	if (!ppi->is_from_self)
		return 0;

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