void IEEE1588Port::becomeMaster( bool annc ) {
port_state = PTP_MASTER;
// Start announce receipt timeout timer
// Start sync receipt timeout timer
clock->deleteEventTimer( this, ANNOUNCE_RECEIPT_TIMEOUT_EXPIRES );
clock->deleteEventTimer( this, SYNC_RECEIPT_TIMEOUT_EXPIRES );
if( annc ) {
startAnnounce();
}
clock->addEventTimer( this, SYNC_INTERVAL_TIMEOUT_EXPIRES, 16000000 );
fprintf( stderr, "Switching to Master\n" );
return;
}
void EtherPort::becomeMaster( bool annc ) {
setPortState( PTP_MASTER );
// Stop announce receipt timeout timer
clock->deleteEventTimerLocked( this, ANNOUNCE_RECEIPT_TIMEOUT_EXPIRES );
// Stop sync receipt timeout timer
stopSyncReceiptTimer();
if( annc ) {
if (!automotive_profile) {
startAnnounce();
}
}
startSyncIntervalTimer(16000000);
GPTP_LOG_STATUS("Switching to Master" );
clock->updateFUPInfo();
return;
}
bool CommonPort::processSyncAnnounceTimeout( Event e )
{
// We're Grandmaster, set grandmaster info to me
ClockIdentity clock_identity;
unsigned char priority1;
unsigned char priority2;
ClockQuality clock_quality;
Timestamp system_time;
Timestamp device_time;
uint32_t local_clock, nominal_clock_rate;
// Nothing to do
if( clock->getPriority1() == 255 )
return true;
// Restart timer
if( e == ANNOUNCE_RECEIPT_TIMEOUT_EXPIRES ) {
clock->addEventTimerLocked
(this, ANNOUNCE_RECEIPT_TIMEOUT_EXPIRES,
(ANNOUNCE_RECEIPT_TIMEOUT_MULTIPLIER*
(unsigned long long)
(pow((double)2,getAnnounceInterval())*
1000000000.0)));
} else {
startSyncReceiptTimer
((unsigned long long)
(SYNC_RECEIPT_TIMEOUT_MULTIPLIER *
((double) pow((double)2, getSyncInterval()) *
1000000000.0)));
}
if( getPortState() == PTP_MASTER )
return true;
GPTP_LOG_STATUS(
"*** %s Timeout Expired - Becoming Master",
e == ANNOUNCE_RECEIPT_TIMEOUT_EXPIRES ? "Announce" :
"Sync" );
clock_identity = getClock()->getClockIdentity();
getClock()->setGrandmasterClockIdentity( clock_identity );
priority1 = getClock()->getPriority1();
getClock()->setGrandmasterPriority1( priority1 );
priority2 = getClock()->getPriority2();
getClock()->setGrandmasterPriority2( priority2 );
clock_quality = getClock()->getClockQuality();
getClock()->setGrandmasterClockQuality( clock_quality );
setPortState( PTP_MASTER );
getDeviceTime( system_time, device_time,
local_clock, nominal_clock_rate );
(void) clock->calcLocalSystemClockRateDifference
( device_time, system_time );
setQualifiedAnnounce( NULL );
clock->addEventTimerLocked
( this, SYNC_INTERVAL_TIMEOUT_EXPIRES,
16000000 );
startAnnounce();
return true;
}
bool EtherPort::_processEvent( Event e )
{
bool ret;
switch (e) {
case POWERUP:
case INITIALIZE:
if (!automotive_profile) {
if ( getPortState() != PTP_SLAVE &&
getPortState() != PTP_MASTER )
{
GPTP_LOG_STATUS("Starting PDelay");
startPDelay();
}
}
else {
startPDelay();
}
port_ready_condition->wait_prelock();
if( !linkWatch(watchNetLinkWrapper, (void *)this) )
{
GPTP_LOG_ERROR("Error creating port link thread");
ret = false;
break;
}
if( !linkOpen(openPortWrapper, (void *)this) )
{
GPTP_LOG_ERROR("Error creating port thread");
ret = false;
break;
}
port_ready_condition->wait();
if (automotive_profile) {
setStationState(STATION_STATE_ETHERNET_READY);
if (getTestMode())
{
APMessageTestStatus *testStatusMsg = new APMessageTestStatus(this);
if (testStatusMsg) {
testStatusMsg->sendPort(this);
delete testStatusMsg;
}
}
if (!isGM) {
// Send an initial signalling message
PTPMessageSignalling *sigMsg = new PTPMessageSignalling(this);
if (sigMsg) {
sigMsg->setintervals(PTPMessageSignalling::sigMsgInterval_NoSend, getSyncInterval(), PTPMessageSignalling::sigMsgInterval_NoSend);
sigMsg->sendPort(this, NULL);
delete sigMsg;
}
startSyncReceiptTimer((unsigned long long)
(SYNC_RECEIPT_TIMEOUT_MULTIPLIER *
((double) pow((double)2, getSyncInterval()) *
1000000000.0)));
}
}
ret = true;
break;
case STATE_CHANGE_EVENT:
// If the automotive profile is enabled, handle the event by
// doing nothing and returning true, preventing the default
// action from executing
if( automotive_profile )
ret = true;
else
ret = false;
break;
case LINKUP:
haltPdelay(false);
startPDelay();
if (automotive_profile) {
GPTP_LOG_EXCEPTION("LINKUP");
}
else {
GPTP_LOG_STATUS("LINKUP");
}
if( clock->getPriority1() == 255 || getPortState() == PTP_SLAVE ) {
becomeSlave( true );
} else if( getPortState() == PTP_MASTER ) {
becomeMaster( true );
} else {
startAnnounce();
}
if (automotive_profile) {
setAsCapable( true );
setStationState(STATION_STATE_ETHERNET_READY);
if (getTestMode())
{
APMessageTestStatus *testStatusMsg = new APMessageTestStatus(this);
if (testStatusMsg) {
testStatusMsg->sendPort(this);
delete testStatusMsg;
}
}
resetInitSyncInterval();
setAnnounceInterval( 0 );
log_min_mean_pdelay_req_interval = initialLogPdelayReqInterval;
if (!isGM) {
// Send an initial signaling message
PTPMessageSignalling *sigMsg = new PTPMessageSignalling(this);
if (sigMsg) {
sigMsg->setintervals(PTPMessageSignalling::sigMsgInterval_NoSend, getSyncInterval(), PTPMessageSignalling::sigMsgInterval_NoSend);
sigMsg->sendPort(this, NULL);
delete sigMsg;
}
startSyncReceiptTimer((unsigned long long)
(SYNC_RECEIPT_TIMEOUT_MULTIPLIER *
((double) pow((double)2, getSyncInterval()) *
1000000000.0)));
}
// Reset Sync count and pdelay count
setPdelayCount(0);
setSyncCount(0);
// Start AVB SYNC at 2. It will decrement after each sync. When it reaches 0 the Test Status message
// can be sent
if (isGM) {
avbSyncState = 1;
}
else {
avbSyncState = 2;
}
if (getTestMode())
{
linkUpCount++;
}
}
this->timestamper_reset();
ret = true;
break;
case LINKDOWN:
stopPDelay();
if (automotive_profile) {
GPTP_LOG_EXCEPTION("LINK DOWN");
}
else {
setAsCapable(false);
GPTP_LOG_STATUS("LINK DOWN");
}
if (getTestMode())
{
linkDownCount++;
}
ret = true;
break;
case ANNOUNCE_RECEIPT_TIMEOUT_EXPIRES:
case SYNC_RECEIPT_TIMEOUT_EXPIRES:
if( !automotive_profile )
{
ret = false;
break;
}
// Automotive Profile specific action
if (e == SYNC_RECEIPT_TIMEOUT_EXPIRES) {
GPTP_LOG_EXCEPTION("SYNC receipt timeout");
startSyncReceiptTimer((unsigned long long)
(SYNC_RECEIPT_TIMEOUT_MULTIPLIER *
((double) pow((double)2, getSyncInterval()) *
1000000000.0)));
}
ret = true;
break;
case PDELAY_INTERVAL_TIMEOUT_EXPIRES:
GPTP_LOG_DEBUG("PDELAY_INTERVAL_TIMEOUT_EXPIRES occured");
{
Timestamp req_timestamp;
PTPMessagePathDelayReq *pdelay_req =
new PTPMessagePathDelayReq(this);
PortIdentity dest_id;
getPortIdentity(dest_id);
pdelay_req->setPortIdentity(&dest_id);
{
Timestamp pending =
PDELAY_PENDING_TIMESTAMP;
pdelay_req->setTimestamp(pending);
}
if (last_pdelay_req != NULL) {
delete last_pdelay_req;
}
setLastPDelayReq(pdelay_req);
getTxLock();
pdelay_req->sendPort(this, NULL);
GPTP_LOG_DEBUG("*** Sent PDelay Request message");
putTxLock();
{
long long timeout;
long long interval;
timeout = PDELAY_RESP_RECEIPT_TIMEOUT_MULTIPLIER *
((long long)
(pow((double)2,getPDelayInterval())*1000000000.0));
timeout = timeout > EVENT_TIMER_GRANULARITY ?
timeout : EVENT_TIMER_GRANULARITY;
clock->addEventTimerLocked
(this, PDELAY_RESP_RECEIPT_TIMEOUT_EXPIRES, timeout );
GPTP_LOG_DEBUG("Schedule PDELAY_RESP_RECEIPT_TIMEOUT_EXPIRES, "
"PDelay interval %d, timeout %lld",
getPDelayInterval(), timeout);
interval =
((long long)
(pow((double)2,getPDelayInterval())*1000000000.0));
interval = interval > EVENT_TIMER_GRANULARITY ?
interval : EVENT_TIMER_GRANULARITY;
startPDelayIntervalTimer(interval);
}
}
break;
case SYNC_INTERVAL_TIMEOUT_EXPIRES:
{
/* Set offset from master to zero, update device vs
system time offset */
// Send a sync message and then a followup to broadcast
PTPMessageSync *sync = new PTPMessageSync(this);
PortIdentity dest_id;
bool tx_succeed;
getPortIdentity(dest_id);
sync->setPortIdentity(&dest_id);
getTxLock();
tx_succeed = sync->sendPort(this, NULL);
GPTP_LOG_DEBUG("Sent SYNC message");
if ( automotive_profile &&
getPortState() == PTP_MASTER )
{
if (avbSyncState > 0) {
avbSyncState--;
if (avbSyncState == 0) {
// Send Avnu Automotive Profile status message
setStationState(STATION_STATE_AVB_SYNC);
if (getTestMode()) {
APMessageTestStatus *testStatusMsg = new APMessageTestStatus(this);
if (testStatusMsg) {
testStatusMsg->sendPort(this);
delete testStatusMsg;
}
}
}
}
}
putTxLock();
if ( tx_succeed )
{
Timestamp sync_timestamp = sync->getTimestamp();
GPTP_LOG_VERBOSE("Successful Sync timestamp");
GPTP_LOG_VERBOSE("Seconds: %u",
sync_timestamp.seconds_ls);
GPTP_LOG_VERBOSE("Nanoseconds: %u",
sync_timestamp.nanoseconds);
PTPMessageFollowUp *follow_up = new PTPMessageFollowUp(this);
PortIdentity dest_id;
getPortIdentity(dest_id);
follow_up->setClockSourceTime(getClock()->getFUPInfo());
follow_up->setPortIdentity(&dest_id);
follow_up->setSequenceId(sync->getSequenceId());
follow_up->setPreciseOriginTimestamp
(sync_timestamp);
follow_up->sendPort(this, NULL);
delete follow_up;
} else {
GPTP_LOG_ERROR
("*** Unsuccessful Sync timestamp");
}
delete sync;
}
break;
case FAULT_DETECTED:
GPTP_LOG_ERROR("Received FAULT_DETECTED event");
if (!automotive_profile) {
setAsCapable(false);
}
break;
case PDELAY_DEFERRED_PROCESSING:
GPTP_LOG_DEBUG("PDELAY_DEFERRED_PROCESSING occured");
pdelay_rx_lock->lock();
if (last_pdelay_resp_fwup == NULL) {
GPTP_LOG_ERROR("PDelay Response Followup is NULL!");
abort();
}
last_pdelay_resp_fwup->processMessage(this);
if (last_pdelay_resp_fwup->garbage()) {
delete last_pdelay_resp_fwup;
this->setLastPDelayRespFollowUp(NULL);
}
pdelay_rx_lock->unlock();
break;
case PDELAY_RESP_RECEIPT_TIMEOUT_EXPIRES:
if (!automotive_profile) {
GPTP_LOG_EXCEPTION("PDelay Response Receipt Timeout");
setAsCapable(false);
}
setPdelayCount( 0 );
break;
case PDELAY_RESP_PEER_MISBEHAVING_TIMEOUT_EXPIRES:
GPTP_LOG_EXCEPTION("PDelay Resp Peer Misbehaving timeout expired! Restarting PDelay");
haltPdelay(false);
if( getPortState() != PTP_SLAVE &&
getPortState() != PTP_MASTER )
{
GPTP_LOG_STATUS("Starting PDelay" );
startPDelay();
}
break;
case SYNC_RATE_INTERVAL_TIMEOUT_EXPIRED:
{
GPTP_LOG_INFO("SYNC_RATE_INTERVAL_TIMEOUT_EXPIRED occured");
sync_rate_interval_timer_started = false;
bool sendSignalMessage = false;
if ( getSyncInterval() != operLogSyncInterval )
{
setSyncInterval( operLogSyncInterval );
sendSignalMessage = true;
}
if (log_min_mean_pdelay_req_interval != operLogPdelayReqInterval) {
log_min_mean_pdelay_req_interval = operLogPdelayReqInterval;
sendSignalMessage = true;
}
if (sendSignalMessage) {
if (!isGM) {
// Send operational signalling message
PTPMessageSignalling *sigMsg = new PTPMessageSignalling(this);
if (sigMsg) {
if (automotive_profile)
sigMsg->setintervals(PTPMessageSignalling::sigMsgInterval_NoChange, getSyncInterval(), PTPMessageSignalling::sigMsgInterval_NoChange);
else
sigMsg->setintervals(log_min_mean_pdelay_req_interval, getSyncInterval(), PTPMessageSignalling::sigMsgInterval_NoChange);
sigMsg->sendPort(this, NULL);
delete sigMsg;
}
startSyncReceiptTimer((unsigned long long)
(SYNC_RECEIPT_TIMEOUT_MULTIPLIER *
((double) pow((double)2, getSyncInterval()) *
1000000000.0)));
}
}
}
break;
default:
GPTP_LOG_ERROR
( "Unhandled event type in "
"EtherPort::processEvent(), %d", e );
ret = false;
break;
}
return ret;
}
void IEEE1588Port::processEvent(Event e)
{
bool changed_external_master;
switch (e) {
case POWERUP:
case INITIALIZE:
GPTP_LOG_DEBUG("Received POWERUP/INITIALIZE event");
{
unsigned long long interval3;
unsigned long long interval4;
Event e3 = NULL_EVENT;
Event e4 = NULL_EVENT;
if (!automotive_profile) {
if (port_state != PTP_SLAVE && port_state != PTP_MASTER) {
GPTP_LOG_STATUS("Starting PDelay");
startPDelay();
}
}
else {
startPDelay();
}
if( clock->getPriority1() == 255 || port_state == PTP_SLAVE ) {
becomeSlave( true );
} else if( port_state == PTP_MASTER ) {
becomeMaster( true );
} else {
/*TODO: Should I remove the commented code below ?*/
//e3 = SYNC_RECEIPT_TIMEOUT_EXPIRES;
e4 = ANNOUNCE_RECEIPT_TIMEOUT_EXPIRES;
interval3 = (unsigned long long)
(SYNC_RECEIPT_TIMEOUT_MULTIPLIER*
pow((double)2,getSyncInterval())*1000000000.0);
interval4 = (unsigned long long)
(ANNOUNCE_RECEIPT_TIMEOUT_MULTIPLIER*
pow((double)2,getAnnounceInterval())*1000000000.0);
}
port_ready_condition->wait_prelock();
link_thread = thread_factory->createThread();
if(!link_thread->start(watchNetLinkWrapper, (void *)this))
{
GPTP_LOG_ERROR("Error creating port link thread");
return;
}
listening_thread = thread_factory->createThread();
if (!listening_thread->
start (openPortWrapper, (void *)this))
{
GPTP_LOG_ERROR("Error creating port thread");
return;
}
port_ready_condition->wait();
if (e3 != NULL_EVENT)
clock->addEventTimerLocked(this, e3, interval3);
if (e4 != NULL_EVENT)
clock->addEventTimerLocked(this, e4, interval4);
}
if (automotive_profile) {
setStationState(STATION_STATE_ETHERNET_READY);
if (testMode) {
APMessageTestStatus *testStatusMsg = new APMessageTestStatus(this);
if (testStatusMsg) {
testStatusMsg->sendPort(this);
delete testStatusMsg;
}
}
if (!isGM) {
// Send an initial signalling message
PTPMessageSignalling *sigMsg = new PTPMessageSignalling(this);
if (sigMsg) {
sigMsg->setintervals(PTPMessageSignalling::sigMsgInterval_NoSend, log_mean_sync_interval, PTPMessageSignalling::sigMsgInterval_NoSend);
sigMsg->sendPort(this, NULL);
delete sigMsg;
}
startSyncReceiptTimer((unsigned long long)
(SYNC_RECEIPT_TIMEOUT_MULTIPLIER *
((double) pow((double)2, getSyncInterval()) *
1000000000.0)));
}
}
break;
case STATE_CHANGE_EVENT:
if (!automotive_profile) { // BMCA is not active with Automotive Profile
if ( clock->getPriority1() != 255 ) {
int number_ports, j;
PTPMessageAnnounce *EBest = NULL;
char EBestClockIdentity[PTP_CLOCK_IDENTITY_LENGTH];
IEEE1588Port **ports;
clock->getPortList(number_ports, ports);
/* Find EBest for all ports */
j = 0;
for (int i = 0; i < number_ports; ++i) {
while (ports[j] == NULL)
++j;
if (ports[j]->port_state == PTP_DISABLED
|| ports[j]->port_state == PTP_FAULTY) {
continue;
}
if (EBest == NULL) {
EBest = ports[j]->calculateERBest();
} else if (ports[j]->calculateERBest()) {
if (ports[j]->calculateERBest()->isBetterThan(EBest)) {
EBest = ports[j]->calculateERBest();
}
}
}
if (EBest == NULL) {
break;
}
/* Check if we've changed */
{
uint8_t LastEBestClockIdentity[PTP_CLOCK_IDENTITY_LENGTH];
clock->getLastEBestIdentity().
getIdentityString( LastEBestClockIdentity );
EBest->getGrandmasterIdentity( EBestClockIdentity );
if( memcmp( EBestClockIdentity, LastEBestClockIdentity,
PTP_CLOCK_IDENTITY_LENGTH ) != 0 )
{
ClockIdentity newGM;
changed_external_master = true;
newGM.set((uint8_t *) EBestClockIdentity );
clock->setLastEBestIdentity( newGM );
} else {
changed_external_master = false;
}
}
if( clock->isBetterThan( EBest )) {
// We're Grandmaster, set grandmaster info to me
ClockIdentity clock_identity;
unsigned char priority1;
unsigned char priority2;
ClockQuality clock_quality;
clock_identity = getClock()->getClockIdentity();
getClock()->setGrandmasterClockIdentity( clock_identity );
priority1 = getClock()->getPriority1();
getClock()->setGrandmasterPriority1( priority1 );
priority2 = getClock()->getPriority2();
getClock()->setGrandmasterPriority2( priority2 );
clock_quality = getClock()->getClockQuality();
getClock()->setGrandmasterClockQuality( clock_quality );
}
j = 0;
for (int i = 0; i < number_ports; ++i) {
while (ports[j] == NULL)
++j;
if (ports[j]->port_state == PTP_DISABLED
|| ports[j]->port_state == PTP_FAULTY) {
continue;
}
if (clock->isBetterThan(EBest)) {
// We are the GrandMaster, all ports are master
EBest = NULL; // EBest == NULL : we were grandmaster
ports[j]->recommendState(PTP_MASTER,
changed_external_master);
} else {
if( EBest == ports[j]->calculateERBest() ) {
// The "best" Announce was recieved on this port
ClockIdentity clock_identity;
unsigned char priority1;
unsigned char priority2;
ClockQuality *clock_quality;
ports[j]->recommendState
( PTP_SLAVE, changed_external_master );
clock_identity = EBest->getGrandmasterClockIdentity();
getClock()->setGrandmasterClockIdentity(clock_identity);
priority1 = EBest->getGrandmasterPriority1();
getClock()->setGrandmasterPriority1( priority1 );
priority2 = EBest->getGrandmasterPriority2();
getClock()->setGrandmasterPriority2( priority2 );
clock_quality = EBest->getGrandmasterClockQuality();
getClock()->setGrandmasterClockQuality(*clock_quality);
} else {
/* Otherwise we are the master because we have
sync'd to a better clock */
ports[j]->recommendState
(PTP_MASTER, changed_external_master);
}
}
}
}
}
break;
case LINKUP:
haltPdelay(false);
startPDelay();
if (automotive_profile) {
GPTP_LOG_EXCEPTION("LINKUP");
}
else {
GPTP_LOG_STATUS("LINKUP");
}
if (automotive_profile) {
asCapable = true;
setStationState(STATION_STATE_ETHERNET_READY);
if (testMode) {
APMessageTestStatus *testStatusMsg = new APMessageTestStatus(this);
if (testStatusMsg) {
testStatusMsg->sendPort(this);
delete testStatusMsg;
}
}
log_mean_sync_interval = initialLogSyncInterval;
log_mean_announce_interval = 0;
log_min_mean_pdelay_req_interval = initialLogPdelayReqInterval;
if (!isGM) {
// Send an initial signaling message
PTPMessageSignalling *sigMsg = new PTPMessageSignalling(this);
if (sigMsg) {
sigMsg->setintervals(PTPMessageSignalling::sigMsgInterval_NoSend, log_mean_sync_interval, PTPMessageSignalling::sigMsgInterval_NoSend);
sigMsg->sendPort(this, NULL);
delete sigMsg;
}
startSyncReceiptTimer((unsigned long long)
(SYNC_RECEIPT_TIMEOUT_MULTIPLIER *
((double) pow((double)2, getSyncInterval()) *
1000000000.0)));
}
// Reset Sync count and pdelay count
setPdelayCount(0);
setSyncCount(0);
// Start AVB SYNC at 2. It will decrement after each sync. When it reaches 0 the Test Status message
// can be sent
if (isGM) {
avbSyncState = 1;
}
else {
avbSyncState = 2;
}
if (testMode) {
linkUpCount++;
}
}
this->timestamper_reset();
break;
case LINKDOWN:
stopPDelay();
if (automotive_profile) {
GPTP_LOG_EXCEPTION("LINK DOWN");
}
else {
setAsCapable(false);
GPTP_LOG_STATUS("LINK DOWN");
}
if (testMode) {
linkDownCount++;
}
break;
case ANNOUNCE_RECEIPT_TIMEOUT_EXPIRES:
case SYNC_RECEIPT_TIMEOUT_EXPIRES:
{
if (e == ANNOUNCE_RECEIPT_TIMEOUT_EXPIRES) {
incCounter_ieee8021AsPortStatAnnounceReceiptTimeouts();
}
else if (e == SYNC_RECEIPT_TIMEOUT_EXPIRES) {
incCounter_ieee8021AsPortStatRxSyncReceiptTimeouts();
}
if (!automotive_profile) {
if( clock->getPriority1() == 255 ) {
// Restart timer
if( e == ANNOUNCE_RECEIPT_TIMEOUT_EXPIRES ) {
clock->addEventTimerLocked
(this, ANNOUNCE_RECEIPT_TIMEOUT_EXPIRES,
(ANNOUNCE_RECEIPT_TIMEOUT_MULTIPLIER*
(unsigned long long)
(pow((double)2,getAnnounceInterval())*
1000000000.0)));
} else {
startSyncReceiptTimer((unsigned long long)
(SYNC_RECEIPT_TIMEOUT_MULTIPLIER *
((double) pow((double)2, getSyncInterval()) *
1000000000.0)));
}
}
if (port_state == PTP_INITIALIZING
|| port_state == PTP_UNCALIBRATED
|| port_state == PTP_SLAVE
|| port_state == PTP_PRE_MASTER) {
GPTP_LOG_STATUS(
"*** %s Timeout Expired - Becoming Master",
e == ANNOUNCE_RECEIPT_TIMEOUT_EXPIRES ? "Announce" :
"Sync" );
{
// We're Grandmaster, set grandmaster info to me
ClockIdentity clock_identity;
unsigned char priority1;
unsigned char priority2;
ClockQuality clock_quality;
clock_identity = getClock()->getClockIdentity();
getClock()->setGrandmasterClockIdentity( clock_identity );
priority1 = getClock()->getPriority1();
getClock()->setGrandmasterPriority1( priority1 );
priority2 = getClock()->getPriority2();
getClock()->setGrandmasterPriority2( priority2 );
clock_quality = getClock()->getClockQuality();
getClock()->setGrandmasterClockQuality( clock_quality );
}
port_state = PTP_MASTER;
Timestamp system_time;
Timestamp device_time;
uint32_t local_clock, nominal_clock_rate;
getDeviceTime(system_time, device_time,
local_clock, nominal_clock_rate);
(void) clock->calcLocalSystemClockRateDifference
( device_time, system_time );
delete qualified_announce;
qualified_announce = NULL;
// Add timers for Announce and Sync, this is as close to immediately as we get
if( clock->getPriority1() != 255) {
clock->addEventTimerLocked
( this, SYNC_INTERVAL_TIMEOUT_EXPIRES, 16000000 );
}
startAnnounce();
}
}
else {
// Automotive Profile
if (e == SYNC_RECEIPT_TIMEOUT_EXPIRES) {
GPTP_LOG_EXCEPTION("SYNC receipt timeout");
startSyncReceiptTimer((unsigned long long)
(SYNC_RECEIPT_TIMEOUT_MULTIPLIER *
((double) pow((double)2, getSyncInterval()) *
1000000000.0)));
}
}
}
break;
case PDELAY_INTERVAL_TIMEOUT_EXPIRES:
GPTP_LOG_DEBUG("PDELAY_INTERVAL_TIMEOUT_EXPIRES occured");
{
int ts_good;
Timestamp req_timestamp;
int iter = TX_TIMEOUT_ITER;
long req = TX_TIMEOUT_BASE;
unsigned req_timestamp_counter_value;
long long wait_time = 0;
PTPMessagePathDelayReq *pdelay_req =
new PTPMessagePathDelayReq(this);
PortIdentity dest_id;
getPortIdentity(dest_id);
pdelay_req->setPortIdentity(&dest_id);
{
Timestamp pending =
PDELAY_PENDING_TIMESTAMP;
pdelay_req->setTimestamp(pending);
}
if (last_pdelay_req != NULL) {
delete last_pdelay_req;
}
setLastPDelayReq(pdelay_req);
getTxLock();
pdelay_req->sendPort(this, NULL);
GPTP_LOG_DEBUG("*** Sent PDelay Request message");
OSTimer *timer = timer_factory->createTimer();
ts_good = getTxTimestamp
(pdelay_req, req_timestamp, req_timestamp_counter_value, false);
while (ts_good != GPTP_EC_SUCCESS && iter-- != 0) {
timer->sleep(req);
wait_time += req;
if (ts_good != GPTP_EC_EAGAIN && iter < 1)
GPTP_LOG_ERROR(
"Error (TX) timestamping PDelay request "
"(Retrying-%d), error=%d", iter, ts_good);
ts_good =
getTxTimestamp
(pdelay_req, req_timestamp,
req_timestamp_counter_value, iter == 0);
req *= 2;
}
delete timer;
putTxLock();
if (ts_good == GPTP_EC_SUCCESS) {
pdelay_req->setTimestamp(req_timestamp);
GPTP_LOG_DEBUG(
"PDelay Request message, Timestamp %u (sec) %u (ns), seqID %u",
req_timestamp.seconds_ls, req_timestamp.nanoseconds,
pdelay_req->getSequenceId());
} else {
Timestamp failed = INVALID_TIMESTAMP;
pdelay_req->setTimestamp(failed);
GPTP_LOG_ERROR( "Invalid TX" );
}
if (ts_good != GPTP_EC_SUCCESS) {
char msg
[HWTIMESTAMPER_EXTENDED_MESSAGE_SIZE];
getExtendedError(msg);
GPTP_LOG_ERROR(
"Error (TX) timestamping PDelay request, error=%d\t%s",
ts_good, msg);
}
{
long long timeout;
long long interval;
timeout = PDELAY_RESP_RECEIPT_TIMEOUT_MULTIPLIER *
((long long)
(pow((double)2,getPDelayInterval())*1000000000.0)) -
wait_time*1000;
timeout = timeout > EVENT_TIMER_GRANULARITY ?
timeout : EVENT_TIMER_GRANULARITY;
clock->addEventTimerLocked
(this, PDELAY_RESP_RECEIPT_TIMEOUT_EXPIRES, timeout );
GPTP_LOG_DEBUG("Schedule PDELAY_RESP_RECEIPT_TIMEOUT_EXPIRES, "
"PDelay interval %d, wait_time %lld, timeout %lld",
getPDelayInterval(), wait_time, timeout);
interval =
((long long)
(pow((double)2,getPDelayInterval())*1000000000.0)) -
wait_time*1000;
interval = interval > EVENT_TIMER_GRANULARITY ?
interval : EVENT_TIMER_GRANULARITY;
startPDelayIntervalTimer(interval);
}
}
break;
case SYNC_INTERVAL_TIMEOUT_EXPIRES:
GPTP_LOG_DEBUG("SYNC_INTERVAL_TIMEOUT_EXPIRES occured");
{
/* Set offset from master to zero, update device vs
system time offset */
Timestamp system_time;
Timestamp device_time;
FrequencyRatio local_system_freq_offset;
int64_t local_system_offset;
long long wait_time = 0;
uint32_t local_clock, nominal_clock_rate;
// Send a sync message and then a followup to broadcast
if (asCapable) {
PTPMessageSync *sync = new PTPMessageSync(this);
PortIdentity dest_id;
getPortIdentity(dest_id);
sync->setPortIdentity(&dest_id);
getTxLock();
sync->sendPort(this, NULL);
GPTP_LOG_DEBUG("Sent SYNC message");
if (automotive_profile && port_state == PTP_MASTER) {
if (avbSyncState > 0) {
avbSyncState--;
if (avbSyncState == 0) {
// Send Avnu Automotive Profile status message
setStationState(STATION_STATE_AVB_SYNC);
if (testMode) {
APMessageTestStatus *testStatusMsg = new APMessageTestStatus(this);
if (testStatusMsg) {
testStatusMsg->sendPort(this);
delete testStatusMsg;
}
}
}
}
}
int ts_good;
Timestamp sync_timestamp;
unsigned sync_timestamp_counter_value;
int iter = TX_TIMEOUT_ITER;
long req = TX_TIMEOUT_BASE;
OSTimer *timer = timer_factory->createTimer();
ts_good =
getTxTimestamp(sync, sync_timestamp,
sync_timestamp_counter_value,
false);
while (ts_good != GPTP_EC_SUCCESS && iter-- != 0) {
timer->sleep(req);
wait_time += req;
if (ts_good != GPTP_EC_EAGAIN && iter < 1)
GPTP_LOG_ERROR(
"Error (TX) timestamping Sync (Retrying), "
"error=%d", ts_good);
ts_good =
getTxTimestamp
(sync, sync_timestamp,
sync_timestamp_counter_value, iter == 0);
req *= 2;
}
delete timer;
putTxLock();
if (ts_good != GPTP_EC_SUCCESS) {
char msg [HWTIMESTAMPER_EXTENDED_MESSAGE_SIZE];
getExtendedError(msg);
GPTP_LOG_ERROR(
"Error (TX) timestamping Sync, error="
"%d\n%s",
ts_good, msg );
}
if (ts_good == GPTP_EC_SUCCESS) {
GPTP_LOG_VERBOSE("Successful Sync timestamp");
GPTP_LOG_VERBOSE("Seconds: %u",
sync_timestamp.seconds_ls);
GPTP_LOG_VERBOSE("Nanoseconds: %u",
sync_timestamp.nanoseconds);
} else {
GPTP_LOG_ERROR
("*** Unsuccessful Sync timestamp");
}
PTPMessageFollowUp *follow_up;
if (ts_good == GPTP_EC_SUCCESS) {
follow_up =
new PTPMessageFollowUp(this);
PortIdentity dest_id;
getPortIdentity(dest_id);
follow_up->setClockSourceTime(getClock()->getFUPInfo());
follow_up->setPortIdentity(&dest_id);
follow_up->setSequenceId(sync->getSequenceId());
follow_up->setPreciseOriginTimestamp(sync_timestamp);
follow_up->sendPort(this, NULL);
delete follow_up;
} else {
}
delete sync;
}
/* Do getDeviceTime() after transmitting sync frame
causing an update to local/system timestamp */
getDeviceTime
(system_time, device_time, local_clock, nominal_clock_rate);
GPTP_LOG_VERBOSE
("port::processEvent(): System time: %u,%u Device Time: %u,%u",
system_time.seconds_ls, system_time.nanoseconds,
device_time.seconds_ls, device_time.nanoseconds);
local_system_offset =
TIMESTAMP_TO_NS(system_time) -
TIMESTAMP_TO_NS(device_time);
local_system_freq_offset =
clock->calcLocalSystemClockRateDifference
( device_time, system_time );
clock->setMasterOffset
(this, 0, device_time, 1.0, local_system_offset,
system_time, local_system_freq_offset, sync_count,
pdelay_count, port_state, asCapable );
syncDone();
wait_time = ((long long) (pow((double)2, getSyncInterval()) * 1000000000.0));
wait_time = wait_time > EVENT_TIMER_GRANULARITY ? wait_time : EVENT_TIMER_GRANULARITY;
startSyncIntervalTimer(wait_time);
}
break;
case ANNOUNCE_INTERVAL_TIMEOUT_EXPIRES:
GPTP_LOG_DEBUG("ANNOUNCE_INTERVAL_TIMEOUT_EXPIRES occured");
if (asCapable) {
// Send an announce message
PTPMessageAnnounce *annc = new PTPMessageAnnounce(this);
PortIdentity dest_id;
PortIdentity gmId;
ClockIdentity clock_id = clock->getClockIdentity();
gmId.setClockIdentity(clock_id);
getPortIdentity(dest_id);
annc->setPortIdentity(&dest_id);
annc->sendPort(this, NULL);
delete annc;
}
startAnnounceIntervalTimer((uint64_t)(pow((double)2, getAnnounceInterval()) * 1000000000.0));
break;
case FAULT_DETECTED:
GPTP_LOG_ERROR("Received FAULT_DETECTED event");
if (!automotive_profile) {
setAsCapable(false);
}
break;
case PDELAY_DEFERRED_PROCESSING:
GPTP_LOG_DEBUG("PDELAY_DEFERRED_PROCESSING occured");
pdelay_rx_lock->lock();
if (last_pdelay_resp_fwup == NULL) {
GPTP_LOG_ERROR("PDelay Response Followup is NULL!");
abort();
}
last_pdelay_resp_fwup->processMessage(this);
if (last_pdelay_resp_fwup->garbage()) {
delete last_pdelay_resp_fwup;
this->setLastPDelayRespFollowUp(NULL);
}
pdelay_rx_lock->unlock();
break;
case PDELAY_RESP_RECEIPT_TIMEOUT_EXPIRES:
if (!automotive_profile) {
GPTP_LOG_EXCEPTION("PDelay Response Receipt Timeout");
setAsCapable(false);
}
pdelay_count = 0;
break;
case PDELAY_RESP_PEER_MISBEHAVING_TIMEOUT_EXPIRES:
GPTP_LOG_EXCEPTION("PDelay Resp Peer Misbehaving timeout expired! Restarting PDelay");
haltPdelay(false);
if( port_state != PTP_SLAVE && port_state != PTP_MASTER ) {
GPTP_LOG_STATUS("Starting PDelay" );
startPDelay();
}
break;
case SYNC_RATE_INTERVAL_TIMEOUT_EXPIRED:
{
GPTP_LOG_INFO("SYNC_RATE_INTERVAL_TIMEOUT_EXPIRED occured");
sync_rate_interval_timer_started = false;
bool sendSignalMessage = false;
if (log_mean_sync_interval != operLogSyncInterval) {
log_mean_sync_interval = operLogSyncInterval;
sendSignalMessage = true;
}
if (log_min_mean_pdelay_req_interval != operLogPdelayReqInterval) {
log_min_mean_pdelay_req_interval = operLogPdelayReqInterval;
sendSignalMessage = true;
}
if (sendSignalMessage) {
if (!isGM) {
// Send operational signalling message
PTPMessageSignalling *sigMsg = new PTPMessageSignalling(this);
if (sigMsg) {
if (automotive_profile)
sigMsg->setintervals(PTPMessageSignalling::sigMsgInterval_NoChange, log_mean_sync_interval, PTPMessageSignalling::sigMsgInterval_NoChange);
else
sigMsg->setintervals(log_min_mean_pdelay_req_interval, log_mean_sync_interval, PTPMessageSignalling::sigMsgInterval_NoChange);
sigMsg->sendPort(this, NULL);
delete sigMsg;
}
startSyncReceiptTimer((unsigned long long)
(SYNC_RECEIPT_TIMEOUT_MULTIPLIER *
((double) pow((double)2, getSyncInterval()) *
1000000000.0)));
}
}
}
break;
default:
GPTP_LOG_ERROR
("Unhandled event type in IEEE1588Port::processEvent(), %d",
e);
break;
}
return;
}
void IEEE1588Port::processEvent(Event e)
{
bool changed_external_master;
OSTimer *timer = timer_factory->createTimer();
switch (e) {
case POWERUP:
case INITIALIZE:
XPTPD_INFO("Received POWERUP/INITIALIZE event");
clock->getTimerQLock();
{
unsigned long long interval3;
unsigned long long interval4;
Event e3 = NULL_EVENT;
Event e4 = NULL_EVENT;
if( port_state != PTP_MASTER ) {
_accelerated_sync_count = -1;
}
if( port_state != PTP_SLAVE && port_state != PTP_MASTER ) {
fprintf( stderr, "Starting PDelay\n" );
startPDelay();
}
if( clock->getPriority1() == 255 || port_state == PTP_SLAVE ) {
becomeSlave( false );
} else if( port_state == PTP_MASTER ) {
becomeMaster( true );
} else {
//e3 = SYNC_RECEIPT_TIMEOUT_EXPIRES;
e4 = ANNOUNCE_RECEIPT_TIMEOUT_EXPIRES;
interval3 = (unsigned long long)
(SYNC_RECEIPT_TIMEOUT_MULTIPLIER*
pow((double)2,getSyncInterval())*1000000000.0);
interval4 = (unsigned long long)
(ANNOUNCE_RECEIPT_TIMEOUT_MULTIPLIER*
pow((double)2,getAnnounceInterval())*1000000000.0);
}
port_ready_condition->wait_prelock();
listening_thread = thread_factory->createThread();
if (!listening_thread->
start (openPortWrapper, (void *)this))
{
XPTPD_ERROR("Error creating port thread");
return;
}
port_ready_condition->wait();
if (e3 != NULL_EVENT)
clock->addEventTimer(this, e3, interval3);
if (e4 != NULL_EVENT)
clock->addEventTimer(this, e4, interval4);
}
clock->putTimerQLock();
break;
case STATE_CHANGE_EVENT:
if ( clock->getPriority1() != 255 ) {
int number_ports, j;
PTPMessageAnnounce *EBest = NULL;
char EBestClockIdentity[PTP_CLOCK_IDENTITY_LENGTH];
IEEE1588Port **ports;
clock->getPortList(number_ports, ports);
/* Find EBest for all ports */
j = 0;
for (int i = 0; i < number_ports; ++i) {
while (ports[j] == NULL)
++j;
if (ports[j]->port_state == PTP_DISABLED
|| ports[j]->port_state == PTP_FAULTY) {
continue;
}
if (EBest == NULL) {
EBest = ports[j]->calculateERBest();
} else {
if (ports[j]->calculateERBest()->isBetterThan(EBest)) {
EBest = ports[j]->calculateERBest();
}
}
}
/* Check if we've changed */
{
uint8_t LastEBestClockIdentity[PTP_CLOCK_IDENTITY_LENGTH];
clock->getLastEBestIdentity().
getIdentityString( LastEBestClockIdentity );
EBest->getGrandmasterIdentity( EBestClockIdentity );
if( memcmp
( EBestClockIdentity, LastEBestClockIdentity,
PTP_CLOCK_IDENTITY_LENGTH ) != 0 )
{
ClockIdentity newGM;
changed_external_master = true;
newGM.set((uint8_t *) EBestClockIdentity );
clock->setLastEBestIdentity( newGM );
} else {
changed_external_master = false;
}
}
if( clock->isBetterThan( EBest )) {
// We're Grandmaster, set grandmaster info to me
ClockIdentity clock_identity;
unsigned char priority1;
unsigned char priority2;
ClockQuality clock_quality;
clock_identity = getClock()->getClockIdentity();
getClock()->setGrandmasterClockIdentity( clock_identity );
priority1 = getClock()->getPriority1();
getClock()->setGrandmasterPriority1( priority1 );
priority2 = getClock()->getPriority2();
getClock()->setGrandmasterPriority2( priority2 );
clock_quality = getClock()->getClockQuality();
getClock()->setGrandmasterClockQuality( clock_quality );
}
j = 0;
for (int i = 0; i < number_ports; ++i) {
while (ports[j] == NULL)
++j;
if (ports[j]->port_state == PTP_DISABLED
|| ports[j]->port_state == PTP_FAULTY) {
continue;
}
if (clock->isBetterThan(EBest)) {
// We are the GrandMaster, all ports are master
EBest = NULL; // EBest == NULL : we were grandmaster
ports[j]->recommendState(PTP_MASTER,
changed_external_master);
} else {
if( EBest == ports[j]->calculateERBest() ) {
// The "best" Announce was recieved on this port
ClockIdentity clock_identity;
unsigned char priority1;
unsigned char priority2;
ClockQuality *clock_quality;
ports[j]->recommendState
( PTP_SLAVE, changed_external_master );
clock_identity = EBest->getGrandmasterClockIdentity();
getClock()->setGrandmasterClockIdentity(clock_identity);
priority1 = EBest->getGrandmasterPriority1();
getClock()->setGrandmasterPriority1( priority1 );
priority2 = EBest->getGrandmasterPriority2();
getClock()->setGrandmasterPriority2( priority2 );
clock_quality = EBest->getGrandmasterClockQuality();
getClock()->setGrandmasterClockQuality(*clock_quality);
} else {
/* Otherwise we are the master because we have
sync'd to a better clock */
ports[j]->recommendState
(PTP_MASTER, changed_external_master);
}
}
}
}
break;
case ANNOUNCE_RECEIPT_TIMEOUT_EXPIRES:
case SYNC_RECEIPT_TIMEOUT_EXPIRES:
{
if( clock->getPriority1() == 255 ) {
// Restart timer
if( e == ANNOUNCE_RECEIPT_TIMEOUT_EXPIRES ) {
clock->addEventTimer
(this, ANNOUNCE_RECEIPT_TIMEOUT_EXPIRES,
(ANNOUNCE_RECEIPT_TIMEOUT_MULTIPLIER*
(unsigned long long)
(pow((double)2,getAnnounceInterval())*
1000000000.0)));
} else {
clock->addEventTimer
(this, SYNC_RECEIPT_TIMEOUT_EXPIRES,
(SYNC_RECEIPT_TIMEOUT_MULTIPLIER*
(unsigned long long)
(pow((double)2,getSyncInterval())*
1000000000.0)));
}
return;
}
if (port_state == PTP_INITIALIZING
|| port_state == PTP_UNCALIBRATED
|| port_state == PTP_SLAVE
|| port_state == PTP_PRE_MASTER) {
Timestamp tsTmp(0,0,0);
if(NULL!=p_dbg_HoldLastFollowup) {
tsTmp = p_dbg_HoldLastFollowup->getPreciseOriginTimestamp();
}
fprintf
(stderr,
"*** %s Timeout Expired - Becoming Master LastFollowUpPOT=%16llu\n",
e == ANNOUNCE_RECEIPT_TIMEOUT_EXPIRES ? "Announce" :
"Sync", tsTmp );
{
// We're Grandmaster, set grandmaster info to me
ClockIdentity clock_identity;
unsigned char priority1;
unsigned char priority2;
ClockQuality clock_quality;
clock_identity = getClock()->getClockIdentity();
getClock()->setGrandmasterClockIdentity( clock_identity );
priority1 = getClock()->getPriority1();
getClock()->setGrandmasterPriority1( priority1 );
priority2 = getClock()->getPriority2();
getClock()->setGrandmasterPriority2( priority2 );
clock_quality = getClock()->getClockQuality();
getClock()->setGrandmasterClockQuality( clock_quality );
}
port_state = PTP_MASTER;
Timestamp system_time;
Timestamp device_time;
uint32_t local_clock, nominal_clock_rate;
getDeviceTime(system_time, device_time,
local_clock, nominal_clock_rate);
(void) clock->calcLocalSystemClockRateDifference
( device_time, system_time );
delete qualified_announce;
qualified_announce = NULL;
// Add timers for Announce and Sync, this is as close to immediately as we get
clock->addEventTimer
( this, SYNC_INTERVAL_TIMEOUT_EXPIRES, 16000000 );
startAnnounce();
//
}
}
break;
case PDELAY_INTERVAL_TIMEOUT_EXPIRES:
XPTPD_INFO("PDELAY_INTERVAL_TIMEOUT_EXPIRES occured");
{
int ts_good;
Timestamp req_timestamp;
int iter = TX_TIMEOUT_ITER;
long req = TX_TIMEOUT_BASE;
unsigned req_timestamp_counter_value;
long long wait_time = 0;
PTPMessagePathDelayReq *pdelay_req =
new PTPMessagePathDelayReq(this);
PortIdentity dest_id;
getPortIdentity(dest_id);
pdelay_req->setPortIdentity(&dest_id);
{
Timestamp pending =
PDELAY_PENDING_TIMESTAMP;
pdelay_req->setTimestamp(pending);
}
if (last_pdelay_req != NULL) {
delete last_pdelay_req;
}
setLastPDelayReq(pdelay_req);
getTxLock();
pdelay_req->sendPort(this, NULL);
XPTPD_INFO("Sent PDelay Request");
ts_good =
getTxTimestamp
(pdelay_req, req_timestamp, req_timestamp_counter_value,
false);
while (ts_good != 0 && iter-- != 0) {
timer->sleep(req);
wait_time += req;
if (ts_good != -72 && iter < 1)
fprintf
(stderr,
"Error (TX) timestamping PDelay request "
"(Retrying-%d), error=%d\n", iter, ts_good);
ts_good =
getTxTimestamp
(pdelay_req, req_timestamp,
req_timestamp_counter_value, iter == 0);
req *= 2;
}
putTxLock();
if (ts_good == 0) {
pdelay_req->setTimestamp(req_timestamp);
} else {
Timestamp failed = INVALID_TIMESTAMP;
pdelay_req->setTimestamp(failed);
fprintf( stderr, "Invalid TX\n" );
}
if (ts_good != 0) {
char msg
[HWTIMESTAMPER_EXTENDED_MESSAGE_SIZE];
getExtendedError(msg);
XPTPD_ERROR(
"Error (TX) timestamping PDelay request, error=%d\t%s",
ts_good, msg);
}
#ifdef DEBUG
if (ts_good == 0) {
XPTPD_INFO
("Successful PDelay Req timestamp, %u,%u",
req_timestamp.seconds_ls,
req_timestamp.nanoseconds);
} else {
XPTPD_INFO
("*** Unsuccessful PDelay Req timestamp");
}
#endif
{
long long timeout;
long long interval;
timeout = PDELAY_RESP_RECEIPT_TIMEOUT_MULTIPLIER *
((long long)
(pow((double)2,getPDelayInterval())*1000000000.0)) -
wait_time*1000;
timeout = timeout > EVENT_TIMER_GRANULARITY ?
timeout : EVENT_TIMER_GRANULARITY;
clock->addEventTimer
(this, PDELAY_RESP_RECEIPT_TIMEOUT_EXPIRES, timeout );
interval =
((long long)
(pow((double)2,getPDelayInterval())*1000000000.0)) -
wait_time*1000;
interval = interval > EVENT_TIMER_GRANULARITY ?
interval : EVENT_TIMER_GRANULARITY;
clock->addEventTimer
(this, PDELAY_INTERVAL_TIMEOUT_EXPIRES, interval );
}
}
break;
case SYNC_INTERVAL_TIMEOUT_EXPIRES:
XPTPD_INFO("SYNC_INTERVAL_TIMEOUT_EXPIRES occured");
fprintf(stderr, "STO\n");
{
#ifdef OLD_GPTP
/* Set offset from master to zero, update device vs
system time offset */
Timestamp system_time;
Timestamp device_time;
FrequencyRatio local_system_freq_offset;
int64_t local_system_offset;
long long wait_time = 0;
uint32_t local_clock, nominal_clock_rate;
// Send a sync message and then a followup to broadcast
if (asCapable) {
PTPMessageSync *sync = new PTPMessageSync(this);
PortIdentity dest_id;
getPortIdentity(dest_id);
sync->setPortIdentity(&dest_id);
getTxLock();
sync->sendPort(this, NULL);
XPTPD_INFO("Sent SYNC message");
int ts_good;
Timestamp sync_timestamp;
unsigned sync_timestamp_counter_value;
int iter = TX_TIMEOUT_ITER;
long req = TX_TIMEOUT_BASE;
ts_good =
getTxTimestamp(sync, sync_timestamp,
sync_timestamp_counter_value,
false);
while (ts_good != 0 && iter-- != 0) {
timer->sleep(req);
wait_time += req;
if (ts_good != -72 && iter < 1)
XPTPD_ERROR(
"Error (TX) timestamping Sync (Retrying), "
"error=%d", ts_good);
ts_good =
getTxTimestamp
(sync, sync_timestamp,
sync_timestamp_counter_value, iter == 0);
req *= 2;
}
putTxLock();
if (ts_good != 0) {
char msg
[HWTIMESTAMPER_EXTENDED_MESSAGE_SIZE];
getExtendedError(msg);
fprintf
(stderr,
"Error (TX) timestamping Sync, error="
"%d\n%s",
ts_good, msg );
}
if (ts_good == 0) {
XPTPD_INFO("Successful Sync timestamp");
XPTPD_INFO("Seconds: %u",
sync_timestamp.seconds_ls);
XPTPD_INFO("Nanoseconds: %u",
sync_timestamp.nanoseconds);
} else {
XPTPD_INFO
("*** Unsuccessful Sync timestamp");
}
PTPMessageFollowUp *follow_up;
if (ts_good == 0) {
follow_up =
new PTPMessageFollowUp(this);
PortIdentity dest_id;
getPortIdentity(dest_id);
follow_up->setPortIdentity(&dest_id);
follow_up->setSequenceId(sync->getSequenceId());
follow_up->setPreciseOriginTimestamp(sync_timestamp);
follow_up->sendPort(this, NULL);
delete follow_up;
} else {
}
delete sync;
}
/* Do getDeviceTime() after transmitting sync frame
causing an update to local/system timestamp */
getDeviceTime
(system_time, device_time, local_clock, nominal_clock_rate);
XPTPD_INFO
("port::processEvent(): System time: %u,%u Device Time: %u,%u",
system_time.seconds_ls, system_time.nanoseconds,
device_time.seconds_ls, device_time.nanoseconds);
local_system_offset =
TIMESTAMP_TO_NS(system_time) -
TIMESTAMP_TO_NS(device_time);
local_system_freq_offset =
clock->calcLocalSystemClockRateDifference
( device_time, system_time );
clock->setMasterOffset
(0, device_time, 1.0, local_system_offset,
system_time, local_system_freq_offset, sync_count,
pdelay_count, port_state , 0/*DEBUG*/, system_time, system_time, 0, 0, 0 );
/* If accelerated_sync is non-zero then start 16 ms sync
timer, subtract 1, for last one start PDelay also */
if( _accelerated_sync_count > 0 ) {
clock->addEventTimer
( this, SYNC_INTERVAL_TIMEOUT_EXPIRES, 8000000 );
--_accelerated_sync_count;
} else {
syncDone();
if( _accelerated_sync_count == 0 ) {
--_accelerated_sync_count;
}
wait_time *= 1000; // to ns
wait_time =
((long long)
(pow((double)2,getSyncInterval())*1000000000.0)) -
wait_time;
wait_time = wait_time > EVENT_TIMER_GRANULARITY ? wait_time :
EVENT_TIMER_GRANULARITY;
clock->addEventTimer
( this, SYNC_INTERVAL_TIMEOUT_EXPIRES, wait_time );
}
#else
/* Set offset from master to zero, update device vs
system time offset */
Timestamp system_time;
Timestamp device_time;
Timestamp sync_timestamp;
long long wait_time = 0;
uint32_t local_clock, nominal_clock_rate;
struct masterToLocal master_to_local;
struct localToSystem local_to_system;
// Send a sync message and then a followup to broadcast
if (asCapable) {
PTPMessageSync *sync = new PTPMessageSync(this);
PortIdentity dest_id;
getPortIdentity(dest_id);
sync->setPortIdentity(&dest_id);
getTxLock();
sync->sendPort(this, NULL);
XPTPD_INFO("Sent SYNC message");
int ts_good;
unsigned sync_timestamp_counter_value;
int iter = TX_TIMEOUT_ITER;
long req = TX_TIMEOUT_BASE;
ts_good =
getTxTimestamp(sync, sync_timestamp,
sync_timestamp_counter_value,
false);
while (ts_good != 0 && iter-- != 0) {
timer->sleep(req);
wait_time += req;
if (ts_good != -72 && iter < 1)
XPTPD_ERROR(
"Error (TX) timestamping Sync (Retrying), "
"error=%d", ts_good);
ts_good =
getTxTimestamp
(sync, sync_timestamp,
sync_timestamp_counter_value, iter == 0);
req *= 2;
}
putTxLock();
if (ts_good != 0) {
char msg
[HWTIMESTAMPER_EXTENDED_MESSAGE_SIZE];
getExtendedError(msg);
fprintf
(stderr,
"Error (TX) timestamping Sync, error="
"%d\n%s",
ts_good, msg );
}
if (ts_good == 0) {
XPTPD_INFO("Successful Sync timestamp");
XPTPD_INFO("Seconds: %u",
sync_timestamp.seconds_ls);
XPTPD_INFO("Nanoseconds: %u",
sync_timestamp.nanoseconds);
} else {
XPTPD_INFO
("*** Unsuccessful Sync timestamp");
}
PTPMessageFollowUp *follow_up;
if (ts_good == 0) {
follow_up =
new PTPMessageFollowUp(this);
PortIdentity dest_id;
getPortIdentity(dest_id);
follow_up->setPortIdentity(&dest_id);
follow_up->setSequenceId(sync->getSequenceId());
follow_up->setPreciseOriginTimestamp(sync_timestamp);
follow_up->sendPort(this, NULL);
delete follow_up;
} else {
}
delete sync;
}
/* Do getDeviceTime() after transmitting sync frame
causing an update to local/system timestamp */
getDeviceTime
(system_time, device_time, local_clock, nominal_clock_rate);
XPTPD_INFO
("port::processEvent(): System time: %u,%u Device Time: %u,%u",
system_time.seconds_ls, system_time.nanoseconds,
device_time.seconds_ls, device_time.nanoseconds);
master_to_local.preciseOriginTimestamp = sync_timestamp;
master_to_local.sync_arrival = sync_timestamp;
master_to_local.freq_ratio = 1.0;
local_to_system.device_time = device_time;
local_to_system.system_time = system_time;
local_to_system.freq_ratio = clock->calcLocalSystemClockRateDifference( device_time, system_time );;
clock->setMasterOffset(master_to_local, local_to_system, sync_count,
pdelay_count, port_state );
/* If accelerated_sync is non-zero then start 16 ms sync
timer, subtract 1, for last one start PDelay also */
if( _accelerated_sync_count > 0 ) {
clock->addEventTimer( this, SYNC_INTERVAL_TIMEOUT_EXPIRES, 8000000 );
--_accelerated_sync_count;
} else {
syncDone();
if( _accelerated_sync_count == 0 ) {
--_accelerated_sync_count;
}
wait_time *= 1000; // to ns
wait_time =
((long long)
(pow((double)2,getSyncInterval())*1000000000.0)) -
wait_time;
wait_time = wait_time > EVENT_TIMER_GRANULARITY ? wait_time :
EVENT_TIMER_GRANULARITY;
clock->addEventTimer
( this, SYNC_INTERVAL_TIMEOUT_EXPIRES, wait_time );
}
#endif
}
break;
case ANNOUNCE_INTERVAL_TIMEOUT_EXPIRES:
if (asCapable) {
// Send an announce message
PTPMessageAnnounce *annc = new PTPMessageAnnounce(this);
PortIdentity dest_id;
PortIdentity gmId;
ClockIdentity clock_id = clock->getClockIdentity();
gmId.setClockIdentity(clock_id);
getPortIdentity(dest_id);
annc->setPortIdentity(&dest_id);
annc->sendPort(this, NULL);
delete annc;
}
clock->addEventTimer
(this, ANNOUNCE_INTERVAL_TIMEOUT_EXPIRES,
(unsigned)
(pow ((double)2, getAnnounceInterval()) * 1000000000.0));
break;
case FAULT_DETECTED:
XPTPD_INFO("Received FAULT_DETECTED event");
break;
case PDELAY_DEFERRED_PROCESSING:
pdelay_rx_lock->lock();
if (last_pdelay_resp_fwup == NULL) {
fprintf(stderr, "PDelay Response Followup is NULL!\n");
abort();
}
last_pdelay_resp_fwup->processMessage(this);
if (last_pdelay_resp_fwup->garbage()) {
delete last_pdelay_resp_fwup;
this->setLastPDelayRespFollowUp(NULL);
}
pdelay_rx_lock->unlock();
break;
case PDELAY_RESP_RECEIPT_TIMEOUT_EXPIRES:
setAsCapable(false);
pdelay_count = 0;
break;
default:
XPTPD_INFO
("Unhandled event type in IEEE1588Port::processEvent(), %d",
e);
break;
}
delete timer;
return;
}