Openwrt/target/linux/gemini/patches-4.1/160-gemini-timers.patch

--- a/arch/arm/mach-gemini/time.c
+++ b/arch/arm/mach-gemini/time.c
@@ -15,15 +15,18 @@
 #include <asm/mach/time.h>
 #include <linux/clockchips.h>
 #include <linux/clocksource.h>
+#include <linux/sched_clock.h>
 
 /*
  * Register definitions for the timers
  */
-#define TIMER_COUNT(BASE_ADDR)		(BASE_ADDR  + 0x00)
-#define TIMER_LOAD(BASE_ADDR)		(BASE_ADDR  + 0x04)
-#define TIMER_MATCH1(BASE_ADDR)		(BASE_ADDR  + 0x08)
-#define TIMER_MATCH2(BASE_ADDR)		(BASE_ADDR  + 0x0C)
-#define TIMER_CR(BASE_ADDR)		(BASE_ADDR  + 0x30)
+#define TIMER_COUNT(BASE_ADDR)		(IO_ADDRESS(BASE_ADDR) + 0x00)
+#define TIMER_LOAD(BASE_ADDR)		(IO_ADDRESS(BASE_ADDR) + 0x04)
+#define TIMER_MATCH1(BASE_ADDR)		(IO_ADDRESS(BASE_ADDR) + 0x08)
+#define TIMER_MATCH2(BASE_ADDR)		(IO_ADDRESS(BASE_ADDR) + 0x0C)
+#define TIMER_CR(BASE_ADDR)		(IO_ADDRESS(BASE_ADDR) + 0x30)
+#define TIMER_INTR_STATE(BASE_ADDR)	(IO_ADDRESS(BASE_ADDR) + 0x34)
+#define TIMER_INTR_MASK(BASE_ADDR)	(IO_ADDRESS(BASE_ADDR) + 0x38)
 
 #define TIMER_1_CR_ENABLE		(1 << 0)
 #define TIMER_1_CR_CLOCK		(1 << 1)
@@ -34,27 +37,38 @@
 #define TIMER_3_CR_ENABLE		(1 << 6)
 #define TIMER_3_CR_CLOCK		(1 << 7)
 #define TIMER_3_CR_INT			(1 << 8)
+#define TIMER_1_CR_UPDOWN		(1 << 9)
+#define TIMER_2_CR_UPDOWN		(1 << 10)
+#define TIMER_3_CR_UPDOWN		(1 << 11)
+
+#define TIMER_1_INT_MATCH1		(1 << 0)
+#define TIMER_1_INT_MATCH2		(1 << 1)
+#define TIMER_1_INT_OVERFLOW		(1 << 2)
+#define TIMER_2_INT_MATCH1		(1 << 3)
+#define TIMER_2_INT_MATCH2		(1 << 4)
+#define TIMER_2_INT_OVERFLOW		(1 << 5)
+#define TIMER_3_INT_MATCH1		(1 << 6)
+#define TIMER_3_INT_MATCH2		(1 << 7)
+#define TIMER_3_INT_OVERFLOW		(1 << 8)
+#define TIMER_INT_ALL_MASK		0x1ff
 
 static unsigned int tick_rate;
 
+static u64 notrace gemini_read_sched_clock(void)
+{
+	return readl(TIMER_COUNT(GEMINI_TIMER3_BASE));
+}
+
 static int gemini_timer_set_next_event(unsigned long cycles,
 				       struct clock_event_device *evt)
 {
 	u32 cr;
 
-	cr = readl(TIMER_CR(IO_ADDRESS(GEMINI_TIMER_BASE)));
-
-	/* This may be overdoing it, feel free to test without this */
-	cr &= ~TIMER_2_CR_ENABLE;
-	cr &= ~TIMER_2_CR_INT;
-	writel(cr, TIMER_CR(IO_ADDRESS(GEMINI_TIMER_BASE)));
-
-	/* Set next event */
-	writel(cycles, TIMER_COUNT(IO_ADDRESS(GEMINI_TIMER2_BASE)));
-	writel(cycles, TIMER_LOAD(IO_ADDRESS(GEMINI_TIMER2_BASE)));
-	cr |= TIMER_2_CR_ENABLE;
-	cr |= TIMER_2_CR_INT;
-	writel(cr, TIMER_CR(IO_ADDRESS(GEMINI_TIMER_BASE)));
+	/* Setup the match register */
+	cr = readl(TIMER_COUNT(GEMINI_TIMER1_BASE));
+	writel(cr + cycles, TIMER_MATCH1(GEMINI_TIMER1_BASE));
+	if (readl(TIMER_COUNT(GEMINI_TIMER1_BASE)) - cr > cycles)
+		return -ETIME;
 
 	return 0;
 }
