fsm.c 10.1 KB
Newer Older
1
/*
2 3 4 5
 * Copyright (C) 2011 CERN (www.cern.ch)
 * Author: Alessandro Rubini
 *
 * Released to the public domain
6
 */
Alessandro Rubini's avatar
Alessandro Rubini committed
7
#include <ppsi/ppsi.h>
8

9
unsigned long pp_global_d_flags; /* This is the only "global" file in ppsi */
10

11 12 13 14 15 16
/*
 * This is somehow a duplicate of __pp_diag, but I still want
 * explicit timing in the fsm enter/stay/leave messages,
 * while there's no need to add times to all diagnostic messages
 */
static void pp_fsm_printf(struct pp_instance *ppi, char *fmt, ...)
17 18
{
	va_list args;
19
	struct pp_time t;
20
	unsigned long oflags = pp_global_d_flags;
21

22
	if (!pp_diag_allow(ppi, fsm, 1))
23 24
		return;

25
	/* temporarily set NOTIMELOG, as we'll print the time ourselves */
26
	pp_global_d_flags |= PP_FLAG_NOTIMELOG;
27
	TOPS(ppi)->get(ppi, &t);
28
	pp_global_d_flags = oflags;
29

30
	pp_printf("diag-fsm-1-%s: %09d.%03d: ", ppi->port_name,
31
		  (int)t.secs, (int)((t.scaled_nsecs >> 16)) / 1000000);
32 33 34 35 36
	va_start(args, fmt);
	pp_vprintf(fmt, args);
	va_end(args);
}

37 38 39 40 41 42 43 44 45 46
/*
 * Diagnostics about state machine, enter, leave, remain
 */
enum {
	STATE_ENTER,
	STATE_LOOP,
	STATE_LEAVE
};

static void pp_diag_fsm(struct pp_instance *ppi, char *name, int sequence,
47
			int len)
48 49 50
{
	if (sequence == STATE_ENTER) {
		/* enter with or without a packet len */
51
		pp_fsm_printf(ppi, "ENTER %s, packet len %i\n",
52
			  name, len);
53 54 55
		return;
	}
	if (sequence == STATE_LOOP) {
56
		pp_fsm_printf(ppi, "%s: reenter in %i ms\n", name,
57 58 59 60
				ppi->next_delay);
		return;
	}
	/* leave has one \n more, so different states are separate */
61 62
	pp_fsm_printf(ppi, "LEAVE %s (next: %3i)\n\n",
		      name, ppi->next_state);
63 64
}

65 66 67
static struct pp_state_table_item *
get_current_state_table_item(struct pp_instance *ppi)
{
68
	struct pp_state_table_item *ip = ppi->current_state_item;
69

70
	/* Avoid searching if we already know where we are */
71
	ppi->is_new_state = 0;
72 73
	if (ip && ip->state == ppi->state)
		return ip;
74
	ppi->is_new_state = 1;
75

76
	/* a linear search is affordable up to a few dozen items */
77
	for (ip = pp_state_table; ip->state != PPS_END_OF_TABLE; ip++)
78 79
		if (ip->state == ppi->state) {
			ppi->current_state_item = ip;
80
			return ip;
81
		}
82
	return NULL;
83 84
}

85 86 87 88 89 90 91 92 93 94 95
char *get_state_as_string(struct pp_instance *ppi, int state) {
	static char *def="INVALID";
	struct pp_state_table_item *ip = ppi->current_state_item;

	for (ip = pp_state_table; ip->state != PPS_END_OF_TABLE; ip++)
		if (ip->state == state) {
			return ip->name;
		}
	return def;
}

96 97 98
/*
 * Returns delay to next state, which is always zero.
 */
99
int pp_leave_current_state(struct pp_instance *ppi)
100
{
101
	/* If something has to be done in an extension */
102 103 104 105 106
	if ( ppi->ext_hooks->state_change) {
		// When leaving MASTER/SLAVE states, clear the active peer field
		if ( ppi->state==PPS_MASTER || ppi->state==PPS_SLAVE)
			bzero(ppi->activePeer,sizeof(ppi->activePeer));

107
		ppi->ext_hooks->state_change(ppi);
108
	}
109
	
110
	/* if the next or old state is non standard PTP reset all timeouts */
111
	if ((ppi->state > PPS_LAST_STATE) || (ppi->next_state > PPS_LAST_STATE))
112 113
		pp_timeout_setall(ppi);

114
	ppi->state = ppi->next_state;
115
	if ( ppi->state==PPS_DISABLED ){
116
		ppi->pdstate = PP_PDSTATE_NONE; // Clear state
117 118 119
		/* clear extState so that it is displayed correctly in the wr_mon*/
		ppi->extState = PP_EXSTATE_DISABLE;
        }
120 121 122 123 124 125
	ppi->flags &= ~PPI_FLAGS_WAITING;
	pp_diag_fsm(ppi, ppi->current_state_item->name, STATE_LEAVE, 0);
	/* next_delay unused: go to new state now */
	return 0;
}

