From: Carsten Emde <C.Emde@osadl.org>
Date: Thu, 17 Jul 2014 20:01:10 +0200
Subject: Forward port of the high-resolution sched_clock

The upstream sched_clock uses jiffie resolution (250 Hz equals 4 ms)
which is not sufficient to dignose latency irregularities in the range
of tens of microseconds. This patch contains a forward port of the
high-resolution sched_clock that was available in 2.6.33.7.2-rt30.

Signed-off-by: Carsten Emde <C.Emde@osadl.org>

---

---
 arch/mips/include/asm/time.h |   30 ++++++++++++++++++++++++++++++
 arch/mips/kernel/csrc-r4k.c  |   36 ++++++++++++++++++++++++++++++++++++
 2 files changed, 66 insertions(+)

Index: linux-3.12.19-rt30/arch/mips/include/asm/time.h
===================================================================
@ linux-3.12.19-rt30/arch/mips/include/asm/time.h:91 @ static inline void clockevent_set_clock(
 	clockevents_calc_mult_shift(cd, clock, 4);
 }
 
+static inline unsigned long long mips_sched_clock(struct clocksource *cs, u64 cnt)
+{
+	/* 64-bit arithmatic can overflow, so use 128-bit.  */
+#if (__GNUC__ < 4) || ((__GNUC__ == 4) && (__GNUC_MINOR__ <= 3))
+	u64 t1, t2, t3;
+	unsigned long long rv;
+	u64 mult = cs->mult;
+	u64 shift = cs->shift;
+
+	asm (
+		"dmultu\t%[cnt],%[mult]\n\t"
+		"nor\t%[t1],$0,%[shift]\n\t"
+		"mfhi\t%[t2]\n\t"
+		"mflo\t%[t3]\n\t"
+		"dsll\t%[t2],%[t2],1\n\t"
+		"dsrlv\t%[rv],%[t3],%[shift]\n\t"
+		"dsllv\t%[t1],%[t2],%[t1]\n\t"
+		"or\t%[rv],%[t1],%[rv]\n\t"
+		: [rv] "=&r" (rv), [t1] "=&r" (t1), [t2] "=&r" (t2), [t3] "=&r" (t3)
+		: [cnt] "r" (cnt), [mult] "r" (mult), [shift] "r" (shift)
+		: "hi", "lo");
+	return rv;
+#else	/* GCC > 4.3 do it the easy way.  */
+	unsigned int __attribute__((mode(TI))) t = cnt;
+
+	t = (t * cs->mult) >> cs->shift;
+	return (unsigned long long)t;
+#endif
+}
+
 #endif /* _ASM_TIME_H */
Index: linux-3.12.19-rt30/arch/mips/kernel/csrc-r4k.c
===================================================================
--- linux-3.12.19-rt30.orig/arch/mips/kernel/csrc-r4k.c
+++ linux-3.12.19-rt30/arch/mips/kernel/csrc-r4k.c
@ linux-3.12.19-rt30/arch/mips/include/asm/time.h:9 @
  * Copyright (C) 2007 by Ralf Baechle
  */
 #include <linux/clocksource.h>
+#include <linux/cnt32_to_63.h>
 #include <linux/init.h>
 
 #include <asm/time.h>
@ linux-3.12.19-rt30/arch/mips/include/asm/time.h:26 @ static struct clocksource clocksource_mi
 	.flags		= CLOCK_SOURCE_IS_CONTINUOUS,
 };
 
+static inline unsigned long long notrace read_c0_clock(void)
+{
+	u64 cnt = cnt32_to_63(read_c0_count());
+
+	if (cnt & 0x8000000000000000)
+		cnt &= 0x7fffffffffffffff;
+	return cnt;
+}
+
+unsigned long long notrace sched_clock(void)
+{
+	u64 cnt = read_c0_clock();
+
+	return mips_sched_clock(&clocksource_mips, cnt);
+}
+
+static struct timer_list cnt32_to_63_keepwarm_timer;
+
+static void notrace cnt32_to_63_keepwarm(unsigned long data)
+{
+	mod_timer(&cnt32_to_63_keepwarm_timer, round_jiffies(jiffies + data));
+	read_c0_clock();
+}
+
+static inline void setup_hres_sched_clock(unsigned long clock)
+{
+	unsigned long data;
+
+	data = 0xFFFFFFFFUL / clock * HZ;
+	setup_timer(&cnt32_to_63_keepwarm_timer, cnt32_to_63_keepwarm, data);
+	mod_timer(&cnt32_to_63_keepwarm_timer, round_jiffies(jiffies + data));
+}
+
 int __init init_r4k_clocksource(void)
 {
 	if (!cpu_has_counter || !mips_hpt_frequency)
@ linux-3.12.19-rt30/arch/mips/include/asm/time.h:67 @ int __init init_r4k_clocksource(void)
 	/* Calculate a somewhat reasonable rating value */
 	clocksource_mips.rating = 200 + mips_hpt_frequency / 10000000;
 
+	setup_hres_sched_clock(mips_hpt_frequency);
+
 	clocksource_register_hz(&clocksource_mips, mips_hpt_frequency);
 
 	return 0;