msg.c 14.9 KB
Newer Older
1 2
/*
 * FIXME header
3 4
 */

5
#include <pptp/pptp.h>
6 7

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

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

16
	/* force reserved bit to zero if not */
17
	hdr->messageLength = htons(*(UInteger16 *) (buf + 2));
18 19 20 21 22 23
	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);
24 25
	hdr->correctionfield.msb = htonl(hdr->correctionfield.msb);
	hdr->correctionfield.lsb = htonl(hdr->correctionfield.lsb);
26 27 28
	pp_memcpy(hdr->sourcePortIdentity.clockIdentity, (buf + 20),
	       PP_CLOCK_IDENTITY_LENGTH);
	hdr->sourcePortIdentity.portNumber =
29 30
		htons(*(UInteger16 *) (buf + 28));
	hdr->sequenceId = htons(*(UInteger16 *) (buf + 30));
31 32 33
	hdr->controlField = (*(UInteger8 *) (buf + 32));
	hdr->logMessageInterval = (*(Integer8 *) (buf + 33));

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

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

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

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

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

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

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

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

86

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

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

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

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

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

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

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

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

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

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

186 187

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

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

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

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

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

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

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

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

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

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

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

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

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

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

262 263

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

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

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

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

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

289 290

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

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

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

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

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

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

315 316 317
	/* Table 24 */

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



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

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

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

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

}


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



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

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

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

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

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

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

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

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

456 457 458
	/* Table 24 */

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

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

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