msg.c 20.6 KB
Newer Older
1
/*
2 3
 * Aurelio Colosimo for CERN, 2011 -- GNU LGPL v2.1 or later
 * Based on PTPd project v. 2.1.0 (see AUTHORS for details)
4 5
 */

6
#include <pptp/pptp.h>
7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66
#include <pptp/diag.h>


static inline void Integer64_display(const char *label, Integer64 *bigint)
{
	PP_VPRINTF("%s:\n", label);
	PP_VPRINTF("LSB: %u\n", bigint->lsb);
	PP_VPRINTF("MSB: %d\n", bigint->msb);
}

static inline void UInteger48_display(const char *label, UInteger48 *bigint)
{
	PP_VPRINTF("%s:\n", label);
	PP_VPRINTF("LSB: %u\n", bigint->lsb);
	PP_VPRINTF("MSB: %u\n", bigint->msb);
}

static inline void timestamp_display(const char *label, Timestamp *timestamp)
{
	PP_VPRINTF("%s:\n", label);
	UInteger48_display("seconds", &timestamp->secondsField);
	PP_VPRINTF("nanoseconds: %u\n", timestamp->nanosecondsField);
}

static inline void msg_display_header(MsgHeader *header)
{
	PP_VPRINTF("Message header: \n");
	PP_VPRINTF("\n");
	PP_VPRINTF("transportSpecific: %d\n", header->transportSpecific);
	PP_VPRINTF("messageType: %d\n", header->messageType);
	PP_VPRINTF("versionPTP: %d\n", header->versionPTP);
	PP_VPRINTF("messageLength: %d\n", header->messageLength);
	PP_VPRINTF("domainNumber: %d\n", header->domainNumber);
	PP_VPRINTF("FlagField %02hhx:%02hhx\n", header->flagField[0],
		   header->flagField[1]);
	Integer64_display("correctionfield",&header->correctionfield);
	/* FIXME diag portIdentity_display(&header->sourcePortIdentity); */
	PP_VPRINTF("sequenceId: %d\n", header->sequenceId);
	PP_VPRINTF("controlField: %d\n", header->controlField);
	PP_VPRINTF("logMessageInterval: %d\n", header->logMessageInterval);
	PP_VPRINTF("\n");
}

static inline void msg_display_announce(MsgAnnounce *announce)
{
        PP_VPRINTF("Message ANNOUNCE:\n");
        timestamp_display("Origin Timestamp", &announce->originTimestamp);
        PP_VPRINTF("currentUtcOffset: %d\n", announce->currentUtcOffset);
        PP_VPRINTF("grandMasterPriority1: %d\n",
		   announce->grandmasterPriority1);
        PP_VPRINTF("grandMasterClockQuality:\n");
        /* FIXME diag clockQuality_display(&announce->grandmasterClockQuality); */
        PP_VPRINTF("grandMasterPriority2: %d\n",
		   announce->grandmasterPriority2);
        PP_VPRINTF("grandMasterIdentity:\n");
        /* FIXME diag clockIdentity_display(announce->grandmasterIdentity); */
        PP_VPRINTF("stepsRemoved: %d\n", announce->stepsRemoved);
        PP_VPRINTF("timeSource: %d\n", announce->timeSource);
        PP_VPRINTF("\n");
}
67 68

