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