mirror of
https://gitee.com/Vancouver2017/luban-lite-t3e-pro.git
synced 2025-12-14 18:38:55 +00:00
291 lines
13 KiB
C
291 lines
13 KiB
C
/* msg.c */
|
|
|
|
#include "../ptpd.h"
|
|
|
|
/* Unpack header message */
|
|
void msgUnpackHeader(const octet_t *buf, MsgHeader *header)
|
|
{
|
|
int32_t msb;
|
|
uint32_t lsb;
|
|
|
|
header->transportSpecific = (*(nibble_t*)(buf + 0)) >> 4;
|
|
header->messageType = (*(enum4bit_t*)(buf + 0)) & 0x0F;
|
|
header->versionPTP = (*(uint4bit_t*)(buf + 1)) & 0x0F; //force reserved bit to zero if not
|
|
header->messageLength = flip16(*(int16_t*)(buf + 2));
|
|
header->domainNumber = (*(uint8_t*)(buf + 4));
|
|
memcpy(header->flagField, (buf + 6), FLAG_FIELD_LENGTH);
|
|
memcpy(&msb, (buf + 8), 4);
|
|
memcpy(&lsb, (buf + 12), 4);
|
|
header->correctionfield = flip32(msb);
|
|
header->correctionfield <<= 32;
|
|
header->correctionfield += flip32(lsb);
|
|
memcpy(header->sourcePortIdentity.clockIdentity, (buf + 20), CLOCK_IDENTITY_LENGTH);
|
|
header->sourcePortIdentity.portNumber = flip16(*(int16_t*)(buf + 28));
|
|
header->sequenceId = flip16(*(int16_t*)(buf + 30));
|
|
header->controlField = (*(uint8_t*)(buf + 32));
|
|
header->logMessageInterval = (*(int8_t*)(buf + 33));
|
|
}
|
|
|
|
/* Pack header message */
|
|
void msgPackHeader(const PtpClock *ptpClock, octet_t *buf)
|
|
{
|
|
nibble_t transport = 0x80; //(spec annex D)
|
|
*(uint8_t*)(buf + 0) = transport;
|
|
*(uint4bit_t*)(buf + 1) = ptpClock->portDS.versionNumber;
|
|
*(uint8_t*)(buf + 4) = ptpClock->defaultDS.domainNumber;
|
|
if (ptpClock->defaultDS.twoStepFlag)
|
|
{
|
|
*(uint8_t*)(buf + 6) = FLAG0_TWO_STEP;
|
|
}
|
|
memset((buf + 8), 0, 8);
|
|
memcpy((buf + 20), ptpClock->portDS.portIdentity.clockIdentity, CLOCK_IDENTITY_LENGTH);
|
|
*(int16_t*)(buf + 28) = flip16(ptpClock->portDS.portIdentity.portNumber);
|
|
*(uint8_t*)(buf + 33) = 0x7F; //Default value (spec Table 24)
|
|
}
|
|
|
|
/* Pack Announce message */
|
|
void msgPackAnnounce(const PtpClock *ptpClock, octet_t *buf)
|
|
{
|
|
/* Changes in header */
|
|
*(char*)(buf + 0) = *(char*)(buf + 0) & 0xF0; //RAZ messageType
|
|
*(char*)(buf + 0) = *(char*)(buf + 0) | ANNOUNCE; //Table 19
|
|
*(int16_t*)(buf + 2) = flip16(ANNOUNCE_LENGTH);
|
|
*(int16_t*)(buf + 30) = flip16(ptpClock->sentAnnounceSequenceId);
|
|
*(uint8_t*)(buf + 32) = CTRL_OTHER; /* Table 23 - controlField */
|
|
*(int8_t*)(buf + 33) = ptpClock->portDS.logAnnounceInterval;
|
|
|
|
/* Announce message */
|
|
memset((buf + 34), 0, 10); /* originTimestamp */
|
|
*(int16_t*)(buf + 44) = flip16(ptpClock->timePropertiesDS.currentUtcOffset);
|
|
*(uint8_t*)(buf + 47) = ptpClock->parentDS.grandmasterPriority1;
|
|
*(uint8_t*)(buf + 48) = ptpClock->defaultDS.clockQuality.clockClass;
|
|
*(enum8bit_t*)(buf + 49) = ptpClock->defaultDS.clockQuality.clockAccuracy;
|
|
*(int16_t*)(buf + 50) = flip16(ptpClock->defaultDS.clockQuality.offsetScaledLogVariance);
|
|
*(uint8_t*)(buf + 52) = ptpClock->parentDS.grandmasterPriority2;
|
|
memcpy((buf + 53), ptpClock->parentDS.grandmasterIdentity, CLOCK_IDENTITY_LENGTH);
|
|
*(int16_t*)(buf + 61) = flip16(ptpClock->currentDS.stepsRemoved);
|
|
*(enum8bit_t*)(buf + 63) = ptpClock->timePropertiesDS.timeSource;
|
|
}
|
|
|
|
/* Unpack Announce message */
|
|
void msgUnpackAnnounce(const octet_t *buf, MsgAnnounce *announce)
|
|
{
|
|
announce->originTimestamp.secondsField.msb = flip16(*(int16_t*)(buf + 34));
|
|
announce->originTimestamp.secondsField.lsb = flip32(*(uint32_t*)(buf + 36));
|
|
announce->originTimestamp.nanosecondsField = flip32(*(uint32_t*)(buf + 40));
|
|
announce->currentUtcOffset = flip16(*(int16_t*)(buf + 44));
|
|
announce->grandmasterPriority1 = *(uint8_t*)(buf + 47);
|
|
announce->grandmasterClockQuality.clockClass = *(uint8_t*)(buf + 48);
|
|
announce->grandmasterClockQuality.clockAccuracy = *(enum8bit_t*)(buf + 49);
|
|
announce->grandmasterClockQuality.offsetScaledLogVariance = flip16(*(int16_t*)(buf + 50));
|
|
announce->grandmasterPriority2 = *(uint8_t*)(buf + 52);
|
|
memcpy(announce->grandmasterIdentity, (buf + 53), CLOCK_IDENTITY_LENGTH);
|
|
announce->stepsRemoved = flip16(*(int16_t*)(buf + 61));
|
|
announce->timeSource = *(enum8bit_t*)(buf + 63);
|
|
}
|
|
|
|
/* Pack SYNC message */
|
|
void msgPackSync(const PtpClock *ptpClock, octet_t *buf, const Timestamp *originTimestamp)
|
|
{
|
|
/* Changes in header */
|
|
*(char*)(buf + 0) = *(char*)(buf + 0) & 0xF0; //RAZ messageType
|
|
*(char*)(buf + 0) = *(char*)(buf + 0) | SYNC; //Table 19
|
|
*(int16_t*)(buf + 2) = flip16(SYNC_LENGTH);
|
|
*(int16_t*)(buf + 30) = flip16(ptpClock->sentSyncSequenceId);
|
|
*(uint8_t*)(buf + 32) = CTRL_SYNC; //Table 23
|
|
*(int8_t*)(buf + 33) = ptpClock->portDS.logSyncInterval;
|
|
memset((buf + 8), 0, 8); /* correction field */
|
|
|
|
/* Sync message */
|
|
*(int16_t*)(buf + 34) = flip16(originTimestamp->secondsField.msb);
|
|
*(uint32_t*)(buf + 36) = flip32(originTimestamp->secondsField.lsb);
|
|
*(uint32_t*)(buf + 40) = flip32(originTimestamp->nanosecondsField);
|
|
}
|
|
|
|
/* Unpack Sync message */
|
|
void msgUnpackSync(const octet_t *buf, MsgSync *sync)
|
|
{
|
|
sync->originTimestamp.secondsField.msb = flip16(*(int16_t*)(buf + 34));
|
|
sync->originTimestamp.secondsField.lsb = flip32(*(uint32_t*)(buf + 36));
|
|
sync->originTimestamp.nanosecondsField = flip32(*(uint32_t*)(buf + 40));
|
|
}
|
|
|
|
/* Pack delayReq message */
|
|
void msgPackDelayReq(const PtpClock *ptpClock, octet_t *buf, const Timestamp *originTimestamp)
|
|
{
|
|
/* Changes in header */
|
|
*(char*)(buf + 0) = *(char*)(buf + 0) & 0xF0; //RAZ messageType
|
|
*(char*)(buf + 0) = *(char*)(buf + 0) | DELAY_REQ; //Table 19
|
|
*(int16_t*)(buf + 2) = flip16(DELAY_REQ_LENGTH);
|
|
*(int16_t*)(buf + 30) = flip16(ptpClock->sentDelayReqSequenceId);
|
|
*(uint8_t*)(buf + 32) = CTRL_DELAY_REQ; //Table 23
|
|
*(int8_t*)(buf + 33) = 0x7F; //Table 24
|
|
memset((buf + 8), 0, 8);
|
|
|
|
/* delay_req message */
|
|
*(int16_t*)(buf + 34) = flip16(originTimestamp->secondsField.msb);
|
|
*(uint32_t*)(buf + 36) = flip32(originTimestamp->secondsField.lsb);
|
|
*(uint32_t*)(buf + 40) = flip32(originTimestamp->nanosecondsField);
|
|
}
|
|
|
|
/* Unpack delayReq message */
|
|
void msgUnpackDelayReq(const octet_t *buf, MsgDelayReq *delayreq)
|
|
{
|
|
delayreq->originTimestamp.secondsField.msb = flip16(*(int16_t*)(buf + 34));
|
|
delayreq->originTimestamp.secondsField.lsb = flip32(*(uint32_t*)(buf + 36));
|
|
delayreq->originTimestamp.nanosecondsField = flip32(*(uint32_t*)(buf + 40));
|
|
}
|
|
|
|
/* Pack Follow_up message */
|
|
void msgPackFollowUp(const PtpClock *ptpClock, octet_t*buf, const Timestamp *preciseOriginTimestamp)
|
|
{
|
|
/* Changes in header */
|
|
*(char*)(buf + 0) = *(char*)(buf + 0) & 0xF0; //RAZ messageType
|
|
*(char*)(buf + 0) = *(char*)(buf + 0) | FOLLOW_UP; //Table 19
|
|
*(int16_t*)(buf + 2) = flip16(FOLLOW_UP_LENGTH);
|
|
*(int16_t*)(buf + 30) = flip16(ptpClock->sentSyncSequenceId - 1);//sentSyncSequenceId has already been incremented in issueSync
|
|
*(uint8_t*)(buf + 32) = CTRL_FOLLOW_UP; //Table 23
|
|
*(int8_t*)(buf + 33) = ptpClock->portDS.logSyncInterval;
|
|
|
|
/* Follow_up message */
|
|
*(int16_t*)(buf + 34) = flip16(preciseOriginTimestamp->secondsField.msb);
|
|
*(uint32_t*)(buf + 36) = flip32(preciseOriginTimestamp->secondsField.lsb);
|
|
*(uint32_t*)(buf + 40) = flip32(preciseOriginTimestamp->nanosecondsField);
|
|
}
|
|
|
|
/* Unpack Follow_up message */
|
|
void msgUnpackFollowUp(const octet_t *buf, MsgFollowUp *follow)
|
|
{
|
|
follow->preciseOriginTimestamp.secondsField.msb = flip16(*(int16_t*)(buf + 34));
|
|
follow->preciseOriginTimestamp.secondsField.lsb = flip32(*(uint32_t*)(buf + 36));
|
|
follow->preciseOriginTimestamp.nanosecondsField = flip32(*(uint32_t*)(buf + 40));
|
|
}
|
|
|
|
/* Pack delayResp message */
|
|
void msgPackDelayResp(const PtpClock *ptpClock, octet_t *buf, const MsgHeader *header, const Timestamp *receiveTimestamp)
|
|
{
|
|
/* Changes in header */
|
|
*(char*)(buf + 0) = *(char*)(buf + 0) & 0xF0; //RAZ messageType
|
|
*(char*)(buf + 0) = *(char*)(buf + 0) | DELAY_RESP; //Table 19
|
|
*(int16_t*)(buf + 2) = flip16(DELAY_RESP_LENGTH);
|
|
/* *(uint8_t*)(buf+4) = header->domainNumber; */ /* TODO: Why? */
|
|
memset((buf + 8), 0, 8);
|
|
|
|
/* Copy correctionField of delayReqMessage */
|
|
*(int32_t*)(buf + 8) = flip32(header->correctionfield >> 32);
|
|
*(int32_t*)(buf + 12) = flip32((int32_t)header->correctionfield);
|
|
*(int16_t*)(buf + 30) = flip16(header->sequenceId);
|
|
*(uint8_t*)(buf + 32) = CTRL_DELAY_RESP; //Table 23
|
|
*(int8_t*)(buf + 33) = ptpClock->portDS.logMinDelayReqInterval; //Table 24
|
|
|
|
/* delay_resp message */
|
|
*(int16_t*)(buf + 34) = flip16(receiveTimestamp->secondsField.msb);
|
|
*(uint32_t*)(buf + 36) = flip32(receiveTimestamp->secondsField.lsb);
|
|
*(uint32_t*)(buf + 40) = flip32(receiveTimestamp->nanosecondsField);
|
|
memcpy((buf + 44), header->sourcePortIdentity.clockIdentity, CLOCK_IDENTITY_LENGTH);
|
|
*(int16_t*)(buf + 52) = flip16(header->sourcePortIdentity.portNumber);
|
|
}
|
|
|
|
/* Unpack delayResp message */
|
|
void msgUnpackDelayResp(const octet_t *buf, MsgDelayResp *resp)
|
|
{
|
|
resp->receiveTimestamp.secondsField.msb = flip16(*(int16_t*)(buf + 34));
|
|
resp->receiveTimestamp.secondsField.lsb = flip32(*(uint32_t*)(buf + 36));
|
|
resp->receiveTimestamp.nanosecondsField = flip32(*(uint32_t*)(buf + 40));
|
|
memcpy(resp->requestingPortIdentity.clockIdentity, (buf + 44), CLOCK_IDENTITY_LENGTH);
|
|
resp->requestingPortIdentity.portNumber = flip16(*(int16_t*)(buf + 52));
|
|
}
|
|
|
|
/* Pack PdelayReq message */
|
|
void msgPackPDelayReq(const PtpClock *ptpClock, octet_t *buf, const Timestamp *originTimestamp)
|
|
{
|
|
/* Changes in header */
|
|
*(char*)(buf + 0) = *(char*)(buf + 0) & 0xF0; //RAZ messageType
|
|
*(char*)(buf + 0) = *(char*)(buf + 0) | PDELAY_REQ; //Table 19
|
|
*(int16_t*)(buf + 2) = flip16(PDELAY_REQ_LENGTH);
|
|
*(int16_t*)(buf + 30) = flip16(ptpClock->sentPDelayReqSequenceId);
|
|
*(uint8_t*)(buf + 32) = CTRL_OTHER; //Table 23
|
|
*(int8_t*)(buf + 33) = 0x7F; //Table 24
|
|
memset((buf + 8), 0, 8);
|
|
|
|
/* Pdelay_req message */
|
|
*(int16_t*)(buf + 34) = flip16(originTimestamp->secondsField.msb);
|
|
*(uint32_t*)(buf + 36) = flip32(originTimestamp->secondsField.lsb);
|
|
*(uint32_t*)(buf + 40) = flip32(originTimestamp->nanosecondsField);
|
|
|
|
memset((buf + 44), 0, 10); // RAZ reserved octets
|
|
}
|
|
|
|
/* Unpack PdelayReq message */
|
|
void msgUnpackPDelayReq(const octet_t *buf, MsgPDelayReq *pdelayreq)
|
|
{
|
|
pdelayreq->originTimestamp.secondsField.msb = flip16(*(int16_t*)(buf + 34));
|
|
pdelayreq->originTimestamp.secondsField.lsb = flip32(*(uint32_t*)(buf + 36));
|
|
pdelayreq->originTimestamp.nanosecondsField = flip32(*(uint32_t*)(buf + 40));
|
|
}
|
|
|
|
/* Pack PdelayResp message */
|
|
void msgPackPDelayResp(octet_t *buf, const MsgHeader *header, const Timestamp *requestReceiptTimestamp)
|
|
{
|
|
/* Changes in header */
|
|
*(char*)(buf + 0) = *(char*)(buf + 0) & 0xF0; //RAZ messageType
|
|
*(char*)(buf + 0) = *(char*)(buf + 0) | PDELAY_RESP; //Table 19
|
|
*(int16_t*)(buf + 2) = flip16(PDELAY_RESP_LENGTH);
|
|
/* *(uint8_t*)(buf+4) = header->domainNumber; */ /* TODO: Why? */
|
|
memset((buf + 8), 0, 8);
|
|
*(int16_t*)(buf + 30) = flip16(header->sequenceId);
|
|
*(uint8_t*)(buf + 32) = CTRL_OTHER; //Table 23
|
|
*(int8_t*)(buf + 33) = 0x7F; //Table 24
|
|
|
|
/* Pdelay_resp message */
|
|
*(int16_t*)(buf + 34) = flip16(requestReceiptTimestamp->secondsField.msb);
|
|
*(uint32_t*)(buf + 36) = flip32(requestReceiptTimestamp->secondsField.lsb);
|
|
*(uint32_t*)(buf + 40) = flip32(requestReceiptTimestamp->nanosecondsField);
|
|
memcpy((buf + 44), header->sourcePortIdentity.clockIdentity, CLOCK_IDENTITY_LENGTH);
|
|
*(int16_t*)(buf + 52) = flip16(header->sourcePortIdentity.portNumber);
|
|
|
|
}
|
|
|
|
/* Unpack PdelayResp message */
|
|
void msgUnpackPDelayResp(const octet_t *buf, MsgPDelayResp *presp)
|
|
{
|
|
presp->requestReceiptTimestamp.secondsField.msb = flip16(*(int16_t*)(buf + 34));
|
|
presp->requestReceiptTimestamp.secondsField.lsb = flip32(*(uint32_t*)(buf + 36));
|
|
presp->requestReceiptTimestamp.nanosecondsField = flip32(*(uint32_t*)(buf + 40));
|
|
memcpy(presp->requestingPortIdentity.clockIdentity, (buf + 44), CLOCK_IDENTITY_LENGTH);
|
|
presp->requestingPortIdentity.portNumber = flip16(*(int16_t*)(buf + 52));
|
|
}
|
|
|
|
/* Pack PdelayRespfollowup message */
|
|
void msgPackPDelayRespFollowUp(octet_t *buf, const MsgHeader *header, const Timestamp *responseOriginTimestamp)
|
|
{
|
|
/* Changes in header */
|
|
*(char*)(buf + 0) = *(char*)(buf + 0) & 0xF0; //RAZ messageType
|
|
*(char*)(buf + 0) = *(char*)(buf + 0) | PDELAY_RESP_FOLLOW_UP; //Table 19
|
|
*(int16_t*)(buf + 2) = flip16(PDELAY_RESP_FOLLOW_UP_LENGTH);
|
|
*(int16_t*)(buf + 30) = flip16(header->sequenceId);
|
|
*(uint8_t*)(buf + 32) = CTRL_OTHER; //Table 23
|
|
*(int8_t*)(buf + 33) = 0x7F; //Table 24
|
|
|
|
/* Copy correctionField of PdelayReqMessage */
|
|
*(int32_t*)(buf + 8) = flip32(header->correctionfield >> 32);
|
|
*(int32_t*)(buf + 12) = flip32((int32_t)header->correctionfield);
|
|
|
|
/* Pdelay_resp_follow_up message */
|
|
*(int16_t*)(buf + 34) = flip16(responseOriginTimestamp->secondsField.msb);
|
|
*(uint32_t*)(buf + 36) = flip32(responseOriginTimestamp->secondsField.lsb);
|
|
*(uint32_t*)(buf + 40) = flip32(responseOriginTimestamp->nanosecondsField);
|
|
memcpy((buf + 44), header->sourcePortIdentity.clockIdentity, CLOCK_IDENTITY_LENGTH);
|
|
*(int16_t*)(buf + 52) = flip16(header->sourcePortIdentity.portNumber);
|
|
}
|
|
|
|
/* Unpack PdelayResp message */
|
|
void msgUnpackPDelayRespFollowUp(const octet_t *buf, MsgPDelayRespFollowUp *prespfollow)
|
|
{
|
|
prespfollow->responseOriginTimestamp.secondsField.msb = flip16(*(int16_t*)(buf + 34));
|
|
prespfollow->responseOriginTimestamp.secondsField.lsb = flip32(*(uint32_t*)(buf + 36));
|
|
prespfollow->responseOriginTimestamp.nanosecondsField = flip32(*(uint32_t*)(buf + 40));
|
|
memcpy(prespfollow->requestingPortIdentity.clockIdentity, (buf + 44), CLOCK_IDENTITY_LENGTH);
|
|
prespfollow->requestingPortIdentity.portNumber = flip16(*(int16_t*)(buf + 52));
|
|
}
|