[PATCH 4/4] bsp clock configs

[Alexander Krutwig]

---
 c/src/lib/libbsp/shared/clockdrv_shell.h    | 31 ++++++++++++-----
 c/src/lib/libbsp/sparc/erc32/clock/ckinit.c | 54 +++++++++++++++++++++++------
 2 files changed, 66 insertions(+), 19 deletions(-)

diff --git a/c/src/lib/libbsp/shared/clockdrv_shell.h b/c/src/lib/libbsp/shared/clockdrv_shell.h
index 5dbea13..cf3237d 100644
--- a/c/src/lib/libbsp/shared/clockdrv_shell.h
+++ b/c/src/lib/libbsp/shared/clockdrv_shell.h
@@ -20,6 +20,10 @@
 #include <bsp.h>
 #include <rtems/clockdrv.h>
 
+#ifdef Clock_driver_nanoseconds_since_last_tick
+#error "adjust your clock driver"
+#endif
+
 /**
  * @defgroup bsp_clock Clock Support
  *
@@ -39,6 +43,14 @@
   #define Clock_driver_support_find_timer()
 #endif
 
+/*
+ * A specialized clock driver may use for example _Timecounter_Tick_simple()
+ * instead of the default.
+ */
+#ifndef Clock_driver_timecounter_tick
+  Clock_driver_timecounter_tick() _Timecounter_Tick()
+#endif
+
 /**
  * @brief ISRs until next clock tick
  */
@@ -53,6 +65,12 @@ volatile uint32_t    Clock_driver_ticks;
 
 void Clock_exit( void );
 
+static void Clock_tick( void )
+{
+  Clock_driver_timecounter_tick();
+  rtems_clock_tick();
+}
+
 /**
  *  @brief Clock_isr
  *
@@ -79,7 +97,7 @@ rtems_isr Clock_isr(
 
   #if CLOCK_DRIVER_USE_FAST_IDLE
     do {
-      rtems_clock_tick();
+      Clock_tick();
     } while (
       _Thread_Heir == _Thread_Executing
         && _Thread_Executing->Start.entry_point
@@ -101,7 +119,7 @@ rtems_isr Clock_isr(
        *  The driver is multiple ISRs per clock tick.
        */
       if ( !Clock_driver_isrs ) {
-        rtems_clock_tick();
+        Clock_tick();
 
         Clock_driver_isrs = CLOCK_DRIVER_ISRS_PER_TICK;
       }
@@ -110,7 +128,7 @@ rtems_isr Clock_isr(
       /*
        *  The driver is one ISR per clock tick.
        */
-      rtems_clock_tick();
+      Clock_tick();
     #endif
   #endif
 }
