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