[patch 4/5] PTP: add PTP_SYS_OFFSET emulation via cross timestamps infrastructure

[Marcelo Tosatti <mtosatti@redhat.com>]

[-- Attachment #1: ptp-add-sysoffset-callback --]
[-- Type: text/plain, Size: 3825 bytes --]

Emulate PTP_SYS_OFFSET by using an arithmetic mean of the
realtime samples from ->getcrosststamp callback.

Note: if the realtime clock changes during cross timestamp sampling,
then the mean will return a value that does not exist. Given the
nature of PTP_SYS_OFFSET users (time synchronization), 
that should not be a problem.

Signed-off-by: Marcelo Tosatti <mtosatti@redhat.com>

---
 drivers/ptp/ptp_chardev.c |   73 ++++++++++++++++++++++++++++++++++++++++++++++
 drivers/ptp/ptp_clock.c   |   10 ++++++
 2 files changed, 83 insertions(+)

v2: drop timekeeper spinlock, move back to drivers/ptp/ptp_chardev.c (Paolo)
    ptp_clock_gettime: support drivers with crosstimestamp but not
    gettime64 callbacks

Index: kvm-ptpdriver/drivers/ptp/ptp_chardev.c
===================================================================
--- kvm-ptpdriver.orig/drivers/ptp/ptp_chardev.c	2017-01-20 12:48:14.201126341 -0200
+++ kvm-ptpdriver/drivers/ptp/ptp_chardev.c	2017-01-20 12:48:45.270207972 -0200
@@ -116,6 +116,74 @@
 	return 0;
 }
 
+static void mean_sys_realtime(struct system_device_crosststamp *xtstamp,
+			      struct system_device_crosststamp *n_xtstamp,
+			      struct ptp_clock_time *pct)
+{
+	ktime_t sys_realtime, n_sys_realtime;
+	struct timespec ts;
+	u64 ns;
+
+	/* estimate realtime with arithmetic mean of two crosststamp samples */
+	sys_realtime = xtstamp->sys_realtime;
+	n_sys_realtime = n_xtstamp->sys_realtime;
+	ns = ktime_divns(ktime_add(sys_realtime, n_sys_realtime), 2);
+	ts = ktime_to_timespec(ns_to_ktime(ns));
+	pct->sec = ts.tv_sec;
+	pct->nsec = ts.tv_nsec;
+}
+
+static int emulate_ptp_sys_offset(struct ptp_clock_info *info,
+				  struct ptp_sys_offset *sysoff,
+				  unsigned long arg)
+{
+	int i, err, len;
+	int n_samples;
+	struct system_device_crosststamp *xtstamps, *xtstamp, *n_xtstamp;
+	struct ptp_clock_time *pct;
+
+	n_samples = sysoff->n_samples + 2;
+
+	len = sizeof(struct system_device_crosststamp);
+	xtstamps = kzalloc(len * n_samples, GFP_KERNEL);
+	if (xtstamps == NULL)
+		return -ENOMEM;
+
+	xtstamp = xtstamps;
+	for (i = 0; i < n_samples; i++) {
+		err = info->getcrosststamp(info, xtstamp);
+		if (err) {
+			kfree(xtstamps);
+			return err;
+		}
+		xtstamp++;
+	}
+
+	pct = &sysoff->ts[0];
+	xtstamp = n_xtstamp = xtstamps;
+	n_xtstamp++;
+	for (i = 0; i < sysoff->n_samples; i++) {
+		struct timespec ts;
+
+		mean_sys_realtime(xtstamp, n_xtstamp, pct);
+		pct++;
+
+		ts = ktime_to_timespec(n_xtstamp->device);
+		pct->sec = ts.tv_sec;
+		pct->nsec = ts.tv_nsec;
+		pct++;
+		xtstamp++;
+		n_xtstamp++;
+	}
+
+	mean_sys_realtime(xtstamp, n_xtstamp, pct);
+	kfree(xtstamps);
+	if (copy_to_user((void __user *)arg, sysoff, sizeof(*sysoff)))
+		return -EFAULT;
+
+	return 0;
+}
+
 long ptp_ioctl(struct posix_clock *pc, unsigned int cmd, unsigned long arg)
 {
 	struct ptp_clock_caps caps;
@@ -219,6 +287,11 @@
 			err = -EINVAL;
 			break;
 		}
+
+		if (!ptp->info->gettime64 && ptp->info->getcrosststamp) {
+			err = emulate_ptp_sys_offset(ptp->info, sysoff, arg);
+			break;
+		}
 		pct = &sysoff->ts[0];
 		for (i = 0; i < sysoff->n_samples; i++) {
 			getnstimeofday64(&ts);
Index: kvm-ptpdriver/drivers/ptp/ptp_clock.c
===================================================================
--- kvm-ptpdriver.orig/drivers/ptp/ptp_clock.c	2017-01-20 12:48:14.201126341 -0200
+++ kvm-ptpdriver/drivers/ptp/ptp_clock.c	2017-01-20 12:48:45.271207975 -0200
@@ -118,6 +118,16 @@
 	struct timespec64 ts;
 	int err;
 
+	if (!ptp->info->gettime64 && ptp->info->getcrosststamp) {
+		struct system_device_crosststamp xtstamp;
+
+		err = ptp->info->getcrosststamp(ptp->info, &xtstamp);
+		if (err)
+			return err;
+		*tp = ktime_to_timespec(xtstamp.device);
+		return err;
+	}
+
 	err = ptp->info->gettime64(ptp->info, &ts);
 	if (!err)
 		*tp = timespec64_to_timespec(ts);