drivers/ptp/ptp_clock.c

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