/*
|
* SPDX-License-Identifier: GPL-2.0
|
*
|
* Written by Gilles Chanteperdrix <gilles.chanteperdrix@xenomai.org>.
|
*/
|
|
#include <cobalt/kernel/clock.h>
|
#include <cobalt/kernel/vdso.h>
|
#include <cobalt/kernel/arith.h>
|
#include <cobalt/kernel/timer.h>
|
#include <xenomai/posix/clock.h>
|
#include <pipeline/machine.h>
|
|
static unsigned long long clockfreq;
|
|
#ifdef XNARCH_HAVE_LLMULSHFT
|
|
static unsigned int tsc_scale, tsc_shift;
|
|
#ifdef XNARCH_HAVE_NODIV_LLIMD
|
|
static struct xnarch_u32frac tsc_frac;
|
|
long long xnclock_core_ns_to_ticks(long long ns)
|
{
|
return xnarch_nodiv_llimd(ns, tsc_frac.frac, tsc_frac.integ);
|
}
|
|
#else /* !XNARCH_HAVE_NODIV_LLIMD */
|
|
long long xnclock_core_ns_to_ticks(long long ns)
|
{
|
return xnarch_llimd(ns, 1 << tsc_shift, tsc_scale);
|
}
|
|
#endif /* !XNARCH_HAVE_NODIV_LLIMD */
|
|
xnsticks_t xnclock_core_ticks_to_ns(xnsticks_t ticks)
|
{
|
return xnarch_llmulshft(ticks, tsc_scale, tsc_shift);
|
}
|
|
xnsticks_t xnclock_core_ticks_to_ns_rounded(xnsticks_t ticks)
|
{
|
unsigned int shift = tsc_shift - 1;
|
return (xnarch_llmulshft(ticks, tsc_scale, shift) + 1) / 2;
|
}
|
|
#else /* !XNARCH_HAVE_LLMULSHFT */
|
|
xnsticks_t xnclock_core_ticks_to_ns(xnsticks_t ticks)
|
{
|
return xnarch_llimd(ticks, 1000000000, clockfreq);
|
}
|
|
xnsticks_t xnclock_core_ticks_to_ns_rounded(xnsticks_t ticks)
|
{
|
return (xnarch_llimd(ticks, 1000000000, clockfreq/2) + 1) / 2;
|
}
|
|
xnsticks_t xnclock_core_ns_to_ticks(xnsticks_t ns)
|
{
|
return xnarch_llimd(ns, clockfreq, 1000000000);
|
}
|
|
#endif /* !XNARCH_HAVE_LLMULSHFT */
|
|
EXPORT_SYMBOL_GPL(xnclock_core_ticks_to_ns);
|
EXPORT_SYMBOL_GPL(xnclock_core_ticks_to_ns_rounded);
|
EXPORT_SYMBOL_GPL(xnclock_core_ns_to_ticks);
|
|
int pipeline_get_host_time(struct timespec64 *tp)
|
{
|
#ifdef CONFIG_IPIPE_HAVE_HOSTRT
|
struct xnvdso_hostrt_data *hostrt_data;
|
u64 now, base, mask, cycle_delta;
|
__u32 mult, shift;
|
unsigned long rem;
|
urwstate_t tmp;
|
__u64 nsec;
|
|
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 urw block is required when a reader is
|
* interleaved by a writer on a different CPU. This follows
|
* the approach from userland, where taking the spinlock is
|
* not possible.
|
*/
|
unsynced_read_block(&tmp, &hostrt_data->lock) {
|
now = xnclock_read_raw(&nkclock);
|
base = hostrt_data->cycle_last;
|
mask = hostrt_data->mask;
|
mult = hostrt_data->mult;
|
shift = hostrt_data->shift;
|
tp->tv_sec = hostrt_data->wall_sec;
|
nsec = hostrt_data->wall_nsec;
|
}
|
|
/*
|
* 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 += xnclock_divrem_billion(nsec, &rem);
|
tp->tv_nsec = rem;
|
|
return 0;
|
#else
|
return -EINVAL;
|
#endif
|
}
|
|
xnticks_t pipeline_read_wallclock(void)
|
{
|
return xnclock_read_monotonic(&nkclock) + xnclock_get_offset(&nkclock);
|
}
|
EXPORT_SYMBOL_GPL(pipeline_read_wallclock);
|
|
int pipeline_set_wallclock(xnticks_t epoch_ns)
|
{
|
xnclock_set_wallclock(epoch_ns);
|
|
return 0;
|
}
|
|
void pipeline_update_clock_freq(unsigned long long freq)
|
{
|
spl_t s;
|
|
xnlock_get_irqsave(&nklock, s);
|
clockfreq = freq;
|
#ifdef XNARCH_HAVE_LLMULSHFT
|
xnarch_init_llmulshft(1000000000, freq, &tsc_scale, &tsc_shift);
|
#ifdef XNARCH_HAVE_NODIV_LLIMD
|
xnarch_init_u32frac(&tsc_frac, 1 << tsc_shift, tsc_scale);
|
#endif
|
#endif
|
cobalt_pipeline.clock_freq = freq;
|
xnlock_put_irqrestore(&nklock, s);
|
}
|
|
void pipeline_init_clock(void)
|
{
|
pipeline_update_clock_freq(cobalt_pipeline.clock_freq);
|
}
|
