void msgUnpackHeader(void *buf, MsgHeader *header)
{
header->versionPTP = flip16(*(UInteger16*)((char*)buf + 0));
header->versionNetwork = flip16(*(UInteger16*)((char*)buf + 2));
DBGV("msgUnpackHeader: versionPTP %d\n", header->versionPTP);
DBGV("msgUnpackHeader: versionNetwork %d\n", header->versionNetwork);
memcpy(header->subdomain, ((char*)buf + 4), 16);
DBGV("msgUnpackHeader: subdomain %s\n", header->subdomain);
header->messageType = *(UInteger8*)((char*)buf + 20);
header->sourceCommunicationTechnology = *(UInteger8*)((char*)buf + 21);
DBGV("msgUnpackHeader: messageType %d\n", header->messageType);
DBGV("msgUnpackHeader: sourceCommunicationTechnology %d\n", header->sourceCommunicationTechnology);
memcpy(header->sourceUuid, ((char*)buf + 22), 6);
DBGV("msgUnpackHeader: sourceUuid %02x:%02x:%02x:%02x:%02x:%02x\n",
header->sourceUuid[0], header->sourceUuid[1], header->sourceUuid[2],
header->sourceUuid[3], header->sourceUuid[4], header->sourceUuid[5]);
header->sourcePortId = flip16(*(UInteger16*)((char*)buf + 28));
header->sequenceId = flip16(*(UInteger16*)((char*)buf + 30));
DBGV("msgUnpackHeader: sourcePortId %d\n", header->sourcePortId);
DBGV("msgUnpackHeader: sequenceId %d\n", header->sequenceId);
header->control = *(UInteger8*)((char*)buf + 32);
DBGV("msgUnpackHeader: control %d\n", header->control);
memcpy(header->flags, ((char*)buf + 34), 2);
DBGV("msgUnpackHeader: flags %02x %02x\n", header->flags[0], header->flags[1]);
}
UInteger16 msgPackManagement(void *buf, MsgManagement *manage, PtpClock *ptpClock)
{
*(UInteger8*)((char*)buf + 20) = 2; /* messageType */
*(Integer32*)((char*)buf + 28) = shift16(flip16(ptpClock->port_id_field), 0) | shift16(flip16(ptpClock->last_general_event_sequence_number), 1);
*(UInteger8*)((char*)buf + 32) = PTP_MANAGEMENT_MESSAGE; /* control */
clearFlag(((char*)buf + 34), PTP_SYNC_BURST);
clearFlag(((char*)buf + 34), PARENT_STATS);
*(Integer32*)((char*)buf + 40) = shift8(manage->targetCommunicationTechnology, 1);
memcpy((char*)buf + 42, manage->targetUuid, 6);
*(Integer32*)((char*)buf + 48) = shift16(flip16(manage->targetPortId), 0) | shift16(flip16(MM_STARTING_BOUNDARY_HOPS), 1);
*(Integer32*)((char*)buf + 52) = shift16(flip16(MM_STARTING_BOUNDARY_HOPS), 0);
*(UInteger8*)((char*)buf + 55) = manage->managementMessageKey;
switch(manage->managementMessageKey)
{
case PTP_MM_GET_FOREIGN_DATA_SET:
*(UInteger16*)((char*)buf + 62) = manage->recordKey;
*(Integer32*)((char*)buf + 56) = shift16(flip16(4), 1);
return 64;
default:
*(Integer32*)((char*)buf + 56) = shift16(flip16(0), 1);
return 60;
}
}
/*pack delayResp message into OUT buffer of ptpClock*/
void msgPackDelayResp(const PtpClock *ptpClock, Octet *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
*(UInteger16*)(buf + 2) = flip16(DELAY_RESP_LENGTH);
/* *(UInteger8*)(buf+4) = header->domainNumber; */ /* TODO: Why? */
memset((buf + 8), 0, 8);
/*Copy correctionField of delayReqMessage*/
*(Integer32*)(buf + 8) = flip32(header->correctionfield >> 32);
*(Integer32*)(buf + 12) = flip32((Integer32)header->correctionfield);
*(UInteger16*)(buf + 30) = flip16(header->sequenceId);
*(UInteger8*)(buf + 32) = CTRL_DELAY_RESP; //Table 23
*(Integer8*)(buf + 33) = ptpClock->portDS.logMinDelayReqInterval; //Table 24
/*delay_resp message*/
*(UInteger16*)(buf + 34) = flip16(receiveTimestamp->secondsField.msb);
*(UInteger32*)(buf + 36) = flip32(receiveTimestamp->secondsField.lsb);
*(UInteger32*)(buf + 40) = flip32(receiveTimestamp->nanosecondsField);
memcpy((buf + 44), header->sourcePortIdentity.clockIdentity, CLOCK_IDENTITY_LENGTH);
*(UInteger16*)(buf + 52) = flip16(header->sourcePortIdentity.portNumber);
}
/*Unpack delayResp message from IN buffer of ptpClock to msgtmp.presp*/
void msgUnpackDelayResp(const Octet *buf, MsgDelayResp *resp)
{
resp->receiveTimestamp.secondsField.msb = flip16(*(UInteger16*)(buf + 34));
resp->receiveTimestamp.secondsField.lsb = flip32(*(UInteger32*)(buf + 36));
resp->receiveTimestamp.nanosecondsField = flip32(*(UInteger32*)(buf + 40));
memcpy(resp->requestingPortIdentity.clockIdentity, (buf + 44), CLOCK_IDENTITY_LENGTH);
resp->requestingPortIdentity.portNumber = flip16(*(UInteger16*)(buf + 52));
}
/*Unpack PdelayResp message from IN buffer of ptpClock to msgtmp.presp*/
void msgUnpackPDelayRespFollowUp(const Octet *buf, MsgPDelayRespFollowUp *prespfollow)
{
prespfollow->responseOriginTimestamp.secondsField.msb = flip16(*(UInteger16*)(buf + 34));
prespfollow->responseOriginTimestamp.secondsField.lsb = flip32(*(UInteger32*)(buf + 36));
prespfollow->responseOriginTimestamp.nanosecondsField = flip32(*(UInteger32*)(buf + 40));
memcpy(prespfollow->requestingPortIdentity.clockIdentity, (buf + 44), CLOCK_IDENTITY_LENGTH);
prespfollow->requestingPortIdentity.portNumber = flip16(*(UInteger16*)(buf + 52));
}
void
msgUnpackSync(void *buf, MsgSync * sync)
{
sync->originTimestamp.seconds = flip32(*(UInteger32 *) (buf + 40));
DBGV("msgUnpackSync: originTimestamp.seconds %u\n", sync->originTimestamp.seconds);
sync->originTimestamp.nanoseconds = flip32(*(Integer32 *) (buf + 44));
DBGV("msgUnpackSync: originTimestamp.nanoseconds %d\n", sync->originTimestamp.nanoseconds);
sync->epochNumber = flip16(*(UInteger16 *) (buf + 48));
DBGV("msgUnpackSync: epochNumber %d\n", sync->epochNumber);
sync->currentUTCOffset = flip16(*(Integer16 *) (buf + 50));
DBGV("msgUnpackSync: currentUTCOffset %d\n", sync->currentUTCOffset);
sync->grandmasterCommunicationTechnology = *(UInteger8 *) (buf + 53);
DBGV("msgUnpackSync: grandmasterCommunicationTechnology %d\n", sync->grandmasterCommunicationTechnology);
memcpy(sync->grandmasterClockUuid, (buf + 54), 6);
DBGV("msgUnpackSync: grandmasterClockUuid %02hhx:%02hhx:%02hhx:%02hhx:%02hhx:%02hhx\n",
sync->grandmasterClockUuid[0], sync->grandmasterClockUuid[1], sync->grandmasterClockUuid[2],
sync->grandmasterClockUuid[3], sync->grandmasterClockUuid[4], sync->grandmasterClockUuid[5]);
sync->grandmasterPortId = flip16(*(UInteger16 *) (buf + 60));
DBGV("msgUnpackSync: grandmasterPortId %d\n", sync->grandmasterPortId);
sync->grandmasterSequenceId = flip16(*(UInteger16 *) (buf + 62));
DBGV("msgUnpackSync: grandmasterSequenceId %d\n", sync->grandmasterSequenceId);
sync->grandmasterClockStratum = *(UInteger8 *) (buf + 67);
DBGV("msgUnpackSync: grandmasterClockStratum %d\n", sync->grandmasterClockStratum);
memcpy(sync->grandmasterClockIdentifier, (buf + 68), 4);
DBGV("msgUnpackSync: grandmasterClockIdentifier %c%c%c%c\n",
sync->grandmasterClockIdentifier[0], sync->grandmasterClockIdentifier[1],
sync->grandmasterClockIdentifier[2], sync->grandmasterClockIdentifier[3]);
sync->grandmasterClockVariance = flip16(*(Integer16 *) (buf + 74));
DBGV("msgUnpackSync: grandmasterClockVariance %d\n", sync->grandmasterClockVariance);
sync->grandmasterPreferred = *(UInteger8 *) (buf + 77);
DBGV("msgUnpackSync: grandmasterPreferred %d\n", sync->grandmasterPreferred);
sync->grandmasterIsBoundaryClock = *(UInteger8 *) (buf + 79);
DBGV("msgUnpackSync: grandmasterIsBoundaryClock %d\n", sync->grandmasterIsBoundaryClock);
sync->syncInterval = *(Integer8 *) (buf + 83);
DBGV("msgUnpackSync: syncInterval %d\n", sync->syncInterval);
sync->localClockVariance = flip16(*(Integer16 *) (buf + 86));
DBGV("msgUnpackSync: localClockVariance %d\n", sync->localClockVariance);
sync->localStepsRemoved = flip16(*(UInteger16 *) (buf + 90));
DBGV("msgUnpackSync: localStepsRemoved %d\n", sync->localStepsRemoved);
sync->localClockStratum = *(UInteger8 *) (buf + 95);
DBGV("msgUnpackSync: localClockStratum %d\n", sync->localClockStratum);
memcpy(sync->localClockIdentifer, (buf + 96), PTP_CODE_STRING_LENGTH);
DBGV("msgUnpackSync: localClockIdentifer %c%c%c%c\n",
sync->localClockIdentifer[0], sync->localClockIdentifer[1],
sync->localClockIdentifer[2], sync->localClockIdentifer[3]);
sync->parentCommunicationTechnology = *(UInteger8 *) (buf + 101);
DBGV("msgUnpackSync: parentCommunicationTechnology %d\n", sync->parentCommunicationTechnology);
memcpy(sync->parentUuid, (buf + 102), PTP_UUID_LENGTH);
DBGV("msgUnpackSync: parentUuid %02hhx:%02hhx:%02hhx:%02hhx:%02hhx:%02hhx\n",
sync->parentUuid[0], sync->parentUuid[1], sync->parentUuid[2],
sync->parentUuid[3], sync->parentUuid[4], sync->parentUuid[5]);
sync->parentPortField = flip16(*(UInteger16 *) (buf + 110));
DBGV("msgUnpackSync: parentPortField %d\n", sync->parentPortField);
sync->estimatedMasterVariance = flip16(*(Integer16 *) (buf + 114));
DBGV("msgUnpackSync: estimatedMasterVariance %d\n", sync->estimatedMasterVariance);
sync->estimatedMasterDrift = flip32(*(Integer32 *) (buf + 116));
DBGV("msgUnpackSync: estimatedMasterDrift %d\n", sync->estimatedMasterDrift);
sync->utcReasonable = *(UInteger8 *) (buf + 123);
DBGV("msgUnpackSync: utcReasonable %d\n", sync->utcReasonable);
}
void msgPackFollowUp(void *buf, UInteger16 associatedSequenceId,
TimeRepresentation *preciseOriginTimestamp, PtpClock *ptpClock)
{
*(UInteger8*)((char*)buf + 20) = 2; /* messageType */
*(Integer32*)((char*)buf + 28) = shift16(flip16(ptpClock->port_id_field), 0) | shift16(flip16(ptpClock->last_general_event_sequence_number), 1);
*(UInteger8*)((char*)buf + 32) = PTP_FOLLOWUP_MESSAGE; /* control */
clearFlag(((char*)buf + 34), PTP_SYNC_BURST);
clearFlag(((char*)buf + 34), PARENT_STATS);
*(Integer32*)((char*)buf + 40) = shift16(flip16(associatedSequenceId), 1);
*(Integer32*)((char*)buf + 44) = flip32(preciseOriginTimestamp->seconds);
*(Integer32*)((char*)buf + 48) = flip32(preciseOriginTimestamp->nanoseconds);
}
void msgPackHeader(void *buf, PtpClock *ptpClock)
{
*(Integer32*)((char*)buf + 0) = shift16(flip16(VERSION_PTP), 0) | shift16(flip16(VERSION_NETWORK), 1);
memcpy(((char*)buf + 4), ptpClock->subdomain_name, 16);
*(Integer32*)((char*)buf + 20) = shift8(ptpClock->port_communication_technology, 1);
memcpy(((char*)buf + 22), ptpClock->port_uuid_field, 6);
if(ptpClock->external_timing)
setFlag(((char*)buf + 34), PTP_EXT_SYNC);
if(ptpClock->clock_followup_capable)
setFlag(((char*)buf + 34), PTP_ASSIST);
if(ptpClock->is_boundary_clock)
setFlag(((char*)buf + 34), PTP_BOUNDARY_CLOCK);
}
/*pack Follow_up message into OUT buffer of ptpClock*/
void msgPackFollowUp(const PtpClock *ptpClock, Octet*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
*(UInteger16*)(buf + 2) = flip16(FOLLOW_UP_LENGTH);
*(UInteger16*)(buf + 30) = flip16(ptpClock->sentSyncSequenceId - 1);//sentSyncSequenceId has already been incremented in issueSync
*(UInteger8*)(buf + 32) = CTRL_FOLLOW_UP; //Table 23
*(Integer8*)(buf + 33) = ptpClock->portDS.logSyncInterval;
/*Follow_up message*/
*(UInteger16*)(buf + 34) = flip16(preciseOriginTimestamp->secondsField.msb);
*(UInteger32*)(buf + 36) = flip32(preciseOriginTimestamp->secondsField.lsb);
*(UInteger32*)(buf + 40) = flip32(preciseOriginTimestamp->nanosecondsField);
}
void msgPackDelayResp(void *buf, MsgHeader *header,
TimeRepresentation *delayReceiptTimestamp, PtpClock *ptpClock)
{
*(UInteger8*)((char*)buf + 20) = 2; /* messageType */
*(Integer32*)((char*)buf + 28) = shift16(flip16(ptpClock->port_id_field), 0) | shift16(flip16(ptpClock->last_general_event_sequence_number), 1);
*(UInteger8*)((char*)buf + 32) = PTP_DELAY_RESP_MESSAGE; /* control */
clearFlag(((char*)buf + 34), PTP_SYNC_BURST);
clearFlag(((char*)buf + 34), PARENT_STATS);
*(Integer32*)((char*)buf + 40) = flip32(delayReceiptTimestamp->seconds);
*(Integer32*)((char*)buf + 44) = flip32(delayReceiptTimestamp->nanoseconds);
*(Integer32*)((char*)buf + 48) = shift8(header->sourceCommunicationTechnology, 1);
memcpy((char*)buf + 50, header->sourceUuid, 6);
*(Integer32*)((char*)buf + 56) = shift16(flip16(header->sourcePortId), 0) | shift16(flip16(header->sequenceId), 1);
}
/*Unpack Announce message from IN buffer of ptpClock to msgtmp.Announce*/
void msgUnpackAnnounce(const Octet *buf, MsgAnnounce *announce)
{
announce->originTimestamp.secondsField.msb = flip16(*(UInteger16*)(buf + 34));
announce->originTimestamp.secondsField.lsb = flip32(*(UInteger32*)(buf + 36));
announce->originTimestamp.nanosecondsField = flip32(*(UInteger32*)(buf + 40));
announce->currentUtcOffset = flip16(*(UInteger16*)(buf + 44));
announce->grandmasterPriority1 = *(UInteger8*)(buf + 47);
announce->grandmasterClockQuality.clockClass = *(UInteger8*)(buf + 48);
announce->grandmasterClockQuality.clockAccuracy = *(Enumeration8*)(buf + 49);
announce->grandmasterClockQuality.offsetScaledLogVariance = flip16(*(UInteger16*)(buf + 50));
announce->grandmasterPriority2 = *(UInteger8*)(buf + 52);
memcpy(announce->grandmasterIdentity, (buf + 53), CLOCK_IDENTITY_LENGTH);
announce->stepsRemoved = flip16(*(UInteger16*)(buf + 61));
announce->timeSource = *(Enumeration8*)(buf + 63);
}
/*pack delayReq message into OUT buffer of ptpClock*/
void msgPackDelayReq(const PtpClock *ptpClock, Octet *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
*(UInteger16*)(buf + 2) = flip16(DELAY_REQ_LENGTH);
*(UInteger16*)(buf + 30) = flip16(ptpClock->sentDelayReqSequenceId);
*(UInteger8*)(buf + 32) = CTRL_DELAY_REQ; //Table 23
*(Integer8*)(buf + 33) = 0x7F; //Table 24
memset((buf + 8), 0, 8);
/*delay_req message*/
*(UInteger16*)(buf + 34) = flip16(originTimestamp->secondsField.msb);
*(UInteger32*)(buf + 36) = flip32(originTimestamp->secondsField.lsb);
*(UInteger32*)(buf + 40) = flip32(originTimestamp->nanosecondsField);
}
/*Pack SYNC message into OUT buffer of ptpClock*/
void msgPackSync(const PtpClock *ptpClock, Octet *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
*(UInteger16*)(buf + 2) = flip16(SYNC_LENGTH);
*(UInteger16*)(buf + 30) = flip16(ptpClock->sentSyncSequenceId);
*(UInteger8*)(buf + 32) = CTRL_SYNC; //Table 23
*(Integer8*)(buf + 33) = ptpClock->portDS.logSyncInterval;
memset((buf + 8), 0, 8); /* correction field */
/*Sync message*/
*(UInteger16*)(buf + 34) = flip16(originTimestamp->secondsField.msb);
*(UInteger32*)(buf + 36) = flip32(originTimestamp->secondsField.lsb);
*(UInteger32*)(buf + 40) = flip32(originTimestamp->nanosecondsField);
}
void msgUnpackDelayResp(void *buf, MsgDelayResp *resp)
{
resp->delayReceiptTimestamp.seconds = flip32(*(UInteger32*)((char*)buf + 40));
DBGV("msgUnpackDelayResp: delayReceiptTimestamp.seconds %u\n", resp->delayReceiptTimestamp.seconds);
resp->delayReceiptTimestamp.nanoseconds = flip32(*(Integer32*)((char*)buf + 44));
DBGV("msgUnpackDelayResp: delayReceiptTimestamp.nanoseconds %d\n", resp->delayReceiptTimestamp.nanoseconds);
resp->requestingSourceCommunicationTechnology = *(UInteger8*)((char*)buf + 49);
DBGV("msgUnpackDelayResp: requestingSourceCommunicationTechnology %d\n", resp->requestingSourceCommunicationTechnology);
memcpy(resp->requestingSourceUuid, ((char*)buf + 50), 6);
DBGV("msgUnpackDelayResp: requestingSourceUuid %02x:%02x:%02x:%02x:%02x:%02x\n",
resp->requestingSourceUuid[0], resp->requestingSourceUuid[1], resp->requestingSourceUuid[2],
resp->requestingSourceUuid[3], resp->requestingSourceUuid[4], resp->requestingSourceUuid[5]);
resp->requestingSourcePortId = flip16(*(UInteger16*)((char*)buf + 56));
DBGV("msgUnpackDelayResp: requestingSourcePortId %d\n", resp->requestingSourcePortId);
resp->requestingSourceSequenceId = flip16(*(UInteger16*)((char*)buf + 58));
DBGV("msgUnpackDelayResp: requestingSourceSequenceId %d\n", resp->requestingSourceSequenceId);
}
void msgUnpackFollowUp(void *buf, MsgFollowUp *follow)
{
follow->associatedSequenceId = flip16(*(UInteger16*)((char*)buf + 42));
DBGV("msgUnpackFollowUp: associatedSequenceId %u\n", follow->associatedSequenceId);
follow->preciseOriginTimestamp.seconds = flip32(*(UInteger32*)((char*)buf + 44));
DBGV("msgUnpackFollowUp: preciseOriginTimestamp.seconds %u\n", follow->preciseOriginTimestamp.seconds);
follow->preciseOriginTimestamp.nanoseconds = flip32(*(Integer32*)((char*)buf + 48));
DBGV("msgUnpackFollowUp: preciseOriginTimestamp.nanoseconds %d\n", follow->preciseOriginTimestamp.nanoseconds);
}
/*pack PdelayRespfollowup message into OUT buffer of ptpClock*/
void msgPackPDelayRespFollowUp(Octet *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
*(UInteger16*)(buf + 2) = flip16(PDELAY_RESP_FOLLOW_UP_LENGTH);
*(UInteger16*)(buf + 30) = flip16(header->sequenceId);
*(UInteger8*)(buf + 32) = CTRL_OTHER; //Table 23
*(Integer8*)(buf + 33) = 0x7F; //Table 24
/*Copy correctionField of PdelayReqMessage*/
*(Integer32*)(buf + 8) = flip32(header->correctionfield >> 32);
*(Integer32*)(buf + 12) = flip32((Integer32)header->correctionfield);
/*Pdelay_resp_follow_up message*/
*(UInteger16*)(buf + 34) = flip16(responseOriginTimestamp->secondsField.msb);
*(UInteger32*)(buf + 36) = flip32(responseOriginTimestamp->secondsField.lsb);
*(UInteger32*)(buf + 40) = flip32(responseOriginTimestamp->nanosecondsField);
memcpy((buf + 44), header->sourcePortIdentity.clockIdentity, CLOCK_IDENTITY_LENGTH);
*(UInteger16*)(buf + 52) = flip16(header->sourcePortIdentity.portNumber);
}
/*Pack Announce message into OUT buffer of ptpClock*/
void msgPackAnnounce(const PtpClock *ptpClock, Octet *buf)
{
/*changes in header*/
*(char*)(buf + 0) = *(char*)(buf + 0) & 0xF0; //RAZ messageType
*(char*)(buf + 0) = *(char*)(buf + 0) | ANNOUNCE; //Table 19
*(UInteger16*)(buf + 2) = flip16(ANNOUNCE_LENGTH);
*(UInteger16*)(buf + 30) = flip16(ptpClock->sentAnnounceSequenceId);
*(UInteger8*)(buf + 32) = CTRL_OTHER; /* Table 23 - controlField */
*(Integer8*)(buf + 33) = ptpClock->portDS.logAnnounceInterval;
/*Announce message*/
memset((buf + 34), 0, 10); /* originTimestamp */
*(Integer16*)(buf + 44) = flip16(ptpClock->timePropertiesDS.currentUtcOffset);
*(UInteger8*)(buf + 47) = ptpClock->parentDS.grandmasterPriority1;
*(UInteger8*)(buf + 48) = ptpClock->defaultDS.clockQuality.clockClass;
*(Enumeration8*)(buf + 49) = ptpClock->defaultDS.clockQuality.clockAccuracy;
*(UInteger16*)(buf + 50) = flip16(ptpClock->defaultDS.clockQuality.offsetScaledLogVariance);
*(UInteger8*)(buf + 52) = ptpClock->parentDS.grandmasterPriority2;
memcpy((buf + 53), ptpClock->parentDS.grandmasterIdentity, CLOCK_IDENTITY_LENGTH);
*(UInteger16*)(buf + 61) = flip16(ptpClock->currentDS.stepsRemoved);
*(Enumeration8*)(buf + 63) = ptpClock->timePropertiesDS.timeSource;
}
void msgUnpackManagement(void *buf, MsgManagement *manage)
{
manage->targetCommunicationTechnology = *(UInteger8*)((char*)buf + 41);
DBGV("msgUnpackManagement: targetCommunicationTechnology %d\n", manage->targetCommunicationTechnology);
memcpy(manage->targetUuid, ((char*)buf + 42), 6);
DBGV("msgUnpackManagement: targetUuid %02x:%02x:%02x:%02x:%02x:%02x\n",
manage->targetUuid[0], manage->targetUuid[1], manage->targetUuid[2],
manage->targetUuid[3], manage->targetUuid[4], manage->targetUuid[5]);
manage->targetPortId = flip16(*(UInteger16*)((char*)buf + 48));
DBGV("msgUnpackManagement: targetPortId %d\n", manage->targetPortId);
manage->startingBoundaryHops = flip16(*(Integer16*)((char*)buf + 50));
DBGV("msgUnpackManagement: startingBoundaryHops %d\n", manage->startingBoundaryHops);
manage->boundaryHops = flip16(*(Integer16*)((char*)buf + 52));
DBGV("msgUnpackManagement: boundaryHops %d\n", manage->boundaryHops);
manage->managementMessageKey = *(UInteger8*)((char*)buf + 55);
DBGV("msgUnpackManagement: managementMessageKey %d\n", manage->managementMessageKey);
manage->parameterLength = flip16(*(UInteger16*)((char*)buf + 58));
DBGV("msgUnpackManagement: parameterLength %d\n", manage->parameterLength);
if(manage->managementMessageKey == PTP_MM_GET_FOREIGN_DATA_SET)
manage->recordKey = flip16(*(UInteger16*)((char*)buf + 62));
}
/*Unpack Header from IN buffer to msgTmpHeader field */
void msgUnpackHeader(const Octet *buf, MsgHeader *header)
{
Integer32 msb;
UInteger32 lsb;
header->transportSpecific = (*(Nibble*)(buf + 0)) >> 4;
header->messageType = (*(Enumeration4*)(buf + 0)) & 0x0F;
header->versionPTP = (*(UInteger4*)(buf + 1)) & 0x0F; //force reserved bit to zero if not
header->messageLength = flip16(*(UInteger16*)(buf + 2));
header->domainNumber = (*(UInteger8*)(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(*(UInteger16*)(buf + 28));
header->sequenceId = flip16(*(UInteger16*)(buf + 30));
header->controlField = (*(UInteger8*)(buf + 32));
header->logMessageInterval = (*(Integer8*)(buf + 33));
}
void msgPackSync(void *buf, Boolean burst,
TimeRepresentation *originTimestamp, PtpClock *ptpClock)
{
*(UInteger8*)((char*)buf +20) = 1; /* messageType */
*(Integer32*)((char*)buf + 28) = shift16(flip16(ptpClock->port_id_field), 0) | shift16(flip16(ptpClock->last_sync_event_sequence_number), 1);
*(UInteger8*)((char*)buf +32) = PTP_SYNC_MESSAGE; /* control */
if(ptpClock->burst_enabled && burst)
setFlag(((char*)buf + 34), PTP_SYNC_BURST);
else
clearFlag(((char*)buf + 34), PTP_SYNC_BURST);
if(ptpClock->parent_stats)
setFlag(((char*)buf + 34), PARENT_STATS);
else
clearFlag(((char*)buf + 34), PARENT_STATS);
*(Integer32*)((char*)buf + 40) = flip32(originTimestamp->seconds);
*(Integer32*)((char*)buf + 44) = flip32(originTimestamp->nanoseconds);
*(Integer32*)((char*)buf + 48) = shift16(flip16(ptpClock->epoch_number), 0) | shift16(flip16(ptpClock->current_utc_offset), 1);
*(Integer32*)((char*)buf + 52) = shift8(ptpClock->grandmaster_communication_technology, 1);
memcpy(((char*)buf + 54), ptpClock->grandmaster_uuid_field, 6);
*(Integer32*)((char*)buf + 60) = shift16(flip16(ptpClock->grandmaster_port_id_field), 0) | shift16(flip16(ptpClock->grandmaster_sequence_number), 1);
*(Integer32*)((char*)buf + 64) = shift8(ptpClock->grandmaster_stratum, 3);
memcpy(((char*)buf + 68), ptpClock->grandmaster_identifier, 4);
*(Integer32*)((char*)buf + 72) = shift16(flip16(ptpClock->grandmaster_variance), 1);
*(Integer32*)((char*)buf + 76) = shift16(flip16(ptpClock->grandmaster_preferred), 0) | shift16(flip16(ptpClock->grandmaster_is_boundary_clock), 1);
*(Integer32*)((char*)buf + 80) = shift16(flip16(ptpClock->sync_interval), 1);
*(Integer32*)((char*)buf + 84) = shift16(flip16(ptpClock->clock_variance), 1);
*(Integer32*)((char*)buf + 88) = shift16(flip16(ptpClock->steps_removed), 1);
*(Integer32*)((char*)buf + 92) = shift8(ptpClock->clock_stratum, 3);
memcpy(((char*)buf + 96), ptpClock->clock_identifier, 4);
*(Integer32*)((char*)buf + 100) = shift8(ptpClock->parent_communication_technology, 1);
memcpy(((char*)buf + 102), ptpClock->parent_uuid, 6);
*(Integer32*)((char*)buf + 108) = shift16(flip16(ptpClock->parent_port_id), 1);
*(Integer32*)((char*)buf + 112) = shift16(flip16(ptpClock->observed_variance), 1);
*(Integer32*)((char*)buf + 116) = flip32(ptpClock->observed_drift);
*(Integer32*)((char*)buf + 120) = shift8(ptpClock->utc_reasonable, 3);
}
/*pack PdelayResp message into OUT buffer of ptpClock*/
void msgPackPDelayResp(Octet *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
*(UInteger16*)(buf + 2) = flip16(PDELAY_RESP_LENGTH);
/* *(UInteger8*)(buf+4) = header->domainNumber; */ /* TODO: Why? */
memset((buf + 8), 0, 8);
*(UInteger16*)(buf + 30) = flip16(header->sequenceId);
*(UInteger8*)(buf + 32) = CTRL_OTHER; //Table 23
*(Integer8*)(buf + 33) = 0x7F; //Table 24
/*Pdelay_resp message*/
*(UInteger16*)(buf + 34) = flip16(requestReceiptTimestamp->secondsField.msb);
*(UInteger32*)(buf + 36) = flip32(requestReceiptTimestamp->secondsField.lsb);
*(UInteger32*)(buf + 40) = flip32(requestReceiptTimestamp->nanosecondsField);
memcpy((buf + 44), header->sourcePortIdentity.clockIdentity, CLOCK_IDENTITY_LENGTH);
*(UInteger16*)(buf + 52) = flip16(header->sourcePortIdentity.portNumber);
}
/*Pack header message into OUT buffer of ptpClock*/
void msgPackHeader(const PtpClock *ptpClock, Octet *buf)
{
Nibble transport = 0x80; //(spec annex D)
*(UInteger8*)(buf + 0) = transport;
*(UInteger4*)(buf + 1) = ptpClock->portDS.versionNumber;
*(UInteger8*)(buf + 4) = ptpClock->defaultDS.domainNumber;
if (ptpClock->defaultDS.twoStepFlag)
{
*(UInteger8*)(buf + 6) = FLAG0_TWO_STEP;
}
memset((buf + 8), 0, 8);
memcpy((buf + 20), ptpClock->portDS.portIdentity.clockIdentity, CLOCK_IDENTITY_LENGTH);
*(UInteger16*)(buf + 28) = flip16(ptpClock->portDS.portIdentity.portNumber);
*(UInteger8*)(buf + 33) = 0x7F; //Default value (spec Table 24)
}
/*Unpack Follow_up message from IN buffer of ptpClock to msgtmp.follow*/
void msgUnpackFollowUp(const Octet *buf, MsgFollowUp *follow)
{
follow->preciseOriginTimestamp.secondsField.msb = flip16(*(UInteger16*)(buf + 34));
follow->preciseOriginTimestamp.secondsField.lsb = flip32(*(UInteger32*)(buf + 36));
follow->preciseOriginTimestamp.nanosecondsField = flip32(*(UInteger32*)(buf + 40));
}
static int mgt_post_recv(struct management_tlv *m, uint16_t data_len,
struct tlv_extra *extra)
{
struct defaultDS *dds;
struct currentDS *cds;
struct parentDS *pds;
struct timePropertiesDS *tp;
struct portDS *p;
struct port_ds_np *pdsnp;
struct time_status_np *tsn;
struct grandmaster_settings_np *gsn;
struct subscribe_events_np *sen;
struct port_properties_np *ppn;
struct mgmt_clock_description *cd;
int extra_len = 0, len;
uint8_t *buf;
uint16_t u16;
switch (m->id) {
case TLV_CLOCK_DESCRIPTION:
cd = &extra->cd;
buf = m->data;
len = data_len;
cd->clockType = (UInteger16 *) buf;
buf += sizeof(*cd->clockType);
len -= sizeof(*cd->clockType);
if (len < 0)
goto bad_length;
flip16(cd->clockType);
cd->physicalLayerProtocol = (struct PTPText *) buf;
buf += sizeof(struct PTPText);
len -= sizeof(struct PTPText);
if (len < 0)
goto bad_length;
buf += cd->physicalLayerProtocol->length;
len -= cd->physicalLayerProtocol->length;
if (len < 0)
goto bad_length;
cd->physicalAddress = (struct PhysicalAddress *) buf;
buf += sizeof(struct PhysicalAddress);
len -= sizeof(struct PhysicalAddress);
if (len < 0)
goto bad_length;
u16 = flip16(&cd->physicalAddress->length);
if (u16 > TRANSPORT_ADDR_LEN)
goto bad_length;
buf += u16;
len -= u16;
if (len < 0)
goto bad_length;
cd->protocolAddress = (struct PortAddress *) buf;
buf += sizeof(struct PortAddress);
len -= sizeof(struct PortAddress);
if (len < 0)
goto bad_length;
flip16(&cd->protocolAddress->networkProtocol);
u16 = flip16(&cd->protocolAddress->addressLength);
if (u16 > TRANSPORT_ADDR_LEN)
goto bad_length;
buf += u16;
len -= u16;
if (len < 0)
goto bad_length;
cd->manufacturerIdentity = buf;
buf += OUI_LEN + 1;
len -= OUI_LEN + 1;
if (len < 0)
goto bad_length;
cd->productDescription = (struct PTPText *) buf;
buf += sizeof(struct PTPText);
len -= sizeof(struct PTPText);
if (len < 0)
goto bad_length;
buf += cd->productDescription->length;
len -= cd->productDescription->length;
if (len < 0)
goto bad_length;
cd->revisionData = (struct PTPText *) buf;
buf += sizeof(struct PTPText);
len -= sizeof(struct PTPText);
if (len < 0)
goto bad_length;
buf += cd->revisionData->length;
len -= cd->revisionData->length;
if (len < 0)
goto bad_length;
cd->userDescription = (struct PTPText *) buf;
buf += sizeof(struct PTPText);
len -= sizeof(struct PTPText);
if (len < 0)
goto bad_length;
buf += cd->userDescription->length;
len -= cd->userDescription->length;
if (len < 0)
goto bad_length;
cd->profileIdentity = buf;
buf += PROFILE_ID_LEN;
len -= PROFILE_ID_LEN;
if (len < 0)
goto bad_length;
extra_len = buf - m->data;
break;
case TLV_USER_DESCRIPTION:
if (data_len < sizeof(struct PTPText))
goto bad_length;
extra->cd.userDescription = (struct PTPText *) m->data;
extra_len = sizeof(struct PTPText);
extra_len += extra->cd.userDescription->length;
break;
case TLV_DEFAULT_DATA_SET:
if (data_len != sizeof(struct defaultDS))
goto bad_length;
dds = (struct defaultDS *) m->data;
dds->numberPorts = ntohs(dds->numberPorts);
dds->clockQuality.offsetScaledLogVariance =
ntohs(dds->clockQuality.offsetScaledLogVariance);
break;
case TLV_CURRENT_DATA_SET:
if (data_len != sizeof(struct currentDS))
goto bad_length;
cds = (struct currentDS *) m->data;
cds->stepsRemoved = ntohs(cds->stepsRemoved);
cds->offsetFromMaster = net2host64(cds->offsetFromMaster);
cds->meanPathDelay = net2host64(cds->meanPathDelay);
break;
case TLV_PARENT_DATA_SET:
if (data_len != sizeof(struct parentDS))
goto bad_length;
pds = (struct parentDS *) m->data;
pds->parentPortIdentity.portNumber =
ntohs(pds->parentPortIdentity.portNumber);
pds->observedParentOffsetScaledLogVariance =
ntohs(pds->observedParentOffsetScaledLogVariance);
pds->observedParentClockPhaseChangeRate =
ntohl(pds->observedParentClockPhaseChangeRate);
pds->grandmasterClockQuality.offsetScaledLogVariance =
ntohs(pds->grandmasterClockQuality.offsetScaledLogVariance);
break;
case TLV_TIME_PROPERTIES_DATA_SET:
if (data_len != sizeof(struct timePropertiesDS))
goto bad_length;
tp = (struct timePropertiesDS *) m->data;
tp->currentUtcOffset = ntohs(tp->currentUtcOffset);
break;
case TLV_PORT_DATA_SET:
if (data_len != sizeof(struct portDS))
goto bad_length;
p = (struct portDS *) m->data;
p->portIdentity.portNumber = ntohs(p->portIdentity.portNumber);
p->peerMeanPathDelay = net2host64(p->peerMeanPathDelay);
break;
case TLV_TIME_STATUS_NP:
if (data_len != sizeof(struct time_status_np))
goto bad_length;
tsn = (struct time_status_np *) m->data;
tsn->master_offset = net2host64(tsn->master_offset);
tsn->ingress_time = net2host64(tsn->ingress_time);
tsn->cumulativeScaledRateOffset = ntohl(tsn->cumulativeScaledRateOffset);
tsn->scaledLastGmPhaseChange = ntohl(tsn->scaledLastGmPhaseChange);
tsn->gmTimeBaseIndicator = ntohs(tsn->gmTimeBaseIndicator);
scaled_ns_n2h(&tsn->lastGmPhaseChange);
tsn->gmPresent = ntohl(tsn->gmPresent);
break;
case TLV_GRANDMASTER_SETTINGS_NP:
if (data_len != sizeof(struct grandmaster_settings_np))
goto bad_length;
gsn = (struct grandmaster_settings_np *) m->data;
gsn->clockQuality.offsetScaledLogVariance =
ntohs(gsn->clockQuality.offsetScaledLogVariance);
gsn->utc_offset = ntohs(gsn->utc_offset);
break;
case TLV_PORT_DATA_SET_NP:
if (data_len != sizeof(struct port_ds_np))
goto bad_length;
pdsnp = (struct port_ds_np *) m->data;
pdsnp->neighborPropDelayThresh = ntohl(pdsnp->neighborPropDelayThresh);
pdsnp->asCapable = ntohl(pdsnp->asCapable);
break;
case TLV_SUBSCRIBE_EVENTS_NP:
if (data_len != sizeof(struct subscribe_events_np))
goto bad_length;
sen = (struct subscribe_events_np *)m->data;
sen->duration = ntohs(sen->duration);
break;
case TLV_PORT_PROPERTIES_NP:
if (data_len < sizeof(struct port_properties_np))
goto bad_length;
ppn = (struct port_properties_np *)m->data;
ppn->portIdentity.portNumber = ntohs(ppn->portIdentity.portNumber);
extra_len = sizeof(struct port_properties_np);
extra_len += ppn->interface.length;
break;
case TLV_SAVE_IN_NON_VOLATILE_STORAGE:
case TLV_RESET_NON_VOLATILE_STORAGE:
case TLV_INITIALIZE:
case TLV_FAULT_LOG_RESET:
case TLV_ENABLE_PORT:
case TLV_DISABLE_PORT:
if (data_len != 0)
goto bad_length;
break;
}
if (extra_len) {
if (extra_len % 2)
extra_len++;
if (extra_len + sizeof(m->id) != m->length)
goto bad_length;
}
return 0;
bad_length:
return -EBADMSG;
}
/*Unpack PdelayReq message from IN buffer of ptpClock to msgtmp.req*/
void msgUnpackPDelayReq(const Octet *buf, MsgPDelayReq *pdelayreq)
{
pdelayreq->originTimestamp.secondsField.msb = flip16(*(UInteger16*)(buf + 34));
pdelayreq->originTimestamp.secondsField.lsb = flip32(*(UInteger32*)(buf + 36));
pdelayreq->originTimestamp.nanosecondsField = flip32(*(UInteger32*)(buf + 40));
}
void msgUnpackManagementPayload(void *buf, MsgManagement *manage)
{
switch(manage->managementMessageKey)
{
case PTP_MM_CLOCK_IDENTITY:
DBGV("msgUnloadManagementPayload: managementMessageKey PTP_MM_CLOCK_IDENTITY\n");
manage->payload.clockIdentity.clockCommunicationTechnology = *(UInteger8*)((char*)buf + 63);
memcpy(manage->payload.clockIdentity.clockUuidField, (char*)buf + 64, PTP_UUID_LENGTH);
manage->payload.clockIdentity.clockPortField = flip16(*(UInteger16*)((char*)buf + 74));
memcpy(manage->payload.clockIdentity.manufacturerIdentity, (char*)buf + 76, MANUFACTURER_ID_LENGTH);
break;
case PTP_MM_DEFAULT_DATA_SET:
DBGV("msgUnloadManagementPayload: managementMessageKey PTP_MM_DEFAULT_DATA_SET\n");
manage->payload.defaultData.clockCommunicationTechnology = *(UInteger8*)((char*)buf + 63);
memcpy(manage->payload.defaultData.clockUuidField, (char*)buf + 64, PTP_UUID_LENGTH);
manage->payload.defaultData.clockPortField = flip16(*(UInteger16*)((char*)buf + 74));
manage->payload.defaultData.clockStratum = *(UInteger8*)((char*)buf + 79);
memcpy(manage->payload.defaultData.clockIdentifier, (char*)buf + 80, PTP_CODE_STRING_LENGTH);
manage->payload.defaultData.clockVariance = flip16(*(UInteger16*)((char*)buf + 86));
manage->payload.defaultData.clockFollowupCapable = *(UInteger8*)((char*)buf + 91);
manage->payload.defaultData.preferred = *(UInteger8*)((char*)buf + 95);
manage->payload.defaultData.initializable = *(UInteger8*)((char*)buf + 99);
manage->payload.defaultData.externalTiming = *(UInteger8*)((char*)buf + 103);
manage->payload.defaultData.isBoundaryClock = *(UInteger8*)((char*)buf + 107);
manage->payload.defaultData.syncInterval = *(UInteger8*)((char*)buf + 111);
memcpy(manage->payload.defaultData.subdomainName, (char*)buf + 112, PTP_SUBDOMAIN_NAME_LENGTH);
manage->payload.defaultData.numberPorts = flip16(*(UInteger16*)((char*)buf + 130));
manage->payload.defaultData.numberForeignRecords = flip16(*(UInteger16*)((char*)buf + 134));
break;
case PTP_MM_CURRENT_DATA_SET:
DBGV("msgUnloadManagementPayload: managementMessageKey PTP_MM_CURRENT_DATA_SET\n");
manage->payload.current.stepsRemoved = flip16(*(UInteger16*)((char*)buf + 62));
manage->payload.current.offsetFromMaster.seconds = flip32(*(UInteger32*)((char*)buf + 64));
manage->payload.current.offsetFromMaster.nanoseconds = flip32(*(UInteger32*)((char*)buf + 68));
manage->payload.current.oneWayDelay.seconds = flip32(*(UInteger32*)((char*)buf + 72));
manage->payload.current.oneWayDelay.nanoseconds = flip32(*(Integer32*)((char*)buf + 76));
break;
case PTP_MM_PARENT_DATA_SET:
DBGV("msgUnloadManagementPayload: managementMessageKey PTP_MM_PORT_DATA_SET\n");
manage->payload.parent.parentCommunicationTechnology = *(UInteger8*)((char*)buf + 63);
memcpy(manage->payload.parent.parentUuid, (char*)buf + 64, PTP_UUID_LENGTH);
manage->payload.parent.parentPortId = flip16(*(UInteger16*)((char*)buf + 74));
manage->payload.parent.parentLastSyncSequenceNumber = flip16(*(UInteger16*)((char*)buf + 74));
manage->payload.parent.parentFollowupCapable = *(UInteger8*)((char*)buf + 83);
manage->payload.parent.parentExternalTiming = *(UInteger8*)((char*)buf + 87);
manage->payload.parent.parentVariance = flip16(*(UInteger16*)((char*)buf + 90));
manage->payload.parent.parentStats = *(UInteger8*)((char*)buf + 85);
manage->payload.parent.observedVariance = flip16(*(Integer16*)((char*)buf + 98));
manage->payload.parent.observedDrift = flip32(*(Integer32*)((char*)buf + 100));
manage->payload.parent.utcReasonable = *(UInteger8*)((char*)buf + 107);
manage->payload.parent.grandmasterCommunicationTechnology = *(UInteger8*)((char*)buf + 111);
memcpy(manage->payload.parent.grandmasterUuidField, (char*)buf + 112, PTP_UUID_LENGTH);
manage->payload.parent.grandmasterPortIdField = flip16(*(UInteger16*)((char*)buf + 122));
manage->payload.parent.grandmasterStratum = *(UInteger8*)((char*)buf + 127);
memcpy(manage->payload.parent.grandmasterIdentifier, (char*)buf + 128, PTP_CODE_STRING_LENGTH);
manage->payload.parent.grandmasterVariance = flip16(*(Integer16*)((char*)buf + 134));
manage->payload.parent.grandmasterPreferred = *(UInteger8*)((char*)buf + 139);
manage->payload.parent.grandmasterIsBoundaryClock = *(UInteger8*)((char*)buf + 144);
manage->payload.parent.grandmasterSequenceNumber = flip16(*(UInteger16*)((char*)buf + 146));
break;
case PTP_MM_PORT_DATA_SET:
DBGV("msgUnloadManagementPayload: managementMessageKey PTP_MM_FOREIGN_DATA_SET\n");
manage->payload.port.returnedPortNumber = flip16(*(UInteger16*)((char*)buf + 62));
manage->payload.port.portState = *(UInteger8*)((char*)buf + 67);
manage->payload.port.lastSyncEventSequenceNumber = flip16(*(UInteger16*)((char*)buf + 70));
manage->payload.port.lastGeneralEventSequenceNumber = flip16(*(UInteger16*)((char*)buf + 74));
manage->payload.port.portCommunicationTechnology = *(UInteger8*)((char*)buf + 79);
memcpy(manage->payload.port.portUuidField, (char*)buf + 80, PTP_UUID_LENGTH);
manage->payload.port.portIdField = flip16(*(UInteger16*)((char*)buf + 90));
manage->payload.port.burstEnabled = *(UInteger8*)((char*)buf + 95);
manage->payload.port.subdomainAddressOctets = *(UInteger8*)((char*)buf + 97);
manage->payload.port.eventPortAddressOctets = *(UInteger8*)((char*)buf + 98);
manage->payload.port.generalPortAddressOctets = *(UInteger8*)((char*)buf + 99);
memcpy(manage->payload.port.subdomainAddress, (char*)buf + 100, SUBDOMAIN_ADDRESS_LENGTH);
memcpy(manage->payload.port.eventPortAddress, (char*)buf + 106, PORT_ADDRESS_LENGTH);
memcpy(manage->payload.port.generalPortAddress, (char*)buf + 110, PORT_ADDRESS_LENGTH);
break;
case PTP_MM_GLOBAL_TIME_DATA_SET:
DBGV("msgUnloadManagementPayload: managementMessageKey PTP_MM_GLOBAL_TIME_DATA_SET\n");
manage->payload.globalTime.localTime.seconds = flip32(*(UInteger32*)((char*)buf + 60));
manage->payload.globalTime.localTime.nanoseconds = flip32(*(Integer32*)((char*)buf + 64));
manage->payload.globalTime.currentUtcOffset = flip16(*(Integer16*)((char*)buf + 70));
manage->payload.globalTime.leap59 = *(UInteger8*)((char*)buf + 75);
manage->payload.globalTime.leap61 = *(UInteger8*)((char*)buf + 79);
manage->payload.globalTime.epochNumber = flip16(*(UInteger16*)((char*)buf + 82));
break;
case PTP_MM_FOREIGN_DATA_SET:
DBGV("msgUnloadManagementPayload: managementMessageKey PTP_MM_FOREIGN_DATA_SET\n");
manage->payload.foreign.returnedPortNumber = flip16(*(UInteger16*)((char*)buf + 62));
manage->payload.foreign.returnedRecordNumber = flip16(*(UInteger16*)((char*)buf + 68));
manage->payload.foreign.foreignMasterCommunicationTechnology = *(UInteger8*)((char*)buf + 71);
memcpy(manage->payload.foreign.foreignMasterUuid, (char*)buf + 72, PTP_UUID_LENGTH);
manage->payload.foreign.foreignMasterPortId = flip16(*(UInteger16*)((char*)buf + 82));
manage->payload.foreign.foreignMasterSyncs = flip16(*(UInteger16*)((char*)buf + 66));
break;
case PTP_MM_NULL:
DBGV("msgUnloadManagementPayload: managementMessageKey NULL\n");
break;
default:
DBGV("msgUnloadManagementPayload: managementMessageKey ?\n");
break;
}
return;
}
UInteger8 msgUnloadManagement(void *buf, MsgManagement *manage,
PtpClock *ptpClock, RunTimeOpts *rtOpts)
{
TimeInternal internalTime;
TimeRepresentation externalTime;
switch(manage->managementMessageKey)
{
case PTP_MM_INITIALIZE_CLOCK:
if(ptpClock->initializable)
return PTP_INITIALIZING;
break;
case PTP_MM_GOTO_FAULTY_STATE:
DBG("event FAULT_DETECTED (forced by management message)\n");
return PTP_FAULTY;
break;
case PTP_MM_DISABLE_PORT:
if(manage->targetPortId == 1)
{
DBG("event DESIGNATED_DISABLED\n");
return PTP_DISABLED;
}
break;
case PTP_MM_ENABLE_PORT:
if(manage->targetPortId == 1)
{
DBG("event DESIGNATED_ENABLED\n");
return PTP_INITIALIZING;
}
break;
case PTP_MM_CLEAR_DESIGNATED_PREFERRED_MASTER:
ptpClock->preferred = FALSE;
break;
case PTP_MM_SET_DESIGNATED_PREFERRED_MASTER:
ptpClock->preferred = TRUE;
break;
case PTP_MM_DISABLE_BURST:
break;
case PTP_MM_ENABLE_BURST:
break;
case PTP_MM_SET_SYNC_INTERVAL:
rtOpts->syncInterval = *(Integer8*)((char*)buf + 63);
break;
case PTP_MM_SET_SUBDOMAIN:
memcpy(rtOpts->subdomainName, (char*)buf + 60, 16);
DBG("set subdomain to %s\n", rtOpts->subdomainName);
break;
case PTP_MM_SET_TIME:
externalTime.seconds = flip32(*(UInteger32*)((char*)buf + 60));
externalTime.nanoseconds = flip32(*(Integer32*)((char*)buf + 64));
toInternalTime(&internalTime, &externalTime, &ptpClock->halfEpoch);
setTime(&internalTime);
break;
case PTP_MM_UPDATE_DEFAULT_DATA_SET:
if(!rtOpts->slaveOnly)
ptpClock->clock_stratum = *(UInteger8*)((char*)buf + 63);
memcpy(ptpClock->clock_identifier, (char*)buf + 64, 4);
ptpClock->clock_variance = flip16(*(Integer16*)((char*)buf + 70));
ptpClock->preferred = *(UInteger8*)((char*)buf + 75);
rtOpts->syncInterval = *(UInteger8*)((char*)buf + 79);
memcpy(rtOpts->subdomainName, (char*)buf + 80, 16);
break;
case PTP_MM_UPDATE_GLOBAL_TIME_PROPERTIES:
ptpClock->current_utc_offset = flip16(*(Integer16*)((char*)buf + 62));
ptpClock->leap_59 = *(UInteger8*)((char*)buf + 67);
ptpClock->leap_61 = *(UInteger8*)((char*)buf + 71);
ptpClock->epoch_number = flip16(*(UInteger16*)((char*)buf + 74));
break;
default:
break;
}
return ptpClock->port_state;
}
static ssize_t netRecv(Octet *buf, TimeInternal *time, BufQueue * msgQueue)
{
ssize_t length;
int i, j;
uint8_t mes;
uint32_t seq;
TimeInternal const_delay;
/* get actual buffer */
struct pbuf * p, *pcopy;
const_delay.seconds = 0;
const_delay.nanoseconds = CONST_DELAY;
//Integer32 cur_t = current_t.seconds;
p = (struct pbuf*)netQGet(msgQueue);
if (!p)
{
return 0;
}
pcopy = p;
/* Here, p points to a valid PBUF structure. Verify that we have
* enough space to store the contents. */
if (p->tot_len > PACKET_SIZE)
{
ERROR("netRecv: received truncated message\n");
return 0;
}
/* if (NULL != time)
{
#if LWIP_PTP
getReceivTimestamp(DP83848_PHY_ADDRESS, mes, seq, &time->seconds, &time->nanoseconds);
#else
getTime(time);
#endif
}*/
/* Copy the PBUF payload into the buffer. */
j = 0;
length = p->tot_len;
for (i = 0; i < length; i++)
{
buf[i] = ((u8_t *)pcopy->payload)[j++];
if (j == pcopy->len)
{
pcopy = pcopy->next;
j = 0;
}
}
if (NULL != time && length > 20)
{
#if LWIP_PTP
/*time->seconds = buf[5];
time->seconds |= (cur_t & 0xFFFFFF00);
if(time->seconds > cur_t + 60)
time->seconds &= 0xFFFFFEFF;
time->nanoseconds = *((uint32_t*)buf + 4);
time->nanoseconds += 215;*/
mes = (*(Enumeration4*)(buf + 0)) & 0x0F;
seq = flip16(*(UInteger16*)(buf + 30));
__disable_irq ();
getReceivTimestamp(DP83848_PHY_ADDRESS, mes, seq, &time->seconds, &time->nanoseconds);
__enable_irq ();
subtime(time, &const_delay, time);
#else
getTime(time);
#endif
}
/* Free up the pbuf (chain). */
pbuf_free(p);
return length;
}
UInteger16 msgPackManagementResponse(void *buf, MsgHeader *header, MsgManagement *manage, PtpClock *ptpClock)
{
TimeInternal internalTime;
TimeRepresentation externalTime;
*(UInteger8*)((char*)buf + 20) = 2; /* messageType */
*(Integer32*)((char*)buf + 28) = shift16(flip16(ptpClock->port_id_field), 0) | shift16(flip16(ptpClock->last_general_event_sequence_number), 1);
*(UInteger8*)((char*)buf + 32) = PTP_MANAGEMENT_MESSAGE; /* control */
clearFlag(((char*)buf + 34), PTP_SYNC_BURST);
clearFlag(((char*)buf + 34), PARENT_STATS);
*(Integer32*)((char*)buf + 40) = shift8(header->sourceCommunicationTechnology, 1);
memcpy((char*)buf + 42, header->sourceUuid, 6);
*(Integer32*)((char*)buf + 48) = shift16(flip16(header->sourcePortId), 0) | shift16(flip16(MM_STARTING_BOUNDARY_HOPS), 1);
*(Integer32*)((char*)buf + 52) = shift16(flip16(manage->startingBoundaryHops - manage->boundaryHops + 1), 0);
switch(manage->managementMessageKey)
{
case PTP_MM_OBTAIN_IDENTITY:
*(UInteger8*)((char*)buf + 55) = PTP_MM_CLOCK_IDENTITY;
*(Integer32*)((char*)buf + 56) = shift16(flip16(64), 1);
*(Integer32*)((char*)buf + 60) = shift8(ptpClock->clock_communication_technology, 3);
memcpy((char*)buf + 64, ptpClock->clock_uuid_field, 6);
*(Integer32*)((char*)buf + 72) = shift16(flip16(ptpClock->clock_port_id_field), 1);
memcpy(((char*)buf + 76), MANUFACTURER_ID, 48);
return 124;
case PTP_MM_GET_DEFAULT_DATA_SET:
*(UInteger8*)((char*)buf + 55) = PTP_MM_DEFAULT_DATA_SET;
*(Integer32*)((char*)buf + 56) = shift16(flip16(76), 1);
*(Integer32*)((char*)buf + 60) = shift8(ptpClock->clock_communication_technology, 3);
memcpy((char*)buf + 64, ptpClock->clock_uuid_field, 6);
*(Integer32*)((char*)buf + 72) = shift16(flip16(ptpClock->clock_port_id_field), 1);
*(Integer32*)((char*)buf + 76) = shift8(ptpClock->clock_stratum, 3);
memcpy((char*)buf + 80, ptpClock->clock_identifier, 4);
*(Integer32*)((char*)buf + 84) = shift16(flip16(ptpClock->clock_variance), 1);
*(Integer32*)((char*)buf + 88) = shift8(ptpClock->clock_followup_capable, 3);
*(Integer32*)((char*)buf + 92) = shift8(ptpClock->preferred, 3);
*(Integer32*)((char*)buf + 96) = shift8(ptpClock->initializable, 3);
*(Integer32*)((char*)buf + 100) = shift8(ptpClock->external_timing, 3);
*(Integer32*)((char*)buf + 104) = shift8(ptpClock->is_boundary_clock, 3);
*(Integer32*)((char*)buf + 108) = shift8(ptpClock->sync_interval, 3);
memcpy((char*)buf + 112, ptpClock->subdomain_name, 16);
*(Integer32*)((char*)buf + 128) = shift16(flip16(ptpClock->number_ports), 1);
*(Integer32*)((char*)buf + 132) = shift16(flip16(ptpClock->number_foreign_records), 1);
return 136;
case PTP_MM_GET_CURRENT_DATA_SET:
*(UInteger8*)((char*)buf + 55) = PTP_MM_CURRENT_DATA_SET;
*(Integer32*)((char*)buf + 56) = shift16(flip16(20), 1);
*(Integer32*)((char*)buf + 60) = shift16(flip16(ptpClock->steps_removed), 1);
fromInternalTime(&ptpClock->offset_from_master, &externalTime, 0);
*(Integer32*)((char*)buf + 64) = flip32(externalTime.seconds);
*(Integer32*)((char*)buf + 68) = flip32(externalTime.nanoseconds);
fromInternalTime(&ptpClock->one_way_delay, &externalTime, 0);
*(Integer32*)((char*)buf + 72) = flip32(externalTime.seconds);
*(Integer32*)((char*)buf + 76) = flip32(externalTime.nanoseconds);
return 80;
case PTP_MM_GET_PARENT_DATA_SET:
*(UInteger8*)((char*)buf + 55) = PTP_MM_PARENT_DATA_SET;
*(Integer32*)((char*)buf + 56) = shift16(flip16(90), 1);
*(Integer32*)((char*)buf + 60) = shift8(ptpClock->parent_communication_technology, 3);
memcpy((char*)buf + 64, ptpClock->parent_uuid, 6);
*(Integer32*)((char*)buf + 72) = shift16(flip16(ptpClock->parent_port_id), 1);
*(Integer32*)((char*)buf + 76) = shift16(flip16(ptpClock->parent_last_sync_sequence_number), 1);
*(Integer32*)((char*)buf + 80) = shift8(ptpClock->parent_followup_capable, 1);
*(Integer32*)((char*)buf + 84) = shift8(ptpClock->parent_external_timing, 3);
*(Integer32*)((char*)buf + 88) = shift16(flip16(ptpClock->parent_variance), 1);
*(Integer32*)((char*)buf + 92) = shift8(ptpClock->parent_stats, 3);
*(Integer32*)((char*)buf + 96) = shift16(flip16(ptpClock->observed_variance), 1);
*(Integer32*)((char*)buf + 100) = flip32(ptpClock->observed_drift);
*(Integer32*)((char*)buf + 104) = shift8(ptpClock->utc_reasonable, 3);
*(Integer32*)((char*)buf + 108) = shift8(ptpClock->grandmaster_communication_technology, 3);
memcpy((char*)buf + 112, ptpClock->grandmaster_uuid_field, 6);
*(Integer32*)((char*)buf + 120) = shift16(flip16(ptpClock->grandmaster_port_id_field), 1);
*(Integer32*)((char*)buf + 124) = shift8(ptpClock->grandmaster_stratum, 3);
memcpy((char*)buf + 128, ptpClock->grandmaster_identifier, 4);
*(Integer32*)((char*)buf + 132) = shift16(flip16(ptpClock->grandmaster_variance), 1);
*(Integer32*)((char*)buf + 136) = shift8(ptpClock->grandmaster_preferred, 3);
*(Integer32*)((char*)buf + 140) = shift8(ptpClock->grandmaster_is_boundary_clock, 3);
*(Integer32*)((char*)buf + 144) = shift16(flip16(ptpClock->grandmaster_sequence_number), 1);
return 148;
case PTP_MM_GET_PORT_DATA_SET:
if(manage->targetPortId && manage->targetPortId != ptpClock->port_id_field)
{
*(UInteger8*)((char*)buf + 55) = PTP_MM_NULL;
*(Integer32*)((char*)buf + 56) = shift16(flip16(0), 1);
return 0;
}
*(UInteger8*)((char*)buf + 55) = PTP_MM_PORT_DATA_SET;
*(Integer32*)((char*)buf + 56) = shift16(flip16(52), 1);
*(Integer32*)((char*)buf + 60) = shift16(flip16(ptpClock->port_id_field), 1);
*(Integer32*)((char*)buf + 64) = shift8(ptpClock->port_state, 3);
*(Integer32*)((char*)buf + 68) = shift16(flip16(ptpClock->last_sync_event_sequence_number), 1);
*(Integer32*)((char*)buf + 72) = shift16(flip16(ptpClock->last_general_event_sequence_number), 1);
*(Integer32*)((char*)buf + 76) = shift8(ptpClock->port_communication_technology, 3);
memcpy((char*)buf + 80, ptpClock->port_uuid_field, 6);
*(Integer32*)((char*)buf + 88) = shift16(flip16(ptpClock->port_id_field), 1);
*(Integer32*)((char*)buf + 92) = shift8(ptpClock->burst_enabled, 3);
*(Integer32*)((char*)buf + 96) = shift8(4, 1) | shift8(2, 2) | shift8(2, 3);
memcpy((char*)buf + 100, ptpClock->subdomain_address, 4);
memcpy((char*)buf + 106, ptpClock->event_port_address, 2);
memcpy((char*)buf + 110, ptpClock->general_port_address, 2);
return 112;
case PTP_MM_GET_GLOBAL_TIME_DATA_SET:
*(UInteger8*)((char*)buf + 55) = PTP_MM_GLOBAL_TIME_DATA_SET;
*(Integer32*)((char*)buf + 56) = shift16(flip16(24), 1);
getTime(&internalTime);
fromInternalTime(&internalTime, &externalTime, ptpClock->halfEpoch);
*(Integer32*)((char*)buf + 60) = flip32(externalTime.seconds);
*(Integer32*)((char*)buf + 64) = flip32(externalTime.nanoseconds);
*(Integer32*)((char*)buf + 68) = shift16(flip16(ptpClock->current_utc_offset), 1);
*(Integer32*)((char*)buf + 72) = shift8(ptpClock->leap_59, 3);
*(Integer32*)((char*)buf + 76) = shift8(ptpClock->leap_61, 3);
*(Integer32*)((char*)buf + 80) = shift16(flip16(ptpClock->epoch_number), 1);
return 84;
case PTP_MM_GET_FOREIGN_DATA_SET:
if((manage->targetPortId && manage->targetPortId != ptpClock->port_id_field)
|| !manage->recordKey || manage->recordKey > ptpClock->number_foreign_records)
{
*(UInteger8*)((char*)buf + 55) = PTP_MM_NULL;
*(Integer32*)((char*)buf + 56) = shift16(flip16(0), 1);
return 0;
}
*(UInteger8*)((char*)buf + 55) = PTP_MM_FOREIGN_DATA_SET;
*(Integer32*)((char*)buf + 56) = shift16(flip16(28), 1);
*(Integer32*)((char*)buf + 60) = shift16(flip16(ptpClock->port_id_field), 1);
*(Integer32*)((char*)buf + 64) = shift16(flip16(manage->recordKey - 1), 1);
*(Integer32*)((char*)buf + 68) = shift8(ptpClock->foreign[manage->recordKey - 1].foreign_master_communication_technology, 3);
memcpy((char*)buf + 72, ptpClock->foreign[manage->recordKey - 1].foreign_master_uuid, 6);
*(Integer32*)((char*)buf + 80) = shift16(flip16(ptpClock->foreign[manage->recordKey - 1].foreign_master_port_id), 1);
*(Integer32*)((char*)buf + 84) = shift16(flip16(ptpClock->foreign[manage->recordKey - 1].foreign_master_syncs), 1);
return 88;
default:
return 0;
}
}
static void mgt_pre_send(struct management_tlv *m, struct tlv_extra *extra)
{
struct defaultDS *dds;
struct currentDS *cds;
struct parentDS *pds;
struct timePropertiesDS *tp;
struct portDS *p;
struct port_ds_np *pdsnp;
struct time_status_np *tsn;
struct grandmaster_settings_np *gsn;
struct subscribe_events_np *sen;
struct port_properties_np *ppn;
struct mgmt_clock_description *cd;
switch (m->id) {
case TLV_CLOCK_DESCRIPTION:
if (extra) {
cd = &extra->cd;
flip16(cd->clockType);
flip16(&cd->physicalAddress->length);
flip16(&cd->protocolAddress->networkProtocol);
flip16(&cd->protocolAddress->addressLength);
}
break;
case TLV_DEFAULT_DATA_SET:
dds = (struct defaultDS *) m->data;
dds->numberPorts = htons(dds->numberPorts);
dds->clockQuality.offsetScaledLogVariance =
htons(dds->clockQuality.offsetScaledLogVariance);
break;
case TLV_CURRENT_DATA_SET:
cds = (struct currentDS *) m->data;
cds->stepsRemoved = htons(cds->stepsRemoved);
cds->offsetFromMaster = host2net64(cds->offsetFromMaster);
cds->meanPathDelay = host2net64(cds->meanPathDelay);
break;
case TLV_PARENT_DATA_SET:
pds = (struct parentDS *) m->data;
pds->parentPortIdentity.portNumber =
htons(pds->parentPortIdentity.portNumber);
pds->observedParentOffsetScaledLogVariance =
htons(pds->observedParentOffsetScaledLogVariance);
pds->observedParentClockPhaseChangeRate =
htonl(pds->observedParentClockPhaseChangeRate);
pds->grandmasterClockQuality.offsetScaledLogVariance =
htons(pds->grandmasterClockQuality.offsetScaledLogVariance);
break;
case TLV_TIME_PROPERTIES_DATA_SET:
tp = (struct timePropertiesDS *) m->data;
tp->currentUtcOffset = htons(tp->currentUtcOffset);
break;
case TLV_PORT_DATA_SET:
p = (struct portDS *) m->data;
p->portIdentity.portNumber = htons(p->portIdentity.portNumber);
p->peerMeanPathDelay = host2net64(p->peerMeanPathDelay);
break;
case TLV_TIME_STATUS_NP:
tsn = (struct time_status_np *) m->data;
tsn->master_offset = host2net64(tsn->master_offset);
tsn->ingress_time = host2net64(tsn->ingress_time);
tsn->cumulativeScaledRateOffset = htonl(tsn->cumulativeScaledRateOffset);
tsn->scaledLastGmPhaseChange = htonl(tsn->scaledLastGmPhaseChange);
tsn->gmTimeBaseIndicator = htons(tsn->gmTimeBaseIndicator);
scaled_ns_h2n(&tsn->lastGmPhaseChange);
tsn->gmPresent = htonl(tsn->gmPresent);
break;
case TLV_GRANDMASTER_SETTINGS_NP:
gsn = (struct grandmaster_settings_np *) m->data;
gsn->clockQuality.offsetScaledLogVariance =
htons(gsn->clockQuality.offsetScaledLogVariance);
gsn->utc_offset = htons(gsn->utc_offset);
break;
case TLV_PORT_DATA_SET_NP:
pdsnp = (struct port_ds_np *) m->data;
pdsnp->neighborPropDelayThresh = htonl(pdsnp->neighborPropDelayThresh);
pdsnp->asCapable = htonl(pdsnp->asCapable);
break;
case TLV_SUBSCRIBE_EVENTS_NP:
sen = (struct subscribe_events_np *)m->data;
sen->duration = htons(sen->duration);
break;
case TLV_PORT_PROPERTIES_NP:
ppn = (struct port_properties_np *)m->data;
ppn->portIdentity.portNumber = htons(ppn->portIdentity.portNumber);
break;
}
}
