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