/* Unpack header from in buffer to msg_tmp_header field */
69
void msg_unpack_header(struct pp_instance *ppi, void *buf)
70
{
71 72
	MsgHeader *hdr = &ppi->msg_tmp_header;

73 74 75
	hdr->transportSpecific = (*(Nibble *) (buf + 0)) >> 4;
	hdr->messageType = (*(Enumeration4 *) (buf + 0)) & 0x0F;
	hdr->versionPTP = (*(UInteger4 *) (buf + 1)) & 0x0F;
76

77
	/* force reserved bit to zero if not */
78
	hdr->messageLength = htons(*(UInteger16 *) (buf + 2));
79 80
	hdr->domainNumber = (*(UInteger8 *) (buf + 4));

81
	pp_memcpy(hdr->flagField, (buf + 6), PP_FLAG_FIELD_LENGTH);
82 83 84

	pp_memcpy(&hdr->correctionfield.msb, (buf + 8), 4);
	pp_memcpy(&hdr->correctionfield.lsb, (buf + 12), 4);
85 86
	hdr->correctionfield.msb = htonl(hdr->correctionfield.msb);
	hdr->correctionfield.lsb = htonl(hdr->correctionfield.lsb);
87 88 89
	pp_memcpy(hdr->sourcePortIdentity.clockIdentity, (buf + 20),
	       PP_CLOCK_IDENTITY_LENGTH);
	hdr->sourcePortIdentity.portNumber =
90 91
		htons(*(UInteger16 *) (buf + 28));
	hdr->sequenceId = htons(*(UInteger16 *) (buf + 30));
92 93 94
	hdr->controlField = (*(UInteger8 *) (buf + 32));
	hdr->logMessageInterval = (*(Integer8 *) (buf + 33));

95 96 97 98 99 100 101 102 103
	if (DSPOR(ppi)->portIdentity.portNumber ==
	    ppi->msg_tmp_header.sourcePortIdentity.portNumber
	    && !pp_memcmp(ppi->msg_tmp_header.sourcePortIdentity.clockIdentity,
			DSPOR(ppi)->portIdentity.clockIdentity,
			PP_CLOCK_IDENTITY_LENGTH))
		ppi->is_from_self = 1;
	else
		ppi->is_from_self = 0;

104 105 106 107 108 109 110 111
	if (!pp_memcmp(DSPAR(ppi)->parentPortIdentity.clockIdentity,
			hdr->sourcePortIdentity.clockIdentity,
			PP_CLOCK_IDENTITY_LENGTH) &&
			(DSPAR(ppi)->parentPortIdentity.portNumber ==
			hdr->sourcePortIdentity.portNumber))
		ppi->is_from_cur_par = 1;
	else
		ppi->is_from_cur_par = 0;
112

113
	msg_display_header(hdr);
114 115
}

116
/* Pack header message into out buffer of ppi */
117
void msg_pack_header(struct pp_instance *ppi, void *buf)
118 119 120 121 122
{
	Nibble transport = 0x80;

	/* (spec annex D) */
	*(UInteger8 *) (buf + 0) = transport;
123 124
	*(UInteger4 *) (buf + 1) = DSPOR(ppi)->versionNumber;
	*(UInteger8 *) (buf + 4) = DSDEF(ppi)->domainNumber;
125

126 127
	if (DSDEF(ppi)->twoStepFlag)
		*(UInteger8 *) (buf + 6) = PP_TWO_STEP_FLAG;
128

129 130 131 132
	pp_memset((buf + 8), 0, 8);
	pp_memcpy((buf + 20), DSPOR(ppi)->portIdentity.clockIdentity,
	       PP_CLOCK_IDENTITY_LENGTH);
	*(UInteger16 *) (buf + 28) =
133
				htons(DSPOR(ppi)->portIdentity.portNumber);
134 135 136 137
	*(UInteger8 *) (buf + 33) = 0x7F;
	/* Default value(spec Table 24) */
}

138
void *msg_copy_header(MsgHeader *dest, MsgHeader *src)
139
{
140
	return pp_memcpy(dest, src, sizeof(MsgHeader));
141 142
}

143

144
/* Pack Sync message into out buffer of ppi */
145
void msg_pack_sync(struct pp_instance *ppi, Timestamp *orig_tstamp)
146
{
147 148 149 150
	void *buf;

	buf = ppi->buf_out;

151 152 153 154
	/* changes in header */
	*(char *)(buf + 0) = *(char *)(buf + 0) & 0xF0;
	/* RAZ messageType */
	*(char *)(buf + 0) = *(char *)(buf + 0) | 0x00;
155

156
	/* Table 19 */
157 158
	*(UInteger16 *) (buf + 2) = htons(PP_SYNC_LENGTH);
	*(UInteger16 *) (buf + 30) = htons(ppi->sent_seq_id[PPM_SYNC]);
159
	*(UInteger8 *) (buf + 32) = 0x00;
160

161
	/* Table 23 */
162 163
	*(Integer8 *) (buf + 33) = DSPOR(ppi)->logSyncInterval;
	pp_memset((buf + 8), 0, 8);
164 165

	/* Sync message */
166 167 168
	*(UInteger16 *) (buf + 34) = htons(orig_tstamp->secondsField.msb);
	*(UInteger32 *) (buf + 36) = htonl(orig_tstamp->secondsField.lsb);
	*(UInteger32 *) (buf + 40) = htonl(orig_tstamp->nanosecondsField);
169 170
}

