[PATCH v5] MIPS: Add a high resolution sched_clock() via cnt32_to_63().

[Wu Zhangjin <wuzhangjin@gmail.com>]

From: Wu Zhangjin <wuzhangjin@gmail.com>

(This v5 revision incorporates with the feedbacks from Ingo.)

This patch adds a cnt32_to_63() and MIPS c0 count based sched_clock(),
which provides high resolution. and also, one new kernel option
(HR_SCHED_CLOCK) is added to enable/disable this sched_clock().

Without it, the Ftrace for MIPS will give useless timestamp information.

Because cnt32_to_63() needs to be called at least once per half period
to work properly, Differ from the old version, this v2 revision set up a
kernel timer to ensure the requirement of some MIPSs which have short c0
count period.

Signed-off-by: Wu Zhangjin <wuzhangjin@gmail.com>
---
 arch/mips/Kconfig                |   18 ++++++++++++
 arch/mips/include/asm/time.h     |   15 ++++++++++
 arch/mips/kernel/Makefile        |    1 +
 arch/mips/kernel/csrc-r4k-hres.c |   54 ++++++++++++++++++++++++++++++++++++++
 arch/mips/kernel/csrc-r4k.c      |    2 +
 5 files changed, 90 insertions(+), 0 deletions(-)
 create mode 100644 arch/mips/kernel/csrc-r4k-hres.c

diff --git a/arch/mips/Kconfig b/arch/mips/Kconfig
index da5d7fd..b3b6334 100644
--- a/arch/mips/Kconfig
+++ b/arch/mips/Kconfig
@@ -1953,6 +1953,24 @@ config NR_CPUS
 source "kernel/time/Kconfig"
 
 #
+# High Resolution sched_clock() Configuration
+#
+
+config HR_SCHED_CLOCK
+       bool "High Resolution sched_clock()"
+       depends on CSRC_R4K
+       default n
+       help
+         This option enables the MIPS c0 count based high resolution
+         sched_clock().
+
+         If you need a ns precision timestamp, you are recommended to enable
+         this option. For example, if you are using the Ftrace subsystem to do
+         real time tracing, this option is needed.
+
+         If unsure, disable it.
+
+#
 # Timer Interrupt Frequency Configuration
 #
 
diff --git a/arch/mips/include/asm/time.h b/arch/mips/include/asm/time.h
index df6a430..a697f7d 100644
--- a/arch/mips/include/asm/time.h
+++ b/arch/mips/include/asm/time.h
@@ -84,6 +84,21 @@ static inline int init_mips_clocksource(void)
 #endif
 }
 
+/*
+ * Setup the high resolution sched_clock()
+ */
+#ifdef CONFIG_HR_SCHED_CLOCK
+extern void setup_r4k_sched_clock(struct clocksource cs, unsigned int clock);
+#endif
+
+static inline void setup_hres_sched_clock(struct clocksource cs,
+               unsigned int clock)
+{
+#ifdef CONFIG_HR_SCHED_CLOCK
+       setup_r4k_sched_clock(cs, clock);
+#endif
+}
+
 extern void clocksource_set_clock(struct clocksource *cs, unsigned int clock);
 extern void clockevent_set_clock(struct clock_event_device *cd,
                unsigned int clock);
diff --git a/arch/mips/kernel/Makefile b/arch/mips/kernel/Makefile
index 9326af5..a6e06c0 100644
--- a/arch/mips/kernel/Makefile
+++ b/arch/mips/kernel/Makefile
@@ -24,6 +24,7 @@ obj-$(CONFIG_CSRC_BCM1480)    += csrc-bcm1480.o
 obj-$(CONFIG_CSRC_IOASIC)      += csrc-ioasic.o
 obj-$(CONFIG_CSRC_POWERTV)     += csrc-powertv.o
 obj-$(CONFIG_CSRC_R4K_LIB)     += csrc-r4k.o
+obj-$(CONFIG_HR_SCHED_CLOCK)   += csrc-r4k-hres.o
 obj-$(CONFIG_CSRC_SB1250)      += csrc-sb1250.o
 obj-$(CONFIG_SYNC_R4K)         += sync-r4k.o
 
diff --git a/arch/mips/kernel/csrc-r4k-hres.c b/arch/mips/kernel/csrc-r4k-hres.c
new file mode 100644
index 0000000..2fe8be7
--- /dev/null
+++ b/arch/mips/kernel/csrc-r4k-hres.c
@@ -0,0 +1,54 @@
+/*
+ * MIPS sched_clock implementation.
+ *
+ * Copyright (C) 2009 Lemote Inc.
+ * Author: Wu Zhangjin, wuzj@lemote.com
+ *
+ * because cnt32_to_63() needs to be called at least once per half period to
+ * work properly, and some of the MIPS frequency is high, perhaps a kernel
+ * timer is needed to be set up to ensure this requirement is always met.
+ * Please refer to arch/arm/plat-orion/time.c and include/linux/cnt32_to_63.h
+ */
+
+#include <linux/clocksource.h>
+#include <linux/cnt32_to_63.h>
+#include <linux/init.h>
+#include <linux/timer.h>
+
+static unsigned long __read_mostly cycle2ns_scale;
+static unsigned long __read_mostly cycle2ns_scale_factor;
+
+unsigned long long notrace sched_clock(void)
+{
+       unsigned long long v = cnt32_to_63(read_c0_count());
+       return (v * cycle2ns_scale) >> cycle2ns_scale_factor;
+}
+
+static struct timer_list cnt32_to_63_keepwarm_timer;
+
+static void cnt32_to_63_keepwarm(unsigned long data)
+{
+       mod_timer(&cnt32_to_63_keepwarm_timer, round_jiffies(jiffies + data));
+       sched_clock();
+}
+
+void setup_r4k_sched_clock(struct clocksource cs, unsigned int clock)
+{
+       unsigned long long v;
+       unsigned long data;
+
+       v = cs.mult;
+       /*
+        * We want an even value to automatically clear the top bit
+        * returned by cnt32_to_63() without an additional run time
+        * instruction. So if the LSB is 1 then round it up.
+        */
+       if (v & 1)
+               v++;
+       cycle2ns_scale = v;
+       cycle2ns_scale_factor = cs.shift;
+
+       data = 0x80000000 / 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));
+}
diff --git a/arch/mips/kernel/csrc-r4k.c b/arch/mips/kernel/csrc-r4k.c
index e95a3cd..3bd89bb 100644
--- a/arch/mips/kernel/csrc-r4k.c
+++ b/arch/mips/kernel/csrc-r4k.c
@@ -32,6 +32,8 @@ int __init init_r4k_clocksource(void)
 
        clocksource_set_clock(&clocksource_mips, mips_hpt_frequency);
 
+       setup_hres_sched_clock(clocksource_mips, mips_hpt_frequency);
+
        clocksource_register(&clocksource_mips);
 
        return 0;
-- 
1.6.2.1