common-fun.c 8.43 KB
Newer Older
1
/*
2 3
 * Copyright (C) 2011 CERN (www.cern.ch)
 * Author: Aurelio Colosimo
4
 * Based on PTPd project v. 2.1.0 (see AUTHORS for details)
5 6
 *
 * Released according to the GNU LGPL, version 2.1 or any later version.
7
 */
Alessandro Rubini's avatar
Alessandro Rubini committed
8
#include <ppsi/ppsi.h>
9
#include "common-fun.h"
10
#include "../lib/network_types.h"
11
#include "../proto-ext-whiterabbit/wr-api.h" /* FIXME: phase_to_cf_units */
12

13 14 15 16 17 18
#ifdef CONFIG_ARCH_WRS
#define ARCH_IS_WRS 1
#else
#define ARCH_IS_WRS 0
#endif

19
void *msg_copy_header(MsgHeader *dest, MsgHeader *src)
20 21 22 23
{
	return memcpy(dest, src, sizeof(MsgHeader));
}

24 25 26 27 28 29 30 31 32 33 34 35
static void *__align_pointer(void *p)
{
	unsigned long ip, align = 0;

	ip = (unsigned long)p;
	if (ip & 3)
		align = 4 - (ip & 3);
	return p + align;
}

void pp_prepare_pointers(struct pp_instance *ppi)
{
36 37 38 39 40 41 42 43 44
	/*
	 * Horrible thing: when we receive vlan, we get standard eth header,
	 * but when we send we must fill the complete vlan header.
	 * So we reserve a different number of bytes.
	 */
	switch(ppi->proto) {
	case PPSI_PROTO_RAW:
		ppi->tx_offset = ETH_HLEN; /* 14, I know! */
		ppi->rx_offset = ETH_HLEN;
45 46 47 48
	#ifdef CONFIG_ARCH_WRPC
		ppi->tx_offset = 0; /* Currently, wrpc has a separate header */
		ppi->rx_offset = 0;
	#endif
49
		break;
50 51
	case PPSI_PROTO_VLAN:
		ppi->tx_offset = sizeof(struct pp_vlanhdr);
52 53 54 55 56
		/* Hack warning: with wrs we get the whole header */
		if (ARCH_IS_WRS)
			ppi->rx_offset = sizeof(struct pp_vlanhdr);
		else
			ppi->rx_offset = ETH_HLEN;
57
		break;
58
	case PPSI_PROTO_UDP:
59
		ppi->tx_offset = 0;
60 61 62 63 64
		ppi->rx_offset = 0;
		break;
	}
	ppi->tx_ptp = __align_pointer(ppi->__tx_buffer + ppi->tx_offset);
	ppi->rx_ptp = __align_pointer(ppi->__rx_buffer + ppi->rx_offset);
65 66

	/* Now that ptp payload is aligned, get back the header */
67 68
	ppi->tx_frame = ppi->tx_ptp - ppi->tx_offset;
	ppi->rx_frame = ppi->rx_ptp - ppi->rx_offset;
69 70 71

	if (0) { /* enable to verify... it works for me though */
		pp_printf("%p -> %p %p\n",
72
			  ppi->__tx_buffer, ppi->tx_frame, ppi->tx_ptp);
73
		pp_printf("%p -> %p %p\n",
74
			  ppi->__rx_buffer, ppi->rx_frame, ppi->rx_ptp);
75 76 77
	}
}

78
/* Called by listening, passive, slave, uncalibrated */
79
int st_com_execute_slave(struct pp_instance *ppi)
80
{
81
	int ret = 0;
82 83 84 85 86 87 88 89

	if (pp_hooks.execute_slave)
		ret = pp_hooks.execute_slave(ppi);
	if (ret == 1) /* done: just return */
		return 0;
	if (ret < 0)
		return ret;

90
	if (pp_timeout(ppi, PP_TO_ANN_RECEIPT)) {
91
		ppi->frgn_rec_num = 0;
92
		if (DSDEF(ppi)->clockQuality.clockClass != PP_CLASS_SLAVE_ONLY
93
		    && (ppi->role != PPSI_ROLE_SLAVE)) {
94
			ppi->next_state = PPS_MASTER;
95
		} else {
96
			ppi->next_state = PPS_LISTENING;
97
			pp_timeout_set(ppi, PP_TO_ANN_RECEIPT);
98 99
		}
	}
100
	return 0;
101 102
}

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