171 172
/* Unpack Sync message from in buffer */
void msg_unpack_sync(void *buf, MsgSync *sync)
173
{
174
	sync->originTimestamp.secondsField.msb =
175
		htons(*(UInteger16 *) (buf + 34));
176
	sync->originTimestamp.secondsField.lsb =
177
		htonl(*(UInteger32 *) (buf + 36));
178
	sync->originTimestamp.nanosecondsField =
179
		htonl(*(UInteger32 *) (buf + 40));
180

181 182 183
	PP_VPRINTF("Message SYNC\n");
	timestamp_display("Origin Timestamp", &sync->originTimestamp);
	PP_VPRINTF("\n");
184 185
}

186
/* Pack Announce message into out buffer of ppi */
187
void msg_pack_announce(struct pp_instance *ppi)
188
{
189 190 191
	void *buf;

	buf = ppi->buf_out;
192 193 194 195 196
	/* changes in header */
	*(char *)(buf + 0) = *(char *)(buf + 0) & 0xF0;
	/* RAZ messageType */
	*(char *)(buf + 0) = *(char *)(buf + 0) | 0x0B;
	/* Table 19 */
197 198
	*(UInteger16 *) (buf + 2) = htons(PP_ANNOUNCE_LENGTH);
	*(UInteger16 *) (buf + 30) = htons(ppi->sent_seq_id[PPM_ANNOUNCE]);
199 200
	*(UInteger8 *) (buf + 32) = 0x05;
	/* Table 23 */
201
	*(Integer8 *) (buf + 33) = DSPOR(ppi)->logAnnounceInterval;
202 203

	/* Announce message */
204
	pp_memset((buf + 34), 0, 10);
205
	*(Integer16 *) (buf + 44) = htons(DSPRO(ppi)->currentUtcOffset);
206 207 208
	*(UInteger8 *) (buf + 47) = DSPAR(ppi)->grandmasterPriority1;
	*(UInteger8 *) (buf + 48) = DSDEF(ppi)->clockQuality.clockClass;
	*(Enumeration8 *) (buf + 49) = DSDEF(ppi)->clockQuality.clockAccuracy;
209
	*(UInteger16 *) (buf + 50) =
210
		htons(DSDEF(ppi)->clockQuality.offsetScaledLogVariance);
211 212 213
	*(UInteger8 *) (buf + 52) = DSPAR(ppi)->grandmasterPriority2;
	pp_memcpy((buf + 53), DSPAR(ppi)->grandmasterIdentity,
	       PP_CLOCK_IDENTITY_LENGTH);
214
	*(UInteger16 *) (buf + 61) = htons(DSCUR(ppi)->stepsRemoved);
215
	*(Enumeration8 *) (buf + 63) = DSPRO(ppi)->timeSource;
216 217
}

218 219
/* Unpack Announce message from in buffer of ppi to msgtmp. Announce */
void msg_unpack_announce(void *buf, MsgAnnounce *ann)
220
{
221
	ann->originTimestamp.secondsField.msb =
222
		htons(*(UInteger16 *) (buf + 34));
223
	ann->originTimestamp.secondsField.lsb =
224
		htonl(*(UInteger32 *) (buf + 36));
225
	ann->originTimestamp.nanosecondsField =
226 227
		htonl(*(UInteger32 *) (buf + 40));
	ann->currentUtcOffset = htons(*(UInteger16 *) (buf + 44));
228 229
	ann->grandmasterPriority1 = *(UInteger8 *) (buf + 47);
	ann->grandmasterClockQuality.clockClass =
230
		*(UInteger8 *) (buf + 48);
231
	ann->grandmasterClockQuality.clockAccuracy =
232
		*(Enumeration8 *) (buf + 49);
233
	ann->grandmasterClockQuality.offsetScaledLogVariance =
234
		htons(*(UInteger16 *) (buf + 50));
235 236 237
	ann->grandmasterPriority2 = *(UInteger8 *) (buf + 52);
	pp_memcpy(ann->grandmasterIdentity, (buf + 53),
	       PP_CLOCK_IDENTITY_LENGTH);
238
	ann->stepsRemoved = htons(*(UInteger16 *) (buf + 61));
239 240
	ann->timeSource = *(Enumeration8 *) (buf + 63);

241
	msg_display_announce(ann);
242 243
}

