Message-ID: <20160916141130.GG1252@kroah.com>
Date:   Fri, 16 Sep 2016 16:11:30 +0200
From:   Greg KH <gregkh@...uxfoundation.org>
To:     Arnd Bergmann <arnd@...db.de>, linux-kernel@...r.kernel.org
Cc:     Johan Hovold <johan@...oldconsulting.com>,
        Rui Miguel Silva <rmfrfs@...il.com>,
        Laurent Pinchart <laurent.pinchart@...asonboard.com>,
        Sandeep Patil <sspatil@...gle.com>,
        Matt Porter <mporter@...nel.crashing.org>,
        John Stultz <john.stultz@...aro.org>,
        Rob Herring <robh@...nel.org>,
        Viresh Kumar <viresh.kumar@...aro.org>,
        Alex Elder <elder@...aro.org>, David Lin <dtwlin@...gle.com>,
        Bryan O'Donoghue <pure.logic@...us-software.ie>,
        Vaibhav Agarwal <vaibhav.agarwal@...aro.org>,
        Mark Greer <mgreer@...malcreek.com>
Subject: [patch 19/32] greybus: timesync driver

This code implements the Greybus timesync logic.  It has the ability to
sync a time signal across all Unipro devices allowing them to stay in
sync properly.

Signed-off-by: Greg Kroah-Hartman <gregkh@...uxfoundation.org>
---
 drivers/greybus/timesync.c          | 1357 ++++++++++++++++++++++++++++++++++++
 drivers/greybus/timesync.h          |   45 +
 drivers/greybus/timesync_platform.c |   77 ++
 3 files changed, 1479 insertions(+)

