Commit | Line | Data |
---|---|---|
8512287a NL |
1 | /* |
2 | * Copyright 2015 Mentor Graphics Corporation. | |
3 | * | |
4 | * This program is free software; you can redistribute it and/or | |
5 | * modify it under the terms of the GNU General Public License | |
6 | * as published by the Free Software Foundation; version 2 of the | |
7 | * License. | |
8 | * | |
9 | * This program is distributed in the hope that it will be useful, but | |
10 | * WITHOUT ANY WARRANTY; without even the implied warranty of | |
11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU | |
12 | * General Public License for more details. | |
13 | * | |
14 | * You should have received a copy of the GNU General Public License | |
15 | * along with this program. If not, see <http://www.gnu.org/licenses/>. | |
16 | */ | |
17 | ||
18 | #include <linux/compiler.h> | |
19 | #include <linux/hrtimer.h> | |
20 | #include <linux/time.h> | |
21 | #include <asm/arch_timer.h> | |
22 | #include <asm/barrier.h> | |
23 | #include <asm/bug.h> | |
24 | #include <asm/page.h> | |
25 | #include <asm/unistd.h> | |
26 | #include <asm/vdso_datapage.h> | |
27 | ||
28 | #ifndef CONFIG_AEABI | |
29 | #error This code depends on AEABI system call conventions | |
30 | #endif | |
31 | ||
32 | extern struct vdso_data *__get_datapage(void); | |
33 | ||
34 | static notrace u32 __vdso_read_begin(const struct vdso_data *vdata) | |
35 | { | |
36 | u32 seq; | |
37 | repeat: | |
38 | seq = ACCESS_ONCE(vdata->seq_count); | |
39 | if (seq & 1) { | |
40 | cpu_relax(); | |
41 | goto repeat; | |
42 | } | |
43 | return seq; | |
44 | } | |
45 | ||
46 | static notrace u32 vdso_read_begin(const struct vdso_data *vdata) | |
47 | { | |
48 | u32 seq; | |
49 | ||
50 | seq = __vdso_read_begin(vdata); | |
51 | ||
52 | smp_rmb(); /* Pairs with smp_wmb in vdso_write_end */ | |
53 | return seq; | |
54 | } | |
55 | ||
56 | static notrace int vdso_read_retry(const struct vdso_data *vdata, u32 start) | |
57 | { | |
58 | smp_rmb(); /* Pairs with smp_wmb in vdso_write_begin */ | |
59 | return vdata->seq_count != start; | |
60 | } | |
61 | ||
62 | static notrace long clock_gettime_fallback(clockid_t _clkid, | |
63 | struct timespec *_ts) | |
64 | { | |
65 | register struct timespec *ts asm("r1") = _ts; | |
66 | register clockid_t clkid asm("r0") = _clkid; | |
67 | register long ret asm ("r0"); | |
68 | register long nr asm("r7") = __NR_clock_gettime; | |
69 | ||
70 | asm volatile( | |
71 | " swi #0\n" | |
72 | : "=r" (ret) | |
73 | : "r" (clkid), "r" (ts), "r" (nr) | |
74 | : "memory"); | |
75 | ||
76 | return ret; | |
77 | } | |
78 | ||
79 | static notrace int do_realtime_coarse(struct timespec *ts, | |
80 | struct vdso_data *vdata) | |
81 | { | |
82 | u32 seq; | |
83 | ||
84 | do { | |
85 | seq = vdso_read_begin(vdata); | |
86 | ||
87 | ts->tv_sec = vdata->xtime_coarse_sec; | |
88 | ts->tv_nsec = vdata->xtime_coarse_nsec; | |
89 | ||
90 | } while (vdso_read_retry(vdata, seq)); | |
91 | ||
92 | return 0; | |
93 | } | |
94 | ||
95 | static notrace int do_monotonic_coarse(struct timespec *ts, | |
96 | struct vdso_data *vdata) | |
97 | { | |
98 | struct timespec tomono; | |
99 | u32 seq; | |
100 | ||
101 | do { | |
102 | seq = vdso_read_begin(vdata); | |
103 | ||
104 | ts->tv_sec = vdata->xtime_coarse_sec; | |
105 | ts->tv_nsec = vdata->xtime_coarse_nsec; | |
106 | ||
107 | tomono.tv_sec = vdata->wtm_clock_sec; | |
108 | tomono.tv_nsec = vdata->wtm_clock_nsec; | |
109 | ||
110 | } while (vdso_read_retry(vdata, seq)); | |
111 | ||
112 | ts->tv_sec += tomono.tv_sec; | |
113 | timespec_add_ns(ts, tomono.tv_nsec); | |
114 | ||
115 | return 0; | |
116 | } | |
117 | ||
118 | #ifdef CONFIG_ARM_ARCH_TIMER | |
119 | ||
120 | static notrace u64 get_ns(struct vdso_data *vdata) | |
121 | { | |
122 | u64 cycle_delta; | |
123 | u64 cycle_now; | |
124 | u64 nsec; | |
125 | ||
126 | cycle_now = arch_counter_get_cntvct(); | |
127 | ||
128 | cycle_delta = (cycle_now - vdata->cs_cycle_last) & vdata->cs_mask; | |
129 | ||
130 | nsec = (cycle_delta * vdata->cs_mult) + vdata->xtime_clock_snsec; | |
131 | nsec >>= vdata->cs_shift; | |
132 | ||
133 | return nsec; | |
134 | } | |
135 | ||
136 | static notrace int do_realtime(struct timespec *ts, struct vdso_data *vdata) | |
137 | { | |
138 | u64 nsecs; | |
139 | u32 seq; | |
140 | ||
141 | do { | |
142 | seq = vdso_read_begin(vdata); | |
143 | ||
144 | if (!vdata->tk_is_cntvct) | |
145 | return -1; | |
146 | ||
147 | ts->tv_sec = vdata->xtime_clock_sec; | |
148 | nsecs = get_ns(vdata); | |
149 | ||
150 | } while (vdso_read_retry(vdata, seq)); | |
151 | ||
152 | ts->tv_nsec = 0; | |
153 | timespec_add_ns(ts, nsecs); | |
154 | ||
155 | return 0; | |
156 | } | |
157 | ||
158 | static notrace int do_monotonic(struct timespec *ts, struct vdso_data *vdata) | |
159 | { | |
160 | struct timespec tomono; | |
161 | u64 nsecs; | |
162 | u32 seq; | |
163 | ||
164 | do { | |
165 | seq = vdso_read_begin(vdata); | |
166 | ||
167 | if (!vdata->tk_is_cntvct) | |
168 | return -1; | |
169 | ||
170 | ts->tv_sec = vdata->xtime_clock_sec; | |
171 | nsecs = get_ns(vdata); | |
172 | ||
173 | tomono.tv_sec = vdata->wtm_clock_sec; | |
174 | tomono.tv_nsec = vdata->wtm_clock_nsec; | |
175 | ||
176 | } while (vdso_read_retry(vdata, seq)); | |
177 | ||
178 | ts->tv_sec += tomono.tv_sec; | |
179 | ts->tv_nsec = 0; | |
180 | timespec_add_ns(ts, nsecs + tomono.tv_nsec); | |
181 | ||
182 | return 0; | |
183 | } | |
184 | ||
185 | #else /* CONFIG_ARM_ARCH_TIMER */ | |
186 | ||
187 | static notrace int do_realtime(struct timespec *ts, struct vdso_data *vdata) | |
188 | { | |
189 | return -1; | |
190 | } | |
191 | ||
192 | static notrace int do_monotonic(struct timespec *ts, struct vdso_data *vdata) | |
193 | { | |
194 | return -1; | |
195 | } | |
196 | ||
197 | #endif /* CONFIG_ARM_ARCH_TIMER */ | |
198 | ||
199 | notrace int __vdso_clock_gettime(clockid_t clkid, struct timespec *ts) | |
200 | { | |
201 | struct vdso_data *vdata; | |
202 | int ret = -1; | |
203 | ||
204 | vdata = __get_datapage(); | |
205 | ||
206 | switch (clkid) { | |
207 | case CLOCK_REALTIME_COARSE: | |
208 | ret = do_realtime_coarse(ts, vdata); | |
209 | break; | |
210 | case CLOCK_MONOTONIC_COARSE: | |
211 | ret = do_monotonic_coarse(ts, vdata); | |
212 | break; | |
213 | case CLOCK_REALTIME: | |
214 | ret = do_realtime(ts, vdata); | |
215 | break; | |
216 | case CLOCK_MONOTONIC: | |
217 | ret = do_monotonic(ts, vdata); | |
218 | break; | |
219 | default: | |
220 | break; | |
221 | } | |
222 | ||
223 | if (ret) | |
224 | ret = clock_gettime_fallback(clkid, ts); | |
225 | ||
226 | return ret; | |
227 | } | |
228 | ||
229 | static notrace long gettimeofday_fallback(struct timeval *_tv, | |
230 | struct timezone *_tz) | |
231 | { | |
232 | register struct timezone *tz asm("r1") = _tz; | |
233 | register struct timeval *tv asm("r0") = _tv; | |
234 | register long ret asm ("r0"); | |
235 | register long nr asm("r7") = __NR_gettimeofday; | |
236 | ||
237 | asm volatile( | |
238 | " swi #0\n" | |
239 | : "=r" (ret) | |
240 | : "r" (tv), "r" (tz), "r" (nr) | |
241 | : "memory"); | |
242 | ||
243 | return ret; | |
244 | } | |
245 | ||
246 | notrace int __vdso_gettimeofday(struct timeval *tv, struct timezone *tz) | |
247 | { | |
248 | struct timespec ts; | |
249 | struct vdso_data *vdata; | |
250 | int ret; | |
251 | ||
252 | vdata = __get_datapage(); | |
253 | ||
254 | ret = do_realtime(&ts, vdata); | |
255 | if (ret) | |
256 | return gettimeofday_fallback(tv, tz); | |
257 | ||
258 | if (tv) { | |
259 | tv->tv_sec = ts.tv_sec; | |
260 | tv->tv_usec = ts.tv_nsec / 1000; | |
261 | } | |
262 | if (tz) { | |
263 | tz->tz_minuteswest = vdata->tz_minuteswest; | |
264 | tz->tz_dsttime = vdata->tz_dsttime; | |
265 | } | |
266 | ||
267 | return ret; | |
268 | } | |
269 | ||
270 | /* Avoid unresolved references emitted by GCC */ | |
271 | ||
272 | void __aeabi_unwind_cpp_pr0(void) | |
273 | { | |
274 | } | |
275 | ||
276 | void __aeabi_unwind_cpp_pr1(void) | |
277 | { | |
278 | } | |
279 | ||
280 | void __aeabi_unwind_cpp_pr2(void) | |
281 | { | |
282 | } |