Richard Cochran | d94ba80 | 2011-04-22 12:03:08 +0200 | [diff] [blame] | 1 | /* |
| 2 | * PTP 1588 clock support - character device implementation. |
| 3 | * |
| 4 | * Copyright (C) 2010 OMICRON electronics GmbH |
| 5 | * |
| 6 | * This program is free software; you can redistribute it and/or modify |
| 7 | * it under the terms of the GNU General Public License as published by |
| 8 | * the Free Software Foundation; either version 2 of the License, or |
| 9 | * (at your option) any later version. |
| 10 | * |
| 11 | * This program is distributed in the hope that it will be useful, |
| 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
| 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| 14 | * GNU General Public License for more details. |
| 15 | * |
| 16 | * You should have received a copy of the GNU General Public License |
| 17 | * along with this program; if not, write to the Free Software |
| 18 | * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. |
| 19 | */ |
| 20 | #include <linux/module.h> |
| 21 | #include <linux/posix-clock.h> |
| 22 | #include <linux/poll.h> |
| 23 | #include <linux/sched.h> |
Richard Cochran | c7ec0ba | 2012-11-26 01:44:34 +0000 | [diff] [blame] | 24 | #include <linux/slab.h> |
Christopher S. Hall | 719f1aa | 2016-02-22 03:15:25 -0800 | [diff] [blame] | 25 | #include <linux/timekeeping.h> |
Richard Cochran | d94ba80 | 2011-04-22 12:03:08 +0200 | [diff] [blame] | 26 | |
| 27 | #include "ptp_private.h" |
| 28 | |
Richard Cochran | 6092315 | 2014-03-20 22:21:52 +0100 | [diff] [blame] | 29 | static int ptp_disable_pinfunc(struct ptp_clock_info *ops, |
| 30 | enum ptp_pin_function func, unsigned int chan) |
| 31 | { |
| 32 | struct ptp_clock_request rq; |
| 33 | int err = 0; |
| 34 | |
| 35 | memset(&rq, 0, sizeof(rq)); |
| 36 | |
| 37 | switch (func) { |
| 38 | case PTP_PF_NONE: |
| 39 | break; |
| 40 | case PTP_PF_EXTTS: |
| 41 | rq.type = PTP_CLK_REQ_EXTTS; |
| 42 | rq.extts.index = chan; |
| 43 | err = ops->enable(ops, &rq, 0); |
| 44 | break; |
| 45 | case PTP_PF_PEROUT: |
| 46 | rq.type = PTP_CLK_REQ_PEROUT; |
| 47 | rq.perout.index = chan; |
| 48 | err = ops->enable(ops, &rq, 0); |
| 49 | break; |
| 50 | case PTP_PF_PHYSYNC: |
| 51 | break; |
| 52 | default: |
| 53 | return -EINVAL; |
| 54 | } |
| 55 | |
| 56 | return err; |
| 57 | } |
| 58 | |
| 59 | int ptp_set_pinfunc(struct ptp_clock *ptp, unsigned int pin, |
| 60 | enum ptp_pin_function func, unsigned int chan) |
| 61 | { |
| 62 | struct ptp_clock_info *info = ptp->info; |
| 63 | struct ptp_pin_desc *pin1 = NULL, *pin2 = &info->pin_config[pin]; |
| 64 | unsigned int i; |
| 65 | |
| 66 | /* Check to see if any other pin previously had this function. */ |
| 67 | for (i = 0; i < info->n_pins; i++) { |
| 68 | if (info->pin_config[i].func == func && |
| 69 | info->pin_config[i].chan == chan) { |
| 70 | pin1 = &info->pin_config[i]; |
| 71 | break; |
| 72 | } |
| 73 | } |
| 74 | if (pin1 && i == pin) |
| 75 | return 0; |
| 76 | |
| 77 | /* Check the desired function and channel. */ |
| 78 | switch (func) { |
| 79 | case PTP_PF_NONE: |
| 80 | break; |
| 81 | case PTP_PF_EXTTS: |
| 82 | if (chan >= info->n_ext_ts) |
| 83 | return -EINVAL; |
| 84 | break; |
| 85 | case PTP_PF_PEROUT: |
| 86 | if (chan >= info->n_per_out) |
| 87 | return -EINVAL; |
| 88 | break; |
| 89 | case PTP_PF_PHYSYNC: |
Stefan Sørensen | 72df7a7 | 2014-06-27 12:05:33 +0200 | [diff] [blame] | 90 | if (chan != 0) |
| 91 | return -EINVAL; |
Gustavo A. R. Silva | 5ac2bc6 | 2018-07-17 20:17:33 -0500 | [diff] [blame] | 92 | break; |
Richard Cochran | 6092315 | 2014-03-20 22:21:52 +0100 | [diff] [blame] | 93 | default: |
| 94 | return -EINVAL; |
| 95 | } |
| 96 | |
Richard Cochran | 6092315 | 2014-03-20 22:21:52 +0100 | [diff] [blame] | 97 | if (info->verify(info, pin, func, chan)) { |
| 98 | pr_err("driver cannot use function %u on pin %u\n", func, chan); |
| 99 | return -EOPNOTSUPP; |
| 100 | } |
| 101 | |
| 102 | /* Disable whatever function was previously assigned. */ |
| 103 | if (pin1) { |
| 104 | ptp_disable_pinfunc(info, func, chan); |
| 105 | pin1->func = PTP_PF_NONE; |
| 106 | pin1->chan = 0; |
| 107 | } |
| 108 | ptp_disable_pinfunc(info, pin2->func, pin2->chan); |
| 109 | pin2->func = func; |
| 110 | pin2->chan = chan; |
| 111 | |
| 112 | return 0; |
| 113 | } |
| 114 | |
Richard Cochran | d94ba80 | 2011-04-22 12:03:08 +0200 | [diff] [blame] | 115 | int ptp_open(struct posix_clock *pc, fmode_t fmode) |
| 116 | { |
| 117 | return 0; |
| 118 | } |
| 119 | |
| 120 | long ptp_ioctl(struct posix_clock *pc, unsigned int cmd, unsigned long arg) |
| 121 | { |
| 122 | struct ptp_clock_caps caps; |
| 123 | struct ptp_clock_request req; |
Richard Cochran | c3484c2 | 2012-11-26 01:44:35 +0000 | [diff] [blame] | 124 | struct ptp_sys_offset *sysoff = NULL; |
Christopher S. Hall | 719f1aa | 2016-02-22 03:15:25 -0800 | [diff] [blame] | 125 | struct ptp_sys_offset_precise precise_offset; |
Richard Cochran | 6092315 | 2014-03-20 22:21:52 +0100 | [diff] [blame] | 126 | struct ptp_pin_desc pd; |
Richard Cochran | d94ba80 | 2011-04-22 12:03:08 +0200 | [diff] [blame] | 127 | struct ptp_clock *ptp = container_of(pc, struct ptp_clock, clock); |
| 128 | struct ptp_clock_info *ops = ptp->info; |
Richard Cochran | 215b13d | 2012-10-31 06:19:07 +0000 | [diff] [blame] | 129 | struct ptp_clock_time *pct; |
Richard Cochran | e13cfcb | 2015-03-29 23:11:52 +0200 | [diff] [blame] | 130 | struct timespec64 ts; |
Christopher S. Hall | 719f1aa | 2016-02-22 03:15:25 -0800 | [diff] [blame] | 131 | struct system_device_crosststamp xtstamp; |
Richard Cochran | d94ba80 | 2011-04-22 12:03:08 +0200 | [diff] [blame] | 132 | int enable, err = 0; |
Richard Cochran | 6092315 | 2014-03-20 22:21:52 +0100 | [diff] [blame] | 133 | unsigned int i, pin_index; |
Richard Cochran | d94ba80 | 2011-04-22 12:03:08 +0200 | [diff] [blame] | 134 | |
| 135 | switch (cmd) { |
| 136 | |
| 137 | case PTP_CLOCK_GETCAPS: |
| 138 | memset(&caps, 0, sizeof(caps)); |
| 139 | caps.max_adj = ptp->info->max_adj; |
| 140 | caps.n_alarm = ptp->info->n_alarm; |
| 141 | caps.n_ext_ts = ptp->info->n_ext_ts; |
| 142 | caps.n_per_out = ptp->info->n_per_out; |
| 143 | caps.pps = ptp->info->pps; |
Richard Cochran | 6092315 | 2014-03-20 22:21:52 +0100 | [diff] [blame] | 144 | caps.n_pins = ptp->info->n_pins; |
Christopher S. Hall | 719f1aa | 2016-02-22 03:15:25 -0800 | [diff] [blame] | 145 | caps.cross_timestamping = ptp->info->getcrosststamp != NULL; |
Dan Carpenter | e23ef22 | 2011-05-29 22:53:12 +0300 | [diff] [blame] | 146 | if (copy_to_user((void __user *)arg, &caps, sizeof(caps))) |
| 147 | err = -EFAULT; |
Richard Cochran | d94ba80 | 2011-04-22 12:03:08 +0200 | [diff] [blame] | 148 | break; |
| 149 | |
| 150 | case PTP_EXTTS_REQUEST: |
| 151 | if (copy_from_user(&req.extts, (void __user *)arg, |
| 152 | sizeof(req.extts))) { |
| 153 | err = -EFAULT; |
| 154 | break; |
| 155 | } |
| 156 | if (req.extts.index >= ops->n_ext_ts) { |
| 157 | err = -EINVAL; |
| 158 | break; |
| 159 | } |
| 160 | req.type = PTP_CLK_REQ_EXTTS; |
| 161 | enable = req.extts.flags & PTP_ENABLE_FEATURE ? 1 : 0; |
| 162 | err = ops->enable(ops, &req, enable); |
| 163 | break; |
| 164 | |
| 165 | case PTP_PEROUT_REQUEST: |
| 166 | if (copy_from_user(&req.perout, (void __user *)arg, |
| 167 | sizeof(req.perout))) { |
| 168 | err = -EFAULT; |
| 169 | break; |
| 170 | } |
| 171 | if (req.perout.index >= ops->n_per_out) { |
| 172 | err = -EINVAL; |
| 173 | break; |
| 174 | } |
| 175 | req.type = PTP_CLK_REQ_PEROUT; |
| 176 | enable = req.perout.period.sec || req.perout.period.nsec; |
| 177 | err = ops->enable(ops, &req, enable); |
| 178 | break; |
| 179 | |
| 180 | case PTP_ENABLE_PPS: |
| 181 | if (!capable(CAP_SYS_TIME)) |
| 182 | return -EPERM; |
| 183 | req.type = PTP_CLK_REQ_PPS; |
| 184 | enable = arg ? 1 : 0; |
| 185 | err = ops->enable(ops, &req, enable); |
| 186 | break; |
| 187 | |
Christopher S. Hall | 719f1aa | 2016-02-22 03:15:25 -0800 | [diff] [blame] | 188 | case PTP_SYS_OFFSET_PRECISE: |
| 189 | if (!ptp->info->getcrosststamp) { |
| 190 | err = -EOPNOTSUPP; |
| 191 | break; |
| 192 | } |
| 193 | err = ptp->info->getcrosststamp(ptp->info, &xtstamp); |
| 194 | if (err) |
| 195 | break; |
| 196 | |
Vlad Tsyrklevich | 02a9079 | 2016-10-11 15:02:47 +0200 | [diff] [blame] | 197 | memset(&precise_offset, 0, sizeof(precise_offset)); |
Christopher S. Hall | 719f1aa | 2016-02-22 03:15:25 -0800 | [diff] [blame] | 198 | ts = ktime_to_timespec64(xtstamp.device); |
| 199 | precise_offset.device.sec = ts.tv_sec; |
| 200 | precise_offset.device.nsec = ts.tv_nsec; |
| 201 | ts = ktime_to_timespec64(xtstamp.sys_realtime); |
| 202 | precise_offset.sys_realtime.sec = ts.tv_sec; |
| 203 | precise_offset.sys_realtime.nsec = ts.tv_nsec; |
| 204 | ts = ktime_to_timespec64(xtstamp.sys_monoraw); |
| 205 | precise_offset.sys_monoraw.sec = ts.tv_sec; |
| 206 | precise_offset.sys_monoraw.nsec = ts.tv_nsec; |
| 207 | if (copy_to_user((void __user *)arg, &precise_offset, |
| 208 | sizeof(precise_offset))) |
| 209 | err = -EFAULT; |
| 210 | break; |
| 211 | |
Richard Cochran | 215b13d | 2012-10-31 06:19:07 +0000 | [diff] [blame] | 212 | case PTP_SYS_OFFSET: |
Muhammad Falak R Wani | 2ece068 | 2016-05-20 17:51:02 +0530 | [diff] [blame] | 213 | sysoff = memdup_user((void __user *)arg, sizeof(*sysoff)); |
| 214 | if (IS_ERR(sysoff)) { |
| 215 | err = PTR_ERR(sysoff); |
Dan Carpenter | 6756325 | 2016-05-26 09:46:22 +0300 | [diff] [blame] | 216 | sysoff = NULL; |
Richard Cochran | 215b13d | 2012-10-31 06:19:07 +0000 | [diff] [blame] | 217 | break; |
| 218 | } |
Richard Cochran | c3484c2 | 2012-11-26 01:44:35 +0000 | [diff] [blame] | 219 | if (sysoff->n_samples > PTP_MAX_SAMPLES) { |
Richard Cochran | 215b13d | 2012-10-31 06:19:07 +0000 | [diff] [blame] | 220 | err = -EINVAL; |
| 221 | break; |
| 222 | } |
Richard Cochran | c3484c2 | 2012-11-26 01:44:35 +0000 | [diff] [blame] | 223 | pct = &sysoff->ts[0]; |
| 224 | for (i = 0; i < sysoff->n_samples; i++) { |
Richard Cochran | e13cfcb | 2015-03-29 23:11:52 +0200 | [diff] [blame] | 225 | getnstimeofday64(&ts); |
Richard Cochran | 215b13d | 2012-10-31 06:19:07 +0000 | [diff] [blame] | 226 | pct->sec = ts.tv_sec; |
| 227 | pct->nsec = ts.tv_nsec; |
| 228 | pct++; |
Richard Cochran | ed7c631 | 2015-03-29 23:12:13 +0200 | [diff] [blame] | 229 | ptp->info->gettime64(ptp->info, &ts); |
Richard Cochran | 215b13d | 2012-10-31 06:19:07 +0000 | [diff] [blame] | 230 | pct->sec = ts.tv_sec; |
| 231 | pct->nsec = ts.tv_nsec; |
| 232 | pct++; |
| 233 | } |
Richard Cochran | e13cfcb | 2015-03-29 23:11:52 +0200 | [diff] [blame] | 234 | getnstimeofday64(&ts); |
Richard Cochran | 215b13d | 2012-10-31 06:19:07 +0000 | [diff] [blame] | 235 | pct->sec = ts.tv_sec; |
| 236 | pct->nsec = ts.tv_nsec; |
Richard Cochran | c3484c2 | 2012-11-26 01:44:35 +0000 | [diff] [blame] | 237 | if (copy_to_user((void __user *)arg, sysoff, sizeof(*sysoff))) |
Richard Cochran | 215b13d | 2012-10-31 06:19:07 +0000 | [diff] [blame] | 238 | err = -EFAULT; |
| 239 | break; |
| 240 | |
Richard Cochran | 6092315 | 2014-03-20 22:21:52 +0100 | [diff] [blame] | 241 | case PTP_PIN_GETFUNC: |
| 242 | if (copy_from_user(&pd, (void __user *)arg, sizeof(pd))) { |
| 243 | err = -EFAULT; |
| 244 | break; |
| 245 | } |
| 246 | pin_index = pd.index; |
| 247 | if (pin_index >= ops->n_pins) { |
| 248 | err = -EINVAL; |
| 249 | break; |
| 250 | } |
| 251 | if (mutex_lock_interruptible(&ptp->pincfg_mux)) |
| 252 | return -ERESTARTSYS; |
| 253 | pd = ops->pin_config[pin_index]; |
| 254 | mutex_unlock(&ptp->pincfg_mux); |
| 255 | if (!err && copy_to_user((void __user *)arg, &pd, sizeof(pd))) |
| 256 | err = -EFAULT; |
| 257 | break; |
| 258 | |
| 259 | case PTP_PIN_SETFUNC: |
| 260 | if (copy_from_user(&pd, (void __user *)arg, sizeof(pd))) { |
| 261 | err = -EFAULT; |
| 262 | break; |
| 263 | } |
| 264 | pin_index = pd.index; |
| 265 | if (pin_index >= ops->n_pins) { |
| 266 | err = -EINVAL; |
| 267 | break; |
| 268 | } |
| 269 | if (mutex_lock_interruptible(&ptp->pincfg_mux)) |
| 270 | return -ERESTARTSYS; |
| 271 | err = ptp_set_pinfunc(ptp, pin_index, pd.func, pd.chan); |
| 272 | mutex_unlock(&ptp->pincfg_mux); |
| 273 | break; |
| 274 | |
Richard Cochran | d94ba80 | 2011-04-22 12:03:08 +0200 | [diff] [blame] | 275 | default: |
| 276 | err = -ENOTTY; |
| 277 | break; |
| 278 | } |
Richard Cochran | c3484c2 | 2012-11-26 01:44:35 +0000 | [diff] [blame] | 279 | |
| 280 | kfree(sysoff); |
Richard Cochran | d94ba80 | 2011-04-22 12:03:08 +0200 | [diff] [blame] | 281 | return err; |
| 282 | } |
| 283 | |
| 284 | unsigned int ptp_poll(struct posix_clock *pc, struct file *fp, poll_table *wait) |
| 285 | { |
| 286 | struct ptp_clock *ptp = container_of(pc, struct ptp_clock, clock); |
| 287 | |
| 288 | poll_wait(fp, &ptp->tsev_wq, wait); |
| 289 | |
| 290 | return queue_cnt(&ptp->tsevq) ? POLLIN : 0; |
| 291 | } |
| 292 | |
Richard Cochran | c7ec0ba | 2012-11-26 01:44:34 +0000 | [diff] [blame] | 293 | #define EXTTS_BUFSIZE (PTP_BUF_TIMESTAMPS * sizeof(struct ptp_extts_event)) |
| 294 | |
Richard Cochran | d94ba80 | 2011-04-22 12:03:08 +0200 | [diff] [blame] | 295 | ssize_t ptp_read(struct posix_clock *pc, |
| 296 | uint rdflags, char __user *buf, size_t cnt) |
| 297 | { |
| 298 | struct ptp_clock *ptp = container_of(pc, struct ptp_clock, clock); |
| 299 | struct timestamp_event_queue *queue = &ptp->tsevq; |
Richard Cochran | c7ec0ba | 2012-11-26 01:44:34 +0000 | [diff] [blame] | 300 | struct ptp_extts_event *event; |
Richard Cochran | d94ba80 | 2011-04-22 12:03:08 +0200 | [diff] [blame] | 301 | unsigned long flags; |
| 302 | size_t qcnt, i; |
Richard Cochran | c7ec0ba | 2012-11-26 01:44:34 +0000 | [diff] [blame] | 303 | int result; |
Richard Cochran | d94ba80 | 2011-04-22 12:03:08 +0200 | [diff] [blame] | 304 | |
| 305 | if (cnt % sizeof(struct ptp_extts_event) != 0) |
| 306 | return -EINVAL; |
| 307 | |
Richard Cochran | c7ec0ba | 2012-11-26 01:44:34 +0000 | [diff] [blame] | 308 | if (cnt > EXTTS_BUFSIZE) |
| 309 | cnt = EXTTS_BUFSIZE; |
Richard Cochran | d94ba80 | 2011-04-22 12:03:08 +0200 | [diff] [blame] | 310 | |
| 311 | cnt = cnt / sizeof(struct ptp_extts_event); |
| 312 | |
| 313 | if (mutex_lock_interruptible(&ptp->tsevq_mux)) |
| 314 | return -ERESTARTSYS; |
| 315 | |
| 316 | if (wait_event_interruptible(ptp->tsev_wq, |
| 317 | ptp->defunct || queue_cnt(queue))) { |
| 318 | mutex_unlock(&ptp->tsevq_mux); |
| 319 | return -ERESTARTSYS; |
| 320 | } |
| 321 | |
Dan Carpenter | fb5a18c | 2011-05-29 22:54:07 +0300 | [diff] [blame] | 322 | if (ptp->defunct) { |
| 323 | mutex_unlock(&ptp->tsevq_mux); |
Richard Cochran | d94ba80 | 2011-04-22 12:03:08 +0200 | [diff] [blame] | 324 | return -ENODEV; |
Dan Carpenter | fb5a18c | 2011-05-29 22:54:07 +0300 | [diff] [blame] | 325 | } |
Richard Cochran | d94ba80 | 2011-04-22 12:03:08 +0200 | [diff] [blame] | 326 | |
Richard Cochran | c7ec0ba | 2012-11-26 01:44:34 +0000 | [diff] [blame] | 327 | event = kmalloc(EXTTS_BUFSIZE, GFP_KERNEL); |
| 328 | if (!event) { |
| 329 | mutex_unlock(&ptp->tsevq_mux); |
| 330 | return -ENOMEM; |
| 331 | } |
| 332 | |
Richard Cochran | d94ba80 | 2011-04-22 12:03:08 +0200 | [diff] [blame] | 333 | spin_lock_irqsave(&queue->lock, flags); |
| 334 | |
| 335 | qcnt = queue_cnt(queue); |
| 336 | |
| 337 | if (cnt > qcnt) |
| 338 | cnt = qcnt; |
| 339 | |
| 340 | for (i = 0; i < cnt; i++) { |
| 341 | event[i] = queue->buf[queue->head]; |
| 342 | queue->head = (queue->head + 1) % PTP_MAX_TIMESTAMPS; |
| 343 | } |
| 344 | |
| 345 | spin_unlock_irqrestore(&queue->lock, flags); |
| 346 | |
| 347 | cnt = cnt * sizeof(struct ptp_extts_event); |
| 348 | |
| 349 | mutex_unlock(&ptp->tsevq_mux); |
| 350 | |
Richard Cochran | c7ec0ba | 2012-11-26 01:44:34 +0000 | [diff] [blame] | 351 | result = cnt; |
Dan Carpenter | fb5a18c | 2011-05-29 22:54:07 +0300 | [diff] [blame] | 352 | if (copy_to_user(buf, event, cnt)) |
Richard Cochran | c7ec0ba | 2012-11-26 01:44:34 +0000 | [diff] [blame] | 353 | result = -EFAULT; |
Richard Cochran | d94ba80 | 2011-04-22 12:03:08 +0200 | [diff] [blame] | 354 | |
Richard Cochran | c7ec0ba | 2012-11-26 01:44:34 +0000 | [diff] [blame] | 355 | kfree(event); |
| 356 | return result; |
Richard Cochran | d94ba80 | 2011-04-22 12:03:08 +0200 | [diff] [blame] | 357 | } |