void
updateDelay(Filter * owd_filt, RunTimeOpts * rtOpts, PtpClock * ptpClock, TimeInternal * correctionField)
{
/* updates paused, leap second pending - do nothing */
if(ptpClock->leapSecondInProgress)
return;
DBGV("updateDelay\n");
/* todo: do all intermediate calculations on temp vars */
TimeInternal prev_meanPathDelay = ptpClock->meanPathDelay;
ptpClock->char_last_msg = 'D';
{
//perform basic checks, using local variables only
TimeInternal slave_to_master_delay;
/* calc 'slave_to_master_delay' */
subTime(&slave_to_master_delay, &ptpClock->delay_req_receive_time,
&ptpClock->delay_req_send_time);
if (rtOpts->maxDelay && /* If maxDelay is 0 then it's OFF */
rtOpts->offset_first_updated) {
if ((slave_to_master_delay.nanoseconds < 0) &&
(abs(slave_to_master_delay.nanoseconds) > rtOpts->maxDelay)) {
INFO("updateDelay aborted, "
"delay (sec: %d ns: %d) is negative\n",
slave_to_master_delay.seconds,
slave_to_master_delay.nanoseconds);
INFO("send (sec: %d ns: %d)\n",
ptpClock->delay_req_send_time.seconds,
ptpClock->delay_req_send_time.nanoseconds);
INFO("recv (sec: %d n s: %d)\n",
ptpClock->delay_req_receive_time.seconds,
ptpClock->delay_req_receive_time.nanoseconds);
goto display;
}
if (slave_to_master_delay.seconds && rtOpts->maxDelay) {
INFO("updateDelay aborted, delay %d.%d greater than 1 second\n",
slave_to_master_delay.seconds,
slave_to_master_delay.nanoseconds);
if (rtOpts->displayPackets)
msgDump(ptpClock);
goto display;
}
if (slave_to_master_delay.nanoseconds > rtOpts->maxDelay) {
INFO("updateDelay aborted, delay %d greater than "
"administratively set maximum %d\n",
slave_to_master_delay.nanoseconds,
rtOpts->maxDelay);
if (rtOpts->displayPackets)
msgDump(ptpClock);
goto display;
}
}
}
/*
* The packet has passed basic checks, so we'll:
* - update the global delaySM variable
* - calculate a new filtered MPD
*/
if (rtOpts->offset_first_updated) {
/*
* calc 'slave_to_master_delay' (Master to Slave delay is
* already computed in updateOffset )
*/
DBG("==> UpdateDelay(): %s\n",
dump_TimeInternal2("Req_RECV:", &ptpClock->delay_req_receive_time,
"Req_SENT:", &ptpClock->delay_req_send_time));
#ifdef PTPD_STATISTICS
if (rtOpts->delaySMOutlierFilterEnabled) {
subTime(&ptpClock->rawDelaySM, &ptpClock->delay_req_receive_time,
&ptpClock->delay_req_send_time);
if(!isDoublePeircesOutlier(ptpClock->delaySMRawStats, timeInternalToDouble(&ptpClock->rawDelaySM), rtOpts->delaySMOutlierFilterThreshold)) {
ptpClock->delaySM = ptpClock->rawDelaySM;
ptpClock->delaySMoutlier = FALSE;
} else {
ptpClock->delaySMoutlier = TRUE;
ptpClock->counters.delaySMOutliersFound++;
if (!rtOpts->delaySMOutlierFilterDiscard) {
ptpClock->delaySM = doubleToTimeInternal(ptpClock->delaySMFiltered->mean);
} else {
goto statistics;
}
}
} else {
subTime(&ptpClock->delaySM, &ptpClock->delay_req_receive_time,
&ptpClock->delay_req_send_time);
}
#else
subTime(&ptpClock->delaySM, &ptpClock->delay_req_receive_time,
&ptpClock->delay_req_send_time);
#endif
/* update 'one_way_delay' */
addTime(&ptpClock->meanPathDelay, &ptpClock->delaySM,
&ptpClock->delayMS);
/* Substract correctionField */
subTime(&ptpClock->meanPathDelay, &ptpClock->meanPathDelay,
correctionField);
/* Compute one-way delay */
div2Time(&ptpClock->meanPathDelay);
if (ptpClock->meanPathDelay.seconds) {
DBG("update delay: cannot filter with large OFM, "
"clearing filter\n");
INFO("Servo: Ignoring delayResp because of large OFM\n");
FilterClear(owd_filt);
/* revert back to previous value */
ptpClock->meanPathDelay = prev_meanPathDelay;
goto display;
}
if(ptpClock->meanPathDelay.nanoseconds < 0){
DBG("update delay: found negative value for OWD, "
"so ignoring this value: %d\n",
ptpClock->meanPathDelay.nanoseconds);
/* revert back to previous value */
ptpClock->meanPathDelay = prev_meanPathDelay;
#ifdef PTPD_STATISTICS
goto statistics;
#else
goto display;
#endif /* PTPD_STATISTICS */
}
{
// TODO: remove hack
char s_text[32];
sprintf(s_text, "%d", rtOpts->s);
FilterConfigure(owd_filt, "stiffness", s_text);
}
FilterFeed(owd_filt, &ptpClock->meanPathDelay.nanoseconds);
/* Update relevant statistics containers, feed outlier filter thresholds etc. */
#ifdef PTPD_STATISTICS
statistics:
if (rtOpts->delaySMOutlierFilterEnabled) {
double dDelaySM = timeInternalToDouble(&ptpClock->rawDelaySM);
/* If this is an outlier, bring it by a factor closer to mean before allowing to influence stdDev */
if(ptpClock->delaySMoutlier) {
/* Allow [weight] * [deviation from mean] to influence std dev in the next outlier checks */
DBG("DelaySM outlier: %.09f\n", dDelaySM);
if((rtOpts->calibrationDelay<1) || ptpClock->isCalibrated)
dDelaySM = ptpClock->delaySMRawStats->meanContainer->mean + rtOpts->delaySMOutlierWeight * ( dDelaySM - ptpClock->delaySMRawStats->meanContainer->mean);
}
feedDoubleMovingStdDev(ptpClock->delaySMRawStats, dDelaySM);
feedDoubleMovingMean(ptpClock->delaySMFiltered, timeInternalToDouble(&ptpClock->delaySM));
}
feedDoublePermanentStdDev(&ptpClock->slaveStats.owdStats, timeInternalToDouble(&ptpClock->meanPathDelay));
#endif
DBGV("delay filter %d\n", ptpClock->meanPathDelay.nanoseconds);
} else {
INFO("Ignoring delayResp because we didn't receive any sync yet\n");
}
display:
logStatistics(rtOpts, ptpClock);
}
void
updateDelay(one_way_delay_filter * owd_filt, const RunTimeOpts * rtOpts, PtpClock * ptpClock, TimeInternal * correctionField)
{
/* updates paused, leap second pending - do nothing */
if(ptpClock->leapSecondInProgress)
return;
DBGV("updateDelay\n");
/* todo: do all intermediate calculations on temp vars */
TimeInternal prev_meanPathDelay = ptpClock->meanPathDelay;
ptpClock->char_last_msg = 'D';
Boolean maxDelayHit = FALSE;
{
#ifdef PTPD_STATISTICS
/* if maxDelayStableOnly configured, only check once servo is stable */
Boolean checkThreshold = rtOpts-> maxDelayStableOnly ?
(ptpClock->servo.isStable && rtOpts->maxDelay) :
(rtOpts->maxDelay);
#else
Boolean checkThreshold = rtOpts->maxDelay;
#endif
//perform basic checks, using local variables only
TimeInternal slave_to_master_delay;
/* calc 'slave_to_master_delay' */
subTime(&slave_to_master_delay, &ptpClock->delay_req_receive_time,
&ptpClock->delay_req_send_time);
if (checkThreshold && /* If maxDelay is 0 then it's OFF */
ptpClock->offsetFirstUpdated) {
if ((slave_to_master_delay.nanoseconds < 0) &&
(abs(slave_to_master_delay.nanoseconds) > rtOpts->maxDelay)) {
INFO("updateDelay aborted, "
"delay (sec: %d ns: %d) is negative\n",
slave_to_master_delay.seconds,
slave_to_master_delay.nanoseconds);
INFO("send (sec: %d ns: %d)\n",
ptpClock->delay_req_send_time.seconds,
ptpClock->delay_req_send_time.nanoseconds);
INFO("recv (sec: %d n s: %d)\n",
ptpClock->delay_req_receive_time.seconds,
ptpClock->delay_req_receive_time.nanoseconds);
goto finish;
}
if (slave_to_master_delay.seconds && checkThreshold) {
INFO("updateDelay aborted, slave to master delay %d.%d greater than 1 second\n",
slave_to_master_delay.seconds,
slave_to_master_delay.nanoseconds);
if (rtOpts->displayPackets)
msgDump(ptpClock);
goto finish;
}
if (slave_to_master_delay.nanoseconds > rtOpts->maxDelay) {
ptpClock->counters.maxDelayDrops++;
DBG("updateDelay aborted, slave to master delay %d greater than "
"administratively set maximum %d\n",
slave_to_master_delay.nanoseconds,
rtOpts->maxDelay);
if(rtOpts->maxDelayMaxRejected) {
maxDelayHit = TRUE;
/* if we blocked maxDelayMaxRejected samples, reset the slave to unblock the filter */
if(++ptpClock->maxDelayRejected > rtOpts->maxDelayMaxRejected) {
WARNING("%d consecutive measurements above %d threshold - resetting slave\n",
rtOpts->maxDelayMaxRejected, slave_to_master_delay.nanoseconds);
toState(PTP_LISTENING, rtOpts, ptpClock);
}
}
if (rtOpts->displayPackets)
msgDump(ptpClock);
goto finish;
} else {
ptpClock->maxDelayRejected=0;
}
}
}
/*
* The packet has passed basic checks, so we'll:
* - update the global delaySM variable
* - calculate a new filtered MPD
*/
if (ptpClock->offsetFirstUpdated) {
Integer16 s;
/*
* calc 'slave_to_master_delay' (Master to Slave delay is
* already computed in updateOffset )
*/
DBG("==> UpdateDelay(): %s\n",
dump_TimeInternal2("Req_RECV:", &ptpClock->delay_req_receive_time,
"Req_SENT:", &ptpClock->delay_req_send_time));
/* raw value before filtering */
subTime(&ptpClock->rawDelaySM, &ptpClock->delay_req_receive_time,
&ptpClock->delay_req_send_time);
#ifdef PTPD_STATISTICS
/* testing only: step detection */
#if 0
TimeInternal bob;
bob.nanoseconds = -1000000;
bob.seconds = 0;
if(ptpClock->addOffset) {
addTime(&ptpClock->rawDelaySM, &ptpClock->rawDelaySM, &bob);
}
#endif
/* run the delayMS stats filter */
if(rtOpts->filterSMOpts.enabled) {
if(!feedDoubleMovingStatFilter(ptpClock->filterSM, timeInternalToDouble(&ptpClock->rawDelaySM))) {
return;
}
ptpClock->rawDelaySM = doubleToTimeInternal(ptpClock->filterSM->output);
}
/* run the delaySM outlier filter */
if(!rtOpts->noAdjust && ptpClock->oFilterSM.config.enabled && (ptpClock->oFilterSM.config.alwaysFilter || !ptpClock->servo.runningMaxOutput) ) {
if(ptpClock->oFilterSM.filter(&ptpClock->oFilterSM, timeInternalToDouble(&ptpClock->rawDelaySM))) {
ptpClock->delaySM = doubleToTimeInternal(ptpClock->oFilterSM.output);
} else {
ptpClock->counters.delaySMOutliersFound++;
/* If the outlier filter has blocked the sample, "reverse" the last maxDelay action */
if (maxDelayHit) {
ptpClock->maxDelayRejected--;
}
goto finish;
}
} else {
ptpClock->delaySM = ptpClock->rawDelaySM;
}
#else
subTime(&ptpClock->delaySM, &ptpClock->delay_req_receive_time,
&ptpClock->delay_req_send_time);
#endif
/* update MeanPathDelay */
addTime(&ptpClock->meanPathDelay, &ptpClock->delaySM,
&ptpClock->delayMS);
/* Subtract correctionField */
subTime(&ptpClock->meanPathDelay, &ptpClock->meanPathDelay,
correctionField);
/* Compute one-way delay */
div2Time(&ptpClock->meanPathDelay);
if (ptpClock->meanPathDelay.seconds) {
DBG("update delay: cannot filter with large OFM, "
"clearing filter\n");
INFO("Servo: Ignoring delayResp because of large OFM\n");
owd_filt->s_exp = owd_filt->nsec_prev = 0;
/* revert back to previous value */
ptpClock->meanPathDelay = prev_meanPathDelay;
goto finish;
}
if(ptpClock->meanPathDelay.nanoseconds < 0){
DBG("update delay: found negative value for OWD, "
"so ignoring this value: %d\n",
ptpClock->meanPathDelay.nanoseconds);
/* revert back to previous value */
ptpClock->meanPathDelay = prev_meanPathDelay;
goto finish;
}
/* avoid overflowing filter */
s = rtOpts->s;
while (abs(owd_filt->y) >> (31 - s))
--s;
/* crank down filter cutoff by increasing 's_exp' */
if (owd_filt->s_exp < 1)
owd_filt->s_exp = 1;
else if (owd_filt->s_exp < 1 << s)
++owd_filt->s_exp;
else if (owd_filt->s_exp > 1 << s)
owd_filt->s_exp = 1 << s;
/* filter 'meanPathDelay' */
double fy =
(double)((owd_filt->s_exp - 1.0) *
owd_filt->y / (owd_filt->s_exp + 0.0) +
(ptpClock->meanPathDelay.nanoseconds / 2.0 +
owd_filt->nsec_prev / 2.0) / (owd_filt->s_exp + 0.0));
owd_filt->nsec_prev = ptpClock->meanPathDelay.nanoseconds;
owd_filt->y = round(fy);
ptpClock->meanPathDelay.nanoseconds = owd_filt->y;
DBGV("delay filter %d, %d\n", owd_filt->y, owd_filt->s_exp);
} else {
