void handleDelayResp(MsgHeader *header, Octet *msgIbuf, ssize_t length, Boolean isFromSelf, PtpClock *ptpClock)
{
MsgDelayResp *resp;
if(length < DELAY_RESP_PACKET_LENGTH)
{
ERROR("short delay request message\n");
toState(PTP_FAULTY, ptpClock);
return;
}
switch(ptpClock->port_state)
{
case PTP_SLAVE:
if(isFromSelf)
{
DBG("handleDelayResp: ignore from self\n");
return;
}
resp = &ptpClock->msgTmp.resp;
msgUnpackDelayResp(ptpClock->msgIbuf, resp);
if( ptpClock->sentDelayReq
&& resp->requestingSourceSequenceId == ptpClock->sentDelayReqSequenceId
&& resp->requestingSourceCommunicationTechnology == ptpClock->port_communication_technology
&& resp->requestingSourcePortId == ptpClock->port_id_field
&& !memcmp(resp->requestingSourceUuid, ptpClock->port_uuid_field, PTP_UUID_LENGTH)
&& header->sourceCommunicationTechnology == ptpClock->parent_communication_technology
&& header->sourcePortId == ptpClock->parent_port_id
&& !memcmp(header->sourceUuid, ptpClock->parent_uuid, PTP_UUID_LENGTH) )
{
ptpClock->sentDelayReq = FALSE;
toInternalTime(&ptpClock->delay_req_receive_time, &resp->delayReceiptTimestamp, &ptpClock->halfEpoch);
if(ptpClock->delay_req_send_time.seconds)
{
updateDelay(&ptpClock->delay_req_send_time, &ptpClock->delay_req_receive_time,
&ptpClock->owd_filt, ptpClock);
ptpClock->delay_req_send_time.seconds = 0;
ptpClock->delay_req_send_time.nanoseconds = 0;
ptpClock->delay_req_receive_time.seconds = 0;
ptpClock->delay_req_receive_time.nanoseconds = 0;
}
}
else
{
DBGV("handleDelayResp: unwanted\n");
}
break;
default:
DBGV("handleDelayResp: disreguard\n");
return;
}
}
void
handlePDelayRespFollowUp(MsgHeader *header, Octet *msgIbuf, ssize_t length,
Boolean isFromSelf, RunTimeOpts *rtOpts,
PtpClock *ptpClock){
if (ptpClock->delayMechanism == P2P) {
TimeInternal responseOriginTimestamp;
TimeInternal correctionField;
DBGV("PdelayRespfollowup message received : \n");
if(length < PDELAY_RESP_FOLLOW_UP_LENGTH) {
ERROR("short PDelayRespfollowup message\n");
toState(PTP_FAULTY, rtOpts, ptpClock);
return;
}
switch(ptpClock->portState) {
case PTP_INITIALIZING:
case PTP_FAULTY:
case PTP_DISABLED:
case PTP_UNCALIBRATED:
DBGV("HandlePdelayResp : disregard\n");
return;
case PTP_SLAVE:
case PTP_MASTER:
if ((header->sequenceId ==
ptpClock->sentPDelayReqSequenceId-1) && (header->sequenceId == ptpClock->recvPDelayRespSequenceId)) {
msgUnpackPDelayRespFollowUp(
ptpClock->msgIbuf,
&ptpClock->msgTmp.prespfollow);
toInternalTime(
&responseOriginTimestamp,
&ptpClock->msgTmp.prespfollow.responseOriginTimestamp);
ptpClock->pdelay_resp_send_time.seconds =
responseOriginTimestamp.seconds;
ptpClock->pdelay_resp_send_time.nanoseconds =
responseOriginTimestamp.nanoseconds;
integer64_to_internalTime(
ptpClock->msgTmpHeader.correctionfield,
&correctionField);
addTime(&correctionField,&correctionField,
&ptpClock->lastPdelayRespCorrectionField);
updatePeerDelay (&ptpClock->owd_filt,
rtOpts, ptpClock,
&correctionField,TRUE);
break;
}
default:
DBGV("Disregard PdelayRespFollowUp message \n");
}
} else { /* (End to End mode..) */
ERROR("Peer Delay messages are disregarded in End to End "
"mode \n");
}
}
void handleFollowUp(MsgHeader *header, Octet *msgIbuf, ssize_t length, Boolean isFromSelf, PtpClock *ptpClock)
{
MsgFollowUp *follow;
TimeInternal preciseOriginTimestamp;
if(length < FOLLOW_UP_PACKET_LENGTH)
{
ERROR("short folow up message\n");
toState(PTP_FAULTY, ptpClock);
return;
}
switch(ptpClock->port_state)
{
case PTP_SLAVE:
if(isFromSelf)
{
DBG("handleFollowUp: ignore from self\n");
return;
}
if(getFlag(header->flags, PTP_SYNC_BURST) && !ptpClock->burst_enabled)
return;
DBGV("handleFollowUp: looking for uuid %02hhx:%02hhx:%02hhx:%02hhx:%02hhx:%02hhx\n",
ptpClock->parent_uuid[0], ptpClock->parent_uuid[1], ptpClock->parent_uuid[2],
ptpClock->parent_uuid[3], ptpClock->parent_uuid[4], ptpClock->parent_uuid[5]);
follow = &ptpClock->msgTmp.follow;
msgUnpackFollowUp(ptpClock->msgIbuf, follow);
if( ptpClock->waitingForFollow
&& follow->associatedSequenceId == ptpClock->parent_last_sync_sequence_number
&& header->sourceCommunicationTechnology == ptpClock->parent_communication_technology
&& header->sourcePortId == ptpClock->parent_port_id
&& !memcmp(header->sourceUuid, ptpClock->parent_uuid, PTP_UUID_LENGTH) )
{
ptpClock->waitingForFollow = FALSE;
toInternalTime(&preciseOriginTimestamp, &follow->preciseOriginTimestamp, &ptpClock->halfEpoch);
updateOffset(&preciseOriginTimestamp, &ptpClock->sync_receive_time,
&ptpClock->ofm_filt, ptpClock);
updateClock(ptpClock);
}
else
{
DBGV("handleFollowUp: unwanted\n");
}
break;
default:
DBGV("handleFollowUp: disreguard\n");
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;
}
void handleSync(MsgHeader *header, Octet *msgIbuf, ssize_t length, TimeInternal *time, Boolean badTime, Boolean isFromSelf, PtpClock *ptpClock)
{
MsgSync *sync;
TimeInternal originTimestamp;
if(length < SYNC_PACKET_LENGTH)
{
ERROR("short sync message\n");
toState(PTP_FAULTY, ptpClock);
return;
}
switch(ptpClock->port_state)
{
case PTP_FAULTY:
case PTP_INITIALIZING:
case PTP_DISABLED:
DBGV("handleSync: disreguard\n");
return;
case PTP_UNCALIBRATED:
case PTP_SLAVE:
if(isFromSelf)
{
DBG("handleSync: ignore from self\n");
return;
}
if(getFlag(header->flags, PTP_SYNC_BURST) && !ptpClock->burst_enabled)
return;
DBGV("handleSync: looking for uuid %02hhx:%02hhx:%02hhx:%02hhx:%02hhx:%02hhx\n",
ptpClock->parent_uuid[0], ptpClock->parent_uuid[1], ptpClock->parent_uuid[2],
ptpClock->parent_uuid[3], ptpClock->parent_uuid[4], ptpClock->parent_uuid[5]);
if( header->sequenceId > ptpClock->parent_last_sync_sequence_number
&& header->sourceCommunicationTechnology == ptpClock->parent_communication_technology
&& header->sourcePortId == ptpClock->parent_port_id
&& !memcmp(header->sourceUuid, ptpClock->parent_uuid, PTP_UUID_LENGTH) )
{
/* addForeign() takes care of msgUnpackSync() */
ptpClock->record_update = TRUE;
sync = addForeign(ptpClock->msgIbuf, &ptpClock->msgTmpHeader, ptpClock);
if(sync->syncInterval != ptpClock->sync_interval)
{
DBGV("message's sync interval is %d, but clock's is %d\n", sync->syncInterval, ptpClock->sync_interval);
/* spec recommends handling a sync interval discrepancy as a fault */
}
/*
* TODO: Sync packets without hardware time stamp are rare, but might happen.
* Need to decide what to do with the bad default time stamp, similar to handleDelayReq().
*/
ptpClock->sync_receive_time.seconds = time->seconds;
ptpClock->sync_receive_time.nanoseconds = time->nanoseconds;
if(!getFlag(header->flags, PTP_ASSIST))
{
ptpClock->waitingForFollow = FALSE;
toInternalTime(&originTimestamp, &sync->originTimestamp, &ptpClock->halfEpoch);
updateOffset(&originTimestamp, &ptpClock->sync_receive_time,
&ptpClock->ofm_filt, ptpClock);
updateClock(ptpClock);
}
else
{
ptpClock->waitingForFollow = TRUE;
}
s1(header, sync, ptpClock);
if(!(--ptpClock->R))
{
issueDelayReq(ptpClock);
ptpClock->Q = 0;
ptpClock->R = getRand(&ptpClock->random_seed)%(PTP_DELAY_REQ_INTERVAL - 2) + 2;
DBG("Q = %d, R = %d\n", ptpClock->Q, ptpClock->R);
}
DBGV("SYNC_RECEIPT_TIMER reset\n");
timerStart(SYNC_RECEIPT_TIMER, PTP_SYNC_RECEIPT_TIMEOUT(ptpClock->sync_interval), ptpClock->itimer);
}
else
{
DBGV("handleSync: unwanted\n");
}
case PTP_MASTER:
default:
if( header->sourceCommunicationTechnology == ptpClock->clock_communication_technology
|| header->sourceCommunicationTechnology == PTP_DEFAULT
|| ptpClock->clock_communication_technology == PTP_DEFAULT )
{
if(!isFromSelf)
{
ptpClock->record_update = TRUE;
addForeign(ptpClock->msgIbuf, &ptpClock->msgTmpHeader, ptpClock);
}
else if(ptpClock->port_state == PTP_MASTER && ptpClock->clock_followup_capable)
{
addTime(time, time, &ptpClock->runTimeOpts.outboundLatency);
issueFollowup(time, ptpClock);
}
}
break;
}
}
static void handleDelayResp(PtpClock *ptpClock, Boolean isFromSelf)
{
Boolean isFromCurrentParent = FALSE;
Boolean isCurrentRequest = FALSE;
TimeInternal correctionField;
switch (ptpClock->portDS.delayMechanism)
{
case E2E:
DBGV("handleDelayResp: received\n");
if (ptpClock->msgIbufLength < DELAY_RESP_LENGTH)
{
ERROR("handleDelayResp: short message\n");
toState(ptpClock, PTP_FAULTY);
return;
}
switch (ptpClock->portDS.portState)
{
case PTP_INITIALIZING:
case PTP_FAULTY:
case PTP_DISABLED:
case PTP_LISTENING:
DBGV("handleDelayResp: disreguard\n");
return;
case PTP_UNCALIBRATED:
case PTP_SLAVE:
msgUnpackDelayResp(ptpClock->msgIbuf, &ptpClock->msgTmp.resp);
isFromCurrentParent = isSamePortIdentity(
&ptpClock->parentDS.parentPortIdentity,
&ptpClock->msgTmpHeader.sourcePortIdentity);
isCurrentRequest = isSamePortIdentity(
&ptpClock->portDS.portIdentity,
&ptpClock->msgTmp.resp.requestingPortIdentity);
if (((ptpClock->sentDelayReqSequenceId - 1) == ptpClock->msgTmpHeader.sequenceId)
&& isCurrentRequest && isFromCurrentParent)
{
/* TODO: revisit 11.3 */
toInternalTime(&ptpClock->timestamp_delayReqRecieve, &ptpClock->msgTmp.resp.receiveTimestamp);
scaledNanosecondsToInternalTime(&ptpClock->msgTmpHeader.correctionfield, &correctionField);
updateDelay(ptpClock, &ptpClock->timestamp_delayReqSend, &ptpClock->timestamp_delayReqRecieve, &correctionField);
ptpClock->portDS.logMinDelayReqInterval = ptpClock->msgTmpHeader.logMessageInterval;
}
else
{
DBGV("handleDelayResp: doesn't match with the delayReq\n");
break;
}
}
break;
case P2P:
ERROR("handleDelayResp: disreguard in P2P mode\n");
break;
default:
break;
}
}
static void handleFollowUp(PtpClock *ptpClock, Boolean isFromSelf)
{
TimeInternal preciseOriginTimestamp;
TimeInternal correctionField;
Boolean isFromCurrentParent = FALSE;
DBGV("handleFollowup: received\n");
if (ptpClock->msgIbufLength < FOLLOW_UP_LENGTH)
{
ERROR("handleFollowup: short message\n");
toState(ptpClock, PTP_FAULTY);
return;
}
if (isFromSelf)
{
DBGV("handleFollowup: ignore from self\n");
return;
}
switch (ptpClock->portDS.portState)
{
case PTP_INITIALIZING:
case PTP_FAULTY:
case PTP_DISABLED:
case PTP_LISTENING:
DBGV("handleFollowup: disreguard\n");
break;
case PTP_UNCALIBRATED:
case PTP_SLAVE:
isFromCurrentParent = isSamePortIdentity(
&ptpClock->parentDS.parentPortIdentity,
&ptpClock->msgTmpHeader.sourcePortIdentity);
if (!ptpClock->waitingForFollowUp)
{
DBGV("handleFollowup: not waiting a message\n");
break;
}
if (!isFromCurrentParent)
{
DBGV("handleFollowup: not from current parent\n");
break;
}
if (ptpClock->recvSyncSequenceId != ptpClock->msgTmpHeader.sequenceId)
{
DBGV("handleFollowup: SequenceID doesn't match with last Sync message\n");
break;
}
msgUnpackFollowUp(ptpClock->msgIbuf, &ptpClock->msgTmp.follow);
ptpClock->waitingForFollowUp = FALSE;
/* synchronize local clock */
toInternalTime(&preciseOriginTimestamp, &ptpClock->msgTmp.follow.preciseOriginTimestamp);
scaledNanosecondsToInternalTime(&ptpClock->msgTmpHeader.correctionfield, &correctionField);
addTime(&correctionField, &correctionField, &ptpClock->correctionField_sync);
updateOffset(ptpClock, &ptpClock->timestamp_syncRecieve, &preciseOriginTimestamp, &correctionField);
updateClock(ptpClock);
issueDelayReqTimerExpired(ptpClock);
break;
case PTP_MASTER:
DBGV("handleFollowup: from another master\n");
break;
case PTP_PASSIVE:
issueDelayReqTimerExpired(ptpClock);
DBGV("handleFollowup: disreguard\n");
break;
default:
DBG("handleFollowup: unrecognized state\n");
break;
}//Switch on (port_state)
}
static void handleSync(PtpClock *ptpClock, TimeInternal *time, Boolean isFromSelf)
{
TimeInternal originTimestamp;
TimeInternal correctionField;
Boolean isFromCurrentParent = FALSE;
DBGV("handleSync: received\n");
if (ptpClock->msgIbufLength < SYNC_LENGTH)
{
ERROR("handleSync: short message\n");
toState(ptpClock, PTP_FAULTY);
return;
}
switch (ptpClock->portDS.portState)
{
case PTP_INITIALIZING:
case PTP_FAULTY:
case PTP_DISABLED:
DBGV("handleSync: disreguard\n");
break;
case PTP_UNCALIBRATED:
case PTP_SLAVE:
if (isFromSelf)
{
DBGV("handleSync: ignore from self\n");
break;
}
isFromCurrentParent = isSamePortIdentity(
&ptpClock->parentDS.parentPortIdentity,
&ptpClock->msgTmpHeader.sourcePortIdentity);
if (!isFromCurrentParent)
{
DBGV("handleSync: ignore from another master\n");
break;
}
ptpClock->timestamp_syncRecieve = *time;
scaledNanosecondsToInternalTime(&ptpClock->msgTmpHeader.correctionfield, &correctionField);
if (getFlag(ptpClock->msgTmpHeader.flagField[0], FLAG0_TWO_STEP))
{
ptpClock->waitingForFollowUp = TRUE;
ptpClock->recvSyncSequenceId = ptpClock->msgTmpHeader.sequenceId;
/* Save correctionField of Sync message for future use */
ptpClock->correctionField_sync = correctionField;
}
else
{
msgUnpackSync(ptpClock->msgIbuf, &ptpClock->msgTmp.sync);
ptpClock->waitingForFollowUp = FALSE;
/* Synchronize local clock */
toInternalTime(&originTimestamp, &ptpClock->msgTmp.sync.originTimestamp);
/* use correctionField of Sync message for future use */
updateOffset(ptpClock, &ptpClock->timestamp_syncRecieve, &originTimestamp, &correctionField);
updateClock(ptpClock);
issueDelayReqTimerExpired(ptpClock);
}
break;
case PTP_MASTER:
if (!isFromSelf)
{
DBGV("handleSync: from another master\n");
break;
}
else
{
DBGV("handleSync: ignore from self\n");
break;
}
// if waitingForLoopback && TWO_STEP_FLAG
// {
// /*Add latency*/
// addTime(time, time, &rtOpts->outboundLatency);
//
// issueFollowup(ptpClock, time);
// break;
// }
case PTP_PASSIVE:
issueDelayReqTimerExpired(ptpClock);
DBGV("handleSync: disreguard\n");
break;
default:
DBGV("handleSync: disreguard\n");
break;
}
}
static void handlePDelayRespFollowUp(PtpClock *ptpClock, Boolean isFromSelf)
{
TimeInternal responseOriginTimestamp;
TimeInternal correctionField;
switch (ptpClock->portDS.delayMechanism)
{
case E2E:
ERROR("handlePDelayRespFollowUp: disreguard in E2E mode\n");
break;
case P2P:
DBGV("handlePDelayRespFollowUp: received\n");
if (ptpClock->msgIbufLength < PDELAY_RESP_FOLLOW_UP_LENGTH)
{
ERROR("handlePDelayRespFollowUp: short message\n");
toState(ptpClock, PTP_FAULTY);
return;
}
switch (ptpClock->portDS.portState)
{
case PTP_INITIALIZING:
case PTP_FAULTY:
case PTP_DISABLED:
case PTP_UNCALIBRATED:
DBGV("handlePDelayRespFollowUp: disreguard\n");
return;
case PTP_SLAVE:
case PTP_MASTER:
if (!ptpClock->waitingForPDelayRespFollowUp)
{
DBG("handlePDelayRespFollowUp: not waiting a message\n");
break;
}
if (ptpClock->msgTmpHeader.sequenceId == ptpClock->sentPDelayReqSequenceId - 1)
{
msgUnpackPDelayRespFollowUp(ptpClock->msgIbuf, &ptpClock->msgTmp.prespfollow);
toInternalTime(&responseOriginTimestamp, &ptpClock->msgTmp.prespfollow.responseOriginTimestamp);
ptpClock->pdelay_t3 = responseOriginTimestamp;
scaledNanosecondsToInternalTime(&ptpClock->msgTmpHeader.correctionfield, &correctionField);
addTime(&correctionField, &correctionField, &ptpClock->correctionField_pDelayResp);
updatePeerDelay(ptpClock, &correctionField, TRUE);
ptpClock->waitingForPDelayRespFollowUp = FALSE;
break;
}
default:
DBGV("handlePDelayRespFollowUp: unrecognized state\n");
}
break;
default:
/* nothing */
break;
}
}
static void handlePDelayResp(PtpClock *ptpClock, TimeInternal *time, Boolean isFromSelf)
{
TimeInternal requestReceiptTimestamp;
TimeInternal correctionField;
Boolean isCurrentRequest;
switch (ptpClock->portDS.delayMechanism)
{
case E2E:
ERROR("handlePDelayReq: disreguard in E2E mode\n");
break;
case P2P:
DBGV("handlePDelayReq: received\n");
if (ptpClock->msgIbufLength < PDELAY_RESP_LENGTH)
{
ERROR("handlePDelayReq: short message\n");
toState(ptpClock, PTP_FAULTY);
return;
}
switch (ptpClock->portDS.portState)
{
case PTP_INITIALIZING:
case PTP_FAULTY:
case PTP_DISABLED:
case PTP_UNCALIBRATED:
case PTP_LISTENING:
DBGV("handlePDelayReq: disreguard\n");
return;
case PTP_MASTER:
case PTP_SLAVE:
if (isFromSelf)
{
DBGV("handlePDelayReq: ignore from self\n");
break;
}
// if (isFromSelf) && loopback mode
// {
// addTime(time, time, &rtOpts->outboundLatency);
// issuePDelayRespFollowUp(time, ptpClock);
// break;
// }
msgUnpackPDelayResp(ptpClock->msgIbuf, &ptpClock->msgTmp.presp);
isCurrentRequest = isSamePortIdentity(
&ptpClock->portDS.portIdentity,
&ptpClock->msgTmp.presp.requestingPortIdentity);
if (((ptpClock->sentPDelayReqSequenceId - 1) == ptpClock->msgTmpHeader.sequenceId)
&& isCurrentRequest)
{
if (getFlag(ptpClock->msgTmpHeader.flagField[0], FLAG0_TWO_STEP))
{
ptpClock->waitingForPDelayRespFollowUp = TRUE;
/*Store t4 (Fig 35)*/
ptpClock->pdelay_t4 = *time;
/*store t2 (Fig 35)*/
toInternalTime(&requestReceiptTimestamp, &ptpClock->msgTmp.presp.requestReceiptTimestamp);
ptpClock->pdelay_t2 = requestReceiptTimestamp;
scaledNanosecondsToInternalTime(&ptpClock->msgTmpHeader.correctionfield, &correctionField);
ptpClock->correctionField_pDelayResp = correctionField;
}//Two Step Clock
else //One step Clock
{
ptpClock->waitingForPDelayRespFollowUp = FALSE;
/*Store t4 (Fig 35)*/
ptpClock->pdelay_t4 = *time;
scaledNanosecondsToInternalTime(&ptpClock->msgTmpHeader.correctionfield, &correctionField);
updatePeerDelay(ptpClock, &correctionField, FALSE);
}
}
else
{
DBGV("handlePDelayReq: PDelayResp doesn't match with the PDelayReq.\n");
}
break;
default:
DBG("handlePDelayReq: unrecognized state\n");
break;
}
break;
default:
/* nothing */
break;
}
}
void
handleFollowUp(MsgHeader *header, Octet *msgIbuf, ssize_t length,
Boolean isFromSelf, RunTimeOpts *rtOpts, PtpClock *ptpClock)
{
DBGV("Handlefollowup : Follow up message received \n");
TimeInternal preciseOriginTimestamp;
TimeInternal correctionField;
Boolean isFromCurrentParent = FALSE;
if (length < FOLLOW_UP_LENGTH)
{
ERROR("short Follow up message\n");
toState(PTP_FAULTY, rtOpts, ptpClock);
return;
}
if (isFromSelf)
{
DBGV("Handlefollowup : Ignore message from self \n");
return;
}
switch (ptpClock->portState)
{
case PTP_INITIALIZING:
case PTP_FAULTY:
case PTP_DISABLED:
case PTP_LISTENING:
DBGV("Handfollowup : disregard\n");
return;
case PTP_UNCALIBRATED:
case PTP_SLAVE:
isFromCurrentParent =
!memcmp(ptpClock->parentPortIdentity.clockIdentity,
header->sourcePortIdentity.clockIdentity,
CLOCK_IDENTITY_LENGTH) &&
(ptpClock->parentPortIdentity.portNumber ==
header->sourcePortIdentity.portNumber);
if (isFromCurrentParent) {
if (ptpClock->waitingForFollow) {
if ((ptpClock->recvSyncSequenceId ==
header->sequenceId)) {
msgUnpackFollowUp(ptpClock->msgIbuf,
&ptpClock->msgTmp.follow);
ptpClock->waitingForFollow = FALSE;
toInternalTime(&preciseOriginTimestamp,
&ptpClock->msgTmp.follow.preciseOriginTimestamp);
integer64_to_internalTime(ptpClock->msgTmpHeader.correctionfield,
&correctionField);
addTime(&correctionField,&correctionField,
&ptpClock->lastSyncCorrectionField);
/*
send_time = preciseOriginTimestamp (received inside followup)
recv_time = sync_receive_time (received as CMSG in handleEvent)
*/
updateOffset(&preciseOriginTimestamp,
&ptpClock->sync_receive_time,&ptpClock->ofm_filt,
rtOpts,ptpClock,
&correctionField);
updateClock(rtOpts,ptpClock);
break;
} else
INFO("Ignored followup, SequenceID doesn't match with "
"last Sync message \n");
} else
DBG2("Ignored followup, Slave was not waiting a follow up "
"message \n");
} else
DBG2("Ignored, Follow up message is not from current parent \n");
case PTP_MASTER:
case PTP_PASSIVE:
DBGV("Ignored, Follow up message received from another master \n");
break;
default:
DBG("do unrecognized state1\n");
break;
} /* Switch on (port_state) */
}
void
handleSync(MsgHeader *header, Octet *msgIbuf, ssize_t length,
TimeInternal *time, Boolean isFromSelf,
RunTimeOpts *rtOpts, PtpClock *ptpClock)
{
TimeInternal OriginTimestamp;
TimeInternal correctionField;
Boolean isFromCurrentParent = FALSE;
DBGV("Sync message received : \n");
if (length < SYNC_LENGTH) {
ERROR("short Sync message\n");
toState(PTP_FAULTY, rtOpts, ptpClock);
return;
}
switch (ptpClock->portState) {
case PTP_INITIALIZING:
case PTP_FAULTY:
case PTP_DISABLED:
DBGV("HandleSync : disregard\n");
return;
case PTP_UNCALIBRATED:
case PTP_SLAVE:
if (isFromSelf) {
DBGV("HandleSync: Ignore message from self \n");
return;
}
isFromCurrentParent =
!memcmp(ptpClock->parentPortIdentity.clockIdentity,
header->sourcePortIdentity.clockIdentity,
CLOCK_IDENTITY_LENGTH) &&
(ptpClock->parentPortIdentity.portNumber ==
header->sourcePortIdentity.portNumber);
if (isFromCurrentParent) {
/* We only start our own delayReq timer after receiving the first sync */
if (ptpClock->waiting_for_first_sync) {
ptpClock->waiting_for_first_sync = FALSE;
NOTICE("Received first Sync from Master\n");
NOTICE(" going to arm DelayReq timer for the first time, with initial rate: %d\n",
ptpClock->logMinDelayReqInterval
);
if (ptpClock->delayMechanism == E2E)
timerStart(DELAYREQ_INTERVAL_TIMER,
pow(2,ptpClock->logMinDelayReqInterval),
ptpClock->itimer);
else if (ptpClock->delayMechanism == P2P)
timerStart(PDELAYREQ_INTERVAL_TIMER,
pow(2,ptpClock->logMinPdelayReqInterval),
ptpClock->itimer);
}
ptpClock->sync_receive_time.seconds = time->seconds;
ptpClock->sync_receive_time.nanoseconds = time->nanoseconds;
recordSync(rtOpts, header->sequenceId, time);
if ((header->flagField[0] & PTP_TWO_STEP) == PTP_TWO_STEP) {
DBG2("HandleSync: waiting for follow-up \n");
ptpClock->waitingForFollow = TRUE;
ptpClock->recvSyncSequenceId =
header->sequenceId;
/*Save correctionField of Sync message*/
integer64_to_internalTime(
header->correctionfield,
&correctionField);
ptpClock->lastSyncCorrectionField.seconds =
correctionField.seconds;
ptpClock->lastSyncCorrectionField.nanoseconds =
correctionField.nanoseconds;
break;
} else {
msgUnpackSync(ptpClock->msgIbuf,
&ptpClock->msgTmp.sync);
integer64_to_internalTime(
ptpClock->msgTmpHeader.correctionfield,
&correctionField);
timeInternal_display(&correctionField);
ptpClock->waitingForFollow = FALSE;
toInternalTime(&OriginTimestamp,
&ptpClock->msgTmp.sync.originTimestamp);
updateOffset(&OriginTimestamp,
&ptpClock->sync_receive_time,
&ptpClock->ofm_filt,rtOpts,
ptpClock,&correctionField);
updateClock(rtOpts,ptpClock);
break;
}
} else {
DBG("HandleSync: Sync message received from "
"another Master not our own \n");
}
break;
case PTP_MASTER:
default :
if (!isFromSelf) {
DBGV("HandleSync: Sync message received from "
"another Master \n");
break;
} if (ptpClock->twoStepFlag) {
DBGV("HandleSync: going to send followup message\n ");
/*Add latency*/
addTime(time,time,&rtOpts->outboundLatency);
issueFollowup(time,rtOpts,ptpClock);
break;
} else {
DBGV("HandleSync: Sync message received from self\n ");
}
}
}
void
handlePDelayResp(MsgHeader *header, Octet *msgIbuf, TimeInternal *time,
ssize_t length, Boolean isFromSelf,
RunTimeOpts *rtOpts, PtpClock *ptpClock)
{
if (ptpClock->delayMechanism == P2P) {
/* Boolean isFromCurrentParent = FALSE; NOTE: This is never used in this function */
TimeInternal requestReceiptTimestamp;
TimeInternal correctionField;
DBGV("PdelayResp message received : \n");
if (length < PDELAY_RESP_LENGTH) {
ERROR("short PDelayResp message\n");
toState(PTP_FAULTY, rtOpts, ptpClock);
return;
}
switch (ptpClock->portState ) {
case PTP_INITIALIZING:
case PTP_FAULTY:
case PTP_DISABLED:
case PTP_UNCALIBRATED:
case PTP_LISTENING:
DBGV("HandlePdelayResp : disregard\n");
return;
case PTP_SLAVE:
case PTP_MASTER:
if (ptpClock->twoStepFlag && isFromSelf) {
addTime(time,time,&rtOpts->outboundLatency);
issuePDelayRespFollowUp(time,
&ptpClock->PdelayReqHeader,
rtOpts,ptpClock);
break;
}
msgUnpackPDelayResp(ptpClock->msgIbuf,
&ptpClock->msgTmp.presp);
#if 0 /* NOTE: This is never used in this function. Should it? */
isFromCurrentParent = !memcmp(ptpClock->parentPortIdentity.clockIdentity,
header->sourcePortIdentity.clockIdentity,CLOCK_IDENTITY_LENGTH) &&
(ptpClock->parentPortIdentity.portNumber ==
header->sourcePortIdentity.portNumber);
#endif
if (!((ptpClock->sentPDelayReqSequenceId ==
header->sequenceId) &&
(!memcmp(ptpClock->portIdentity.clockIdentity,ptpClock->msgTmp.presp.requestingPortIdentity.clockIdentity,CLOCK_IDENTITY_LENGTH))
&& ( ptpClock->portIdentity.portNumber == ptpClock->msgTmp.presp.requestingPortIdentity.portNumber))) {
/* Two Step Clock */
if ((header->flagField[0] & PTP_TWO_STEP) == PTP_TWO_STEP) {
/*Store t4 (Fig 35)*/
ptpClock->pdelay_resp_receive_time.seconds = time->seconds;
ptpClock->pdelay_resp_receive_time.nanoseconds = time->nanoseconds;
/*store t2 (Fig 35)*/
toInternalTime(&requestReceiptTimestamp,
&ptpClock->msgTmp.presp.requestReceiptTimestamp);
ptpClock->pdelay_req_receive_time.seconds = requestReceiptTimestamp.seconds;
ptpClock->pdelay_req_receive_time.nanoseconds = requestReceiptTimestamp.nanoseconds;
integer64_to_internalTime(header->correctionfield,&correctionField);
ptpClock->lastPdelayRespCorrectionField.seconds = correctionField.seconds;
ptpClock->lastPdelayRespCorrectionField.nanoseconds = correctionField.nanoseconds;
break;
} else {
/* One step Clock */
/*Store t4 (Fig 35)*/
ptpClock->pdelay_resp_receive_time.seconds = time->seconds;
ptpClock->pdelay_resp_receive_time.nanoseconds = time->nanoseconds;
integer64_to_internalTime(header->correctionfield,&correctionField);
updatePeerDelay (&ptpClock->owd_filt,rtOpts,ptpClock,&correctionField,FALSE);
break;
}
ptpClock->recvPDelayRespSequenceId = header->sequenceId;
} else {
DBGV("HandlePdelayResp : Pdelayresp doesn't "
"match with the PdelayReq. \n");
break;
}
break; /* XXX added by gnn for safety */
default:
DBG("do unrecognized state4\n");
break;
}
} else { /* (End to End mode..) */
ERROR("Peer Delay messages are disregarded in End to End "
"mode \n");
}
}
void
handleDelayResp(MsgHeader *header, Octet *msgIbuf, ssize_t length,
Boolean isFromSelf, RunTimeOpts *rtOpts, PtpClock *ptpClock)
{
if (ptpClock->delayMechanism == E2E) {
Boolean isFromCurrentParent = FALSE;
TimeInternal requestReceiptTimestamp;
TimeInternal correctionField;
DBGV("delayResp message received : \n");
if(length < DELAY_RESP_LENGTH) {
ERROR("short DelayResp message\n");
toState(PTP_FAULTY, rtOpts, ptpClock);
return;
}
switch(ptpClock->portState) {
case PTP_INITIALIZING:
case PTP_FAULTY:
case PTP_DISABLED:
case PTP_UNCALIBRATED:
case PTP_LISTENING:
DBGV("HandledelayResp : disregard\n");
return;
case PTP_SLAVE:
msgUnpackDelayResp(ptpClock->msgIbuf,
&ptpClock->msgTmp.resp);
if ((memcmp(ptpClock->parentPortIdentity.clockIdentity,
header->sourcePortIdentity.clockIdentity,
CLOCK_IDENTITY_LENGTH) == 0 ) &&
(ptpClock->parentPortIdentity.portNumber ==
header->sourcePortIdentity.portNumber))
isFromCurrentParent = TRUE;
if ((memcmp(ptpClock->portIdentity.clockIdentity,
ptpClock->msgTmp.resp.requestingPortIdentity.clockIdentity,
CLOCK_IDENTITY_LENGTH) == 0) &&
((ptpClock->sentDelayReqSequenceId - 1)==
header->sequenceId) &&
(ptpClock->portIdentity.portNumber ==
ptpClock->msgTmp.resp.requestingPortIdentity.portNumber)
&& isFromCurrentParent) {
DBG("==> Handle DelayResp (%d)\n",
header->sequenceId);
if (!ptpClock->waitingForDelayResp) {
break;
}
ptpClock->waitingForDelayResp = FALSE;
toInternalTime(&requestReceiptTimestamp,
&ptpClock->msgTmp.resp.receiveTimestamp);
ptpClock->delay_req_receive_time.seconds =
requestReceiptTimestamp.seconds;
ptpClock->delay_req_receive_time.nanoseconds =
requestReceiptTimestamp.nanoseconds;
integer64_to_internalTime(
header->correctionfield,
&correctionField);
/*
send_time = delay_req_send_time (received as CMSG in handleEvent)
recv_time = requestReceiptTimestamp (received inside delayResp)
*/
updateDelay(&ptpClock->owd_filt,
rtOpts,ptpClock, &correctionField);
if (ptpClock->waiting_for_first_delayresp) {
ptpClock->waiting_for_first_delayresp = FALSE;
NOTICE(" received first DelayResp from Master\n");
}
if (rtOpts->ignore_delayreq_interval_master == 0) {
DBGV("current delay_req: %d new delay req: %d \n",
ptpClock->logMinDelayReqInterval,
header->logMessageInterval);
/* Accept new DelayReq value from the Master */
if (ptpClock->logMinDelayReqInterval != header->logMessageInterval) {
NOTICE(" received new DelayReq frequency %d from Master (was: %d)\n",
header->logMessageInterval, ptpClock->logMinDelayReqInterval );
}
// collect new value indicated from the Master
ptpClock->logMinDelayReqInterval = header->logMessageInterval;
/* FIXME: the actual rearming of this timer with the new value only happens later in doState()/issueDelayReq() */
} else {
if (ptpClock->logMinDelayReqInterval != rtOpts->subsequent_delayreq) {
NOTICE(" received new DelayReq frequency %d from command line (was: %d)\n",
rtOpts->subsequent_delayreq, ptpClock->logMinDelayReqInterval);
}
ptpClock->logMinDelayReqInterval = rtOpts->subsequent_delayreq;
}
} else {
DBG("HandledelayResp : delayResp doesn't match with the delayReq. \n");
break;
}
}
} else { /* (Peer to Peer mode) */
ERROR("Delay messages are disregarded in Peer to Peer mode \n");
}
}