msg.c 13.9 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 8
 */

Alessandro Rubini's avatar
Alessandro Rubini committed
9
#include <ppsi/ppsi.h>
10
#include "common-fun.h"
11
#include "msg.h"
12

13
const int endianess=1; /* use to check endianess */
14

15 16 17 18 19 20 21 22 23 24 25 26
/* return 1 if the frame is from the current master, else 0 */
int msg_from_current_master(struct pp_instance *ppi)
{
	MsgHeader *hdr = &ppi->received_ptp_header;
	
	if (!bmc_pidcmp(&DSPAR(ppi)->parentPortIdentity,
			&hdr->sourcePortIdentity))
		return 1;
	else
		return 0;
}

27
/* Unpack header from in buffer to receieved_ptp_header field */
28
int msg_unpack_header(struct pp_instance *ppi, void *buf, int len)
29
{
30
	MsgHeader *hdr = &ppi->received_ptp_header;
31 32 33
	hdr->transportSpecific = (*(Nibble *) (buf + 0)) >> 4;
	hdr->messageType = (*(Enumeration4 *) (buf + 0)) & 0x0F;
	hdr->versionPTP = (*(UInteger4 *) (buf + 1)) & 0x0F;
34

35
	/* force reserved bit to zero if not */
36
	hdr->messageLength = htons(*(UInteger16 *) (buf + 2));
37 38
	hdr->domainNumber = (*(UInteger8 *) (buf + 4));

39
	memcpy(hdr->flagField, (buf + 6), PP_FLAG_FIELD_LENGTH);
40

41
	hdr->cField.secs = 0LL;
42 43
	hdr->cField.scaled_nsecs = ntohll(*(uint64_t *)(buf + 8));

44
	memcpy(&hdr->sourcePortIdentity.clockIdentity, (buf + 20),
45 46
	       PP_CLOCK_IDENTITY_LENGTH);
	hdr->sourcePortIdentity.portNumber =
47 48
		ntohs(*(UInteger16 *) (buf + 28));
	hdr->sequenceId = ntohs(*(UInteger16 *) (buf + 30));
49 50
	hdr->logMessageInterval = (*(Integer8 *) (buf + 33));

51
	return 0;
52 53
}

54
/* Pack header into output buffer -- only called by state-initializing */
55
void msg_init_header(struct pp_instance *ppi, void *buf)
56
{
57
	memset(buf, 0, 34);
58
	*(char *)(buf + 1) = DSPOR(ppi)->minorVersionNumber<<4 | (DSPOR(ppi)->versionNumber & 0xF);
59
	*(char *)(buf + 4) = DSDEF(ppi)->domainNumber;
60

61
	memcpy((buf + 20), &DSPOR(ppi)->portIdentity.clockIdentity,
62
	       PP_CLOCK_IDENTITY_LENGTH);
63
	*(UInteger16 *)(buf + 28) =
64
				htons(DSPOR(ppi)->portIdentity.portNumber);
65 66
}

67
/* set the sequence id in the buffer and update the stored one */
68
void __msg_set_seq_id(struct pp_instance *ppi, struct pp_msgtype_info *mf) {
69 70 71 72 73
	void *buf = ppi->tx_ptp;
	ppi->sent_seq[mf->msg_type]++;
	*(UInteger16 *) (buf + 30) = htons(ppi->sent_seq[mf->msg_type]); /* SequenceId */
}

74
/* Helper used by all "msg_pack" below */
75
int __msg_pack_header(struct pp_instance *ppi, struct pp_msgtype_info *msg_fmt)
76 77 78 79 80 81
{
	void *buf = ppi->tx_ptp;
	int len, log;
	uint16_t *flags16 = buf + 6;
	signed char *logp = buf + 33;

82 83 84
	len = msg_fmt->msglen;
	log = msg_fmt->logMessageInterval;
	*(char *)(buf + 0) = msg_fmt->msg_type;
85
	*(UInteger16 *) (buf + 2) = htons(len);
86
	*(uint64_t *)(buf + 8)=0; /* correctionField: default is cleared */
87
	*flags16 = 0; /* most message types wont 0 here */
88
	*(UInteger8 *)(buf + 32) = msg_fmt->controlField;
89 90 91 92 93 94 95 96 97 98 99 100 101
	switch(log) {
	case PP_LOG_ANNOUNCE:
		*logp = DSPOR(ppi)->logAnnounceInterval; break;
	case PP_LOG_SYNC:
		*logp = DSPOR(ppi)->logSyncInterval; break;
	case PP_LOG_REQUEST:
		*logp = DSPOR(ppi)->logMinDelayReqInterval; break;
	default:
		*logp = log; break;
	}
	return len;
}

