msg.c 19.8 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

12
/* Unpack header from in buffer to receieved_ptp_header field */
13
int msg_unpack_header(struct pp_instance *ppi, void *buf, int plen)
14
{
15
	MsgHeader *hdr = &ppi->received_ptp_header;
16

17 18 19
	hdr->transportSpecific = (*(Nibble *) (buf + 0)) >> 4;
	hdr->messageType = (*(Enumeration4 *) (buf + 0)) & 0x0F;
	hdr->versionPTP = (*(UInteger4 *) (buf + 1)) & 0x0F;
20

21
	/* force reserved bit to zero if not */
22
	hdr->messageLength = htons(*(UInteger16 *) (buf + 2));
23 24
	hdr->domainNumber = (*(UInteger8 *) (buf + 4));

25
	memcpy(hdr->flagField, (buf + 6), PP_FLAG_FIELD_LENGTH);
26

27 28
	memcpy(&hdr->correctionfield.msb, (buf + 8), 4);
	memcpy(&hdr->correctionfield.lsb, (buf + 12), 4);
29 30
	hdr->correctionfield.msb = htonl(hdr->correctionfield.msb);
	hdr->correctionfield.lsb = htonl(hdr->correctionfield.lsb);
31
	memcpy(&hdr->sourcePortIdentity.clockIdentity, (buf + 20),
32 33
	       PP_CLOCK_IDENTITY_LENGTH);
	hdr->sourcePortIdentity.portNumber =
34 35
		htons(*(UInteger16 *) (buf + 28));
	hdr->sequenceId = htons(*(UInteger16 *) (buf + 30));
36 37 38
	hdr->controlField = (*(UInteger8 *) (buf + 32));
	hdr->logMessageInterval = (*(Integer8 *) (buf + 33));

39

40
	/*
41
	 * This FLAG_FROM_CURRENT_PARENT must be killed. Meanwhile, say it's
42 43 44 45
	 * from current parent if we have no current parent, so the rest works
	 */
	if (!DSPAR(ppi)->parentPortIdentity.portNumber ||
	    (!memcmp(&DSPAR(ppi)->parentPortIdentity.clockIdentity,
46
			&hdr->sourcePortIdentity.clockIdentity,
47 48
			PP_CLOCK_IDENTITY_LENGTH) &&
			(DSPAR(ppi)->parentPortIdentity.portNumber ==
49
			 hdr->sourcePortIdentity.portNumber)))
50
		ppi->flags |= PPI_FLAG_FROM_CURRENT_PARENT;
51
	else
52
		ppi->flags &= ~PPI_FLAG_FROM_CURRENT_PARENT;
53
	return 0;
54 55
}

56
/* Pack header message into out buffer of ppi */
57
void msg_pack_header(struct pp_instance *ppi, void *buf)
58
{
59 60
	/* (spec annex D and F) */
	*(UInteger8 *) (buf + 0) = 0; /* message type changed later */
61 62
	*(UInteger4 *) (buf + 1) = DSPOR(ppi)->versionNumber;
	*(UInteger8 *) (buf + 4) = DSDEF(ppi)->domainNumber;
63

64 65
	/* Zero out flags, we set them when needed */
	*(UInteger8 *) (buf + 6) = 0;
66

67
	memset((buf + 8), 0, 8);
68
	memcpy((buf + 20), &DSPOR(ppi)->portIdentity.clockIdentity,
69 70
	       PP_CLOCK_IDENTITY_LENGTH);
	*(UInteger16 *) (buf + 28) =
71
				htons(DSPOR(ppi)->portIdentity.portNumber);
72 73 74 75
	*(UInteger8 *) (buf + 33) = 0x7F;
	/* Default value(spec Table 24) */
}

