1 files changed, 53 insertions, 40 deletions

diff --git a/bsps/sparc/leon2/clock/ckinit.c b/bsps/sparc/leon2/clock/ckinit.c
index f622975832..2707c7daa2 100644
--- a/bsps/sparc/leon2/clock/ckinit.c
+++ b/bsps/sparc/leon2/clock/ckinit.c
@@ -24,42 +24,73 @@
 
 #include <bsp.h>
 #include <bspopts.h>
+#include <rtems/sysinit.h>
 #include <rtems/timecounter.h>
 #include <rtems/score/sparcimpl.h>
 
-static rtems_timecounter_simple leon2_tc;
+extern int CLOCK_SPEED;
+
+#define LEON2_TIMER_1_FREQUENCY 1000000
+
+static struct timecounter leon2_tc;
 
-static uint32_t leon2_tc_get( rtems_timecounter_simple *tc )
+static void leon2_clock_init( void )
 {
-  return LEON_REG.Timer_Counter_1;
+  struct timecounter *tc;
+
+  tc = &leon2_tc;
+  tc->tc_get_timecount = _SPARC_Get_timecount_clock;
+  tc->tc_counter_mask = 0xffffffff;
+  tc->tc_frequency = LEON2_TIMER_1_FREQUENCY;
+  tc->tc_quality = RTEMS_TIMECOUNTER_QUALITY_CLOCK_DRIVER;
+  rtems_timecounter_install(tc);
 }
 
-static bool leon2_tc_is_pending( rtems_timecounter_simple *tc )
+static void leon2_clock_at_tick( void )
 {
-  return LEON_Is_interrupt_pending( LEON_INTERRUPT_TIMER1 );
+  SPARC_Counter *counter;
+  rtems_interrupt_level level;
+
+  counter = &_SPARC_Counter_mutable;
+  rtems_interrupt_local_disable(level);
+
+  LEON_Clear_interrupt( LEON_INTERRUPT_TIMER1 );
+  counter->accumulated += counter->interval;
+
+  rtems_interrupt_local_enable(level);
 }
 
-static uint32_t leon2_tc_get_timecount( struct timecounter *tc )
+static void leon2_clock_initialize_early( void )
 {
-  return rtems_timecounter_simple_downcounter_get(
-    tc,
-    leon2_tc_get,
-    leon2_tc_is_pending
+  SPARC_Counter *counter;
+
+  LEON_REG.Timer_Reload_1 =
+    rtems_configuration_get_microseconds_per_tick() - 1;
+  LEON_REG.Timer_Control_1 = (
+    LEON_REG_TIMER_COUNTER_ENABLE_COUNTING |
+      LEON_REG_TIMER_COUNTER_RELOAD_AT_ZERO |
+      LEON_REG_TIMER_COUNTER_LOAD_COUNTER
   );
-}
 
-static void leon2_tc_at_tick( rtems_timecounter_simple *tc )
-{
-  /* Nothing to do */
+  counter = &_SPARC_Counter_mutable;
+  counter->read_isr_disabled = _SPARC_Counter_read_clock_isr_disabled;
+  counter->read = _SPARC_Counter_read_clock;
+  counter->counter_register = &LEON_REG.Timer_Counter_1;
+  counter->pending_register = &LEON_REG.Interrupt_Pending;
+  counter->pending_mask = UINT32_C(1) << LEON_INTERRUPT_TIMER1;
+  counter->accumulated = rtems_configuration_get_microseconds_per_tick();
+  counter->interval = rtems_configuration_get_microseconds_per_tick();
 }
 
-static void leon2_tc_tick( void )
+RTEMS_SYSINIT_ITEM(
+  leon2_clock_initialize_early,
+  RTEMS_SYSINIT_CPU_COUNTER,
+  RTEMS_SYSINIT_ORDER_FIRST
+);
+
+uint32_t _CPU_Counter_frequency(void)
 {
-  rtems_timecounter_simple_downcounter_tick(
-    &leon2_tc,
-    leon2_tc_get,
-    leon2_tc_at_tick
-  );
+  return LEON2_TIMER_1_FREQUENCY;
 }
 
 /*
@@ -71,27 +102,9 @@ static void leon2_tc_tick( void )
 #define Clock_driver_support_install_isr( _new ) \
   set_vector( _new, CLOCK_VECTOR, 1 )
 
-extern int CLOCK_SPEED;
+#define Clock_driver_support_at_tick() leon2_clock_at_tick()
 
-#define Clock_driver_support_initialize_hardware() \
-  do { \
-    LEON_REG.Timer_Reload_1 = \
-        rtems_configuration_get_microseconds_per_tick() - 1; \
-    \
-    LEON_REG.Timer_Control_1 = ( \
-      LEON_REG_TIMER_COUNTER_ENABLE_COUNTING |  \
-        LEON_REG_TIMER_COUNTER_RELOAD_AT_ZERO | \
-        LEON_REG_TIMER_COUNTER_LOAD_COUNTER  \
-    ); \
-    rtems_timecounter_simple_install( \
-      &leon2_tc, \
-      1000000, \
-      rtems_configuration_get_microseconds_per_tick(), \
-      leon2_tc_get_timecount \
-    ); \
-  } while (0)
-
-#define Clock_driver_timecounter_tick() leon2_tc_tick()
+#define Clock_driver_support_initialize_hardware() leon2_clock_init()
 
 #include "../../../shared/dev/clock/clockimpl.h"