Use the clock count register as a timecounter, as it's more accurate.

1 files changed, 13 insertions, 16 deletions

diff --git a/sys/arm/xscale/i80321/i80321_timer.c b/sys/arm/xscale/i80321/i80321_timer.c
index 3747381..66d34ef 100644
--- a/sys/arm/xscale/i80321/i80321_timer.c
+++ b/sys/arm/xscale/i80321/i80321_timer.c
@@ -72,8 +72,6 @@ static unsigned i80321_timer_get_timecount(struct timecounter *tc);
 
 static uint32_t counts_per_hz;
 
-static uint32_t offset = 0;
-static uint32_t last = -1;
 static int ticked = 0;
 
 #define	COUNTS_PER_SEC		200000000	/* 200MHz */
@@ -83,7 +81,7 @@ static struct timecounter i80321_timer_timecounter = {
 	i80321_timer_get_timecount, /* get_timecount */
 	NULL,			    /* no poll_pps */
 	~0u,			    /* counter_mask */
-	COUNTS_PER_SEC,	 	   /* frequency */
+	COUNTS_PER_SEC * 3,	 	   /* frequency */
 	"i80321 timer",		    /* name */
 	1000			    /* quality */
 };
@@ -236,19 +234,11 @@ tisr_read(void)
 static unsigned
 i80321_timer_get_timecount(struct timecounter *tc)
 {
-	uint32_t cur = tcr0_read();
-	
-	if (cur > last && last != -1) {
-		offset += counts_per_hz;
-		if (ticked > 0)
-									                        ticked--;
-	}
-	if (ticked) {
-		offset += ticked * counts_per_hz;
-		ticked = 0;
-	}
-	last = cur;
-	return (counts_per_hz - cur + offset);
+	uint32_t ret;
+
+	__asm __volatile("mrc p14, 0, %0, c1, c0, 0\n"
+	    : "=r" (ret));
+	return (ret);
 }
 
 /*
@@ -329,6 +319,13 @@ cpu_initclocks(void)
 
 	tc_init(&i80321_timer_timecounter);
 	restore_interrupts(oldirqstate);
+	rid = 0;
+	/* Enable the clock count register. */
+	__asm __volatile("mrc p14, 0, %0, c0, c0, 0\n" : "=r" (rid));
+	rid &= ~(1 <<  3);
+	rid |= (1 << 2) | 1;
+	__asm __volatile("mcr p14, 0, %0, c0, c0, 0\n"
+	    : : "r" (rid));
 }