76
/* Pack Sync message into out buffer of ppi */
77
static void msg_pack_sync(struct pp_instance *ppi, Timestamp *orig_tstamp)
78
{
79
	void *buf;
80
	UInteger8 *flags;
81

82
	buf = ppi->tx_ptp;
83
	flags = buf + 6;
84

85 86 87 88
	/* changes in header */
	*(char *)(buf + 0) = *(char *)(buf + 0) & 0xF0;
	/* RAZ messageType */
	*(char *)(buf + 0) = *(char *)(buf + 0) | 0x00;
89

90
	/* Table 19 */
91
	*(UInteger16 *) (buf + 2) = htons(PP_SYNC_LENGTH);
92
	ppi->sent_seq[PPM_SYNC]++;
93 94
	/* We're a two step clock, set relevant flag in sync (see Table 20) */
	flags[0] = PP_TWO_STEP_FLAG;
95
	*(UInteger16 *) (buf + 30) = htons(ppi->sent_seq[PPM_SYNC]);
96
	*(UInteger8 *) (buf + 32) = 0x00;
97

98
	/* Table 23 */
99
	*(Integer8 *) (buf + 33) = DSPOR(ppi)->logSyncInterval;
100
	memset((buf + 8), 0, 8);
101 102

	/* Sync message */
103 104 105
	*(UInteger16 *) (buf + 34) = htons(orig_tstamp->secondsField.msb);
	*(UInteger32 *) (buf + 36) = htonl(orig_tstamp->secondsField.lsb);
	*(UInteger32 *) (buf + 40) = htonl(orig_tstamp->nanosecondsField);
106 107
}

108 109
/* Unpack Sync message from in buffer */
void msg_unpack_sync(void *buf, MsgSync *sync)
110
{
111
	sync->originTimestamp.secondsField.msb =
112
		htons(*(UInteger16 *) (buf + 34));
113
	sync->originTimestamp.secondsField.lsb =
114
		htonl(*(UInteger32 *) (buf + 36));
115
	sync->originTimestamp.nanosecondsField =
116
		htonl(*(UInteger32 *) (buf + 40));
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
/*
 * Setup flags for an announce message.
 * Set byte 1 of flags taking it from timepropertiesDS' flags field,
 * see 13.3.2.6, Table 20
 */
static void msg_set_announce_flags(UInteger8 *flags, struct pp_instance *ppi)
{
	struct DSTimeProperties *prop = DSPRO(ppi);
	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);
}

147
/* Pack Announce message into out buffer of ppi */
148
static int msg_pack_announce(struct pp_instance *ppi)
149
{
150
	void *buf;
151
	UInteger8 *flags;
152

153
	buf = ppi->tx_ptp;
154
	flags = buf + 6;
155 156 157 158 159
	/* changes in header */
	*(char *)(buf + 0) = *(char *)(buf + 0) & 0xF0;
	/* RAZ messageType */
	*(char *)(buf + 0) = *(char *)(buf + 0) | 0x0B;
	/* Table 19 */
160
	*(UInteger16 *) (buf + 2) = htons(PP_ANNOUNCE_LENGTH);
161
	ppi->sent_seq[PPM_ANNOUNCE]++;
162 163 164 165 166 167
	/*
         * set byte 1 of flags taking it from timepropertiesDS' flags field,
         * see 13.3.2.6, Table 20
         */
	msg_set_announce_flags(flags, ppi);

168 169
	/* Table 21, set cf to zero */
	memset(buf + 8, 0, 8);
170
	*(UInteger16 *) (buf + 30) = htons(ppi->sent_seq[PPM_ANNOUNCE]);
171 172
	*(UInteger8 *) (buf + 32) = 0x05;
	/* Table 23 */
173
	*(Integer8 *) (buf + 33) = DSPOR(ppi)->logAnnounceInterval;
174 175

	/* Announce message */
176
	memset((buf + 34), 0, 10);
177
	*(Integer16 *) (buf + 44) = htons(DSPRO(ppi)->currentUtcOffset);
178
	*(UInteger8 *) (buf + 47) = DSPAR(ppi)->grandmasterPriority1;
179 180
	*(UInteger8 *) (buf + 48) = DSPAR(ppi)->grandmasterClockQuality.clockClass;
	*(Enumeration8 *) (buf + 49) = DSPAR(ppi)->grandmasterClockQuality.clockAccuracy;
181
	*(UInteger16 *) (buf + 50) =
182
		htons(DSPAR(ppi)->grandmasterClockQuality.offsetScaledLogVariance);
183
	*(UInteger8 *) (buf + 52) = DSPAR(ppi)->grandmasterPriority2;
184
	memcpy((buf + 53), &DSPAR(ppi)->grandmasterIdentity,
185
	       PP_CLOCK_IDENTITY_LENGTH);
186
	*(UInteger16 *) (buf + 61) = htons(DSCUR(ppi)->stepsRemoved);
187
	*(Enumeration8 *) (buf + 63) = DSPRO(ppi)->timeSource;
188

189 190
	if (pp_hooks.pack_announce)
		return pp_hooks.pack_announce(ppi);
191
	return PP_ANNOUNCE_LENGTH;
192 193
}

