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