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
32extern struct vdso_data *__get_datapage(void);
33
34static notrace u32 __vdso_read_begin(const struct vdso_data *vdata)
35{
36	u32 seq;
37repeat:
38	seq = ACCESS_ONCE(vdata->seq_count);
39	if (seq & 1) {
40		cpu_relax();
41		goto repeat;
42	}
43	return seq;
44}
45
46static 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
56static 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
62static 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
79static 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
95static 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
120static 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
136static 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
158static 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
187static notrace int do_realtime(struct timespec *ts, struct vdso_data *vdata)
188{
189	return -1;
190}
191
192static notrace int do_monotonic(struct timespec *ts, struct vdso_data *vdata)
193{
194	return -1;
195}
196
197#endif /* CONFIG_ARM_ARCH_TIMER */
198
199notrace 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
229static 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
246notrace 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
272void __aeabi_unwind_cpp_pr0(void)
273{
274}
275
276void __aeabi_unwind_cpp_pr1(void)
277{
278}
279
280void __aeabi_unwind_cpp_pr2(void)
281{
282}
283