fsm-lib.c 3.96 KB
Newer Older
1 2 3 4
/*
 * Copyright (C) 2011 CERN (www.cern.ch)
 * Author: Aurelio Colosimo
 * Copyright (C) 2014 GSI (www.gsi.de)
5
 * Author: Alessandro Rubini
6 7 8 9 10
 *
 * Released according to the GNU LGPL, version 2.1 or any later version.
 */
#include <ppsi/ppsi.h>

11 12 13 14 15 16 17 18 19 20 21 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 69 70 71 72 73 74 75 76 77 78 79 80
/* Local functions that build to nothing when Kconfig selects 0/1 vlans */
static int pp_vlan_issue_announce(struct pp_instance *ppi)
{
	int i, vlan = 0;

	if (CONFIG_VLAN_ARRAY_SIZE && ppi->nvlans == 1)
		vlan = ppi->vlans[0];

	if (CONFIG_VLAN_ARRAY_SIZE <= 1 || ppi->nvlans <= 1) {
		ppi->peer_vid = vlan;
		return msg_issue_announce(ppi);
	}

	/*
	 * If Kconfig selected 0/1 vlans, this code is not built.
	 * If we have several vlans, we replace peer_vid and proceed;
	 */
	for (i = 0; i < ppi->nvlans; i++) {
		ppi->peer_vid = ppi->vlans[i];
		msg_issue_announce(ppi);
		/* ignore errors: each vlan is separate */
	}
	return 0;
}

static int pp_vlan_issue_sync_followup(struct pp_instance *ppi)
{
	int i, vlan = 0;

	if (CONFIG_VLAN_ARRAY_SIZE && ppi->nvlans == 1)
		vlan = ppi->vlans[0];

	if (CONFIG_VLAN_ARRAY_SIZE <= 1 || ppi->nvlans <= 1) {
		ppi->peer_vid = vlan;
		return msg_issue_sync_followup(ppi);
	}

	/*
	 * If Kconfig selected 0/1 vlans, this code is not built.
	 * If we have several vlans, we replace peer_vid and proceed;
	 */
	for (i = 0; i < ppi->nvlans; i++) {
		ppi->peer_vid = ppi->vlans[i];
		msg_issue_sync_followup(ppi);
		/* ignore errors: each vlan is separate */
	}
	return 0;
}

/*
 * The following set of functions help the states in the state machine.
 * Ideally, we should manage to get to a completely table-driven fsm
 * implementation based on these helpers
 */

int pp_lib_may_issue_sync(struct pp_instance *ppi)
{
	int e;

	if (!pp_timeout(ppi, PP_TO_SYNC_SEND))
		return 0;

	pp_timeout_set(ppi, PP_TO_SYNC_SEND);
	e = pp_vlan_issue_sync_followup(ppi);
	if (e)
		pp_diag(ppi, frames, 1, "could not send sync\n");
	return e;
}

int pp_lib_may_issue_announce(struct pp_instance *ppi)
81 82 83
{
	int e;

84 85 86 87 88 89 90 91 92 93 94 95 96 97
	if (!pp_timeout(ppi, PP_TO_ANN_SEND))
		return 0;

	pp_timeout_set(ppi, PP_TO_ANN_SEND);
	e = pp_vlan_issue_announce(ppi);
	if (e)
		pp_diag(ppi, frames, 1, "could not send announce\n");
	return e;
}

int pp_lib_may_issue_request(struct pp_instance *ppi)
{
	int e = 0;

98 99 100 101
	if (!pp_timeout(ppi, PP_TO_REQUEST))
		return 0;

	pp_timeout_set(ppi, PP_TO_REQUEST);
102
	e = msg_issue_request(ppi); /* FIXME: what about multiple vlans? */
103 104
	ppi->t3 = ppi->last_snt_time;
	if (e == PP_SEND_ERROR) {
105
		pp_diag(ppi, frames, 1, "could not send request\n");
106
		return e;
107
	}
108 109
	return 0;
}
110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 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

/* Called by this file, basically when an announce is got, all states */
static void __lib_add_foreign(struct pp_instance *ppi, unsigned char *buf)
{
	int i;
	MsgHeader *hdr = &ppi->received_ptp_header;

	/* Check if foreign master is already known */
	for (i = 0; i < ppi->frgn_rec_num; i++) {
		if (!memcmp(&hdr->sourcePortIdentity,
			    &ppi->frgn_master[i].port_id,
			    sizeof(hdr->sourcePortIdentity))) {
			/* already in Foreign master data set, update info */
			msg_copy_header(&ppi->frgn_master[i].hdr, hdr);
			msg_unpack_announce(buf, &ppi->frgn_master[i].ann);
			return;
		}
	}

	/* New foreign master */
	if (ppi->frgn_rec_num < PP_NR_FOREIGN_RECORDS)
		ppi->frgn_rec_num++;

	/* FIXME: replace the worst */
	i = ppi->frgn_rec_num - 1;

	/* Copy new foreign master data set from announce message */
	memcpy(&ppi->frgn_master[i].port_id,
	       &hdr->sourcePortIdentity, sizeof(hdr->sourcePortIdentity));

	/*
	 * header and announce field of each Foreign Master are
	 * useful to run Best Master Clock Algorithm
	 */
	msg_copy_header(&ppi->frgn_master[i].hdr, hdr);
	msg_unpack_announce(buf, &ppi->frgn_master[i].ann);

	pp_diag(ppi, bmc, 1, "New foreign Master %i added\n", i);
}

int pp_lib_handle_announce(struct pp_instance *ppi, unsigned char *buf, int len)
{
	__lib_add_foreign(ppi, buf);

	ppi->next_state = bmc(ppi); /* got a new announce: run bmc */
	pp_timeout_set(ppi, PP_TO_ANN_RECEIPT);

	if (pp_hooks.handle_announce)
		return pp_hooks.handle_announce(ppi);
	return 0;
}