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