+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((void __iomem *) time_interpolator->addr);
+
+ case TIME_SOURCE_MMIO32 :
+ return readl((void __iomem *) time_interpolator->addr);
+
+ default: return get_cycles();
+ }
+}
+
+static inline u64 time_interpolator_get_counter(void)
+{
+ 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;
+ /* 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();
+}
+
+#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(), time_interpolator);
+}
+
+#define INTERPOLATOR_ADJUST 65536
+#define INTERPOLATOR_MAX_SKIP 10*INTERPOLATOR_ADJUST
+
+static 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();
+ 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;
+ }
+}