109
	/* Check if foreign master is already known */
110
	for (i = 0; i < ppi->frgn_rec_num; i++) {
111
		if (!memcmp(&hdr->sourcePortIdentity,
112
			    &ppi->frgn_master[i].port_id,
113
			    sizeof(hdr->sourcePortIdentity))) {
114
			/* already in Foreign master data set, update info */
115 116
			msg_copy_header(&ppi->frgn_master[i].hdr, hdr);
			msg_unpack_announce(buf, &ppi->frgn_master[i].ann);
117
			return;
118 119 120
		}
	}

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

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

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

132 133
	/*
	 * header and announce field of each Foreign Master are
Aurelio Colosimo's avatar
Aurelio Colosimo committed
134
	 * useful to run Best Master Clock Algorithm
135
	 */
136 137
	msg_copy_header(&ppi->frgn_master[i].hdr, hdr);
	msg_unpack_announce(buf, &ppi->frgn_master[i].ann);
138

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


143
/* Called by slave and uncalibrated */
144 145
int st_com_slave_handle_announce(struct pp_instance *ppi, unsigned char *buf,
				 int len)
146 147 148 149
{
	if (len < PP_ANNOUNCE_LENGTH)
		return -1;

150 151
	/* st_com_add_foreign takes care of announce unpacking */
	st_com_add_foreign(ppi, buf);
152 153

	/*Reset Timer handling Announce receipt timeout*/
154
	pp_timeout_set(ppi, PP_TO_ANN_RECEIPT);
155

156 157
	ppi->next_state = bmc(ppi); /* got a new announce: run bmc */

158 159 160
	if (pp_hooks.handle_announce)
		pp_hooks.handle_announce(ppi);

161 162
	return 0;
}
163

164
/* Called by slave and uncalibrated */
165
int st_com_slave_handle_sync(struct pp_instance *ppi, unsigned char *buf,
166
			     int len)
167
{
168
	MsgHeader *hdr = &ppi->received_ptp_header;
Alessandro Rubini's avatar
Alessandro Rubini committed
169
	MsgSync sync;
170 171 172

	if (len < PP_SYNC_LENGTH)
		return -1;
173
	if (!(ppi->flags & PPI_FLAG_FROM_CURRENT_PARENT))
174 175
		return 0;

176
	/* t2 may be overriden by follow-up, cField is always valid */
177
	ppi->t2 = ppi->last_rcv_time;
178
	cField_to_TimeInternal(&ppi->cField, hdr->correctionfield);
179 180

	if ((hdr->flagField[0] & PP_TWO_STEP_FLAG) != 0) {
181
		ppi->flags |= PPI_FLAG_WAITING_FOR_F_UP;
182 183
		ppi->recv_sync_sequence_id = hdr->sequenceId;
		return 0;
184
	}
185
	msg_unpack_sync(buf, &sync);
186
	ppi->flags &= ~PPI_FLAG_WAITING_FOR_F_UP;
187 188
	to_TimeInternal(&ppi->t1,
			&sync.originTimestamp);
189 190 191 192
	if (GLBS(ppi)->delay_mech)
		pp_servo_got_psync(ppi);
	else
		pp_servo_got_sync(ppi);
193 194 195
	return 0;
}

196 197 198 199 200 201 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 231 232
int st_com_peer_handle_pres(struct pp_instance *ppi, unsigned char *buf,
			    int len)
{
	MsgPDelayResp resp;
	MsgHeader *hdr = &ppi->received_ptp_header;

	if (len < PP_PDELAY_RESP_LENGTH)
		return -1;

	msg_unpack_pdelay_resp(buf, &resp);

	if ((memcmp(&DSPOR(ppi)->portIdentity.clockIdentity,
		    &resp.requestingPortIdentity.clockIdentity,
		    PP_CLOCK_IDENTITY_LENGTH) == 0) &&
	    ((ppi->sent_seq[PPM_PDELAY_REQ]) ==
	     hdr->sequenceId) &&
	    (DSPOR(ppi)->portIdentity.portNumber ==
	     resp.requestingPortIdentity.portNumber) &&
	    (ppi->flags & PPI_FLAG_FROM_CURRENT_PARENT)) {

		to_TimeInternal(&ppi->t4, &resp.requestReceiptTimestamp);
		ppi->t6 = ppi->last_rcv_time;
		ppi->t6_cf = phase_to_cf_units(ppi->last_rcv_time.phase);
		ppi->flags |= PPI_FLAG_WAITING_FOR_RF_UP;

		/* todo: in one clock the presp carries t5-t4 */

	} else {
		pp_diag(ppi, frames, 2, "pp_pclock : "
			"PDelay Resp doesn't match PDelay Req\n");
	}
	return 0;
}

