UInteger8 v2bmc(ForeignMasterRecord *foreign,
RunTimeOpts *rtOpts,
PtpClock *ptpClock
)
{
Integer16 i, best;
/* Check if any foreign masters */
if(!ptpClock->number_foreign_records)
{
DBGV("v2bmc: number_foreign_records is zero, state: %u\n",
ptpClock->port_state
);
/* No foreign masters, run m1 if we are currently master */
if(ptpClock->port_state == PTP_MASTER)
{
DBGV("v2bmc: calling m1\n");
m1(ptpClock);
}
return ptpClock->port_state; /* no change */
}
DBGV("v2bmc: number_foreign_records is non zero, state: %u\n",
ptpClock->port_state
);
/* There is at least one foreign master. Scan through foreign master database and compare
* to look for best master to use
*/
for(i = 1, best = 0; i < ptpClock->number_foreign_records; ++i)
{
/* Check current loop indx record versus current "best" record */
if(v2bmcDataSetComparison(&foreign[i].v2_header,
&foreign[i].announce,
&foreign[best].v2_header,
&foreign[best].announce,
ptpClock,
ptpClock
) > 0
)
{
/* Current loop index is better than previous "best", set new "best" */
best = i;
}
DBGV("v2bmc: comparison loop i=%d, best=%d\n",i,best);
}
/* Best record found, store index to best foreign master */
DBGV("v2bmc: best record %d\n", best);
ptpClock->foreign_record_best = best;
/* Now that best is found, determine recommended state */
return v2bmcStateDecision(&foreign[best].v2_header,
&foreign[best].announce,
rtOpts,
ptpClock
);
}
static void issueDelayReqTimerExpired(PtpClock *ptpClock)
{
switch (ptpClock->portDS.delayMechanism)
{
case E2E:
if(ptpClock->portDS.portState != PTP_SLAVE)
{
break;
}
if (timerExpired(DELAYREQ_INTERVAL_TIMER, ptpClock->itimer))
{
timerStart(DELAYREQ_INTERVAL_TIMER, getRand(pow2ms(ptpClock->portDS.logMinDelayReqInterval + 1)), ptpClock->itimer);
DBGV("event DELAYREQ_INTERVAL_TIMEOUT_EXPIRES\n");
issueDelayReq(ptpClock);
}
break;
case P2P:
if (timerExpired(PDELAYREQ_INTERVAL_TIMER, ptpClock->itimer))
{
timerStart(PDELAYREQ_INTERVAL_TIMER, getRand(pow2ms(ptpClock->portDS.logMinPdelayReqInterval + 1)), ptpClock->itimer);
DBGV("event PDELAYREQ_INTERVAL_TIMEOUT_EXPIRES\n");
issuePDelayReq(ptpClock);
}
break;
default:
break;
}
}
/* Set the RTC to the desired time time */
static void setRtc(ClockDriver *self, TimeInternal *timeToSet)
{
#ifdef HAVE_LINUX_RTC_H
char* rtcDev;
struct tm* tmTime;
time_t seconds;
int rtcFd;
struct stat statBuf;
if(stat("/dev/misc/rtc", &statBuf) == 0) {
rtcDev="/dev/misc/rtc\0";
} else if(stat("/dev/rtc", &statBuf) == 0) {
rtcDev="/dev/rtc\0";
} else if(stat("/dev/rtc0", &statBuf) == 0) {
rtcDev="/dev/rtc0\0";
} else {
ERROR(THIS_COMPONENT"Could not set RTC time - no suitable rtc device found\n");
return;
}
if(!S_ISCHR(statBuf.st_mode)) {
ERROR(THIS_COMPONENT"Could not set RTC time - device %s is not a character device\n",
rtcDev);
return;
}
DBGV("Usable RTC device: %s\n",rtcDev);
if(isTimeZero(timeToSet)) {
getTime(self, timeToSet);
}
if((rtcFd = open(rtcDev, O_RDONLY)) < 0) {
PERROR(THIS_COMPONENT"Could not set RTC time: error opening %s", rtcDev);
return;
}
seconds = (time_t)timeToSet->seconds;
if(timeToSet->nanoseconds >= 500000) seconds++;
tmTime = gmtime(&seconds);
DBGV("Set RTC from %d seconds to y: %d m: %d d: %d \n",timeToSet->seconds,tmTime->tm_year,tmTime->tm_mon,tmTime->tm_mday);
if(ioctl(rtcFd, RTC_SET_TIME, tmTime) < 0) {
PERROR(THIS_COMPONENT"Could not set RTC time on %s - ioctl failed", rtcDev);
goto cleanup;
}
NOTIFY(THIS_COMPONENT"Succesfully set RTC time using %s\n", rtcDev);
cleanup:
close(rtcFd);
#endif /* HAVE_LINUX_RTC_H */
}
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]);
}
UInteger8
bmc(ForeignMasterRecord *foreignMaster,
RunTimeOpts *rtOpts, PtpClock *ptpClock)
{
Integer16 i,best;
DBGV("number_foreign_records : %d \n", ptpClock->number_foreign_records);
if (!ptpClock->number_foreign_records)
if (ptpClock->portState == PTP_MASTER) {
m1(rtOpts,ptpClock);
return ptpClock->portState;
}
for (i=1,best = 0; i<ptpClock->number_foreign_records;i++)
if ((bmcDataSetComparison(&foreignMaster[i].header,
&foreignMaster[i].announce,
&foreignMaster[best].header,
&foreignMaster[best].announce,
ptpClock)) < 0)
best = i;
DBGV("Best record : %d \n",best);
ptpClock->foreign_record_best = best;
return (bmcStateDecision(&foreignMaster[best].header,
&foreignMaster[best].announce,
rtOpts,ptpClock));
}
static void handleDelayReq(PtpClock *ptpClock, TimeInternal *time, Boolean isFromSelf)
{
switch (ptpClock->portDS.delayMechanism)
{
case E2E:
DBGV("handleDelayReq: received\n");
if (ptpClock->msgIbufLength < DELAY_REQ_LENGTH)
{
ERROR("handleDelayReq: 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("handleDelayReq: disreguard\n");
return;
case PTP_SLAVE:
DBGV("handleDelayReq: disreguard\n");
// if (isFromSelf)
// {
// /* waitingForLoopback? */
// /* Get sending timestamp from IP stack with So_TIMESTAMP*/
// ptpClock->delay_req_send_time = *time;
// /*Add latency*/
// addTime(&ptpClock->delay_req_send_time, &ptpClock->delay_req_send_time, &rtOpts->outboundLatency);
// break;
// }
break;
case PTP_MASTER:
/* TODO: manage the value of ptpClock->logMinDelayReqInterval form logSyncInterval to logSyncInterval + 5 */
issueDelayResp(ptpClock, time, &ptpClock->msgTmpHeader);
break;
default:
DBG("handleDelayReq: unrecognized state\n");
break;
}
break;
case P2P:
ERROR("handleDelayReq: disreguard in P2P mode\n");
break;
default:
/* none */
break;
}
}
static Boolean
setFrequency (ClockDriver *self, double adj, double tau) {
if (self->config.readOnly){
DBGV("adjFreq2: noAdjust on, returning\n");
return FALSE;
}
self->_tau = tau;
/*
* adjFreq simulation for QNX: correct clock by x ns per tick over clock adjust interval,
* to make it equal adj ns per second. Makes sense only if intervals are regular.
*/
#ifdef __QNXNTO__
struct _clockadjust clockadj;
struct _clockperiod period;
if (ClockPeriod (CLOCK_REALTIME, 0, &period, 0) < 0)
return FALSE;
adj = clampDouble(adj, self->maxFrequency);
/* adjust clock for the duration of 0.9 clock update period in ticks (so we're done before the next) */
clockadj.tick_count = 0.9 * tau * 1E9 / (period.nsec + 0.0);
/* scale adjustment per second to adjustment per single tick */
clockadj.tick_nsec_inc = (adj * tau / clockadj.tick_count) / 0.9;
DBGV("QNX: adj: %.09f, dt: %.09f, ticks per dt: %d, inc per tick %d\n",
adj, tau, clockadj.tick_count, clockadj.tick_nsec_inc);
if (ClockAdjust(CLOCK_REALTIME, &clockadj, NULL) < 0) {
DBGV("QNX: failed to call ClockAdjust: %s\n", strERROR(THIS_COMPONENTerrno));
}
/* regular adjFreq */
#elif defined(HAVE_SYS_TIMEX_H)
DBG2(" adjFreq2: call adjfreq to %.09f us \n", adj / DBG_UNIT);
adjFreq_unix(self, adj);
/* otherwise use adjtime */
#else
struct timeval tv;
adj = clampDouble(adj, self->maxFrequency);
tv.tv_sec = 0;
tv.tv_usec = (adj / 1000);
if((tau > 0) && (tau < 1.0)) {
tv.tv_usec *= tau;
}
adjtime(&tv, NULL);
#endif
self->lastFrequency = adj;
return TRUE;
}
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 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);
}
void
addForeign(Octet *buf,MsgHeader *header,PtpClock *ptpClock)
{
int i,j;
Boolean found = FALSE;
j = ptpClock->foreign_record_best;
/*Check if Foreign master is already known*/
for (i=0;i<ptpClock->number_foreign_records;i++) {
if (!memcmp(header->sourcePortIdentity.clockIdentity,
ptpClock->foreign[j].foreignMasterPortIdentity.clockIdentity,
CLOCK_IDENTITY_LENGTH) &&
(header->sourcePortIdentity.portNumber ==
ptpClock->foreign[j].foreignMasterPortIdentity.portNumber))
{
/*Foreign Master is already in Foreignmaster data set*/
ptpClock->foreign[j].foreignMasterAnnounceMessages++;
found = TRUE;
DBGV("addForeign : AnnounceMessage incremented \n");
msgUnpackHeader(buf,&ptpClock->foreign[j].header);
msgUnpackAnnounce(buf,&ptpClock->foreign[j].announce);
break;
}
j = (j+1)%ptpClock->number_foreign_records;
}
/*New Foreign Master*/
if (!found) {
if (ptpClock->number_foreign_records <
ptpClock->max_foreign_records) {
ptpClock->number_foreign_records++;
}
j = ptpClock->foreign_record_i;
/*Copy new foreign master data set from Announce message*/
memcpy(ptpClock->foreign[j].foreignMasterPortIdentity.clockIdentity,
header->sourcePortIdentity.clockIdentity,
CLOCK_IDENTITY_LENGTH);
ptpClock->foreign[j].foreignMasterPortIdentity.portNumber =
header->sourcePortIdentity.portNumber;
ptpClock->foreign[j].foreignMasterAnnounceMessages = 0;
/*
* header and announce field of each Foreign Master are
* usefull to run Best Master Clock Algorithm
*/
msgUnpackHeader(buf,&ptpClock->foreign[j].header);
msgUnpackAnnounce(buf,&ptpClock->foreign[j].announce);
DBGV("New foreign Master added \n");
ptpClock->foreign_record_i =
(ptpClock->foreign_record_i+1) %
ptpClock->max_foreign_records;
}
}
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");
}
}
static ssize_t netSend(const octet_t *buf, int16_t length, TimeInternal *time, const int32_t * addr, struct udp_pcb * pcb)
{
err_t result;
struct pbuf * p;
/* Allocate the tx pbuf based on the current size. */
p = pbuf_alloc(PBUF_TRANSPORT, length, PBUF_RAM);
if (NULL == p)
{
ERROR("netSend: Failed to allocate Tx Buffer\n");
goto fail01;
}
/* Copy the incoming data into the pbuf payload. */
result = pbuf_take(p, buf, length);
if (ERR_OK != result)
{
ERROR("netSend: Failed to copy data to Pbuf (%d)\n", result);
goto fail02;
}
/* send the buffer. */
result = udp_sendto(pcb, p, (void *)addr, pcb->local_port);
if (ERR_OK != result)
{
ERROR("netSend: Failed to send data (%d)\n", result);
goto fail02;
}
if (time != NULL)
{
#if LWIP_PTP
time->seconds = p->time_sec;
time->nanoseconds = p->time_nsec;
#else
/* TODO: use of loopback mode */
/*
time->seconds = 0;
time->nanoseconds = 0;
*/
getTime(time);
#endif
DBGV("netSend: %d sec %d nsec\n", time->seconds, time->nanoseconds);
} else {
DBGV("netSend\n");
}
fail02:
pbuf_free(p);
fail01:
return length;
/* return (0 == result) ? length : 0; */
}
void
handlePDelayReq(MsgHeader *header, Octet *msgIbuf, ssize_t length,
TimeInternal *time, Boolean isFromSelf,
RunTimeOpts *rtOpts, PtpClock *ptpClock)
{
if (ptpClock->delayMechanism == P2P) {
DBGV("PdelayReq message received : \n");
if(length < PDELAY_REQ_LENGTH) {
ERROR("short PDelayReq 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("HandlePdelayReq : disregard\n");
return;
case PTP_SLAVE:
case PTP_MASTER:
case PTP_PASSIVE:
if (isFromSelf) {
/*
* Get sending timestamp from IP stack
* with SO_TIMESTAMP
*/
ptpClock->pdelay_req_send_time.seconds =
time->seconds;
ptpClock->pdelay_req_send_time.nanoseconds =
time->nanoseconds;
/*Add latency*/
addTime(&ptpClock->pdelay_req_send_time,
&ptpClock->pdelay_req_send_time,
&rtOpts->outboundLatency);
break;
} else {
msgUnpackHeader(ptpClock->msgIbuf,
&ptpClock->PdelayReqHeader);
issuePDelayResp(time, header, rtOpts,
ptpClock);
break;
}
default:
DBG("do unrecognized state3\n");
break;
}
} 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;
}
}
ssize_t
netRecvGeneral(Octet * buf, NetPath * netPath)
{
ssize_t ret = 0;
struct sockaddr_in from_addr;
#ifdef PTPD_PCAP
struct pcap_pkthdr *pkt_header;
const u_char *pkt_data;
#endif
socklen_t from_addr_len = sizeof(from_addr);
#ifdef PTPD_PCAP
if (netPath->pcapGeneral == NULL) {
#endif
ret=recvfrom(netPath->generalSock, buf, PACKET_SIZE, MSG_DONTWAIT, (struct sockaddr*)&from_addr, &from_addr_len);
netPath->lastRecvAddr = from_addr.sin_addr.s_addr;
return ret;
#ifdef PTPD_PCAP
}
#endif
#ifdef PTPD_PCAP
else { /* Using PCAP */
/* Discard packet on socket */
if (netPath->generalSock >= 0)
recv(netPath->generalSock, buf, PACKET_SIZE, MSG_DONTWAIT);
if (( ret = pcap_next_ex(netPath->pcapGeneral, &pkt_header,
&pkt_data)) < 1) {
if (ret < 0)
DBGV("netRecvGeneral: pcap_next_ex failed %d %s\n",
ret, pcap_geterr(netPath->pcapGeneral));
return 0;
}
/* Make sure this is IP (could dot1q get here?) */
if( ntohs(*(u_short *)(pkt_data + 12)) != ETHERTYPE_IP)
DBGV("PCAP payload received is not Ethernet: 0x%04x\n",
ntohs(*(u_short *)(pkt_data + 12)));
/* Retrieve source IP from the payload - 14 eth + 12 IP src*/
netPath->lastRecvAddr = *(Integer32 *)(pkt_data + 26);
netPath->receivedPackets++;
/* XXX Total cheat */
memcpy(buf, pkt_data + netPath->headerOffset,
pkt_header->caplen - netPath->headerOffset);
fflush(NULL);
ret = pkt_header->caplen - netPath->headerOffset;
}
#endif
return ret;
}
/* add or update an entry in the foreign master data set */
MsgSync * addForeign(Octet *buf, MsgHeader *header, PtpClock *ptpClock)
{
int i, j;
Boolean found = FALSE;
DBGV("updateForeign\n");
j = ptpClock->foreign_record_best;
for(i = 0; i < ptpClock->number_foreign_records; ++i)
{
if(header->sourceCommunicationTechnology == ptpClock->foreign[j].foreign_master_communication_technology
&& header->sourcePortId == ptpClock->foreign[j].foreign_master_port_id
&& !memcmp(header->sourceUuid, ptpClock->foreign[j].foreign_master_uuid, PTP_UUID_LENGTH))
{
++ptpClock->foreign[j].foreign_master_syncs;
found = TRUE;
DBGV("updateForeign: update record %d\n", j);
break;
}
j = (j + 1)%ptpClock->number_foreign_records;
}
if(!found)
{
if(ptpClock->number_foreign_records < ptpClock->max_foreign_records)
++ptpClock->number_foreign_records;
j = ptpClock->foreign_record_i;
ptpClock->foreign[j].foreign_master_communication_technology =
header->sourceCommunicationTechnology;
ptpClock->foreign[j].foreign_master_port_id =
header->sourcePortId;
memcpy(ptpClock->foreign[j].foreign_master_uuid,
header->sourceUuid, PTP_UUID_LENGTH);
DBG("updateForeign: new record (%d,%d) %d %d %02hhx:%02hhx:%02hhx:%02hhx:%02hhx:%02hhx\n",
ptpClock->foreign_record_i, ptpClock->number_foreign_records,
ptpClock->foreign[j].foreign_master_communication_technology,
ptpClock->foreign[j].foreign_master_port_id,
ptpClock->foreign[j].foreign_master_uuid[0], ptpClock->foreign[j].foreign_master_uuid[1],
ptpClock->foreign[j].foreign_master_uuid[2], ptpClock->foreign[j].foreign_master_uuid[3],
ptpClock->foreign[j].foreign_master_uuid[4], ptpClock->foreign[j].foreign_master_uuid[5]);
ptpClock->foreign_record_i = (ptpClock->foreign_record_i + 1)%ptpClock->max_foreign_records;
}
msgUnpackHeader(buf, &ptpClock->foreign[j].header);
msgUnpackSync(buf, &ptpClock->foreign[j].sync);
return &ptpClock->foreign[j].sync;
}
void updateTime(const TimeInternal *time)
{
struct ptptime_t timeoffset;
DBGV("updateTime: %d sec %d nsec\n", time->seconds, time->nanoseconds);
timeoffset.tv_sec = -time->seconds;
timeoffset.tv_nsec = -time->nanoseconds;
/* Coarse update method */
ETH_PTPTime_UpdateOffset(&timeoffset);
DBGV("updateTime: updated\n");
}
/*Pack and send on general multicast ip adress an Announce message*/
void
issueAnnounce(RunTimeOpts *rtOpts,PtpClock *ptpClock)
{
msgPackAnnounce(ptpClock->msgObuf,ptpClock);
if (!netSendGeneral(ptpClock->msgObuf,ANNOUNCE_LENGTH,
&ptpClock->netPath, 0)) {
toState(PTP_FAULTY,rtOpts,ptpClock);
DBGV("Announce message can't be sent -> FAULTY state \n");
} else {
DBGV("Announce MSG sent ! \n");
ptpClock->sentAnnounceSequenceId++;
}
}
/* forever after, call this function in a non-rtos system */
void protocol_loop(RunTimeOpts *rtOpts, PtpClock *ptpClock)
{
if(ptpClock->port_state != PTP_INITIALIZING)
doState(rtOpts, ptpClock);
else if(!doInit(rtOpts, ptpClock))
return;
if(ptpClock->message_activity)
DBGV("activity\n");
#if 0
else
DBGV("no activity\n");
#endif
}
void load_data(const char* datafile) {
if(str_ends_with(datafile, "descriptors.txt")) load_all(datafile);
else X.load(datafile);
if(LIMIT_NDATA!=-1 && X.height > LIMIT_NDATA) X.height = LIMIT_NDATA;
n = X.height;
D = X.width;
distribute_data();
DBGV(LIMIT_NDATA);
DBGV(N);
DBGV(D);
DBGV(n);
}
Boolean isDoublePeircesOutlier(DoubleMovingStdDev *container, double sample, double threshold) {
double maxDev;
/* Sanity check - race condition was seen when enabling and disabling filters repeatedly */
if(container == NULL || container->meanContainer == NULL)
return FALSE;
maxDev = container->stdDev * getpeircesCriterion(container->meanContainer->count, 1) * threshold;
/*
* Two cases:
* - Too early - we got a -1 from Peirce's table,
* - safeguard: std dev is zero and filter is blocking
* everything, hus, we let the sample through
*/
if (maxDev <= 0.0 ) {
return FALSE;
}
if(fabs((double)(sample - container->meanContainer->mean)) > maxDev) {
DBGV("Peirce %s outlier: val: %.09f, cnt: %d, mxd: %.09f (%.03f * dev * %.03f), dev: %.09f, mea: %.09f, dif: %.09f\n", container->identifier,
sample, container->meanContainer->count, maxDev, getpeircesCriterion(container->meanContainer->count, 1), threshold,
container->stdDev,
container->meanContainer->mean, fabs(sample - container->meanContainer->mean));
return TRUE;
}
return FALSE;
}
/*Pack and send on general multicast ip adress a FollowUp message*/
void
issueFollowup(TimeInternal *time,RunTimeOpts *rtOpts,PtpClock *ptpClock)
{
Timestamp preciseOriginTimestamp;
fromInternalTime(time,&preciseOriginTimestamp);
msgPackFollowUp(ptpClock->msgObuf,&preciseOriginTimestamp,ptpClock);
if (!netSendGeneral(ptpClock->msgObuf,FOLLOW_UP_LENGTH,
&ptpClock->netPath, 0)) {
toState(PTP_FAULTY,rtOpts,ptpClock);
DBGV("FollowUp message can't be sent -> FAULTY state \n");
} else {
DBGV("FollowUp MSG sent ! \n");
}
}
/* loop forever. doState() has a switch for the actions and events to be
checked for 'port_state'. the actions and events may or may not change
'port_state' by calling toState(), but once they are done we loop around
again and perform the actions required for the new 'port_state'. */
void
protocol(RunTimeOpts *rtOpts, PtpClock *ptpClock)
{
DBG("event POWERUP\n");
toState(PTP_INITIALIZING, rtOpts, ptpClock);
DBG("Debug Initializing...\n");
for (;;)
{
/* 20110701: this main loop was rewritten to be more clear */
if (ptpClock->portState == PTP_INITIALIZING) {
if (!doInit(rtOpts, ptpClock)) {
return;
}
} else {
doState(rtOpts, ptpClock);
}
if (ptpClock->message_activity)
DBGV("activity\n");
/* Perform the heavy signal processing synchronously */
check_signals(rtOpts, ptpClock);
}
}
/*Pack and send on event multicast ip adress a Sync message*/
static void issueSync(PtpClock *ptpClock)
{
Timestamp originTimestamp;
TimeInternal internalTime;
/* try to predict outgoing time stamp */
getTime(&internalTime);
fromInternalTime(&internalTime, &originTimestamp);
msgPackSync(ptpClock, ptpClock->msgObuf, &originTimestamp);
if (!netSendEvent(&ptpClock->netPath, ptpClock->msgObuf, SYNC_LENGTH, &internalTime))
{
ERROR("issueSync: can't sent\n");
toState(ptpClock, PTP_FAULTY);
}
else
{
DBGV("issueSync\n");
ptpClock->sentSyncSequenceId++;
/* sync TX timestamp is valid */
if ((internalTime.seconds != 0) && (ptpClock->defaultDS.twoStepFlag))
{
// waitingForLoopback = false;
addTime(&internalTime, &internalTime, &ptpClock->outboundLatency);
issueFollowup(ptpClock, &internalTime);
}
else
{
// waitingForLoopback = ptpClock->twoStepFlag;
}
}
}
void v2copyD0(V2MsgHeader *header,
MsgAnnounce *announce,
PtpClock *ptpClock
)
{
DBGV("v2copyD0:\n");
memcpy(announce->grandmasterIdentity,
ptpClock->port_clock_identity,
8
);
announce->grandmasterPriority1 = ptpClock->grandmaster_priority1;
announce->grandmasterPriority2 = ptpClock->grandmaster_priority2;
memcpy(&announce->grandmasterClockQuality,
&ptpClock->grandmaster_clock_quality,
sizeof(announce->grandmasterClockQuality)
);
announce->stepsRemoved = 0;
memcpy(header->sourcePortId.clockIdentity,
ptpClock->port_clock_identity,
8
);
header->sourcePortId.portNumber = ptpClock->port_id_field;
header->sequenceId = ptpClock->grandmaster_sequence_number;
}
void issueDelayReq(PtpClock *ptpClock)
{
TimeInternal internalTime;
TimeRepresentation originTimestamp;
ptpClock->sentDelayReq = TRUE;
ptpClock->sentDelayReqSequenceId = ++ptpClock->last_sync_event_sequence_number;
/* try to predict outgoing time stamp */
getTime(&internalTime, ptpClock);
fromInternalTime(&internalTime, &originTimestamp, ptpClock->halfEpoch);
msgPackDelayReq(ptpClock->msgObuf, FALSE, FALSE, &originTimestamp, ptpClock);
if(!netSendEvent(ptpClock->msgObuf, DELAY_REQ_PACKET_LENGTH,
ptpClock->delayedTiming ? &internalTime : NULL,
ptpClock))
toState(PTP_FAULTY, ptpClock);
else
{
DBGV("sent delay request message\n");
if(ptpClock->delayedTiming)
{
if (internalTime.seconds || internalTime.nanoseconds) {
/* compensate with configurable latency, then store for later use */
addTime(&internalTime, &internalTime, &ptpClock->runTimeOpts.outboundLatency);
ptpClock->delay_req_send_time = internalTime;
} else {
NOTIFY("WARNING: delay request message without hardware time stamp, will skip response\n");
ptpClock->sentDelayReq = FALSE;
}
}
}
}
/* pack and send various messages */
void issueSync(PtpClock *ptpClock)
{
TimeInternal internalTime;
TimeRepresentation originTimestamp;
++ptpClock->last_sync_event_sequence_number;
ptpClock->grandmaster_sequence_number = ptpClock->last_sync_event_sequence_number;
/* try to predict outgoing time stamp */
getTime(&internalTime, ptpClock);
fromInternalTime(&internalTime, &originTimestamp, ptpClock->halfEpoch);
msgPackSync(ptpClock->msgObuf, FALSE, TRUE, &originTimestamp, ptpClock);
if(!netSendEvent(ptpClock->msgObuf, SYNC_PACKET_LENGTH,
ptpClock->delayedTiming ? &internalTime : NULL,
ptpClock))
toState(PTP_FAULTY, ptpClock);
else
{
DBGV("sent sync message\n");
if(ptpClock->delayedTiming)
{
if (internalTime.seconds || internalTime.nanoseconds) {
/* compensate with configurable latency, then tell client real time stamp */
addTime(&internalTime, &internalTime, &ptpClock->runTimeOpts.outboundLatency);
issueFollowup(&internalTime, ptpClock);
} else {
NOTIFY("WARNING: sync message without hardware time stamp, skipped followup\n");
}
}
}
}
Boolean
ntpShutdown(NTPoptions* options, NTPcontrol* control)
{
/* Attempt reverting ntpd flags to the original value */
if(control->flagsCaptured) {
/* we only control the kernel and ntp flags */
/* just to avoid -Wunused* */
#ifdef RUNTIME_DEBUG
int resC = ntpdClearFlags(options, control, ~(control->originalFlags) & (INFO_FLAG_KERNEL | INFO_FLAG_NTP));
int resS = ntpdSetFlags(options, control, control->originalFlags & (INFO_FLAG_KERNEL | INFO_FLAG_NTP));
DBGV("Attempting to revert NTPd flags to %d - result: clear %d set %d\n", control->originalFlags,
resC, resS);
#else
ntpdClearFlags(options, control, ~(control->originalFlags) & (INFO_FLAG_KERNEL | INFO_FLAG_NTP));
ntpdSetFlags(options, control, control->originalFlags & (INFO_FLAG_KERNEL | INFO_FLAG_NTP));
#endif /* RUNTIME_DEBUG */
}
if (control->sockFD > 0)
close(control->sockFD);
control->sockFD = -1;
return TRUE;
}
/*Pack and send on event multicast ip adress a PDelayReq message*/
static void issuePDelayReq(PtpClock *ptpClock)
{
Timestamp originTimestamp;
TimeInternal internalTime;
getTime(&internalTime);
fromInternalTime(&internalTime, &originTimestamp);
msgPackPDelayReq(ptpClock, ptpClock->msgObuf, &originTimestamp);
if (!netSendPeerEvent(&ptpClock->netPath, ptpClock->msgObuf, PDELAY_REQ_LENGTH, &internalTime))
{
ERROR("issuePDelayReq: can't sent\n");
toState(ptpClock, PTP_FAULTY);
}
else
{
DBGV("issuePDelayReq\n");
ptpClock->sentPDelayReqSequenceId++;
/* Delay req TX timestamp is valid */
if (internalTime.seconds != 0)
{
addTime(&internalTime, &internalTime, &ptpClock->outboundLatency);
ptpClock->pdelay_t1 = internalTime;
}
}
}
Boolean
ntpInit(NTPoptions* options, NTPcontrol* control)
{
int res = TRUE;
TimingService service = control->timingService;
control->sockFD = -1;
if(!options->enableEngine)
return FALSE;
memset(control, 0, sizeof(*control));
/* preserve TimingService... temporary */
control->timingService = service;
if(!hostLookup(options->hostAddress, &control->serverAddress)) {
control->serverAddress = 0;
return FALSE;
}
if ((control->sockFD = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP)) < 0 ) {
PERROR("failed to initalize NTP control socket");
return FALSE;
}
/* This will attempt to read the ntpd control flags for the first time */
res = ntpdInControl(options, control);
if (res != INFO_YES && res != INFO_NO) {
return FALSE;
}
DBGV("NTPd original flags: %d\n", control->originalFlags);
return TRUE;
}
