ptp_vclock.c 6.7 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295
  1. // SPDX-License-Identifier: GPL-2.0-or-later
  2. /*
  3. * PTP virtual clock driver
  4. *
  5. * Copyright 2021 NXP
  6. */
  7. #include <linux/slab.h>
  8. #include <linux/hashtable.h>
  9. #include "ptp_private.h"
  10. #define PTP_VCLOCK_CC_SHIFT 31
  11. #define PTP_VCLOCK_CC_MULT (1 << PTP_VCLOCK_CC_SHIFT)
  12. #define PTP_VCLOCK_FADJ_SHIFT 9
  13. #define PTP_VCLOCK_FADJ_DENOMINATOR 15625ULL
  14. #define PTP_VCLOCK_REFRESH_INTERVAL (HZ * 2)
  15. /* protects vclock_hash addition/deletion */
  16. static DEFINE_SPINLOCK(vclock_hash_lock);
  17. static DEFINE_READ_MOSTLY_HASHTABLE(vclock_hash, 8);
  18. static void ptp_vclock_hash_add(struct ptp_vclock *vclock)
  19. {
  20. spin_lock(&vclock_hash_lock);
  21. hlist_add_head_rcu(&vclock->vclock_hash_node,
  22. &vclock_hash[vclock->clock->index % HASH_SIZE(vclock_hash)]);
  23. spin_unlock(&vclock_hash_lock);
  24. }
  25. static void ptp_vclock_hash_del(struct ptp_vclock *vclock)
  26. {
  27. spin_lock(&vclock_hash_lock);
  28. hlist_del_init_rcu(&vclock->vclock_hash_node);
  29. spin_unlock(&vclock_hash_lock);
  30. synchronize_rcu();
  31. }
  32. static int ptp_vclock_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
  33. {
  34. struct ptp_vclock *vclock = info_to_vclock(ptp);
  35. s64 adj;
  36. adj = (s64)scaled_ppm << PTP_VCLOCK_FADJ_SHIFT;
  37. adj = div_s64(adj, PTP_VCLOCK_FADJ_DENOMINATOR);
  38. if (mutex_lock_interruptible(&vclock->lock))
  39. return -EINTR;
  40. timecounter_read(&vclock->tc);
  41. vclock->cc.mult = PTP_VCLOCK_CC_MULT + adj;
  42. mutex_unlock(&vclock->lock);
  43. return 0;
  44. }
  45. static int ptp_vclock_adjtime(struct ptp_clock_info *ptp, s64 delta)
  46. {
  47. struct ptp_vclock *vclock = info_to_vclock(ptp);
  48. if (mutex_lock_interruptible(&vclock->lock))
  49. return -EINTR;
  50. timecounter_adjtime(&vclock->tc, delta);
  51. mutex_unlock(&vclock->lock);
  52. return 0;
  53. }
  54. static int ptp_vclock_gettime(struct ptp_clock_info *ptp,
  55. struct timespec64 *ts)
  56. {
  57. struct ptp_vclock *vclock = info_to_vclock(ptp);
  58. u64 ns;
  59. if (mutex_lock_interruptible(&vclock->lock))
  60. return -EINTR;
  61. ns = timecounter_read(&vclock->tc);
  62. mutex_unlock(&vclock->lock);
  63. *ts = ns_to_timespec64(ns);
  64. return 0;
  65. }
  66. static int ptp_vclock_gettimex(struct ptp_clock_info *ptp,
  67. struct timespec64 *ts,
  68. struct ptp_system_timestamp *sts)
  69. {
  70. struct ptp_vclock *vclock = info_to_vclock(ptp);
  71. struct ptp_clock *pptp = vclock->pclock;
  72. struct timespec64 pts;
  73. int err;
  74. u64 ns;
  75. err = pptp->info->getcyclesx64(pptp->info, &pts, sts);
  76. if (err)
  77. return err;
  78. if (mutex_lock_interruptible(&vclock->lock))
  79. return -EINTR;
  80. ns = timecounter_cyc2time(&vclock->tc, timespec64_to_ns(&pts));
  81. mutex_unlock(&vclock->lock);
  82. *ts = ns_to_timespec64(ns);
  83. return 0;
  84. }
  85. static int ptp_vclock_settime(struct ptp_clock_info *ptp,
  86. const struct timespec64 *ts)
  87. {
  88. struct ptp_vclock *vclock = info_to_vclock(ptp);
  89. u64 ns = timespec64_to_ns(ts);
  90. if (mutex_lock_interruptible(&vclock->lock))
  91. return -EINTR;
  92. timecounter_init(&vclock->tc, &vclock->cc, ns);
  93. mutex_unlock(&vclock->lock);
  94. return 0;
  95. }
  96. static int ptp_vclock_getcrosststamp(struct ptp_clock_info *ptp,
  97. struct system_device_crosststamp *xtstamp)
  98. {
  99. struct ptp_vclock *vclock = info_to_vclock(ptp);
  100. struct ptp_clock *pptp = vclock->pclock;
  101. int err;
  102. u64 ns;
  103. err = pptp->info->getcrosscycles(pptp->info, xtstamp);
  104. if (err)
  105. return err;
  106. if (mutex_lock_interruptible(&vclock->lock))
  107. return -EINTR;
  108. ns = timecounter_cyc2time(&vclock->tc, ktime_to_ns(xtstamp->device));
  109. mutex_unlock(&vclock->lock);
  110. xtstamp->device = ns_to_ktime(ns);
  111. return 0;
  112. }
  113. static long ptp_vclock_refresh(struct ptp_clock_info *ptp)
  114. {
  115. struct ptp_vclock *vclock = info_to_vclock(ptp);
  116. struct timespec64 ts;
  117. ptp_vclock_gettime(&vclock->info, &ts);
  118. return PTP_VCLOCK_REFRESH_INTERVAL;
  119. }
  120. static const struct ptp_clock_info ptp_vclock_info = {
  121. .owner = THIS_MODULE,
  122. .name = "ptp virtual clock",
  123. .max_adj = 500000000,
  124. .adjfine = ptp_vclock_adjfine,
  125. .adjtime = ptp_vclock_adjtime,
  126. .settime64 = ptp_vclock_settime,
  127. .do_aux_work = ptp_vclock_refresh,
  128. };
  129. static u64 ptp_vclock_read(const struct cyclecounter *cc)
  130. {
  131. struct ptp_vclock *vclock = cc_to_vclock(cc);
  132. struct ptp_clock *ptp = vclock->pclock;
  133. struct timespec64 ts = {};
  134. ptp->info->getcycles64(ptp->info, &ts);
  135. return timespec64_to_ns(&ts);
  136. }
  137. static const struct cyclecounter ptp_vclock_cc = {
  138. .read = ptp_vclock_read,
  139. .mask = CYCLECOUNTER_MASK(32),
  140. .mult = PTP_VCLOCK_CC_MULT,
  141. .shift = PTP_VCLOCK_CC_SHIFT,
  142. };
  143. struct ptp_vclock *ptp_vclock_register(struct ptp_clock *pclock)
  144. {
  145. struct ptp_vclock *vclock;
  146. vclock = kzalloc(sizeof(*vclock), GFP_KERNEL);
  147. if (!vclock)
  148. return NULL;
  149. vclock->pclock = pclock;
  150. vclock->info = ptp_vclock_info;
  151. if (pclock->info->getcyclesx64)
  152. vclock->info.gettimex64 = ptp_vclock_gettimex;
  153. else
  154. vclock->info.gettime64 = ptp_vclock_gettime;
  155. if (pclock->info->getcrosscycles)
  156. vclock->info.getcrosststamp = ptp_vclock_getcrosststamp;
  157. vclock->cc = ptp_vclock_cc;
  158. snprintf(vclock->info.name, PTP_CLOCK_NAME_LEN, "ptp%d_virt",
  159. pclock->index);
  160. INIT_HLIST_NODE(&vclock->vclock_hash_node);
  161. mutex_init(&vclock->lock);
  162. vclock->clock = ptp_clock_register(&vclock->info, &pclock->dev);
  163. if (IS_ERR_OR_NULL(vclock->clock)) {
  164. kfree(vclock);
  165. return NULL;
  166. }
  167. timecounter_init(&vclock->tc, &vclock->cc, 0);
  168. ptp_schedule_worker(vclock->clock, PTP_VCLOCK_REFRESH_INTERVAL);
  169. ptp_vclock_hash_add(vclock);
  170. return vclock;
  171. }
  172. void ptp_vclock_unregister(struct ptp_vclock *vclock)
  173. {
  174. ptp_vclock_hash_del(vclock);
  175. ptp_clock_unregister(vclock->clock);
  176. kfree(vclock);
  177. }
  178. #if IS_BUILTIN(CONFIG_PTP_1588_CLOCK)
  179. int ptp_get_vclocks_index(int pclock_index, int **vclock_index)
  180. {
  181. char name[PTP_CLOCK_NAME_LEN] = "";
  182. struct ptp_clock *ptp;
  183. struct device *dev;
  184. int num = 0;
  185. if (pclock_index < 0)
  186. return num;
  187. snprintf(name, PTP_CLOCK_NAME_LEN, "ptp%d", pclock_index);
  188. dev = class_find_device_by_name(ptp_class, name);
  189. if (!dev)
  190. return num;
  191. ptp = dev_get_drvdata(dev);
  192. if (mutex_lock_interruptible(&ptp->n_vclocks_mux)) {
  193. put_device(dev);
  194. return num;
  195. }
  196. *vclock_index = kzalloc(sizeof(int) * ptp->n_vclocks, GFP_KERNEL);
  197. if (!(*vclock_index))
  198. goto out;
  199. memcpy(*vclock_index, ptp->vclock_index, sizeof(int) * ptp->n_vclocks);
  200. num = ptp->n_vclocks;
  201. out:
  202. mutex_unlock(&ptp->n_vclocks_mux);
  203. put_device(dev);
  204. return num;
  205. }
  206. EXPORT_SYMBOL(ptp_get_vclocks_index);
  207. ktime_t ptp_convert_timestamp(const ktime_t *hwtstamp, int vclock_index)
  208. {
  209. unsigned int hash = vclock_index % HASH_SIZE(vclock_hash);
  210. struct ptp_vclock *vclock;
  211. u64 ns;
  212. u64 vclock_ns = 0;
  213. ns = ktime_to_ns(*hwtstamp);
  214. rcu_read_lock();
  215. hlist_for_each_entry_rcu(vclock, &vclock_hash[hash], vclock_hash_node) {
  216. if (vclock->clock->index != vclock_index)
  217. continue;
  218. if (mutex_lock_interruptible(&vclock->lock))
  219. break;
  220. vclock_ns = timecounter_cyc2time(&vclock->tc, ns);
  221. mutex_unlock(&vclock->lock);
  222. break;
  223. }
  224. rcu_read_unlock();
  225. return ns_to_ktime(vclock_ns);
  226. }
  227. EXPORT_SYMBOL(ptp_convert_timestamp);
  228. #endif