194
/* Unpack Announce message from in buffer of ppi to internal structure */
195
void msg_unpack_announce(void *buf, MsgAnnounce *ann)
196
{
197
	ann->originTimestamp.secondsField.msb =
198
		htons(*(UInteger16 *) (buf + 34));
199
	ann->originTimestamp.secondsField.lsb =
200
		htonl(*(UInteger32 *) (buf + 36));
201
	ann->originTimestamp.nanosecondsField =
202 203
		htonl(*(UInteger32 *) (buf + 40));
	ann->currentUtcOffset = htons(*(UInteger16 *) (buf + 44));
204 205
	ann->grandmasterPriority1 = *(UInteger8 *) (buf + 47);
	ann->grandmasterClockQuality.clockClass =
206
		*(UInteger8 *) (buf + 48);
207
	ann->grandmasterClockQuality.clockAccuracy =
208
		*(Enumeration8 *) (buf + 49);
209
	ann->grandmasterClockQuality.offsetScaledLogVariance =
210
		htons(*(UInteger16 *) (buf + 50));
211
	ann->grandmasterPriority2 = *(UInteger8 *) (buf + 52);
212
	memcpy(&ann->grandmasterIdentity, (buf + 53),
213
	       PP_CLOCK_IDENTITY_LENGTH);
214
	ann->stepsRemoved = htons(*(UInteger16 *) (buf + 61));
215 216
	ann->timeSource = *(Enumeration8 *) (buf + 63);

217 218
	if (pp_hooks.unpack_announce)
		pp_hooks.unpack_announce(buf, ann);
219 220
}

221
/* Pack Follow Up message into out buffer of ppi*/
222
static void msg_pack_follow_up(struct pp_instance *ppi, Timestamp *prec_orig_tstamp)
223
{
224 225
	void *buf;

226
	buf = ppi->tx_ptp;
227

228 229 230 231
	/* changes in header */
	*(char *)(buf + 0) = *(char *)(buf + 0) & 0xF0;
	/* RAZ messageType */
	*(char *)(buf + 0) = *(char *)(buf + 0) | 0x08;
232

233
	/* Table 19 */
234
	*(UInteger16 *) (buf + 2) = htons(PP_FOLLOW_UP_LENGTH);
235
	*(UInteger16 *) (buf + 30) = htons(ppi->sent_seq[PPM_SYNC]);
236

237
	/* sentSyncSequenceId has already been incremented in msg_issue_sync */
238
	*(UInteger8 *) (buf + 32) = 0x02;
239

240
	/* Table 23 */
241
	*(Integer8 *) (buf + 33) = DSPOR(ppi)->logSyncInterval;
242

243
	/* Follow Up message */
244
	*(UInteger16 *) (buf + 34) =
245
		htons(prec_orig_tstamp->secondsField.msb);
246
	*(UInteger32 *) (buf + 36) =
247
		htonl(prec_orig_tstamp->secondsField.lsb);
248
	*(UInteger32 *) (buf + 40) =
249
		htonl(prec_orig_tstamp->nanosecondsField);
250 251
}

