common-fun.c 8.58 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 slave and uncalibrated */
105
int st_com_slave_handle_sync(struct pp_instance *ppi, unsigned char *buf,
106
			     int len)
107
{
108
	MsgHeader *hdr = &ppi->received_ptp_header;
Alessandro Rubini's avatar
Alessandro Rubini committed
109
	MsgSync sync;
110

111
	if (!(ppi->flags & PPI_FLAG_FROM_CURRENT_PARENT))
112 113
		return 0;

114
	/* t2 may be overriden by follow-up, cField is always valid */
115
	ppi->t2 = ppi->last_rcv_time;
116
	cField_to_TimeInternal(&ppi->cField, hdr->correctionfield);
117 118

	if ((hdr->flagField[0] & PP_TWO_STEP_FLAG) != 0) {
119
		ppi->flags |= PPI_FLAG_WAITING_FOR_F_UP;
120 121
		ppi->recv_sync_sequence_id = hdr->sequenceId;
		return 0;
122
	}
123
	msg_unpack_sync(buf, &sync);
124
	ppi->flags &= ~PPI_FLAG_WAITING_FOR_F_UP;
125 126
	to_TimeInternal(&ppi->t1,
			&sync.originTimestamp);
127
	if (CONFIG_HAS_P2P && ppi->mech == PP_P2P_MECH)
128 129 130
		pp_servo_got_psync(ppi);
	else
		pp_servo_got_sync(ppi);
131 132 133
	return 0;
}

134 135 136 137 138
int st_com_peer_handle_pres(struct pp_instance *ppi, unsigned char *buf,
			    int len)
{
	MsgPDelayResp resp;
	MsgHeader *hdr = &ppi->received_ptp_header;
139
	int e = 0;
140 141 142 143 144 145 146 147 148 149 150 151 152 153 154

	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);
155 156
		if ((hdr->flagField[0] & PP_TWO_STEP_FLAG) != 0)
			ppi->flags |= PPI_FLAG_WAITING_FOR_RF_UP;
157
		else {
158
			ppi->flags &= ~PPI_FLAG_WAITING_FOR_RF_UP;
159 160 161 162 163 164
			/*
			 * Make sure responseOriginTimestamp is forced to 0
			 * for one-step responders
			 */
			memset(&ppi->t5, 0, sizeof(ppi->t5));
		}
165

166 167
		/* Save correctionField of pdelay_resp, see 11.4.3 d 3/4 */
		cField_to_TimeInternal(&ppi->cField, hdr->correctionfield);
168

169 170 171 172 173 174
		if (!(hdr->flagField[0] & PP_TWO_STEP_FLAG)) {
			if (pp_hooks.handle_presp)
				e = pp_hooks.handle_presp(ppi);
			else
				pp_servo_got_presp(ppi);
		}
175 176 177 178
	} else {
		pp_diag(ppi, frames, 2, "pp_pclock : "
			"PDelay Resp doesn't match PDelay Req\n");
	}
179
	return e;
180 181
}

182 183 184 185 186 187 188

int st_com_peer_handle_pres_followup(struct pp_instance *ppi,
				     unsigned char *buf, int plen)
{
	MsgHeader *hdr = &ppi->received_ptp_header;
	MsgPDelayRespFollowUp respFllw;
	int e = 0;
189
	TimeInternal tmp;
190

191
	if (plen < PP_PDELAY_R_FUP_LENGTH)
192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207
		/* Ignore */
		return e;

	msg_unpack_pdelay_resp_follow_up(buf, &respFllw);

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

		to_TimeInternal(&ppi->t5,
				&respFllw.responseOriginTimestamp);
208 209 210 211 212 213
		/*
		 * Add correctionField of pdelay_resp_followup to
		 * cf of pdelay_resp (see 11.4.3 d 4)
		 */
		cField_to_TimeInternal(&tmp, hdr->correctionfield);
		add_TimeInternal(&ppi->cField, &ppi->cField, &tmp);
214 215 216 217 218 219 220 221 222 223 224 225 226

		if (pp_hooks.handle_presp)
			e = pp_hooks.handle_presp(ppi);
		else
			pp_servo_got_presp(ppi);
	} else {
		pp_diag(ppi, frames, 2, "%s: "
			"PDelay Resp F-up doesn't match PDelay Req\n",
			__func__);
	}
	return e;
}

