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/barrier.h>
22 #include <asm/bug.h>
23 #include <asm/cp15.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
__vdso_read_begin(const struct vdso_data * vdata)34 static notrace u32 __vdso_read_begin(const struct vdso_data *vdata)
35 {
36 u32 seq;
37 repeat:
38 seq = READ_ONCE(vdata->seq_count);
39 if (seq & 1) {
40 cpu_relax();
41 goto repeat;
42 }
43 return seq;
44 }
45
vdso_read_begin(const struct vdso_data * vdata)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
vdso_read_retry(const struct vdso_data * vdata,u32 start)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
clock_gettime_fallback(clockid_t _clkid,struct timespec * _ts)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
do_realtime_coarse(struct timespec * ts,struct vdso_data * vdata)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
do_monotonic_coarse(struct timespec * ts,struct vdso_data * vdata)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
get_ns(struct vdso_data * vdata)120 static notrace u64 get_ns(struct vdso_data *vdata)
121 {
122 u64 cycle_delta;
123 u64 cycle_now;
124 u64 nsec;
125
126 isb();
127 cycle_now = read_sysreg(CNTVCT);
128
129 cycle_delta = (cycle_now - vdata->cs_cycle_last) & vdata->cs_mask;
130
131 nsec = (cycle_delta * vdata->cs_mult) + vdata->xtime_clock_snsec;
132 nsec >>= vdata->cs_shift;
133
134 return nsec;
135 }
136
do_realtime(struct timespec * ts,struct vdso_data * vdata)137 static notrace int do_realtime(struct timespec *ts, struct vdso_data *vdata)
138 {
139 u64 nsecs;
140 u32 seq;
141
142 do {
143 seq = vdso_read_begin(vdata);
144
145 if (!vdata->tk_is_cntvct)
146 return -1;
147
148 ts->tv_sec = vdata->xtime_clock_sec;
149 nsecs = get_ns(vdata);
150
151 } while (vdso_read_retry(vdata, seq));
152
153 ts->tv_nsec = 0;
154 timespec_add_ns(ts, nsecs);
155
156 return 0;
157 }
158
do_monotonic(struct timespec * ts,struct vdso_data * vdata)159 static notrace int do_monotonic(struct timespec *ts, struct vdso_data *vdata)
160 {
161 struct timespec tomono;
162 u64 nsecs;
163 u32 seq;
164
165 do {
166 seq = vdso_read_begin(vdata);
167
168 if (!vdata->tk_is_cntvct)
169 return -1;
170
171 ts->tv_sec = vdata->xtime_clock_sec;
172 nsecs = get_ns(vdata);
173
174 tomono.tv_sec = vdata->wtm_clock_sec;
175 tomono.tv_nsec = vdata->wtm_clock_nsec;
176
177 } while (vdso_read_retry(vdata, seq));
178
179 ts->tv_sec += tomono.tv_sec;
180 ts->tv_nsec = 0;
181 timespec_add_ns(ts, nsecs + tomono.tv_nsec);
182
183 return 0;
184 }
185
186 #else /* CONFIG_ARM_ARCH_TIMER */
187
do_realtime(struct timespec * ts,struct vdso_data * vdata)188 static notrace int do_realtime(struct timespec *ts, struct vdso_data *vdata)
189 {
190 return -1;
191 }
192
do_monotonic(struct timespec * ts,struct vdso_data * vdata)193 static notrace int do_monotonic(struct timespec *ts, struct vdso_data *vdata)
194 {
195 return -1;
196 }
197
198 #endif /* CONFIG_ARM_ARCH_TIMER */
199
__vdso_clock_gettime(clockid_t clkid,struct timespec * ts)200 notrace int __vdso_clock_gettime(clockid_t clkid, struct timespec *ts)
201 {
202 struct vdso_data *vdata;
203 int ret = -1;
204
205 vdata = __get_datapage();
206
207 switch (clkid) {
208 case CLOCK_REALTIME_COARSE:
209 ret = do_realtime_coarse(ts, vdata);
210 break;
211 case CLOCK_MONOTONIC_COARSE:
212 ret = do_monotonic_coarse(ts, vdata);
213 break;
214 case CLOCK_REALTIME:
215 ret = do_realtime(ts, vdata);
216 break;
217 case CLOCK_MONOTONIC:
218 ret = do_monotonic(ts, vdata);
219 break;
220 default:
221 break;
222 }
223
224 if (ret)
225 ret = clock_gettime_fallback(clkid, ts);
226
227 return ret;
228 }
229
gettimeofday_fallback(struct timeval * _tv,struct timezone * _tz)230 static notrace long gettimeofday_fallback(struct timeval *_tv,
231 struct timezone *_tz)
232 {
233 register struct timezone *tz asm("r1") = _tz;
234 register struct timeval *tv asm("r0") = _tv;
235 register long ret asm ("r0");
236 register long nr asm("r7") = __NR_gettimeofday;
237
238 asm volatile(
239 " swi #0\n"
240 : "=r" (ret)
241 : "r" (tv), "r" (tz), "r" (nr)
242 : "memory");
243
244 return ret;
245 }
246
__vdso_gettimeofday(struct timeval * tv,struct timezone * tz)247 notrace int __vdso_gettimeofday(struct timeval *tv, struct timezone *tz)
248 {
249 struct timespec ts;
250 struct vdso_data *vdata;
251 int ret;
252
253 vdata = __get_datapage();
254
255 ret = do_realtime(&ts, vdata);
256 if (ret)
257 return gettimeofday_fallback(tv, tz);
258
259 if (tv) {
260 tv->tv_sec = ts.tv_sec;
261 tv->tv_usec = ts.tv_nsec / 1000;
262 }
263 if (tz) {
264 tz->tz_minuteswest = vdata->tz_minuteswest;
265 tz->tz_dsttime = vdata->tz_dsttime;
266 }
267
268 return ret;
269 }
270
271 /* Avoid unresolved references emitted by GCC */
272
__aeabi_unwind_cpp_pr0(void)273 void __aeabi_unwind_cpp_pr0(void)
274 {
275 }
276
__aeabi_unwind_cpp_pr1(void)277 void __aeabi_unwind_cpp_pr1(void)
278 {
279 }
280
__aeabi_unwind_cpp_pr2(void)281 void __aeabi_unwind_cpp_pr2(void)
282 {
283 }
284