ptp_chardev.c 12 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505
  1. // SPDX-License-Identifier: GPL-2.0-or-later
  2. /*
  3. * PTP 1588 clock support - character device implementation.
  4. *
  5. * Copyright (C) 2010 OMICRON electronics GmbH
  6. */
  7. #include <linux/module.h>
  8. #include <linux/posix-clock.h>
  9. #include <linux/poll.h>
  10. #include <linux/sched.h>
  11. #include <linux/slab.h>
  12. #include <linux/timekeeping.h>
  13. #include <linux/nospec.h>
  14. #include "ptp_private.h"
  15. static int ptp_disable_pinfunc(struct ptp_clock_info *ops,
  16. enum ptp_pin_function func, unsigned int chan)
  17. {
  18. struct ptp_clock_request rq;
  19. int err = 0;
  20. memset(&rq, 0, sizeof(rq));
  21. switch (func) {
  22. case PTP_PF_NONE:
  23. break;
  24. case PTP_PF_EXTTS:
  25. rq.type = PTP_CLK_REQ_EXTTS;
  26. rq.extts.index = chan;
  27. err = ops->enable(ops, &rq, 0);
  28. break;
  29. case PTP_PF_PEROUT:
  30. rq.type = PTP_CLK_REQ_PEROUT;
  31. rq.perout.index = chan;
  32. err = ops->enable(ops, &rq, 0);
  33. break;
  34. case PTP_PF_PHYSYNC:
  35. break;
  36. default:
  37. return -EINVAL;
  38. }
  39. return err;
  40. }
  41. int ptp_set_pinfunc(struct ptp_clock *ptp, unsigned int pin,
  42. enum ptp_pin_function func, unsigned int chan)
  43. {
  44. struct ptp_clock_info *info = ptp->info;
  45. struct ptp_pin_desc *pin1 = NULL, *pin2 = &info->pin_config[pin];
  46. unsigned int i;
  47. /* Check to see if any other pin previously had this function. */
  48. for (i = 0; i < info->n_pins; i++) {
  49. if (info->pin_config[i].func == func &&
  50. info->pin_config[i].chan == chan) {
  51. pin1 = &info->pin_config[i];
  52. break;
  53. }
  54. }
  55. if (pin1 && i == pin)
  56. return 0;
  57. /* Check the desired function and channel. */
  58. switch (func) {
  59. case PTP_PF_NONE:
  60. break;
  61. case PTP_PF_EXTTS:
  62. if (chan >= info->n_ext_ts)
  63. return -EINVAL;
  64. break;
  65. case PTP_PF_PEROUT:
  66. if (chan >= info->n_per_out)
  67. return -EINVAL;
  68. break;
  69. case PTP_PF_PHYSYNC:
  70. if (chan != 0)
  71. return -EINVAL;
  72. break;
  73. default:
  74. return -EINVAL;
  75. }
  76. if (info->verify(info, pin, func, chan)) {
  77. pr_err("driver cannot use function %u on pin %u\n", func, chan);
  78. return -EOPNOTSUPP;
  79. }
  80. /* Disable whatever function was previously assigned. */
  81. if (pin1) {
  82. ptp_disable_pinfunc(info, func, chan);
  83. pin1->func = PTP_PF_NONE;
  84. pin1->chan = 0;
  85. }
  86. ptp_disable_pinfunc(info, pin2->func, pin2->chan);
  87. pin2->func = func;
  88. pin2->chan = chan;
  89. return 0;
  90. }
  91. int ptp_open(struct posix_clock *pc, fmode_t fmode)
  92. {
  93. return 0;
  94. }
  95. long ptp_ioctl(struct posix_clock *pc, unsigned int cmd, unsigned long arg)
  96. {
  97. struct ptp_clock *ptp = container_of(pc, struct ptp_clock, clock);
  98. struct ptp_sys_offset_extended *extoff = NULL;
  99. struct ptp_sys_offset_precise precise_offset;
  100. struct system_device_crosststamp xtstamp;
  101. struct ptp_clock_info *ops = ptp->info;
  102. struct ptp_sys_offset *sysoff = NULL;
  103. struct ptp_system_timestamp sts;
  104. struct ptp_clock_request req;
  105. struct ptp_clock_caps caps;
  106. struct ptp_clock_time *pct;
  107. unsigned int i, pin_index;
  108. struct ptp_pin_desc pd;
  109. struct timespec64 ts;
  110. int enable, err = 0;
  111. switch (cmd) {
  112. case PTP_CLOCK_GETCAPS:
  113. case PTP_CLOCK_GETCAPS2:
  114. memset(&caps, 0, sizeof(caps));
  115. caps.max_adj = ptp->info->max_adj;
  116. caps.n_alarm = ptp->info->n_alarm;
  117. caps.n_ext_ts = ptp->info->n_ext_ts;
  118. caps.n_per_out = ptp->info->n_per_out;
  119. caps.pps = ptp->info->pps;
  120. caps.n_pins = ptp->info->n_pins;
  121. caps.cross_timestamping = ptp->info->getcrosststamp != NULL;
  122. caps.adjust_phase = ptp->info->adjphase != NULL;
  123. if (copy_to_user((void __user *)arg, &caps, sizeof(caps)))
  124. err = -EFAULT;
  125. break;
  126. case PTP_EXTTS_REQUEST:
  127. case PTP_EXTTS_REQUEST2:
  128. memset(&req, 0, sizeof(req));
  129. if (copy_from_user(&req.extts, (void __user *)arg,
  130. sizeof(req.extts))) {
  131. err = -EFAULT;
  132. break;
  133. }
  134. if (cmd == PTP_EXTTS_REQUEST2) {
  135. /* Tell the drivers to check the flags carefully. */
  136. req.extts.flags |= PTP_STRICT_FLAGS;
  137. /* Make sure no reserved bit is set. */
  138. if ((req.extts.flags & ~PTP_EXTTS_VALID_FLAGS) ||
  139. req.extts.rsv[0] || req.extts.rsv[1]) {
  140. err = -EINVAL;
  141. break;
  142. }
  143. /* Ensure one of the rising/falling edge bits is set. */
  144. if ((req.extts.flags & PTP_ENABLE_FEATURE) &&
  145. (req.extts.flags & PTP_EXTTS_EDGES) == 0) {
  146. err = -EINVAL;
  147. break;
  148. }
  149. } else if (cmd == PTP_EXTTS_REQUEST) {
  150. req.extts.flags &= PTP_EXTTS_V1_VALID_FLAGS;
  151. req.extts.rsv[0] = 0;
  152. req.extts.rsv[1] = 0;
  153. }
  154. if (req.extts.index >= ops->n_ext_ts) {
  155. err = -EINVAL;
  156. break;
  157. }
  158. req.type = PTP_CLK_REQ_EXTTS;
  159. enable = req.extts.flags & PTP_ENABLE_FEATURE ? 1 : 0;
  160. if (mutex_lock_interruptible(&ptp->pincfg_mux))
  161. return -ERESTARTSYS;
  162. err = ops->enable(ops, &req, enable);
  163. mutex_unlock(&ptp->pincfg_mux);
  164. break;
  165. case PTP_PEROUT_REQUEST:
  166. case PTP_PEROUT_REQUEST2:
  167. memset(&req, 0, sizeof(req));
  168. if (copy_from_user(&req.perout, (void __user *)arg,
  169. sizeof(req.perout))) {
  170. err = -EFAULT;
  171. break;
  172. }
  173. if (cmd == PTP_PEROUT_REQUEST2) {
  174. struct ptp_perout_request *perout = &req.perout;
  175. if (perout->flags & ~PTP_PEROUT_VALID_FLAGS) {
  176. err = -EINVAL;
  177. break;
  178. }
  179. /*
  180. * The "on" field has undefined meaning if
  181. * PTP_PEROUT_DUTY_CYCLE isn't set, we must still treat
  182. * it as reserved, which must be set to zero.
  183. */
  184. if (!(perout->flags & PTP_PEROUT_DUTY_CYCLE) &&
  185. (perout->rsv[0] || perout->rsv[1] ||
  186. perout->rsv[2] || perout->rsv[3])) {
  187. err = -EINVAL;
  188. break;
  189. }
  190. if (perout->flags & PTP_PEROUT_DUTY_CYCLE) {
  191. /* The duty cycle must be subunitary. */
  192. if (perout->on.sec > perout->period.sec ||
  193. (perout->on.sec == perout->period.sec &&
  194. perout->on.nsec > perout->period.nsec)) {
  195. err = -ERANGE;
  196. break;
  197. }
  198. }
  199. if (perout->flags & PTP_PEROUT_PHASE) {
  200. /*
  201. * The phase should be specified modulo the
  202. * period, therefore anything equal or larger
  203. * than 1 period is invalid.
  204. */
  205. if (perout->phase.sec > perout->period.sec ||
  206. (perout->phase.sec == perout->period.sec &&
  207. perout->phase.nsec >= perout->period.nsec)) {
  208. err = -ERANGE;
  209. break;
  210. }
  211. }
  212. } else if (cmd == PTP_PEROUT_REQUEST) {
  213. req.perout.flags &= PTP_PEROUT_V1_VALID_FLAGS;
  214. req.perout.rsv[0] = 0;
  215. req.perout.rsv[1] = 0;
  216. req.perout.rsv[2] = 0;
  217. req.perout.rsv[3] = 0;
  218. }
  219. if (req.perout.index >= ops->n_per_out) {
  220. err = -EINVAL;
  221. break;
  222. }
  223. req.type = PTP_CLK_REQ_PEROUT;
  224. enable = req.perout.period.sec || req.perout.period.nsec;
  225. if (mutex_lock_interruptible(&ptp->pincfg_mux))
  226. return -ERESTARTSYS;
  227. err = ops->enable(ops, &req, enable);
  228. mutex_unlock(&ptp->pincfg_mux);
  229. break;
  230. case PTP_ENABLE_PPS:
  231. case PTP_ENABLE_PPS2:
  232. memset(&req, 0, sizeof(req));
  233. if (!capable(CAP_SYS_TIME))
  234. return -EPERM;
  235. req.type = PTP_CLK_REQ_PPS;
  236. enable = arg ? 1 : 0;
  237. if (mutex_lock_interruptible(&ptp->pincfg_mux))
  238. return -ERESTARTSYS;
  239. err = ops->enable(ops, &req, enable);
  240. mutex_unlock(&ptp->pincfg_mux);
  241. break;
  242. case PTP_SYS_OFFSET_PRECISE:
  243. case PTP_SYS_OFFSET_PRECISE2:
  244. if (!ptp->info->getcrosststamp) {
  245. err = -EOPNOTSUPP;
  246. break;
  247. }
  248. err = ptp->info->getcrosststamp(ptp->info, &xtstamp);
  249. if (err)
  250. break;
  251. memset(&precise_offset, 0, sizeof(precise_offset));
  252. ts = ktime_to_timespec64(xtstamp.device);
  253. precise_offset.device.sec = ts.tv_sec;
  254. precise_offset.device.nsec = ts.tv_nsec;
  255. ts = ktime_to_timespec64(xtstamp.sys_realtime);
  256. precise_offset.sys_realtime.sec = ts.tv_sec;
  257. precise_offset.sys_realtime.nsec = ts.tv_nsec;
  258. ts = ktime_to_timespec64(xtstamp.sys_monoraw);
  259. precise_offset.sys_monoraw.sec = ts.tv_sec;
  260. precise_offset.sys_monoraw.nsec = ts.tv_nsec;
  261. if (copy_to_user((void __user *)arg, &precise_offset,
  262. sizeof(precise_offset)))
  263. err = -EFAULT;
  264. break;
  265. case PTP_SYS_OFFSET_EXTENDED:
  266. case PTP_SYS_OFFSET_EXTENDED2:
  267. if (!ptp->info->gettimex64) {
  268. err = -EOPNOTSUPP;
  269. break;
  270. }
  271. extoff = memdup_user((void __user *)arg, sizeof(*extoff));
  272. if (IS_ERR(extoff)) {
  273. err = PTR_ERR(extoff);
  274. extoff = NULL;
  275. break;
  276. }
  277. if (extoff->n_samples > PTP_MAX_SAMPLES
  278. || extoff->rsv[0] || extoff->rsv[1] || extoff->rsv[2]) {
  279. err = -EINVAL;
  280. break;
  281. }
  282. for (i = 0; i < extoff->n_samples; i++) {
  283. err = ptp->info->gettimex64(ptp->info, &ts, &sts);
  284. if (err)
  285. goto out;
  286. extoff->ts[i][0].sec = sts.pre_ts.tv_sec;
  287. extoff->ts[i][0].nsec = sts.pre_ts.tv_nsec;
  288. extoff->ts[i][1].sec = ts.tv_sec;
  289. extoff->ts[i][1].nsec = ts.tv_nsec;
  290. extoff->ts[i][2].sec = sts.post_ts.tv_sec;
  291. extoff->ts[i][2].nsec = sts.post_ts.tv_nsec;
  292. }
  293. if (copy_to_user((void __user *)arg, extoff, sizeof(*extoff)))
  294. err = -EFAULT;
  295. break;
  296. case PTP_SYS_OFFSET:
  297. case PTP_SYS_OFFSET2:
  298. sysoff = memdup_user((void __user *)arg, sizeof(*sysoff));
  299. if (IS_ERR(sysoff)) {
  300. err = PTR_ERR(sysoff);
  301. sysoff = NULL;
  302. break;
  303. }
  304. if (sysoff->n_samples > PTP_MAX_SAMPLES) {
  305. err = -EINVAL;
  306. break;
  307. }
  308. pct = &sysoff->ts[0];
  309. for (i = 0; i < sysoff->n_samples; i++) {
  310. ktime_get_real_ts64(&ts);
  311. pct->sec = ts.tv_sec;
  312. pct->nsec = ts.tv_nsec;
  313. pct++;
  314. if (ops->gettimex64)
  315. err = ops->gettimex64(ops, &ts, NULL);
  316. else
  317. err = ops->gettime64(ops, &ts);
  318. if (err)
  319. goto out;
  320. pct->sec = ts.tv_sec;
  321. pct->nsec = ts.tv_nsec;
  322. pct++;
  323. }
  324. ktime_get_real_ts64(&ts);
  325. pct->sec = ts.tv_sec;
  326. pct->nsec = ts.tv_nsec;
  327. if (copy_to_user((void __user *)arg, sysoff, sizeof(*sysoff)))
  328. err = -EFAULT;
  329. break;
  330. case PTP_PIN_GETFUNC:
  331. case PTP_PIN_GETFUNC2:
  332. if (copy_from_user(&pd, (void __user *)arg, sizeof(pd))) {
  333. err = -EFAULT;
  334. break;
  335. }
  336. if ((pd.rsv[0] || pd.rsv[1] || pd.rsv[2]
  337. || pd.rsv[3] || pd.rsv[4])
  338. && cmd == PTP_PIN_GETFUNC2) {
  339. err = -EINVAL;
  340. break;
  341. } else if (cmd == PTP_PIN_GETFUNC) {
  342. pd.rsv[0] = 0;
  343. pd.rsv[1] = 0;
  344. pd.rsv[2] = 0;
  345. pd.rsv[3] = 0;
  346. pd.rsv[4] = 0;
  347. }
  348. pin_index = pd.index;
  349. if (pin_index >= ops->n_pins) {
  350. err = -EINVAL;
  351. break;
  352. }
  353. pin_index = array_index_nospec(pin_index, ops->n_pins);
  354. if (mutex_lock_interruptible(&ptp->pincfg_mux))
  355. return -ERESTARTSYS;
  356. pd = ops->pin_config[pin_index];
  357. mutex_unlock(&ptp->pincfg_mux);
  358. if (!err && copy_to_user((void __user *)arg, &pd, sizeof(pd)))
  359. err = -EFAULT;
  360. break;
  361. case PTP_PIN_SETFUNC:
  362. case PTP_PIN_SETFUNC2:
  363. if (copy_from_user(&pd, (void __user *)arg, sizeof(pd))) {
  364. err = -EFAULT;
  365. break;
  366. }
  367. if ((pd.rsv[0] || pd.rsv[1] || pd.rsv[2]
  368. || pd.rsv[3] || pd.rsv[4])
  369. && cmd == PTP_PIN_SETFUNC2) {
  370. err = -EINVAL;
  371. break;
  372. } else if (cmd == PTP_PIN_SETFUNC) {
  373. pd.rsv[0] = 0;
  374. pd.rsv[1] = 0;
  375. pd.rsv[2] = 0;
  376. pd.rsv[3] = 0;
  377. pd.rsv[4] = 0;
  378. }
  379. pin_index = pd.index;
  380. if (pin_index >= ops->n_pins) {
  381. err = -EINVAL;
  382. break;
  383. }
  384. pin_index = array_index_nospec(pin_index, ops->n_pins);
  385. if (mutex_lock_interruptible(&ptp->pincfg_mux))
  386. return -ERESTARTSYS;
  387. err = ptp_set_pinfunc(ptp, pin_index, pd.func, pd.chan);
  388. mutex_unlock(&ptp->pincfg_mux);
  389. break;
  390. default:
  391. err = -ENOTTY;
  392. break;
  393. }
  394. out:
  395. kfree(extoff);
  396. kfree(sysoff);
  397. return err;
  398. }
  399. __poll_t ptp_poll(struct posix_clock *pc, struct file *fp, poll_table *wait)
  400. {
  401. struct ptp_clock *ptp = container_of(pc, struct ptp_clock, clock);
  402. poll_wait(fp, &ptp->tsev_wq, wait);
  403. return queue_cnt(&ptp->tsevq) ? EPOLLIN : 0;
  404. }
  405. #define EXTTS_BUFSIZE (PTP_BUF_TIMESTAMPS * sizeof(struct ptp_extts_event))
  406. ssize_t ptp_read(struct posix_clock *pc,
  407. uint rdflags, char __user *buf, size_t cnt)
  408. {
  409. struct ptp_clock *ptp = container_of(pc, struct ptp_clock, clock);
  410. struct timestamp_event_queue *queue = &ptp->tsevq;
  411. struct ptp_extts_event *event;
  412. unsigned long flags;
  413. size_t qcnt, i;
  414. int result;
  415. if (cnt % sizeof(struct ptp_extts_event) != 0)
  416. return -EINVAL;
  417. if (cnt > EXTTS_BUFSIZE)
  418. cnt = EXTTS_BUFSIZE;
  419. cnt = cnt / sizeof(struct ptp_extts_event);
  420. if (mutex_lock_interruptible(&ptp->tsevq_mux))
  421. return -ERESTARTSYS;
  422. if (wait_event_interruptible(ptp->tsev_wq,
  423. ptp->defunct || queue_cnt(queue))) {
  424. mutex_unlock(&ptp->tsevq_mux);
  425. return -ERESTARTSYS;
  426. }
  427. if (ptp->defunct) {
  428. mutex_unlock(&ptp->tsevq_mux);
  429. return -ENODEV;
  430. }
  431. event = kmalloc(EXTTS_BUFSIZE, GFP_KERNEL);
  432. if (!event) {
  433. mutex_unlock(&ptp->tsevq_mux);
  434. return -ENOMEM;
  435. }
  436. spin_lock_irqsave(&queue->lock, flags);
  437. qcnt = queue_cnt(queue);
  438. if (cnt > qcnt)
  439. cnt = qcnt;
  440. for (i = 0; i < cnt; i++) {
  441. event[i] = queue->buf[queue->head];
  442. queue->head = (queue->head + 1) % PTP_MAX_TIMESTAMPS;
  443. }
  444. spin_unlock_irqrestore(&queue->lock, flags);
  445. cnt = cnt * sizeof(struct ptp_extts_event);
  446. mutex_unlock(&ptp->tsevq_mux);
  447. result = cnt;
  448. if (copy_to_user(buf, event, cnt))
  449. result = -EFAULT;
  450. kfree(event);
  451. return result;
  452. }