Commit | Line | Data |
---|---|---|
a7f4df4e AS |
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 | ||
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 | ||
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 | ||
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 | ||
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 | ||
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 | ||
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 | ||
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 | */ | |
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 | ||
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 | } |