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; }