244 245

/* Pack Follow Up message into out buffer of ppi*/
246
void msg_pack_follow_up(struct pp_instance *ppi, Timestamp *prec_orig_tstamp)
247
{
248 249 250 251
	void *buf;

	buf = ppi->buf_out;

252 253 254 255
	/* changes in header */
	*(char *)(buf + 0) = *(char *)(buf + 0) & 0xF0;
	/* RAZ messageType */
	*(char *)(buf + 0) = *(char *)(buf + 0) | 0x08;
256

257
	/* Table 19 */
258 259
	*(UInteger16 *) (buf + 2) = htons(PP_FOLLOW_UP_LENGTH);
	*(UInteger16 *) (buf + 30) = htons(ppi->sent_seq_id[PPM_SYNC] - 1);
260

261
	/* sentSyncSequenceId has already been incremented in msg_issue_sync */
262
	*(UInteger8 *) (buf + 32) = 0x02;
263

264
	/* Table 23 */
265
	*(Integer8 *) (buf + 33) = DSPOR(ppi)->logSyncInterval;
266

267
	/* Follow Up message */
268
	*(UInteger16 *) (buf + 34) =
269
		htons(prec_orig_tstamp->secondsField.msb);
270
	*(UInteger32 *) (buf + 36) =
271
		htonl(prec_orig_tstamp->secondsField.lsb);
272
	*(UInteger32 *) (buf + 40) =
273
		htonl(prec_orig_tstamp->nanosecondsField);
274 275
}

276 277
/* Unpack FollowUp message from in buffer of ppi to msgtmp.follow */
void msg_unpack_follow_up(void *buf, MsgFollowUp *flwup)
278
{
279
	flwup->preciseOriginTimestamp.secondsField.msb =
280
		htons(*(UInteger16 *) (buf + 34));
281
	flwup->preciseOriginTimestamp.secondsField.lsb =
282
		htonl(*(UInteger32 *) (buf + 36));
283
	flwup->preciseOriginTimestamp.nanosecondsField =
284
		htonl(*(UInteger32 *) (buf + 40));
285

286 287 288 289
	PP_VPRINTF("Message FOLLOW_UP\n");
	timestamp_display("Precise Origin Timestamp",
			  &flwup->preciseOriginTimestamp);
	PP_VPRINTF("\n");
290 291
}

292
/* pack PdelayReq message into out buffer of ppi */
293
void msg_pack_pdelay_req(struct pp_instance *ppi, Timestamp *orig_tstamp)
294
{
295 296 297 298
	void *buf;

	buf = ppi->buf_out;

299 300 301
	/* changes in header */
	*(char *)(buf + 0) = *(char *)(buf + 0) & 0xF0;
	/* RAZ messageType */
302

303
	*(char *)(buf + 0) = *(char *)(buf + 0) | 0x02;
304

305
	/* Table 19 */
306 307
	*(UInteger16 *) (buf + 2) = htons(PP_PDELAY_REQ_LENGTH);
	*(UInteger16 *) (buf + 30) = htons(ppi->sent_seq_id[PPM_PDELAY_REQ]);
308
	*(UInteger8 *) (buf + 32) = 0x05;
309

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

313
	/* Table 24 */
314
	pp_memset((buf + 8), 0, 8);
315 316

	/* Pdelay_req message */
317 318 319
	*(UInteger16 *) (buf + 34) = htons(orig_tstamp->secondsField.msb);
	*(UInteger32 *) (buf + 36) = htonl(orig_tstamp->secondsField.lsb);
	*(UInteger32 *) (buf + 40) = htonl(orig_tstamp->nanosecondsField);
320

321
	pp_memset((buf + 44), 0, 10);
322 323 324
	/* RAZ reserved octets */
}

