From d11b711b3ed6b4a053a298cf4e9f1209748fae5e Mon Sep 17 00:00:00 2001
From: Nick Withers <nick.withers@anu.edu.au>
Date: Wed, 28 Jan 2015 15:04:26 +1100
Subject: bsps/powerpc: Fix a clock driver

PowerPC Book E: Account for an extra tick period if a tick increment's
pending.

Close #2230.
---
 c/src/lib/libcpu/powerpc/mpc6xx/clock/c_clock.c | 66 +++++++++++++++++++++----
 1 file changed, 57 insertions(+), 9 deletions(-)

(limited to 'c/src/lib/libcpu/powerpc/mpc6xx/clock/c_clock.c')

diff --git a/c/src/lib/libcpu/powerpc/mpc6xx/clock/c_clock.c b/c/src/lib/libcpu/powerpc/mpc6xx/clock/c_clock.c
index f14acab1d8..218828cf4a 100644
--- a/c/src/lib/libcpu/powerpc/mpc6xx/clock/c_clock.c
+++ b/c/src/lib/libcpu/powerpc/mpc6xx/clock/c_clock.c
@@ -46,7 +46,13 @@ volatile uint32_t   Clock_driver_ticks;
 /*
  *  This is the value programmed into the count down timer.
  */
-uint32_t   Clock_Decrementer_value;
+static uint32_t   Clock_Decrementer_value;
+
+/*
+ *  This is the value by which elapsed count down timer ticks are multiplied to
+ *  give an elapsed duration in nanoseconds, left-shifted by 32 bits
+ */
+static uint64_t   Clock_Decrementer_reference;
 
 void clockOff(void* unused)
 {
@@ -209,6 +215,37 @@ static uint32_t Clock_driver_nanoseconds_since_last_tick(void)
   return tmp * 1000;
 }
 
+static uint32_t Clock_driver_nanoseconds_since_last_tick_bookE(void)
+{
+  uint32_t clicks;
+  uint64_t c;
+
+  PPC_Get_decrementer( clicks );
+  c = Clock_Decrementer_value - clicks;
+
+  /*
+   * Check whether a clock tick interrupt is pending and hence that the
+   * decrementer's wrapped. If it has, we'll compensate by returning a time one
+   * tick period longer.
+   *
+   * We have to check interrupt status after reading the decrementer. If we
+   * don't, we may miss an interrupt and read a wrapped decrementer value
+   * without compensating for it
+   */
+  if ( _read_BOOKE_TSR() & BOOKE_TSR_DIS )
+  {
+    /*
+     * Re-read the decrementer: The tick interrupt may have been
+     * generated and the decrementer wrapped during the time since we
+     * last read it and the time we checked the interrupt status
+     */
+    PPC_Get_decrementer( clicks );
+    c = (Clock_Decrementer_value - clicks) + Clock_Decrementer_value;
+  }
+
+  return (uint32_t)((c * Clock_Decrementer_reference) >> 32);
+}
+
 /*
  *  Clock_initialize
  *
@@ -223,7 +260,10 @@ rtems_device_driver Clock_initialize(
   rtems_interrupt_level l,tcr;
 
   Clock_Decrementer_value = (BSP_bus_frequency/BSP_time_base_divisor)*
-            (rtems_configuration_get_microseconds_per_tick()/1000);
+            rtems_configuration_get_milliseconds_per_tick();
+
+  Clock_Decrementer_reference = ((uint64_t)1000000U<<32)/
+            (BSP_bus_frequency/BSP_time_base_divisor);
 
   /* set the decrementer now, prior to installing the handler
    * so no interrupts will happen in a while.
@@ -244,14 +284,22 @@ rtems_device_driver Clock_initialize(
 
     rtems_interrupt_enable(l);
 
+    /*
+     *  Set the nanoseconds since last tick handler
+     */
+    rtems_clock_set_nanoseconds_extension(
+      Clock_driver_nanoseconds_since_last_tick_bookE
+    );
+  }
+  else
+  {
+    /*
+     *  Set the nanoseconds since last tick handler
+     */
+    rtems_clock_set_nanoseconds_extension(
+      Clock_driver_nanoseconds_since_last_tick
+    );
   }
-
-  /*
-   *  Set the nanoseconds since last tick handler
-   */
-  rtems_clock_set_nanoseconds_extension(
-    Clock_driver_nanoseconds_since_last_tick
-  );
 
   /*
    * If a decrementer exception was pending, it is cleared by
-- 
cgit v1.2.3