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 /* private globals */ 28 29 static dev_t ptp_devt; 30 static struct class *ptp_class; 31 32 static DEFINE_IDA(ptp_clocks_map); 33 34 /* time stamp event queue operations */ 35 36 static inline int queue_free(struct timestamp_event_queue *q) 37 { 38 return PTP_MAX_TIMESTAMPS - queue_cnt(q) - 1; 39 } 40 41 static void enqueue_external_timestamp(struct timestamp_event_queue *queue, 42 struct ptp_clock_event *src) 43 { 44 struct ptp_extts_event *dst; 45 unsigned long flags; 46 s64 seconds; 47 u32 remainder; 48 49 seconds = div_u64_rem(src->timestamp, 1000000000, &remainder); 50 51 spin_lock_irqsave(&queue->lock, flags); 52 53 dst = &queue->buf[queue->tail]; 54 dst->index = src->index; 55 dst->t.sec = seconds; 56 dst->t.nsec = remainder; 57 58 if (!queue_free(queue)) 59 queue->head = (queue->head + 1) % PTP_MAX_TIMESTAMPS; 60 61 queue->tail = (queue->tail + 1) % PTP_MAX_TIMESTAMPS; 62 63 spin_unlock_irqrestore(&queue->lock, flags); 64 } 65 66 static s32 scaled_ppm_to_ppb(long ppm) 67 { 68 /* 69 * The 'freq' field in the 'struct timex' is in parts per 70 * million, but with a 16 bit binary fractional field. 71 * 72 * We want to calculate 73 * 74 * ppb = scaled_ppm * 1000 / 2^16 75 * 76 * which simplifies to 77 * 78 * ppb = scaled_ppm * 125 / 2^13 79 */ 80 s64 ppb = 1 + ppm; 81 ppb *= 125; 82 ppb >>= 13; 83 return (s32) ppb; 84 } 85 86 /* posix clock implementation */ 87 88 static int ptp_clock_getres(struct posix_clock *pc, struct timespec64 *tp) 89 { 90 tp->tv_sec = 0; 91 tp->tv_nsec = 1; 92 return 0; 93 } 94 95 static int ptp_clock_settime(struct posix_clock *pc, const struct timespec64 *tp) 96 { 97 struct ptp_clock *ptp = container_of(pc, struct ptp_clock, clock); 98 99 return ptp->info->settime64(ptp->info, tp); 100 } 101 102 static int ptp_clock_gettime(struct posix_clock *pc, struct timespec64 *tp) 103 { 104 struct ptp_clock *ptp = container_of(pc, struct ptp_clock, clock); 105 int err; 106 107 if (ptp->info->gettimex64) 108 err = ptp->info->gettimex64(ptp->info, tp, NULL); 109 else 110 err = ptp->info->gettime64(ptp->info, tp); 111 return err; 112 } 113 114 static int ptp_clock_adjtime(struct posix_clock *pc, struct __kernel_timex *tx) 115 { 116 struct ptp_clock *ptp = container_of(pc, struct ptp_clock, clock); 117 struct ptp_clock_info *ops; 118 int err = -EOPNOTSUPP; 119 120 ops = ptp->info; 121 122 if (tx->modes & ADJ_SETOFFSET) { 123 struct timespec64 ts; 124 ktime_t kt; 125 s64 delta; 126 127 ts.tv_sec = tx->time.tv_sec; 128 ts.tv_nsec = tx->time.tv_usec; 129 130 if (!(tx->modes & ADJ_NANO)) 131 ts.tv_nsec *= 1000; 132 133 if ((unsigned long) ts.tv_nsec >= NSEC_PER_SEC) 134 return -EINVAL; 135 136 kt = timespec64_to_ktime(ts); 137 delta = ktime_to_ns(kt); 138 err = ops->adjtime(ops, delta); 139 } else if (tx->modes & ADJ_FREQUENCY) { 140 s32 ppb = scaled_ppm_to_ppb(tx->freq); 141 if (ppb > ops->max_adj || ppb < -ops->max_adj) 142 return -ERANGE; 143 if (ops->adjfine) 144 err = ops->adjfine(ops, tx->freq); 145 else 146 err = ops->adjfreq(ops, ppb); 147 ptp->dialed_frequency = tx->freq; 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 delete_ptp_clock(struct posix_clock *pc) 169 { 170 struct ptp_clock *ptp = container_of(pc, struct ptp_clock, clock); 171 172 mutex_destroy(&ptp->tsevq_mux); 173 mutex_destroy(&ptp->pincfg_mux); 174 ida_simple_remove(&ptp_clocks_map, ptp->index); 175 kfree(ptp); 176 } 177 178 static void ptp_aux_kworker(struct kthread_work *work) 179 { 180 struct ptp_clock *ptp = container_of(work, struct ptp_clock, 181 aux_work.work); 182 struct ptp_clock_info *info = ptp->info; 183 long delay; 184 185 delay = info->do_aux_work(info); 186 187 if (delay >= 0) 188 kthread_queue_delayed_work(ptp->kworker, &ptp->aux_work, delay); 189 } 190 191 /* public interface */ 192 193 struct ptp_clock *ptp_clock_register(struct ptp_clock_info *info, 194 struct device *parent) 195 { 196 struct ptp_clock *ptp; 197 int err = 0, index, major = MAJOR(ptp_devt); 198 199 if (info->n_alarm > PTP_MAX_ALARMS) 200 return ERR_PTR(-EINVAL); 201 202 /* Initialize a clock structure. */ 203 err = -ENOMEM; 204 ptp = kzalloc(sizeof(struct ptp_clock), GFP_KERNEL); 205 if (ptp == NULL) 206 goto no_memory; 207 208 index = ida_simple_get(&ptp_clocks_map, 0, MINORMASK + 1, GFP_KERNEL); 209 if (index < 0) { 210 err = index; 211 goto no_slot; 212 } 213 214 ptp->clock.ops = ptp_clock_ops; 215 ptp->clock.release = delete_ptp_clock; 216 ptp->info = info; 217 ptp->devid = MKDEV(major, index); 218 ptp->index = index; 219 spin_lock_init(&ptp->tsevq.lock); 220 mutex_init(&ptp->tsevq_mux); 221 mutex_init(&ptp->pincfg_mux); 222 init_waitqueue_head(&ptp->tsev_wq); 223 224 if (ptp->info->do_aux_work) { 225 kthread_init_delayed_work(&ptp->aux_work, ptp_aux_kworker); 226 ptp->kworker = kthread_create_worker(0, "ptp%d", ptp->index); 227 if (IS_ERR(ptp->kworker)) { 228 err = PTR_ERR(ptp->kworker); 229 pr_err("failed to create ptp aux_worker %d\n", err); 230 goto kworker_err; 231 } 232 } 233 234 err = ptp_populate_pin_groups(ptp); 235 if (err) 236 goto no_pin_groups; 237 238 /* Create a new device in our class. */ 239 ptp->dev = device_create_with_groups(ptp_class, parent, ptp->devid, 240 ptp, ptp->pin_attr_groups, 241 "ptp%d", ptp->index); 242 if (IS_ERR(ptp->dev)) { 243 err = PTR_ERR(ptp->dev); 244 goto no_device; 245 } 246 247 /* Register a new PPS source. */ 248 if (info->pps) { 249 struct pps_source_info pps; 250 memset(&pps, 0, sizeof(pps)); 251 snprintf(pps.name, PPS_MAX_NAME_LEN, "ptp%d", index); 252 pps.mode = PTP_PPS_MODE; 253 pps.owner = info->owner; 254 ptp->pps_source = pps_register_source(&pps, PTP_PPS_DEFAULTS); 255 if (IS_ERR(ptp->pps_source)) { 256 err = PTR_ERR(ptp->pps_source); 257 pr_err("failed to register pps source\n"); 258 goto no_pps; 259 } 260 } 261 262 /* Create a posix clock. */ 263 err = posix_clock_register(&ptp->clock, ptp->devid); 264 if (err) { 265 pr_err("failed to create posix clock\n"); 266 goto no_clock; 267 } 268 269 return ptp; 270 271 no_clock: 272 if (ptp->pps_source) 273 pps_unregister_source(ptp->pps_source); 274 no_pps: 275 device_destroy(ptp_class, ptp->devid); 276 no_device: 277 ptp_cleanup_pin_groups(ptp); 278 no_pin_groups: 279 if (ptp->kworker) 280 kthread_destroy_worker(ptp->kworker); 281 kworker_err: 282 mutex_destroy(&ptp->tsevq_mux); 283 mutex_destroy(&ptp->pincfg_mux); 284 ida_simple_remove(&ptp_clocks_map, index); 285 no_slot: 286 kfree(ptp); 287 no_memory: 288 return ERR_PTR(err); 289 } 290 EXPORT_SYMBOL(ptp_clock_register); 291 292 int ptp_clock_unregister(struct ptp_clock *ptp) 293 { 294 ptp->defunct = 1; 295 wake_up_interruptible(&ptp->tsev_wq); 296 297 if (ptp->kworker) { 298 kthread_cancel_delayed_work_sync(&ptp->aux_work); 299 kthread_destroy_worker(ptp->kworker); 300 } 301 302 /* Release the clock's resources. */ 303 if (ptp->pps_source) 304 pps_unregister_source(ptp->pps_source); 305 306 device_destroy(ptp_class, ptp->devid); 307 ptp_cleanup_pin_groups(ptp); 308 309 posix_clock_unregister(&ptp->clock); 310 return 0; 311 } 312 EXPORT_SYMBOL(ptp_clock_unregister); 313 314 void ptp_clock_event(struct ptp_clock *ptp, struct ptp_clock_event *event) 315 { 316 struct pps_event_time evt; 317 318 switch (event->type) { 319 320 case PTP_CLOCK_ALARM: 321 break; 322 323 case PTP_CLOCK_EXTTS: 324 enqueue_external_timestamp(&ptp->tsevq, event); 325 wake_up_interruptible(&ptp->tsev_wq); 326 break; 327 328 case PTP_CLOCK_PPS: 329 pps_get_ts(&evt); 330 pps_event(ptp->pps_source, &evt, PTP_PPS_EVENT, NULL); 331 break; 332 333 case PTP_CLOCK_PPSUSR: 334 pps_event(ptp->pps_source, &event->pps_times, 335 PTP_PPS_EVENT, NULL); 336 break; 337 } 338 } 339 EXPORT_SYMBOL(ptp_clock_event); 340 341 int ptp_clock_index(struct ptp_clock *ptp) 342 { 343 return ptp->index; 344 } 345 EXPORT_SYMBOL(ptp_clock_index); 346 347 int ptp_find_pin(struct ptp_clock *ptp, 348 enum ptp_pin_function func, unsigned int chan) 349 { 350 struct ptp_pin_desc *pin = NULL; 351 int i; 352 353 mutex_lock(&ptp->pincfg_mux); 354 for (i = 0; i < ptp->info->n_pins; i++) { 355 if (ptp->info->pin_config[i].func == func && 356 ptp->info->pin_config[i].chan == chan) { 357 pin = &ptp->info->pin_config[i]; 358 break; 359 } 360 } 361 mutex_unlock(&ptp->pincfg_mux); 362 363 return pin ? i : -1; 364 } 365 EXPORT_SYMBOL(ptp_find_pin); 366 367 int ptp_schedule_worker(struct ptp_clock *ptp, unsigned long delay) 368 { 369 return kthread_mod_delayed_work(ptp->kworker, &ptp->aux_work, delay); 370 } 371 EXPORT_SYMBOL(ptp_schedule_worker); 372 373 /* module operations */ 374 375 static void __exit ptp_exit(void) 376 { 377 class_destroy(ptp_class); 378 unregister_chrdev_region(ptp_devt, MINORMASK + 1); 379 ida_destroy(&ptp_clocks_map); 380 } 381 382 static int __init ptp_init(void) 383 { 384 int err; 385 386 ptp_class = class_create(THIS_MODULE, "ptp"); 387 if (IS_ERR(ptp_class)) { 388 pr_err("ptp: failed to allocate class\n"); 389 return PTR_ERR(ptp_class); 390 } 391 392 err = alloc_chrdev_region(&ptp_devt, 0, MINORMASK + 1, "ptp"); 393 if (err < 0) { 394 pr_err("ptp: failed to allocate device region\n"); 395 goto no_region; 396 } 397 398 ptp_class->dev_groups = ptp_groups; 399 pr_info("PTP clock support registered\n"); 400 return 0; 401 402 no_region: 403 class_destroy(ptp_class); 404 return err; 405 } 406 407 subsys_initcall(ptp_init); 408 module_exit(ptp_exit); 409 410 MODULE_AUTHOR("Richard Cochran <richardcochran@gmail.com>"); 411 MODULE_DESCRIPTION("PTP clocks support"); 412 MODULE_LICENSE("GPL"); 413