325

326 327
/* pack DelayReq message into out buffer of ppi */
void msg_pack_delay_req(struct pp_instance *ppi, Timestamp *orig_tstamp)
328
{
329 330 331 332
	void *buf;

	buf = ppi->buf_out;

333 334 335 336
	/* changes in header */
	*(char *)(buf + 0) = *(char *)(buf + 0) & 0xF0;
	/* RAZ messageType */
	*(char *)(buf + 0) = *(char *)(buf + 0) | 0x01;
337

338
	/* Table 19 */
339 340
	*(UInteger16 *) (buf + 2) = htons(PP_DELAY_REQ_LENGTH);
	*(UInteger16 *) (buf + 30) = htons(ppi->sent_seq_id[PPM_DELAY_REQ]);
341
	*(UInteger8 *) (buf + 32) = 0x01;
342

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

346
	/* Table 24 */
347
	pp_memset((buf + 8), 0, 8);
348 349

	/* Pdelay_req message */
350 351 352
	*(UInteger16 *) (buf + 34) = htons(orig_tstamp->secondsField.msb);
	*(UInteger32 *) (buf + 36) = htonl(orig_tstamp->secondsField.lsb);
	*(UInteger32 *) (buf + 40) = htonl(orig_tstamp->nanosecondsField);
353 354
}

355

356 357
/* pack DelayResp message into OUT buffer of ppi */
void msg_pack_delay_resp(struct pp_instance *ppi,
358
			 MsgHeader *hdr, Timestamp *rcv_tstamp)
359
{
360 361 362 363
	void *buf;

	buf = ppi->buf_out;

364 365 366 367
	/* changes in header */
	*(char *)(buf + 0) = *(char *)(buf + 0) & 0xF0;
	/* RAZ messageType */
	*(char *)(buf + 0) = *(char *)(buf + 0) | 0x09;
368

369
	/* Table 19 */
370
	*(UInteger16 *) (buf + 2) = htons(PP_DELAY_RESP_LENGTH);
371 372
	*(UInteger8 *) (buf + 4) = hdr->domainNumber;
	pp_memset((buf + 8), 0, 8);
373 374

	/* Copy correctionField of PdelayReqMessage */
375 376
	*(Integer32 *) (buf + 8) = htonl(hdr->correctionfield.msb);
	*(Integer32 *) (buf + 12) = htonl(hdr->correctionfield.lsb);
377

378
	*(UInteger16 *) (buf + 30) = htons(hdr->sequenceId);
379

380
	*(UInteger8 *) (buf + 32) = 0x03;
381

382
	/* Table 23 */
383 384
	*(Integer8 *) (buf + 33) = DSPOR(ppi)->logMinDelayReqInterval;

385 386 387
	/* Table 24 */

	/* Pdelay_resp message */
388
	*(UInteger16 *) (buf + 34) =
389 390 391
		htons(rcv_tstamp->secondsField.msb);
	*(UInteger32 *) (buf + 36) = htonl(rcv_tstamp->secondsField.lsb);
	*(UInteger32 *) (buf + 40) = htonl(rcv_tstamp->nanosecondsField);
392 393
	pp_memcpy((buf + 44), hdr->sourcePortIdentity.clockIdentity,
		  PP_CLOCK_IDENTITY_LENGTH);
394
	*(UInteger16 *) (buf + 52) =
395
		htons(hdr->sourcePortIdentity.portNumber);
396 397
}

398
/* Pack PdelayResp message into out buffer of ppi */
399
void msg_pack_pdelay_resp(struct pp_instance *ppi, MsgHeader *hdr,
400
			  Timestamp *req_rec_tstamp)
