ptp_clock.c 9.9 KB

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