1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
|
/*
* Copyright (c) 2006-2024 RT-Thread Development Team
*
* SPDX-License-Identifier: Apache-2.0
*
* Change Logs:
* Date Author Notes
* 2024-07-04 rcitach init ver.
*/
#include <stdio.h>
#include <time.h>
#include <errno.h>
#include <stdbool.h>
#include <vdso_sys.h>
#ifndef rt_vdso_cycles_ready
static inline bool rt_vdso_cycles_ready(uint64_t cycles)
{
return true;
}
#endif
#ifndef rt_vdso_get_ns
static inline
uint64_t rt_vdso_get_ns(uint64_t cycles, uint64_t last)
{
return (cycles - last) * NSEC_PER_SEC / __arch_get_hw_frq();
}
#endif
static int
__rt_vdso_getcoarse(struct timespec *ts, clockid_t clock, const struct vdso_data *vdns)
{
const struct vdso_data *vd;
const struct timespec *vdso_ts;
uint32_t seq;
uint64_t sec, last, ns, cycles;
if (clock != CLOCK_MONOTONIC_RAW)
vd = &vdns[CS_HRES_COARSE];
else
vd = &vdns[CS_RAW];
vdso_ts = &vd->basetime[clock];
do {
seq = rt_vdso_read_begin(vd);
cycles = __arch_get_hw_counter(vd->clock_mode, vd);
if (unlikely(!rt_vdso_cycles_ready(cycles)))
return -1;
ns = vdso_ts->tv_nsec;
last = vd->cycle_last;
ns += rt_vdso_get_ns(cycles, last);
sec = vdso_ts->tv_sec;
} while (unlikely(rt_vdso_read_retry(vd, seq)));
ts->tv_sec = sec + __iter_div_u64_rem(ns, NSEC_PER_SEC, &ns);
ts->tv_nsec = ns;
return 0;
}
static inline int
__vdso_clock_gettime_common(const struct vdso_data *vd, clockid_t clock,
struct timespec *ts)
{
u_int32_t msk;
if (unlikely((u_int32_t) clock >= MAX_CLOCKS))
return -1;
msk = 1U << clock;
if (likely(msk & VDSO_REALTIME))
return __rt_vdso_getcoarse(ts,CLOCK_REALTIME,vd);
else if (msk & VDSO_MONOTIME)
return __rt_vdso_getcoarse(ts,CLOCK_MONOTONIC,vd);
else
return ENOENT;
}
static __maybe_unused int
rt_vdso_clock_gettime_data(const struct vdso_data *vd, clockid_t clock,
struct timespec *ts)
{
int ret = 0;
ret = __vdso_clock_gettime_common(vd, clock, ts);
return ret;
}
int
__kernel_clock_gettime(clockid_t clock, struct timespec *ts)
{
return rt_vdso_clock_gettime_data(__arch_get_vdso_data(), clock, ts);
}
|
