1 // SPDX-License-Identifier: GPL-2.0-or-later
2 /*
3 * PTP 1588 clock support
4 *
5 * Copyright (C) 2010 OMICRON electronics GmbH
6 */
7 #include <linux/idr.h>
8 #include <linux/device.h>
9 #include <linux/err.h>
10 #include <linux/init.h>
11 #include <linux/kernel.h>
12 #include <linux/module.h>
13 #include <linux/posix-clock.h>
14 #include <linux/pps_kernel.h>
15 #include <linux/slab.h>
16 #include <linux/syscalls.h>
17 #include <linux/uaccess.h>
18 #include <uapi/linux/sched/types.h>
19
20 #include "ptp_private.h"
21
22 #define PTP_MAX_ALARMS 4
23 #define PTP_PPS_DEFAULTS (PPS_CAPTUREASSERT | PPS_OFFSETASSERT)
24 #define PTP_PPS_EVENT PPS_CAPTUREASSERT
25 #define PTP_PPS_MODE (PTP_PPS_DEFAULTS | PPS_CANWAIT | PPS_TSFMT_TSPEC)
26
27 struct class *ptp_class;
28
29 /* private globals */
30
31 static dev_t ptp_devt;
32
33 static DEFINE_IDA(ptp_clocks_map);
34
35 /* time stamp event queue operations */
36
queue_free(struct timestamp_event_queue * q)37 static inline int queue_free(struct timestamp_event_queue *q)
38 {
39 return PTP_MAX_TIMESTAMPS - queue_cnt(q) - 1;
40 }
41
enqueue_external_timestamp(struct timestamp_event_queue * queue,struct ptp_clock_event * src)42 static void enqueue_external_timestamp(struct timestamp_event_queue *queue,
43 struct ptp_clock_event *src)
44 {
45 struct ptp_extts_event *dst;
46 unsigned long flags;
47 s64 seconds;
48 u32 remainder;
49
50 seconds = div_u64_rem(src->timestamp, 1000000000, &remainder);
51
52 spin_lock_irqsave(&queue->lock, flags);
53
54 dst = &queue->buf[queue->tail];
55 dst->index = src->index;
56 dst->t.sec = seconds;
57 dst->t.nsec = remainder;
58
59 /* Both WRITE_ONCE() are paired with READ_ONCE() in queue_cnt() */
60 if (!queue_free(queue))
61 WRITE_ONCE(queue->head, (queue->head + 1) % PTP_MAX_TIMESTAMPS);
62
63 WRITE_ONCE(queue->tail, (queue->tail + 1) % PTP_MAX_TIMESTAMPS);
64
65 spin_unlock_irqrestore(&queue->lock, flags);
66 }
67
68 /* posix clock implementation */
69
ptp_clock_getres(struct posix_clock * pc,struct timespec64 * tp)70 static int ptp_clock_getres(struct posix_clock *pc, struct timespec64 *tp)
71 {
72 tp->tv_sec = 0;
73 tp->tv_nsec = 1;
74 return 0;
75 }
76
ptp_clock_settime(struct posix_clock * pc,const struct timespec64 * tp)77 static int ptp_clock_settime(struct posix_clock *pc, const struct timespec64 *tp)
78 {
79 struct ptp_clock *ptp = container_of(pc, struct ptp_clock, clock);
80
81 if (ptp_clock_freerun(ptp)) {
82 pr_err("ptp: physical clock is free running\n");
83 return -EBUSY;
84 }
85
86 return ptp->info->settime64(ptp->info, tp);
87 }
88
ptp_clock_gettime(struct posix_clock * pc,struct timespec64 * tp)89 static int ptp_clock_gettime(struct posix_clock *pc, struct timespec64 *tp)
90 {
91 struct ptp_clock *ptp = container_of(pc, struct ptp_clock, clock);
92 int err;
93
94 if (ptp->info->gettimex64)
95 err = ptp->info->gettimex64(ptp->info, tp, NULL);
96 else
97 err = ptp->info->gettime64(ptp->info, tp);
98 return err;
99 }
100
ptp_clock_adjtime(struct posix_clock * pc,struct __kernel_timex * tx)101 static int ptp_clock_adjtime(struct posix_clock *pc, struct __kernel_timex *tx)
102 {
103 struct ptp_clock *ptp = container_of(pc, struct ptp_clock, clock);
104 struct ptp_clock_info *ops;
105 int err = -EOPNOTSUPP;
106
107 if (tx->modes & (ADJ_SETOFFSET | ADJ_FREQUENCY | ADJ_OFFSET) &&
108 ptp_clock_freerun(ptp)) {
109 pr_err("ptp: physical clock is free running\n");
110 return -EBUSY;
111 }
112
113 ops = ptp->info;
114
115 if (tx->modes & ADJ_SETOFFSET) {
116 struct timespec64 ts;
117 ktime_t kt;
118 s64 delta;
119
120 ts.tv_sec = tx->time.tv_sec;
121 ts.tv_nsec = tx->time.tv_usec;
122
123 if (!(tx->modes & ADJ_NANO))
124 ts.tv_nsec *= 1000;
125
126 if ((unsigned long) ts.tv_nsec >= NSEC_PER_SEC)
127 return -EINVAL;
128
129 kt = timespec64_to_ktime(ts);
130 delta = ktime_to_ns(kt);
131 err = ops->adjtime(ops, delta);
132 } else if (tx->modes & ADJ_FREQUENCY) {
133 long ppb = scaled_ppm_to_ppb(tx->freq);
134 if (ppb > ops->max_adj || ppb < -ops->max_adj)
135 return -ERANGE;
136 err = ops->adjfine(ops, tx->freq);
137 if (!err)
138 ptp->dialed_frequency = tx->freq;
139 } else if (tx->modes & ADJ_OFFSET) {
140 if (ops->adjphase) {
141 s32 max_phase_adj = ops->getmaxphase(ops);
142 s32 offset = tx->offset;
143
144 if (!(tx->modes & ADJ_NANO))
145 offset *= NSEC_PER_USEC;
146
147 if (offset > max_phase_adj || offset < -max_phase_adj)
148 return -ERANGE;
149
150 err = ops->adjphase(ops, offset);
151 }
152 } else if (tx->modes == 0) {
153 tx->freq = ptp->dialed_frequency;
154 err = 0;
155 }
156
157 return err;
158 }
159
160 static struct posix_clock_operations ptp_clock_ops = {
161 .owner = THIS_MODULE,
162 .clock_adjtime = ptp_clock_adjtime,
163 .clock_gettime = ptp_clock_gettime,
164 .clock_getres = ptp_clock_getres,
165 .clock_settime = ptp_clock_settime,
166 .ioctl = ptp_ioctl,
167 .open = ptp_open,
168 .poll = ptp_poll,
169 .read = ptp_read,
170 };
171
ptp_clock_release(struct device * dev)172 static void ptp_clock_release(struct device *dev)
173 {
174 struct ptp_clock *ptp = container_of(dev, struct ptp_clock, dev);
175
176 ptp_cleanup_pin_groups(ptp);
177 kfree(ptp->vclock_index);
178 mutex_destroy(&ptp->tsevq_mux);
179 mutex_destroy(&ptp->pincfg_mux);
180 mutex_destroy(&ptp->n_vclocks_mux);
181 ida_free(&ptp_clocks_map, ptp->index);
182 kfree(ptp);
183 }
184
ptp_getcycles64(struct ptp_clock_info * info,struct timespec64 * ts)185 static int ptp_getcycles64(struct ptp_clock_info *info, struct timespec64 *ts)
186 {
187 if (info->getcyclesx64)
188 return info->getcyclesx64(info, ts, NULL);
189 else
190 return info->gettime64(info, ts);
191 }
192
ptp_enable(struct ptp_clock_info * ptp,struct ptp_clock_request * request,int on)193 static int ptp_enable(struct ptp_clock_info *ptp, struct ptp_clock_request *request, int on)
194 {
195 return -EOPNOTSUPP;
196 }
197
ptp_aux_kworker(struct kthread_work * work)198 static void ptp_aux_kworker(struct kthread_work *work)
199 {
200 struct ptp_clock *ptp = container_of(work, struct ptp_clock,
201 aux_work.work);
202 struct ptp_clock_info *info = ptp->info;
203 long delay;
204
205 delay = info->do_aux_work(info);
206
207 if (delay >= 0)
208 kthread_queue_delayed_work(ptp->kworker, &ptp->aux_work, delay);
209 }
210
211 /* public interface */
212
ptp_clock_register(struct ptp_clock_info * info,struct device * parent)213 struct ptp_clock *ptp_clock_register(struct ptp_clock_info *info,
214 struct device *parent)
215 {
216 struct ptp_clock *ptp;
217 int err = 0, index, major = MAJOR(ptp_devt);
218 size_t size;
219
220 if (info->n_alarm > PTP_MAX_ALARMS)
221 return ERR_PTR(-EINVAL);
222
223 /* Initialize a clock structure. */
224 err = -ENOMEM;
225 ptp = kzalloc(sizeof(struct ptp_clock), GFP_KERNEL);
226 if (ptp == NULL)
227 goto no_memory;
228
229 index = ida_alloc_max(&ptp_clocks_map, MINORMASK, GFP_KERNEL);
230 if (index < 0) {
231 err = index;
232 goto no_slot;
233 }
234
235 ptp->clock.ops = ptp_clock_ops;
236 ptp->info = info;
237 ptp->devid = MKDEV(major, index);
238 ptp->index = index;
239 spin_lock_init(&ptp->tsevq.lock);
240 mutex_init(&ptp->tsevq_mux);
241 mutex_init(&ptp->pincfg_mux);
242 mutex_init(&ptp->n_vclocks_mux);
243 init_waitqueue_head(&ptp->tsev_wq);
244
245 if (ptp->info->getcycles64 || ptp->info->getcyclesx64) {
246 ptp->has_cycles = true;
247 if (!ptp->info->getcycles64 && ptp->info->getcyclesx64)
248 ptp->info->getcycles64 = ptp_getcycles64;
249 } else {
250 /* Free running cycle counter not supported, use time. */
251 ptp->info->getcycles64 = ptp_getcycles64;
252
253 if (ptp->info->gettimex64)
254 ptp->info->getcyclesx64 = ptp->info->gettimex64;
255
256 if (ptp->info->getcrosststamp)
257 ptp->info->getcrosscycles = ptp->info->getcrosststamp;
258 }
259
260 if (!ptp->info->enable)
261 ptp->info->enable = ptp_enable;
262
263 if (ptp->info->do_aux_work) {
264 kthread_init_delayed_work(&ptp->aux_work, ptp_aux_kworker);
265 ptp->kworker = kthread_create_worker(0, "ptp%d", ptp->index);
266 if (IS_ERR(ptp->kworker)) {
267 err = PTR_ERR(ptp->kworker);
268 pr_err("failed to create ptp aux_worker %d\n", err);
269 goto kworker_err;
270 }
271 }
272
273 /* PTP virtual clock is being registered under physical clock */
274 if (parent && parent->class && parent->class->name &&
275 strcmp(parent->class->name, "ptp") == 0)
276 ptp->is_virtual_clock = true;
277
278 if (!ptp->is_virtual_clock) {
279 ptp->max_vclocks = PTP_DEFAULT_MAX_VCLOCKS;
280
281 size = sizeof(int) * ptp->max_vclocks;
282 ptp->vclock_index = kzalloc(size, GFP_KERNEL);
283 if (!ptp->vclock_index) {
284 err = -ENOMEM;
285 goto no_mem_for_vclocks;
286 }
287 }
288
289 err = ptp_populate_pin_groups(ptp);
290 if (err)
291 goto no_pin_groups;
292
293 /* Register a new PPS source. */
294 if (info->pps) {
295 struct pps_source_info pps;
296 memset(&pps, 0, sizeof(pps));
297 snprintf(pps.name, PPS_MAX_NAME_LEN, "ptp%d", index);
298 pps.mode = PTP_PPS_MODE;
299 pps.owner = info->owner;
300 ptp->pps_source = pps_register_source(&pps, PTP_PPS_DEFAULTS);
301 if (IS_ERR(ptp->pps_source)) {
302 err = PTR_ERR(ptp->pps_source);
303 pr_err("failed to register pps source\n");
304 goto no_pps;
305 }
306 ptp->pps_source->lookup_cookie = ptp;
307 }
308
309 /* Initialize a new device of our class in our clock structure. */
310 device_initialize(&ptp->dev);
311 ptp->dev.devt = ptp->devid;
312 ptp->dev.class = ptp_class;
313 ptp->dev.parent = parent;
314 ptp->dev.groups = ptp->pin_attr_groups;
315 ptp->dev.release = ptp_clock_release;
316 dev_set_drvdata(&ptp->dev, ptp);
317 dev_set_name(&ptp->dev, "ptp%d", ptp->index);
318
319 /* Create a posix clock and link it to the device. */
320 err = posix_clock_register(&ptp->clock, &ptp->dev);
321 if (err) {
322 if (ptp->pps_source)
323 pps_unregister_source(ptp->pps_source);
324
325 if (ptp->kworker)
326 kthread_destroy_worker(ptp->kworker);
327
328 put_device(&ptp->dev);
329
330 pr_err("failed to create posix clock\n");
331 return ERR_PTR(err);
332 }
333
334 return ptp;
335
336 no_pps:
337 ptp_cleanup_pin_groups(ptp);
338 no_pin_groups:
339 kfree(ptp->vclock_index);
340 no_mem_for_vclocks:
341 if (ptp->kworker)
342 kthread_destroy_worker(ptp->kworker);
343 kworker_err:
344 mutex_destroy(&ptp->tsevq_mux);
345 mutex_destroy(&ptp->pincfg_mux);
346 mutex_destroy(&ptp->n_vclocks_mux);
347 ida_free(&ptp_clocks_map, index);
348 no_slot:
349 kfree(ptp);
350 no_memory:
351 return ERR_PTR(err);
352 }
353 EXPORT_SYMBOL(ptp_clock_register);
354
unregister_vclock(struct device * dev,void * data)355 static int unregister_vclock(struct device *dev, void *data)
356 {
357 struct ptp_clock *ptp = dev_get_drvdata(dev);
358
359 ptp_vclock_unregister(info_to_vclock(ptp->info));
360 return 0;
361 }
362
ptp_clock_unregister(struct ptp_clock * ptp)363 int ptp_clock_unregister(struct ptp_clock *ptp)
364 {
365 if (ptp_vclock_in_use(ptp)) {
366 device_for_each_child(&ptp->dev, NULL, unregister_vclock);
367 }
368
369 ptp->defunct = 1;
370 wake_up_interruptible(&ptp->tsev_wq);
371
372 if (ptp->kworker) {
373 kthread_cancel_delayed_work_sync(&ptp->aux_work);
374 kthread_destroy_worker(ptp->kworker);
375 }
376
377 /* Release the clock's resources. */
378 if (ptp->pps_source)
379 pps_unregister_source(ptp->pps_source);
380
381 posix_clock_unregister(&ptp->clock);
382
383 return 0;
384 }
385 EXPORT_SYMBOL(ptp_clock_unregister);
386
ptp_clock_event(struct ptp_clock * ptp,struct ptp_clock_event * event)387 void ptp_clock_event(struct ptp_clock *ptp, struct ptp_clock_event *event)
388 {
389 struct pps_event_time evt;
390
391 switch (event->type) {
392
393 case PTP_CLOCK_ALARM:
394 break;
395
396 case PTP_CLOCK_EXTTS:
397 enqueue_external_timestamp(&ptp->tsevq, event);
398 wake_up_interruptible(&ptp->tsev_wq);
399 break;
400
401 case PTP_CLOCK_PPS:
402 pps_get_ts(&evt);
403 pps_event(ptp->pps_source, &evt, PTP_PPS_EVENT, NULL);
404 break;
405
406 case PTP_CLOCK_PPSUSR:
407 pps_event(ptp->pps_source, &event->pps_times,
408 PTP_PPS_EVENT, NULL);
409 break;
410 }
411 }
412 EXPORT_SYMBOL(ptp_clock_event);
413
ptp_clock_index(struct ptp_clock * ptp)414 int ptp_clock_index(struct ptp_clock *ptp)
415 {
416 return ptp->index;
417 }
418 EXPORT_SYMBOL(ptp_clock_index);
419
ptp_find_pin(struct ptp_clock * ptp,enum ptp_pin_function func,unsigned int chan)420 int ptp_find_pin(struct ptp_clock *ptp,
421 enum ptp_pin_function func, unsigned int chan)
422 {
423 struct ptp_pin_desc *pin = NULL;
424 int i;
425
426 for (i = 0; i < ptp->info->n_pins; i++) {
427 if (ptp->info->pin_config[i].func == func &&
428 ptp->info->pin_config[i].chan == chan) {
429 pin = &ptp->info->pin_config[i];
430 break;
431 }
432 }
433
434 return pin ? i : -1;
435 }
436 EXPORT_SYMBOL(ptp_find_pin);
437
ptp_find_pin_unlocked(struct ptp_clock * ptp,enum ptp_pin_function func,unsigned int chan)438 int ptp_find_pin_unlocked(struct ptp_clock *ptp,
439 enum ptp_pin_function func, unsigned int chan)
440 {
441 int result;
442
443 mutex_lock(&ptp->pincfg_mux);
444
445 result = ptp_find_pin(ptp, func, chan);
446
447 mutex_unlock(&ptp->pincfg_mux);
448
449 return result;
450 }
451 EXPORT_SYMBOL(ptp_find_pin_unlocked);
452
ptp_schedule_worker(struct ptp_clock * ptp,unsigned long delay)453 int ptp_schedule_worker(struct ptp_clock *ptp, unsigned long delay)
454 {
455 return kthread_mod_delayed_work(ptp->kworker, &ptp->aux_work, delay);
456 }
457 EXPORT_SYMBOL(ptp_schedule_worker);
458
ptp_cancel_worker_sync(struct ptp_clock * ptp)459 void ptp_cancel_worker_sync(struct ptp_clock *ptp)
460 {
461 kthread_cancel_delayed_work_sync(&ptp->aux_work);
462 }
463 EXPORT_SYMBOL(ptp_cancel_worker_sync);
464
465 /* module operations */
466
ptp_exit(void)467 static void __exit ptp_exit(void)
468 {
469 class_destroy(ptp_class);
470 unregister_chrdev_region(ptp_devt, MINORMASK + 1);
471 ida_destroy(&ptp_clocks_map);
472 }
473
ptp_init(void)474 static int __init ptp_init(void)
475 {
476 int err;
477
478 ptp_class = class_create("ptp");
479 if (IS_ERR(ptp_class)) {
480 pr_err("ptp: failed to allocate class\n");
481 return PTR_ERR(ptp_class);
482 }
483
484 err = alloc_chrdev_region(&ptp_devt, 0, MINORMASK + 1, "ptp");
485 if (err < 0) {
486 pr_err("ptp: failed to allocate device region\n");
487 goto no_region;
488 }
489
490 ptp_class->dev_groups = ptp_groups;
491 pr_info("PTP clock support registered\n");
492 return 0;
493
494 no_region:
495 class_destroy(ptp_class);
496 return err;
497 }
498
499 subsys_initcall(ptp_init);
500 module_exit(ptp_exit);
501
502 MODULE_AUTHOR("Richard Cochran <richardcochran@gmail.com>");
503 MODULE_DESCRIPTION("PTP clocks support");
504 MODULE_LICENSE("GPL");
505