401
{
402 403 404 405
	void *buf;

	buf = ppi->buf_out;

406 407 408 409 410
	/* changes in header */
	*(char *)(buf + 0) = *(char *)(buf + 0) & 0xF0;
	/* RAZ messageType */
	*(char *)(buf + 0) = *(char *)(buf + 0) | 0x03;
	/* Table 19 */
411
	*(UInteger16 *) (buf + 2) = htons(PP_PDELAY_RESP_LENGTH);
412 413
	*(UInteger8 *) (buf + 4) = hdr->domainNumber;
	pp_memset((buf + 8), 0, 8);
414

415
	*(UInteger16 *) (buf + 30) = htons(hdr->sequenceId);
416 417 418 419 420 421 422

	*(UInteger8 *) (buf + 32) = 0x05;
	/* Table 23 */
	*(Integer8 *) (buf + 33) = 0x7F;
	/* Table 24 */

	/* Pdelay_resp message */
423 424 425
	*(UInteger16 *) (buf + 34) = htons(req_rec_tstamp->secondsField.msb);
	*(UInteger32 *) (buf + 36) = htonl(req_rec_tstamp->secondsField.lsb);
	*(UInteger32 *) (buf + 40) = htonl(req_rec_tstamp->nanosecondsField);
426 427 428
	pp_memcpy((buf + 44), hdr->sourcePortIdentity.clockIdentity,
		  PP_CLOCK_IDENTITY_LENGTH);
	*(UInteger16 *) (buf + 52) =
429
		htons(hdr->sourcePortIdentity.portNumber);
430 431 432

}

433 434
/* Unpack delayReq message from in buffer of ppi to msgtmp.req */
void msg_unpack_delay_req(void *buf, MsgDelayReq *delay_req)
435
{
436
	delay_req->originTimestamp.secondsField.msb =
437
		htons(*(UInteger16 *) (buf + 34));
438
	delay_req->originTimestamp.secondsField.lsb =
439
		htonl(*(UInteger32 *) (buf + 36));
440
	delay_req->originTimestamp.nanosecondsField =
441
		htonl(*(UInteger32 *) (buf + 40));
442 443 444 445 446

	PP_VPRINTF("Message DELAY_REQ\n");
	timestamp_display("Origin Timestamp",
			  &delay_req->originTimestamp);
	PP_VPRINTF("\n");
447 448 449 450
}



451 452 453 454
/* Unpack PdelayReq message from IN buffer of ppi to msgtmp.req */
void msg_unpack_pdelay_req(void *buf, MsgPDelayReq *pdelay_req)
{
	pdelay_req->originTimestamp.secondsField.msb =
455
		htons(*(UInteger16 *) (buf + 34));
456
	pdelay_req->originTimestamp.secondsField.lsb =
457
		htonl(*(UInteger32 *) (buf + 36));
458
	pdelay_req->originTimestamp.nanosecondsField =
459
		htonl(*(UInteger32 *) (buf + 40));
460 461 462 463 464

	PP_VPRINTF("Message PDELAY_REQ\n");
	timestamp_display("Origin Timestamp",
			  &pdelay_req->originTimestamp);
	PP_VPRINTF("\n");
465 466
}

467 468
/* Unpack delayResp message from IN buffer of ppi to msgtmp.presp */
void msg_unpack_delay_resp(void *buf, MsgDelayResp *resp)
469
{
470
	resp->receiveTimestamp.secondsField.msb =
471
		htons(*(UInteger16 *) (buf + 34));
472
	resp->receiveTimestamp.secondsField.lsb =
473
		htonl(*(UInteger32 *) (buf + 36));
474
	resp->receiveTimestamp.nanosecondsField =
475
		htonl(*(UInteger32 *) (buf + 40));
476
	pp_memcpy(resp->requestingPortIdentity.clockIdentity,
477
	       (buf + 44), PP_CLOCK_IDENTITY_LENGTH);
478
	resp->requestingPortIdentity.portNumber =
479
		htons(*(UInteger16 *) (buf + 52));
480

481 482 483 484 485
	PP_VPRINTF("Message DELAY_RESP\n");
	timestamp_display("Receive Timestamp",
			  &resp->receiveTimestamp);
	/* FIXME diag display requestingPortIdentity */
	PP_VPRINTF("\n");
486 487
}