227 228 229
int st_com_peer_handle_preq(struct pp_instance *ppi, unsigned char *buf,
			    int len)
{
230 231 232 233 234 235 236
	int e = 0;

	if (pp_hooks.handle_preq)
		e = pp_hooks.handle_preq(ppi);
	if (e)
		return e;

237 238 239 240 241 242
	msg_issue_pdelay_resp(ppi, &ppi->last_rcv_time);
	msg_issue_pdelay_resp_followup(ppi, &ppi->last_snt_time);

	return 0;
}

243
/* Called by slave and uncalibrated */
244 245
int st_com_slave_handle_followup(struct pp_instance *ppi, unsigned char *buf,
				 int len)
246
{
Alessandro Rubini's avatar
Alessandro Rubini committed
247
	MsgFollowUp follow;
248
	int ret = 0;
249
	TimeInternal cField;
250

251
	MsgHeader *hdr = &ppi->received_ptp_header;
252

253
	if (!(ppi->flags & PPI_FLAG_FROM_CURRENT_PARENT)) {
254
		pp_error("%s: Follow up message is not from current parent\n",
255
			__func__);
256 257 258
		return 0;
	}

259
	if (!(ppi->flags & PPI_FLAG_WAITING_FOR_F_UP)) {
260 261
		pp_error("%s: Slave was not waiting a follow up message\n",
			__func__);
262 263 264 265
		return 0;
	}

	if (ppi->recv_sync_sequence_id != hdr->sequenceId) {
266 267
		pp_error("%s: SequenceID %d doesn't match last Sync message %d\n",
				 __func__, hdr->sequenceId, ppi->recv_sync_sequence_id);
268 269 270
		return 0;
	}

Alessandro Rubini's avatar
Alessandro Rubini committed
271
	msg_unpack_follow_up(buf, &follow);
272
	ppi->flags &= ~PPI_FLAG_WAITING_FOR_F_UP;
273
	to_TimeInternal(&ppi->t1, &follow.preciseOriginTimestamp);
274

275 276 277 278
	/* Add correctionField in follow-up to sync correctionField, see 11.2 */
	cField_to_TimeInternal(&cField, hdr->correctionfield);
	add_TimeInternal(&ppi->cField, &ppi->cField, &cField);

279 280
	/* Call the extension; it may do it all and ask to return */
	if (pp_hooks.handle_followup)
281
		ret = pp_hooks.handle_followup(ppi, &ppi->t1, &ppi->cField);
282 283 284 285 286
	if (ret == 1)
		return 0;
	if (ret < 0)
		return ret;

287
	if (CONFIG_HAS_P2P && ppi->mech == PP_P2P_MECH)
288 289 290 291
		pp_servo_got_psync(ppi);
	else
		pp_servo_got_sync(ppi);

292 293
	return 0;
}
294

295 296 297 298
/*
 * Called by master, listenting, passive.
 * FIXME: this must be implemented to support one-step masters
 */
299
int st_com_master_handle_sync(struct pp_instance *ppi, unsigned char *buf,
300
			      int len)
301
{
302
	/* No more used: follow up is sent right after the corresponding sync */
303 304
	return 0;
}
305

306
int __send_and_log(struct pp_instance *ppi, int msglen, int chtype)
307
{
308 309
	int msgtype = ((char *)ppi->tx_ptp)[0] & 0xf;

310
	if (ppi->n_ops->send(ppi, ppi->tx_frame, msglen + ppi->tx_offset,
311
			     msgtype) < msglen) {
312
		pp_diag(ppi, frames, 1, "%s(%d) Message can't be sent\n",
313
			pp_msgtype_info[msgtype].name, msgtype);
314 315 316 317 318 319
		return PP_SEND_ERROR;
	}
	/* FIXME: diagnosticst should be looped back in the send method */
	pp_diag(ppi, frames, 1, "SENT %02d bytes at %d.%09d (%s)\n", msglen,
		(int)(ppi->last_snt_time.seconds),
		(int)(ppi->last_snt_time.nanoseconds),
320
		pp_msgtype_info[msgtype].name);
321 322 323 324 325 326 327 328
	if (chtype == PP_NP_EVT && ppi->last_snt_time.correct == 0)
		return PP_SEND_NO_STAMP;

	/* count sent packets */
	ppi->ptp_tx_count++;

	return 0;
}