102 103 104 105 106 107 108 109 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
/* Pack Signaling message into out buffer of ppi */
static int __msg_pack_signaling(struct pp_instance *ppi,enum pp_msg_format msg_fmt, PortIdentity *target_port_identity,
		UInteger16 tlv_type,UInteger16 tlv_length_field)
{
	void *buf = ppi->tx_ptp;
	int len;
	struct pp_msgtype_info *mf = pp_msgtype_info + msg_fmt;

	if (msg_fmt >=PPM_MSG_FMT_MAX)
		return 0;

	len = __msg_pack_header(ppi, mf);
	__msg_set_seq_id(ppi,mf);

	/* Set target port identity */
	*(ClockIdentity *) (buf + 34) = target_port_identity->clockIdentity;
	*(UInteger16 *) (buf + 42) = target_port_identity->portNumber;

	*(UInteger16 *) (buf + 44) = htons(tlv_type); /* TLV type*/
	*(UInteger16 *)(buf + 46)  = htons(tlv_length_field); /* TLV length field */
	return len;
}

/* Pack Signaling message into out buffer of ppi */
int msg_pack_signaling(struct pp_instance *ppi,PortIdentity *target_port_identity,
		UInteger16 tlv_type,UInteger16 tlv_length_field) {
	return __msg_pack_signaling(ppi,PPM_SIGNALING_FMT,target_port_identity,tlv_type,tlv_length_field);
}

/* Pack Signaling message into out buffer of ppi */
int msg_pack_signaling_no_fowardable(struct pp_instance *ppi,PortIdentity *target_port_identity,
		UInteger16 tlv_type,UInteger16 tlv_length_field) {
	return __msg_pack_signaling(ppi,PPM_SIGNALING_NO_FWD_FMT,target_port_identity,tlv_type,tlv_length_field);
}
/* Unpack signaling message from in buffer */
void msg_unpack_signaling(void *buf, MsgSignaling *signaling)
{
	signaling->targetPortIdentity.clockIdentity= *(ClockIdentity *) (buf + 34);
	signaling->targetPortIdentity.portNumber= *(UInteger16 *) (buf + 42);
	signaling->tlv=buf + 44;
}

144
void __pack_origin_timestamp(void *buf ,struct pp_time *orig_tstamp) {
145 146 147 148 149
	*(UInteger16 *)(buf + 34) = htons(orig_tstamp->secs >> 32);
	*(UInteger32 *)(buf + 36) = htonl(orig_tstamp->secs);
	*(UInteger32 *)(buf + 40) = htonl(orig_tstamp->scaled_nsecs >> 16);
}

150
void __unpack_origin_timestamp(void *buf ,struct pp_time *orig_tstamp) {
151 152 153 154 155
	orig_tstamp->secs  = (((int64_t)ntohs(*(UInteger16 *) (buf + 34)))<<32) |
			             ntohl(*(UInteger32 *) (buf + 36));
	orig_tstamp->scaled_nsecs = ((uint64_t)ntohl(*(UInteger32 *) (buf + 40)))<< 16;
}

156
/* Pack Sync message into out buffer of ppi */
157
int msg_pack_sync(struct pp_instance *ppi, struct pp_time *orig_tstamp)
158
{
159
	void *buf = ppi->tx_ptp;
160 161 162
	UInteger8 *flags8 = buf + 6;
	struct pp_msgtype_info *mf = pp_msgtype_info + PPM_SYNC_FMT;
	int len= __msg_pack_header(ppi, mf);
163

164
	/* Header */
165
	flags8[0] = PP_TWO_STEP_FLAG; /* Table 20 */
166
	__msg_set_seq_id(ppi,mf);
167 168

	/* Sync message */
169
	memset((buf + 34), 0, 10);
170 171 172

	/* Adjust time stamp */
	pp_time_add_interval(orig_tstamp,ppi->timestampCorrectionPortDS.egressLatency);
173
	__pack_origin_timestamp(buf,orig_tstamp);
174
	return len;
175 176
}

