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