2 * PTP 1588 clock support
4 * Copyright (C) 2010 OMICRON electronics GmbH
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.
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.
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.
20 #include <linux/bitops.h>
21 #include <linux/device.h>
22 #include <linux/err.h>
23 #include <linux/init.h>
24 #include <linux/kernel.h>
25 #include <linux/module.h>
26 #include <linux/posix-clock.h>
27 #include <linux/pps_kernel.h>
28 #include <linux/slab.h>
29 #include <linux/syscalls.h>
30 #include <linux/uaccess.h>
32 #include "ptp_private.h"
34 #define PTP_MAX_ALARMS 4
35 #define PTP_MAX_CLOCKS 8
36 #define PTP_PPS_DEFAULTS (PPS_CAPTUREASSERT | PPS_OFFSETASSERT)
37 #define PTP_PPS_EVENT PPS_CAPTUREASSERT
38 #define PTP_PPS_MODE (PTP_PPS_DEFAULTS | PPS_CANWAIT | PPS_TSFMT_TSPEC)
42 static dev_t ptp_devt
;
43 static struct class *ptp_class
;
45 static DECLARE_BITMAP(ptp_clocks_map
, PTP_MAX_CLOCKS
);
46 static DEFINE_MUTEX(ptp_clocks_mutex
); /* protects 'ptp_clocks_map' */
48 /* time stamp event queue operations */
50 static inline int queue_free(struct timestamp_event_queue
*q
)
52 return PTP_MAX_TIMESTAMPS
- queue_cnt(q
) - 1;
55 static void enqueue_external_timestamp(struct timestamp_event_queue
*queue
,
56 struct ptp_clock_event
*src
)
58 struct ptp_extts_event
*dst
;
63 seconds
= div_u64_rem(src
->timestamp
, 1000000000, &remainder
);
65 spin_lock_irqsave(&queue
->lock
, flags
);
67 dst
= &queue
->buf
[queue
->tail
];
68 dst
->index
= src
->index
;
70 dst
->t
.nsec
= remainder
;
72 if (!queue_free(queue
))
73 queue
->head
= (queue
->head
+ 1) % PTP_MAX_TIMESTAMPS
;
75 queue
->tail
= (queue
->tail
+ 1) % PTP_MAX_TIMESTAMPS
;
77 spin_unlock_irqrestore(&queue
->lock
, flags
);
80 static s32
scaled_ppm_to_ppb(long ppm
)
83 * The 'freq' field in the 'struct timex' is in parts per
84 * million, but with a 16 bit binary fractional field.
86 * We want to calculate
88 * ppb = scaled_ppm * 1000 / 2^16
92 * ppb = scaled_ppm * 125 / 2^13
100 /* posix clock implementation */
102 static int ptp_clock_getres(struct posix_clock
*pc
, struct timespec
*tp
)
109 static int ptp_clock_settime(struct posix_clock
*pc
, const struct timespec
*tp
)
111 struct ptp_clock
*ptp
= container_of(pc
, struct ptp_clock
, clock
);
112 return ptp
->info
->settime(ptp
->info
, tp
);
115 static int ptp_clock_gettime(struct posix_clock
*pc
, struct timespec
*tp
)
117 struct ptp_clock
*ptp
= container_of(pc
, struct ptp_clock
, clock
);
118 return ptp
->info
->gettime(ptp
->info
, tp
);
121 static int ptp_clock_adjtime(struct posix_clock
*pc
, struct timex
*tx
)
123 struct ptp_clock
*ptp
= container_of(pc
, struct ptp_clock
, clock
);
124 struct ptp_clock_info
*ops
;
125 int err
= -EOPNOTSUPP
;
129 if (tx
->modes
& ADJ_SETOFFSET
) {
134 ts
.tv_sec
= tx
->time
.tv_sec
;
135 ts
.tv_nsec
= tx
->time
.tv_usec
;
137 if (!(tx
->modes
& ADJ_NANO
))
140 if ((unsigned long) ts
.tv_nsec
>= NSEC_PER_SEC
)
143 kt
= timespec_to_ktime(ts
);
144 delta
= ktime_to_ns(kt
);
145 err
= ops
->adjtime(ops
, delta
);
147 } else if (tx
->modes
& ADJ_FREQUENCY
) {
149 err
= ops
->adjfreq(ops
, scaled_ppm_to_ppb(tx
->freq
));
150 ptp
->dialed_frequency
= tx
->freq
;
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
,
168 static void delete_ptp_clock(struct posix_clock
*pc
)
170 struct ptp_clock
*ptp
= container_of(pc
, struct ptp_clock
, clock
);
172 mutex_destroy(&ptp
->tsevq_mux
);
174 /* Remove the clock from the bit map. */
175 mutex_lock(&ptp_clocks_mutex
);
176 clear_bit(ptp
->index
, ptp_clocks_map
);
177 mutex_unlock(&ptp_clocks_mutex
);
182 /* public interface */
184 struct ptp_clock
*ptp_clock_register(struct ptp_clock_info
*info
)
186 struct ptp_clock
*ptp
;
187 int err
= 0, index
, major
= MAJOR(ptp_devt
);
189 if (info
->n_alarm
> PTP_MAX_ALARMS
)
190 return ERR_PTR(-EINVAL
);
192 /* Find a free clock slot and reserve it. */
194 mutex_lock(&ptp_clocks_mutex
);
195 index
= find_first_zero_bit(ptp_clocks_map
, PTP_MAX_CLOCKS
);
196 if (index
< PTP_MAX_CLOCKS
)
197 set_bit(index
, ptp_clocks_map
);
201 /* Initialize a clock structure. */
203 ptp
= kzalloc(sizeof(struct ptp_clock
), GFP_KERNEL
);
207 ptp
->clock
.ops
= ptp_clock_ops
;
208 ptp
->clock
.release
= delete_ptp_clock
;
210 ptp
->devid
= MKDEV(major
, index
);
212 spin_lock_init(&ptp
->tsevq
.lock
);
213 mutex_init(&ptp
->tsevq_mux
);
214 init_waitqueue_head(&ptp
->tsev_wq
);
216 /* Create a new device in our class. */
217 ptp
->dev
= device_create(ptp_class
, NULL
, ptp
->devid
, ptp
,
218 "ptp%d", ptp
->index
);
219 if (IS_ERR(ptp
->dev
))
222 dev_set_drvdata(ptp
->dev
, ptp
);
224 err
= ptp_populate_sysfs(ptp
);
228 /* Register a new PPS source. */
230 struct pps_source_info pps
;
231 memset(&pps
, 0, sizeof(pps
));
232 snprintf(pps
.name
, PPS_MAX_NAME_LEN
, "ptp%d", index
);
233 pps
.mode
= PTP_PPS_MODE
;
234 pps
.owner
= info
->owner
;
235 ptp
->pps_source
= pps_register_source(&pps
, PTP_PPS_DEFAULTS
);
236 if (!ptp
->pps_source
) {
237 pr_err("failed to register pps source\n");
242 /* Create a posix clock. */
243 err
= posix_clock_register(&ptp
->clock
, ptp
->devid
);
245 pr_err("failed to create posix clock\n");
249 mutex_unlock(&ptp_clocks_mutex
);
254 pps_unregister_source(ptp
->pps_source
);
256 ptp_cleanup_sysfs(ptp
);
258 device_destroy(ptp_class
, ptp
->devid
);
260 mutex_destroy(&ptp
->tsevq_mux
);
263 clear_bit(index
, ptp_clocks_map
);
265 mutex_unlock(&ptp_clocks_mutex
);
268 EXPORT_SYMBOL(ptp_clock_register
);
270 int ptp_clock_unregister(struct ptp_clock
*ptp
)
273 wake_up_interruptible(&ptp
->tsev_wq
);
275 /* Release the clock's resources. */
277 pps_unregister_source(ptp
->pps_source
);
278 ptp_cleanup_sysfs(ptp
);
279 device_destroy(ptp_class
, ptp
->devid
);
281 posix_clock_unregister(&ptp
->clock
);
284 EXPORT_SYMBOL(ptp_clock_unregister
);
286 void ptp_clock_event(struct ptp_clock
*ptp
, struct ptp_clock_event
*event
)
288 struct pps_event_time evt
;
290 switch (event
->type
) {
292 case PTP_CLOCK_ALARM
:
295 case PTP_CLOCK_EXTTS
:
296 enqueue_external_timestamp(&ptp
->tsevq
, event
);
297 wake_up_interruptible(&ptp
->tsev_wq
);
302 pps_event(ptp
->pps_source
, &evt
, PTP_PPS_EVENT
, NULL
);
305 case PTP_CLOCK_PPSUSR
:
306 pps_event(ptp
->pps_source
, &event
->pps_times
,
307 PTP_PPS_EVENT
, NULL
);
311 EXPORT_SYMBOL(ptp_clock_event
);
313 int ptp_clock_index(struct ptp_clock
*ptp
)
317 EXPORT_SYMBOL(ptp_clock_index
);
319 /* module operations */
321 static void __exit
ptp_exit(void)
323 class_destroy(ptp_class
);
324 unregister_chrdev_region(ptp_devt
, PTP_MAX_CLOCKS
);
327 static int __init
ptp_init(void)
331 ptp_class
= class_create(THIS_MODULE
, "ptp");
332 if (IS_ERR(ptp_class
)) {
333 pr_err("ptp: failed to allocate class\n");
334 return PTR_ERR(ptp_class
);
337 err
= alloc_chrdev_region(&ptp_devt
, 0, PTP_MAX_CLOCKS
, "ptp");
339 pr_err("ptp: failed to allocate device region\n");
343 ptp_class
->dev_attrs
= ptp_dev_attrs
;
344 pr_info("PTP clock support registered\n");
348 class_destroy(ptp_class
);
352 subsys_initcall(ptp_init
);
353 module_exit(ptp_exit
);
355 MODULE_AUTHOR("Richard Cochran <richardcochran@gmail.com>");
356 MODULE_DESCRIPTION("PTP clocks support");
357 MODULE_LICENSE("GPL");