Userspace gtod reading without switching to kernel mode From: Wolfgang Mauerer (none) We can do this since the data are on the shared semaphore heap. The basis data structure is placed so that it is accessible from both the Linux kernel and Xenomai kernel/userland. This also requires to make the structure work with both kernel and userland definitions for elementary data types. Although we could read the TSC directly using __xn_rdtsc(), we use the vread function also employed by the kernel to make the code in both worlds aligned as good as possible. Signed-off-by: Wolfgang Mauerer Signed-off-by: Jan Kiszka --- include/nucleus/seqlock_user.h | 77 ++++++++++++++++++++++++++++++++++++++++ src/skins/posix/clock.c | 47 ++++++++++++++++++++++++ src/skins/posix/seqlock.h | 77 ++++++++++++++++++++++++++++++++++++++++ 3 files changed, 201 insertions(+), 0 deletions(-) create mode 100644 include/nucleus/seqlock_user.h create mode 100644 src/skins/posix/seqlock.h diff --git a/include/nucleus/seqlock_user.h b/include/nucleus/seqlock_user.h new file mode 100644 index 0000000..35ed045 --- /dev/null +++ b/include/nucleus/seqlock_user.h @@ -0,0 +1,77 @@ +#ifndef __SEQLOCK_USER_H +#define __SEQLOCK_USER_H + +/* Stolen from the linux kernel and slightly adapted for userland */ + +// TODO: This is just the version for x86, other CPUs require +// different definitions. Though it would make sense to remove +// the cpu_relax() in the waiting path at all. +static inline void rep_nop(void) +{ + asm volatile("rep; nop" ::: "memory"); +} + +static inline void cpu_relax(void) +{ + rep_nop(); +} + + +// This definitely needs some work. It only works for the SMP and x86 case. +#define barrier() __asm__ __volatile__("": : :"memory") + +#define smp_rmb() barrier() +#define smp_wmb() barrier() + +//#define unlikely(x) __builtin_expect(!!(x), 0) + +typedef struct seqcount { + unsigned sequence; +} seqcount_t; + +#define SEQCNT_ZERO { 0 } +#define seqcount_init(x) do { *(x) = (seqcount_t) SEQCNT_ZERO; } while (0) + +/* Start of read using pointer to a sequence counter only. */ +static inline unsigned read_seqcount_begin(const seqcount_t *s) +{ + unsigned ret; + +repeat: + ret = s->sequence; + smp_rmb(); + if (unlikely(ret & 1)) { + cpu_relax(); + goto repeat; + } + return ret; +} + +/* + * Test if reader processed invalid data because sequence number has changed. + */ +static inline int read_seqcount_retry(const seqcount_t *s, unsigned start) +{ + smp_rmb(); + + return s->sequence != start; +} + + +/* + * Sequence counter only version assumes that callers are using their + * own mutexing. + */ +static inline void write_seqcount_begin(seqcount_t *s) +{ + s->sequence++; + smp_wmb(); +} + +static inline void write_seqcount_end(seqcount_t *s) +{ + smp_wmb(); + s->sequence++; +} + +#endif diff --git a/src/skins/posix/clock.c b/src/skins/posix/clock.c index 4c31114..ba1bc20 100644 --- a/src/skins/posix/clock.c +++ b/src/skins/posix/clock.c @@ -25,6 +25,9 @@ #include #include #include +#include +#include +#include extern int __pse51_muxid; @@ -59,6 +62,50 @@ int __wrap_clock_getres(clockid_t clock_id, struct timespec *tp) int __wrap_gettimeofday(struct timeval *tv, void *tzp) { int err; +#ifdef XNARCH_HAVE_NONPRIV_TSC + unsigned int seq; + cycle_t now, base, mask, cycle_delta; + unsigned long mult, shift, nsec, rem; + + struct gtod_data_exchange *exchg = &nkvdso->gtod_data_exchange; + struct ipipe_gtod_data *gtod_data; + +retry: + gtod_data = &exchg->gtod_data[exchg->gtod_active]; + if(unlikely(!gtod_data->vread)) + goto fallback; + + /* + * The following is essentially a verbatim copy of the + * mechanism in the kernel + */ + seq = read_seqcount_begin(>od_data->lock); + + now = gtod_data->vread(); + base = gtod_data->cycle_last; + mask = gtod_data->mask; + mult = gtod_data->mult; + shift = gtod_data->shift; + tv->tv_sec = gtod_data->wall_time_sec; + nsec = gtod_data->wall_time_nsec; + + /* If the data changed during the read, try the + alternative data element */ + if (read_seqcount_retry(>od_data->lock, seq)) + goto retry; + + cycle_delta = (now - base) & mask; + nsec += (cycle_delta * mult) >> shift; + +#define NSEC_PER_USEC 1000L +#define NSEC_PER_SEC 1000000000L + tv->tv_sec += xnarch_divrem_billion(nsec, &rem); + tv->tv_usec = rem / NSEC_PER_USEC; + + return 0; + +fallback: +#endif /* XNARCH_HAVE_NONPRIV_TSC */ err = -XENOMAI_SKINCALL2(__pse51_muxid, __pse51_gettimeofday, diff --git a/src/skins/posix/seqlock.h b/src/skins/posix/seqlock.h new file mode 100644 index 0000000..9929247 --- /dev/null +++ b/src/skins/posix/seqlock.h @@ -0,0 +1,77 @@ +#ifndef __SEQLOCK_H +#define __SEQLOCK_H + +/* Stolen from the linux kernel and slightly adapted for userland */ + +// TODO: This is just the version for x86, other CPUs require +// different definitions. Though it would make sense to remove +// the cpu_relax() in the waiting path at all. +static inline void rep_nop(void) +{ + asm volatile("rep; nop" ::: "memory"); +} + +static inline void cpu_relax(void) +{ + rep_nop(); +} + + +// This definitely needs some work. It only works for the SMP and x86 case. +#define barrier() __asm__ __volatile__("": : :"memory") + +#define smp_rmb() barrier() +#define smp_wmb() barrier() + +//#define unlikely(x) __builtin_expect(!!(x), 0) + +typedef struct seqcount { + unsigned sequence; +} seqcount_t; + +#define SEQCNT_ZERO { 0 } +#define seqcount_init(x) do { *(x) = (seqcount_t) SEQCNT_ZERO; } while (0) + +/* Start of read using pointer to a sequence counter only. */ +static inline unsigned read_seqcount_begin(const seqcount_t *s) +{ + unsigned ret; + +repeat: + ret = s->sequence; + smp_rmb(); + if (unlikely(ret & 1)) { + cpu_relax(); + goto repeat; + } + return ret; +} + +/* + * Test if reader processed invalid data because sequence number has changed. + */ +static inline int read_seqcount_retry(const seqcount_t *s, unsigned start) +{ + smp_rmb(); + + return s->sequence != start; +} + + +/* + * Sequence counter only version assumes that callers are using their + * own mutexing. + */ +static inline void write_seqcount_begin(seqcount_t *s) +{ + s->sequence++; + smp_wmb(); +} + +static inline void write_seqcount_end(seqcount_t *s) +{ + smp_wmb(); + s->sequence++; +} + +#endif