1 /*
2 * Copyright (C) 2015 Imagination Technologies
3 * Author: Alex Smith <alex.smith@imgtec.com>
4 *
5 * This program is free software; you can redistribute it and/or modify it
6 * under the terms of the GNU General Public License as published by the
7 * Free Software Foundation; either version 2 of the License, or (at your
8 * option) any later version.
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/mips-cm.h>
19 #include <asm/unistd.h>
20 #include <asm/vdso.h>
21
22 #ifdef CONFIG_MIPS_CLOCK_VSYSCALL
23
gettimeofday_fallback(struct timeval * _tv,struct timezone * _tz)24 static __always_inline long gettimeofday_fallback(struct timeval *_tv,
25 struct timezone *_tz)
26 {
27 register struct timezone *tz asm("a1") = _tz;
28 register struct timeval *tv asm("a0") = _tv;
29 register long ret asm("v0");
30 register long nr asm("v0") = __NR_gettimeofday;
31 register long error asm("a3");
32
33 asm volatile(
34 " syscall\n"
35 : "=r" (ret), "=r" (error)
36 : "r" (tv), "r" (tz), "r" (nr)
37 : "$1", "$3", "$8", "$9", "$10", "$11", "$12", "$13",
38 "$14", "$15", "$24", "$25", "hi", "lo", "memory");
39
40 return error ? -ret : ret;
41 }
42
43 #endif
44
clock_gettime_fallback(clockid_t _clkid,struct timespec * _ts)45 static __always_inline long clock_gettime_fallback(clockid_t _clkid,
46 struct timespec *_ts)
47 {
48 register struct timespec *ts asm("a1") = _ts;
49 register clockid_t clkid asm("a0") = _clkid;
50 register long ret asm("v0");
51 register long nr asm("v0") = __NR_clock_gettime;
52 register long error asm("a3");
53
54 asm volatile(
55 " syscall\n"
56 : "=r" (ret), "=r" (error)
57 : "r" (clkid), "r" (ts), "r" (nr)
58 : "$1", "$3", "$8", "$9", "$10", "$11", "$12", "$13",
59 "$14", "$15", "$24", "$25", "hi", "lo", "memory");
60
61 return error ? -ret : ret;
62 }
63
do_realtime_coarse(struct timespec * ts,const union mips_vdso_data * data)64 static __always_inline int do_realtime_coarse(struct timespec *ts,
65 const union mips_vdso_data *data)
66 {
67 u32 start_seq;
68
69 do {
70 start_seq = vdso_data_read_begin(data);
71
72 ts->tv_sec = data->xtime_sec;
73 ts->tv_nsec = data->xtime_nsec >> data->cs_shift;
74 } while (vdso_data_read_retry(data, start_seq));
75
76 return 0;
77 }
78
do_monotonic_coarse(struct timespec * ts,const union mips_vdso_data * data)79 static __always_inline int do_monotonic_coarse(struct timespec *ts,
80 const union mips_vdso_data *data)
81 {
82 u32 start_seq;
83 u64 to_mono_sec;
84 u64 to_mono_nsec;
85
86 do {
87 start_seq = vdso_data_read_begin(data);
88
89 ts->tv_sec = data->xtime_sec;
90 ts->tv_nsec = data->xtime_nsec >> data->cs_shift;
91
92 to_mono_sec = data->wall_to_mono_sec;
93 to_mono_nsec = data->wall_to_mono_nsec;
94 } while (vdso_data_read_retry(data, start_seq));
95
96 ts->tv_sec += to_mono_sec;
97 timespec_add_ns(ts, to_mono_nsec);
98
99 return 0;
100 }
101
102 #ifdef CONFIG_CSRC_R4K
103
read_r4k_count(void)104 static __always_inline u64 read_r4k_count(void)
105 {
106 unsigned int count;
107
108 __asm__ __volatile__(
109 " .set push\n"
110 " .set mips32r2\n"
111 " rdhwr %0, $2\n"
112 " .set pop\n"
113 : "=r" (count));
114
115 return count;
116 }
117
118 #endif
119
get_ns(const union mips_vdso_data * data)120 static __always_inline u64 get_ns(const union mips_vdso_data *data)
121 {
122 u64 cycle_now, delta, nsec;
123
124 switch (data->clock_mode) {
125 #ifdef CONFIG_CSRC_R4K
126 case VDSO_CLOCK_R4K:
127 cycle_now = read_r4k_count();
128 break;
129 #endif
130 default:
131 return 0;
132 }
133
134 delta = (cycle_now - data->cs_cycle_last) & data->cs_mask;
135
136 nsec = (delta * data->cs_mult) + data->xtime_nsec;
137 nsec >>= data->cs_shift;
138
139 return nsec;
140 }
141
do_realtime(struct timespec * ts,const union mips_vdso_data * data)142 static __always_inline int do_realtime(struct timespec *ts,
143 const union mips_vdso_data *data)
144 {
145 u32 start_seq;
146 u64 ns;
147
148 do {
149 start_seq = vdso_data_read_begin(data);
150
151 if (data->clock_mode == VDSO_CLOCK_NONE)
152 return -ENOSYS;
153
154 ts->tv_sec = data->xtime_sec;
155 ns = get_ns(data);
156 } while (vdso_data_read_retry(data, start_seq));
157
158 ts->tv_nsec = 0;
159 timespec_add_ns(ts, ns);
160
161 return 0;
162 }
163
do_monotonic(struct timespec * ts,const union mips_vdso_data * data)164 static __always_inline int do_monotonic(struct timespec *ts,
165 const union mips_vdso_data *data)
166 {
167 u32 start_seq;
168 u64 ns;
169 u64 to_mono_sec;
170 u64 to_mono_nsec;
171
172 do {
173 start_seq = vdso_data_read_begin(data);
174
175 if (data->clock_mode == VDSO_CLOCK_NONE)
176 return -ENOSYS;
177
178 ts->tv_sec = data->xtime_sec;
179 ns = get_ns(data);
180
181 to_mono_sec = data->wall_to_mono_sec;
182 to_mono_nsec = data->wall_to_mono_nsec;
183 } while (vdso_data_read_retry(data, start_seq));
184
185 ts->tv_sec += to_mono_sec;
186 ts->tv_nsec = 0;
187 timespec_add_ns(ts, ns + to_mono_nsec);
188
189 return 0;
190 }
191
192 #ifdef CONFIG_MIPS_CLOCK_VSYSCALL
193
194 /*
195 * This is behind the ifdef so that we don't provide the symbol when there's no
196 * possibility of there being a usable clocksource, because there's nothing we
197 * can do without it. When libc fails the symbol lookup it should fall back on
198 * the standard syscall path.
199 */
__vdso_gettimeofday(struct timeval * tv,struct timezone * tz)200 int __vdso_gettimeofday(struct timeval *tv, struct timezone *tz)
201 {
202 const union mips_vdso_data *data = get_vdso_data();
203 struct timespec ts;
204 int ret;
205
206 ret = do_realtime(&ts, data);
207 if (ret)
208 return gettimeofday_fallback(tv, tz);
209
210 if (tv) {
211 tv->tv_sec = ts.tv_sec;
212 tv->tv_usec = ts.tv_nsec / 1000;
213 }
214
215 if (tz) {
216 tz->tz_minuteswest = data->tz_minuteswest;
217 tz->tz_dsttime = data->tz_dsttime;
218 }
219
220 return 0;
221 }
222
223 #endif /* CONFIG_MIPS_CLOCK_VSYSCALL */
224
__vdso_clock_gettime(clockid_t clkid,struct timespec * ts)225 int __vdso_clock_gettime(clockid_t clkid, struct timespec *ts)
226 {
227 const union mips_vdso_data *data = get_vdso_data();
228 int ret = -1;
229
230 switch (clkid) {
231 case CLOCK_REALTIME_COARSE:
232 ret = do_realtime_coarse(ts, data);
233 break;
234 case CLOCK_MONOTONIC_COARSE:
235 ret = do_monotonic_coarse(ts, data);
236 break;
237 case CLOCK_REALTIME:
238 ret = do_realtime(ts, data);
239 break;
240 case CLOCK_MONOTONIC:
241 ret = do_monotonic(ts, data);
242 break;
243 default:
244 break;
245 }
246
247 if (ret)
248 ret = clock_gettime_fallback(clkid, ts);
249
250 return ret;
251 }
252