488 489
/* Unpack PdelayResp message from IN buffer of ppi to msgtmp.presp */
void msg_unpack_pdelay_resp(void *buf, MsgPDelayResp *presp)
490
{
491
	presp->requestReceiptTimestamp.secondsField.msb =
492
		htons(*(UInteger16 *) (buf + 34));
493
	presp->requestReceiptTimestamp.secondsField.lsb =
494
		htonl(*(UInteger32 *) (buf + 36));
495
	presp->requestReceiptTimestamp.nanosecondsField =
496
		htonl(*(UInteger32 *) (buf + 40));
497 498 499
	pp_memcpy(presp->requestingPortIdentity.clockIdentity,
	       (buf + 44), PP_CLOCK_IDENTITY_LENGTH);
	presp->requestingPortIdentity.portNumber =
500
		htons(*(UInteger16 *) (buf + 52));
501

502 503 504 505 506
	PP_VPRINTF("Message PDELAY_RESP\n");
	timestamp_display("Request Receipt Timestamp",
			  &presp->requestReceiptTimestamp);
	/* FIXME diag display requestingPortIdentity */
	PP_VPRINTF("\n");
507 508
}

509
/* Pack PdelayRespFollowUp message into out buffer of ppi */
510
void msg_pack_pdelay_resp_followup(struct pp_instance *ppi,
511 512
				   MsgHeader *hdr,
				   Timestamp *resp_orig_tstamp)
513
{
514 515 516 517
	void *buf;

	buf = ppi->buf_out;

518 519 520 521
	/* changes in header */
	*(char *)(buf + 0) = *(char *)(buf + 0) & 0xF0;
	/* RAZ messageType */
	*(char *)(buf + 0) = *(char *)(buf + 0) | 0x0A;
522

523
	/* Table 19 */
524 525
	*(UInteger16 *) (buf + 2) = htons(PP_PDELAY_RESP_FOLLOW_UP_LENGTH);
	*(UInteger16 *) (buf + 30) = htons(ppi->pdelay_req_hdr.sequenceId);
526
	*(UInteger8 *) (buf + 32) = 0x05;
527

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

531 532 533
	/* Table 24 */

	/* Copy correctionField of PdelayReqMessage */
534 535
	*(Integer32 *) (buf + 8) = htonl(hdr->correctionfield.msb);
	*(Integer32 *) (buf + 12) = htonl(hdr->correctionfield.lsb);
536 537

	/* Pdelay_resp_follow_up message */
538
	*(UInteger16 *) (buf + 34) =
539
		htons(resp_orig_tstamp->secondsField.msb);
540
	*(UInteger32 *) (buf + 36) =
541
		htonl(resp_orig_tstamp->secondsField.lsb);
542
	*(UInteger32 *) (buf + 40) =
543
		htonl(resp_orig_tstamp->nanosecondsField);
544 545 546
	pp_memcpy((buf + 44), hdr->sourcePortIdentity.clockIdentity,
	       PP_CLOCK_IDENTITY_LENGTH);
	*(UInteger16 *) (buf + 52) =
547
		htons(hdr->sourcePortIdentity.portNumber);
548 549
}

550 551 552
/* Unpack PdelayResp message from in buffer of ppi to msgtmp.presp */
void msg_unpack_pdelay_resp_followup(void *buf,
	MsgPDelayRespFollowUp *presp_follow)
553
{
554
	presp_follow->responseOriginTimestamp.secondsField.msb =
555
		htons(*(UInteger16 *) (buf + 34));
556
	presp_follow->responseOriginTimestamp.secondsField.lsb =
557
		htonl(*(UInteger32 *) (buf + 36));
558
	presp_follow->responseOriginTimestamp.nanosecondsField =
559
		htonl(*(UInteger32 *) (buf + 40));
560 561 562
	pp_memcpy(presp_follow->requestingPortIdentity.clockIdentity,
	       (buf + 44), PP_CLOCK_IDENTITY_LENGTH);
	presp_follow->requestingPortIdentity.portNumber =
563
		htons(*(UInteger16 *) (buf + 52));
564 565 566 567 568 569

	PP_VPRINTF("Message PDELAY_RESP_FOLLOW_UP\n");
	timestamp_display("Response Origin Timestamp",
			  &presp_follow->responseOriginTimestamp);
	/* FIXME diag display requestingPortIdentity */
	PP_VPRINTF("\n");
570
}
571 572 573 574

