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