@@ -66,48 +80,68 @@ static void gemini_timer_set_mode(enum c
 	u32 cr;
 
 	switch (mode) {
-        case CLOCK_EVT_MODE_PERIODIC:
-		/* Start the timer */
-		writel(period,
-		       TIMER_COUNT(IO_ADDRESS(GEMINI_TIMER2_BASE)));
-		writel(period,
-		       TIMER_LOAD(IO_ADDRESS(GEMINI_TIMER2_BASE)));
-		cr = readl(TIMER_CR(IO_ADDRESS(GEMINI_TIMER_BASE)));
-		cr |= TIMER_2_CR_ENABLE;
-		cr |= TIMER_2_CR_INT;
-		writel(cr, TIMER_CR(IO_ADDRESS(GEMINI_TIMER_BASE)));
+	case CLOCK_EVT_MODE_PERIODIC:
+		/* Stop timer and interrupt. */
+		cr = readl(TIMER_CR(GEMINI_TIMER_BASE));
+		cr &= ~(TIMER_1_CR_ENABLE | TIMER_1_CR_INT);
+		writel(cr, TIMER_CR(GEMINI_TIMER_BASE));
+
+		/* Setup timer to fire at 1/HZ intervals. */
+		cr = 0xffffffff - (period - 1);
+		writel(cr, TIMER_COUNT(GEMINI_TIMER1_BASE));
+		writel(cr, TIMER_LOAD(GEMINI_TIMER1_BASE));
+
+		/* enable interrupt on overflaw */
+		cr = readl(TIMER_INTR_MASK(GEMINI_TIMER_BASE));
+		cr &= ~(TIMER_1_INT_MATCH1 | TIMER_1_INT_MATCH2);
+		cr |= TIMER_1_INT_OVERFLOW;
+		writel(cr, TIMER_INTR_MASK(GEMINI_TIMER_BASE));
+
+		/* start the timer */
+		cr = readl(TIMER_CR(GEMINI_TIMER_BASE));
+		cr |= TIMER_1_CR_ENABLE | TIMER_1_CR_INT;
+		writel(cr, TIMER_CR(GEMINI_TIMER_BASE));
 		break;
+
 	case CLOCK_EVT_MODE_ONESHOT:
 	case CLOCK_EVT_MODE_UNUSED:
-        case CLOCK_EVT_MODE_SHUTDOWN:
+	case CLOCK_EVT_MODE_SHUTDOWN:
+		/* Stop timer and interrupt. */
+		cr = readl(TIMER_CR(GEMINI_TIMER_BASE));
+		cr &= ~(TIMER_1_CR_ENABLE | TIMER_1_CR_INT);
+		writel(cr, TIMER_CR(GEMINI_TIMER_BASE));
+
+		/* Setup counter start from 0 */
+		writel(0, TIMER_COUNT(GEMINI_TIMER1_BASE));
+		writel(0, TIMER_LOAD(GEMINI_TIMER1_BASE));
+
+		/* enable interrupt */
+		cr = readl(TIMER_INTR_MASK(GEMINI_TIMER_BASE));
+		cr &= ~(TIMER_1_INT_OVERFLOW | TIMER_1_INT_MATCH2);
+		cr |= TIMER_1_INT_MATCH1;
+		writel(cr, TIMER_INTR_MASK(GEMINI_TIMER_BASE));
+
+		/* start the timer */
+		cr = readl(TIMER_CR(GEMINI_TIMER_BASE));
+		cr |= TIMER_1_CR_ENABLE;
+		writel(cr, TIMER_CR(GEMINI_TIMER_BASE));
+		break;
+
 	case CLOCK_EVT_MODE_RESUME:
-		/*
-		 * Disable also for oneshot: the set_next() call will
-		 * arm the timer instead.
-		 */
-		cr = readl(TIMER_CR(IO_ADDRESS(GEMINI_TIMER_BASE)));
-		cr &= ~TIMER_2_CR_ENABLE;
-		cr &= ~TIMER_2_CR_INT;
-		writel(cr, TIMER_CR(IO_ADDRESS(GEMINI_TIMER_BASE)));
 		break;
-	default:
-                break;
 	}
 }
 
