1 | // SPDX-License-Identifier: GPL-2.0-or-later |
2 | /* |
3 | * PTP virtual clock driver |
4 | * |
5 | * Copyright 2021 NXP |
6 | */ |
7 | #include <linux/slab.h> |
8 | #include <linux/hashtable.h> |
9 | #include "ptp_private.h" |
10 | |
11 | #define PTP_VCLOCK_CC_SHIFT 31 |
12 | #define PTP_VCLOCK_CC_MULT (1 << PTP_VCLOCK_CC_SHIFT) |
13 | #define PTP_VCLOCK_FADJ_SHIFT 9 |
14 | #define PTP_VCLOCK_FADJ_DENOMINATOR 15625ULL |
15 | #define PTP_VCLOCK_REFRESH_INTERVAL (HZ * 2) |
16 | |
17 | /* protects vclock_hash addition/deletion */ |
18 | static DEFINE_SPINLOCK(vclock_hash_lock); |
19 | |
20 | static DEFINE_READ_MOSTLY_HASHTABLE(vclock_hash, 8); |
21 | |
22 | static void ptp_vclock_hash_add(struct ptp_vclock *vclock) |
23 | { |
24 | spin_lock(lock: &vclock_hash_lock); |
25 | |
26 | hlist_add_head_rcu(n: &vclock->vclock_hash_node, |
27 | h: &vclock_hash[vclock->clock->index % HASH_SIZE(vclock_hash)]); |
28 | |
29 | spin_unlock(lock: &vclock_hash_lock); |
30 | } |
31 | |
32 | static void ptp_vclock_hash_del(struct ptp_vclock *vclock) |
33 | { |
34 | spin_lock(lock: &vclock_hash_lock); |
35 | |
36 | hlist_del_init_rcu(n: &vclock->vclock_hash_node); |
37 | |
38 | spin_unlock(lock: &vclock_hash_lock); |
39 | |
40 | synchronize_rcu(); |
41 | } |
42 | |
43 | static int ptp_vclock_adjfine(struct ptp_clock_info *ptp, long scaled_ppm) |
44 | { |
45 | struct ptp_vclock *vclock = info_to_vclock(ptp); |
46 | s64 adj; |
47 | |
48 | adj = (s64)scaled_ppm << PTP_VCLOCK_FADJ_SHIFT; |
49 | adj = div_s64(dividend: adj, PTP_VCLOCK_FADJ_DENOMINATOR); |
50 | |
51 | if (mutex_lock_interruptible(&vclock->lock)) |
52 | return -EINTR; |
53 | timecounter_read(tc: &vclock->tc); |
54 | vclock->cc.mult = PTP_VCLOCK_CC_MULT + adj; |
55 | mutex_unlock(lock: &vclock->lock); |
56 | |
57 | return 0; |
58 | } |
59 | |
60 | static int ptp_vclock_adjtime(struct ptp_clock_info *ptp, s64 delta) |
61 | { |
62 | struct ptp_vclock *vclock = info_to_vclock(ptp); |
63 | |
64 | if (mutex_lock_interruptible(&vclock->lock)) |
65 | return -EINTR; |
66 | timecounter_adjtime(tc: &vclock->tc, delta); |
67 | mutex_unlock(lock: &vclock->lock); |
68 | |
69 | return 0; |
70 | } |
71 | |
72 | static int ptp_vclock_gettime(struct ptp_clock_info *ptp, |
73 | struct timespec64 *ts) |
74 | { |
75 | struct ptp_vclock *vclock = info_to_vclock(ptp); |
76 | u64 ns; |
77 | |
78 | if (mutex_lock_interruptible(&vclock->lock)) |
79 | return -EINTR; |
80 | ns = timecounter_read(tc: &vclock->tc); |
81 | mutex_unlock(lock: &vclock->lock); |
82 | *ts = ns_to_timespec64(nsec: ns); |
83 | |
84 | return 0; |
85 | } |
86 | |
87 | static int ptp_vclock_gettimex(struct ptp_clock_info *ptp, |
88 | struct timespec64 *ts, |
89 | struct ptp_system_timestamp *sts) |
90 | { |
91 | struct ptp_vclock *vclock = info_to_vclock(ptp); |
92 | struct ptp_clock *pptp = vclock->pclock; |
93 | struct timespec64 pts; |
94 | int err; |
95 | u64 ns; |
96 | |
97 | err = pptp->info->getcyclesx64(pptp->info, &pts, sts); |
98 | if (err) |
99 | return err; |
100 | |
101 | if (mutex_lock_interruptible(&vclock->lock)) |
102 | return -EINTR; |
103 | ns = timecounter_cyc2time(tc: &vclock->tc, cycle_tstamp: timespec64_to_ns(ts: &pts)); |
104 | mutex_unlock(lock: &vclock->lock); |
105 | |
106 | *ts = ns_to_timespec64(nsec: ns); |
107 | |
108 | return 0; |
109 | } |
110 | |
111 | static int ptp_vclock_settime(struct ptp_clock_info *ptp, |
112 | const struct timespec64 *ts) |
113 | { |
114 | struct ptp_vclock *vclock = info_to_vclock(ptp); |
115 | u64 ns = timespec64_to_ns(ts); |
116 | |
117 | if (mutex_lock_interruptible(&vclock->lock)) |
118 | return -EINTR; |
119 | timecounter_init(tc: &vclock->tc, cc: &vclock->cc, start_tstamp: ns); |
120 | mutex_unlock(lock: &vclock->lock); |
121 | |
122 | return 0; |
123 | } |
124 | |
125 | static int ptp_vclock_getcrosststamp(struct ptp_clock_info *ptp, |
126 | struct system_device_crosststamp *xtstamp) |
127 | { |
128 | struct ptp_vclock *vclock = info_to_vclock(ptp); |
129 | struct ptp_clock *pptp = vclock->pclock; |
130 | int err; |
131 | u64 ns; |
132 | |
133 | err = pptp->info->getcrosscycles(pptp->info, xtstamp); |
134 | if (err) |
135 | return err; |
136 | |
137 | if (mutex_lock_interruptible(&vclock->lock)) |
138 | return -EINTR; |
139 | ns = timecounter_cyc2time(tc: &vclock->tc, cycle_tstamp: ktime_to_ns(kt: xtstamp->device)); |
140 | mutex_unlock(lock: &vclock->lock); |
141 | |
142 | xtstamp->device = ns_to_ktime(ns); |
143 | |
144 | return 0; |
145 | } |
146 | |
147 | static long ptp_vclock_refresh(struct ptp_clock_info *ptp) |
148 | { |
149 | struct ptp_vclock *vclock = info_to_vclock(ptp); |
150 | struct timespec64 ts; |
151 | |
152 | ptp_vclock_gettime(ptp: &vclock->info, ts: &ts); |
153 | |
154 | return PTP_VCLOCK_REFRESH_INTERVAL; |
155 | } |
156 | |
157 | static const struct ptp_clock_info ptp_vclock_info = { |
158 | .owner = THIS_MODULE, |
159 | .name = "ptp virtual clock" , |
160 | .max_adj = 500000000, |
161 | .adjfine = ptp_vclock_adjfine, |
162 | .adjtime = ptp_vclock_adjtime, |
163 | .settime64 = ptp_vclock_settime, |
164 | .do_aux_work = ptp_vclock_refresh, |
165 | }; |
166 | |
167 | static u64 ptp_vclock_read(const struct cyclecounter *cc) |
168 | { |
169 | struct ptp_vclock *vclock = cc_to_vclock(cc); |
170 | struct ptp_clock *ptp = vclock->pclock; |
171 | struct timespec64 ts = {}; |
172 | |
173 | ptp->info->getcycles64(ptp->info, &ts); |
174 | |
175 | return timespec64_to_ns(ts: &ts); |
176 | } |
177 | |
178 | static const struct cyclecounter ptp_vclock_cc = { |
179 | .read = ptp_vclock_read, |
180 | .mask = CYCLECOUNTER_MASK(32), |
181 | .mult = PTP_VCLOCK_CC_MULT, |
182 | .shift = PTP_VCLOCK_CC_SHIFT, |
183 | }; |
184 | |
185 | struct ptp_vclock *ptp_vclock_register(struct ptp_clock *pclock) |
186 | { |
187 | struct ptp_vclock *vclock; |
188 | |
189 | vclock = kzalloc(size: sizeof(*vclock), GFP_KERNEL); |
190 | if (!vclock) |
191 | return NULL; |
192 | |
193 | vclock->pclock = pclock; |
194 | vclock->info = ptp_vclock_info; |
195 | if (pclock->info->getcyclesx64) |
196 | vclock->info.gettimex64 = ptp_vclock_gettimex; |
197 | else |
198 | vclock->info.gettime64 = ptp_vclock_gettime; |
199 | if (pclock->info->getcrosscycles) |
200 | vclock->info.getcrosststamp = ptp_vclock_getcrosststamp; |
201 | vclock->cc = ptp_vclock_cc; |
202 | |
203 | snprintf(buf: vclock->info.name, PTP_CLOCK_NAME_LEN, fmt: "ptp%d_virt" , |
204 | pclock->index); |
205 | |
206 | INIT_HLIST_NODE(h: &vclock->vclock_hash_node); |
207 | |
208 | mutex_init(&vclock->lock); |
209 | |
210 | vclock->clock = ptp_clock_register(info: &vclock->info, parent: &pclock->dev); |
211 | if (IS_ERR_OR_NULL(ptr: vclock->clock)) { |
212 | kfree(objp: vclock); |
213 | return NULL; |
214 | } |
215 | |
216 | timecounter_init(tc: &vclock->tc, cc: &vclock->cc, start_tstamp: 0); |
217 | ptp_schedule_worker(ptp: vclock->clock, PTP_VCLOCK_REFRESH_INTERVAL); |
218 | |
219 | ptp_vclock_hash_add(vclock); |
220 | |
221 | return vclock; |
222 | } |
223 | |
224 | void ptp_vclock_unregister(struct ptp_vclock *vclock) |
225 | { |
226 | ptp_vclock_hash_del(vclock); |
227 | |
228 | ptp_clock_unregister(ptp: vclock->clock); |
229 | kfree(objp: vclock); |
230 | } |
231 | |
232 | #if IS_BUILTIN(CONFIG_PTP_1588_CLOCK) |
233 | int ptp_get_vclocks_index(int pclock_index, int **vclock_index) |
234 | { |
235 | char name[PTP_CLOCK_NAME_LEN] = "" ; |
236 | struct ptp_clock *ptp; |
237 | struct device *dev; |
238 | int num = 0; |
239 | |
240 | if (pclock_index < 0) |
241 | return num; |
242 | |
243 | snprintf(buf: name, PTP_CLOCK_NAME_LEN, fmt: "ptp%d" , pclock_index); |
244 | dev = class_find_device_by_name(class: &ptp_class, name); |
245 | if (!dev) |
246 | return num; |
247 | |
248 | ptp = dev_get_drvdata(dev); |
249 | |
250 | if (mutex_lock_interruptible(&ptp->n_vclocks_mux)) { |
251 | put_device(dev); |
252 | return num; |
253 | } |
254 | |
255 | *vclock_index = kzalloc(size: sizeof(int) * ptp->n_vclocks, GFP_KERNEL); |
256 | if (!(*vclock_index)) |
257 | goto out; |
258 | |
259 | memcpy(*vclock_index, ptp->vclock_index, sizeof(int) * ptp->n_vclocks); |
260 | num = ptp->n_vclocks; |
261 | out: |
262 | mutex_unlock(lock: &ptp->n_vclocks_mux); |
263 | put_device(dev); |
264 | return num; |
265 | } |
266 | EXPORT_SYMBOL(ptp_get_vclocks_index); |
267 | |
268 | ktime_t ptp_convert_timestamp(const ktime_t *hwtstamp, int vclock_index) |
269 | { |
270 | unsigned int hash = vclock_index % HASH_SIZE(vclock_hash); |
271 | struct ptp_vclock *vclock; |
272 | u64 ns; |
273 | u64 vclock_ns = 0; |
274 | |
275 | ns = ktime_to_ns(kt: *hwtstamp); |
276 | |
277 | rcu_read_lock(); |
278 | |
279 | hlist_for_each_entry_rcu(vclock, &vclock_hash[hash], vclock_hash_node) { |
280 | if (vclock->clock->index != vclock_index) |
281 | continue; |
282 | |
283 | if (mutex_lock_interruptible(&vclock->lock)) |
284 | break; |
285 | vclock_ns = timecounter_cyc2time(tc: &vclock->tc, cycle_tstamp: ns); |
286 | mutex_unlock(lock: &vclock->lock); |
287 | break; |
288 | } |
289 | |
290 | rcu_read_unlock(); |
291 | |
292 | return ns_to_ktime(ns: vclock_ns); |
293 | } |
294 | EXPORT_SYMBOL(ptp_convert_timestamp); |
295 | #endif |
296 | |