diff options
Diffstat (limited to 'drivers/ptp/ptp_clock.c')
| -rw-r--r-- | drivers/ptp/ptp_clock.c | 49 |
1 files changed, 44 insertions, 5 deletions
diff --git a/drivers/ptp/ptp_clock.c b/drivers/ptp/ptp_clock.c index 9a50bfb564..15b804ba48 100644 --- a/drivers/ptp/ptp_clock.c +++ b/drivers/ptp/ptp_clock.c @@ -15,6 +15,7 @@ #include <linux/slab.h> #include <linux/syscalls.h> #include <linux/uaccess.h> +#include <linux/debugfs.h> #include <uapi/linux/sched/types.h> #include "ptp_private.h" @@ -163,6 +164,7 @@ static struct posix_clock_operations ptp_clock_ops = { .clock_settime = ptp_clock_settime, .ioctl = ptp_ioctl, .open = ptp_open, + .release = ptp_release, .poll = ptp_poll, .read = ptp_read, }; @@ -170,12 +172,22 @@ static struct posix_clock_operations ptp_clock_ops = { static void ptp_clock_release(struct device *dev) { struct ptp_clock *ptp = container_of(dev, struct ptp_clock, dev); + struct timestamp_event_queue *tsevq; + unsigned long flags; ptp_cleanup_pin_groups(ptp); kfree(ptp->vclock_index); - mutex_destroy(&ptp->tsevq_mux); mutex_destroy(&ptp->pincfg_mux); mutex_destroy(&ptp->n_vclocks_mux); + /* Delete first entry */ + spin_lock_irqsave(&ptp->tsevqs_lock, flags); + tsevq = list_first_entry(&ptp->tsevqs, struct timestamp_event_queue, + qlist); + list_del(&tsevq->qlist); + spin_unlock_irqrestore(&ptp->tsevqs_lock, flags); + bitmap_free(tsevq->mask); + kfree(tsevq); + debugfs_remove(ptp->debugfs_root); ida_free(&ptp_clocks_map, ptp->index); kfree(ptp); } @@ -207,7 +219,9 @@ struct ptp_clock *ptp_clock_register(struct ptp_clock_info *info, struct device *parent) { struct ptp_clock *ptp; + struct timestamp_event_queue *queue = NULL; int err = 0, index, major = MAJOR(ptp_devt); + char debugfsname[16]; size_t size; if (info->n_alarm > PTP_MAX_ALARMS) @@ -229,8 +243,17 @@ struct ptp_clock *ptp_clock_register(struct ptp_clock_info *info, ptp->info = info; ptp->devid = MKDEV(major, index); ptp->index = index; - spin_lock_init(&ptp->tsevq.lock); - mutex_init(&ptp->tsevq_mux); + INIT_LIST_HEAD(&ptp->tsevqs); + queue = kzalloc(sizeof(*queue), GFP_KERNEL); + if (!queue) + goto no_memory_queue; + list_add_tail(&queue->qlist, &ptp->tsevqs); + spin_lock_init(&ptp->tsevqs_lock); + queue->mask = bitmap_alloc(PTP_MAX_CHANNELS, GFP_KERNEL); + if (!queue->mask) + goto no_memory_bitmap; + bitmap_set(queue->mask, 0, PTP_MAX_CHANNELS); + spin_lock_init(&queue->lock); mutex_init(&ptp->pincfg_mux); mutex_init(&ptp->n_vclocks_mux); init_waitqueue_head(&ptp->tsev_wq); @@ -321,6 +344,10 @@ struct ptp_clock *ptp_clock_register(struct ptp_clock_info *info, return ERR_PTR(err); } + /* Debugfs initialization */ + snprintf(debugfsname, sizeof(debugfsname), "ptp%d", ptp->index); + ptp->debugfs_root = debugfs_create_dir(debugfsname, NULL); + return ptp; no_pps: @@ -331,9 +358,13 @@ no_mem_for_vclocks: if (ptp->kworker) kthread_destroy_worker(ptp->kworker); kworker_err: - mutex_destroy(&ptp->tsevq_mux); mutex_destroy(&ptp->pincfg_mux); mutex_destroy(&ptp->n_vclocks_mux); + bitmap_free(queue->mask); +no_memory_bitmap: + list_del(&queue->qlist); + kfree(queue); +no_memory_queue: ida_free(&ptp_clocks_map, index); no_slot: kfree(ptp); @@ -376,7 +407,9 @@ EXPORT_SYMBOL(ptp_clock_unregister); void ptp_clock_event(struct ptp_clock *ptp, struct ptp_clock_event *event) { + struct timestamp_event_queue *tsevq; struct pps_event_time evt; + unsigned long flags; switch (event->type) { @@ -384,7 +417,13 @@ void ptp_clock_event(struct ptp_clock *ptp, struct ptp_clock_event *event) break; case PTP_CLOCK_EXTTS: - enqueue_external_timestamp(&ptp->tsevq, event); + /* Enqueue timestamp on selected queues */ + spin_lock_irqsave(&ptp->tsevqs_lock, flags); + list_for_each_entry(tsevq, &ptp->tsevqs, qlist) { + if (test_bit((unsigned int)event->index, tsevq->mask)) + enqueue_external_timestamp(tsevq, event); + } + spin_unlock_irqrestore(&ptp->tsevqs_lock, flags); wake_up_interruptible(&ptp->tsev_wq); break; |