177 178
/* Unpack Sync message from in buffer */
void msg_unpack_sync(void *buf, MsgSync *sync)
179
{
180
	/* The cField is added in the caller according to 1-step vs. 2-step */
181
	__unpack_origin_timestamp(buf,&sync->originTimestamp);
182 183
}

184 185 186 187 188
/*
 * Setup flags for an announce message.
 * Set byte 1 of flags taking it from timepropertiesDS' flags field,
 * see 13.3.2.6, Table 20
 */
189
static void msg_set_announce_flags(struct pp_instance *ppi, UInteger8 *flags)
190
{
191
	timePropertiesDS_t *prop = DSPRO(ppi);
192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211
	const Boolean *ptrs[] = {
		&prop->leap61,
		&prop->leap59,
		&prop->currentUtcOffsetValid,
		&prop->ptpTimescale,
		&prop->timeTraceable,
		&prop->frequencyTraceable,
	};
	int i;

	/*
	 * alternate master always false, twoStepFlag false in announce,
	 * unicastFlag always false, other flags always false
	 */
	flags[0] = 0;
	for (flags[1] = 0, i = 0; i < ARRAY_SIZE(ptrs); i++)
		if (*ptrs[i])
			flags[1] |= (1 << i);
}

212
/* Pack Announce message into out buffer of ppi */
213
static int msg_pack_announce(struct pp_instance *ppi)
214
{
215 216
	void *buf = ppi->tx_ptp;
	UInteger8 *flags8 = buf + 6;;
217 218
	struct pp_msgtype_info *mf = pp_msgtype_info + PPM_ANNOUNCE_FMT;
	int len= __msg_pack_header(ppi, mf);
219

220
	/* Header */
221
	__msg_set_seq_id(ppi,mf);
222
	msg_set_announce_flags(ppi, flags8);
223 224

	/* Announce message */
225
	memset((buf + 34), 0, 30);
226
	*(Integer16 *) (buf + 44) = htons(DSPRO(ppi)->currentUtcOffset);
227
	*(UInteger8 *) (buf + 47) = DSPAR(ppi)->grandmasterPriority1;
228 229
	*(UInteger8 *) (buf + 48) = DSPAR(ppi)->grandmasterClockQuality.clockClass;
	*(Enumeration8 *) (buf + 49) = DSPAR(ppi)->grandmasterClockQuality.clockAccuracy;
230
	*(UInteger16 *) (buf + 50) =
231
		htons(DSPAR(ppi)->grandmasterClockQuality.offsetScaledLogVariance);
232
	*(UInteger8 *) (buf + 52) = DSPAR(ppi)->grandmasterPriority2;
233
	memcpy((buf + 53), &DSPAR(ppi)->grandmasterIdentity,
234
	       PP_CLOCK_IDENTITY_LENGTH);
235 236 237
	/* WORKAROUND 16bit casting doesn't seem to work on unaligned addresses */
	*(UInteger8 *) (buf + 61) = (UInteger8)(DSCUR(ppi)->stepsRemoved >> 8);
	*(UInteger8 *) (buf + 62) = (UInteger8)DSCUR(ppi)->stepsRemoved;
238
	*(Enumeration8 *) (buf + 63) = DSPRO(ppi)->timeSource;
239

baujc's avatar
baujc committed
240
	if (is_ext_hook_available(ppi,pack_announce))
241
		len = ppi->ext_hooks->pack_announce(ppi);
242
	return len;
243 244
}

