xnticks_t xntimer_get_timeout_aperiodic(xntimer_t *timer)
{
xnticks_t tsc = xnarch_get_cpu_tsc();
if (xntimerh_date(&timer->aplink) < tsc)
return 1; /* Will elapse shortly. */
return xnarch_tsc_to_ns(xntimerh_date(&timer->aplink) - tsc);
}
/**
* Read the host-synchronised realtime clock.
*
* Obtain the current time with NTP corrections from the Linux domain
*
* @param tp pointer to a struct timespec
*
* @retval 0 on success;
* @retval -1 if no suitable NTP-corrected clocksource is availabel
*
* @see
* <a href="http://www.opengroup.org/onlinepubs/000095399/functions/gettimeofday.html">
* Specification.</a>
*
*/
static int do_clock_host_realtime(struct timespec *tp)
{
#ifdef CONFIG_XENO_OPT_HOSTRT
cycle_t now, base, mask, cycle_delta;
unsigned long mult, shift, nsec, rem;
struct xnvdso_hostrt_data *hostrt_data;
unsigned int seq;
hostrt_data = get_hostrt_data();
BUG_ON(!hostrt_data);
if (unlikely(!hostrt_data->live))
return -1;
/*
* Note: Disabling HW interrupts around writes to hostrt_data ensures
* that a reader (on the Xenomai side) cannot interrupt a writer (on
* the Linux kernel side) on the same CPU. The sequence counter is
* required when a reader is interleaved by a writer on a different
* CPU. This follows the approach from userland, where tasking the
* spinlock is not possible.
*/
retry:
seq = xnread_seqcount_begin(&hostrt_data->seqcount);
now = xnarch_get_cpu_tsc();
base = hostrt_data->cycle_last;
mask = hostrt_data->mask;
mult = hostrt_data->mult;
shift = hostrt_data->shift;
tp->tv_sec = hostrt_data->wall_time_sec;
nsec = hostrt_data->wall_time_nsec;
if (xnread_seqcount_retry(&hostrt_data->seqcount, seq))
goto retry;
/*
* At this point, we have a consistent copy of the fundamental
* data structure - calculate the interval between the current
* and base time stamp cycles, and convert the difference
* to nanoseconds.
*/
cycle_delta = (now - base) & mask;
nsec += (cycle_delta * mult) >> shift;
/* Convert to the desired sec, usec representation */
tp->tv_sec += xnarch_divrem_billion(nsec, &rem);
tp->tv_nsec = rem;
return 0;
#else /* CONFIG_XENO_OPT_HOSTRT */
return -EINVAL;
#endif
}
int xntimer_start_aperiodic(xntimer_t *timer,
xnticks_t value, xnticks_t interval,
xntmode_t mode)
{
xnticks_t date, now;
trace_mark(xn_nucleus, timer_start,
"timer %p base %s value %Lu interval %Lu mode %u",
timer, xntimer_base(timer)->name, value, interval, mode);
if (!testbits(timer->status, XNTIMER_DEQUEUED))
xntimer_dequeue_aperiodic(timer);
now = xnarch_get_cpu_tsc();
__clrbits(timer->status,
XNTIMER_REALTIME | XNTIMER_FIRED | XNTIMER_PERIODIC);
switch (mode) {
case XN_RELATIVE:
if ((xnsticks_t)value < 0)
return -ETIMEDOUT;
date = xnarch_ns_to_tsc(value) + now;
break;
case XN_REALTIME:
__setbits(timer->status, XNTIMER_REALTIME);
value -= nktbase.wallclock_offset;
/* fall through */
default: /* XN_ABSOLUTE || XN_REALTIME */
date = xnarch_ns_to_tsc(value);
if ((xnsticks_t)(date - now) <= 0)
return -ETIMEDOUT;
break;
}
xntimerh_date(&timer->aplink) = date;
timer->interval = XN_INFINITE;
if (interval != XN_INFINITE) {
timer->interval = xnarch_ns_to_tsc(interval);
timer->pexpect = date;
__setbits(timer->status, XNTIMER_PERIODIC);
}
xntimer_enqueue_aperiodic(timer);
if (xntimer_heading_p(timer)) {
if (xntimer_sched(timer) != xnpod_current_sched())
xntimer_next_remote_shot(xntimer_sched(timer));
else
xntimer_next_local_shot(xntimer_sched(timer));
}
return 0;
}
static void
xntimer_adjust_aperiodic(xntimer_t *timer, xnsticks_t delta)
{
xntimerh_date(&timer->aplink) -= delta;
if (testbits(timer->status, XNTIMER_PERIODIC)) {
xnticks_t period = xntimer_interval(timer);
xnsticks_t diff;
xnticks_t mod;
timer->pexpect -= delta;
diff = xnarch_get_cpu_tsc() - xntimerh_date(&timer->aplink);
if ((xnsticks_t) (diff - period) >= 0) {
/* timer should tick several times before now, instead
of calling timer->handler several times, we change
the timer date without changing its pexpect, so that
timer will tick only once and the lost ticks will be
counted as overruns. */
mod = xnarch_mod64(diff, period);
xntimerh_date(&timer->aplink) += diff - mod;
} else if (delta < 0
&& testbits(timer->status, XNTIMER_FIRED)
&& (xnsticks_t) (diff + period) <= 0) {
/* timer is periodic and NOT waiting for its first shot,
so we make it tick sooner than its original date in
order to avoid the case where by adjusting time to a
sooner date, real-time periodic timers do not tick
until the original date has passed. */
mod = xnarch_mod64(-diff, period);
xntimerh_date(&timer->aplink) += diff + mod;
timer->pexpect += diff + mod;
}
}
xntimer_enqueue_aperiodic(timer);
}
void xntimer_next_local_shot(xnsched_t *sched)
{
struct xntimer *timer;
xnsticks_t delay;
xntimerq_it_t it;
xntimerh_t *h;
/*
* Do not reprogram locally when inside the tick handler -
* will be done on exit anyway. Also exit if there is no
* pending timer.
*/
if (testbits(sched->status, XNINTCK))
return;
h = xntimerq_it_begin(&sched->timerqueue, &it);
if (h == NULL)
return;
/*
* Here we try to defer the host tick heading the timer queue,
* so that it does not preempt a real-time activity uselessly,
* in two cases:
*
* 1) a rescheduling is pending for the current CPU. We may
* assume that a real-time thread is about to resume, so we
* want to move the host tick out of the way until the host
* kernel resumes, unless there is no other outstanding
* timers.
*
* 2) the current thread is running in primary mode, in which
* case we may also defer the host tick until the host kernel
* resumes.
*
* The host tick deferral is cleared whenever Xenomai is about
* to yield control to the host kernel (see
* __xnpod_schedule()), or a timer with an earlier timeout
* date is scheduled, whichever comes first.
*/
__clrbits(sched->status, XNHDEFER);
timer = aplink2timer(h);
if (unlikely(timer == &sched->htimer)) {
if (xnsched_self_resched_p(sched) ||
!xnthread_test_state(sched->curr, XNROOT)) {
h = xntimerq_it_next(&sched->timerqueue, &it, h);
if (h) {
__setbits(sched->status, XNHDEFER);
timer = aplink2timer(h);
}
}
}
delay = xntimerh_date(&timer->aplink) -
(xnarch_get_cpu_tsc() + nklatency);
if (delay < 0)
delay = 0;
else if (delay > ULONG_MAX)
delay = ULONG_MAX;
xnarch_trace_tick((unsigned)delay);
xnarch_program_timer_shot(delay);
}
void xntimer_tick_aperiodic(void)
{
xnsched_t *sched = xnpod_current_sched();
xntimerq_t *timerq = &sched->timerqueue;
xntimerh_t *holder;
xntimer_t *timer;
xnsticks_t delta;
xnticks_t now;
/*
* Optimisation: any local timer reprogramming triggered by
* invoked timer handlers can wait until we leave the tick
* handler. Use this status flag as hint to
* xntimer_start_aperiodic.
*/
__setbits(sched->status, XNINTCK);
now = xnarch_get_cpu_tsc();
while ((holder = xntimerq_head(timerq)) != NULL) {
timer = aplink2timer(holder);
/*
* If the delay to the next shot is greater than the
* intrinsic latency value, we may stop scanning the
* timer queue there, since timeout dates are ordered
* by increasing values.
*/
delta = (xnsticks_t)(xntimerh_date(&timer->aplink) - now);
if (delta > (xnsticks_t)nklatency)
break;
trace_mark(xn_nucleus, timer_expire, "timer %p", timer);
xntimer_dequeue_aperiodic(timer);
xnstat_counter_inc(&timer->fired);
if (likely(timer != &sched->htimer)) {
if (likely(!testbits(nktbase.status, XNTBLCK)
|| testbits(timer->status, XNTIMER_NOBLCK))) {
timer->handler(timer);
now = xnarch_get_cpu_tsc();
/*
* If the elapsed timer has no reload
* value, or was re-enqueued or killed
* by the timeout handler: don't not
* re-enqueue it for the next shot.
*/
if (!xntimer_reload_p(timer))
continue;
__setbits(timer->status, XNTIMER_FIRED);
} else if (likely(!testbits(timer->status, XNTIMER_PERIODIC))) {
/*
* Postpone the next tick to a
* reasonable date in the future,
* waiting for the timebase to be
* unlocked at some point.
*/
xntimerh_date(&timer->aplink) = xntimerh_date(&sched->htimer.aplink);
continue;
}
} else {
/*
* By postponing the propagation of the
* low-priority host tick to the interrupt
* epilogue (see xnintr_irq_handler()), we
* save some I-cache, which translates into
* precious microsecs on low-end hw.
*/
__setbits(sched->status, XNHTICK);
__clrbits(sched->status, XNHDEFER);
if (!testbits(timer->status, XNTIMER_PERIODIC))
continue;
}
do {
xntimerh_date(&timer->aplink) += timer->interval;
} while (xntimerh_date(&timer->aplink) < now + nklatency);
xntimer_enqueue_aperiodic(timer);
}
__clrbits(sched->status, XNINTCK);
xntimer_next_local_shot(sched);
}