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

[Marcelo Tosatti]

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

Signed-off-by: Marcelo Tosatti <mtosatti@xxxxxxxxxx>

---
 drivers/ptp/ptp_chardev.c        |    6 ++
 include/linux/ptp_clock_kernel.h |    5 ++
 include/linux/timekeeping_ptp.h  |   10 ++++
 kernel/time/timekeeping.c        |   79 +++++++++++++++++++++++++++++++++++++++
 4 files changed, 100 insertions(+)

Index: kvm-ptpdriver/drivers/ptp/ptp_chardev.c
===================================================================
--- kvm-ptpdriver.orig/drivers/ptp/ptp_chardev.c	2017-01-19 15:34:52.181338789 -0200
+++ kvm-ptpdriver/drivers/ptp/ptp_chardev.c	2017-01-19 15:35:31.705421911 -0200
@@ -23,6 +23,7 @@
 #include <linux/sched.h>
 #include <linux/slab.h>
 #include <linux/timekeeping.h>
+#include <linux/timekeeping_ptp.h>
 
 #include "ptp_private.h"
 
@@ -219,6 +220,11 @@
 			err = -EINVAL;
 			break;
 		}
+
+		if (ptp->info->emulate_ptp_sys_offset_mean) {
+			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/include/linux/ptp_clock_kernel.h
===================================================================
--- kvm-ptpdriver.orig/include/linux/ptp_clock_kernel.h	2017-01-19 15:34:52.181338789 -0200
+++ kvm-ptpdriver/include/linux/ptp_clock_kernel.h	2017-01-19 15:35:31.706421913 -0200
@@ -99,6 +99,10 @@
  *            parameter func: the desired function to use.
  *            parameter chan: the function channel index to use.
  *
+ * @emulate_ptp_sys_offset_mean: Emulate PTP_SYS_OFFSET ioctl
+ *            using arithmetic mean of series of PTP_SYS_OFFSET_PRECISE
+ *            ioctl.
+ *
  * Drivers should embed their ptp_clock_info within a private
  * structure, obtaining a reference to it using container_of().
  *
@@ -115,6 +119,7 @@
 	int n_pins;
 	int pps;
 	struct ptp_pin_desc *pin_config;
+	bool emulate_ptp_sys_offset_mean;
 	int (*adjfine)(struct ptp_clock_info *ptp, long scaled_ppm);
 	int (*adjfreq)(struct ptp_clock_info *ptp, s32 delta);
 	int (*adjtime)(struct ptp_clock_info *ptp, s64 delta);
Index: kvm-ptpdriver/include/linux/timekeeping_ptp.h
===================================================================
--- /dev/null	1970-01-01 00:00:00.000000000 +0000
+++ kvm-ptpdriver/include/linux/timekeeping_ptp.h	2017-01-19 15:35:31.706421913 -0200
@@ -0,0 +1,10 @@
+#ifndef _LINUX_TIMEKEEPING_PTP_H
+#define _LINUX_TIMEKEEPING_PTP_H
+
+#include <uapi/linux/ptp_clock.h>
+#include <linux/ptp_clock_kernel.h>
+
+extern int emulate_ptp_sys_offset(struct ptp_clock_info *info,
+			struct ptp_sys_offset *sysoff,
+			unsigned long arg);
+#endif
Index: kvm-ptpdriver/kernel/time/timekeeping.c
===================================================================
--- kvm-ptpdriver.orig/kernel/time/timekeeping.c	2017-01-19 15:34:52.181338789 -0200
+++ kvm-ptpdriver/kernel/time/timekeeping.c	2017-01-20 09:29:13.978434149 -0200
@@ -23,6 +23,10 @@
 #include <linux/stop_machine.h>
 #include <linux/pvclock_gtod.h>
 #include <linux/compiler.h>
+#include <linux/timekeeping_ptp.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
 
 #include "tick-internal.h"
 #include "ntp_internal.h"
@@ -1168,6 +1172,81 @@
 }
 EXPORT_SYMBOL_GPL(get_device_system_crosststamp);
 
+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;
+}
+
+int emulate_ptp_sys_offset(struct ptp_clock_info *info,
+				  struct ptp_sys_offset *sysoff,
+				  unsigned long arg)
+{
+	unsigned long flags;
+	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;
+
+	/* stop update_wall_time, settimeofday */
+	raw_spin_lock_irqsave(&timekeeper_lock, flags);
+
+	xtstamp = xtstamps;
+	for (i = 0; i < n_samples; i++) {
+		err = info->getcrosststamp(info, xtstamp);
+		if (err) {
+			kfree(xtstamps);
+			raw_spin_unlock_irqrestore(&timekeeper_lock, flags);
+			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);
+	raw_spin_unlock_irqrestore(&timekeeper_lock, flags);
+	kfree(xtstamps);
+	if (copy_to_user((void __user *)arg, sysoff, sizeof(*sysoff)))
+		return -EFAULT;
+
+	return 0;
+}
+EXPORT_SYMBOL(emulate_ptp_sys_offset);
+
 /**
  * do_gettimeofday - Returns the time of day in a timeval
  * @tv:		pointer to the timeval to be set