245
/* Unpack Announce message from in buffer of ppi to internal structure */
246
void msg_unpack_announce(struct pp_instance *ppi, void *buf, MsgAnnounce *ann)
247
{
248 249
	__unpack_origin_timestamp(buf,&ann->originTimestamp);
	ann->currentUtcOffset = ntohs(*(UInteger16 *) (buf + 44));
250 251
	ann->grandmasterPriority1 = *(UInteger8 *) (buf + 47);
	ann->grandmasterClockQuality.clockClass =
252
		*(UInteger8 *) (buf + 48);
253
	ann->grandmasterClockQuality.clockAccuracy =
254
		*(Enumeration8 *) (buf + 49);
255
	ann->grandmasterClockQuality.offsetScaledLogVariance =
256
		ntohs(*(UInteger16 *) (buf + 50));
257
	ann->grandmasterPriority2 = *(UInteger8 *) (buf + 52);
258
	memcpy(&ann->grandmasterIdentity, (buf + 53),
259
	       PP_CLOCK_IDENTITY_LENGTH);
260 261 262
	/* WORKAROUND htons doesn't seem to work on unaligned addresses */
	ann->stepsRemoved = *(UInteger8 *)(buf + 61);
	ann->stepsRemoved = (ann->stepsRemoved << 8) + *(UInteger8 *)(buf + 62);
263
	ann->timeSource = *(Enumeration8 *) (buf + 63);
264
	bzero(ann->ext_specific,sizeof(ann->ext_specific));
265

266
	/* this can fill in extention specific flags otherwise just zero them*/
baujc's avatar
baujc committed
267
	if (is_ext_hook_available(ppi,unpack_announce))
268
		ppi->ext_hooks->unpack_announce(ppi,buf, ann);
269 270
}

271
/* Pack Follow Up message into out buffer of ppi*/
272 273
static int msg_pack_follow_up(struct pp_instance *ppi,
			       struct pp_time *prec_orig_tstamp)
274
{
275
	void *buf = ppi->tx_ptp;
276 277
	struct pp_msgtype_info *mf = pp_msgtype_info + PPM_FOLLOW_UP_FMT;
	int len= __msg_pack_header(ppi, mf);
278

279
	/* Header */
280 281 282
	/* Clause 9.5.10: The value of the sequenceId field of the Follow_Up message
	 * shall be the value of the sequenceId field of the associated Sync message.
	 */
283
	*(UInteger16 *) (buf + 30) = htons(ppi->sent_seq[PPM_SYNC]);
284

285
	/* Follow Up message */
286
	__pack_origin_timestamp(buf,prec_orig_tstamp);
287
	/* Fractional part in cField */
288 289
	*(UInteger64 *)(buf + 8) =
		htonll(prec_orig_tstamp->scaled_nsecs & 0xffff);
290
	return len;
291 292
}

293
/* Unpack FollowUp message from in buffer of ppi to internal structure */
294
void msg_unpack_follow_up(void *buf, MsgFollowUp *flwup)
295
{
296

297
	__unpack_origin_timestamp(buf,&flwup->preciseOriginTimestamp);
298
	/* cField added by the caller, from already-converted header */
299 300
}

301
/* pack DelayReq message into out buffer of ppi */
302
static int msg_pack_delay_req(struct pp_instance *ppi,
303
			       struct pp_time *now)
304
{
305
	void *buf = ppi->tx_ptp;
306 307
	struct pp_msgtype_info *mf = pp_msgtype_info + PPM_DELAY_REQ_FMT;
	int len= __msg_pack_header(ppi, mf);
308
	Integer64 correction_field;
309

310
	/* Header */
311
	__msg_set_seq_id(ppi,mf);
312

313 314 315 316 317 318 319
	correction_field=
			is_ext_hook_available(ppi,is_correction_field_compliant) &&
			!ppi->ext_hooks->is_correction_field_compliant(ppi) ?
					0 :
					-ppi->portDS->delayAsymmetry; /* Set -delayAsymmetry in CF */
	*(Integer64 *) (buf + 8) =  htonll(correction_field);

320
	/* Delay_req message - we may send zero instead */
321
	memset((buf + 34), 0, 10);
322 323
	/* Adjust time stamp */
	pp_time_add_interval(now,ppi->timestampCorrectionPortDS.egressLatency);
324
	__pack_origin_timestamp(buf,now);
325
	return len;
326 327
}

328
/* pack DelayResp message into OUT buffer of ppi */
329
static int msg_pack_delay_resp(struct pp_instance *ppi,
330
			 MsgHeader *hdr, struct pp_time *rcv_tstamp)