126 127 128 129 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 157 158 159 160 161 162 163 164 165 166 167
/*
 * Checks whether a packet has to be discarded and maybe updates port status
 * accordingly. Returns new packet length (0 if packet has to be discarded)
 */
static int pp_packet_prefilter(struct pp_instance *ppi)
{
	MsgHeader *hdr = &ppi->received_ptp_header;

	/*
	 * 9.5.1:
	 * Only PTP messages where the domainNumber field of the PTP message
	 * header (see 13.3.2.5) is identical to the defaultDS.domainNumber
	 * shall be accepted for processing by the protocol.
	 */
	if (hdr->domainNumber != GDSDEF(GLBS(ppi))->domainNumber) {
		pp_diag(ppi, frames, 1, "Wrong domain %i: discard\n",
			hdr->domainNumber);
		return -1;
	}

	/*
	 * Alternate masters (17.4) not supported
	 * 17.4.2, NOTE:
	 * A slave node that does not want to use information from alternate
	 * masters merely ignores all messages with alternateMasterFlag TRUE.
	 */
	if (hdr->flagField[0] & PP_ALTERNATE_MASTER_FLAG) {
		pp_diag(ppi, frames, 1, "Alternate master: discard\n");
		return -1;
	}

	/*
	 * If the message is from the same port that sent it, we should
	 * discard it (9.5.2.2)
	 */
	if (!memcmp(&ppi->received_ptp_header.sourcePortIdentity,
		    &DSPOR(ppi)->portIdentity,
		    sizeof(PortIdentity))) {
		pp_diag(ppi, frames, 1, "Looping frame: discard\n");
		return -1;
	}

168
	/*
169 170 171 172 173 174
	 * 9.5.2.3 & 9.5.2.2: For BCs the BMC (an extention to it)
	 * handles the Announce (go to Passive), other messages are dropped
	 */	
	if (!memcmp(&hdr->sourcePortIdentity.clockIdentity,
		&DSPOR(ppi)->portIdentity.clockIdentity,
		sizeof(ClockIdentity))) {
baujc's avatar
baujc committed
175
		if ( get_numberPorts(DSDEF(ppi)) > 1) {
176
			/* Announces are handled by the BMC, since otherwise the state 
177
			 * also the PASSIVE states in this case is overwritten */
178 179 180 181 182
			if (hdr->messageType != PPM_ANNOUNCE) {
				/* ignore messages, except announce coming from its own clock */
				return -1;	
			}		
		}
183
	}
184
	
185 186 187 188 189 190 191 192 193 194
	/** 9.2.5 State machines
	 * INITIALIZING: No port of the clock shall place any PTP messages on its communication path.
	 * FAULTY: A port in this state shall not place any PTP messages except for
	 *         management messages that are a required response to another management message on its
	 *         communication path
	 * DISABLED: The port shall not place any messages on its communication path
	 */
	if ( ppi->state==PPS_INITIALIZING || ppi->state==PPS_DISABLED || ppi->state==PPS_FAULTY ) {
		return -1;/* ignore messages all messages */
	}
195 196 197
	return 0;
}

198 199 200 201 202 203 204 205
/* used to make basic checks before the individual state is called */
static int type_length[__PP_NR_MESSAGES_TYPES] = {
	[PPM_SYNC]		= PP_SYNC_LENGTH,
	[PPM_DELAY_REQ]		= PP_DELAY_REQ_LENGTH,
	[PPM_PDELAY_REQ]	= PP_PDELAY_REQ_LENGTH,
	[PPM_PDELAY_RESP]	= PP_PDELAY_RESP_LENGTH,
	[PPM_FOLLOW_UP]		= PP_FOLLOW_UP_LENGTH,
	[PPM_DELAY_RESP]	= PP_DELAY_RESP_LENGTH,
206
	[PPM_PDELAY_R_FUP]	= PP_PDELAY_RESP_FOLLOW_UP_LENGTH,
207
	[PPM_ANNOUNCE]		= PP_ANNOUNCE_LENGTH,
208
	[PPM_SIGNALING]		= PP_HEADER_LENGTH,
209 210 211 212
	[PPM_MANAGEMENT]	= PP_MANAGEMENT_LENGTH,
};

static int fsm_unpack_verify_frame(struct pp_instance *ppi,
213
				   void *buf, int len)
214
{
215
	int msgtype = 0;
216

217 218 219
	if (len)
		msgtype = ((*(UInteger8 *) (buf + 0)) & 0x0F);
	if (msgtype >= __PP_NR_MESSAGES_TYPES || len < type_length[msgtype])
220
		return 1; /* too short */
221
	if (((*(UInteger8 *) (buf + 1)) & 0x0F) != 2)
222
		return 1; /* wrong ptp version */
223
	return msg_unpack_header(ppi, buf, len);
224 225
}

226 227 228 229 230 231 232 233 234
/*
 * This is the state machine code. i.e. the extension-independent
 * function that runs the machine. Errors are managed and reported
 * here (based on the diag module). The returned value is the time
 * in milliseconds to wait before reentering the state machine.
 * the normal protocol. If an extended protocol is used, the table used
 * is that of the extension, otherwise the one in state-table-default.c
 */