#define MSG_SEND_AND_RET(x,y)\
	if (pp_send_packet(ppi, ppi->buf_out, PP_## x ##_LENGTH, PP_NP_## y) <\
		PP_## x ##_LENGTH) {\
575
		PP_PRINTF("## x ## Message can't be sent -> FAULTY state!");\
576 577
		return -1;\
	}\
578
	PP_VPRINTF("## x ## Message sent");\
579 580 581 582 583 584 585 586 587 588 589 590 591 592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615 616 617 618 619 620 621 622 623 624 625 626 627 628 629 630 631 632 633 634 635 636 637 638 639 640 641 642 643 644 645 646 647 648 649 650 651 652 653 654 655 656 657 658 659 660 661 662 663 664 665 666 667 668 669 670 671 672
	ppi->sent_seq_id[PPM_## x]++;\
	return 0;

/* Pack and send on general multicast ip adress an Announce message */
int msg_issue_announce(struct pp_instance *ppi)
{
	msg_pack_announce(ppi);

	MSG_SEND_AND_RET(ANNOUNCE, GEN);
}

/* Pack and send on event multicast ip adress a Sync message */
int msg_issue_sync(struct pp_instance *ppi)
{
	Timestamp orig_tstamp;
	TimeInternal now;
	pp_get_tstamp(&now);
	from_TimeInternal(&now, &orig_tstamp);

	msg_pack_sync(ppi,&orig_tstamp);

	MSG_SEND_AND_RET(SYNC, EVT);
}

/* Pack and send on general multicast ip adress a FollowUp message */
int msg_issue_followup(struct pp_instance *ppi, TimeInternal *time)
{
	Timestamp prec_orig_tstamp;
	from_TimeInternal(time, &prec_orig_tstamp);

	msg_pack_follow_up(ppi, &prec_orig_tstamp);

	MSG_SEND_AND_RET(FOLLOW_UP, GEN);
}

/* Pack and send on event multicast ip adress a DelayReq message */
int msg_issue_delay_req(struct pp_instance *ppi)
{
	Timestamp orig_tstamp;
	TimeInternal now;
	pp_get_tstamp(&now);
	from_TimeInternal(&now, &orig_tstamp);

	msg_pack_delay_req(ppi, &orig_tstamp);

	MSG_SEND_AND_RET(DELAY_REQ, EVT);
}

/* Pack and send on event multicast ip adress a PDelayReq message */
int msg_issue_pdelay_req(struct pp_instance *ppi)
{
	Timestamp orig_tstamp;
	TimeInternal now;
	pp_get_tstamp(&now);
	from_TimeInternal(&now, &orig_tstamp);

	msg_pack_pdelay_req(ppi, &orig_tstamp);

	MSG_SEND_AND_RET(PDELAY_REQ, EVT);
}

/* Pack and send on event multicast ip adress a PDelayResp message */
int msg_issue_pdelay_resp(struct pp_instance *ppi, TimeInternal *time,
			MsgHeader *hdr)
{
	Timestamp req_rec_tstamp;
	from_TimeInternal(time, &req_rec_tstamp);
	msg_pack_pdelay_resp(ppi, hdr, &req_rec_tstamp);

	MSG_SEND_AND_RET(PDELAY_RESP, EVT);
}

/* 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);

	msg_pack_delay_resp(ppi, &ppi->delay_req_hdr, &rcv_tstamp);

	MSG_SEND_AND_RET(PDELAY_RESP, GEN);
}

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

	msg_pack_pdelay_resp_followup(ppi, &ppi->pdelay_req_hdr,
		&resp_orig_tstamp);

	MSG_SEND_AND_RET(PDELAY_RESP_FOLLOW_UP, GEN);
}