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

/* Unpack header from in buffer to msg_tmp_header field */
9
void msg_unpack_header(struct pp_instance *ppi, void *buf)
10
{
11 12
	MsgHeader *hdr = &ppi->msg_tmp_header;

13 14 15
	hdr->transportSpecific = (*(Nibble *) (buf + 0)) >> 4;
	hdr->messageType = (*(Enumeration4 *) (buf + 0)) & 0x0F;
	hdr->versionPTP = (*(UInteger4 *) (buf + 1)) & 0x0F;
16

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

	/* pp_memcpy(hdr->flagField, (buf + 6), FIXME:FLAG_FIELD_LENGTH); */

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

35 36 37 38 39 40 41 42 43
	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;

44 45 46 47 48 49 50 51
	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;
52

53
/* FIXME: diag
54 55
#ifdef PTPD_DBG
	msgHeader_display(header);
56 57
#endif
*/
58 59
}

60
/* Pack header message into out buffer of ppi */
61
void msg_pack_header(struct pp_instance *ppi, void *buf)
62 63 64 65 66
{
	Nibble transport = 0x80;

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

70 71
	if (DSDEF(ppi)->twoStepFlag)
		*(UInteger8 *) (buf + 6) = PP_TWO_STEP_FLAG;
72

73 74 75 76
	pp_memset((buf + 8), 0, 8);
	pp_memcpy((buf + 20), DSPOR(ppi)->portIdentity.clockIdentity,
	       PP_CLOCK_IDENTITY_LENGTH);
	*(UInteger16 *) (buf + 28) =
77
				htons(DSPOR(ppi)->portIdentity.portNumber);
78 79 80 81
	*(UInteger8 *) (buf + 33) = 0x7F;
	/* Default value(spec Table 24) */
}

82
void *msg_copy_header(MsgHeader *dest, MsgHeader *src)
83
{
84
	return pp_memcpy(dest, src, sizeof(MsgHeader));
85 86
}

87

