state-common-fun.c 8.05 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20
/*
 * FIXME: header
 */
#include <pproto/pproto.h>
#include "state-common-fun.h"

void st_com_execute_slave(struct pp_instance *ppi)
{
	if (pp_timer_expired(ppi->timers[PP_TIMER_ANNOUNCE_RECEIPT])) {
		/* FIXME diag
		 * DBGV("event ANNOUNCE_RECEIPT_TIMEOUT_EXPIRES\n");
		 */
		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;
		}
		else {
21
			ppi->next_state = PPS_LISTENING;
22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68
			st_com_restart_annrec_timer(ppi);
		}
	}

	if (ppi->rt_opts->e2e_mode) {
		if (pp_timer_expired(ppi->timers[PP_TIMER_DELAYREQ_INTERVAL])) {
			/* FIXME diag
			 * DBGV("event DELAYREQ_INTERVAL_TIMEOUT_EXPIRES\n");
			 */
			/* TODO issueDelayReq(rtOpts,ptpClock); */
		}
	} else {
		if (pp_timer_expired(ppi->timers[PP_TIMER_PDELAYREQ_INTERVAL]))
		{
			/* FIXME diag
			DBGV("event PDELAYREQ_INTERVAL_TIMEOUT_EXPIRES\n");
			*/
			/* 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,
			ppi->timers[PP_TIMER_ANNOUNCE_RECEIPT]);
}


int st_com_check_record_update(struct pp_instance *ppi)
{
	if(ppi->record_update) {
		/* FIXME diag DBGV("event STATE_DECISION_EVENT\n"); */
		ppi->record_update = FALSE;
		ppi->next_state = bmc(ppi->frgn_master,
				      ppi->rt_opts, ppi);

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

69
void st_com_add_foreign(unsigned char *buf, struct pp_instance *ppi)
70
{
71
	int i, j;
72
	Boolean found = FALSE;
73
	MsgHeader *hdr = &ppi->msg_tmp_header;
74

75
	j = ppi->foreign_record_best;
76

77 78 79 80 81 82 83 84
	/* 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))
85
		{
86 87 88
			/* Foreign Master is already in Foreign master data set
			 */
			ppi->frgn_master[j].ann_messages++;
89
			found = TRUE;
90
			/* FIXME diag
91
			DBGV("addForeign : AnnounceMessage incremented \n");
92 93 94
			*/
			msg_copy_header(&ppi->frgn_master[j].hdr, hdr);
			msg_unpack_announce(buf, &ppi->frgn_master[j].ann);
95 96 97
			break;
		}

98
		j = (j + 1) % ppi->number_foreign_records;
99 100
	}

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

108 109 110 111 112 113 114 115 116
		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;
117 118 119 120 121

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

124 125 126 127 128 129
		msg_unpack_announce(buf, &ppi->frgn_master[j].ann);

		/* FIXME diag DBGV("New foreign Master added \n");*/

		ppi->foreign_record_i = (ppi->foreign_record_i+1) %
			ppi->max_foreign_records;
130 131
	}
}
132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156


int st_com_slave_handle_announce(unsigned char *buf, int len,
				  struct pp_instance *ppi)
{
	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);
		s1(hdr, &ppi->msg_tmp.announce, ppi);
	}
	else {
		/* st_com_add_foreign takes care of announce unpacking */
157
		st_com_add_foreign(buf, ppi);
158 159 160 161 162 163 164
	}

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

	return 0;
}
165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198

int st_com_slave_handle_sync(unsigned char *buf, int len, TimeInternal *time,
				struct pp_instance *ppi)
{
	TimeInternal origin_tstamp;
	TimeInternal correction_field;
	MsgHeader *hdr = &ppi->msg_tmp_header;

	if (len < PP_SYNC_LENGTH)
		return -1;

	if (ppi->is_from_self) {
		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);
199
			ppi->last_sync_corr_field.seconds =
200
				correction_field.seconds;
201
			ppi->last_sync_corr_field.nanoseconds =
202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230
				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);
			pp_update_offset(&origin_tstamp,
					&ppi->sync_receive_time,
					&correction_field,ppi);
			pp_update_clock(ppi);
		}
	}
	return 0;
}

int st_com_slave_handle_followup(unsigned char *buf, int len,
				 struct pp_instance *ppi)
{
	TimeInternal precise_orig_timestamp;
	TimeInternal correction_field;

	MsgHeader *hdr = &ppi->msg_tmp_header;

231 232 233
	if (len < PP_FOLLOW_UP_LENGTH)
		return -1;

234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265
	if (!ppi->is_from_cur_par) {
		/* FIXME diag
		DBGV("SequenceID doesn't match with "
			"last Sync message \n");
		*/
		return 0;
	}

	if (!ppi->waiting_for_follow) {
		/* FIXME diag
		DBGV("Slave was not waiting a follow up "
			"message \n");
		*/
		return 0;
	}

	if (ppi->recv_sync_sequence_id != hdr->sequenceId) {
		/* FIXME diag
		DBGV("Follow up message is not from current parent \n");
		*/
		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);

	add_TimeInternal(&correction_field,&correction_field,
266
		&ppi->last_sync_corr_field);
267 268 269 270 271 272 273 274

	pp_update_offset(&precise_orig_timestamp,
			&ppi->sync_receive_time,
			&correction_field, ppi);

	pp_update_clock(ppi);
	return 0;
}
275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302


int st_com_handle_pdelay_req(unsigned char *buf, int len,
		      TimeInternal *time, struct pp_instance *ppi)
{
	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 {
		msg_copy_header(&ppi->pdelay_req_hdr,hdr);

303
		/* TODO issuePDelayResp(time, header, rtOpts,
304 305 306 307 308
				ptpClock);
		*/
	}
	return 0;
}
309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347

int st_com_master_handle_announce(unsigned char *buf, int len,
				struct pp_instance *ppi)
{
	if (len < PP_ANNOUNCE_LENGTH)
		return -1;

	if (ppi->is_from_self) {
		/* FIXME diag
		DBGV("HandleAnnounce : Ignore message from self \n");
		*/
		return 0;
	}

	/* FIXME diag
	 * DBGV("Announce message from another foreign master");
	 */

	st_com_add_foreign(buf, ppi);

	ppi->record_update = TRUE;

	return 0;
}

int st_com_master_handle_sync(unsigned char *buf, int len, TimeInternal *time,
				struct pp_instance *ppi)
{
	if (len < PP_SYNC_LENGTH)
		return -1;

	if (!ppi->is_from_self)
		return 0;

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