252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285
/* Pack PDelay Follow Up message into out buffer of ppi*/
void msg_pack_pdelay_resp_follow_up(struct pp_instance *ppi,
				    MsgHeader * hdr,
				    Timestamp * prec_orig_tstamp)
{
	void *buf;

	buf = ppi->tx_ptp;

	/* header */
	*(char *)(buf + 0) = *(char *)(buf + 0) & 0xF0;
	/* RAZ messageType */
	*(char *)(buf + 0) = *(char *)(buf + 0) | 0x0A;

	*(UInteger16 *) (buf + 2) = htons(PP_PDELAY_RESP_LENGTH);
	*(UInteger8 *) (buf + 4) = hdr->domainNumber;
	/* copy the correction field, 11.4.3 c.3) */
	*(Integer32 *) (buf + 8) = htonl(hdr->correctionfield.msb);
	*(Integer32 *) (buf + 12) = htonl(hdr->correctionfield.lsb);

	*(UInteger16 *) (buf + 30) = htons(hdr->sequenceId);
	*(UInteger8 *) (buf + 32) = 0x05;	/* controlField */

	/* requestReceiptTimestamp */
	*(UInteger16 *) (buf + 34) = htons(prec_orig_tstamp->secondsField.msb);
	*(UInteger32 *) (buf + 36) = htonl(prec_orig_tstamp->secondsField.lsb);
	*(UInteger32 *) (buf + 40) = htonl(prec_orig_tstamp->nanosecondsField);

	/* requestingPortIdentity */
	memcpy((buf + 44), &hdr->sourcePortIdentity.clockIdentity,
	       PP_CLOCK_IDENTITY_LENGTH);
	*(UInteger16 *) (buf + 52) = htons(hdr->sourcePortIdentity.portNumber);
}

286
/* Unpack FollowUp message from in buffer of ppi to internal structure */
287
void msg_unpack_follow_up(void *buf, MsgFollowUp *flwup)
288
{
289
	flwup->preciseOriginTimestamp.secondsField.msb =
290
		htons(*(UInteger16 *) (buf + 34));
291
	flwup->preciseOriginTimestamp.secondsField.lsb =
292
		htonl(*(UInteger32 *) (buf + 36));
293
	flwup->preciseOriginTimestamp.nanosecondsField =
294
		htonl(*(UInteger32 *) (buf + 40));
295 296
}

297
/* Unpack PDelayRespFollowUp message from in buffer of ppi to internal struct */
298 299 300 301 302 303 304 305 306 307 308 309 310 311 312
void msg_unpack_pdelay_resp_follow_up(void *buf,
				      MsgPDelayRespFollowUp * pdelay_resp_flwup)
{
	pdelay_resp_flwup->responseOriginTimestamp.secondsField.msb =
	    htons(*(UInteger16 *) (buf + 34));
	pdelay_resp_flwup->responseOriginTimestamp.secondsField.lsb =
	    htonl(*(UInteger32 *) (buf + 36));
	pdelay_resp_flwup->responseOriginTimestamp.nanosecondsField =
	    htonl(*(UInteger32 *) (buf + 40));
	memcpy(&pdelay_resp_flwup->requestingPortIdentity.clockIdentity,
	       (buf + 44), PP_CLOCK_IDENTITY_LENGTH);
	pdelay_resp_flwup->requestingPortIdentity.portNumber =
	    htons(*(UInteger16 *) (buf + 52));
}

