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;
}
int
adjtime (struct timeval *delta, struct timeval *olddelta)
{
double delta_nsec;
double delta_nsec_old;
struct _clockadjust adj;
struct _clockadjust oldadj;
/*
* How many nanoseconds are we adjusting?
*/
if (delta != NULL)
delta_nsec = 1e9 * (long)delta->tv_sec + 1e3 * delta->tv_usec;
else
delta_nsec = 0;
/*
* Build the adjust structure and call ClockAdjust()
*/
if (delta_nsec != 0)
{
struct _clockperiod period;
long count;
long increment;
long increment_limit;
int isneg = 0;
/*
* Convert to absolute value for future processing
*/
if (delta_nsec < 0)
{
isneg = 1;
delta_nsec = -delta_nsec;
}
/*
* Get the current clock period (nanoseconds)
*/
if (ClockPeriod (CLOCK_REALTIME, 0, &period, 0) < 0)
return -1;
/*
* Compute count and nanoseconds increment
*/
count = 1e9 * ADJUST_PERIOD / period.nsec;
increment = delta_nsec / count + .5;
/* Reduce relative error */
if (count > increment + 1)
{
increment = 1 + (long)((delta_nsec - 1) / count);
count = delta_nsec / increment + .5;
}
/*
* Limit the adjust increment to appropriate value
*/
increment_limit = CORR_SLEW_LIMIT * period.nsec;
if (increment > increment_limit)
{
increment = increment_limit;
count = delta_nsec / increment + .5;
/* Reduce relative error */
if (increment > count + 1)
{
count = 1 + (long)((delta_nsec - 1) / increment);
increment = delta_nsec / count + .5;
}
}
adj.tick_nsec_inc = isneg ? -increment : increment;
adj.tick_count = count;
}
else
{
adj.tick_nsec_inc = 0;
adj.tick_count = 0;
}
if (ClockAdjust (CLOCK_REALTIME, &adj, &oldadj) < 0)
return -1;
/*
* Build olddelta
*/
delta_nsec_old = (double)oldadj.tick_count * oldadj.tick_nsec_inc;
if (olddelta != NULL)
{
if (delta_nsec_old != 0)
{
/* Reduce rounding error */
delta_nsec_old += (delta_nsec_old < 0) ? -500 : 500;
olddelta->tv_sec = delta_nsec_old / 1e9;
olddelta->tv_usec = (long)(delta_nsec_old - 1e9
* (long)olddelta->tv_sec) / 1000;
}
else
{
olddelta->tv_sec = 0;
olddelta->tv_usec = 0;
}
}
return 0;
}
