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
		ppi->rx_offset = ETH_HLEN;
53
		break;
54
	case PPSI_PROTO_UDP:
55
		ppi->tx_offset = 0;
56 57 58 59 60
		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);
61 62

	/* Now that ptp payload is aligned, get back the header */
63 64
	ppi->tx_frame = ppi->tx_ptp - ppi->tx_offset;
	ppi->rx_frame = ppi->rx_ptp - ppi->rx_offset;
65 66 67

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

74
/* Called by listening, passive, slave, uncalibrated */
75
int st_com_execute_slave(struct pp_instance *ppi)
76
{
77
	int ret = 0;
78 79 80 81 82 83 84 85

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

86 87 88 89 90 91
	if (pp_timeout(ppi, PP_TO_ANN_RECEIPT)
	    || pp_timeout(ppi, PP_TO_FAULT)) {
		/* 
		 * Note: TO_FAULTY == SYNCHRONIZATION_FAULT
		 * should move us to UNCALIBRATED (not implemented)
		 */
92
		ppi->frgn_rec_num = 0;
93
		if (DSDEF(ppi)->clockQuality.clockClass != PP_CLASS_SLAVE_ONLY
94
		    && (ppi->role != PPSI_ROLE_SLAVE)) {
95
			ppi->next_state = PPS_MASTER;
96
		} else {
97
			ppi->next_state = PPS_LISTENING;
98
			pp_timeout_set(ppi, PP_TO_ANN_RECEIPT);
99 100
		}
	}
101
	return 0;
102 103
}

104

105
/* Called by slave and uncalibrated */
106
int st_com_slave_handle_sync(struct pp_instance *ppi, unsigned char *buf,
107
			     int len)
108
{
109
	MsgHeader *hdr = &ppi->received_ptp_header;
Alessandro Rubini's avatar
Alessandro Rubini committed
110
	MsgSync sync;
111

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

115
	/* t2 may be overriden by follow-up, save it immediately */
116
	ppi->t2 = ppi->last_rcv_time;
117
	msg_unpack_sync(buf, &sync);
118 119

	if ((hdr->flagField[0] & PP_TWO_STEP_FLAG) != 0) {
120
		ppi->flags |= PPI_FLAG_WAITING_FOR_F_UP;
121
		ppi->recv_sync_sequence_id = hdr->sequenceId;
122 123
		/* for two-step, the stamp comes later */
		ppi->t1 = hdr->cField; /* most likely 0 */
124
		return 0;
125
	}
126
	/* one-step folllows */
127
	ppi->flags &= ~PPI_FLAG_WAITING_FOR_F_UP;
128
	ppi->t1 = sync.originTimestamp;
129 130
	pp_time_add(&ppi->t1, &hdr->cField);
	ppi->syncCF = 0;
131
	if (CONFIG_HAS_P2P && ppi->mech == PP_P2P_MECH)
132 133 134
		pp_servo_got_psync(ppi);
	else
		pp_servo_got_sync(ppi);
135 136 137
	return 0;
}

138 139 140 141
static int presp_call_servo(struct pp_instance *ppi)
{
	int ret = 0;

142
	if (is_incorrect(&ppi->t4))
143 144
		return 0; /* not an error, just no data */

145 146 147 148 149
	pp_timeout_set(ppi, PP_TO_FAULT);
	if (pp_hooks.handle_presp)
		ret = pp_hooks.handle_presp(ppi);
	else
		pp_servo_got_presp(ppi);
150

151 152 153
	return ret;
}

154 155 156 157 158
int st_com_peer_handle_pres(struct pp_instance *ppi, unsigned char *buf,
			    int len)
{
	MsgPDelayResp resp;
	MsgHeader *hdr = &ppi->received_ptp_header;
159
	int e = 0;
160 161 162 163 164 165 166 167 168 169 170 171

	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)) {

172
		ppi->t4 = resp.requestReceiptTimestamp;
173 174
		pp_time_add(&ppi->t4, &hdr->cField);
		/* WARNING: should be "sub" (see README-cfield::BUG)  */
175
		ppi->t6 = ppi->last_rcv_time;
176 177
		if ((hdr->flagField[0] & PP_TWO_STEP_FLAG) != 0)
			ppi->flags |= PPI_FLAG_WAITING_FOR_RF_UP;
178
		else {
179
			ppi->flags &= ~PPI_FLAG_WAITING_FOR_RF_UP;
180 181 182 183 184 185
			/*
			 * Make sure responseOriginTimestamp is forced to 0
			 * for one-step responders
			 */
			memset(&ppi->t5, 0, sizeof(ppi->t5));
		}
186

187 188
		if (!(hdr->flagField[0] & PP_TWO_STEP_FLAG))
			e = presp_call_servo(ppi);
189 190 191 192
	} else {
		pp_diag(ppi, frames, 2, "pp_pclock : "
			"PDelay Resp doesn't match PDelay Req\n");
	}
193
	return e;
194 195
}

196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214

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;

	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)) {

215
		ppi->t5 = respFllw.responseOriginTimestamp;
216
		pp_time_add(&ppi->t5, &hdr->cField);
217

218
		e = presp_call_servo(ppi);
219 220 221 222 223 224 225 226
	} 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

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

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

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

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

Alessandro Rubini's avatar
Alessandro Rubini committed
270
	msg_unpack_follow_up(buf, &follow);
271
	ppi->flags &= ~PPI_FLAG_WAITING_FOR_F_UP;
272 273 274 275
	/* t1 for calculations is T1 + Csyn + Cful -- see README-cfield */
	pp_time_add(&ppi->t1, &follow.preciseOriginTimestamp);
	pp_time_add(&ppi->t1, &hdr->cField);
	ppi->syncCF = hdr->cField.scaled_nsecs; /* for diag about TC */
276

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

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

290 291
	return 0;
}
292

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

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

310 311 312 313 314
	ret = ppi->n_ops->send(ppi, ppi->tx_frame, msglen + ppi->tx_offset,
			       msgtype);
	if (ret == PP_SEND_DROP)
		return 0; /* don't report as error, nor count nor log as sent */
	if (ret < msglen) {
315
		pp_diag(ppi, frames, 1, "%s(%d) Message can't be sent\n",
316
			pp_msgtype_info[msgtype].name, msgtype);
317 318 319
		return PP_SEND_ERROR;
	}
	/* FIXME: diagnosticst should be looped back in the send method */
320 321 322
	pp_diag(ppi, frames, 1, "SENT %02d bytes at %d.%09d.%03d (%s)\n",
		msglen, (int)t->secs, (int)(t->scaled_nsecs >> 16),
		((int)(t->scaled_nsecs & 0xffff) * 1000) >> 16,
323
		pp_msgtype_info[msgtype].name);
324
	if (chtype == PP_NP_EVT && is_incorrect(&ppi->last_snt_time))
325 326 327 328 329 330 331
		return PP_SEND_NO_STAMP;

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

	return 0;
}