--- /dev/null
+++ b/drivers/greybus/timesync.c
@@ -0,0 +1,1357 @@
+/*
+ * TimeSync API driver.
+ *
+ * Copyright 2016 Google Inc.
+ * Copyright 2016 Linaro Ltd.
+ *
+ * Released under the GPLv2 only.
+ */
+#include <linux/debugfs.h>
+#include <linux/hrtimer.h>
+#include "greybus.h"
+#include "timesync.h"
+#include "greybus_trace.h"
+
+/*
+ * Minimum inter-strobe value of one millisecond is chosen because it
+ * just-about fits the common definition of a jiffy.
+ *
+ * Maximum value OTOH is constrained by the number of bits the SVC can fit
+ * into a 16 bit up-counter. The SVC configures the timer in microseconds
+ * so the maximum allowable value is 65535 microseconds. We clip that value
+ * to 10000 microseconds for the sake of using nice round base 10 numbers
+ * and since right-now there's no imaginable use-case requiring anything
+ * other than a one millisecond inter-strobe time, let alone something
+ * higher than ten milliseconds.
+ */
+#define GB_TIMESYNC_STROBE_DELAY_US		1000
+#define GB_TIMESYNC_DEFAULT_OFFSET_US		1000
+
+/* Work queue timers long, short and SVC strobe timeout */
+#define GB_TIMESYNC_DELAYED_WORK_LONG		msecs_to_jiffies(10)
+#define GB_TIMESYNC_DELAYED_WORK_SHORT		msecs_to_jiffies(1)
+#define GB_TIMESYNC_MAX_WAIT_SVC		msecs_to_jiffies(5000)
+#define GB_TIMESYNC_KTIME_UPDATE		msecs_to_jiffies(1000)
+#define GB_TIMESYNC_MAX_KTIME_CONVERSION	15
+
+/* Maximum number of times we'll retry a failed synchronous sync */
+#define GB_TIMESYNC_MAX_RETRIES			5
+
+/* Reported nanoseconds/femtoseconds per clock */
+static u64 gb_timesync_ns_per_clock;
+static u64 gb_timesync_fs_per_clock;
+
+/* Maximum difference we will accept converting FrameTime to ktime */
+static u32 gb_timesync_max_ktime_diff;
+
+/* Reported clock rate */
+static unsigned long gb_timesync_clock_rate;
+
+/* Workqueue */
+static void gb_timesync_worker(struct work_struct *work);
+
+/* List of SVCs with one FrameTime per SVC */
+static LIST_HEAD(gb_timesync_svc_list);
+
+/* Synchronize parallel contexts accessing a valid timesync_svc pointer */
+static DEFINE_MUTEX(gb_timesync_svc_list_mutex);
+
+/* Structure to convert from FrameTime to timespec/ktime */
+struct gb_timesync_frame_time_data {
+	u64 frame_time;
+	struct timespec ts;
+};
+
+struct gb_timesync_svc {
+	struct list_head list;
+	struct list_head interface_list;
+	struct gb_svc *svc;
+	struct gb_timesync_host_device *timesync_hd;
+
+	spinlock_t spinlock;	/* Per SVC spinlock to sync with ISR */
+	struct mutex mutex;	/* Per SVC mutex for regular synchronization */
+
+	struct dentry *frame_time_dentry;
+	struct dentry *frame_ktime_dentry;
+	struct workqueue_struct *work_queue;
+	wait_queue_head_t wait_queue;
+	struct delayed_work delayed_work;
+	struct timer_list ktime_timer;
+
+	/* The current local FrameTime */
+	u64 frame_time_offset;
+	struct gb_timesync_frame_time_data strobe_data[GB_TIMESYNC_MAX_STROBES];
+	struct gb_timesync_frame_time_data ktime_data;
+
+	/* The SVC FrameTime and relative AP FrameTime @ last TIMESYNC_PING */
+	u64 svc_ping_frame_time;
+	u64 ap_ping_frame_time;
+
+	/* Transitory settings */
+	u32 strobe_mask;
+	bool offset_down;
+	bool print_ping;
+	bool capture_ping;
+	int strobe;
+
+	/* Current state */
+	int state;
+};
+
+struct gb_timesync_host_device {
+	struct list_head list;
+	struct gb_host_device *hd;
+	u64 ping_frame_time;
+};
+
+struct gb_timesync_interface {
+	struct list_head list;
+	struct gb_interface *interface;
+	u64 ping_frame_time;
+};
+
+enum gb_timesync_state {
+	GB_TIMESYNC_STATE_INVALID		= 0,
+	GB_TIMESYNC_STATE_INACTIVE		= 1,
+	GB_TIMESYNC_STATE_INIT			= 2,
+	GB_TIMESYNC_STATE_WAIT_SVC		= 3,
+	GB_TIMESYNC_STATE_AUTHORITATIVE		= 4,
+	GB_TIMESYNC_STATE_PING			= 5,
+	GB_TIMESYNC_STATE_ACTIVE		= 6,
+};
+
+static void gb_timesync_ktime_timer_fn(unsigned long data);
+
+static u64 gb_timesync_adjust_count(struct gb_timesync_svc *timesync_svc,
+				    u64 counts)
+{
+	if (timesync_svc->offset_down)
+		return counts - timesync_svc->frame_time_offset;
+	else
+		return counts + timesync_svc->frame_time_offset;
+}
+
+/*
+ * This function provides the authoritative FrameTime to a calling function. It
+ * is designed to be lockless and should remain that way the caller is assumed
+ * to be state-aware.
+ */
+static u64 __gb_timesync_get_frame_time(struct gb_timesync_svc *timesync_svc)
+{
+	u64 clocks = gb_timesync_platform_get_counter();
+
+	return gb_timesync_adjust_count(timesync_svc, clocks);
+}
+
+static void gb_timesync_schedule_svc_timeout(struct gb_timesync_svc
+					     *timesync_svc)
+{
+	queue_delayed_work(timesync_svc->work_queue,
+			   &timesync_svc->delayed_work,
+			   GB_TIMESYNC_MAX_WAIT_SVC);
+}
+
+static void gb_timesync_set_state(struct gb_timesync_svc *timesync_svc,
+				  int state)
+{
+	switch (state) {
+	case GB_TIMESYNC_STATE_INVALID:
+		timesync_svc->state = state;
+		wake_up(&timesync_svc->wait_queue);
+		break;
+	case GB_TIMESYNC_STATE_INACTIVE:
+		timesync_svc->state = state;
+		wake_up(&timesync_svc->wait_queue);
+		break;
+	case GB_TIMESYNC_STATE_INIT:
+		if (timesync_svc->state != GB_TIMESYNC_STATE_INVALID) {
+			timesync_svc->strobe = 0;
+			timesync_svc->frame_time_offset = 0;
+			timesync_svc->state = state;
+			cancel_delayed_work(&timesync_svc->delayed_work);
+			queue_delayed_work(timesync_svc->work_queue,
+					   &timesync_svc->delayed_work,
+					   GB_TIMESYNC_DELAYED_WORK_LONG);
+		}
+		break;
+	case GB_TIMESYNC_STATE_WAIT_SVC:
+		if (timesync_svc->state == GB_TIMESYNC_STATE_INIT)
+			timesync_svc->state = state;
+		break;
+	case GB_TIMESYNC_STATE_AUTHORITATIVE:
+		if (timesync_svc->state == GB_TIMESYNC_STATE_WAIT_SVC) {
+			timesync_svc->state = state;
+			cancel_delayed_work(&timesync_svc->delayed_work);
+			queue_delayed_work(timesync_svc->work_queue,
+					   &timesync_svc->delayed_work, 0);
+		}
+		break;
+	case GB_TIMESYNC_STATE_PING:
+		if (timesync_svc->state == GB_TIMESYNC_STATE_ACTIVE) {
+			timesync_svc->state = state;
+			queue_delayed_work(timesync_svc->work_queue,
+					   &timesync_svc->delayed_work,
+					   GB_TIMESYNC_DELAYED_WORK_SHORT);
+		}
+		break;
+	case GB_TIMESYNC_STATE_ACTIVE:
+		if (timesync_svc->state == GB_TIMESYNC_STATE_AUTHORITATIVE ||
+		    timesync_svc->state == GB_TIMESYNC_STATE_PING) {
+			timesync_svc->state = state;
+			wake_up(&timesync_svc->wait_queue);
+		}
+		break;
+	}
+
+	if (WARN_ON(timesync_svc->state != state)) {
+		pr_err("Invalid state transition %d=>%d\n",
+		       timesync_svc->state, state);
+	}
+}
+
+static void gb_timesync_set_state_atomic(struct gb_timesync_svc *timesync_svc,
+					 int state)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&timesync_svc->spinlock, flags);
+	gb_timesync_set_state(timesync_svc, state);
+	spin_unlock_irqrestore(&timesync_svc->spinlock, flags);
+}
+
+static u64 gb_timesync_diff(u64 x, u64 y)
+{
+	if (x > y)
+		return x - y;
+	else
+		return y - x;
+}
+
+static void gb_timesync_adjust_to_svc(struct gb_timesync_svc *svc,
+				      u64 svc_frame_time, u64 ap_frame_time)
+{
+	if (svc_frame_time > ap_frame_time) {
+		svc->frame_time_offset = svc_frame_time - ap_frame_time;
+		svc->offset_down = false;
+	} else {
+		svc->frame_time_offset = ap_frame_time - svc_frame_time;
+		svc->offset_down = true;
+	}
+}
+
+/*
+ * Associate a FrameTime with a ktime timestamp represented as struct timespec
+ * Requires the calling context to hold timesync_svc->mutex
+ */
+static void gb_timesync_store_ktime(struct gb_timesync_svc *timesync_svc,
+				    struct timespec ts, u64 frame_time)
+{
+	timesync_svc->ktime_data.ts = ts;
+	timesync_svc->ktime_data.frame_time = frame_time;
+}
+
+/*
+ * Find the two pulses that best-match our expected inter-strobe gap and
+ * then calculate the difference between the SVC time at the second pulse
+ * to the local time at the second pulse.
+ */
+static void gb_timesync_collate_frame_time(struct gb_timesync_svc *timesync_svc,
+					   u64 *frame_time)
+{
+	int i = 0;
+	u64 delta, ap_frame_time;
+	u64 strobe_delay_ns = GB_TIMESYNC_STROBE_DELAY_US * NSEC_PER_USEC;
+	u64 least = 0;
+
+	for (i = 1; i < GB_TIMESYNC_MAX_STROBES; i++) {
+		delta = timesync_svc->strobe_data[i].frame_time -
+			timesync_svc->strobe_data[i - 1].frame_time;
+		delta *= gb_timesync_ns_per_clock;
+		delta = gb_timesync_diff(delta, strobe_delay_ns);
+
+		if (!least || delta < least) {
+			least = delta;
+			gb_timesync_adjust_to_svc(timesync_svc, frame_time[i],
+						  timesync_svc->strobe_data[i].frame_time);
+
+			ap_frame_time = timesync_svc->strobe_data[i].frame_time;
+			ap_frame_time = gb_timesync_adjust_count(timesync_svc,
+								 ap_frame_time);
+			gb_timesync_store_ktime(timesync_svc,
+						timesync_svc->strobe_data[i].ts,
+						ap_frame_time);
+
+			pr_debug("adjust %s local %llu svc %llu delta %llu\n",
+				 timesync_svc->offset_down ? "down" : "up",
+				 timesync_svc->strobe_data[i].frame_time,
+				 frame_time[i], delta);
+		}
+	}
+}
+
+static void gb_timesync_teardown(struct gb_timesync_svc *timesync_svc)
+{
+	struct gb_timesync_interface *timesync_interface;
+	struct gb_svc *svc = timesync_svc->svc;
+	struct gb_interface *interface;
+	struct gb_host_device *hd;
+	int ret;
+
+	list_for_each_entry(timesync_interface,
+			    &timesync_svc->interface_list, list) {
+		interface = timesync_interface->interface;
+		ret = gb_interface_timesync_disable(interface);
+		if (ret) {
+			dev_err(&interface->dev,
+				"interface timesync_disable %d\n", ret);
+		}
+	}
+
+	hd = timesync_svc->timesync_hd->hd;
+	ret = hd->driver->timesync_disable(hd);
+	if (ret < 0) {
+		dev_err(&hd->dev, "host timesync_disable %d\n",
+			ret);
+	}
+
+	gb_svc_timesync_wake_pins_release(svc);
+	gb_svc_timesync_disable(svc);
+	gb_timesync_platform_unlock_bus();
+
+	gb_timesync_set_state_atomic(timesync_svc, GB_TIMESYNC_STATE_INACTIVE);
+}
+
+static void gb_timesync_platform_lock_bus_fail(struct gb_timesync_svc
+						*timesync_svc, int ret)
+{
+	if (ret == -EAGAIN) {
+		gb_timesync_set_state(timesync_svc, timesync_svc->state);
+	} else {
+		pr_err("Failed to lock timesync bus %d\n", ret);
+		gb_timesync_set_state(timesync_svc, GB_TIMESYNC_STATE_INACTIVE);
+	}
+}
+
+static void gb_timesync_enable(struct gb_timesync_svc *timesync_svc)
+{
+	struct gb_svc *svc = timesync_svc->svc;
+	struct gb_host_device *hd;
+	struct gb_timesync_interface *timesync_interface;
+	struct gb_interface *interface;
+	u64 init_frame_time;
+	unsigned long clock_rate = gb_timesync_clock_rate;
+	int ret;
+
+	/*
+	 * Get access to the wake pins in the AP and SVC
+	 * Release these pins either in gb_timesync_teardown() or in
+	 * gb_timesync_authoritative()
+	 */
+	ret = gb_timesync_platform_lock_bus(timesync_svc);
+	if (ret < 0) {
+		gb_timesync_platform_lock_bus_fail(timesync_svc, ret);
+		return;
+	}
+	ret = gb_svc_timesync_wake_pins_acquire(svc, timesync_svc->strobe_mask);
+	if (ret) {
+		dev_err(&svc->dev,
+			"gb_svc_timesync_wake_pins_acquire %d\n", ret);
+		gb_timesync_teardown(timesync_svc);
+		return;
+	}
+
+	/* Choose an initial time in the future */
+	init_frame_time = __gb_timesync_get_frame_time(timesync_svc) + 100000UL;
+
+	/* Send enable command to all relevant participants */
+	list_for_each_entry(timesync_interface, &timesync_svc->interface_list,
+			    list) {
+		interface = timesync_interface->interface;
+		ret = gb_interface_timesync_enable(interface,
+						   GB_TIMESYNC_MAX_STROBES,
+						   init_frame_time,
+						   GB_TIMESYNC_STROBE_DELAY_US,
+						   clock_rate);
+		if (ret) {
+			dev_err(&interface->dev,
+				"interface timesync_enable %d\n", ret);
+		}
+	}
+
+	hd = timesync_svc->timesync_hd->hd;
+	ret = hd->driver->timesync_enable(hd, GB_TIMESYNC_MAX_STROBES,
+					  init_frame_time,
+					  GB_TIMESYNC_STROBE_DELAY_US,
+					  clock_rate);
+	if (ret < 0) {
+		dev_err(&hd->dev, "host timesync_enable %d\n",
+			ret);
+	}
+
+	gb_timesync_set_state_atomic(timesync_svc, GB_TIMESYNC_STATE_WAIT_SVC);
+	ret = gb_svc_timesync_enable(svc, GB_TIMESYNC_MAX_STROBES,
+				     init_frame_time,
+				     GB_TIMESYNC_STROBE_DELAY_US,
+				     clock_rate);
+	if (ret) {
+		dev_err(&svc->dev,
+			"gb_svc_timesync_enable %d\n", ret);
+		gb_timesync_teardown(timesync_svc);
+		return;
+	}
+
+	/* Schedule a timeout waiting for SVC to complete strobing */
+	gb_timesync_schedule_svc_timeout(timesync_svc);
+}
+
+static void gb_timesync_authoritative(struct gb_timesync_svc *timesync_svc)
+{
+	struct gb_svc *svc = timesync_svc->svc;
+	struct gb_host_device *hd;
+	struct gb_timesync_interface *timesync_interface;
+	struct gb_interface *interface;
+	u64 svc_frame_time[GB_TIMESYNC_MAX_STROBES];
+	int ret;
+
+	/* Get authoritative time from SVC and adjust local clock */
+	ret = gb_svc_timesync_authoritative(svc, svc_frame_time);
+	if (ret) {
+		dev_err(&svc->dev,
+			"gb_svc_timesync_authoritative %d\n", ret);
+		gb_timesync_teardown(timesync_svc);
+		return;
+	}
+	gb_timesync_collate_frame_time(timesync_svc, svc_frame_time);
+
+	/* Transmit authoritative time to downstream slaves */
+	hd = timesync_svc->timesync_hd->hd;
+	ret = hd->driver->timesync_authoritative(hd, svc_frame_time);
+	if (ret < 0)
+		dev_err(&hd->dev, "host timesync_authoritative %d\n", ret);
+
+	list_for_each_entry(timesync_interface,
+			    &timesync_svc->interface_list, list) {
+		interface = timesync_interface->interface;
+		ret = gb_interface_timesync_authoritative(
+						interface,
+						svc_frame_time);
+		if (ret) {
+			dev_err(&interface->dev,
+				"interface timesync_authoritative %d\n", ret);
+		}
+	}
+
+	/* Release wake pins */
+	gb_svc_timesync_wake_pins_release(svc);
+	gb_timesync_platform_unlock_bus();
+
+	/* Transition to state ACTIVE */
+	gb_timesync_set_state_atomic(timesync_svc, GB_TIMESYNC_STATE_ACTIVE);
+
+	/* Schedule a ping to verify the synchronized system time */
+	timesync_svc->print_ping = true;
+	gb_timesync_set_state_atomic(timesync_svc, GB_TIMESYNC_STATE_PING);
+}
+
+static int __gb_timesync_get_status(struct gb_timesync_svc *timesync_svc)
+{
+	int ret = -EINVAL;
+
+	switch (timesync_svc->state) {
+	case GB_TIMESYNC_STATE_INVALID:
+	case GB_TIMESYNC_STATE_INACTIVE:
+		ret = -ENODEV;
+		break;
+	case GB_TIMESYNC_STATE_INIT:
+	case GB_TIMESYNC_STATE_WAIT_SVC:
+	case GB_TIMESYNC_STATE_AUTHORITATIVE:
+		ret = -EAGAIN;
+		break;
+	case GB_TIMESYNC_STATE_PING:
+	case GB_TIMESYNC_STATE_ACTIVE:
+		ret = 0;
+		break;
+	}
+	return ret;
+}
+
+/*
+ * This routine takes a FrameTime and derives the difference with-respect
+ * to a reference FrameTime/ktime pair. It then returns the calculated
+ * ktime based on the difference between the supplied FrameTime and
+ * the reference FrameTime.
+ *
+ * The time difference is calculated to six decimal places. Taking 19.2MHz
+ * as an example this means we have 52.083333~ nanoseconds per clock or
+ * 52083333~ femtoseconds per clock.
+ *
+ * Naively taking the count difference and converting to
+ * seconds/nanoseconds would quickly see the 0.0833 component produce
+ * noticeable errors. For example a time difference of one second would
+ * loose 19200000 * 0.08333x nanoseconds or 1.59 seconds.
+ *
+ * In contrast calculating in femtoseconds the same example of 19200000 *
+ * 0.000000083333x nanoseconds per count of error is just 1.59 nanoseconds!
+ *
+ * Continuing the example of 19.2 MHz we cap the maximum error difference
+ * at a worst-case 0.3 microseconds over a potential calculation window of
+ * abount 15 seconds, meaning you can convert a FrameTime that is <= 15
+ * seconds older/younger than the reference time with a maximum error of
+ * 0.2385 useconds. Note 19.2MHz is an example frequency not a requirement.
+ */
+static int gb_timesync_to_timespec(struct gb_timesync_svc *timesync_svc,
+				   u64 frame_time, struct timespec *ts)
+{
+	unsigned long flags;
+	u64 delta_fs, counts, sec, nsec;
+	bool add;
+	int ret = 0;
+
+	memset(ts, 0x00, sizeof(*ts));
+	mutex_lock(&timesync_svc->mutex);
+	spin_lock_irqsave(&timesync_svc->spinlock, flags);
+
+	ret = __gb_timesync_get_status(timesync_svc);
+	if (ret)
+		goto done;
+
+	/* Support calculating ktime upwards or downwards from the reference */
+	if (frame_time < timesync_svc->ktime_data.frame_time) {
+		add = false;
+		counts = timesync_svc->ktime_data.frame_time - frame_time;
+	} else {
+		add = true;
+		counts = frame_time - timesync_svc->ktime_data.frame_time;
+	}
+
+	/* Enforce the .23 of a usecond boundary @ 19.2MHz */
+	if (counts > gb_timesync_max_ktime_diff) {
+		ret = -EINVAL;
+		goto done;
+	}
+
+	/* Determine the time difference in femtoseconds */
+	delta_fs = counts * gb_timesync_fs_per_clock;
+
+	/* Convert to seconds */
+	sec = delta_fs;
+	do_div(sec, NSEC_PER_SEC);
+	do_div(sec, 1000000UL);
+
+	/* Get the nanosecond remainder */
+	nsec = do_div(delta_fs, sec);
+	do_div(nsec, 1000000UL);
+
+	if (add) {
+		/* Add the calculated offset - overflow nanoseconds upwards */
+		ts->tv_sec = timesync_svc->ktime_data.ts.tv_sec + sec;
+		ts->tv_nsec = timesync_svc->ktime_data.ts.tv_nsec + nsec;
+		if (ts->tv_nsec >= NSEC_PER_SEC) {
+			ts->tv_sec++;
+			ts->tv_nsec -= NSEC_PER_SEC;
+		}
+	} else {
+		/* Subtract the difference over/underflow as necessary */
+		if (nsec > timesync_svc->ktime_data.ts.tv_nsec) {
+			sec++;
+			nsec = nsec + timesync_svc->ktime_data.ts.tv_nsec;
+			nsec = do_div(nsec, NSEC_PER_SEC);
+		} else {
+			nsec = timesync_svc->ktime_data.ts.tv_nsec - nsec;
+		}
+		/* Cannot return a negative second value */
+		if (sec > timesync_svc->ktime_data.ts.tv_sec) {
+			ret = -EINVAL;
+			goto done;
+		}
+		ts->tv_sec = timesync_svc->ktime_data.ts.tv_sec - sec;
+		ts->tv_nsec = nsec;
+	}
+done:
+	spin_unlock_irqrestore(&timesync_svc->spinlock, flags);
+	mutex_unlock(&timesync_svc->mutex);
+	return ret;
+}
+
+static size_t gb_timesync_log_frame_time(struct gb_timesync_svc *timesync_svc,
+					 char *buf, size_t buflen)
+{
+	struct gb_svc *svc = timesync_svc->svc;
+	struct gb_host_device *hd;
+	struct gb_timesync_interface *timesync_interface;
+	struct gb_interface *interface;
+	unsigned int len;
+	size_t off;
+
+	/* AP/SVC */
+	off = snprintf(buf, buflen, "%s frametime: ap=%llu %s=%llu ",
+		       greybus_bus_type.name,
+		       timesync_svc->ap_ping_frame_time, dev_name(&svc->dev),
+		       timesync_svc->svc_ping_frame_time);
+	len = buflen - off;
+
+	/* APB/GPB */
+	if (len < buflen) {
+		hd = timesync_svc->timesync_hd->hd;
+		off += snprintf(&buf[off], len, "%s=%llu ", dev_name(&hd->dev),
+				timesync_svc->timesync_hd->ping_frame_time);
+		len = buflen - off;
+	}
+
+	list_for_each_entry(timesync_interface,
+			    &timesync_svc->interface_list, list) {
+		if (len < buflen) {
+			interface = timesync_interface->interface;
+			off += snprintf(&buf[off], len, "%s=%llu ",
+					dev_name(&interface->dev),
+					timesync_interface->ping_frame_time);
+			len = buflen - off;
+		}
+	}
+	if (len < buflen)
+		off += snprintf(&buf[off], len, "\n");
+	return off;
+}
+
+static size_t gb_timesync_log_frame_ktime(struct gb_timesync_svc *timesync_svc,
+					  char *buf, size_t buflen)
+{
+	struct gb_svc *svc = timesync_svc->svc;
+	struct gb_host_device *hd;
+	struct gb_timesync_interface *timesync_interface;
+	struct gb_interface *interface;
+	struct timespec ts;
+	unsigned int len;
+	size_t off;
+
+	/* AP */
+	gb_timesync_to_timespec(timesync_svc, timesync_svc->ap_ping_frame_time,
+				&ts);
+	off = snprintf(buf, buflen, "%s frametime: ap=%lu.%lu ",
+		       greybus_bus_type.name, ts.tv_sec, ts.tv_nsec);
+	len = buflen - off;
+	if (len >= buflen)
+		goto done;
+
+	/* SVC */
+	gb_timesync_to_timespec(timesync_svc, timesync_svc->svc_ping_frame_time,
+				&ts);
+	off += snprintf(&buf[off], len, "%s=%lu.%lu ", dev_name(&svc->dev),
+			ts.tv_sec, ts.tv_nsec);
+	len = buflen - off;
+	if (len >= buflen)
+		goto done;
+
+	/* APB/GPB */
+	hd = timesync_svc->timesync_hd->hd;
+	gb_timesync_to_timespec(timesync_svc,
+				timesync_svc->timesync_hd->ping_frame_time,
+				&ts);
+	off += snprintf(&buf[off], len, "%s=%lu.%lu ",
+			dev_name(&hd->dev),
+			ts.tv_sec, ts.tv_nsec);
+	len = buflen - off;
+	if (len >= buflen)
+		goto done;
+
+	list_for_each_entry(timesync_interface,
+			    &timesync_svc->interface_list, list) {
+		interface = timesync_interface->interface;
+		gb_timesync_to_timespec(timesync_svc,
+					timesync_interface->ping_frame_time,
+					&ts);
+		off += snprintf(&buf[off], len, "%s=%lu.%lu ",
+				dev_name(&interface->dev),
+				ts.tv_sec, ts.tv_nsec);
+		len = buflen - off;
+		if (len >= buflen)
+			goto done;
+	}
+	off += snprintf(&buf[off], len, "\n");
+done:
+	return off;
+}
+
+/*
+ * Send an SVC initiated wake 'ping' to each TimeSync participant.
+ * Get the FrameTime from each participant associated with the wake
+ * ping.
+ */
+static void gb_timesync_ping(struct gb_timesync_svc *timesync_svc)
+{
+	struct gb_svc *svc = timesync_svc->svc;
+	struct gb_host_device *hd;
+	struct gb_timesync_interface *timesync_interface;
+	struct gb_control *control;
+	u64 *ping_frame_time;
+	int ret;
+
+	/* Get access to the wake pins in the AP and SVC */
+	ret = gb_timesync_platform_lock_bus(timesync_svc);
+	if (ret < 0) {
+		gb_timesync_platform_lock_bus_fail(timesync_svc, ret);
+		return;
+	}
+	ret = gb_svc_timesync_wake_pins_acquire(svc, timesync_svc->strobe_mask);
+	if (ret) {
+		dev_err(&svc->dev,
+			"gb_svc_timesync_wake_pins_acquire %d\n", ret);
+		gb_timesync_teardown(timesync_svc);
+		return;
+	}
+
+	/* Have SVC generate a timesync ping */
+	timesync_svc->capture_ping = true;
+	timesync_svc->svc_ping_frame_time = 0;
+	ret = gb_svc_timesync_ping(svc, &timesync_svc->svc_ping_frame_time);
+	timesync_svc->capture_ping = false;
+	if (ret) {
+		dev_err(&svc->dev,
+			"gb_svc_timesync_ping %d\n", ret);
+		gb_timesync_teardown(timesync_svc);
+		return;
+	}
+
+	/* Get the ping FrameTime from each APB/GPB */
+	hd = timesync_svc->timesync_hd->hd;
+	timesync_svc->timesync_hd->ping_frame_time = 0;
+	ret = hd->driver->timesync_get_last_event(hd,
+		&timesync_svc->timesync_hd->ping_frame_time);
+	if (ret)
+		dev_err(&hd->dev, "host timesync_get_last_event %d\n", ret);
+
+	list_for_each_entry(timesync_interface,
+			    &timesync_svc->interface_list, list) {
+		control = timesync_interface->interface->control;
+		timesync_interface->ping_frame_time = 0;
+		ping_frame_time = &timesync_interface->ping_frame_time;
+		ret = gb_control_timesync_get_last_event(control,
+							 ping_frame_time);
+		if (ret) {
+			dev_err(&timesync_interface->interface->dev,
+				"gb_control_timesync_get_last_event %d\n", ret);
+		}
+	}
+
+	/* Ping success - move to timesync active */
+	gb_svc_timesync_wake_pins_release(svc);
+	gb_timesync_platform_unlock_bus();
+	gb_timesync_set_state_atomic(timesync_svc, GB_TIMESYNC_STATE_ACTIVE);
+}
+
+static void gb_timesync_log_ping_time(struct gb_timesync_svc *timesync_svc)
+{
+	char *buf;
+
+	if (!timesync_svc->print_ping)
+		return;
+
+	buf = kzalloc(PAGE_SIZE, GFP_KERNEL);
+	if (buf) {
+		gb_timesync_log_frame_time(timesync_svc, buf, PAGE_SIZE);
+		dev_dbg(&timesync_svc->svc->dev, "%s", buf);
+		kfree(buf);
+	}
+}
+
+/*
+ * Perform the actual work of scheduled TimeSync logic.
+ */
+static void gb_timesync_worker(struct work_struct *work)
+{
+	struct delayed_work *delayed_work = to_delayed_work(work);
+	struct gb_timesync_svc *timesync_svc =
+		container_of(delayed_work, struct gb_timesync_svc, delayed_work);
+
+	mutex_lock(&timesync_svc->mutex);
+
+	switch (timesync_svc->state) {
+	case GB_TIMESYNC_STATE_INIT:
+		gb_timesync_enable(timesync_svc);
+		break;
+
+	case GB_TIMESYNC_STATE_WAIT_SVC:
+		dev_err(&timesync_svc->svc->dev,
+			"timeout SVC strobe completion %d/%d\n",
+			timesync_svc->strobe, GB_TIMESYNC_MAX_STROBES);
+		gb_timesync_teardown(timesync_svc);
+		break;
+
+	case GB_TIMESYNC_STATE_AUTHORITATIVE:
+		gb_timesync_authoritative(timesync_svc);
+		break;
+
+	case GB_TIMESYNC_STATE_PING:
+		gb_timesync_ping(timesync_svc);
+		gb_timesync_log_ping_time(timesync_svc);
+		break;
+
+	default:
+		pr_err("Invalid state %d for delayed work\n",
+		       timesync_svc->state);
+		break;
+	}
+
+	mutex_unlock(&timesync_svc->mutex);
+}
+
+/*
+ * Schedule a new TimeSync INIT or PING operation serialized w/r to
+ * gb_timesync_worker().
+ */
+static int gb_timesync_schedule(struct gb_timesync_svc *timesync_svc, int state)
+{
+	int ret = 0;
+
+	if (state != GB_TIMESYNC_STATE_INIT && state != GB_TIMESYNC_STATE_PING)
+		return -EINVAL;
+
+	mutex_lock(&timesync_svc->mutex);
+	if (timesync_svc->state !=  GB_TIMESYNC_STATE_INVALID) {
+		gb_timesync_set_state_atomic(timesync_svc, state);
+	} else {
+		ret = -ENODEV;
+	}
+	mutex_unlock(&timesync_svc->mutex);
+	return ret;
+}
+
+static int __gb_timesync_schedule_synchronous(
+	struct gb_timesync_svc *timesync_svc, int state)
+{
+	unsigned long flags;
+	int ret;
+
+	ret = gb_timesync_schedule(timesync_svc, state);
+	if (ret)
+		return ret;
+
+	ret = wait_event_interruptible(timesync_svc->wait_queue,
+			(timesync_svc->state == GB_TIMESYNC_STATE_ACTIVE ||
+			 timesync_svc->state == GB_TIMESYNC_STATE_INACTIVE ||
+			 timesync_svc->state == GB_TIMESYNC_STATE_INVALID));
+	if (ret)
+		return ret;
+
+	mutex_lock(&timesync_svc->mutex);
+	spin_lock_irqsave(&timesync_svc->spinlock, flags);
+
+	ret = __gb_timesync_get_status(timesync_svc);
+
+	spin_unlock_irqrestore(&timesync_svc->spinlock, flags);
+	mutex_unlock(&timesync_svc->mutex);
+
+	return ret;
+}
+
+static struct gb_timesync_svc *gb_timesync_find_timesync_svc(
+	struct gb_host_device *hd)
+{
+	struct gb_timesync_svc *timesync_svc;
+
+	list_for_each_entry(timesync_svc, &gb_timesync_svc_list, list) {
+		if (timesync_svc->svc == hd->svc)
+			return timesync_svc;
+	}
+	return NULL;
+}
+
+static struct gb_timesync_interface *gb_timesync_find_timesync_interface(
+	struct gb_timesync_svc *timesync_svc,
+	struct gb_interface *interface)
+{
+	struct gb_timesync_interface *timesync_interface;
+
+	list_for_each_entry(timesync_interface, &timesync_svc->interface_list, list) {
+		if (timesync_interface->interface == interface)
+			return timesync_interface;
+	}
+	return NULL;
+}
+
+int gb_timesync_schedule_synchronous(struct gb_interface *interface)
+{
+	int ret;
+	struct gb_timesync_svc *timesync_svc;
+	int retries;
+
+	if (!(interface->features & GREYBUS_INTERFACE_FEATURE_TIMESYNC))
+		return 0;
+
+	mutex_lock(&gb_timesync_svc_list_mutex);
+	for (retries = 0; retries < GB_TIMESYNC_MAX_RETRIES; retries++) {
+		timesync_svc = gb_timesync_find_timesync_svc(interface->hd);
+		if (!timesync_svc) {
+			ret = -ENODEV;
+			goto done;
+		}
+
+		ret = __gb_timesync_schedule_synchronous(timesync_svc,
+						 GB_TIMESYNC_STATE_INIT);
+		if (!ret)
+			break;
+	}
+	if (ret && retries == GB_TIMESYNC_MAX_RETRIES)
+		ret = -ETIMEDOUT;
+done:
+	mutex_unlock(&gb_timesync_svc_list_mutex);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(gb_timesync_schedule_synchronous);
+
+void gb_timesync_schedule_asynchronous(struct gb_interface *interface)
+{
+	struct gb_timesync_svc *timesync_svc;
+
+	if (!(interface->features & GREYBUS_INTERFACE_FEATURE_TIMESYNC))
+		return;
+
+	mutex_lock(&gb_timesync_svc_list_mutex);
+	timesync_svc = gb_timesync_find_timesync_svc(interface->hd);
+	if (!timesync_svc)
+		goto done;
+
+	gb_timesync_schedule(timesync_svc, GB_TIMESYNC_STATE_INIT);
+done:
+	mutex_unlock(&gb_timesync_svc_list_mutex);
+	return;
+}
+EXPORT_SYMBOL_GPL(gb_timesync_schedule_asynchronous);
+
+static ssize_t gb_timesync_ping_read(struct file *file, char __user *ubuf,
+				     size_t len, loff_t *offset, bool ktime)
+{
+	struct gb_timesync_svc *timesync_svc = file->f_inode->i_private;
+	char *buf;
+	ssize_t ret = 0;
+
+	mutex_lock(&gb_timesync_svc_list_mutex);
+	mutex_lock(&timesync_svc->mutex);
+	if (list_empty(&timesync_svc->interface_list))
+		ret = -ENODEV;
+	timesync_svc->print_ping = false;
+	mutex_unlock(&timesync_svc->mutex);
+	if (ret)
+		goto done;
+
+	ret = __gb_timesync_schedule_synchronous(timesync_svc,
+						 GB_TIMESYNC_STATE_PING);
+	if (ret)
+		goto done;
+
+	buf = kzalloc(PAGE_SIZE, GFP_KERNEL);
+	if (!buf) {
+		ret = -ENOMEM;
+		goto done;
+	}
+
+	if (ktime)
+		ret = gb_timesync_log_frame_ktime(timesync_svc, buf, PAGE_SIZE);
+	else
+		ret = gb_timesync_log_frame_time(timesync_svc, buf, PAGE_SIZE);
+	if (ret > 0)
+		ret = simple_read_from_buffer(ubuf, len, offset, buf, ret);
+	kfree(buf);
+done:
+	mutex_unlock(&gb_timesync_svc_list_mutex);
+	return ret;
+}
+
+static ssize_t gb_timesync_ping_read_frame_time(struct file *file,
+						char __user *buf,
+						size_t len, loff_t *offset)
+{
+	return gb_timesync_ping_read(file, buf, len, offset, false);
+}
+
+static ssize_t gb_timesync_ping_read_frame_ktime(struct file *file,
+						 char __user *buf,
+						 size_t len, loff_t *offset)
+{
+	return gb_timesync_ping_read(file, buf, len, offset, true);
+}
+
+static const struct file_operations gb_timesync_debugfs_frame_time_ops = {
+	.read		= gb_timesync_ping_read_frame_time,
+};
+
+static const struct file_operations gb_timesync_debugfs_frame_ktime_ops = {
+	.read		= gb_timesync_ping_read_frame_ktime,
+};
+
+static int gb_timesync_hd_add(struct gb_timesync_svc *timesync_svc,
+			      struct gb_host_device *hd)
+{
+	struct gb_timesync_host_device *timesync_hd;
+
+	timesync_hd = kzalloc(sizeof(*timesync_hd), GFP_KERNEL);
+	if (!timesync_hd)
+		return -ENOMEM;
+
+	WARN_ON(timesync_svc->timesync_hd);
+	timesync_hd->hd = hd;
+	timesync_svc->timesync_hd = timesync_hd;
+
+	return 0;
+}
+
+static void gb_timesync_hd_remove(struct gb_timesync_svc *timesync_svc,
+				  struct gb_host_device *hd)
+{
+	if (timesync_svc->timesync_hd->hd == hd) {
+		kfree(timesync_svc->timesync_hd);
+		timesync_svc->timesync_hd = NULL;
+		return;
+	}
+	WARN_ON(1);
+}
+
+int gb_timesync_svc_add(struct gb_svc *svc)
+{
+	struct gb_timesync_svc *timesync_svc;
+	int ret;
+
+	timesync_svc = kzalloc(sizeof(*timesync_svc), GFP_KERNEL);
+	if (!timesync_svc)
+		return -ENOMEM;
+
+	timesync_svc->work_queue =
+		create_singlethread_workqueue("gb-timesync-work_queue");
+
+	if (!timesync_svc->work_queue) {
+		kfree(timesync_svc);
+		return -ENOMEM;
+	}
+
+	mutex_lock(&gb_timesync_svc_list_mutex);
+	INIT_LIST_HEAD(&timesync_svc->interface_list);
+	INIT_DELAYED_WORK(&timesync_svc->delayed_work, gb_timesync_worker);
+	mutex_init(&timesync_svc->mutex);
+	spin_lock_init(&timesync_svc->spinlock);
+	init_waitqueue_head(&timesync_svc->wait_queue);
+
+	timesync_svc->svc = svc;
+	timesync_svc->frame_time_offset = 0;
+	timesync_svc->capture_ping = false;
+	gb_timesync_set_state_atomic(timesync_svc, GB_TIMESYNC_STATE_INACTIVE);
+
+	timesync_svc->frame_time_dentry =
+		debugfs_create_file("frame-time", S_IRUGO, svc->debugfs_dentry,
+				    timesync_svc,
+				    &gb_timesync_debugfs_frame_time_ops);
+	timesync_svc->frame_ktime_dentry =
+		debugfs_create_file("frame-ktime", S_IRUGO, svc->debugfs_dentry,
+				    timesync_svc,
+				    &gb_timesync_debugfs_frame_ktime_ops);
+
+	list_add(&timesync_svc->list, &gb_timesync_svc_list);
+	ret = gb_timesync_hd_add(timesync_svc, svc->hd);
+	if (ret) {
+		list_del(&timesync_svc->list);
+		debugfs_remove(timesync_svc->frame_ktime_dentry);
+		debugfs_remove(timesync_svc->frame_time_dentry);
+		destroy_workqueue(timesync_svc->work_queue);
+		kfree(timesync_svc);
+		goto done;
+	}
+
+	init_timer(&timesync_svc->ktime_timer);
+	timesync_svc->ktime_timer.function = gb_timesync_ktime_timer_fn;
+	timesync_svc->ktime_timer.expires = jiffies + GB_TIMESYNC_KTIME_UPDATE;
+	timesync_svc->ktime_timer.data = (unsigned long)timesync_svc;
+	add_timer(&timesync_svc->ktime_timer);
+done:
+	mutex_unlock(&gb_timesync_svc_list_mutex);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(gb_timesync_svc_add);
+
+void gb_timesync_svc_remove(struct gb_svc *svc)
+{
+	struct gb_timesync_svc *timesync_svc;
+	struct gb_timesync_interface *timesync_interface;
+	struct gb_timesync_interface *next;
+
+	mutex_lock(&gb_timesync_svc_list_mutex);
+	timesync_svc = gb_timesync_find_timesync_svc(svc->hd);
+	if (!timesync_svc)
+		goto done;
+
+	cancel_delayed_work_sync(&timesync_svc->delayed_work);
+
+	mutex_lock(&timesync_svc->mutex);
+
+	gb_timesync_set_state_atomic(timesync_svc, GB_TIMESYNC_STATE_INVALID);
+	del_timer_sync(&timesync_svc->ktime_timer);
+	gb_timesync_teardown(timesync_svc);
+
+	gb_timesync_hd_remove(timesync_svc, svc->hd);
+	list_for_each_entry_safe(timesync_interface, next,
+				 &timesync_svc->interface_list, list) {
+		list_del(&timesync_interface->list);
+		kfree(timesync_interface);
+	}
+	debugfs_remove(timesync_svc->frame_ktime_dentry);
+	debugfs_remove(timesync_svc->frame_time_dentry);
+	destroy_workqueue(timesync_svc->work_queue);
+	list_del(&timesync_svc->list);
+
+	mutex_unlock(&timesync_svc->mutex);
+
+	kfree(timesync_svc);
+done:
+	mutex_unlock(&gb_timesync_svc_list_mutex);
+}
+EXPORT_SYMBOL_GPL(gb_timesync_svc_remove);
+
+/*
+ * Add a Greybus Interface to the set of TimeSync Interfaces.
+ */
+int gb_timesync_interface_add(struct gb_interface *interface)
+{
+	struct gb_timesync_svc *timesync_svc;
+	struct gb_timesync_interface *timesync_interface;
+	int ret = 0;
+
+	if (!(interface->features & GREYBUS_INTERFACE_FEATURE_TIMESYNC))
+		return 0;
+
+	mutex_lock(&gb_timesync_svc_list_mutex);
+	timesync_svc = gb_timesync_find_timesync_svc(interface->hd);
+	if (!timesync_svc) {
+		ret = -ENODEV;
+		goto done;
+	}
+
+	timesync_interface = kzalloc(sizeof(*timesync_interface), GFP_KERNEL);
+	if (!timesync_interface) {
+		ret = -ENOMEM;
+		goto done;
+	}
+
+	mutex_lock(&timesync_svc->mutex);
+	timesync_interface->interface = interface;
+	list_add(&timesync_interface->list, &timesync_svc->interface_list);
+	timesync_svc->strobe_mask |= 1 << interface->interface_id;
+	mutex_unlock(&timesync_svc->mutex);
+
+done:
+	mutex_unlock(&gb_timesync_svc_list_mutex);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(gb_timesync_interface_add);
+
+/*
+ * Remove a Greybus Interface from the set of TimeSync Interfaces.
+ */
+void gb_timesync_interface_remove(struct gb_interface *interface)
+{
+	struct gb_timesync_svc *timesync_svc;
+	struct gb_timesync_interface *timesync_interface;
+
+	if (!(interface->features & GREYBUS_INTERFACE_FEATURE_TIMESYNC))
+		return;
+
+	mutex_lock(&gb_timesync_svc_list_mutex);
+	timesync_svc = gb_timesync_find_timesync_svc(interface->hd);
+	if (!timesync_svc)
+		goto done;
+
+	timesync_interface = gb_timesync_find_timesync_interface(timesync_svc,
+								 interface);
+	if (!timesync_interface)
+		goto done;
+
+	mutex_lock(&timesync_svc->mutex);
+	timesync_svc->strobe_mask &= ~(1 << interface->interface_id);
+	list_del(&timesync_interface->list);
+	kfree(timesync_interface);
+	mutex_unlock(&timesync_svc->mutex);
+done:
+	mutex_unlock(&gb_timesync_svc_list_mutex);
+}
+EXPORT_SYMBOL_GPL(gb_timesync_interface_remove);
+
+/*
+ * Give the authoritative FrameTime to the calling function. Returns zero if we
+ * are not in GB_TIMESYNC_STATE_ACTIVE.
+ */
+static u64 gb_timesync_get_frame_time(struct gb_timesync_svc *timesync_svc)
+{
+	unsigned long flags;
+	u64 ret;
+
+	spin_lock_irqsave(&timesync_svc->spinlock, flags);
+	if (timesync_svc->state == GB_TIMESYNC_STATE_ACTIVE)
+		ret = __gb_timesync_get_frame_time(timesync_svc);
+	else
+		ret = 0;
+	spin_unlock_irqrestore(&timesync_svc->spinlock, flags);
+	return ret;
+}
+
+u64 gb_timesync_get_frame_time_by_interface(struct gb_interface *interface)
+{
+	struct gb_timesync_svc *timesync_svc;
+	u64 ret = 0;
+
+	mutex_lock(&gb_timesync_svc_list_mutex);
+	timesync_svc = gb_timesync_find_timesync_svc(interface->hd);
+	if (!timesync_svc)
+		goto done;
+
+	ret = gb_timesync_get_frame_time(timesync_svc);
+done:
+	mutex_unlock(&gb_timesync_svc_list_mutex);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(gb_timesync_get_frame_time_by_interface);
+
+u64 gb_timesync_get_frame_time_by_svc(struct gb_svc *svc)
+{
+	struct gb_timesync_svc *timesync_svc;
+	u64 ret = 0;
+
+	mutex_lock(&gb_timesync_svc_list_mutex);
+	timesync_svc = gb_timesync_find_timesync_svc(svc->hd);
+	if (!timesync_svc)
+		goto done;
+
+	ret = gb_timesync_get_frame_time(timesync_svc);
+done:
+	mutex_unlock(&gb_timesync_svc_list_mutex);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(gb_timesync_get_frame_time_by_svc);
+
+/* Incrementally updates the conversion base from FrameTime to ktime */
+static void gb_timesync_ktime_timer_fn(unsigned long data)
+{
+	struct gb_timesync_svc *timesync_svc =
+		(struct gb_timesync_svc *)data;
+	unsigned long flags;
+	u64 frame_time;
+	struct timespec ts;
+
+	spin_lock_irqsave(&timesync_svc->spinlock, flags);
+
+	if (timesync_svc->state != GB_TIMESYNC_STATE_ACTIVE)
+		goto done;
+
+	ktime_get_ts(&ts);
+	frame_time = __gb_timesync_get_frame_time(timesync_svc);
+	gb_timesync_store_ktime(timesync_svc, ts, frame_time);
+
+done:
+	spin_unlock_irqrestore(&timesync_svc->spinlock, flags);
+	mod_timer(&timesync_svc->ktime_timer,
+		  jiffies + GB_TIMESYNC_KTIME_UPDATE);
+}
+
+int gb_timesync_to_timespec_by_svc(struct gb_svc *svc, u64 frame_time,
+				   struct timespec *ts)
+{
+	struct gb_timesync_svc *timesync_svc;
+	int ret = 0;
+
+	mutex_lock(&gb_timesync_svc_list_mutex);
+	timesync_svc = gb_timesync_find_timesync_svc(svc->hd);
+	if (!timesync_svc) {
+		ret = -ENODEV;
+		goto done;
+	}
+	ret = gb_timesync_to_timespec(timesync_svc, frame_time, ts);
+done:
+	mutex_unlock(&gb_timesync_svc_list_mutex);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(gb_timesync_to_timespec_by_svc);
+
+int gb_timesync_to_timespec_by_interface(struct gb_interface *interface,
+					 u64 frame_time, struct timespec *ts)
+{
+	struct gb_timesync_svc *timesync_svc;
+	int ret = 0;
+
+	mutex_lock(&gb_timesync_svc_list_mutex);
+	timesync_svc = gb_timesync_find_timesync_svc(interface->hd);
+	if (!timesync_svc) {
+		ret = -ENODEV;
+		goto done;
+	}
+
+	ret = gb_timesync_to_timespec(timesync_svc, frame_time, ts);
+done:
+	mutex_unlock(&gb_timesync_svc_list_mutex);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(gb_timesync_to_timespec_by_interface);
+
+void gb_timesync_irq(struct gb_timesync_svc *timesync_svc)
+{
+	unsigned long flags;
+	u64 strobe_time;
+	bool strobe_is_ping = true;
+	struct timespec ts;
+
+	ktime_get_ts(&ts);
+	strobe_time = __gb_timesync_get_frame_time(timesync_svc);
+
+	spin_lock_irqsave(&timesync_svc->spinlock, flags);
+
+	if (timesync_svc->state == GB_TIMESYNC_STATE_PING) {
+		if (!timesync_svc->capture_ping)
+			goto done_nolog;
+		timesync_svc->ap_ping_frame_time = strobe_time;
+		goto done_log;
+	} else if (timesync_svc->state != GB_TIMESYNC_STATE_WAIT_SVC) {
+		goto done_nolog;
+	}
+
+	timesync_svc->strobe_data[timesync_svc->strobe].frame_time = strobe_time;
+	timesync_svc->strobe_data[timesync_svc->strobe].ts = ts;
+
+	if (++timesync_svc->strobe == GB_TIMESYNC_MAX_STROBES) {
+		gb_timesync_set_state(timesync_svc,
+				      GB_TIMESYNC_STATE_AUTHORITATIVE);
+	}
+	strobe_is_ping = false;
+done_log:
+	trace_gb_timesync_irq(strobe_is_ping, timesync_svc->strobe,
+			      GB_TIMESYNC_MAX_STROBES, strobe_time);
+done_nolog:
+	spin_unlock_irqrestore(&timesync_svc->spinlock, flags);
+}
+EXPORT_SYMBOL(gb_timesync_irq);
+
+int __init gb_timesync_init(void)
+{
+	int ret = 0;
+
+	ret = gb_timesync_platform_init();
+	if (ret) {
+		pr_err("timesync platform init fail!\n");
+		return ret;
+	}
+
+	gb_timesync_clock_rate = gb_timesync_platform_get_clock_rate();
+
+	/* Calculate nanoseconds and femtoseconds per clock */
+	gb_timesync_fs_per_clock = FSEC_PER_SEC;
+	do_div(gb_timesync_fs_per_clock, gb_timesync_clock_rate);
+	gb_timesync_ns_per_clock = NSEC_PER_SEC;
+	do_div(gb_timesync_ns_per_clock, gb_timesync_clock_rate);
+
+	/* Calculate the maximum number of clocks we will convert to ktime */
+	gb_timesync_max_ktime_diff =
+		GB_TIMESYNC_MAX_KTIME_CONVERSION * gb_timesync_clock_rate;
+
+	pr_info("Time-Sync @ %lu Hz max ktime conversion +/- %d seconds\n",
+		gb_timesync_clock_rate, GB_TIMESYNC_MAX_KTIME_CONVERSION);
+	return 0;
+}
+
+void gb_timesync_exit(void)
+{
+	gb_timesync_platform_exit();
+}
--- /dev/null
+++ b/drivers/greybus/timesync.h
@@ -0,0 +1,45 @@
+/*
+ * TimeSync API driver.
+ *
+ * Copyright 2016 Google Inc.
+ * Copyright 2016 Linaro Ltd.
+ *
+ * Released under the GPLv2 only.
+ */
+
+#ifndef __TIMESYNC_H
+#define __TIMESYNC_H
+
+struct gb_svc;
+struct gb_interface;
+struct gb_timesync_svc;
+
+/* Platform */
+u64 gb_timesync_platform_get_counter(void);
+u32 gb_timesync_platform_get_clock_rate(void);
+int gb_timesync_platform_lock_bus(struct gb_timesync_svc *pdata);
+void gb_timesync_platform_unlock_bus(void);
+
+int gb_timesync_platform_init(void);
+void gb_timesync_platform_exit(void);
+
+/* Core API */
+int gb_timesync_interface_add(struct gb_interface *interface);
+void gb_timesync_interface_remove(struct gb_interface *interface);
+int gb_timesync_svc_add(struct gb_svc *svc);
+void gb_timesync_svc_remove(struct gb_svc *svc);
+
+u64 gb_timesync_get_frame_time_by_interface(struct gb_interface *interface);
+u64 gb_timesync_get_frame_time_by_svc(struct gb_svc *svc);
+int gb_timesync_to_timespec_by_svc(struct gb_svc *svc, u64 frame_time,
+				   struct timespec *ts);
+int gb_timesync_to_timespec_by_interface(struct gb_interface *interface,
+					 u64 frame_time, struct timespec *ts);
+
+int gb_timesync_schedule_synchronous(struct gb_interface *intf);
+void gb_timesync_schedule_asynchronous(struct gb_interface *intf);
+void gb_timesync_irq(struct gb_timesync_svc *timesync_svc);
+int gb_timesync_init(void);
+void gb_timesync_exit(void);
+
+#endif /* __TIMESYNC_H */
--- /dev/null
+++ b/drivers/greybus/timesync_platform.c
@@ -0,0 +1,77 @@
+/*
+ * TimeSync API driver.
+ *
+ * Copyright 2016 Google Inc.
+ * Copyright 2016 Linaro Ltd.
+ *
+ * Released under the GPLv2 only.
+ *
+ * This code reads directly from an ARMv7 memory-mapped timer that lives in
+ * MMIO space. Since this counter lives inside of MMIO space its shared between
+ * cores and that means we don't have to worry about issues like TSC on x86
+ * where each time-stamp-counter (TSC) is local to a particular core.
+ *
+ * Register-level access code is based on
+ * drivers/clocksource/arm_arch_timer.c
+ */
+#include <linux/cpufreq.h>
+#include <linux/of_platform.h>
+
+#include "greybus.h"
+#include "arche_platform.h"
+
+static u32 gb_timesync_clock_frequency;
+int (*arche_platform_change_state_cb)(enum arche_platform_state state,
+				      struct gb_timesync_svc *pdata);
+EXPORT_SYMBOL_GPL(arche_platform_change_state_cb);
+
+u64 gb_timesync_platform_get_counter(void)
+{
+	return (u64)get_cycles();
+}
+
+u32 gb_timesync_platform_get_clock_rate(void)
+{
+	if (unlikely(!gb_timesync_clock_frequency))
+		return cpufreq_get(0);
+
+	return gb_timesync_clock_frequency;
+}
+
+int gb_timesync_platform_lock_bus(struct gb_timesync_svc *pdata)
+{
+	return arche_platform_change_state_cb(ARCHE_PLATFORM_STATE_TIME_SYNC,
+					      pdata);
+}
+
+void gb_timesync_platform_unlock_bus(void)
+{
+	arche_platform_change_state_cb(ARCHE_PLATFORM_STATE_ACTIVE, NULL);
+}
+
+static const struct of_device_id arch_timer_of_match[] = {
+	{ .compatible   = "google,greybus-frame-time-counter", },
+	{},
+};
+
+int __init gb_timesync_platform_init(void)
+{
+	struct device_node *np;
+
+	np = of_find_matching_node(NULL, arch_timer_of_match);
+	if (!np) {
+		/* Tolerate not finding to allow BBB etc to continue */
+		pr_warn("Unable to find a compatible ARMv7 timer\n");
+		return 0;
+	}
+
+	if (of_property_read_u32(np, "clock-frequency",
+				 &gb_timesync_clock_frequency)) {
+		pr_err("Unable to find timer clock-frequency\n");
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+void gb_timesync_platform_exit(void) {}

Powered by blists - more mailing lists