88
/* Pack Sync message into out buffer of ppi */
89
void msg_pack_sync(struct pp_instance *ppi, void *buf, Timestamp *orig_tstamp)
90 91 92 93 94
{
	/* changes in header */
	*(char *)(buf + 0) = *(char *)(buf + 0) & 0xF0;
	/* RAZ messageType */
	*(char *)(buf + 0) = *(char *)(buf + 0) | 0x00;
95

96
	/* Table 19 */
97 98
	*(UInteger16 *) (buf + 2) = htons(PP_SYNC_LENGTH);
	*(UInteger16 *) (buf + 30) = htons(ppi->sent_seq_id[PPM_SYNC]);
99
	*(UInteger8 *) (buf + 32) = 0x00;
100

101
	/* Table 23 */
102 103
	*(Integer8 *) (buf + 33) = DSPOR(ppi)->logSyncInterval;
	pp_memset((buf + 8), 0, 8);
104 105

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

111 112
/* Unpack Sync message from in buffer */
void msg_unpack_sync(void *buf, MsgSync *sync)
113
{
114
	sync->originTimestamp.secondsField.msb =
115
		htons(*(UInteger16 *) (buf + 34));
116
	sync->originTimestamp.secondsField.lsb =
117
		htonl(*(UInteger32 *) (buf + 36));
118
	sync->originTimestamp.nanosecondsField =
119
		htonl(*(UInteger32 *) (buf + 40));
120

121
/* FIXME: diag
122 123
#ifdef PTPD_DBG
	msgSync_display(sync);
124 125
#endif
*/
126 127
}

128
/* Pack Announce message into out buffer of ppi */
129
void msg_pack_announce(struct pp_instance *ppi, void *buf)
130 131 132 133 134 135
{
	/* changes in header */
	*(char *)(buf + 0) = *(char *)(buf + 0) & 0xF0;
	/* RAZ messageType */
	*(char *)(buf + 0) = *(char *)(buf + 0) | 0x0B;
	/* Table 19 */
136 137
	*(UInteger16 *) (buf + 2) = htons(PP_ANNOUNCE_LENGTH);
	*(UInteger16 *) (buf + 30) = htons(ppi->sent_seq_id[PPM_ANNOUNCE]);
138 139
	*(UInteger8 *) (buf + 32) = 0x05;
	/* Table 23 */
140
	*(Integer8 *) (buf + 33) = DSPOR(ppi)->logAnnounceInterval;
141 142

	/* Announce message */
143
	pp_memset((buf + 34), 0, 10);
144
	*(Integer16 *) (buf + 44) = htons(DSPRO(ppi)->currentUtcOffset);
145 146 147
	*(UInteger8 *) (buf + 47) = DSPAR(ppi)->grandmasterPriority1;
	*(UInteger8 *) (buf + 48) = DSDEF(ppi)->clockQuality.clockClass;
	*(Enumeration8 *) (buf + 49) = DSDEF(ppi)->clockQuality.clockAccuracy;
148
	*(UInteger16 *) (buf + 50) =
149
		htons(DSDEF(ppi)->clockQuality.offsetScaledLogVariance);
150 151 152
	*(UInteger8 *) (buf + 52) = DSPAR(ppi)->grandmasterPriority2;
	pp_memcpy((buf + 53), DSPAR(ppi)->grandmasterIdentity,
	       PP_CLOCK_IDENTITY_LENGTH);
153
	*(UInteger16 *) (buf + 61) = htons(DSCUR(ppi)->stepsRemoved);
154
	*(Enumeration8 *) (buf + 63) = DSPRO(ppi)->timeSource;
155 156
}

157 158
/* Unpack Announce message from in buffer of ppi to msgtmp. Announce */
void msg_unpack_announce(void *buf, MsgAnnounce *ann)
159
{
160
	ann->originTimestamp.secondsField.msb =
161
		htons(*(UInteger16 *) (buf + 34));
162
	ann->originTimestamp.secondsField.lsb =
163
		htonl(*(UInteger32 *) (buf + 36));
164
	ann->originTimestamp.nanosecondsField =
165 166
		htonl(*(UInteger32 *) (buf + 40));
	ann->currentUtcOffset = htons(*(UInteger16 *) (buf + 44));
167 168
	ann->grandmasterPriority1 = *(UInteger8 *) (buf + 47);
	ann->grandmasterClockQuality.clockClass =
169
		*(UInteger8 *) (buf + 48);
170
	ann->grandmasterClockQuality.clockAccuracy =
171
		*(Enumeration8 *) (buf + 49);
172
	ann->grandmasterClockQuality.offsetScaledLogVariance =
173
		htons(*(UInteger16 *) (buf + 50));
174 175 176
	ann->grandmasterPriority2 = *(UInteger8 *) (buf + 52);
	pp_memcpy(ann->grandmasterIdentity, (buf + 53),
	       PP_CLOCK_IDENTITY_LENGTH);
177
	ann->stepsRemoved = htons(*(UInteger16 *) (buf + 61));
178 179 180
	ann->timeSource = *(Enumeration8 *) (buf + 63);

/* FIXME: diag
181 182
#ifdef PTPD_DBG
	msgAnnounce_display(announce);
183 184
#endif
*/
185 186
}

187 188

/* Pack Follow Up message into out buffer of ppi*/
189 190
void msg_pack_follow_up(struct pp_instance *ppi, void *buf,
			Timestamp *prec_orig_tstamp)
191 192 193 194 195
{
	/* changes in header */
	*(char *)(buf + 0) = *(char *)(buf + 0) & 0xF0;
	/* RAZ messageType */
	*(char *)(buf + 0) = *(char *)(buf + 0) | 0x08;
196

197
	/* Table 19 */
198 199
	*(UInteger16 *) (buf + 2) = htons(PP_FOLLOW_UP_LENGTH);
	*(UInteger16 *) (buf + 30) = htons(ppi->sent_seq_id[PPM_SYNC] - 1);
200

201 202
	/* sentSyncSequenceId has already been incremented in "issueSync" */
	*(UInteger8 *) (buf + 32) = 0x02;
203

204
	/* Table 23 */
205
	*(Integer8 *) (buf + 33) = DSPOR(ppi)->logSyncInterval;
206

207
	/* Follow Up message */
208
	*(UInteger16 *) (buf + 34) =
209
		htons(prec_orig_tstamp->secondsField.msb);
210
	*(UInteger32 *) (buf + 36) =
211
		htonl(prec_orig_tstamp->secondsField.lsb);
212
	*(UInteger32 *) (buf + 40) =
213
		htonl(prec_orig_tstamp->nanosecondsField);
214 215
}

216 217
/* Unpack FollowUp message from in buffer of ppi to msgtmp.follow */
void msg_unpack_follow_up(void *buf, MsgFollowUp *flwup)
218
{
219
	flwup->preciseOriginTimestamp.secondsField.msb =
220
		htons(*(UInteger16 *) (buf + 34));
221
	flwup->preciseOriginTimestamp.secondsField.lsb =
222
		htonl(*(UInteger32 *) (buf + 36));
223
	flwup->preciseOriginTimestamp.nanosecondsField =
224
		htonl(*(UInteger32 *) (buf + 40));
225 226

/* FIXME: diag
227 228
#ifdef PTPD_DBG
	msgFollowUp_display(follow);
229 230
#endif
*/
231 232
}

233
/* pack PdelayReq message into out buffer of ppi */
234 235
void msg_pack_pdelay_req(struct pp_instance *ppi, void *buf,
			 Timestamp *orig_tstamp)
236 237 238 239
{
	/* changes in header */
	*(char *)(buf + 0) = *(char *)(buf + 0) & 0xF0;
	/* RAZ messageType */
240

241
	*(char *)(buf + 0) = *(char *)(buf + 0) | 0x02;
242

243
	/* Table 19 */
244 245
	*(UInteger16 *) (buf + 2) = htons(PP_PDELAY_REQ_LENGTH);
	*(UInteger16 *) (buf + 30) = htons(ppi->sent_seq_id[PPM_PDELAY_REQ]);
246
	*(UInteger8 *) (buf + 32) = 0x05;
247

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

251
	/* Table 24 */
252
	pp_memset((buf + 8), 0, 8);
253 254

	/* Pdelay_req message */
255 256 257
	*(UInteger16 *) (buf + 34) = htons(orig_tstamp->secondsField.msb);
	*(UInteger32 *) (buf + 36) = htonl(orig_tstamp->secondsField.lsb);
	*(UInteger32 *) (buf + 40) = htonl(orig_tstamp->nanosecondsField);
258

259
	pp_memset((buf + 44), 0, 10);
260 261 262
	/* RAZ reserved octets */
}

263 264

/*pack DelayReq message into out buffer of ppi*/
265 266
void msg_pack_delay_req(struct pp_instance *ppi, void *buf,
			Timestamp *orig_tstamp)
267 268 269 270 271
{
	/* changes in header */
	*(char *)(buf + 0) = *(char *)(buf + 0) & 0xF0;
	/* RAZ messageType */
	*(char *)(buf + 0) = *(char *)(buf + 0) | 0x01;
272

273
	/* Table 19 */
274 275
	*(UInteger16 *) (buf + 2) = htons(PP_DELAY_REQ_LENGTH);
	*(UInteger16 *) (buf + 30) = htons(ppi->sent_seq_id[PPM_DELAY_REQ]);
276
	*(UInteger8 *) (buf + 32) = 0x01;
277

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

281
	/* Table 24 */
282
	pp_memset((buf + 8), 0, 8);
283 284

	/* Pdelay_req message */
285 286 287
	*(UInteger16 *) (buf + 34) = htons(orig_tstamp->secondsField.msb);
	*(UInteger32 *) (buf + 36) = htonl(orig_tstamp->secondsField.lsb);
	*(UInteger32 *) (buf + 40) = htonl(orig_tstamp->nanosecondsField);
288 289
}

290 291

/*pack delayResp message into OUT buffer of ppi*/
292 293
void msg_pack_delay_resp(struct pp_instance *ppi, void *buf,
			 MsgHeader *hdr, Timestamp *rcv_tstamp)
294 295 296 297 298
{
	/* changes in header */
	*(char *)(buf + 0) = *(char *)(buf + 0) & 0xF0;
	/* RAZ messageType */
	*(char *)(buf + 0) = *(char *)(buf + 0) | 0x09;
299

300
	/* Table 19 */
301
	*(UInteger16 *) (buf + 2) = htons(PP_DELAY_RESP_LENGTH);
302 303
	*(UInteger8 *) (buf + 4) = hdr->domainNumber;
	pp_memset((buf + 8), 0, 8);
304 305

	/* Copy correctionField of PdelayReqMessage */
306 307
	*(Integer32 *) (buf + 8) = htonl(hdr->correctionfield.msb);
	*(Integer32 *) (buf + 12) = htonl(hdr->correctionfield.lsb);
308

309
	*(UInteger16 *) (buf + 30) = htons(hdr->sequenceId);
310

311
	*(UInteger8 *) (buf + 32) = 0x03;
312

313
	/* Table 23 */
314 315
	*(Integer8 *) (buf + 33) = DSPOR(ppi)->logMinDelayReqInterval;

316 317 318
	/* Table 24 */

	/* Pdelay_resp message */
319
	*(UInteger16 *) (buf + 34) =
320 321 322
		htons(rcv_tstamp->secondsField.msb);
	*(UInteger32 *) (buf + 36) = htonl(rcv_tstamp->secondsField.lsb);
	*(UInteger32 *) (buf + 40) = htonl(rcv_tstamp->nanosecondsField);
323 324
	pp_memcpy((buf + 44), hdr->sourcePortIdentity.clockIdentity,
		  PP_CLOCK_IDENTITY_LENGTH);
325
	*(UInteger16 *) (buf + 52) =
326
		htons(hdr->sourcePortIdentity.portNumber);
327 328 329 330
}



331
/* Pack PdelayResp message into out buffer of ppi */
332 333
void msg_pack_pdelay_resp(struct pp_instance *ppi, void *buf, MsgHeader *hdr,
			  Timestamp *req_rec_tstamp)
334 335 336 337 338 339
{
	/* changes in header */
	*(char *)(buf + 0) = *(char *)(buf + 0) & 0xF0;
	/* RAZ messageType */
	*(char *)(buf + 0) = *(char *)(buf + 0) | 0x03;
	/* Table 19 */
340
	*(UInteger16 *) (buf + 2) = htons(PP_PDELAY_RESP_LENGTH);
341 342
	*(UInteger8 *) (buf + 4) = hdr->domainNumber;
	pp_memset((buf + 8), 0, 8);
343

344
	*(UInteger16 *) (buf + 30) = htons(hdr->sequenceId);
345 346 347 348 349 350 351

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

	/* Pdelay_resp message */
352 353 354
	*(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);
355 356 357
	pp_memcpy((buf + 44), hdr->sourcePortIdentity.clockIdentity,
		  PP_CLOCK_IDENTITY_LENGTH);
	*(UInteger16 *) (buf + 52) =
358
		htons(hdr->sourcePortIdentity.portNumber);
359 360 361 362

}


363 364
/* Unpack delayReq message from in buffer of ppi to msgtmp.req */
void msg_unpack_delay_req(void *buf, MsgDelayReq *delay_req)
365
{
366
	delay_req->originTimestamp.secondsField.msb =
367
		htons(*(UInteger16 *) (buf + 34));
368
	delay_req->originTimestamp.secondsField.lsb =
369
		htonl(*(UInteger32 *) (buf + 36));
370
	delay_req->originTimestamp.nanosecondsField =
371
		htonl(*(UInteger32 *) (buf + 40));
372
/* FIXME: diag
373 374
#ifdef PTPD_DBG
	msgDelayReq_display(delayreq);
375 376
#endif
*/
377 378 379 380
}



381 382 383 384
/* 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 =
385
		htons(*(UInteger16 *) (buf + 34));
386
	pdelay_req->originTimestamp.secondsField.lsb =
387
		htonl(*(UInteger32 *) (buf + 36));
388
	pdelay_req->originTimestamp.nanosecondsField =
389
		htonl(*(UInteger32 *) (buf + 40));
390
/* FIXME: diag
391 392
#ifdef PTPD_DBG
	msgPDelayReq_display(pdelayreq);
393 394
#endif
*/
395 396
}

