270 lines
5.9 KiB
C
270 lines
5.9 KiB
C
// SPDX-License-Identifier: GPL-2.0
|
|
// Copyright (C) 2005-2017 Andes Technology Corporation
|
|
|
|
#include <linux/compiler.h>
|
|
#include <linux/hrtimer.h>
|
|
#include <linux/time.h>
|
|
#include <asm/io.h>
|
|
#include <asm/barrier.h>
|
|
#include <asm/bug.h>
|
|
#include <asm/page.h>
|
|
#include <asm/unistd.h>
|
|
#include <asm/vdso_datapage.h>
|
|
#include <asm/vdso_timer_info.h>
|
|
#include <asm/asm-offsets.h>
|
|
|
|
#define X(x) #x
|
|
#define Y(x) X(x)
|
|
|
|
extern struct vdso_data *__get_datapage(void);
|
|
extern struct vdso_data *__get_timerpage(void);
|
|
|
|
static notrace unsigned int __vdso_read_begin(const struct vdso_data *vdata)
|
|
{
|
|
u32 seq;
|
|
repeat:
|
|
seq = READ_ONCE(vdata->seq_count);
|
|
if (seq & 1) {
|
|
cpu_relax();
|
|
goto repeat;
|
|
}
|
|
return seq;
|
|
}
|
|
|
|
static notrace unsigned int vdso_read_begin(const struct vdso_data *vdata)
|
|
{
|
|
unsigned int seq;
|
|
|
|
seq = __vdso_read_begin(vdata);
|
|
|
|
smp_rmb(); /* Pairs with smp_wmb in vdso_write_end */
|
|
return seq;
|
|
}
|
|
|
|
static notrace int vdso_read_retry(const struct vdso_data *vdata, u32 start)
|
|
{
|
|
smp_rmb(); /* Pairs with smp_wmb in vdso_write_begin */
|
|
return vdata->seq_count != start;
|
|
}
|
|
|
|
static notrace long clock_gettime_fallback(clockid_t _clkid,
|
|
struct __kernel_old_timespec *_ts)
|
|
{
|
|
register struct __kernel_old_timespec *ts asm("$r1") = _ts;
|
|
register clockid_t clkid asm("$r0") = _clkid;
|
|
register long ret asm("$r0");
|
|
|
|
asm volatile ("movi $r15, %3\n"
|
|
"syscall 0x0\n"
|
|
:"=r" (ret)
|
|
:"r"(clkid), "r"(ts), "i"(__NR_clock_gettime)
|
|
:"$r15", "memory");
|
|
|
|
return ret;
|
|
}
|
|
|
|
static notrace int do_realtime_coarse(struct __kernel_old_timespec *ts,
|
|
struct vdso_data *vdata)
|
|
{
|
|
u32 seq;
|
|
|
|
do {
|
|
seq = vdso_read_begin(vdata);
|
|
|
|
ts->tv_sec = vdata->xtime_coarse_sec;
|
|
ts->tv_nsec = vdata->xtime_coarse_nsec;
|
|
|
|
} while (vdso_read_retry(vdata, seq));
|
|
return 0;
|
|
}
|
|
|
|
static notrace int do_monotonic_coarse(struct __kernel_old_timespec *ts,
|
|
struct vdso_data *vdata)
|
|
{
|
|
u32 seq;
|
|
u64 ns;
|
|
|
|
do {
|
|
seq = vdso_read_begin(vdata);
|
|
|
|
ts->tv_sec = vdata->xtime_coarse_sec + vdata->wtm_clock_sec;
|
|
ns = vdata->xtime_coarse_nsec + vdata->wtm_clock_nsec;
|
|
|
|
} while (vdso_read_retry(vdata, seq));
|
|
|
|
ts->tv_sec += __iter_div_u64_rem(ns, NSEC_PER_SEC, &ns);
|
|
ts->tv_nsec = ns;
|
|
|
|
return 0;
|
|
}
|
|
|
|
static notrace inline u64 vgetsns(struct vdso_data *vdso)
|
|
{
|
|
u32 cycle_now;
|
|
u32 cycle_delta;
|
|
u32 *timer_cycle_base;
|
|
|
|
timer_cycle_base =
|
|
(u32 *) ((char *)__get_timerpage() + vdso->cycle_count_offset);
|
|
cycle_now = readl_relaxed(timer_cycle_base);
|
|
if (true == vdso->cycle_count_down)
|
|
cycle_now = ~(*timer_cycle_base);
|
|
cycle_delta = cycle_now - (u32) vdso->cs_cycle_last;
|
|
return ((u64) cycle_delta & vdso->cs_mask) * vdso->cs_mult;
|
|
}
|
|
|
|
static notrace int do_realtime(struct __kernel_old_timespec *ts, struct vdso_data *vdata)
|
|
{
|
|
unsigned count;
|
|
u64 ns;
|
|
do {
|
|
count = vdso_read_begin(vdata);
|
|
ts->tv_sec = vdata->xtime_clock_sec;
|
|
ns = vdata->xtime_clock_nsec;
|
|
ns += vgetsns(vdata);
|
|
ns >>= vdata->cs_shift;
|
|
} while (vdso_read_retry(vdata, count));
|
|
|
|
ts->tv_sec += __iter_div_u64_rem(ns, NSEC_PER_SEC, &ns);
|
|
ts->tv_nsec = ns;
|
|
|
|
return 0;
|
|
}
|
|
|
|
static notrace int do_monotonic(struct __kernel_old_timespec *ts, struct vdso_data *vdata)
|
|
{
|
|
u64 ns;
|
|
u32 seq;
|
|
|
|
do {
|
|
seq = vdso_read_begin(vdata);
|
|
|
|
ts->tv_sec = vdata->xtime_clock_sec;
|
|
ns = vdata->xtime_clock_nsec;
|
|
ns += vgetsns(vdata);
|
|
ns >>= vdata->cs_shift;
|
|
|
|
ts->tv_sec += vdata->wtm_clock_sec;
|
|
ns += vdata->wtm_clock_nsec;
|
|
|
|
} while (vdso_read_retry(vdata, seq));
|
|
|
|
ts->tv_sec += __iter_div_u64_rem(ns, NSEC_PER_SEC, &ns);
|
|
ts->tv_nsec = ns;
|
|
|
|
return 0;
|
|
}
|
|
|
|
notrace int __vdso_clock_gettime(clockid_t clkid, struct __kernel_old_timespec *ts)
|
|
{
|
|
struct vdso_data *vdata;
|
|
int ret = -1;
|
|
|
|
vdata = __get_datapage();
|
|
if (vdata->cycle_count_offset == EMPTY_REG_OFFSET)
|
|
return clock_gettime_fallback(clkid, ts);
|
|
|
|
switch (clkid) {
|
|
case CLOCK_REALTIME_COARSE:
|
|
ret = do_realtime_coarse(ts, vdata);
|
|
break;
|
|
case CLOCK_MONOTONIC_COARSE:
|
|
ret = do_monotonic_coarse(ts, vdata);
|
|
break;
|
|
case CLOCK_REALTIME:
|
|
ret = do_realtime(ts, vdata);
|
|
break;
|
|
case CLOCK_MONOTONIC:
|
|
ret = do_monotonic(ts, vdata);
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
|
|
if (ret)
|
|
ret = clock_gettime_fallback(clkid, ts);
|
|
|
|
return ret;
|
|
}
|
|
|
|
static notrace int clock_getres_fallback(clockid_t _clk_id,
|
|
struct __kernel_old_timespec *_res)
|
|
{
|
|
register clockid_t clk_id asm("$r0") = _clk_id;
|
|
register struct __kernel_old_timespec *res asm("$r1") = _res;
|
|
register int ret asm("$r0");
|
|
|
|
asm volatile ("movi $r15, %3\n"
|
|
"syscall 0x0\n"
|
|
:"=r" (ret)
|
|
:"r"(clk_id), "r"(res), "i"(__NR_clock_getres)
|
|
:"$r15", "memory");
|
|
|
|
return ret;
|
|
}
|
|
|
|
notrace int __vdso_clock_getres(clockid_t clk_id, struct __kernel_old_timespec *res)
|
|
{
|
|
struct vdso_data *vdata = __get_datapage();
|
|
|
|
if (res == NULL)
|
|
return 0;
|
|
switch (clk_id) {
|
|
case CLOCK_REALTIME:
|
|
case CLOCK_MONOTONIC:
|
|
case CLOCK_MONOTONIC_RAW:
|
|
res->tv_sec = 0;
|
|
res->tv_nsec = vdata->hrtimer_res;
|
|
break;
|
|
case CLOCK_REALTIME_COARSE:
|
|
case CLOCK_MONOTONIC_COARSE:
|
|
res->tv_sec = 0;
|
|
res->tv_nsec = CLOCK_COARSE_RES;
|
|
break;
|
|
default:
|
|
return clock_getres_fallback(clk_id, res);
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
static notrace inline int gettimeofday_fallback(struct __kernel_old_timeval *_tv,
|
|
struct timezone *_tz)
|
|
{
|
|
register struct __kernel_old_timeval *tv asm("$r0") = _tv;
|
|
register struct timezone *tz asm("$r1") = _tz;
|
|
register int ret asm("$r0");
|
|
|
|
asm volatile ("movi $r15, %3\n"
|
|
"syscall 0x0\n"
|
|
:"=r" (ret)
|
|
:"r"(tv), "r"(tz), "i"(__NR_gettimeofday)
|
|
:"$r15", "memory");
|
|
|
|
return ret;
|
|
}
|
|
|
|
notrace int __vdso_gettimeofday(struct __kernel_old_timeval *tv, struct timezone *tz)
|
|
{
|
|
struct __kernel_old_timespec ts;
|
|
struct vdso_data *vdata;
|
|
int ret;
|
|
|
|
vdata = __get_datapage();
|
|
|
|
if (vdata->cycle_count_offset == EMPTY_REG_OFFSET)
|
|
return gettimeofday_fallback(tv, tz);
|
|
|
|
ret = do_realtime(&ts, vdata);
|
|
|
|
if (tv) {
|
|
tv->tv_sec = ts.tv_sec;
|
|
tv->tv_usec = ts.tv_nsec / 1000;
|
|
}
|
|
if (tz) {
|
|
tz->tz_minuteswest = vdata->tz_minuteswest;
|
|
tz->tz_dsttime = vdata->tz_dsttime;
|
|
}
|
|
|
|
return ret;
|
|
}
|