1
2
3
4
5
6
7
8
9
10
11#include "vdso.h"
12
13#include <linux/compiler.h>
14#include <linux/time.h>
15
16#include <asm/clocksource.h>
17#include <asm/io.h>
18#include <asm/unistd.h>
19#include <asm/vdso.h>
20
21#ifdef CONFIG_MIPS_CLOCK_VSYSCALL
22
23static __always_inline long gettimeofday_fallback(struct timeval *_tv,
24 struct timezone *_tz)
25{
26 register struct timezone *tz asm("a1") = _tz;
27 register struct timeval *tv asm("a0") = _tv;
28 register long ret asm("v0");
29 register long nr asm("v0") = __NR_gettimeofday;
30 register long error asm("a3");
31
32 asm volatile(
33 " syscall\n"
34 : "=r" (ret), "=r" (error)
35 : "r" (tv), "r" (tz), "r" (nr)
36 : "$1", "$3", "$8", "$9", "$10", "$11", "$12", "$13",
37 "$14", "$15", "$24", "$25", "hi", "lo", "memory");
38
39 return error ? -ret : ret;
40}
41
42#endif
43
44static __always_inline long clock_gettime_fallback(clockid_t _clkid,
45 struct timespec *_ts)
46{
47 register struct timespec *ts asm("a1") = _ts;
48 register clockid_t clkid asm("a0") = _clkid;
49 register long ret asm("v0");
50 register long nr asm("v0") = __NR_clock_gettime;
51 register long error asm("a3");
52
53 asm volatile(
54 " syscall\n"
55 : "=r" (ret), "=r" (error)
56 : "r" (clkid), "r" (ts), "r" (nr)
57 : "$1", "$3", "$8", "$9", "$10", "$11", "$12", "$13",
58 "$14", "$15", "$24", "$25", "hi", "lo", "memory");
59
60 return error ? -ret : ret;
61}
62
63static __always_inline int do_realtime_coarse(struct timespec *ts,
64 const union mips_vdso_data *data)
65{
66 u32 start_seq;
67
68 do {
69 start_seq = vdso_data_read_begin(data);
70
71 ts->tv_sec = data->xtime_sec;
72 ts->tv_nsec = data->xtime_nsec >> data->cs_shift;
73 } while (vdso_data_read_retry(data, start_seq));
74
75 return 0;
76}
77
78static __always_inline int do_monotonic_coarse(struct timespec *ts,
79 const union mips_vdso_data *data)
80{
81 u32 start_seq;
82 u64 to_mono_sec;
83 u64 to_mono_nsec;
84
85 do {
86 start_seq = vdso_data_read_begin(data);
87
88 ts->tv_sec = data->xtime_sec;
89 ts->tv_nsec = data->xtime_nsec >> data->cs_shift;
90
91 to_mono_sec = data->wall_to_mono_sec;
92 to_mono_nsec = data->wall_to_mono_nsec;
93 } while (vdso_data_read_retry(data, start_seq));
94
95 ts->tv_sec += to_mono_sec;
96 timespec_add_ns(ts, to_mono_nsec);
97
98 return 0;
99}
100
101#ifdef CONFIG_CSRC_R4K
102
103static __always_inline u64 read_r4k_count(void)
104{
105 unsigned int count;
106
107 __asm__ __volatile__(
108 " .set push\n"
109 " .set mips32r2\n"
110 " rdhwr %0, $2\n"
111 " .set pop\n"
112 : "=r" (count));
113
114 return count;
115}
116
117#endif
118
119#ifdef CONFIG_CLKSRC_MIPS_GIC
120
121static __always_inline u64 read_gic_count(const union mips_vdso_data *data)
122{
123 void __iomem *gic = get_gic(data);
124 u32 hi, hi2, lo;
125
126 do {
127 hi = __raw_readl(gic + sizeof(lo));
128 lo = __raw_readl(gic);
129 hi2 = __raw_readl(gic + sizeof(lo));
130 } while (hi2 != hi);
131
132 return (((u64)hi) << 32) + lo;
133}
134
135#endif
136
137static __always_inline u64 get_ns(const union mips_vdso_data *data)
138{
139 u64 cycle_now, delta, nsec;
140
141 switch (data->clock_mode) {
142#ifdef CONFIG_CSRC_R4K
143 case VDSO_CLOCK_R4K:
144 cycle_now = read_r4k_count();
145 break;
146#endif
147#ifdef CONFIG_CLKSRC_MIPS_GIC
148 case VDSO_CLOCK_GIC:
149 cycle_now = read_gic_count(data);
150 break;
151#endif
152 default:
153 return 0;
154 }
155
156 delta = (cycle_now - data->cs_cycle_last) & data->cs_mask;
157
158 nsec = (delta * data->cs_mult) + data->xtime_nsec;
159 nsec >>= data->cs_shift;
160
161 return nsec;
162}
163
164static __always_inline int do_realtime(struct timespec *ts,
165 const union mips_vdso_data *data)
166{
167 u32 start_seq;
168 u64 ns;
169
170 do {
171 start_seq = vdso_data_read_begin(data);
172
173 if (data->clock_mode == VDSO_CLOCK_NONE)
174 return -ENOSYS;
175
176 ts->tv_sec = data->xtime_sec;
177 ns = get_ns(data);
178 } while (vdso_data_read_retry(data, start_seq));
179
180 ts->tv_nsec = 0;
181 timespec_add_ns(ts, ns);
182
183 return 0;
184}
185
186static __always_inline int do_monotonic(struct timespec *ts,
187 const union mips_vdso_data *data)
188{
189 u32 start_seq;
190 u64 ns;
191 u64 to_mono_sec;
192 u64 to_mono_nsec;
193
194 do {
195 start_seq = vdso_data_read_begin(data);
196
197 if (data->clock_mode == VDSO_CLOCK_NONE)
198 return -ENOSYS;
199
200 ts->tv_sec = data->xtime_sec;
201 ns = get_ns(data);
202
203 to_mono_sec = data->wall_to_mono_sec;
204 to_mono_nsec = data->wall_to_mono_nsec;
205 } while (vdso_data_read_retry(data, start_seq));
206
207 ts->tv_sec += to_mono_sec;
208 ts->tv_nsec = 0;
209 timespec_add_ns(ts, ns + to_mono_nsec);
210
211 return 0;
212}
213
214#ifdef CONFIG_MIPS_CLOCK_VSYSCALL
215
216
217
218
219
220
221
222int __vdso_gettimeofday(struct timeval *tv, struct timezone *tz)
223{
224 const union mips_vdso_data *data = get_vdso_data();
225 struct timespec ts;
226 int ret;
227
228 ret = do_realtime(&ts, data);
229 if (ret)
230 return gettimeofday_fallback(tv, tz);
231
232 if (tv) {
233 tv->tv_sec = ts.tv_sec;
234 tv->tv_usec = ts.tv_nsec / 1000;
235 }
236
237 if (tz) {
238 tz->tz_minuteswest = data->tz_minuteswest;
239 tz->tz_dsttime = data->tz_dsttime;
240 }
241
242 return 0;
243}
244
245#endif
246
247int __vdso_clock_gettime(clockid_t clkid, struct timespec *ts)
248{
249 const union mips_vdso_data *data = get_vdso_data();
250 int ret = -1;
251
252 switch (clkid) {
253 case CLOCK_REALTIME_COARSE:
254 ret = do_realtime_coarse(ts, data);
255 break;
256 case CLOCK_MONOTONIC_COARSE:
257 ret = do_monotonic_coarse(ts, data);
258 break;
259 case CLOCK_REALTIME:
260 ret = do_realtime(ts, data);
261 break;
262 case CLOCK_MONOTONIC:
263 ret = do_monotonic(ts, data);
264 break;
265 default:
266 break;
267 }
268
269 if (ret)
270 ret = clock_gettime_fallback(clkid, ts);
271
272 return ret;
273}
274
