+struct time_interpolator *time_interpolator __read_mostly;
+static struct time_interpolator *time_interpolator_list __read_mostly;
+static DEFINE_SPINLOCK(time_interpolator_lock);
+
+static inline u64 time_interpolator_get_cycles(unsigned int src)
+{
+ unsigned long (*x)(void);
+
+ switch (src)
+ {
+ case TIME_SOURCE_FUNCTION:
+ x = time_interpolator->addr;
+ return x();
+
+ case TIME_SOURCE_MMIO64 :
+ return readq_relaxed((void __iomem *)time_interpolator->addr);
+
+ case TIME_SOURCE_MMIO32 :
+ return readl_relaxed((void __iomem *)time_interpolator->addr);
+
+ default: return get_cycles();
+ }
+}
+
+static inline u64 time_interpolator_get_counter(int writelock)
+{
+ unsigned int src = time_interpolator->source;
+
+ if (time_interpolator->jitter)
+ {
+ u64 lcycle;
+ u64 now;
+
+ do {
+ lcycle = time_interpolator->last_cycle;
+ now = time_interpolator_get_cycles(src);
+ if (lcycle && time_after(lcycle, now))
+ return lcycle;
+
+ /* When holding the xtime write lock, there's no need
+ * to add the overhead of the cmpxchg. Readers are
+ * force to retry until the write lock is released.
+ */
+ if (writelock) {
+ time_interpolator->last_cycle = now;
+ return now;
+ }
+ /* Keep track of the last timer value returned. The use of cmpxchg here
+ * will cause contention in an SMP environment.
+ */
+ } while (unlikely(cmpxchg(&time_interpolator->last_cycle, lcycle, now) != lcycle));
+ return now;
+ }
+ else
+ return time_interpolator_get_cycles(src);
+}
+
+void time_interpolator_reset(void)
+{
+ time_interpolator->offset = 0;
+ time_interpolator->last_counter = time_interpolator_get_counter(1);
+}
+
+#define GET_TI_NSECS(count,i) (((((count) - i->last_counter) & (i)->mask) * (i)->nsec_per_cyc) >> (i)->shift)
+
+unsigned long time_interpolator_get_offset(void)
+{
+ /* If we do not have a time interpolator set up then just return zero */
+ if (!time_interpolator)
+ return 0;
+
+ return time_interpolator->offset +
+ GET_TI_NSECS(time_interpolator_get_counter(0), time_interpolator);
+}
+
+#define INTERPOLATOR_ADJUST 65536
+#define INTERPOLATOR_MAX_SKIP 10*INTERPOLATOR_ADJUST
+
+void time_interpolator_update(long delta_nsec)
+{
+ u64 counter;
+ unsigned long offset;
+
+ /* If there is no time interpolator set up then do nothing */
+ if (!time_interpolator)
+ return;
+
+ /*
+ * The interpolator compensates for late ticks by accumulating the late
+ * time in time_interpolator->offset. A tick earlier than expected will
+ * lead to a reset of the offset and a corresponding jump of the clock
+ * forward. Again this only works if the interpolator clock is running
+ * slightly slower than the regular clock and the tuning logic insures
+ * that.
+ */
+
+ counter = time_interpolator_get_counter(1);
+ offset = time_interpolator->offset +
+ GET_TI_NSECS(counter, time_interpolator);
+
+ if (delta_nsec < 0 || (unsigned long) delta_nsec < offset)
+ time_interpolator->offset = offset - delta_nsec;
+ else {
+ time_interpolator->skips++;
+ time_interpolator->ns_skipped += delta_nsec - offset;
+ time_interpolator->offset = 0;
+ }
+ time_interpolator->last_counter = counter;
+
+ /* Tuning logic for time interpolator invoked every minute or so.
+ * Decrease interpolator clock speed if no skips occurred and an offset is carried.
+ * Increase interpolator clock speed if we skip too much time.
+ */
+ if (jiffies % INTERPOLATOR_ADJUST == 0)
+ {
+ if (time_interpolator->skips == 0 && time_interpolator->offset > tick_nsec)
+ time_interpolator->nsec_per_cyc--;
+ if (time_interpolator->ns_skipped > INTERPOLATOR_MAX_SKIP && time_interpolator->offset == 0)
+ time_interpolator->nsec_per_cyc++;
+ time_interpolator->skips = 0;
+ time_interpolator->ns_skipped = 0;
+ }
+}