/* greater than operation */
int gtTime(TimeInternal *x, TimeInternal *y)
{
TimeInternal r;
subTime(&r, x, y);
return !isTimeInternalNegative(&r);
}
/* greater than operation */
int
gtTime(const TimeInternal *x, const TimeInternal *y)
{
TimeInternal r;
subTime(&r, x, y);
return !isTimeNegative(&r);
}
void
QSS_PAR_initialize (SIM_simulator simulate)
{
QSS_simulator simulator = (QSS_simulator) simulate->state->sim;
QSS_PAR_allocRootSimulatorData (simulator);
getTime (simulator->iTime);
PRT_partition partition = PRT_Partition (simulator->data,
simulator->output->name);
getTime (simulator->sdTime);
subTime (simulator->sdTime, simulator->iTime);
simulator->stats->partitioningTime = getTimeValue (simulator->sdTime);
getTime (simulator->iTime);
LP_initializeDataStructs (simulator, partition);
getTime (simulator->sdTime);
subTime (simulator->sdTime, simulator->iTime);
PRT_freePartition (partition);
QSS_PAR_logMemory (simulator);
simulator->stats->initializeLPS = getTimeValue (simulator->sdTime);
}
void timeDelta(TimeInternal *before, TimeInternal *meas, TimeInternal *after, TimeInternal *delta)
{
TimeInternal tmpDelta;
div2Time(before);
div2Time(after);
addTime(&tmpDelta, before, after);
subTime(delta, &tmpDelta, meas);
}
static void
waitTime (
pwr_tTime *t
)
{
pwr_tStatus sts;
pwr_tTime now;
pwr_tTime then = *t;
char tims[24];
short len;
struct dsc$descriptor_s tims_desc = {
sizeof(tims)-1, DSC$K_DTYPE_T, DSC$K_CLASS_S,};
#if 0
subTime(&then, nowTime(&now));
#endif
if ((int)then.tv_sec > 0 || ((int)then.tv_sec == 0 && then.tv_nsec > 0)) {
#if defined OS_VMS || defined OS_ELN
int tv_nsec;
int vmstime[2];
int multiplier = -10000000; /* Used to convert 1 s to 100 ns, delta time. */
static pwr_tTime tick = {0, 10000000};
addTime(&then, &tick);
tv_nsec = -then.tv_nsec/100; /* Convert to 100 ns. */
sts = lib$emul(&then.tv_sec, &multiplier, &tv_nsec, vmstime);
#if defined OS_VMS
tims_desc.dsc$a_pointer = tims;
sys$asctim( &len, &tims_desc, vmstime, 0);
tims[len] = '\0';
printf(" %s\n", tims);
#if 0
sts = sys$clref(timerFlag);
sts = sys$setimr(timerFlag, vmstime, 0, 0, 0);
sts = sys$waitfr(timerFlag);
#endif
#elif defined OS_ELN
eln$time_string(tims, vmstime);
tims[23] = '\0';
printf(" %s\n", tims);
#if 0
ker$wait_any(&sts, NULL, vmstime);
#endif
#endif
#elif defined OS_LYNX
pwr_tTime rmt;
nanosleep(&then, &rmt);
#endif
}
}
void KTimeEdit::keyPressEvent(QKeyEvent *qke)
{
switch(qke->key())
{
case Key_Down:
addTime(QTime(0, 1, 0));
break;
case Key_Up:
subTime(QTime(0, 1, 0));
break;
case Key_Prior:
subTime(QTime(1, 0, 0));
break;
case Key_Next:
addTime(QTime(1, 0, 0));
break;
default:
QComboBox::keyPressEvent(qke);
break;
} // switch
}
void
QSS_PAR_saveLog (QSS_simulator simulator)
{
int outputs = simulator->data->lp->outputs;
if (outputs)
{
getTime (simulator->sTime);
OUT_save (simulator->log);
getTime (simulator->sdTime);
subTime (simulator->sdTime, simulator->sTime);
simulator->saveTime = getTimeValue (simulator->sdTime);
}
}
/* if 2 time values are close enough for X nanoseconds */
int is_Time_close(TimeInternal *x, TimeInternal *y, int nanos)
{
TimeInternal r1;
TimeInternal r2;
// first, subtract the 2 values. then call abs(), then call gtTime for requested the number of nanoseconds
subTime(&r1, x, y);
absTime(&r1);
nano_to_Time(&r2, nanos);
return !gtTime(&r1, &r2);
}
inline
void
SimulatedBlock::EXECUTE_DIRECT(Uint32 block,
Uint32 gsn,
Signal* signal,
Uint32 len){
signal->setLength(len);
#ifdef VM_TRACE
if(globalData.testOn){
signal->header.theVerId_signalNumber = gsn;
signal->header.theReceiversBlockNumber = block;
signal->header.theSendersBlockRef = reference();
globalSignalLoggers.executeDirect(signal->header,
0, // in
&signal->theData[0],
globalData.ownId);
}
#endif
SimulatedBlock* b = globalData.getBlock(block);
#ifdef VM_TRACE_TIME
Uint32 us1, us2;
Uint64 ms1, ms2;
NdbTick_CurrentMicrosecond(&ms1, &us1);
Uint32 tGsn = m_currentGsn;
b->m_currentGsn = gsn;
#endif
b->executeFunction(gsn, signal);
#ifdef VM_TRACE_TIME
NdbTick_CurrentMicrosecond(&ms2, &us2);
Uint64 diff = ms2;
diff -= ms1;
diff *= 1000000;
diff += us2;
diff -= us1;
b->addTime(gsn, diff);
m_currentGsn = tGsn;
subTime(tGsn, diff);
#endif
#ifdef VM_TRACE
if(globalData.testOn){
signal->header.theVerId_signalNumber = gsn;
signal->header.theReceiversBlockNumber = block;
signal->header.theSendersBlockRef = reference();
globalSignalLoggers.executeDirect(signal->header,
1, // out
&signal->theData[0],
globalData.ownId);
}
#endif
}
void safeToProcess(const Event* const thisEvent, Time* safeTimestamp) {
Time tempTime;
if (thisEvent->offsetTime.secs < 0 || (thisEvent->offsetTime.secs == 0
&& thisEvent->offsetTime.nsecs < 0)) {
tempTime.secs = (uint32) (-thisEvent->offsetTime.secs);
tempTime.nsecs = (uint32) (-thisEvent->offsetTime.nsecs);
addTime(thisEvent->tag.timestamp, tempTime, safeTimestamp);
} else {
int16 out;
tempTime.secs = (uint32) (thisEvent->offsetTime.secs);
tempTime.nsecs = (uint32) (thisEvent->offsetTime.nsecs);
out = subTime(thisEvent->tag.timestamp, tempTime, safeTimestamp);
if (out == -1) {
safeTimestamp->secs = 0;
safeTimestamp->nsecs = 0;
}
}
}
void
updatePeerDelay(Filter * owd_filt, RunTimeOpts * rtOpts, PtpClock * ptpClock, TimeInternal * correctionField, Boolean twoStep)
{
/* updates paused, leap second pending - do nothing */
if(ptpClock->leapSecondInProgress)
return;
DBGV("updatePeerDelay\n");
ptpClock->char_last_msg = 'P';
if (twoStep) {
/* calc 'slave_to_master_delay' */
subTime(&ptpClock->pdelayMS,
&ptpClock->pdelay_resp_receive_time,
&ptpClock->pdelay_resp_send_time);
subTime(&ptpClock->pdelaySM,
&ptpClock->pdelay_req_receive_time,
&ptpClock->pdelay_req_send_time);
/* update 'one_way_delay' */
addTime(&ptpClock->peerMeanPathDelay,
&ptpClock->pdelayMS,
&ptpClock->pdelaySM);
/* Substract correctionField */
subTime(&ptpClock->peerMeanPathDelay,
&ptpClock->peerMeanPathDelay, correctionField);
/* Compute one-way delay */
div2Time(&ptpClock->peerMeanPathDelay);
} else {
/* One step clock */
subTime(&ptpClock->peerMeanPathDelay,
&ptpClock->pdelay_resp_receive_time,
&ptpClock->pdelay_req_send_time);
/* Substract correctionField */
subTime(&ptpClock->peerMeanPathDelay,
&ptpClock->peerMeanPathDelay, correctionField);
/* Compute one-way delay */
div2Time(&ptpClock->peerMeanPathDelay);
}
if (ptpClock->peerMeanPathDelay.seconds) {
/* cannot filter with secs, clear filter */
FilterClear(owd_filt);
return;
}
{
// TODO: remove hack
char s_text[32];
sprintf(s_text, "%d", rtOpts->s);
FilterConfigure(owd_filt, "stiffness", s_text);
}
FilterFeed(owd_filt, &ptpClock->peerMeanPathDelay.nanoseconds);
DBGV("delay filter %d\n", ptpClock->peerMeanPathDelay.nanoseconds);
//display:
if(ptpClock->portState == PTP_SLAVE)
logStatistics(rtOpts, ptpClock);
}
void
QSS_SEQ_initialize (SIM_simulator simulate)
{
QSS_simulator simulator = (QSS_simulator) simulate->state->sim;
getTime (simulator->iTime);
int i, j, k, s, forUL;
double e, zc[4];
simulator->frw = FRW_Framework (simulator->data);
double t = simulator->time->time;
char logFile[128];
strcpy (logFile, simulator->output->name);
simulator->simulationLog = SD_SimulationLog (logFile);
// Local mappings.
QSS_data qssData = simulator->data;
QSS_time qssTime = simulator->time;
FRW_framework frw = simulator->frw;
double *q = qssData->q;
double *x = qssData->x;
const int order = qssData->order;
const int coeffs = order + 1;
#ifdef DEBUG
SD_simulationSettings settings = simulator->settings;
SD_simulationLog simulationLog = simulator->simulationLog;
#endif
QSS_model qssModel = simulator->model;
QA_quantizer quantizer;
SD_output output = simulator->output;
#ifdef DEBUG
if (settings->debug & SD_DBG_InitValues)
{
SD_print (simulationLog, "Initialize simulation\n");
}
#endif
forUL = qssData->states;
for (i = 0; i < forUL; i++)
{
qssData->lqu[i] = qssData->dQRel[i] * fabs (x[i * coeffs]);
if (qssData->lqu[i] < qssData->dQMin[i])
{
qssData->lqu[i] = qssData->dQMin[i];
}
#ifdef DEBUG
if (settings->debug & SD_DBG_InitValues)
{
SD_print (simulationLog, "Initial value: x[%d][0] = %g", i, x[i * coeffs]);
}
#endif
}
#ifdef DEBUG
if (settings->debug & SD_DBG_InitValues)
{
SD_print (simulationLog, "Initialize solver...");
}
#endif
simulator->quantizer = QA_Quantizer (qssData, qssTime);
quantizer = simulator->quantizer;
#ifdef DEBUG
if (settings->debug & SD_DBG_InitValues)
{
SD_print (simulationLog, "done.");
SD_print (simulationLog, "Initialize state derivatives...");
}
#endif
forUL = qssData->states;
for (i = 0; i < forUL; i++)
{
FRW_recomputeDerivative (frw, qssModel, qssData, qssTime, i);
QA_nextTime (quantizer,i,t,qssTime->nextStateTime,x,qssData->lqu);
#ifdef DEBUG
if (settings->debug & SD_DBG_InitValues)
{
SD_print (simulationLog, "Initial derivative: x[%d][1] = %g", i,
x[i * coeffs + 1]);
}
#endif
}
#ifdef DEBUG
if (settings->debug & SD_DBG_InitValues)
{
SD_print (simulationLog, "done.");
SD_print (simulationLog, "Initialize input...");
}
#endif
forUL = qssData->inputs;
for (i = 0; i < forUL; i++)
{
j = qssData->TD[i];
FRW_nextInputTime (frw, qssModel, qssData, qssTime, 0, j, i);
}
#ifdef DEBUG
if (settings->debug & SD_DBG_InitValues)
{
SD_print (simulationLog, "done.");
SD_print (simulationLog, "Initialize events...");
}
#endif
forUL = qssData->events;
for (i = 0; i < forUL; i++)
{
if (qssData->nZS[i])
{
int nZS = qssData->nZS[i];
e = INF;
for (j = 0; j < nZS; j++)
{
k = qssData->ZS[i][j];
if (qssData->dQMin[k] < e)
{
e = qssData->dQMin[k];
}
}
}
else
{
e = qssData->ft * qssData->params->zcHyst;
}
qssModel->events->zeroCrossing (i, q, qssData->d, qssData->alg,
qssTime->time, zc);
s = sign (zc[0]);
qssData->event[i].zcSign = s;
qssData->event[i].zcHyst = e / 10.0;
if (qssData->event[i].direction == 0 || qssData->event[i].direction == s
|| zc[0] == 0)
{
if (zc[0] == 0 && qssData->event[i].relation == 3)
{
qssModel->events->handlerPos (i, q, qssData->d, qssData->alg, t);
}
else if (zc[0] == 0 && qssData->event[i].relation == 1)
{
qssModel->events->handlerNeg (i, q, qssData->d, qssData->alg, t);
}
else if (s >= 0)
{
qssModel->events->handlerPos (i, q, qssData->d, qssData->alg, t);
}
else
{
qssModel->events->handlerNeg (i, q, qssData->d, qssData->alg, t);
}
qssModel->events->zeroCrossing (i, q, qssData->d, qssData->alg, t,
zc);
qssData->event[i].zcSign = sign (zc[0]);
int nHZ = qssData->nHZ[i];
for (k = 0; k < nHZ; k++)
{
j = qssData->HZ[i][k];
FRW_nextEventTime (frw, qssModel, qssData, qssTime, j);
}
int nHD = qssData->nHD[i];
for (k = 0; k < nHD; k++)
{
j = qssData->HD[i][k];
e = t - qssTime->tx[j];
if (e > 0)
{
int cf0 = j * coeffs;
x[cf0] = evaluatePoly (cf0, e, x, order);
}
qssTime->tx[j] = t;
FRW_recomputeDerivative (frw, qssModel, qssData, qssTime, j);
}
if (nHD)
{
QA_recomputeNextTimes (quantizer, nHD, qssData->HD[i], t,
qssTime->nextStateTime, x, qssData->lqu,
q);
}
}
FRW_nextEventTime (frw, qssModel, qssData, qssTime, i);
}
#ifdef DEBUG
if (settings->debug & SD_DBG_InitValues)
{
SD_print (simulationLog, "done.");
SD_print (simulationLog, "Initialize state variables time...");
}
#endif
forUL = qssData->states;
for (i = 0; i < forUL; i++)
{
QA_recomputeNextTime (quantizer, i, qssTime->time, qssTime->nextStateTime,
x, qssData->lqu, q);
}
#ifdef DEBUG
if (settings->debug & SD_DBG_InitValues)
{
SD_print (simulationLog, "done.");
SD_print (simulationLog, "Initialize output...");
}
#endif
simulator->log = OUT_Output (qssData, qssTime, output);
#ifdef DEBUG
if (settings->debug & SD_DBG_InitValues)
{
SD_print (simulationLog, "done.");
SD_print (simulationLog, "Initialize time...");
}
#endif
simulator->scheduler = SC_Scheduler (qssData, qssTime);
#ifdef DEBUG
if (settings->debug & SD_DBG_InitValues)
{
SD_print (simulationLog, "done.");
}
if (settings->debug & SD_DBG_VarChanges)
{
SD_setSimulationLogVariables(simulationLog, qssData->states, qssData->events);
}
#endif
QSS_SEQ_logMemory (simulator);
getTime (simulator->sTime);
subTime (simulator->sTime, simulator->iTime);
simulator->initTime += getTimeValue (simulator->sTime);
}
/* check and handle received messages */
void
handle(RunTimeOpts *rtOpts, PtpClock *ptpClock)
{
int ret;
ssize_t length;
Boolean isFromSelf;
TimeInternal time = { 0, 0 };
if (!ptpClock->message_activity) {
ret = netSelect(0, &ptpClock->netPath);
if (ret < 0) {
PERROR("failed to poll sockets");
toState(PTP_FAULTY, rtOpts, ptpClock);
return;
} else if (!ret) {
/* DBGV("handle: nothing\n"); */
return;
}
/* else length > 0 */
}
DBGV("handle: something\n");
/* TODO: this should be based on the select actual FDs (if(FD_ISSET(...)) */
length = netRecvEvent(ptpClock->msgIbuf, &time, &ptpClock->netPath);
if (length < 0) {
PERROR("failed to receive on the event socket");
toState(PTP_FAULTY, rtOpts, ptpClock);
return;
} else if (!length) {
length = netRecvGeneral(ptpClock->msgIbuf, &time,
&ptpClock->netPath);
if (length < 0) {
PERROR("failed to receive on the general socket");
toState(PTP_FAULTY, rtOpts, ptpClock);
return;
} else if (!length)
return;
}
/*
* make sure we use the TAI to UTC offset specified, if the master is sending the UTC_VALID bit
*
*
* On the slave, all timestamps that we handle here have been collected by our local clock (loopback+kernel-level timestamp)
* This includes delayReq just send, and delayResp, when it arrives.
*
* these are then adjusted to the same timebase of the Master (+34 leap seconds, as of 2011)
*
*/
DBGV("__UTC_offset: %d %d \n", ptpClock->currentUtcOffsetValid, ptpClock->currentUtcOffset);
if (ptpClock->currentUtcOffsetValid) {
time.seconds += ptpClock->currentUtcOffset;
}
ptpClock->message_activity = TRUE;
if (length < HEADER_LENGTH) {
ERROR("message shorter than header length\n");
toState(PTP_FAULTY, rtOpts, ptpClock);
return;
}
msgUnpackHeader(ptpClock->msgIbuf, &ptpClock->msgTmpHeader);
if (ptpClock->msgTmpHeader.versionPTP != ptpClock->versionNumber) {
DBG2("ignore version %d message\n", ptpClock->msgTmpHeader.versionPTP);
return;
}
if(ptpClock->msgTmpHeader.domainNumber != ptpClock->domainNumber) {
DBG2("ignore message from domainNumber %d\n", ptpClock->msgTmpHeader.domainNumber);
return;
}
/*Spec 9.5.2.2*/
isFromSelf = (ptpClock->portIdentity.portNumber == ptpClock->msgTmpHeader.sourcePortIdentity.portNumber
&& !memcmp(ptpClock->msgTmpHeader.sourcePortIdentity.clockIdentity, ptpClock->portIdentity.clockIdentity, CLOCK_IDENTITY_LENGTH));
/*
* subtract the inbound latency adjustment if it is not a loop
* back and the time stamp seems reasonable
*/
if (!isFromSelf && time.seconds > 0)
subTime(&time, &time, &rtOpts->inboundLatency);
#ifdef PTPD_DBG
/* easy display of received messages */
char *st;
switch(ptpClock->msgTmpHeader.messageType)
{
case ANNOUNCE:
st = "Announce";
break;
case SYNC:
st = "Sync";
break;
case FOLLOW_UP:
st = "FollowUp";
break;
case DELAY_REQ:
st = "DelayReq";
break;
case DELAY_RESP:
st = "DelayResp";
break;
default:
st = "Unk";
break;
}
DBG(" ==> %s received\n", st);
#endif
/*
* on the table below, note that only the event messsages are passed the local time,
* (collected by us by loopback+kernel TS, and adjusted with UTC seconds
*
* (SYNC / DELAY_REQ / PDELAY_REQ / PDELAY_RESP)
*/
switch (ptpClock->msgTmpHeader.messageType)
{
case ANNOUNCE:
handleAnnounce(&ptpClock->msgTmpHeader, ptpClock->msgIbuf,
length, isFromSelf, rtOpts, ptpClock);
break;
case SYNC:
handleSync(&ptpClock->msgTmpHeader, ptpClock->msgIbuf,
length, &time, isFromSelf, rtOpts, ptpClock);
break;
case FOLLOW_UP:
handleFollowUp(&ptpClock->msgTmpHeader, ptpClock->msgIbuf,
length, isFromSelf, rtOpts, ptpClock);
break;
case DELAY_REQ:
handleDelayReq(&ptpClock->msgTmpHeader, ptpClock->msgIbuf,
length, &time, isFromSelf, rtOpts, ptpClock);
break;
case PDELAY_REQ:
handlePDelayReq(&ptpClock->msgTmpHeader, ptpClock->msgIbuf,
length, &time, isFromSelf, rtOpts, ptpClock);
break;
case DELAY_RESP:
handleDelayResp(&ptpClock->msgTmpHeader, ptpClock->msgIbuf,
length, isFromSelf, rtOpts, ptpClock);
break;
case PDELAY_RESP:
handlePDelayResp(&ptpClock->msgTmpHeader, ptpClock->msgIbuf,
&time, length, isFromSelf, rtOpts, ptpClock);
break;
case PDELAY_RESP_FOLLOW_UP:
handlePDelayRespFollowUp(&ptpClock->msgTmpHeader,
ptpClock->msgIbuf, length,
isFromSelf, rtOpts, ptpClock);
break;
case MANAGEMENT:
handleManagement(&ptpClock->msgTmpHeader, ptpClock->msgIbuf,
length, isFromSelf, rtOpts, ptpClock);
break;
case SIGNALING:
handleSignaling(&ptpClock->msgTmpHeader, ptpClock->msgIbuf,
length, isFromSelf, rtOpts, ptpClock);
break;
default:
DBG("handle: unrecognized message\n");
break;
}
if (rtOpts->displayPackets)
msgDump(ptpClock);
}
static double
feed (PIservo* self, Integer32 input, double tau) {
TimeInternal now, delta;
Boolean runningMaxOutput;
self->input = input;
switch(self->tauMethod) {
case DT_MEASURED:
getSystemClock()->getTimeMonotonic(getSystemClock(), &now);
if(isTimeZero(&self->lastUpdate)) {
self->tau = 1.0;
} else {
subTime(&delta, &now, &self->lastUpdate);
self->tau = delta.seconds + delta.nanoseconds / 1E9;
}
if(self->tau > (self->maxTau * tau)) {
self->tau = (self->maxTau + 0.0) * tau;
}
break;
case DT_CONSTANT:
self->tau = tau;
break;
default:
self->tau = 1.0;
}
if(self->tau <= ZEROF) {
self->tau = 1.0;
}
self->integral = clampDouble(self->integral, self->maxOutput);
self->output = clampDouble(self->output, self->maxOutput);
if (self->kP < 0.000001)
self->kP = 0.000001;
if (self->kI < 0.000001)
self->kI = 0.000001;
self->integral += (self->tau / self->delayFactor) * ((input + 0.0 ) * self->kI);
self->output = (self->kP * (input + 0.0) ) + self->integral;
self->integral = clampDouble(self->integral, self->maxOutput);
self->output = clampDouble(self->output, self->maxOutput);
runningMaxOutput = (fabs(self->output) >= self->maxOutput);
if(self->controller) {
DBG("%s tau %.09f input %d fabs %f out %f, mo %f\n", self->controller->name, self->tau, input, fabs(self->output), self->output, self->maxOutput);
}
if(runningMaxOutput && !self->runningMaxOutput) {
WARNING(THIS_COMPONENT"Clock %s servo now running at maximum output\n", self->controller->name);
}
self->runningMaxOutput = runningMaxOutput;
if(self->tauMethod == DT_MEASURED) {
self->lastUpdate = now;
}
self->_updated = TRUE;
self->_lastInput = self->input;
self->_lastOutput = self->output;
return self->output;
}
/* check and handle received messages */
void handle(PtpClock *ptpClock)
{
int ret;
ssize_t length;
Boolean isFromSelf;
Boolean isEvent;
Boolean badTime = FALSE;
TimeInternal time = { 0, 0 };
if(!ptpClock->message_activity)
{
ret = netSelect(0, ptpClock);
if(ret < 0)
{
PERROR("failed to poll sockets");
toState(PTP_FAULTY, ptpClock);
return;
}
else if(!ret)
{
DBGV("handle: nothing\n");
return;
}
/* else length > 0 */
}
DBGV("handle: something\n");
isEvent = TRUE;
length = netRecvEvent(ptpClock->msgIbuf,
ptpClock->delayedTiming ? NULL : &time,
ptpClock);
if(length < 0)
{
PERROR("failed to receive on the event socket");
toState(PTP_FAULTY, ptpClock);
return;
}
else if(!length)
{
isEvent = FALSE;
length = netRecvGeneral(ptpClock->msgIbuf, ptpClock);
if(length < 0)
{
PERROR("failed to receive on the general socket");
toState(PTP_FAULTY, ptpClock);
return;
}
else if(!length)
return;
}
ptpClock->message_activity = TRUE;
if(!msgPeek(ptpClock->msgIbuf, length))
return;
if(length < HEADER_LENGTH)
{
ERROR("message shorter than header length\n");
toState(PTP_FAULTY, ptpClock);
return;
}
msgUnpackHeader(ptpClock->msgIbuf, &ptpClock->msgTmpHeader);
if(isEvent && ptpClock->delayedTiming)
{
/* query hardware for matching receive time stamp */
if(!getReceiveTime(&time, ptpClock->msgTmpHeader.sourceUuid, ptpClock->msgTmpHeader.sequenceId, ptpClock))
{
/*
* Incoming packets without hardware time stamp cannot be ignored outright because
* a master might only be able to time stamp DelayReq packets; ignoring the Sync
* packets from another, better clock would break the clock selection protocol.
* Therefore set system time as fallback and decide below what to do.
*/
DBGV("*** message with no time stamp ***\n");
getTime(&time, ptpClock);
badTime = TRUE;
}
}
DBGV("%s Receipt of Message\n"
" version %d\n"
" type %d\n"
" uuid %02hhx:%02hhx:%02hhx:%02hhx:%02hhx:%02hhx\n"
" sequence %d\n"
" time %us %dns\n",
isEvent ? "event" : "control",
ptpClock->msgTmpHeader.versionPTP,
ptpClock->msgTmpHeader.control,
ptpClock->msgTmpHeader.sourceUuid[0], ptpClock->msgTmpHeader.sourceUuid[1],
ptpClock->msgTmpHeader.sourceUuid[2], ptpClock->msgTmpHeader.sourceUuid[3],
ptpClock->msgTmpHeader.sourceUuid[4], ptpClock->msgTmpHeader.sourceUuid[5],
ptpClock->msgTmpHeader.sequenceId,
time.seconds, time.nanoseconds);
if(ptpClock->msgTmpHeader.versionPTP != VERSION_PTP)
{
DBGV("ignore version %d message\n", ptpClock->msgTmpHeader.versionPTP);
return;
}
if( memcmp(ptpClock->msgTmpHeader.subdomain, ptpClock->subdomain_name,
PTP_SUBDOMAIN_NAME_LENGTH) )
{
DBGV("ignore message from subdomain %s\n", ptpClock->msgTmpHeader.subdomain);
return;
}
isFromSelf = ptpClock->msgTmpHeader.sourceCommunicationTechnology == ptpClock->port_communication_technology
&& ptpClock->msgTmpHeader.sourcePortId == ptpClock->port_id_field
&& !memcmp(ptpClock->msgTmpHeader.sourceUuid, ptpClock->port_uuid_field, PTP_UUID_LENGTH);
/* subtract the inbound latency adjustment if it is not a loop back and the
time stamp seems reasonable */
if(!isFromSelf && time.seconds > 0)
subTime(&time, &time, &ptpClock->runTimeOpts.inboundLatency);
switch(ptpClock->msgTmpHeader.control)
{
case PTP_SYNC_MESSAGE:
handleSync(&ptpClock->msgTmpHeader, ptpClock->msgIbuf, length, &time, badTime, isFromSelf, ptpClock);
break;
case PTP_FOLLOWUP_MESSAGE:
handleFollowUp(&ptpClock->msgTmpHeader, ptpClock->msgIbuf, length, isFromSelf, ptpClock);
break;
case PTP_DELAY_REQ_MESSAGE:
handleDelayReq(&ptpClock->msgTmpHeader, ptpClock->msgIbuf, length, &time, badTime, isFromSelf, ptpClock);
break;
case PTP_DELAY_RESP_MESSAGE:
handleDelayResp(&ptpClock->msgTmpHeader, ptpClock->msgIbuf, length, isFromSelf, ptpClock);
break;
case PTP_MANAGEMENT_MESSAGE:
handleManagement(&ptpClock->msgTmpHeader, ptpClock->msgIbuf, length, isFromSelf, ptpClock);
break;
default:
DBG("handle: unrecognized message\n");
break;
}
}
/* check and handle received messages */
void handle(RunTimeOpts *rtOpts, PtpClock *ptpClock)
{
int ret;
ssize_t length;
Boolean isFromSelf;
TimeInternal time = { 0, 0 };
TimeInternal now = { 0, 0 };
if(!ptpClock->message_activity)
{
ret = netSelect(0, &ptpClock->netPath);
if(ret < 0)
{
PERROR("failed to poll sockets");
toState(PTP_FAULTY, rtOpts, ptpClock);
return;
}
else if(!ret)
{
#if 0
DBGV("handle: nothing\n");
#endif
return;
}
/* else length > 0 */
}
DBGV("handle: something\n");
length = netRecvEvent(ptpClock->msgIbuf, &time, &ptpClock->netPath);
getTime(&now);
DBG("Time is %ds %dns\n", now.seconds, now.nanoseconds);
if(length < 0)
{
PERROR("failed to receive on the event socket");
toState(PTP_FAULTY, rtOpts, ptpClock);
return;
}
else if(!length)
{
length = netRecvGeneral(ptpClock->msgIbuf, &ptpClock->netPath);
if(length < 0)
{
PERROR("failed to receive on the general socket");
toState(PTP_FAULTY, rtOpts, ptpClock);
return;
}
else if(!length)
return;
}
ptpClock->message_activity = TRUE;
if(!msgPeek(ptpClock->msgIbuf, length))
return;
if(length < HEADER_LENGTH)
{
ERROR("message shorter than header length\n");
toState(PTP_FAULTY, rtOpts, ptpClock);
return;
}
msgUnpackHeader(ptpClock->msgIbuf, &ptpClock->msgTmpHeader);
DBG("event Receipt of Message\n type %d\n"
" uuid %02x:%02x:%02x:%02x:%02x:%02x\n"
" sequence %d\n time %us %dns\n",
ptpClock->msgTmpHeader.control,
ptpClock->msgTmpHeader.sourceUuid[0], ptpClock->msgTmpHeader.sourceUuid[1], ptpClock->msgTmpHeader.sourceUuid[2],
ptpClock->msgTmpHeader.sourceUuid[3], ptpClock->msgTmpHeader.sourceUuid[4], ptpClock->msgTmpHeader.sourceUuid[5],
ptpClock->msgTmpHeader.sequenceId,
time.seconds, time.nanoseconds);
isFromSelf = ptpClock->msgTmpHeader.sourceCommunicationTechnology == ptpClock->port_communication_technology
&& ptpClock->msgTmpHeader.sourcePortId == ptpClock->port_id_field
&& !memcmp(ptpClock->msgTmpHeader.sourceUuid, ptpClock->port_uuid_field, PTP_UUID_LENGTH);
/* subtract the inbound latency adjustment if it is not a loop back and the
time stamp seems reasonable */
if(!isFromSelf && time.seconds > 0)
subTime(&time, &time, &rtOpts->inboundLatency);
switch(ptpClock->msgTmpHeader.control)
{
case PTP_SYNC_MESSAGE:
handleSync(&ptpClock->msgTmpHeader, ptpClock->msgIbuf, length, &time, isFromSelf, rtOpts, ptpClock);
break;
case PTP_FOLLOWUP_MESSAGE:
handleFollowUp(&ptpClock->msgTmpHeader, ptpClock->msgIbuf, length, isFromSelf, rtOpts, ptpClock);
break;
case PTP_DELAY_REQ_MESSAGE:
handleDelayReq(&ptpClock->msgTmpHeader, ptpClock->msgIbuf, length, &time, isFromSelf, rtOpts, ptpClock);
break;
case PTP_DELAY_RESP_MESSAGE:
handleDelayResp(&ptpClock->msgTmpHeader, ptpClock->msgIbuf, length, isFromSelf, rtOpts, ptpClock);
break;
case PTP_MANAGEMENT_MESSAGE:
handleManagement(&ptpClock->msgTmpHeader, ptpClock->msgIbuf, length, isFromSelf, rtOpts, ptpClock);
break;
default:
DBG("handle: unrecognized message\n");
break;
}
}
/* check and handle received messages */
static void handle(PtpClock *ptpClock)
{
int ret;
Boolean isFromSelf;
TimeInternal time = { 0, 0 };
if (FALSE == ptpClock->messageActivity)
{
ret = netSelect(&ptpClock->netPath, 0);
if (ret < 0)
{
ERROR("handle: failed to poll sockets\n");
toState(ptpClock, PTP_FAULTY);
return;
}
else if (!ret)
{
DBGVV("handle: nothing\n");
return;
}
}
DBGVV("handle: something\n");
//msgUnpackHeader(ptpClock->msgIbuf, &ptpClock->msgTmpHeader);//**********************************************/
ptpClock->msgIbufLength = netRecvEvent(&ptpClock->netPath, ptpClock->msgIbuf, &time);
/* local time is not UTC, we can calculate UTC on demand, otherwise UTC time is not used */
/* time.seconds += ptpClock->timePropertiesDS.currentUtcOffset; */
if (ptpClock->msgIbufLength < 0)
{
ERROR("handle: failed to receive on the event socket\n");
toState(ptpClock, PTP_FAULTY);
return;
}
else if (!ptpClock->msgIbufLength)
{
ptpClock->msgIbufLength = netRecvGeneral(&ptpClock->netPath, ptpClock->msgIbuf, &time);
if (ptpClock->msgIbufLength < 0)
{
ERROR("handle: failed to receive on the general socket\n");
toState(ptpClock, PTP_FAULTY);
return;
}
else if (!ptpClock->msgIbufLength)
return;
}
ptpClock->messageActivity = TRUE;
if (ptpClock->msgIbufLength < HEADER_LENGTH)
{
ERROR("handle: message shorter than header length\n");
toState(ptpClock, PTP_FAULTY);
return;
}
msgUnpackHeader(ptpClock->msgIbuf, &ptpClock->msgTmpHeader);
if (ptpClock->msgTmpHeader.versionPTP != ptpClock->portDS.versionNumber)
{
DBGV("handle: ignore version %d message\n", ptpClock->msgTmpHeader.versionPTP);
return;
}
if (ptpClock->msgTmpHeader.domainNumber != ptpClock->defaultDS.domainNumber)
{
DBGV("handle: ignore message from domainNumber %d\n", ptpClock->msgTmpHeader.domainNumber);
return;
}
/*Spec 9.5.2.2*/
isFromSelf = isSamePortIdentity(
&ptpClock->portDS.portIdentity,
&ptpClock->msgTmpHeader.sourcePortIdentity);
/* subtract the inbound latency adjustment if it is not a loop back and the
time stamp seems reasonable */
if (!isFromSelf && time.seconds > 0)
subTime(&time, &time, &ptpClock->inboundLatency);
switch (ptpClock->msgTmpHeader.messageType)
{
case ANNOUNCE:
handleAnnounce(ptpClock, isFromSelf);
break;
case SYNC:
handleSync(ptpClock, &time, isFromSelf);
break;
case FOLLOW_UP:
handleFollowUp(ptpClock, isFromSelf);
break;
case DELAY_REQ:
handleDelayReq(ptpClock, &time, isFromSelf);
break;
case PDELAY_REQ:
handlePDelayReq(ptpClock, &time, isFromSelf);
break;
case DELAY_RESP:
handleDelayResp(ptpClock, isFromSelf);
break;
case PDELAY_RESP:
handlePDelayResp(ptpClock, &time, isFromSelf);
break;
case PDELAY_RESP_FOLLOW_UP:
handlePDelayRespFollowUp(ptpClock, isFromSelf);
break;
case MANAGEMENT:
handleManagement(ptpClock, isFromSelf);
break;
case SIGNALING:
handleSignaling(ptpClock, isFromSelf);
break;
default:
DBG("handle: unrecognized message %d\n", ptpClock->msgTmpHeader.messageType);
break;
}
}
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 {
void updateClock(RunTimeOpts *rtOpts, PtpClock *ptpClock)
{
Integer32 adj=0;
TimeInternal timeTmpA; // AKB: Added values for adjusting calc based on time to get time
TimeInternal timeTmpB;
TimeInternal timeTmpC;
TimeInternal timeTmpD;
TimeInternal timeTmpE;
TimeInternal timeTmpF;
Integer64 delta_time_calc;
DBGV("updateClock:\n");
if(ptpClock->offset_from_master.seconds)
{
/* if offset from master seconds is non-zero, then this is a "big jump:
* in time. Check Run Time options to see if we will reset the clock or
* set frequency adjustment to max to adjust the time
*/
if(!rtOpts->noAdjust)
{
if(!rtOpts->noResetClock)
{
if (!isNonZeroTime(&ptpClock->t1_sync_delta_time))
{
// Delta time is zero, so this is the first sync to capture and we'll do the major
// adjustment on the next sync instead of this one
//
// Store t1 and t2 times as current delta, next time we'll subtract
//
copyTime(&ptpClock->t1_sync_delta_time,
&ptpClock->t1_sync_tx_time
);
copyTime(&ptpClock->t2_sync_delta_time,
&ptpClock->t2_sync_rx_time
);
NOTIFY("updateClock: Storing current T1 and T2 values for later calc\n");
DBG("updateClock: Storing T1: %10ds %11dns\n",
ptpClock->t1_sync_delta_time.seconds,
ptpClock->t1_sync_delta_time.nanoseconds
);
DBG("updateClock: Storing T2: %10ds %11dns\n",
ptpClock->t2_sync_delta_time.seconds,
ptpClock->t2_sync_delta_time.nanoseconds
);
return;
}
// If we are here then t1 and t2 sync delta were set to previous t1 and t2
// values. Now we calculate the deltas
DBG("updateClock: Current T1: %10ds %11dns\n",
ptpClock->t1_sync_tx_time.seconds,
ptpClock->t1_sync_tx_time.nanoseconds
);
DBG("updateClock: Current T2: %10ds %11dns\n",
ptpClock->t2_sync_rx_time.seconds,
ptpClock->t2_sync_rx_time.nanoseconds
);
subTime(&ptpClock->t1_sync_delta_time,
&ptpClock->t1_sync_tx_time,
&ptpClock->t1_sync_delta_time
);
subTime(&ptpClock->t2_sync_delta_time,
&ptpClock->t2_sync_rx_time,
&ptpClock->t2_sync_delta_time
);
DBG("updateClock: Delta T1: %10ds %11dns\n",
ptpClock->t1_sync_delta_time.seconds,
ptpClock->t1_sync_delta_time.nanoseconds
);
DBG("updateClock: Delta T2: %10ds %11dns\n",
ptpClock->t2_sync_delta_time.seconds,
ptpClock->t2_sync_delta_time.nanoseconds
);
// Now we get the difference between the two time bases and store in the T2 time delta
// as we will use the T1 time as the divisor (so master clock drives the time)
subTime(&ptpClock->t2_sync_delta_time,
&ptpClock->t2_sync_delta_time,
&ptpClock->t1_sync_delta_time
);
DBG("updateClock: Delta T2 - Delta T1: %10ds %11dns\n",
ptpClock->t2_sync_delta_time.seconds,
ptpClock->t2_sync_delta_time.nanoseconds
);
delta_time_calc = getNanoseconds(&ptpClock->t2_sync_delta_time)
* 1000000000;
delta_time_calc /= getNanoseconds(&ptpClock->t1_sync_delta_time);
DBG("updateClock: Calculated Parts/billion: %d\n",
(int)delta_time_calc
);
/* clamp the accumulator to ADJ_FREQ_MAX for sanity */
if( delta_time_calc > ADJ_FREQ_MAX)
adj = ADJ_FREQ_MAX;
else if(delta_time_calc < -ADJ_FREQ_MAX)
adj = -ADJ_FREQ_MAX;
else
adj = (UInteger32)delta_time_calc;
NOTIFY("updateClock: Initial clock adjust: %d, base: %d\n",
adj,
ptpClock->baseAdjustValue
);
NOTIFY("updateClock: Offset from Master %ds.%9.9d seconds\n",
ptpClock->offset_from_master.seconds,
ptpClock->offset_from_master.nanoseconds
);
DBG( "updateClock: offset_from_master seconds != 0\n");
DBGV(" master-to-slave delay: %10ds %11dns\n",
ptpClock->master_to_slave_delay.seconds,
ptpClock->master_to_slave_delay.nanoseconds
);
DBGV(" slave-to-master delay: %10ds %11dns\n",
ptpClock->slave_to_master_delay.seconds,
ptpClock->slave_to_master_delay.nanoseconds
);
DBGV(" one-way delay: %10ds %11dns\n",
ptpClock->one_way_delay.seconds,
ptpClock->one_way_delay.nanoseconds
);
DBG( " offset from master: %10ds %11dns\n",
ptpClock->offset_from_master.seconds,
ptpClock->offset_from_master.nanoseconds
);
DBG( " observed drift: %10d\n",
ptpClock->observed_drift
);
getTime(&timeTmpA, ptpClock->current_utc_offset); // Get current time #1
getTime(&timeTmpB, ptpClock->current_utc_offset); // Get current time #2
subTime(&timeTmpC, // Calculate time #3, time elapsed between calls
&timeTmpB,
&timeTmpA
);
getTime(&timeTmpD, ptpClock->current_utc_offset); // Get current time #4
subTime(&timeTmpE, // Subtract calculated offset from master
&timeTmpD,
&ptpClock->offset_from_master
);
addTime(&timeTmpF, // Add calculated time to get timer value
&timeTmpE,
&timeTmpC
);
setTime(&timeTmpF, ptpClock->current_utc_offset); // Set new PTP time
DBGV(" get Time A : %10ds %11dns\n",
timeTmpA.seconds,
timeTmpA.nanoseconds
);
DBGV(" get Time B : %10ds %11dns\n",
timeTmpB.seconds,
timeTmpB.nanoseconds
);
DBGV(" calc Time C (B-A) : %10ds %11dns\n",
timeTmpC.seconds,
timeTmpC.nanoseconds
);
DBGV(" get Time D : %10ds %11dns\n",
timeTmpD.seconds,
timeTmpD.nanoseconds
);
DBGV(" offset from master : %10ds %11dns\n",
ptpClock->offset_from_master.seconds,
ptpClock->offset_from_master.nanoseconds
);
DBGV(" calc Time E (D+offset): %10ds %11dns\n",
timeTmpE.seconds,
timeTmpE.nanoseconds
);
DBGV(" calc Time F (E+C) : %10ds %11dns\n",
timeTmpF.seconds,
timeTmpF.nanoseconds
);
DBGV("updateClock: set time to Time F\n");
// Initialize clock variables based on run time options (rtOpts)
initClockVars(rtOpts, ptpClock);
// Adjust clock based on calculation from Delta T1, T2 times
adjFreq(ptpClock->baseAdjustValue - adj);
// Set initial observed drift to this calculated value
ptpClock->observed_drift = adj;
DBG( "updateClock: after initClock:\n");
DBGV(" master-to-slave delay: %10ds %11dns\n",
ptpClock->master_to_slave_delay.seconds,
ptpClock->master_to_slave_delay.nanoseconds
);
DBGV(" slave-to-master delay: %10ds %11dns\n",
ptpClock->slave_to_master_delay.seconds,
ptpClock->slave_to_master_delay.nanoseconds
);
DBG( " one-way delay: %10ds %11dns\n",
ptpClock->one_way_delay.seconds,
ptpClock->one_way_delay.nanoseconds
);
DBG( " offset from master: %10ds %11dns\n",
ptpClock->offset_from_master.seconds,
ptpClock->offset_from_master.nanoseconds
);
DBG( " observed drift: %10d\n",
ptpClock->observed_drift
);
}
else
{
/* Run time options indicate we can't reset the clock, so we slow
* it down or speed it up based on ADJ_FREQ_MAX adjustment rather
* than actually setting the time.
*/
adj = ptpClock->offset_from_master.nanoseconds > 0 ? ADJ_FREQ_MAX : -ADJ_FREQ_MAX;
adjFreq(ptpClock->baseAdjustValue - adj);
}
}
}
else
{
/* Offset from master is less than one second. Use the the PI controller
* to adjust the time
*/
DBGV("updateClock: using PI controller to update clock\n");
/* no negative or zero attenuation */
if(rtOpts->ap < 1)
rtOpts->ap = 1;
if(rtOpts->ai < 1)
rtOpts->ai = 1;
DBGV(" previous observed drift: %10d\n",
ptpClock->observed_drift
);
DBGV(" run time opts P: %10d\n",
rtOpts->ap
);
DBGV(" run time opts I: %10d\n",
rtOpts->ai
);
DBGV(" current observed drift: %d\n",
ptpClock->observed_drift
);
DBGV(" current offset %dns\n",
rtOpts->ai
);
/* the accumulator for the I component */
ptpClock->observed_drift += ptpClock->offset_from_master.nanoseconds/rtOpts->ai;
DBGV(" new observed drift (I): %d\n",
ptpClock->observed_drift
);
/* clamp the accumulator to ADJ_FREQ_MAX for sanity */
if( ptpClock->observed_drift > ADJ_FREQ_MAX)
ptpClock->observed_drift = ADJ_FREQ_MAX;
else if(ptpClock->observed_drift < -ADJ_FREQ_MAX)
ptpClock->observed_drift = -ADJ_FREQ_MAX;
DBGV(" clamped drift: %d\n",
ptpClock->observed_drift
);
adj = ptpClock->offset_from_master.nanoseconds/rtOpts->ap + ptpClock->observed_drift;
DBGV(" calculated adjust: %d\n",
adj
);
DBGV(" base adjust: %d\n",
ptpClock->baseAdjustValue
);
/* apply controller output as a clock tick rate adjustment */
if(!rtOpts->noAdjust)
{
DBGV(" calling adjFreq with: %d\n",
(ptpClock->baseAdjustValue-adj)
);
adjFreq(ptpClock->baseAdjustValue - adj);
if (rtOpts->rememberAdjustValue == TRUE)
{
if ( ptpClock->offset_from_master.nanoseconds <= 100
&& ptpClock->offset_from_master.nanoseconds >= -100
)
{
ptpClock->lastAdjustValue = -adj; // Store value if it gave a good clock
// result.
}
}
}
}
/* Display statistics (save to a file if -f specified) if run time option enabled */
if(rtOpts->displayStats)
displayStats(rtOpts, ptpClock);
DBGV(" offset from master: %10ds %11dns\n",
ptpClock->offset_from_master.seconds,
ptpClock->offset_from_master.nanoseconds
);
DBGV(" master-to-slave delay: %10ds %11dns\n",
ptpClock->master_to_slave_delay.seconds,
ptpClock->master_to_slave_delay.nanoseconds
);
DBGV(" slave-to-master delay: %10ds %11dns\n",
ptpClock->slave_to_master_delay.seconds,
ptpClock->slave_to_master_delay.nanoseconds
);
DBGV(" one-way delay: %10ds %11dns\n",
ptpClock->one_way_delay.seconds,
ptpClock->one_way_delay.nanoseconds
);
DBGV( " current observed drift: %10d\n",
ptpClock->observed_drift
);
DBGV(" clock adjust value: %10d\n",
(ptpClock->baseAdjustValue - adj)
);
}
void updateDelay(TimeInternal * send_time, // Delay Req. sent by slave time
TimeInternal * recv_time, // Delay Req. received by master time
one_way_delay_filter * owd_filt, // one way delay filter
RunTimeOpts * rtOpts, // run time options
PtpClock * ptpClock // PTP main data structure
)
{
Integer16 s;
DBGV("updateDelay:\n");
/* calc 'slave_to_master_delay' */
subTime(&ptpClock->slave_to_master_delay, // Result
recv_time, // Send time
send_time // minus Receive time
);
/* Correction for V2 Delay Resp (variable is zero if V1) */
subTime(&ptpClock->slave_to_master_delay,
&ptpClock->slave_to_master_delay,
&ptpClock->delay_resp_correction
);
/* update 'one_way_delay' */
addTime(&ptpClock->one_way_delay, // Result (divided by 2 later)
&ptpClock->master_to_slave_delay, // Master to slave delay (from sync/follow-up)
&ptpClock->slave_to_master_delay // Slave to master delay (from delay request/response)
);
halveTime(&ptpClock->one_way_delay); // Divide by 2 to get one way delay
// Assumes delay is symetrical
if(ptpClock->one_way_delay.seconds) // Check if delay is larger than one second
{
/* Delay is larger than one second, clear s_exp and timestamp
* of previously received sent time of Sync message (usually from
* preciseOriginTimestamp of follow up message) and return
*/
DBG("updateDelay: One way delay seconds != 0\n");
DBG("updateDelay: Clearing one way delay filter s_exp, nsec_prev\n");
owd_filt->s_exp = owd_filt->nsec_prev = 0;
return;
}
/* avoid overflowing filter */
s = rtOpts->s;
while(abs(owd_filt->y)>>(31-s))
--s;
DBGV("updateDelay: rtOpts->s: %d, s:%d\n",
rtOpts->s,
s
);
DBGV("updateDelay: current owd_filt->y: %d, s_exp: %d\n",
owd_filt->y,
owd_filt->s_exp
);
/* 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 'one_way_delay' */
owd_filt->y = (owd_filt->s_exp-1)
*owd_filt->y/owd_filt->s_exp
+ (ptpClock->one_way_delay.nanoseconds/2
+ owd_filt->nsec_prev/2
)
/owd_filt->s_exp;
/* Record previous one way delay nanosecond value
* and update it with value calculated above
*/
owd_filt->nsec_prev = ptpClock->one_way_delay.nanoseconds;
ptpClock->one_way_delay.nanoseconds = owd_filt->y;
DBGV("updateDelay: delay filter y:%d, s_exp:%d\n",
owd_filt->y,
owd_filt->s_exp
);
}
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
DASSL_integrate (SIM_simulator simulate)
{
CLC_simulator simulator = (CLC_simulator) simulate->state->sim;
clcData = simulator->data;
clcModel = simulator->model;
simOutput = simulator->output;
int i;
double t = clcData->it;
double tout;
const double _ft = clcData->ft;
double dQRel = clcData->dQRel[0];
double dQMin = clcData->dQMin[0];
double *_x = clcData->x;
double *rwork;
int is_sampled = simOutput->commInterval != CI_Step;
double step_size;
if (is_sampled)
{
step_size = simOutput->sampled->period[0];
}
const int num_steps = (
is_sampled ? ceil (_ft / step_size) + 2 : MAX_OUTPUT_POINTS);
double **solution = checkedMalloc (sizeof(double*) * simOutput->outputs);
double *solution_time = checkedMalloc (sizeof(double) * num_steps);
double **outvar = checkedMalloc (sizeof(double) * simOutput->outputs);
int info[20], lrw, liw, *iwork;
double *x, *dx, rel_tol = dQRel, abs_tol = dQMin;
int numofconstr = clcData->events, method_info = 0;
int *root_output;
int size = clcData->states;
int event_detected = 0;
x = checkedMalloc (sizeof(double) * clcData->states);
dx = checkedMalloc (sizeof(double) * clcData->states);
root_output = checkedMalloc (sizeof(int) * clcData->events);
lrw = 5000 + 15000 * clcData->states
+ /*clcData->states * clcData->states +*/8 * clcData->events;
rwork = checkedMalloc (sizeof(double) * lrw);
CLC_compute_outputs (simOutput, solution, num_steps);
for (i = 0; i < clcData->states; i++)
x[i] = _x[i];
cleanDoubleVector (dx, 0, clcData->states);
cleanVector (root_output, 0, clcData->events);
cleanVector (info, 0, 20);
if (!is_sampled)
{
info[2] = 1;
}
liw = 60040;
iwork = checkedMalloc (sizeof(int) * liw);
int percentage = 0;
// Save first step
CLC_save_step (simOutput, solution, solution_time, t,
clcData->totalOutputSteps, x, clcData->d, clcData->alg);
clcData->totalOutputSteps++;
getTime (simulator->sTime);
#ifdef SYNC_RT
setInitRealTime();
#endif
while (t < _ft)
{
if (!is_sampled)
{
tout = _ft;
}
else
{
if (!event_detected)
{
tout = t + step_size;
}
else
{
if (fabs (tout - t) < 1e-12)
{
CLC_save_step (simOutput, solution, solution_time, tout,
clcData->totalOutputSteps, x, clcData->d,
clcData->alg);
clcData->totalOutputSteps++;
tout = t + step_size;
}
}
event_detected = 0;
}
if (tout > _ft)
tout = _ft;
ddaskr_ (DASSL_model, &size, &t, x, dx, &tout, info, &rel_tol, &abs_tol,
&method_info, rwork, &lrw, iwork, &liw,
NULL,
NULL, NULL, NULL, DASSL_events, &numofconstr, root_output);
if (method_info < 0)
{
printf (
"Error: DASSL returned IDID = %d. Check DASSL documentation\n",
method_info);
exit (-1);
}
#ifdef SYNC_RT
/* Sync */
waitUntil(t);
#endif
if (method_info == 5)
{
CLC_handle_event (clcData, clcModel, x, root_output, t, iwork);
if (is_sampled)
event_detected = 1;
info[0] = 0;
}
if (!is_sampled)
{
CLC_save_step (simOutput, solution, solution_time, t,
clcData->totalOutputSteps, x, clcData->d,
clcData->alg);
clcData->totalOutputSteps++;
}
else
{
if (!event_detected)
{
if (fabs (tout - solution_time[clcData->totalOutputSteps - 1])
> step_size / 10)
{
CLC_save_step (simOutput, solution, solution_time, tout,
clcData->totalOutputSteps, x, clcData->d,
clcData->alg);
clcData->totalOutputSteps++;
}
}
else
{
}
}
if ((int) (t * 100 / _ft) > percentage)
{
percentage = 100 * t / _ft;
fprintf (stderr, "*%g", t);
fflush (stderr);
}
}
/*
if (!event_detected && is_sampled) {
if (solution_time[totalOutputSteps]<t) {
CLC_save_step(simOutput,solution,solution_time,t,totalOutputSteps,x, clcData->d);
totalOutputSteps++;
}
}
*/
clcData->totalSteps += iwork[10];
clcData->totalStepsDASSL += iwork[11];
clcData->totalJacobians += iwork[12];
clcData->totalCrossingEvaluations += iwork[35];
getTime (simulator->sTime);
subTime (simulator->sTime, simulator->iTime);
if (simulator->settings->debug == 0 || simulator->settings->debug > 1)
{
SD_print (simulator->simulationLog, "Simulation time (DASSL):");
SD_print (simulator->simulationLog, "----------------");
SD_print (simulator->simulationLog, "Miliseconds: %g",
getTimeValue (simulator->sTime));
SD_print (simulator->simulationLog, "Function evaluations: %llu",
clcData->funEvaluations);
//SD_print (simulator->simulationLog, "Scalar function evaluations: %d", clcData->scalarEvaluations);
//SD_print (simulator->simulationLog, "Zero Crossings : %d", clcData->zeroCrossings);
SD_print (simulator->simulationLog,
"Function evaluations (reported by DASSL): %d",
clcData->totalStepsDASSL);
SD_print (simulator->simulationLog, "Jacobian evaluations : %d",
clcData->totalJacobians);
SD_print (simulator->simulationLog, "Zero crossing evaluations : %d",
clcData->totalCrossingEvaluations);
SD_print (simulator->simulationLog, "Output steps: %d",
clcData->totalOutputSteps);
SD_print (simulator->simulationLog, "Simulation steps: %d",
clcData->totalSteps);
SD_print (simulator->simulationLog, "Events detected : %d",
clcData->totalEvents);
}
CLC_write_output (simOutput, solution, solution_time,
clcData->totalOutputSteps);
// To avoid QSS output
free (x);
free (dx);
free (outvar);
free (root_output);
free (solution_time);
free (rwork);
free (iwork);
for (i = 0; i < simOutput->outputs; i++)
{
free (solution[i]);
}
free (solution);
}
void updateOffset(TimeInternal * send_time, // Sync message reported Transmit time
TimeInternal * recv_time, // Sync message local Receive time
offset_from_master_filter * ofm_filt, // Offset from Master filter
RunTimeOpts * rtOpts, // Run Time Options
PtpClock * ptpClock // PTP main data structure
)
{
DBGV("updateOffset:\n");
/* calc 'master_to_slave_delay' */
subTime(&ptpClock->master_to_slave_delay, // Result: Master to slave delay
recv_time, // Recorded time of Sync message
send_time // minus Send time of Sync message (from follow-up)
);
/* Update for V2 corrections (set to zero if received Sync/Follow-up is from V1 MASTER) */
subTime(&ptpClock->master_to_slave_delay,
&ptpClock->master_to_slave_delay,
&ptpClock->sync_correction
);
subTime(&ptpClock->master_to_slave_delay,
&ptpClock->master_to_slave_delay,
&ptpClock->followup_correction
);
/* update 'offset_from_master' */
subTime(&ptpClock->offset_from_master, // Result: Offset from master
&ptpClock->master_to_slave_delay, // From above calculation
&ptpClock->one_way_delay // minus one way delay calc from Delay Request/response
);
if(ptpClock->offset_from_master.seconds)
{
/* cannot filter with secs, clear filter */
ofm_filt->nsec_prev = -1; /* AKB: Use invalid number to make sure next calc correct */
#ifdef CONFIG_MPC831X
/* set meter to max */
led_meter(255);
#endif
return;
}
/* filter 'offset_from_master' */
/*
* Offset from Master Filtering
* this uses a simple two sample average:
*
* y[x] = x[n]/2 + x[n-1]/2
*
*/
if (ofm_filt->nsec_prev != -1) /* AKB: Make sure previous timestamp is valid */
{
// Previous timestamp is valid, calculate new offset from master
// based on previous and current timestamps
ofm_filt->y = // Offset from master filter Y
ptpClock->offset_from_master.nanoseconds/2 // current / 2
+ ofm_filt->nsec_prev/2; // previous / 2
ofm_filt->nsec_prev = ptpClock->offset_from_master.nanoseconds;// Store current for next time
ptpClock->offset_from_master.nanoseconds = ofm_filt->y; // Set offset to current Y value
#ifdef CONFIG_MPC831X
if (abs(ptpClock->offset_from_master.nanoseconds) > 255)
{
/* set meter to max */
led_meter(255);
}
else
{
/* set meter based on nanoseconds between 0 and 255 absolute */
led_meter(abs(ptpClock->offset_from_master.nanoseconds));
}
#endif
}
else
{
// AKB: Previous timestamp is not valid, set filter Y to current value
ofm_filt->y = ptpClock->offset_from_master.nanoseconds;
ofm_filt->nsec_prev = ptpClock->offset_from_master.nanoseconds;
}
DBGV("updateOffset: offset filter y:%d\n", ofm_filt->y);
}
void
updateOffset(TimeInternal * send_time, TimeInternal * recv_time,
Filter * ofm_filt, RunTimeOpts * rtOpts, PtpClock * ptpClock, TimeInternal * correctionField)
{
DBGV("UTCOffset: %d | leap 59: %d | leap61: %d\n",
ptpClock->timePropertiesDS.currentUtcOffset,ptpClock->timePropertiesDS.leap59,ptpClock->timePropertiesDS.leap61);
/* updates paused, leap second pending - do nothing */
if(ptpClock->leapSecondInProgress)
return;
DBGV("==> updateOffset\n");
{
//perform basic checks, using only local variables
TimeInternal master_to_slave_delay;
/* calc 'master_to_slave_delay' */
subTime(&master_to_slave_delay, recv_time, send_time);
if (rtOpts->maxDelay) { /* If maxDelay is 0 then it's OFF */
if (master_to_slave_delay.seconds && rtOpts->maxDelay) {
INFO("updateOffset aborted, delay greater than 1"
" second.\n");
/* msgDump(ptpClock); */
return;
}
if (master_to_slave_delay.nanoseconds > rtOpts->maxDelay) {
INFO("updateOffset aborted, delay %d greater than "
"administratively set maximum %d\n",
master_to_slave_delay.nanoseconds,
rtOpts->maxDelay);
/* msgDump(ptpClock); */
return;
}
}
}
// used for stats feedback
ptpClock->char_last_msg='S';
/*
* The packet has passed basic checks, so we'll:
* - update the global delayMS variable
* - calculate a new filtered OFM
*/
#ifdef PTPD_STATISTICS
if (rtOpts->delayMSOutlierFilterEnabled) {
subTime(&ptpClock->rawDelayMS, recv_time, send_time);
if(!isDoublePeircesOutlier(ptpClock->delayMSRawStats, timeInternalToDouble(&ptpClock->rawDelayMS), rtOpts->delayMSOutlierFilterThreshold)) {
ptpClock->delayMSoutlier = FALSE;
ptpClock->delayMS = ptpClock->rawDelayMS;
} else {
ptpClock->delayMSoutlier = TRUE;
ptpClock->counters.delayMSOutliersFound++;
if(!rtOpts->delayMSOutlierFilterDiscard)
ptpClock->delayMS = doubleToTimeInternal(ptpClock->delayMSFiltered->mean);
}
} else {
subTime(&ptpClock->delayMS, recv_time, send_time);
}
#else
/* Used just for End to End mode. */
subTime(&ptpClock->delayMS, recv_time, send_time);
#endif
/* Take care about correctionField */
subTime(&ptpClock->delayMS,
&ptpClock->delayMS, correctionField);
#ifdef PTPD_STATISTICS
#endif
/* update 'offsetFromMaster' */
if (ptpClock->delayMechanism == P2P) {
subTime(&ptpClock->offsetFromMaster,
&ptpClock->delayMS,
&ptpClock->peerMeanPathDelay);
/* (End to End mode or disabled - if disabled, meanpath delay is zero) */
} else if (ptpClock->delayMechanism == E2E ||
ptpClock->delayMechanism == DELAY_DISABLED ) {
subTime(&ptpClock->offsetFromMaster,
&ptpClock->delayMS,
&ptpClock->meanPathDelay);
}
if (ptpClock->offsetFromMaster.seconds) {
/* cannot filter with secs, clear filter */
FilterClear(ofm_filt);
rtOpts->offset_first_updated = TRUE;
return;
}
FilterFeed(ofm_filt, &ptpClock->offsetFromMaster.nanoseconds);
DBGV("offset filter %d\n", ptpClock->offsetFromMaster.nanoseconds);
/* Apply the offset shift */
subTime(&ptpClock->offsetFromMaster, &ptpClock->offsetFromMaster,
&rtOpts->ofmShift);
/*
* Offset must have been computed at least one time before
* computing end to end delay
*/
rtOpts->offset_first_updated = TRUE;
}
void updatePathDelay(one_way_delay_filter *owd_filt, // one way delay filter
RunTimeOpts *rtOpts, // run time options
PtpClock *ptpClock // PTP main data structure
)
{
Integer16 s;
TimeInternal remote_time;
DBGV("updatePathDelay:\n");
DBGV(" t1 PDelay Req Tx time %10.10ds.%9.9dns\n",
ptpClock->t1_pdelay_req_tx_time.seconds,
ptpClock->t1_pdelay_req_tx_time.nanoseconds
);
DBGV(" t2 PDelay Req Rx time %10.10ds.%9.9dns\n",
ptpClock->t2_pdelay_req_rx_time.seconds,
ptpClock->t2_pdelay_req_rx_time.nanoseconds
);
DBGV(" t3 PDelay Resp Tx time %10.10ds.%9.9dns\n",
ptpClock->t3_pdelay_resp_tx_time.seconds,
ptpClock->t3_pdelay_resp_tx_time.nanoseconds
);
DBGV(" t4 PDelay Resp Rx time %10.10ds.%9.9dns\n",
ptpClock->t4_pdelay_resp_rx_time.seconds,
ptpClock->t4_pdelay_resp_rx_time.nanoseconds
);
DBGV(" PDelay Resp correction %10.10ds.%9.9dns\n",
ptpClock->pdelay_resp_correction.seconds,
ptpClock->pdelay_resp_correction.nanoseconds
);
DBGV(" PDelay Resp follow up %10.10ds.%9.9dns\n",
ptpClock->pdelay_followup_correction.seconds,
ptpClock->pdelay_followup_correction.nanoseconds
);
/* calc 'slave_to_master_delay' */
subTime(&ptpClock->one_way_delay, // Result
&ptpClock->t4_pdelay_resp_rx_time, // PDelay Response Receive time
&ptpClock->t1_pdelay_req_tx_time // minus PDelay Request Transmit time
);
DBGV(" (t4-t1) %10.10ds.%9.9dns\n",
ptpClock->one_way_delay.seconds,
ptpClock->one_way_delay.nanoseconds
);
subTime(&remote_time, // Result
&ptpClock->t3_pdelay_resp_tx_time, // PDelay Resp Transmit time (from responder)
&ptpClock->t2_pdelay_req_rx_time // minus PDelay Request Receive time (from responder)
);
DBGV(" (t3-t2) %10.10ds.%9.9dns\n",
remote_time.seconds,
remote_time.nanoseconds
);
subTime(&ptpClock->one_way_delay, // Result
&ptpClock->one_way_delay, // (T4-T1)
&remote_time // minus (T3-T2)
);
DBGV(" (t4-t1)-(t3-t2) %10.10ds.%9.9dns\n",
ptpClock->one_way_delay.seconds,
ptpClock->one_way_delay.nanoseconds
);
subTime(&ptpClock->one_way_delay, // Result
&ptpClock->one_way_delay, // Current Calculation
&ptpClock->pdelay_resp_correction // minus PDelay Resp Correction
);
DBGV(" minus 1st correction %10.10ds.%9.9dns\n",
ptpClock->one_way_delay.seconds,
ptpClock->one_way_delay.nanoseconds
);
subTime(&ptpClock->one_way_delay, // Result
&ptpClock->one_way_delay, // Current Calculation
&ptpClock->pdelay_followup_correction // minus PDelay Resp Correction
);
DBGV(" minus 2nd correction %10.10ds.%9.9dns\n",
ptpClock->one_way_delay.seconds,
ptpClock->one_way_delay.nanoseconds
);
halveTime(&ptpClock->one_way_delay);
DBGV(" divided by 2 %10.10ds.%9.9dns\n",
ptpClock->one_way_delay.seconds,
ptpClock->one_way_delay.nanoseconds
);
copyTime( &ptpClock->slave_to_master_delay, // Destination
&ptpClock->one_way_delay // Source
);
clearTime(&ptpClock->t1_pdelay_req_tx_time);
clearTime(&ptpClock->t2_pdelay_req_rx_time);
clearTime(&ptpClock->t3_pdelay_resp_tx_time);
clearTime(&ptpClock->t4_pdelay_resp_rx_time);
clearTime(&ptpClock->pdelay_resp_correction);
clearTime(&ptpClock->pdelay_followup_correction);
clearTime(&ptpClock->t1_sync_delta_time);
clearTime(&ptpClock->t2_sync_delta_time);
if(ptpClock->one_way_delay.seconds) // Check if delay is larger than one second
{
/* Delay is larger than one second, clear s_exp and timestamp
* of previously received sent time of Sync message (usually from
* preciseOriginTimestamp of follow up message) and return
*/
DBG("updatePathDelay: One way delay seconds != 0\n");
DBG("updatePathDelay: Clearing one way delay filter s_exp, nsec_prev\n");
owd_filt->s_exp = 0;
owd_filt->nsec_prev = 0;
return;
}
/* avoid overflowing filter */
s = rtOpts->s;
while(abs(owd_filt->y)>>(31-s))
--s;
DBGV("updatePathDelay: rtOpts->s: %d, s:%d\n",
rtOpts->s,
s
);
DBGV("updatePathDelay: current owd_filt->y: %d, s_exp: %d\n",
owd_filt->y,
owd_filt->s_exp
);
/* 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 'one_way_delay' */
owd_filt->y = (owd_filt->s_exp-1)
*owd_filt->y/owd_filt->s_exp
+ (ptpClock->one_way_delay.nanoseconds/2
+ owd_filt->nsec_prev/2
)
/owd_filt->s_exp;
/* Record previous one way delay nanosecond value
* and update it with value calculated above
*/
owd_filt->nsec_prev = ptpClock->one_way_delay.nanoseconds;
ptpClock->one_way_delay.nanoseconds = owd_filt->y;
DBGV("updatePathDelay: delay filter y:%d, s_exp:%d\n",
owd_filt->y,
owd_filt->s_exp
);
}
void
servo_perform_clock_step(RunTimeOpts * rtOpts, PtpClock * ptpClock)
{
if(rtOpts->noAdjust){
WARNING("Could not step clock - clock adjustment disabled\n");
return;
}
TimeInternal oldTime, newTime;
/*No need to reset the frequency offset: if we're far off, it will quickly get back to a high value */
getTime(&oldTime);
subTime(&newTime, &oldTime, &ptpClock->offsetFromMaster);
setTime(&newTime);
#ifdef HAVE_LINUX_RTC_H
if(rtOpts->setRtc) {
setRtc(&newTime);
}
#endif /* HAVE_LINUX_RTC_H */
initClock(rtOpts, ptpClock);
#ifdef HAVE_SYS_TIMEX_H
if(ptpClock->clockQuality.clockClass > 127)
restoreDrift(ptpClock, rtOpts, TRUE);
#endif /* HAVE_SYS_TIMEX_H */
ptpClock->servo.runningMaxOutput = FALSE;
toState(PTP_FAULTY, rtOpts, ptpClock); /* make a full protocol reset */
/* Major time change - need to inform utmp / wtmp */
if(oldTime.seconds != newTime.seconds) {
/* Add the old time entry to utmp/wtmp */
/* About as long as the ntpd implementation, but not any less ugly */
#ifdef HAVE_UTMPX_H
struct utmpx utx;
memset(&utx, 0, sizeof(utx));
strncpy(utx.ut_user, "date", sizeof(utx.ut_user));
#ifndef OTIME_MSG
strncpy(utx.ut_line, "|", sizeof(utx.ut_line));
#else
strncpy(utx.ut_line, OTIME_MSG, sizeof(utx.ut_line));
#endif /* OTIME_MSG */
#ifdef OLD_TIME
utx.ut_tv.tv_sec = oldTime.seconds;
utx.ut_tv.tv_usec = oldTime.nanoseconds / 1000;
utx.ut_type = OLD_TIME;
#else /* no ut_type */
utx.ut_time = oldTime.seconds;
#endif /* OLD_TIME */
/* ======== BEGIN OLD TIME EVENT - UTMPX / WTMPX =========== */
#ifdef HAVE_UTMPXNAME
utmpxname("/var/log/utmp");
#endif /* HAVE_UTMPXNAME */
setutxent();
pututxline(&utx);
endutxent();
#ifdef HAVE_UPDWTMPX
updwtmpx("/var/log/wtmp", &utx);
#endif /* HAVE_IPDWTMPX */
/* ======== END OLD TIME EVENT - UTMPX / WTMPX =========== */
#else /* NO UTMPX_H */
#ifdef HAVE_UTMP_H
struct utmp ut;
memset(&ut, 0, sizeof(ut));
strncpy(ut.ut_name, "date", sizeof(ut.ut_name));
#ifndef OTIME_MSG
strncpy(ut.ut_line, "|", sizeof(ut.ut_line));
#else
strncpy(ut.ut_line, OTIME_MSG, sizeof(ut.ut_line));
#endif /* OTIME_MSG */
#ifdef OLD_TIME
ut.ut_tv.tv_sec = oldTime.seconds;
ut.ut_tv.tv_usec = oldTime.nanoseconds / 1000;
ut.ut_type = OLD_TIME;
#else /* no ut_type */
ut.ut_time = oldTime.seconds;
#endif /* OLD_TIME */
/* ======== BEGIN OLD TIME EVENT - UTMP / WTMP =========== */
#ifdef HAVE_UTMPNAME
utmpname(UTMP_FILE);
#endif /* HAVE_UTMPNAME */
#ifdef HAVE_SETUTENT
setutent();
#endif /* HAVE_SETUTENT */
#ifdef HAVE_PUTUTLINE
pututline(&ut);
#endif /* HAVE_PUTUTLINE */
#ifdef HAVE_ENDUTENT
endutent();
#endif /* HAVE_ENDUTENT */
#ifdef HAVE_UTMPNAME
utmpname(WTMP_FILE);
#endif /* HAVE_UTMPNAME */
#ifdef HAVE_SETUTENT
setutent();
#endif /* HAVE_SETUTENT */
#ifdef HAVE_PUTUTLINE
pututline(&ut);
#endif /* HAVE_PUTUTLINE */
#ifdef HAVE_ENDUTENT
endutent();
#endif /* HAVE_ENDUTENT */
/* ======== END OLD TIME EVENT - UTMP / WTMP =========== */
#endif /* HAVE_UTMP_H */
#endif /* HAVE_UTMPX_H */
/* Add the new time entry to utmp/wtmp */
#ifdef HAVE_UTMPX_H
memset(&utx, 0, sizeof(utx));
strncpy(utx.ut_user, "date", sizeof(utx.ut_user));
#ifndef NTIME_MSG
strncpy(utx.ut_line, "}", sizeof(utx.ut_line));
#else
strncpy(utx.ut_line, NTIME_MSG, sizeof(utx.ut_line));
#endif /* NTIME_MSG */
#ifdef NEW_TIME
utx.ut_tv.tv_sec = newTime.seconds;
utx.ut_tv.tv_usec = newTime.nanoseconds / 1000;
utx.ut_type = NEW_TIME;
#else /* no ut_type */
utx.ut_time = newTime.seconds;
#endif /* NEW_TIME */
/* ======== BEGIN NEW TIME EVENT - UTMPX / WTMPX =========== */
#ifdef HAVE_UTMPXNAME
utmpxname("/var/log/utmp");
#endif /* HAVE_UTMPXNAME */
setutxent();
pututxline(&utx);
endutxent();
#ifdef HAVE_UPDWTMPX
updwtmpx("/var/log/wtmp", &utx);
#endif /* HAVE_UPDWTMPX */
/* ======== END NEW TIME EVENT - UTMPX / WTMPX =========== */
#else /* NO UTMPX_H */
#ifdef HAVE_UTMP_H
memset(&ut, 0, sizeof(ut));
strncpy(ut.ut_name, "date", sizeof(ut.ut_name));
#ifndef NTIME_MSG
strncpy(ut.ut_line, "}", sizeof(ut.ut_line));
#else
strncpy(ut.ut_line, NTIME_MSG, sizeof(ut.ut_line));
#endif /* NTIME_MSG */
#ifdef NEW_TIME
ut.ut_tv.tv_sec = newTime.seconds;
ut.ut_tv.tv_usec = newTime.nanoseconds / 1000;
ut.ut_type = NEW_TIME;
#else /* no ut_type */
ut.ut_time = newTime.seconds;
#endif /* NEW_TIME */
/* ======== BEGIN NEW TIME EVENT - UTMP / WTMP =========== */
#ifdef HAVE_UTMPNAME
utmpname(UTMP_FILE);
#endif /* HAVE_UTMPNAME */
#ifdef HAVE_SETUTENT
setutent();
#endif /* HAVE_SETUTENT */
#ifdef HAVE_PUTUTLINE
pututline(&ut);
#endif /* HAVE_PUTUTLINE */
#ifdef HAVE_ENDUTENT
endutent();
#endif /* HAVE_ENDUTENT */
#ifdef HAVE_UTMPNAME
utmpname(WTMP_FILE);
#endif /* HAVE_UTMPNAME */
#ifdef HAVE_SETUTENT
setutent();
#endif /* HAVE_SETUTENT */
#ifdef HAVE_PUTUTLINE
pututline(&ut);
#endif /* HAVE_PUTUTLINE */
#ifdef HAVE_ENDUTENT
endutent();
#endif /* HAVE_ENDUTENT */
/* ======== END NEW TIME EVENT - UTMP / WTMP =========== */
#endif /* HAVE_UTMP_H */
#endif /* HAVE_UTMPX_H */
}
}
static Boolean
setTime (ClockDriver *self, TimeInternal *time, Boolean force) {
GET_CONFIG_CLOCKDRIVER(self, myConfig, unix);
TimeInternal oldTime, delta;
getTime(self, &oldTime);
subTime(&delta, &oldTime, time);
if(self->config.readOnly) {
return FALSE;
}
if(force) {
self->lockedUp = FALSE;
}
if(!force && !self->config.negativeStep && isTimeNegative(&delta)) {
CRITICAL(THIS_COMPONENT"Cannot step Unix clock %s backwards\n", self->name);
CRITICAL(THIS_COMPONENT"Manual intervention required or SIGUSR1 to force %s clock step\n", self->name);
self->lockedUp = TRUE;
self->setState(self, CS_NEGSTEP);
return FALSE;
}
#if defined(_POSIX_TIMERS) && (_POSIX_TIMERS > 0)
struct timespec tp;
tp.tv_sec = time->seconds;
tp.tv_nsec = time->nanoseconds;
if(tp.tv_sec == 0) {
ERROR(THIS_COMPONENT"Unix clock driver %s: cannot set time to zero seconds\n", self->name);
return FALSE;
}
if(tp.tv_sec <= 0) {
ERROR(THIS_COMPONENT"Unic clock driver %s: cannot set time to a negative value %d\n", self->name, tp.tv_sec);
return FALSE;
}
#else
struct timeval tv;
tv.tv_sec = time->seconds;
tv.tv_usec = time->nanoseconds / 1000;
if(tv.tv_sec == 0) {
ERROR(THIS_COMPONENT"Unix clock %s: cannot set time to zero seconds\n", self->name);
return FALSE;
}
if(tv.tv_sec < 0) {
ERROR(THIS_COMPONENT"Unic clock %s: cannot set time to a negative value %d\n", self->name, tv.tv_sec);
return FALSE;
}
#endif /* _POSIX_TIMERS */
#if defined(_POSIX_TIMERS) && (_POSIX_TIMERS > 0)
if (clock_settime(CLOCK_REALTIME, &tp) < 0) {
PERROR(THIS_COMPONENT"Could not set system time");
return FALSE;
}
addTime(&_stepAccumulator, &_stepAccumulator, &delta);
#else
settimeofday(&tv, 0);
addTime(&_stepAccumulator, &_stepAccumulator, &delta);
#endif /* _POSIX_TIMERS */
if(oldTime.seconds != time->seconds) {
updateXtmp_unix(oldTime, *time);
if(myConfig->setRtc) {
setRtc(self, time);
}
}
self->_stepped = TRUE;
struct timespec tmpTs = { time->seconds,0 };
char timeStr[MAXTIMESTR];
strftime(timeStr, MAXTIMESTR, "%x %X", localtime(&tmpTs.tv_sec));
NOTICE(THIS_COMPONENT"Unix clock %s: stepped the system clock to: %s.%d\n", self->name,
timeStr, time->nanoseconds);
self->setState(self, CS_FREERUN);
return TRUE;
}
double
runPIservo(PIservo* servo, const Integer32 input)
{
double dt;
TimeInternal now, delta;
switch (servo->dTmethod) {
case DT_MEASURED:
getTime(&now);
if(servo->lastUpdate.seconds == 0 &&
servo->lastUpdate.nanoseconds == 0) {
dt = 1.0;
} else {
subTime(&delta, &now, &servo->lastUpdate);
dt = delta.nanoseconds / 1E9;
}
/* Don't use dT > 2 * target update interval */
if(dt > 2 * pow(2, servo->logdT))
dt = 2 * pow(2, servo->logdT);
break;
case DT_CONSTANT:
dt = pow(2, servo->logdT);
break;
case DT_NONE:
default:
dt = 1.0;
break;
}
if(dt <= 0.0)
dt = 1.0;
servo->input = input;
if (servo->kP < 0.000001)
servo->kP = 0.000001;
if (servo->kI < 0.000001)
servo->kI = 0.000001;
servo->observedDrift +=
dt * ((input + 0.0 ) * servo->kI);
if(servo->observedDrift >= servo->maxOutput) {
servo->observedDrift = servo->maxOutput;
servo->runningMaxOutput = TRUE;
}
else if(servo->observedDrift <= -servo->maxOutput) {
servo->observedDrift = -servo->maxOutput;
servo->runningMaxOutput = TRUE;
} else {
servo->runningMaxOutput = FALSE;
}
servo->output = (servo->kP * (input + 0.0) ) + servo->observedDrift;
if(servo->dTmethod == DT_MEASURED)
servo->lastUpdate = now;
DBGV("Servo dt: %.09f, input (ofm): %d, output(adj): %.09f, accumulator (observed drift): %.09f\n", dt, input, servo->output, servo->observedDrift);
return -servo->output;
}