313
/* pack DelayReq message into out buffer of ppi */
314
static void msg_pack_delay_req(struct pp_instance *ppi, Timestamp *orig_tstamp)
315
{
316 317
	void *buf;

318
	buf = ppi->tx_ptp;
319

320 321 322 323
	/* changes in header */
	*(char *)(buf + 0) = *(char *)(buf + 0) & 0xF0;
	/* RAZ messageType */
	*(char *)(buf + 0) = *(char *)(buf + 0) | 0x01;
324

325
	/* Table 19 */
326
	*(UInteger16 *) (buf + 2) = htons(PP_DELAY_REQ_LENGTH);
327 328
	ppi->sent_seq[PPM_DELAY_REQ]++;
	*(UInteger16 *) (buf + 30) = htons(ppi->sent_seq[PPM_DELAY_REQ]);
329
	*(UInteger8 *) (buf + 32) = 0x01;
330

331 332
	/* Table 23 */
	*(Integer8 *) (buf + 33) = 0x7F;
333

334
	/* Table 24 */
335
	memset((buf + 8), 0, 8);
336

337
	/* Delay_req message */
338 339 340
	*(UInteger16 *) (buf + 34) = htons(orig_tstamp->secondsField.msb);
	*(UInteger32 *) (buf + 36) = htonl(orig_tstamp->secondsField.lsb);
	*(UInteger32 *) (buf + 40) = htonl(orig_tstamp->nanosecondsField);
341 342
}

343
/* pack DelayReq message into out buffer of ppi */
344 345
static void msg_pack_pdelay_req(struct pp_instance *ppi,
				Timestamp * orig_tstamp)
346 347
{
	void *buf;
348
	UInteger8 *flags;
349 350

	buf = ppi->tx_ptp;
351
	flags = buf + 6;
352 353 354 355 356 357 358

	/* changes in header 11.4.3 */
	*(char *)(buf + 0) = *(char *)(buf + 0) & 0xF0;
	/* RAZ messageType */
	*(char *)(buf + 0) = *(char *)(buf + 0) | 0x02;

	*(UInteger16 *) (buf + 2) = htons(PP_PDELAY_REQ_LENGTH);
359
	ppi->sent_seq[PPM_PDELAY_REQ]++;
360

361 362 363
	/* Reset all flags (see Table 20) */
	flags[0] = flags[1] = 0;

364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379
	/* TO DO, 11.4.3 a.1) if synthed peer-to-peer TC */
	/* *(char *)(buf + 4) = 0 .- not sythonized / X synt domain */

	memset((buf + 8), 0, 8);
	*(UInteger16 *) (buf + 30) = htons(ppi->sent_seq[PPM_PDELAY_REQ]);
	*(UInteger8 *) (buf + 32) = 0x05;

	/* Table 23 */
	*(Integer8 *) (buf + 33) = 0x7F;

	/* PDelay_req message */
	*(UInteger16 *) (buf + 34) = htons(orig_tstamp->secondsField.msb);
	*(UInteger32 *) (buf + 36) = htonl(orig_tstamp->secondsField.lsb);
	*(UInteger32 *) (buf + 40) = htonl(orig_tstamp->nanosecondsField);
}

