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