@@ -149,6 +167,7 @@ rtems_device_driver Clock_initialize(
 
   Clock_driver_ticks = 0;
 
+
   /*
    *  Find timer -- some BSPs search buses for hardware timer
    */
@@ -160,12 +179,6 @@ rtems_device_driver Clock_initialize(
   (void) Old_ticker;
   Clock_driver_support_install_isr( Clock_isr, Old_ticker );
 
-  #if defined(Clock_driver_nanoseconds_since_last_tick)
-    rtems_clock_set_nanoseconds_extension(
-      Clock_driver_nanoseconds_since_last_tick
-    );
-  #endif
-
   /*
    *  Now initialize the hardware that is the source of the tick ISR.
    */
diff --git a/c/src/lib/libbsp/sparc/erc32/clock/ckinit.c b/c/src/lib/libbsp/sparc/erc32/clock/ckinit.c
index 2afe770..1fb186c 100644
--- a/c/src/lib/libbsp/sparc/erc32/clock/ckinit.c
+++ b/c/src/lib/libbsp/sparc/erc32/clock/ckinit.c
@@ -25,6 +25,7 @@
 #include <bsp.h>
 #include <bspopts.h>
 #include <rtems/counter.h>
+#include <rtems/score/timecounterimpl.h>
 
 #if SIMSPARC_FAST_IDLE==1
 #define CLOCK_DRIVER_USE_FAST_IDLE 1
@@ -44,25 +45,33 @@
 
 extern int CLOCK_SPEED;
 
-static uint32_t bsp_clock_nanoseconds_since_last_tick(void)
+static struct timecounter erc32_tc;
+
+static uint64_t erc32_tc_scaler;
+
+static uint32_t erc32_tc_interval;
+
+static uint32_t erc32_tc_current(void)
+{
+  return (ERC32_MEC.Real_Time_Clock_Counter * erc32_tc_scaler) >> 32;
+}
+
+static u_int erc32_get_timecount(struct timecounter *tc)
 {
   uint32_t clicks;
-  uint32_t usecs;
+  uint32_t interval;
 
-  clicks = ERC32_MEC.Real_Time_Clock_Counter;
+  interval = erc32_tc_interval;
+  clicks = erc32_tc_current();
 
   if ( ERC32_Is_interrupt_pending( ERC32_INTERRUPT_REAL_TIME_CLOCK ) ) {
-    clicks = ERC32_MEC.Real_Time_Clock_Counter;
-    usecs = (2*rtems_configuration_get_microseconds_per_tick() - clicks);
+    clicks = erc32_tc_current();
+    return (2 * interval - clicks);
   } else {
-    usecs = (rtems_configuration_get_microseconds_per_tick() - clicks);
+    return (interval - clicks);
   }
-  return usecs * 1000;
 }
 
-#define Clock_driver_nanoseconds_since_last_tick \
-  bsp_clock_nanoseconds_since_last_tick
-
 static CPU_Counter_ticks erc32_counter_difference(
   CPU_Counter_ticks second,
   CPU_Counter_ticks first
@@ -75,6 +84,7 @@ static CPU_Counter_ticks erc32_counter_difference(
 
 #define Clock_driver_support_initialize_hardware() \
   do { \
+    uint32_t mask; \
     /* approximately 1 us per countdown */ \
     ERC32_MEC.Real_Time_Clock_Scalar  = CLOCK_SPEED - 1; \
     ERC32_MEC.Real_Time_Clock_Counter = \
@@ -89,7 +99,18 @@ static CPU_Counter_ticks erc32_counter_difference(
     ERC32_MEC_Set_Real_Time_Clock_Timer_Control( \
         ERC32_MEC_TIMER_COUNTER_ENABLE_COUNTING | \
         ERC32_MEC_TIMER_COUNTER_RELOAD_AT_ZERO \
+    );  \
+    _Timecounter_Get_binary_scaler_and_mask( \
+        rtems_configuration_get_microseconds_per_tick(), \
+        &erc32_tc_scaler, \
+        &mask \
     ); \
+    erc32_tc_interval = (mask + 1) / 2; \
+    erc32_tc.tc_get_timecount = erc32_get_timecount; \
+    erc32_tc.tc_counter_mask = mask; \
+    erc32_tc.tc_frequency = (1000000 * erc32_tc_scaler) >> 32; \
+    erc32_tc.tc_quality = TIMECOUNTER_DEFAULT_DRIVER_QUALITY; \
+    _Timecounter_Install(&erc32_tc); \
     _SPARC_Counter_initialize( \
       &ERC32_MEC.Real_Time_Clock_Counter, \
       erc32_counter_difference \
@@ -97,6 +118,19 @@ static CPU_Counter_ticks erc32_counter_difference(
     rtems_counter_initialize_converter(1000000); \
   } while (0)
 
+static void erc32_tc_tick(void)
+{
+  uint32_t clicks;
+  uint32_t interval;
+
+  interval = erc32_tc_interval;
+  clicks = interval - erc32_tc_current();
+
+  _Timecounter_Tick_simple(interval + clicks, clicks);
+}
+
+#define Clock_driver_timecounter_tick() erc32_tc_tick()
+
 #define Clock_driver_support_shutdown_hardware() \
   do { \
     ERC32_Mask_interrupt( ERC32_INTERRUPT_REAL_TIME_CLOCK ); \
-- 
1.8.4.5