380 381 382 383 384
/* pack PDelayResp message into OUT buffer of ppi */
void msg_pack_pdelay_resp(struct pp_instance *ppi,
			  MsgHeader * hdr, Timestamp * rcv_tstamp)
{
	void *buf;
385
	UInteger8 *flags;
386 387

	buf = ppi->tx_ptp;
388
	flags = buf + 6;
389 390 391 392 393 394 395 396

	/* header */
	*(char *)(buf + 0) = *(char *)(buf + 0) & 0xF0;
	/* RAZ messageType */
	*(char *)(buf + 0) = *(char *)(buf + 0) | 0x03;

	*(UInteger16 *) (buf + 2) = htons(PP_PDELAY_RESP_LENGTH);
	*(UInteger8 *) (buf + 4) = hdr->domainNumber;
397 398 399 400
	/* We're a two step clock, set relevant flag (see Table 20) */
	flags[0] = PP_TWO_STEP_FLAG;
	/* Flags in byte 1 are all zero for pdelay response */
	flags[1] = 0;
401 402
	/* set 0 the correction field, 11.4.3 c.3) */
	memset((buf + 8), 0, 8);
403

404 405 406 407 408 409 410 411 412 413 414 415 416 417
	*(UInteger16 *) (buf + 30) = htons(hdr->sequenceId);
	*(UInteger8 *) (buf + 32) = 0x05;	/* controlField */

	/* requestReceiptTimestamp */
	*(UInteger16 *) (buf + 34) = htons(rcv_tstamp->secondsField.msb);
	*(UInteger32 *) (buf + 36) = htonl(rcv_tstamp->secondsField.lsb);
	*(UInteger32 *) (buf + 40) = htonl(rcv_tstamp->nanosecondsField);

	/* requestingPortIdentity */
	memcpy((buf + 44), &hdr->sourcePortIdentity.clockIdentity,
	       PP_CLOCK_IDENTITY_LENGTH);
	*(UInteger16 *) (buf + 52) = htons(hdr->sourcePortIdentity.portNumber);
}

418
/* pack DelayResp message into OUT buffer of ppi */
419
static void msg_pack_delay_resp(struct pp_instance *ppi,
420
			 MsgHeader *hdr, Timestamp *rcv_tstamp)
421
{
422
	void *buf;
423
	UInteger8 *flags;
424

425
	buf = ppi->tx_ptp;
426
	flags = buf + 6;
427

428 429 430 431
	/* changes in header */
	*(char *)(buf + 0) = *(char *)(buf + 0) & 0xF0;
	/* RAZ messageType */
	*(char *)(buf + 0) = *(char *)(buf + 0) | 0x09;
432

433
	/* Table 19 */
434
	*(UInteger16 *) (buf + 2) = htons(PP_DELAY_RESP_LENGTH);
435
	*(UInteger8 *) (buf + 4) = hdr->domainNumber;
436 437
	/* Flags are all zero for delay resp, see Table 20 */
	flags[0] = flags[1] = 0;
438
	memset((buf + 8), 0, 8);
439

Aurelio Colosimo's avatar
Aurelio Colosimo committed
440
	/* Copy correctionField of delayReqMessage */
441 442
	*(Integer32 *) (buf + 8) = htonl(hdr->correctionfield.msb);
	*(Integer32 *) (buf + 12) = htonl(hdr->correctionfield.lsb);
443

444
	*(UInteger16 *) (buf + 30) = htons(hdr->sequenceId);
445

446
	*(UInteger8 *) (buf + 32) = 0x03;
447

448
	/* Table 23 */
449 450
	*(Integer8 *) (buf + 33) = DSPOR(ppi)->logMinDelayReqInterval;

451 452
	/* Table 24 */

453
	/* Delay_resp message */
454
	*(UInteger16 *) (buf + 34) =
455 456 457
		htons(rcv_tstamp->secondsField.msb);
	*(UInteger32 *) (buf + 36) = htonl(rcv_tstamp->secondsField.lsb);
	*(UInteger32 *) (buf + 40) = htonl(rcv_tstamp->nanosecondsField);
458
	memcpy((buf + 44), &hdr->sourcePortIdentity.clockIdentity,
459
		  PP_CLOCK_IDENTITY_LENGTH);
460
	*(UInteger16 *) (buf + 52) =
461
		htons(hdr->sourcePortIdentity.portNumber);
462 463
}

464
/* Unpack delayReq message from in buffer of ppi to internal structure */
465
void msg_unpack_delay_req(void *buf, MsgDelayReq *delay_req)
466
{
467
	delay_req->originTimestamp.secondsField.msb =
468
		htons(*(UInteger16 *) (buf + 34));
469
	delay_req->originTimestamp.secondsField.lsb =
470
		htonl(*(UInteger32 *) (buf + 36));
471
	delay_req->originTimestamp.nanosecondsField =
472
		htonl(*(UInteger32 *) (buf + 40));
473 474
}

