diff options
| -rw-r--r-- | drivers/ptp/ptp_chardev.c | 26 | ||||
| -rw-r--r-- | drivers/ptp/ptp_clock.c | 12 | ||||
| -rw-r--r-- | drivers/ptp/ptp_private.h | 3 | ||||
| -rw-r--r-- | include/uapi/linux/ptp_clock.h | 2 |
4 files changed, 41 insertions, 2 deletions
diff --git a/drivers/ptp/ptp_chardev.c b/drivers/ptp/ptp_chardev.c index abe94bb80cf6..ac2f2b5ea0b7 100644 --- a/drivers/ptp/ptp_chardev.c +++ b/drivers/ptp/ptp_chardev.c @@ -110,6 +110,12 @@ int ptp_open(struct posix_clock_context *pccontext, fmode_t fmode) queue = kzalloc(sizeof(*queue), GFP_KERNEL); if (!queue) return -EINVAL; + queue->mask = bitmap_alloc(PTP_MAX_CHANNELS, GFP_KERNEL); + if (!queue->mask) { + kfree(queue); + return -EINVAL; + } + bitmap_set(queue->mask, 0, PTP_MAX_CHANNELS); spin_lock_init(&queue->lock); list_add_tail(&queue->qlist, &ptp->tsevqs); pccontext->private_clkdata = queue; @@ -126,6 +132,7 @@ int ptp_release(struct posix_clock_context *pccontext) spin_lock_irqsave(&queue->lock, flags); list_del(&queue->qlist); spin_unlock_irqrestore(&queue->lock, flags); + bitmap_free(queue->mask); kfree(queue); } return 0; @@ -141,6 +148,7 @@ long ptp_ioctl(struct posix_clock_context *pccontext, unsigned int cmd, struct system_device_crosststamp xtstamp; struct ptp_clock_info *ops = ptp->info; struct ptp_sys_offset *sysoff = NULL; + struct timestamp_event_queue *tsevq; struct ptp_system_timestamp sts; struct ptp_clock_request req; struct ptp_clock_caps caps; @@ -150,6 +158,8 @@ long ptp_ioctl(struct posix_clock_context *pccontext, unsigned int cmd, struct timespec64 ts; int enable, err = 0; + tsevq = pccontext->private_clkdata; + switch (cmd) { case PTP_CLOCK_GETCAPS: @@ -448,6 +458,22 @@ long ptp_ioctl(struct posix_clock_context *pccontext, unsigned int cmd, mutex_unlock(&ptp->pincfg_mux); break; + case PTP_MASK_CLEAR_ALL: + bitmap_clear(tsevq->mask, 0, PTP_MAX_CHANNELS); + break; + + case PTP_MASK_EN_SINGLE: + if (copy_from_user(&i, (void __user *)arg, sizeof(i))) { + err = -EFAULT; + break; + } + if (i >= PTP_MAX_CHANNELS) { + err = -EFAULT; + break; + } + set_bit(i, tsevq->mask); + break; + default: err = -ENOTTY; break; diff --git a/drivers/ptp/ptp_clock.c b/drivers/ptp/ptp_clock.c index 74f1ce2dbccb..ed16d9787ce9 100644 --- a/drivers/ptp/ptp_clock.c +++ b/drivers/ptp/ptp_clock.c @@ -183,6 +183,7 @@ static void ptp_clock_release(struct device *dev) spin_lock_irqsave(&tsevq->lock, flags); list_del(&tsevq->qlist); spin_unlock_irqrestore(&tsevq->lock, flags); + bitmap_free(tsevq->mask); kfree(tsevq); ida_free(&ptp_clocks_map, ptp->index); kfree(ptp); @@ -243,6 +244,10 @@ struct ptp_clock *ptp_clock_register(struct ptp_clock_info *info, if (!queue) goto no_memory_queue; list_add_tail(&queue->qlist, &ptp->tsevqs); + 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); @@ -346,6 +351,8 @@ no_mem_for_vclocks: kworker_err: 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: @@ -400,9 +407,10 @@ void ptp_clock_event(struct ptp_clock *ptp, struct ptp_clock_event *event) break; case PTP_CLOCK_EXTTS: - /* Enqueue timestamp on all queues */ + /* Enqueue timestamp on selected queues */ list_for_each_entry(tsevq, &ptp->tsevqs, qlist) { - enqueue_external_timestamp(tsevq, event); + if (test_bit((unsigned int)event->index, tsevq->mask)) + enqueue_external_timestamp(tsevq, event); } wake_up_interruptible(&ptp->tsev_wq); break; diff --git a/drivers/ptp/ptp_private.h b/drivers/ptp/ptp_private.h index 9d5f3d95058e..ad4ce1b25c86 100644 --- a/drivers/ptp/ptp_private.h +++ b/drivers/ptp/ptp_private.h @@ -16,10 +16,12 @@ #include <linux/ptp_clock_kernel.h> #include <linux/time.h> #include <linux/list.h> +#include <linux/bitmap.h> #define PTP_MAX_TIMESTAMPS 128 #define PTP_BUF_TIMESTAMPS 30 #define PTP_DEFAULT_MAX_VCLOCKS 20 +#define PTP_MAX_CHANNELS 2048 struct timestamp_event_queue { struct ptp_extts_event buf[PTP_MAX_TIMESTAMPS]; @@ -27,6 +29,7 @@ struct timestamp_event_queue { int tail; spinlock_t lock; struct list_head qlist; + unsigned long *mask; }; struct ptp_clock { diff --git a/include/uapi/linux/ptp_clock.h b/include/uapi/linux/ptp_clock.h index 05cc35fc94ac..da700999cad4 100644 --- a/include/uapi/linux/ptp_clock.h +++ b/include/uapi/linux/ptp_clock.h @@ -224,6 +224,8 @@ struct ptp_pin_desc { _IOWR(PTP_CLK_MAGIC, 17, struct ptp_sys_offset_precise) #define PTP_SYS_OFFSET_EXTENDED2 \ _IOWR(PTP_CLK_MAGIC, 18, struct ptp_sys_offset_extended) +#define PTP_MASK_CLEAR_ALL _IO(PTP_CLK_MAGIC, 19) +#define PTP_MASK_EN_SINGLE _IOW(PTP_CLK_MAGIC, 20, unsigned int) struct ptp_extts_event { struct ptp_clock_time t; /* Time event occured. */ |