1 // SPDX-License-Identifier: GPL-2.0-or-later
3 * PTP 1588 clock support
5 * Copyright (C) 2010 OMICRON electronics GmbH
8 #include <linux/device.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>
20 #include "ptp_private.h"
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)
27 struct class *ptp_class
;
31 static dev_t ptp_devt
;
33 static DEFINE_IDA(ptp_clocks_map
);
35 /* time stamp event queue operations */
37 static inline int queue_free(struct timestamp_event_queue
*q
)
39 return PTP_MAX_TIMESTAMPS
- queue_cnt(q
) - 1;
42 static void enqueue_external_timestamp(struct timestamp_event_queue
*queue
,
43 struct ptp_clock_event
*src
)
45 struct ptp_extts_event
*dst
;
50 seconds
= div_u64_rem(src
->timestamp
, 1000000000, &remainder
);
52 spin_lock_irqsave(&queue
->lock
, flags
);
54 dst
= &queue
->buf
[queue
->tail
];
55 dst
->index
= src
->index
;
57 dst
->t
.nsec
= remainder
;
59 if (!queue_free(queue
))
60 queue
->head
= (queue
->head
+ 1) % PTP_MAX_TIMESTAMPS
;
62 queue
->tail
= (queue
->tail
+ 1) % PTP_MAX_TIMESTAMPS
;
64 spin_unlock_irqrestore(&queue
->lock
, flags
);
67 /* posix clock implementation */
69 static int ptp_clock_getres(struct posix_clock
*pc
, struct timespec64
*tp
)
76 static int ptp_clock_settime(struct posix_clock
*pc
, const struct timespec64
*tp
)
78 struct ptp_clock
*ptp
= container_of(pc
, struct ptp_clock
, clock
);
80 if (ptp_vclock_in_use(ptp
)) {
81 pr_err("ptp: virtual clock in use\n");
85 return ptp
->info
->settime64(ptp
->info
, tp
);
88 static int ptp_clock_gettime(struct posix_clock
*pc
, struct timespec64
*tp
)
90 struct ptp_clock
*ptp
= container_of(pc
, struct ptp_clock
, clock
);
93 if (ptp
->info
->gettimex64
)
94 err
= ptp
->info
->gettimex64(ptp
->info
, tp
, NULL
);
96 err
= ptp
->info
->gettime64(ptp
->info
, tp
);
100 static int ptp_clock_adjtime(struct posix_clock
*pc
, struct __kernel_timex
*tx
)
102 struct ptp_clock
*ptp
= container_of(pc
, struct ptp_clock
, clock
);
103 struct ptp_clock_info
*ops
;
104 int err
= -EOPNOTSUPP
;
106 if (ptp_vclock_in_use(ptp
)) {
107 pr_err("ptp: virtual clock in use\n");
113 if (tx
->modes
& ADJ_SETOFFSET
) {
114 struct timespec64 ts
;
118 ts
.tv_sec
= tx
->time
.tv_sec
;
119 ts
.tv_nsec
= tx
->time
.tv_usec
;
121 if (!(tx
->modes
& ADJ_NANO
))
124 if ((unsigned long) ts
.tv_nsec
>= NSEC_PER_SEC
)
127 kt
= timespec64_to_ktime(ts
);
128 delta
= ktime_to_ns(kt
);
129 err
= ops
->adjtime(ops
, delta
);
130 } else if (tx
->modes
& ADJ_FREQUENCY
) {
131 long ppb
= scaled_ppm_to_ppb(tx
->freq
);
132 if (ppb
> ops
->max_adj
|| ppb
< -ops
->max_adj
)
135 err
= ops
->adjfine(ops
, tx
->freq
);
137 err
= ops
->adjfreq(ops
, ppb
);
138 ptp
->dialed_frequency
= tx
->freq
;
139 } else if (tx
->modes
& ADJ_OFFSET
) {
141 s32 offset
= tx
->offset
;
143 if (!(tx
->modes
& ADJ_NANO
))
144 offset
*= NSEC_PER_USEC
;
146 err
= ops
->adjphase(ops
, offset
);
148 } else if (tx
->modes
== 0) {
149 tx
->freq
= ptp
->dialed_frequency
;
156 static struct posix_clock_operations ptp_clock_ops
= {
157 .owner
= THIS_MODULE
,
158 .clock_adjtime
= ptp_clock_adjtime
,
159 .clock_gettime
= ptp_clock_gettime
,
160 .clock_getres
= ptp_clock_getres
,
161 .clock_settime
= ptp_clock_settime
,
168 static void ptp_clock_release(struct device
*dev
)
170 struct ptp_clock
*ptp
= container_of(dev
, struct ptp_clock
, dev
);
172 /* Release the clock's resources. */
174 pps_unregister_source(ptp
->pps_source
);
175 ptp_cleanup_pin_groups(ptp
);
176 kfree(ptp
->vclock_index
);
177 mutex_destroy(&ptp
->tsevq_mux
);
178 mutex_destroy(&ptp
->pincfg_mux
);
179 mutex_destroy(&ptp
->n_vclocks_mux
);
180 ida_simple_remove(&ptp_clocks_map
, ptp
->index
);
184 static void ptp_aux_kworker(struct kthread_work
*work
)
186 struct ptp_clock
*ptp
= container_of(work
, struct ptp_clock
,
188 struct ptp_clock_info
*info
= ptp
->info
;
191 delay
= info
->do_aux_work(info
);
194 kthread_queue_delayed_work(ptp
->kworker
, &ptp
->aux_work
, delay
);
197 /* public interface */
199 struct ptp_clock
*ptp_clock_register(struct ptp_clock_info
*info
,
200 struct device
*parent
)
202 struct ptp_clock
*ptp
;
203 int err
= 0, index
, major
= MAJOR(ptp_devt
);
206 if (info
->n_alarm
> PTP_MAX_ALARMS
)
207 return ERR_PTR(-EINVAL
);
209 /* Initialize a clock structure. */
211 ptp
= kzalloc(sizeof(struct ptp_clock
), GFP_KERNEL
);
215 index
= ida_simple_get(&ptp_clocks_map
, 0, MINORMASK
+ 1, GFP_KERNEL
);
221 ptp
->clock
.ops
= ptp_clock_ops
;
223 ptp
->devid
= MKDEV(major
, index
);
225 spin_lock_init(&ptp
->tsevq
.lock
);
226 mutex_init(&ptp
->tsevq_mux
);
227 mutex_init(&ptp
->pincfg_mux
);
228 mutex_init(&ptp
->n_vclocks_mux
);
229 init_waitqueue_head(&ptp
->tsev_wq
);
231 if (ptp
->info
->do_aux_work
) {
232 kthread_init_delayed_work(&ptp
->aux_work
, ptp_aux_kworker
);
233 ptp
->kworker
= kthread_create_worker(0, "ptp%d", ptp
->index
);
234 if (IS_ERR(ptp
->kworker
)) {
235 err
= PTR_ERR(ptp
->kworker
);
236 pr_err("failed to create ptp aux_worker %d\n", err
);
241 /* PTP virtual clock is being registered under physical clock */
242 if (parent
&& parent
->class && parent
->class->name
&&
243 strcmp(parent
->class->name
, "ptp") == 0)
244 ptp
->is_virtual_clock
= true;
246 if (!ptp
->is_virtual_clock
) {
247 ptp
->max_vclocks
= PTP_DEFAULT_MAX_VCLOCKS
;
249 size
= sizeof(int) * ptp
->max_vclocks
;
250 ptp
->vclock_index
= kzalloc(size
, GFP_KERNEL
);
251 if (!ptp
->vclock_index
) {
253 goto no_mem_for_vclocks
;
257 err
= ptp_populate_pin_groups(ptp
);
261 /* Register a new PPS source. */
263 struct pps_source_info pps
;
264 memset(&pps
, 0, sizeof(pps
));
265 snprintf(pps
.name
, PPS_MAX_NAME_LEN
, "ptp%d", index
);
266 pps
.mode
= PTP_PPS_MODE
;
267 pps
.owner
= info
->owner
;
268 ptp
->pps_source
= pps_register_source(&pps
, PTP_PPS_DEFAULTS
);
269 if (IS_ERR(ptp
->pps_source
)) {
270 err
= PTR_ERR(ptp
->pps_source
);
271 pr_err("failed to register pps source\n");
274 ptp
->pps_source
->lookup_cookie
= ptp
;
277 /* Initialize a new device of our class in our clock structure. */
278 device_initialize(&ptp
->dev
);
279 ptp
->dev
.devt
= ptp
->devid
;
280 ptp
->dev
.class = ptp_class
;
281 ptp
->dev
.parent
= parent
;
282 ptp
->dev
.groups
= ptp
->pin_attr_groups
;
283 ptp
->dev
.release
= ptp_clock_release
;
284 dev_set_drvdata(&ptp
->dev
, ptp
);
285 dev_set_name(&ptp
->dev
, "ptp%d", ptp
->index
);
287 /* Create a posix clock and link it to the device. */
288 err
= posix_clock_register(&ptp
->clock
, &ptp
->dev
);
291 pps_unregister_source(ptp
->pps_source
);
294 kthread_destroy_worker(ptp
->kworker
);
296 put_device(&ptp
->dev
);
298 pr_err("failed to create posix clock\n");
305 ptp_cleanup_pin_groups(ptp
);
307 kfree(ptp
->vclock_index
);
310 kthread_destroy_worker(ptp
->kworker
);
312 mutex_destroy(&ptp
->tsevq_mux
);
313 mutex_destroy(&ptp
->pincfg_mux
);
314 mutex_destroy(&ptp
->n_vclocks_mux
);
315 ida_simple_remove(&ptp_clocks_map
, index
);
321 EXPORT_SYMBOL(ptp_clock_register
);
323 static int unregister_vclock(struct device
*dev
, void *data
)
325 struct ptp_clock
*ptp
= dev_get_drvdata(dev
);
327 ptp_vclock_unregister(info_to_vclock(ptp
->info
));
331 int ptp_clock_unregister(struct ptp_clock
*ptp
)
333 if (ptp_vclock_in_use(ptp
)) {
334 device_for_each_child(&ptp
->dev
, NULL
, unregister_vclock
);
338 wake_up_interruptible(&ptp
->tsev_wq
);
341 kthread_cancel_delayed_work_sync(&ptp
->aux_work
);
342 kthread_destroy_worker(ptp
->kworker
);
344 posix_clock_unregister(&ptp
->clock
);
348 EXPORT_SYMBOL(ptp_clock_unregister
);
350 void ptp_clock_event(struct ptp_clock
*ptp
, struct ptp_clock_event
*event
)
352 struct pps_event_time evt
;
354 switch (event
->type
) {
356 case PTP_CLOCK_ALARM
:
359 case PTP_CLOCK_EXTTS
:
360 enqueue_external_timestamp(&ptp
->tsevq
, event
);
361 wake_up_interruptible(&ptp
->tsev_wq
);
366 pps_event(ptp
->pps_source
, &evt
, PTP_PPS_EVENT
, NULL
);
369 case PTP_CLOCK_PPSUSR
:
370 pps_event(ptp
->pps_source
, &event
->pps_times
,
371 PTP_PPS_EVENT
, NULL
);
375 EXPORT_SYMBOL(ptp_clock_event
);
377 int ptp_clock_index(struct ptp_clock
*ptp
)
381 EXPORT_SYMBOL(ptp_clock_index
);
383 int ptp_find_pin(struct ptp_clock
*ptp
,
384 enum ptp_pin_function func
, unsigned int chan
)
386 struct ptp_pin_desc
*pin
= NULL
;
389 for (i
= 0; i
< ptp
->info
->n_pins
; i
++) {
390 if (ptp
->info
->pin_config
[i
].func
== func
&&
391 ptp
->info
->pin_config
[i
].chan
== chan
) {
392 pin
= &ptp
->info
->pin_config
[i
];
399 EXPORT_SYMBOL(ptp_find_pin
);
401 int ptp_find_pin_unlocked(struct ptp_clock
*ptp
,
402 enum ptp_pin_function func
, unsigned int chan
)
406 mutex_lock(&ptp
->pincfg_mux
);
408 result
= ptp_find_pin(ptp
, func
, chan
);
410 mutex_unlock(&ptp
->pincfg_mux
);
414 EXPORT_SYMBOL(ptp_find_pin_unlocked
);
416 int ptp_schedule_worker(struct ptp_clock
*ptp
, unsigned long delay
)
418 return kthread_mod_delayed_work(ptp
->kworker
, &ptp
->aux_work
, delay
);
420 EXPORT_SYMBOL(ptp_schedule_worker
);
422 void ptp_cancel_worker_sync(struct ptp_clock
*ptp
)
424 kthread_cancel_delayed_work_sync(&ptp
->aux_work
);
426 EXPORT_SYMBOL(ptp_cancel_worker_sync
);
428 /* module operations */
430 static void __exit
ptp_exit(void)
432 class_destroy(ptp_class
);
433 unregister_chrdev_region(ptp_devt
, MINORMASK
+ 1);
434 ida_destroy(&ptp_clocks_map
);
437 static int __init
ptp_init(void)
441 ptp_class
= class_create(THIS_MODULE
, "ptp");
442 if (IS_ERR(ptp_class
)) {
443 pr_err("ptp: failed to allocate class\n");
444 return PTR_ERR(ptp_class
);
447 err
= alloc_chrdev_region(&ptp_devt
, 0, MINORMASK
+ 1, "ptp");
449 pr_err("ptp: failed to allocate device region\n");
453 ptp_class
->dev_groups
= ptp_groups
;
454 pr_info("PTP clock support registered\n");
458 class_destroy(ptp_class
);
462 subsys_initcall(ptp_init
);
463 module_exit(ptp_exit
);
465 MODULE_AUTHOR("Richard Cochran <richardcochran@gmail.com>");
466 MODULE_DESCRIPTION("PTP clocks support");
467 MODULE_LICENSE("GPL");