1 files changed, 56 insertions, 5 deletions

diff --git a/drivers/ptp/ptp_chardev.c b/drivers/ptp/ptp_chardev.c
index e7f301da2..34a0c60 100644
--- a/drivers/ptp/ptp_chardev.c
+++ b/drivers/ptp/ptp_chardev.c
@@ -21,6 +21,7 @@
 #include <linux/posix-clock.h>
 #include <linux/poll.h>
 #include <linux/sched.h>
+#include <linux/slab.h>
 
 #include "ptp_private.h"
 
@@ -33,9 +34,13 @@ long ptp_ioctl(struct posix_clock *pc, unsigned int cmd, unsigned long arg)
 {
 	struct ptp_clock_caps caps;
 	struct ptp_clock_request req;
+	struct ptp_sys_offset *sysoff = NULL;
 	struct ptp_clock *ptp = container_of(pc, struct ptp_clock, clock);
 	struct ptp_clock_info *ops = ptp->info;
+	struct ptp_clock_time *pct;
+	struct timespec ts;
 	int enable, err = 0;
+	unsigned int i;
 
 	switch (cmd) {
 
@@ -88,10 +93,45 @@ long ptp_ioctl(struct posix_clock *pc, unsigned int cmd, unsigned long arg)
 		err = ops->enable(ops, &req, enable);
 		break;
 
+	case PTP_SYS_OFFSET:
+		sysoff = kmalloc(sizeof(*sysoff), GFP_KERNEL);
+		if (!sysoff) {
+			err = -ENOMEM;
+			break;
+		}
+		if (copy_from_user(sysoff, (void __user *)arg,
+				   sizeof(*sysoff))) {
+			err = -EFAULT;
+			break;
+		}
+		if (sysoff->n_samples > PTP_MAX_SAMPLES) {
+			err = -EINVAL;
+			break;
+		}
+		pct = &sysoff->ts[0];
+		for (i = 0; i < sysoff->n_samples; i++) {
+			getnstimeofday(&ts);
+			pct->sec = ts.tv_sec;
+			pct->nsec = ts.tv_nsec;
+			pct++;
+			ptp->info->gettime(ptp->info, &ts);
+			pct->sec = ts.tv_sec;
+			pct->nsec = ts.tv_nsec;
+			pct++;
+		}
+		getnstimeofday(&ts);
+		pct->sec = ts.tv_sec;
+		pct->nsec = ts.tv_nsec;
+		if (copy_to_user((void __user *)arg, sysoff, sizeof(*sysoff)))
+			err = -EFAULT;
+		break;
+
 	default:
 		err = -ENOTTY;
 		break;
 	}
+
+	kfree(sysoff);
 	return err;
 }
 
@@ -104,20 +144,23 @@ unsigned int ptp_poll(struct posix_clock *pc, struct file *fp, poll_table *wait)
 	return queue_cnt(&ptp->tsevq) ? POLLIN : 0;
 }
 
+#define EXTTS_BUFSIZE (PTP_BUF_TIMESTAMPS * sizeof(struct ptp_extts_event))
+
 ssize_t ptp_read(struct posix_clock *pc,
 		 uint rdflags, char __user *buf, size_t cnt)
 {
 	struct ptp_clock *ptp = container_of(pc, struct ptp_clock, clock);
 	struct timestamp_event_queue *queue = &ptp->tsevq;
-	struct ptp_extts_event event[PTP_BUF_TIMESTAMPS];
+	struct ptp_extts_event *event;
 	unsigned long flags;
 	size_t qcnt, i;
+	int result;
 
 	if (cnt % sizeof(struct ptp_extts_event) != 0)
 		return -EINVAL;
 
-	if (cnt > sizeof(event))
-		cnt = sizeof(event);
+	if (cnt > EXTTS_BUFSIZE)
+		cnt = EXTTS_BUFSIZE;
 
 	cnt = cnt / sizeof(struct ptp_extts_event);
 
@@ -135,6 +178,12 @@ ssize_t ptp_read(struct posix_clock *pc,
 		return -ENODEV;
 	}
 
+	event = kmalloc(EXTTS_BUFSIZE, GFP_KERNEL);
+	if (!event) {
+		mutex_unlock(&ptp->tsevq_mux);
+		return -ENOMEM;
+	}
+
 	spin_lock_irqsave(&queue->lock, flags);
 
 	qcnt = queue_cnt(queue);
@@ -153,8 +202,10 @@ ssize_t ptp_read(struct posix_clock *pc,
 
 	mutex_unlock(&ptp->tsevq_mux);
 
+	result = cnt;
 	if (copy_to_user(buf, event, cnt))
-		return -EFAULT;
+		result = -EFAULT;
 
-	return cnt;
+	kfree(event);
+	return result;
 }