int st_com_peer_handle_preq(struct pp_instance *ppi, unsigned char *buf,
			    int len)
{
233 234
	int e = 0;

235 236 237
	if (len < PP_PDELAY_REQ_LENGTH)
		return -1;

238 239 240 241 242
	if (pp_hooks.handle_preq)
		e = pp_hooks.handle_preq(ppi);
	if (e)
		return e;

243 244 245 246 247 248
	msg_issue_pdelay_resp(ppi, &ppi->last_rcv_time);
	msg_issue_pdelay_resp_followup(ppi, &ppi->last_snt_time);

	return 0;
}

249
/* Called by slave and uncalibrated */
250 251
int st_com_slave_handle_followup(struct pp_instance *ppi, unsigned char *buf,
				 int len)
252
{
Alessandro Rubini's avatar
Alessandro Rubini committed
253
	MsgFollowUp follow;
254
	int ret = 0;
255
	TimeInternal cField;
256

257
	MsgHeader *hdr = &ppi->received_ptp_header;
258

259 260 261
	if (len < PP_FOLLOW_UP_LENGTH)
		return -1;

262
	if (!(ppi->flags & PPI_FLAG_FROM_CURRENT_PARENT)) {
263
		pp_error("%s: Follow up message is not from current parent\n",
264
			__func__);
265 266 267
		return 0;
	}

268
	if (!(ppi->flags & PPI_FLAG_WAITING_FOR_F_UP)) {
269 270
		pp_error("%s: Slave was not waiting a follow up message\n",
			__func__);
271 272 273 274
		return 0;
	}

	if (ppi->recv_sync_sequence_id != hdr->sequenceId) {
275 276
		pp_error("%s: SequenceID %d doesn't match last Sync message %d\n",
				 __func__, hdr->sequenceId, ppi->recv_sync_sequence_id);
277 278 279
		return 0;
	}

Alessandro Rubini's avatar
Alessandro Rubini committed
280
	msg_unpack_follow_up(buf, &follow);
281
	ppi->flags &= ~PPI_FLAG_WAITING_FOR_F_UP;
282
	to_TimeInternal(&ppi->t1, &follow.preciseOriginTimestamp);
283

284 285 286 287
	/* Add correctionField in follow-up to sync correctionField, see 11.2 */
	cField_to_TimeInternal(&cField, hdr->correctionfield);
	add_TimeInternal(&ppi->cField, &ppi->cField, &cField);

288 289
	/* Call the extension; it may do it all and ask to return */
	if (pp_hooks.handle_followup)
290
		ret = pp_hooks.handle_followup(ppi, &ppi->t1, &ppi->cField);
291 292 293 294 295
	if (ret == 1)
		return 0;
	if (ret < 0)
		return ret;

296 297 298 299 300
	if (GLBS(ppi)->delay_mech)
		pp_servo_got_psync(ppi);
	else
		pp_servo_got_sync(ppi);

301 302
	return 0;
}
303

304
/* Called by master, listenting, passive. */
305 306
int st_com_master_handle_announce(struct pp_instance *ppi, unsigned char *buf,
				  int len)
307 308 309 310
{
	if (len < PP_ANNOUNCE_LENGTH)
		return -1;

311
	pp_diag(ppi, bmc, 2, "Announce message from another foreign master\n");
312

313
	st_com_add_foreign(ppi, buf);
314
	ppi->next_state = bmc(ppi); /* got a new announce: run bmc */
315 316 317 318

	if (pp_hooks.handle_announce)
		pp_hooks.handle_announce(ppi);

319 320 321
	return 0;
}

322 323 324 325
/*
 * Called by master, listenting, passive.
 * FIXME: this must be implemented to support one-step masters
 */
326
int st_com_master_handle_sync(struct pp_instance *ppi, unsigned char *buf,
327
			      int len)
328
{
329
	/* No more used: follow up is sent right after the corresponding sync */
330 331
	return 0;
}