397 398
/* Unpack delayResp message from IN buffer of ppi to msgtmp.presp */
void msg_unpack_delay_resp(void *buf, MsgDelayResp *resp)
399
{
400
	resp->receiveTimestamp.secondsField.msb =
401
		htons(*(UInteger16 *) (buf + 34));
402
	resp->receiveTimestamp.secondsField.lsb =
403
		htonl(*(UInteger32 *) (buf + 36));
404
	resp->receiveTimestamp.nanosecondsField =
405
		htonl(*(UInteger32 *) (buf + 40));
406
	pp_memcpy(resp->requestingPortIdentity.clockIdentity,
407
	       (buf + 44), PP_CLOCK_IDENTITY_LENGTH);
408
	resp->requestingPortIdentity.portNumber =
409
		htons(*(UInteger16 *) (buf + 52));
410

411
/* FIXME: diag
412 413
#ifdef PTPD_DBG
	msgDelayResp_display(resp);
414 415
#endif
*/
416 417
}

418 419
/* Unpack PdelayResp message from IN buffer of ppi to msgtmp.presp */
void msg_unpack_pdelay_resp(void *buf, MsgPDelayResp *presp)
420
{
421
	presp->requestReceiptTimestamp.secondsField.msb =
422
		htons(*(UInteger16 *) (buf + 34));
423
	presp->requestReceiptTimestamp.secondsField.lsb =
424
		htonl(*(UInteger32 *) (buf + 36));
425
	presp->requestReceiptTimestamp.nanosecondsField =
426
		htonl(*(UInteger32 *) (buf + 40));
427 428 429
	pp_memcpy(presp->requestingPortIdentity.clockIdentity,
	       (buf + 44), PP_CLOCK_IDENTITY_LENGTH);
	presp->requestingPortIdentity.portNumber =
430
		htons(*(UInteger16 *) (buf + 52));
431 432

/* FIXME: diag
433 434
#ifdef PTPD_DBG
	msgPDelayResp_display(presp);
435 436
#endif
*/
437 438
}