-/* Use TIMER2 as clock event */
 static struct clock_event_device gemini_clockevent = {
-	.name		= "TIMER2",
-	.rating		= 300, /* Reasonably fast and accurate clock event */
-	.features	= CLOCK_EVT_FEAT_PERIODIC | CLOCK_EVT_FEAT_ONESHOT,
-	.set_next_event	= gemini_timer_set_next_event,
-	.set_mode	= gemini_timer_set_mode,
+	.name           = "gemini_timer_1",
+	.features       = CLOCK_EVT_FEAT_PERIODIC | CLOCK_EVT_FEAT_ONESHOT,
+	.shift          = 32,
+	.rating         = 300,
+	.set_next_event = gemini_timer_set_next_event,
+	.set_mode       = gemini_timer_set_mode,
 };
 
-/*
- * IRQ handler for the timer
- */
-static irqreturn_t gemini_timer_interrupt(int irq, void *dev_id)
+static irqreturn_t gemini_timer_intr(int irq, void *dev_id)
 {
 	struct clock_event_device *evt = &gemini_clockevent;
 
@@ -116,14 +150,11 @@ static irqreturn_t gemini_timer_interrup
 }
 
 static struct irqaction gemini_timer_irq = {
-	.name		= "Gemini Timer Tick",
+	.name		= "gemini timer 1",
 	.flags		= IRQF_TIMER,
-	.handler	= gemini_timer_interrupt,
+	.handler	= gemini_timer_intr,
 };
 
-/*
- * Set up timer interrupt, and return the current time in seconds.
- */
 void __init gemini_timer_init(void)
 {
 	u32 reg_v;
@@ -151,20 +182,35 @@ void __init gemini_timer_init(void)
 	}
 
 	/*
-	 * Make irqs happen for the system timer
+	 * Reset the interrupt mask and status
 	 */
-	setup_irq(IRQ_TIMER2, &gemini_timer_irq);
+	writel(TIMER_INT_ALL_MASK, TIMER_INTR_MASK(GEMINI_TIMER_BASE));
+	writel(0, TIMER_INTR_STATE(GEMINI_TIMER_BASE));
+	writel(TIMER_1_CR_UPDOWN | TIMER_3_CR_ENABLE | TIMER_3_CR_UPDOWN,
+		TIMER_CR(GEMINI_TIMER_BASE));
 
-	/* Enable and use TIMER1 as clock source */
-	writel(0xffffffff, TIMER_COUNT(IO_ADDRESS(GEMINI_TIMER1_BASE)));
-	writel(0xffffffff, TIMER_LOAD(IO_ADDRESS(GEMINI_TIMER1_BASE)));
-	writel(TIMER_1_CR_ENABLE, TIMER_CR(IO_ADDRESS(GEMINI_TIMER_BASE)));
-	if (clocksource_mmio_init(TIMER_COUNT(IO_ADDRESS(GEMINI_TIMER1_BASE)),
-				  "TIMER1", tick_rate, 300, 32,
-				  clocksource_mmio_readl_up))
-		pr_err("timer: failed to initialize gemini clock source\n");
+	/*
+	 * Setup free-running clocksource timer (interrupts
+	 * disabled.)
+	 */
+	writel(0, TIMER_COUNT(GEMINI_TIMER3_BASE));
+	writel(0, TIMER_LOAD(GEMINI_TIMER3_BASE));
+	writel(0, TIMER_MATCH1(GEMINI_TIMER3_BASE));
+	writel(0, TIMER_MATCH2(GEMINI_TIMER3_BASE));
+	clocksource_mmio_init(TIMER_COUNT(GEMINI_TIMER3_BASE),
+			"gemini_clocksource", tick_rate,
+			300, 32, clocksource_mmio_readl_up);
+	sched_clock_register(gemini_read_sched_clock, 32, tick_rate);
 
-	/* Configure and register the clockevent */
+	/*
+	 * Setup clockevent timer (interrupt-driven.)
+	 */
+	writel(0, TIMER_COUNT(GEMINI_TIMER1_BASE));
+	writel(0, TIMER_LOAD(GEMINI_TIMER1_BASE));
+	writel(0, TIMER_MATCH1(GEMINI_TIMER1_BASE));
+	writel(0, TIMER_MATCH2(GEMINI_TIMER1_BASE));
+	setup_irq(IRQ_TIMER1, &gemini_timer_irq);
+	gemini_clockevent.cpumask = cpumask_of(0);
 	clockevents_config_and_register(&gemini_clockevent, tick_rate,
 					1, 0xffffffff);
 }