From faaf81e30b0e38025430f7fda67044e64146fb84 Mon Sep 17 00:00:00 2001
From: Jan Kiszka <jan.kiszka@siemens.com>
Date: Mon, 15 Dec 2014 18:02:49 +0100
Subject: [PATCH] inmates: arm: Enhance gic-demo with latency statistics

Original version by Johann Pfefferl: This transfers the apic-demo to
ARM by letting the timer tick at 10 Hz and print jitter statistics on
each event. In addition, this also lets the green LED on the Banana Pi
blink.

CC: Johann Pfefferl <johann.pfefferl@siemens.com>
Signed-off-by: Jan Kiszka <jan.kiszka@siemens.com>
---
 hypervisor/arch/arm/include/asm/sysregs.h |  2 +
 inmates/demos/arm/gic-demo.c              | 68 ++++++++++++++++++-----
 2 files changed, 55 insertions(+), 15 deletions(-)

diff --git a/hypervisor/arch/arm/include/asm/sysregs.h b/hypervisor/arch/arm/include/asm/sysregs.h
index 3cdd634..d1f63b3 100644
--- a/hypervisor/arch/arm/include/asm/sysregs.h
+++ b/hypervisor/arch/arm/include/asm/sysregs.h
@@ -61,6 +61,8 @@
 #define CNTV_CTL_EL0	SYSREG_32(0, c14, c3, 1)
 #define CNTV_CVAL_EL0	SYSREG_64(3, c14)
 
+#define CNTPCT		SYSREG_64(0, c14)
+
 /*
  * AArch32-specific registers: they are 64bit on AArch64, and will need some
  * helpers if used frequently.
diff --git a/inmates/demos/arm/gic-demo.c b/inmates/demos/arm/gic-demo.c
index 7a0f37f..823f69d 100644
--- a/inmates/demos/arm/gic-demo.c
+++ b/inmates/demos/arm/gic-demo.c
@@ -15,31 +15,67 @@
 #include <inmates/inmate.h>
 #include <mach/timer.h>
 
-static u64 tval;
-static u64 jiffies = 0;
+#define BEATS_PER_SEC		10
+#define TICKS_PER_BEAT		(TIMER_FREQ / BEATS_PER_SEC)
 
-static void timer_arm(void)
+static volatile u64 expected_ticks;
+
+static void timer_arm(u64 timeout)
 {
-	arm_write_sysreg(CNTV_TVAL_EL0, tval);
+	arm_write_sysreg(CNTV_TVAL_EL0, timeout);
 	arm_write_sysreg(CNTV_CTL_EL0, 1);
 }
 
-static int timer_init(void)
+static u64 get_actual_ticks(void)
 {
-	u32 freq = TIMER_FREQ;
+	u64 pct64;
+
+	arm_read_sysreg(CNTPCT, pct64);
+	return pct64;
+}
 
-	tval = freq / 1000;
-	timer_arm();
+static unsigned long emul_division(u64 val, u64 div)
+{
+	unsigned long cnt = 0;
+
+	while (val > div) {
+		val -= div;
+		cnt++;
+	}
+	return cnt;
+}
 
-	return 0;
+static inline unsigned long ticks_to_ns(u64 ticks)
+{
+	return emul_division(ticks * 1000, TIMER_FREQ / 1000 / 1000);
 }
 
 static void handle_IRQ(unsigned int irqn)
 {
-	if (irqn == TIMER_IRQ) {
-		printk("J=%d\n", (u32)jiffies++);
-		timer_arm();
-	}
+	static u64 min_delta = ~0ULL, max_delta = 0;
+	u64 delta;
+
+	if (irqn != TIMER_IRQ)
+		return;
+
+	delta = get_actual_ticks() - expected_ticks;
+	if (delta < min_delta)
+		min_delta = delta;
+	if (delta > max_delta)
+		max_delta = delta;
+
+	printk("Timer fired, jitter: %6ld ns, min: %6ld ns, max: %6ld ns\n",
+	       ticks_to_ns(delta), ticks_to_ns(min_delta),
+	       ticks_to_ns(max_delta));
+
+#ifdef CONFIG_ARCH_SUN7I
+	/* let green LED on Banana Pi blink */
+	#define LED_REG (void *)(0x1c20800 + 7*0x24 + 0x10)
+	mmio_write32(LED_REG, mmio_read32(LED_REG) ^ (1<<24));
+#endif
+
+	expected_ticks = get_actual_ticks() + TICKS_PER_BEAT;
+	timer_arm(TICKS_PER_BEAT);
 }
 
 void inmate_main(void)
@@ -49,7 +85,9 @@ void inmate_main(void)
 	gic_enable_irq(TIMER_IRQ);
 
 	printk("Initializing the timer...\n");
-	timer_init();
+	expected_ticks = get_actual_ticks() + TICKS_PER_BEAT;
+	timer_arm(TICKS_PER_BEAT);
 
-	while (1);
+	while (1)
+		asm volatile("wfi" : : : "memory");
 }
-- 
2.39.2