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_vclock_in_use(ptp)) { 81 pr_err("ptp: virtual clock in use\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_vclock_in_use(ptp)) { 107 pr_err("ptp: virtual clock in use\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 if (ops->adjfine) 135 err = ops->adjfine(ops, tx->freq); 136 else 137 err = ops->adjfreq(ops, ppb); 138 ptp->dialed_frequency = tx->freq; 139 } else if (tx->modes & ADJ_OFFSET) { 140 if (ops->adjphase) { 141 s32 offset = tx->offset; 142 143 if (!(tx->modes & ADJ_NANO)) 144 offset *= NSEC_PER_USEC; 145 146 err = ops->adjphase(ops, offset); 147 } 148 } else if (tx->modes == 0) { 149 tx->freq = ptp->dialed_frequency; 150 err = 0; 151 } 152 153 return err; 154 } 155 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, 162 .ioctl = ptp_ioctl, 163 .open = ptp_open, 164 .poll = ptp_poll, 165 .read = ptp_read, 166 }; 167 168 static void ptp_clock_release(struct device *dev) 169 { 170 struct ptp_clock *ptp = container_of(dev, struct ptp_clock, dev); 171 172 ptp_cleanup_pin_groups(ptp); 173 kfree(ptp->vclock_index); 174 mutex_destroy(&ptp->tsevq_mux); 175 mutex_destroy(&ptp->pincfg_mux); 176 mutex_destroy(&ptp->n_vclocks_mux); 177 ida_simple_remove(&ptp_clocks_map, ptp->index); 178 kfree(ptp); 179 } 180 181 static void ptp_aux_kworker(struct kthread_work *work) 182 { 183 struct ptp_clock *ptp = container_of(work, struct ptp_clock, 184 aux_work.work); 185 struct ptp_clock_info *info = ptp->info; 186 long delay; 187 188 delay = info->do_aux_work(info); 189 190 if (delay >= 0) 191 kthread_queue_delayed_work(ptp->kworker, &ptp->aux_work, delay); 192 } 193 194 /* public interface */ 195 196 struct ptp_clock *ptp_clock_register(struct ptp_clock_info *info, 197 struct device *parent) 198 { 199 struct ptp_clock *ptp; 200 int err = 0, index, major = MAJOR(ptp_devt); 201 size_t size; 202 203 if (info->n_alarm > PTP_MAX_ALARMS) 204 return ERR_PTR(-EINVAL); 205 206 /* Initialize a clock structure. */ 207 err = -ENOMEM; 208 ptp = kzalloc(sizeof(struct ptp_clock), GFP_KERNEL); 209 if (ptp == NULL) 210 goto no_memory; 211 212 index = ida_simple_get(&ptp_clocks_map, 0, MINORMASK + 1, GFP_KERNEL); 213 if (index < 0) { 214 err = index; 215 goto no_slot; 216 } 217 218 ptp->clock.ops = ptp_clock_ops; 219 ptp->info = info; 220 ptp->devid = MKDEV(major, index); 221 ptp->index = index; 222 spin_lock_init(&ptp->tsevq.lock); 223 mutex_init(&ptp->tsevq_mux); 224 mutex_init(&ptp->pincfg_mux); 225 mutex_init(&ptp->n_vclocks_mux); 226 init_waitqueue_head(&ptp->tsev_wq); 227 228 if (ptp->info->do_aux_work) { 229 kthread_init_delayed_work(&ptp->aux_work, ptp_aux_kworker); 230 ptp->kworker = kthread_create_worker(0, "ptp%d", ptp->index); 231 if (IS_ERR(ptp->kworker)) { 232 err = PTR_ERR(ptp->kworker); 233 pr_err("failed to create ptp aux_worker %d\n", err); 234 goto kworker_err; 235 } 236 } 237 238 /* PTP virtual clock is being registered under physical clock */ 239 if (parent && parent->class && parent->class->name && 240 strcmp(parent->class->name, "ptp") == 0) 241 ptp->is_virtual_clock = true; 242 243 if (!ptp->is_virtual_clock) { 244 ptp->max_vclocks = PTP_DEFAULT_MAX_VCLOCKS; 245 246 size = sizeof(int) * ptp->max_vclocks; 247 ptp->vclock_index = kzalloc(size, GFP_KERNEL); 248 if (!ptp->vclock_index) { 249 err = -ENOMEM; 250 goto no_mem_for_vclocks; 251 } 252 } 253 254 err = ptp_populate_pin_groups(ptp); 255 if (err) 256 goto no_pin_groups; 257 258 /* Register a new PPS source. */ 259 if (info->pps) { 260 struct pps_source_info pps; 261 memset(&pps, 0, sizeof(pps)); 262 snprintf(pps.name, PPS_MAX_NAME_LEN, "ptp%d", index); 263 pps.mode = PTP_PPS_MODE; 264 pps.owner = info->owner; 265 ptp->pps_source = pps_register_source(&pps, PTP_PPS_DEFAULTS); 266 if (IS_ERR(ptp->pps_source)) { 267 err = PTR_ERR(ptp->pps_source); 268 pr_err("failed to register pps source\n"); 269 goto no_pps; 270 } 271 ptp->pps_source->lookup_cookie = ptp; 272 } 273 274 /* Initialize a new device of our class in our clock structure. */ 275 device_initialize(&ptp->dev); 276 ptp->dev.devt = ptp->devid; 277 ptp->dev.class = ptp_class; 278 ptp->dev.parent = parent; 279 ptp->dev.groups = ptp->pin_attr_groups; 280 ptp->dev.release = ptp_clock_release; 281 dev_set_drvdata(&ptp->dev, ptp); 282 dev_set_name(&ptp->dev, "ptp%d", ptp->index); 283 284 /* Create a posix clock and link it to the device. */ 285 err = posix_clock_register(&ptp->clock, &ptp->dev); 286 if (err) { 287 if (ptp->pps_source) 288 pps_unregister_source(ptp->pps_source); 289 290 if (ptp->kworker) 291 kthread_destroy_worker(ptp->kworker); 292 293 put_device(&ptp->dev); 294 295 pr_err("failed to create posix clock\n"); 296 return ERR_PTR(err); 297 } 298 299 return ptp; 300 301 no_pps: 302 ptp_cleanup_pin_groups(ptp); 303 no_pin_groups: 304 kfree(ptp->vclock_index); 305 no_mem_for_vclocks: 306 if (ptp->kworker) 307 kthread_destroy_worker(ptp->kworker); 308 kworker_err: 309 mutex_destroy(&ptp->tsevq_mux); 310 mutex_destroy(&ptp->pincfg_mux); 311 mutex_destroy(&ptp->n_vclocks_mux); 312 ida_simple_remove(&ptp_clocks_map, index); 313 no_slot: 314 kfree(ptp); 315 no_memory: 316 return ERR_PTR(err); 317 } 318 EXPORT_SYMBOL(ptp_clock_register); 319 320 static int unregister_vclock(struct device *dev, void *data) 321 { 322 struct ptp_clock *ptp = dev_get_drvdata(dev); 323 324 ptp_vclock_unregister(info_to_vclock(ptp->info)); 325 return 0; 326 } 327 328 int ptp_clock_unregister(struct ptp_clock *ptp) 329 { 330 if (ptp_vclock_in_use(ptp)) { 331 device_for_each_child(&ptp->dev, NULL, unregister_vclock); 332 } 333 334 ptp->defunct = 1; 335 wake_up_interruptible(&ptp->tsev_wq); 336 337 if (ptp->kworker) { 338 kthread_cancel_delayed_work_sync(&ptp->aux_work); 339 kthread_destroy_worker(ptp->kworker); 340 } 341 342 /* Release the clock's resources. */ 343 if (ptp->pps_source) 344 pps_unregister_source(ptp->pps_source); 345 346 posix_clock_unregister(&ptp->clock); 347 348 return 0; 349 } 350 EXPORT_SYMBOL(ptp_clock_unregister); 351 352 void ptp_clock_event(struct ptp_clock *ptp, struct ptp_clock_event *event) 353 { 354 struct pps_event_time evt; 355 356 switch (event->type) { 357 358 case PTP_CLOCK_ALARM: 359 break; 360 361 case PTP_CLOCK_EXTTS: 362 enqueue_external_timestamp(&ptp->tsevq, event); 363 wake_up_interruptible(&ptp->tsev_wq); 364 break; 365 366 case PTP_CLOCK_PPS: 367 pps_get_ts(&evt); 368 pps_event(ptp->pps_source, &evt, PTP_PPS_EVENT, NULL); 369 break; 370 371 case PTP_CLOCK_PPSUSR: 372 pps_event(ptp->pps_source, &event->pps_times, 373 PTP_PPS_EVENT, NULL); 374 break; 375 } 376 } 377 EXPORT_SYMBOL(ptp_clock_event); 378 379 int ptp_clock_index(struct ptp_clock *ptp) 380 { 381 return ptp->index; 382 } 383 EXPORT_SYMBOL(ptp_clock_index); 384 385 int ptp_find_pin(struct ptp_clock *ptp, 386 enum ptp_pin_function func, unsigned int chan) 387 { 388 struct ptp_pin_desc *pin = NULL; 389 int i; 390 391 for (i = 0; i < ptp->info->n_pins; i++) { 392 if (ptp->info->pin_config[i].func == func && 393 ptp->info->pin_config[i].chan == chan) { 394 pin = &ptp->info->pin_config[i]; 395 break; 396 } 397 } 398 399 return pin ? i : -1; 400 } 401 EXPORT_SYMBOL(ptp_find_pin); 402 403 int ptp_find_pin_unlocked(struct ptp_clock *ptp, 404 enum ptp_pin_function func, unsigned int chan) 405 { 406 int result; 407 408 mutex_lock(&ptp->pincfg_mux); 409 410 result = ptp_find_pin(ptp, func, chan); 411 412 mutex_unlock(&ptp->pincfg_mux); 413 414 return result; 415 } 416 EXPORT_SYMBOL(ptp_find_pin_unlocked); 417 418 int ptp_schedule_worker(struct ptp_clock *ptp, unsigned long delay) 419 { 420 return kthread_mod_delayed_work(ptp->kworker, &ptp->aux_work, delay); 421 } 422 EXPORT_SYMBOL(ptp_schedule_worker); 423 424 void ptp_cancel_worker_sync(struct ptp_clock *ptp) 425 { 426 kthread_cancel_delayed_work_sync(&ptp->aux_work); 427 } 428 EXPORT_SYMBOL(ptp_cancel_worker_sync); 429 430 /* module operations */ 431 432 static void __exit ptp_exit(void) 433 { 434 class_destroy(ptp_class); 435 unregister_chrdev_region(ptp_devt, MINORMASK + 1); 436 ida_destroy(&ptp_clocks_map); 437 } 438 439 static int __init ptp_init(void) 440 { 441 int err; 442 443 ptp_class = class_create(THIS_MODULE, "ptp"); 444 if (IS_ERR(ptp_class)) { 445 pr_err("ptp: failed to allocate class\n"); 446 return PTR_ERR(ptp_class); 447 } 448 449 err = alloc_chrdev_region(&ptp_devt, 0, MINORMASK + 1, "ptp"); 450 if (err < 0) { 451 pr_err("ptp: failed to allocate device region\n"); 452 goto no_region; 453 } 454 455 ptp_class->dev_groups = ptp_groups; 456 pr_info("PTP clock support registered\n"); 457 return 0; 458 459 no_region: 460 class_destroy(ptp_class); 461 return err; 462 } 463 464 subsys_initcall(ptp_init); 465 module_exit(ptp_exit); 466 467 MODULE_AUTHOR("Richard Cochran <richardcochran@gmail.com>"); 468 MODULE_DESCRIPTION("PTP clocks support"); 469 MODULE_LICENSE("GPL"); 470