475
/* Unpack PDelayReq message from in buffer of ppi to internal structure */
476 477 478 479 480 481 482 483 484 485
void msg_unpack_pdelay_req(void *buf, MsgPDelayReq * pdelay_req)
{
	pdelay_req->originTimestamp.secondsField.msb =
	    htons(*(UInteger16 *) (buf + 34));
	pdelay_req->originTimestamp.secondsField.lsb =
	    htonl(*(UInteger32 *) (buf + 36));
	pdelay_req->originTimestamp.nanosecondsField =
	    htonl(*(UInteger32 *) (buf + 40));
}

486
/* Unpack delayResp message from IN buffer of ppi to internal structure */
487
void msg_unpack_delay_resp(void *buf, MsgDelayResp *resp)
488
{
489
	resp->receiveTimestamp.secondsField.msb =
490
		htons(*(UInteger16 *) (buf + 34));
491
	resp->receiveTimestamp.secondsField.lsb =
492
		htonl(*(UInteger32 *) (buf + 36));
493
	resp->receiveTimestamp.nanosecondsField =
494
		htonl(*(UInteger32 *) (buf + 40));
495
	memcpy(&resp->requestingPortIdentity.clockIdentity,
496
	       (buf + 44), PP_CLOCK_IDENTITY_LENGTH);
497
	resp->requestingPortIdentity.portNumber =
498
		htons(*(UInteger16 *) (buf + 52));
499 500
}

501
/* Unpack PDelayResp message from IN buffer of ppi to internal structure */
502 503 504 505 506 507 508 509 510 511 512 513 514 515
void msg_unpack_pdelay_resp(void *buf, MsgPDelayResp * presp)
{
	presp->requestReceiptTimestamp.secondsField.msb =
	    htons(*(UInteger16 *) (buf + 34));
	presp->requestReceiptTimestamp.secondsField.lsb =
	    htonl(*(UInteger32 *) (buf + 36));
	presp->requestReceiptTimestamp.nanosecondsField =
	    htonl(*(UInteger32 *) (buf + 40));
	memcpy(&presp->requestingPortIdentity.clockIdentity,
	       (buf + 44), PP_CLOCK_IDENTITY_LENGTH);
	presp->requestingPortIdentity.portNumber =
	    htons(*(UInteger16 *) (buf + 52));
}

516
const char const *pp_msg_names[16] = {
517 518 519 520 521 522 523
	[PPM_SYNC] =			"sync",
	[PPM_DELAY_REQ] =		"delay_req",
	[PPM_PDELAY_REQ] =		"pdelay_req",
	[PPM_PDELAY_RESP] =		"pdelay_resp",

	[PPM_FOLLOW_UP] =		"follow_up",
	[PPM_DELAY_RESP] =		"delay_resp",
524
	[PPM_PDELAY_R_FUP] =		"pdelay_resp_follow_up",
525 526 527
	[PPM_ANNOUNCE] =		"announce",
	[PPM_SIGNALING] =		"signaling",
	[PPM_MANAGEMENT] =		"management",
528 529
};

530 531 532
/* Pack and send on general multicast ip adress an Announce message */
int msg_issue_announce(struct pp_instance *ppi)
{
533 534 535
	int len = msg_pack_announce(ppi);

	return __send_and_log(ppi, len, PPM_ANNOUNCE, PP_NP_GEN);
536 537 538
}