439
/* Pack PdelayRespFollowUp message into out buffer of ppi */
440 441 442
void msg_pack_pdelay_resp_followup(struct pp_instance *ppi, void *buf,
				   MsgHeader *hdr,
				   Timestamp *resp_orig_tstamp)
443 444 445 446 447
{
	/* changes in header */
	*(char *)(buf + 0) = *(char *)(buf + 0) & 0xF0;
	/* RAZ messageType */
	*(char *)(buf + 0) = *(char *)(buf + 0) | 0x0A;
448

449
	/* Table 19 */
450 451
	*(UInteger16 *) (buf + 2) = htons(PP_PDELAY_RESP_FOLLOW_UP_LENGTH);
	*(UInteger16 *) (buf + 30) = htons(ppi->pdelay_req_hdr.sequenceId);
452
	*(UInteger8 *) (buf + 32) = 0x05;
453

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

457 458 459
	/* Table 24 */

	/* Copy correctionField of PdelayReqMessage */
460 461
	*(Integer32 *) (buf + 8) = htonl(hdr->correctionfield.msb);
	*(Integer32 *) (buf + 12) = htonl(hdr->correctionfield.lsb);
462 463

	/* Pdelay_resp_follow_up message */
464
	*(UInteger16 *) (buf + 34) =
465
		htons(resp_orig_tstamp->secondsField.msb);
466
	*(UInteger32 *) (buf + 36) =
467
		htonl(resp_orig_tstamp->secondsField.lsb);
468
	*(UInteger32 *) (buf + 40) =
469
		htonl(resp_orig_tstamp->nanosecondsField);
470 471 472
	pp_memcpy((buf + 44), hdr->sourcePortIdentity.clockIdentity,
	       PP_CLOCK_IDENTITY_LENGTH);
	*(UInteger16 *) (buf + 52) =
473
		htons(hdr->sourcePortIdentity.portNumber);
474 475
}

476 477 478
/* Unpack PdelayResp message from in buffer of ppi to msgtmp.presp */
void msg_unpack_pdelay_resp_followup(void *buf,
	MsgPDelayRespFollowUp *presp_follow)
479
{
480
	presp_follow->responseOriginTimestamp.secondsField.msb =
481
		htons(*(UInteger16 *) (buf + 34));
482
	presp_follow->responseOriginTimestamp.secondsField.lsb =
483
		htonl(*(UInteger32 *) (buf + 36));
484
	presp_follow->responseOriginTimestamp.nanosecondsField =
485
		htonl(*(UInteger32 *) (buf + 40));
486 487 488
	pp_memcpy(presp_follow->requestingPortIdentity.clockIdentity,
	       (buf + 44), PP_CLOCK_IDENTITY_LENGTH);
	presp_follow->requestingPortIdentity.portNumber =
489
		htons(*(UInteger16 *) (buf + 52));
490
}