235
int pp_state_machine(struct pp_instance *ppi, void *buf, int len)
236 237
{
	struct pp_state_table_item *ip;
238
	struct pp_time *t = &ppi->last_rcv_time;
239
	int state, err = 0;
240
	int msgtype;
241

242 243
	if (len > 0) {
		msgtype = ((*(UInteger8 *) (buf + 0)) & 0x0F);
244
		pp_diag(ppi, frames, 1,
245
			"RECV %02d bytes at %9d.%09d.%03d (type %x, %s)\n",
246
			len, (int)t->secs, (int)(t->scaled_nsecs >> 16),
247
			((int)(t->scaled_nsecs & 0xffff) * 1000) >> 16,
248
			msgtype, pp_msgtype_name[msgtype]);
249
	}
250

251 252 253
	/*
	 * Discard too short packets
	 */
254 255 256
	if (len < PP_HEADER_LENGTH) {
		len = 0;
		buf = NULL;
257 258
	}

259
	/*
260 261
	 * Since all ptp frames have the same header, parse it now,
	 * and centralize some error check.
262 263 264
	 * In case of error continue without a frame, so the current
	 * ptp state can update ppi->next_delay and return a proper value
	 */
265
	err = fsm_unpack_verify_frame(ppi, buf, len);
266
	if (err) {
267 268
		len = 0;
		buf = NULL;
269
	}
270

271 272
	state = ppi->state;

273 274 275 276 277 278 279 280 281 282
	ip = get_current_state_table_item(ppi);
	if (!ip) {
		pp_printf("fsm: Unknown state for port %s\n", ppi->port_name);
		return 10000; /* No way out. Repeat message every 10s */
	}

	/* found: handle this state */
	ppi->next_state = state;
	ppi->next_delay = 0;
	if (ppi->is_new_state)
283
		pp_diag_fsm(ppi, ip->name, STATE_ENTER, len);
284

285
	/*
286
	 * Possibly filter out buf and maybe update port state
287
	 */
288
	if (buf) {
289 290
		err = pp_packet_prefilter(ppi);
		if (err < 0) {
291 292
			buf = NULL;
			len = 0;
293 294
		}
	}
295

296
	if (ppi->state != ppi->next_state)
297
		return pp_leave_current_state(ppi);
298

baujc's avatar
baujc committed
299
	if (len) {
300
		if ( ppi->pdstate == PP_PDSTATE_WAIT_MSG ) {
301
			/* First frame received since instance initialization */
302 303 304
			int tmo=is_ext_hook_available(ppi,get_tmo_lstate_detection) ?
					(*ppi->ext_hooks->get_tmo_lstate_detection)(ppi)
					: 20000; // 20s per default
305
			pp_timeout_set(ppi,PP_TO_PROT_STATE, tmo);
306
			pdstate_set_state_pdetection(ppi);
307
		}
baujc's avatar
baujc committed
308
	} else
309
		ppi->received_ptp_header.messageType = PPM_NO_MESSAGE;
310

311
	err = ip->f1(ppi, buf, len);
312 313 314 315 316
	if (err)
		pp_printf("fsm for %s: Error %i in %s\n",
			  ppi->port_name, err, ip->name);

	/* done: if new state mark it, and enter it now (0 ms) */
317
	if (ppi->state != ppi->next_state)
318
		return pp_leave_current_state(ppi);
319

baujc's avatar
baujc committed
320
	/* Check protocol state */
321
	if ( ppi->extState==PP_EXSTATE_ACTIVE &&
322 323 324
			(ppi->pdstate==PP_PDSTATE_PDETECTION || ppi->pdstate==PP_PDSTATE_PDETECTED) &&
			pp_timeout(ppi, PP_TO_PROT_STATE) ) {
		pdstate_disable_extension(ppi);
baujc's avatar
baujc committed
325 326
	}

327 328
	/* run bmc independent of state, and since not message driven do this
	 * here 9.2.6.8 */
baujc's avatar
baujc committed
329 330 331 332
	if ( ppi->bmca_execute) {

		ppi->bmca_execute=0; /* Clear the trigger */
		ppi->next_state = bmc_apply_state_descision(ppi);
333 334 335

		/* done: if new state mark it, and enter it now (0 ms) */
		if (ppi->state != ppi->next_state)
336
			return pp_leave_current_state(ppi);
337 338
	}

339 340
	pp_diag_fsm(ppi, ip->name, STATE_LOOP, 0);

341
	/* Run the extension state machine. The extension can provide its own time-out */
342 343 344 345 346 347
	if ( is_ext_hook_available(ppi,run_ext_state_machine) ) {
		int delay = ppi->ext_hooks->run_ext_state_machine(ppi,buf,len);

		/* if new state mark it, and enter it now (0 ms) */
		if (ppi->state != ppi->next_state)
			return pp_leave_current_state(ppi);
baujc's avatar
baujc committed
348

349 350
		if (delay < ppi->next_delay)
			ppi->next_delay=delay;
351 352
	}

353
	return ppi->next_delay;
354
}