331
{
332
	void *buf = ppi->tx_ptp;
333 334
	struct pp_msgtype_info *mf = pp_msgtype_info + PPM_DELAY_RESP_FMT;
	int len= __msg_pack_header(ppi, mf);
335
	Integer64 correction_field, sub_ns;
336

337
	/* Header */
338 339 340 341 342 343
	sub_ns=rcv_tstamp->scaled_nsecs & 0xffff;
	correction_field=is_ext_hook_available(ppi,is_correction_field_compliant) &&
			!ppi->ext_hooks->is_correction_field_compliant(ppi) ?
					sub_ns : /* None compliant CF */
					pp_time_to_interval(&hdr->cField)-sub_ns; /* Set rxCF-sub_ns */
	*(Integer64 *) (buf + 8) =  htonll(correction_field);
344
	*(UInteger16 *) (buf + 30) = htons(hdr->sequenceId);
345

346
	/* Delay_resp message */
347
	__pack_origin_timestamp(buf,rcv_tstamp);
348

349
	memcpy((buf + 44), &hdr->sourcePortIdentity.clockIdentity,
350
		  PP_CLOCK_IDENTITY_LENGTH);
351
	*(UInteger16 *) (buf + 52) =
352
		htons(hdr->sourcePortIdentity.portNumber);
353
	return len;
354 355
}

356
/* Unpack delayReq message from in buffer of ppi to internal structure */
357
void msg_unpack_delay_req(void *buf, MsgDelayReq *delay_req)
358
{
359
	__unpack_origin_timestamp(buf,&delay_req->originTimestamp);
360 361
}

362
/* Unpack delayResp message from IN buffer of ppi to internal structure */
363
void msg_unpack_delay_resp(void *buf, MsgDelayResp *resp)
364
{
365
	__unpack_origin_timestamp(buf,&resp->receiveTimestamp);
366
	/* cfield added in the caller */
367

368
	memcpy(&resp->requestingPortIdentity.clockIdentity,
369
	       (buf + 44), PP_CLOCK_IDENTITY_LENGTH);
370
	resp->requestingPortIdentity.portNumber =
371
		ntohs(*(UInteger16 *) (buf + 52));
372 373
}

374 375 376
/* Pack and send on general multicast ip adress an Announce message */
int msg_issue_announce(struct pp_instance *ppi)
{
377 378
	int len = msg_pack_announce(ppi);

379
	return __send_and_log(ppi, len, PP_NP_GEN,PPM_ANNOUNCE_FMT);
380 381 382
}

/* Pack and send on event multicast ip adress a Sync message */
383
int msg_issue_sync_followup(struct pp_instance *ppi)
384
{
385
	struct pp_time now;
386
	int e, len;
387

388
	/* Send sync on the event channel with the "current" timestamp */
389
	TOPS(ppi)->get(ppi, &now);
390
	len = msg_pack_sync(ppi, &now);
391
	e = __send_and_log(ppi, len, PP_NP_EVT,PPM_SYNC_FMT);
392 393 394
	if (e) return e;

	/* Send followup on general channel with sent-stamp of sync */
395
	len = msg_pack_follow_up(ppi, &ppi->last_snt_time);
396
	return __send_and_log(ppi, len, PP_NP_GEN,PPM_FOLLOW_UP_FMT);
397 398
}

399

400
/* Pack and send on event multicast ip adress a DelayReq message */
401
static int msg_issue_delay_req(struct pp_instance *ppi)
402
{
403
	struct pp_time now;
404 405
	int len;

406
	TOPS(ppi)->get(ppi, &now);
407
	len = msg_pack_delay_req(ppi, &now);
408

409
	return __send_and_log(ppi, len, PP_NP_EVT,PPM_DELAY_REQ_FMT);
410 411
}

412 413
int msg_issue_request(struct pp_instance *ppi)
{
baujc's avatar
baujc committed
414
	if ( is_delayMechanismP2P(ppi) )
415 416
		return msg_issue_pdelay_req(ppi);
	return msg_issue_delay_req(ppi);
417 418
}

419
/* Pack and send on event multicast ip adress a DelayResp message */
420
int msg_issue_delay_resp(struct pp_instance *ppi, struct pp_time *t)
421
{
422 423
	int len;

424
	len = msg_pack_delay_resp(ppi, &ppi->received_ptp_header, t);
425
	return __send_and_log(ppi, len, PP_NP_GEN,PPM_DELAY_RESP_FMT);
426
}
427