/* Pack and send on event multicast ip adress a Sync message */
539
int msg_issue_sync_followup(struct pp_instance *ppi)
540
{
541 542 543
	Timestamp tstamp;
	TimeInternal now, *time_snt;
	int e;
544

545 546 547 548 549 550 551 552 553 554 555
	/* Send sync on the event channel with the "current" timestamp */
	ppi->t_ops->get(ppi, &now);
	from_TimeInternal(&now, &tstamp);
	msg_pack_sync(ppi, &tstamp);
	e = __send_and_log(ppi, PP_SYNC_LENGTH, PPM_SYNC, PP_NP_EVT);
	if (e) return e;

	/* Send followup on general channel with sent-stamp of sync */
	time_snt = &ppi->last_snt_time;
	from_TimeInternal(time_snt, &tstamp);
	msg_pack_follow_up(ppi, &tstamp);
556 557
	return __send_and_log(ppi, PP_FOLLOW_UP_LENGTH, PPM_FOLLOW_UP,
			      PP_NP_GEN);
558 559
}

560 561 562 563 564 565
/* Pack and send on general multicast ip address a FollowUp message */
int msg_issue_pdelay_resp_followup(struct pp_instance *ppi, TimeInternal * time)
{
	Timestamp prec_orig_tstamp;
	from_TimeInternal(time, &prec_orig_tstamp);

566
	msg_pack_pdelay_resp_follow_up(ppi, &ppi->received_ptp_header,
567 568
				       &prec_orig_tstamp);

569 570
	return __send_and_log(ppi, PP_PDELAY_R_FUP_LENGTH,
			      PPM_PDELAY_R_FUP, PP_NP_GEN);
571 572
}

573
/* Pack and send on event multicast ip adress a DelayReq message */
574
static int msg_issue_delay_req(struct pp_instance *ppi)
575 576 577
{
	Timestamp orig_tstamp;
	TimeInternal now;
578
	ppi->t_ops->get(ppi, &now);
579 580 581 582
	from_TimeInternal(&now, &orig_tstamp);

	msg_pack_delay_req(ppi, &orig_tstamp);

583 584
	return __send_and_log(ppi, PP_DELAY_REQ_LENGTH, PPM_DELAY_REQ,
			      PP_NP_EVT);
585 586
}

587
/* Pack and send on event multicast ip adress a PDelayReq message */
588
static int msg_issue_pdelay_req(struct pp_instance *ppi)
589 590 591 592 593 594 595 596 597 598 599 600
{
	Timestamp orig_tstamp;
	TimeInternal now;
	ppi->t_ops->get(ppi, &now);
	from_TimeInternal(&now, &orig_tstamp);

	msg_pack_pdelay_req(ppi, &orig_tstamp);

	return __send_and_log(ppi, PP_PDELAY_REQ_LENGTH, PPM_PDELAY_REQ,
			      PP_NP_EVT);
}

601 602
int msg_issue_request(struct pp_instance *ppi)
{
603
	if (CONFIG_HAS_P2P && ppi->mech == PP_P2P_MECH)
604 605
		return msg_issue_pdelay_req(ppi);
	return msg_issue_delay_req(ppi);
606 607
}

608 609 610 611 612 613
/* Pack and send on event multicast ip adress a DelayResp message */
int msg_issue_delay_resp(struct pp_instance *ppi, TimeInternal *time)
{
	Timestamp rcv_tstamp;
	from_TimeInternal(time, &rcv_tstamp);

614
	msg_pack_delay_resp(ppi, &ppi->received_ptp_header, &rcv_tstamp);
615

616 617
	return __send_and_log(ppi, PP_DELAY_RESP_LENGTH, PPM_DELAY_RESP,
			      PP_NP_GEN);
618
}
619 620 621 622 623 624 625

/* Pack and send on event multicast ip adress a DelayResp message */
int msg_issue_pdelay_resp(struct pp_instance *ppi, TimeInternal * time)
{
	Timestamp rcv_tstamp;
	from_TimeInternal(time, &rcv_tstamp);

626
	msg_pack_pdelay_resp(ppi, &ppi->received_ptp_header, &rcv_tstamp);
627 628 629 630

	return __send_and_log(ppi, PP_PDELAY_RESP_LENGTH, PPM_PDELAY_RESP,
			      PP_NP_EVT);
}