Simplify _CPU_Counter_difference()

In order to simplify the use of CPU counter values it is beneficial to
have monotonic increasing values within the range of the CPU counter
ticks data type, e.g. 32-bit unsigned integer.  This eases the use of
CPU counter timestamps in external tools which do not know the details
of the CPU counter hardware.  The CPU counter is the fastest way to get
a time on an RTEMS system.

Such a CPU counter may be also used as the timecounter.  Use it on SPARC
for this purpose to simplify the clock drivers.

Update #3456.

5 files changed, 182 insertions, 213 deletions

diff --git a/bsps/sparc/erc32/clock/ckinit.c b/bsps/sparc/erc32/clock/ckinit.c
index e7c5b07f33..6d6a335fed 100644
--- a/bsps/sparc/erc32/clock/ckinit.c
+++ b/bsps/sparc/erc32/clock/ckinit.c
@@ -24,103 +24,97 @@
 
 #include <bsp.h>
 #include <bspopts.h>
-#include <rtems/counter.h>
+#include <rtems/sysinit.h>
 #include <rtems/timecounter.h>
 #include <rtems/score/sparcimpl.h>
 
-#define ERC32_REAL_TIME_CLOCK_FREQUENCY 1000000
-
-/*
- *  The Real Time Clock Counter Timer uses this trap type.
- */
-#define CLOCK_VECTOR ERC32_TRAP_TYPE( ERC32_INTERRUPT_REAL_TIME_CLOCK )
-
-#define Clock_driver_support_install_isr( _new ) \
-  set_vector( _new, CLOCK_VECTOR, 1 )
-
-#define Clock_driver_support_set_interrupt_affinity( _online_processors ) \
-  do { \
-    (void) _online_processors; \
-  } while (0)
-
 extern int CLOCK_SPEED;
 
-static rtems_timecounter_simple erc32_tc;
+#define ERC32_REAL_TIME_CLOCK_FREQUENCY 1000000
 
-static uint32_t erc32_tc_get( rtems_timecounter_simple *tc )
-{
-  return ERC32_MEC.Real_Time_Clock_Counter;
-}
+static struct timecounter erc32_tc;
 
-static bool erc32_tc_is_pending( rtems_timecounter_simple *tc )
+static void erc32_clock_init( void )
 {
-  return ERC32_Is_interrupt_pending( ERC32_INTERRUPT_REAL_TIME_CLOCK );
+  struct timecounter *tc;
+
+  tc = &erc32_tc;
+  tc->tc_get_timecount = _SPARC_Get_timecount_clock;
+  tc->tc_counter_mask = 0xffffffff;
+  tc->tc_frequency = ERC32_REAL_TIME_CLOCK_FREQUENCY;
+  tc->tc_quality = RTEMS_TIMECOUNTER_QUALITY_CLOCK_DRIVER;
+  rtems_timecounter_install(tc);
 }
 
-static uint32_t erc32_tc_get_timecount( struct timecounter *tc )
+uint32_t _CPU_Counter_frequency(void)
 {
-  return rtems_timecounter_simple_downcounter_get(
-    tc,
-    erc32_tc_get,
-    erc32_tc_is_pending
-  );
+  return ERC32_REAL_TIME_CLOCK_FREQUENCY;
 }
 
-static void erc32_tc_at_tick( rtems_timecounter_simple *tc )
+static void erc32_clock_at_tick( void )
 {
-  /* Nothing to do */
-}
+  SPARC_Counter *counter;
+  rtems_interrupt_level level;
 
-static void erc32_tc_tick( void )
-{
-  rtems_timecounter_simple_downcounter_tick(
-    &erc32_tc,
-    erc32_tc_get,
-    erc32_tc_at_tick
-  );
+  counter = &_SPARC_Counter_mutable;
+  rtems_interrupt_local_disable(level);
+
+  ERC32_Clear_interrupt( ERC32_INTERRUPT_REAL_TIME_CLOCK );
+  counter->accumulated += counter->interval;
+
+  rtems_interrupt_local_enable(level);
 }
 
-static void erc32_counter_initialize( void )
+static void erc32_clock_initialize_early( void )
 {
-  _SPARC_Counter_initialize(
-    _SPARC_Counter_read_address,
-    _SPARC_Counter_difference_clock_period,
-    &ERC32_MEC.Real_Time_Clock_Counter
+  SPARC_Counter *counter;
+
+  /* approximately 1 us per countdown */
+  ERC32_MEC.Real_Time_Clock_Scalar = CLOCK_SPEED - 1;
+  ERC32_MEC.Real_Time_Clock_Counter =
+    rtems_configuration_get_microseconds_per_tick();
+  ERC32_MEC_Set_Real_Time_Clock_Timer_Control(
+      ERC32_MEC_TIMER_COUNTER_ENABLE_COUNTING |
+      ERC32_MEC_TIMER_COUNTER_LOAD_SCALER |
+      ERC32_MEC_TIMER_COUNTER_LOAD_COUNTER
+  );
+  ERC32_MEC_Set_Real_Time_Clock_Timer_Control(
+    ERC32_MEC_TIMER_COUNTER_ENABLE_COUNTING |
+    ERC32_MEC_TIMER_COUNTER_RELOAD_AT_ZERO
   );
-}
 
-uint32_t _CPU_Counter_frequency(void)
-{
-  return ERC32_REAL_TIME_CLOCK_FREQUENCY;
+  counter = &_SPARC_Counter_mutable;
+  counter->read_isr_disabled = _SPARC_Counter_read_clock_isr_disabled;
+  counter->read = _SPARC_Counter_read_clock;
+  counter->counter_register = &ERC32_MEC.Real_Time_Clock_Counter,
+  counter->pending_register = &ERC32_MEC.Interrupt_Pending;
+  counter->pending_mask = UINT32_C(1) << ERC32_INTERRUPT_REAL_TIME_CLOCK;
+  counter->accumulated = rtems_configuration_get_microseconds_per_tick();
+  counter->interval = rtems_configuration_get_microseconds_per_tick();
 }
 
-#define Clock_driver_support_initialize_hardware() \
+RTEMS_SYSINIT_ITEM(
+  erc32_clock_initialize_early,
+  RTEMS_SYSINIT_CPU_COUNTER,
+  RTEMS_SYSINIT_ORDER_FIRST
+);
+
+/*
+ *  The Real Time Clock Counter Timer uses this trap type.
+ */
+#define CLOCK_VECTOR ERC32_TRAP_TYPE( ERC32_INTERRUPT_REAL_TIME_CLOCK )
+
+#define Clock_driver_support_install_isr( _new ) \
+  set_vector( _new, CLOCK_VECTOR, 1 )
+
+#define Clock_driver_support_set_interrupt_affinity( _online_processors ) \
   do { \
-    /* approximately 1 us per countdown */ \
-    ERC32_MEC.Real_Time_Clock_Scalar  = CLOCK_SPEED - 1; \
-    ERC32_MEC.Real_Time_Clock_Counter = \
-      rtems_configuration_get_microseconds_per_tick(); \
-    \
-    ERC32_MEC_Set_Real_Time_Clock_Timer_Control( \
-        ERC32_MEC_TIMER_COUNTER_ENABLE_COUNTING | \
-        ERC32_MEC_TIMER_COUNTER_LOAD_SCALER | \
-        ERC32_MEC_TIMER_COUNTER_LOAD_COUNTER \
-    ); \
-    \
-    ERC32_MEC_Set_Real_Time_Clock_Timer_Control( \
-        ERC32_MEC_TIMER_COUNTER_ENABLE_COUNTING | \
-        ERC32_MEC_TIMER_COUNTER_RELOAD_AT_ZERO \
-    );  \
-    rtems_timecounter_simple_install( \
-        &erc32_tc, \
-        ERC32_REAL_TIME_CLOCK_FREQUENCY, \
-        rtems_configuration_get_microseconds_per_tick(), \
-        erc32_tc_get_timecount \
-    ); \
-    erc32_counter_initialize(); \
+    (void) _online_processors; \
   } while (0)
 
-#define Clock_driver_timecounter_tick() erc32_tc_tick()
+#define Clock_driver_support_at_tick() erc32_clock_at_tick()
+
+#define Clock_driver_support_initialize_hardware() erc32_clock_init()
 
 #include "../../../shared/dev/clock/clockimpl.h"
 
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"
 
diff --git a/bsps/sparc/leon3/clock/ckinit.c b/bsps/sparc/leon3/clock/ckinit.c
index 0cd78ea716..fecbf32c1b 100644
--- a/bsps/sparc/leon3/clock/ckinit.c
+++ b/bsps/sparc/leon3/clock/ckinit.c
@@ -27,6 +27,7 @@
 #include <rtems/rtems/intr.h>
 #include <ambapp.h>
 #include <rtems/score/profiling.h>
+#include <rtems/score/sparcimpl.h>
 #include <rtems/timecounter.h>
 
 /* The LEON3 BSP Timer driver can rely on the Driver Manager if the
@@ -42,59 +43,7 @@ static int clkirq;
 
 static void (*leon3_tc_tick)(void);
 
-static rtems_timecounter_simple leon3_tc;
-
-#ifndef RTEMS_SMP
-static uint32_t leon3_tc_get(rtems_timecounter_simple *tc)
-{
-  return LEON3_Timer_Regs->timer[LEON3_CLOCK_INDEX].value;
-}
-
-static bool leon3_tc_is_pending(rtems_timecounter_simple *tc)
-{
-  return LEON_Is_interrupt_pending(clkirq);
-}
-
-static void leon3_tc_at_tick( rtems_timecounter_simple *tc )
-{
-  /* Nothing to do */
-}
-
-static uint32_t leon3_tc_get_timecount(struct timecounter *tc)
-{
-  return rtems_timecounter_simple_downcounter_get(
-    tc,
-    leon3_tc_get,
-    leon3_tc_is_pending
-  );
-}
-
-static void leon3_tc_tick_simple(void)
-{
-  rtems_timecounter_simple_downcounter_tick(
-    &leon3_tc,
-    leon3_tc_get,
-    leon3_tc_at_tick
-  );
-}
-#endif
-
-static uint32_t leon3_tc_get_timecount_up_counter(struct timecounter *tc)
-{
-  return leon3_up_counter_low();
-}
-
-static uint32_t leon3_tc_get_timecount_irqmp(struct timecounter *tc)
-{
-  return LEON3_IrqCtrl_Regs->timestamp[0].counter;
-}
-
-#ifdef RTEMS_SMP
-static uint32_t leon3_tc_get_timecount_second_timer(struct timecounter *tc)
-{
-  return 0xffffffff - LEON3_Timer_Regs->timer[LEON3_CLOCK_INDEX + 1].value;
-}
-#endif
+static struct timecounter leon3_tc;
 
 #ifdef RTEMS_PROFILING
 #define IRQMP_TIMESTAMP_S1_S2 ((1U << 25) | (1U << 26))
@@ -146,12 +95,23 @@ static void leon3_tc_tick_irqmp_timestamp_init(void)
   rtems_timecounter_tick();
 }
 
-#ifdef RTEMS_SMP
-static void leon3_tc_tick_second_timer(void)
+static void leon3_tc_tick_default(void)
 {
+#ifndef RTEMS_SMP
+  SPARC_Counter *counter;
+  rtems_interrupt_level level;
+
+  counter = &_SPARC_Counter_mutable;
+  rtems_interrupt_local_disable(level);
+
+  LEON3_IrqCtrl_Regs->iclear = counter->pending_mask;
+  counter->accumulated += counter->interval;
+
+  rtems_interrupt_local_enable(level);
+#endif
+
   rtems_timecounter_tick();
 }
-#endif
 
 static void leon3_tc_do_tick(void)
 {
@@ -196,9 +156,11 @@ static void leon3_clock_initialize(void)
 {
   volatile struct irqmp_timestamp_regs *irqmp_ts;
   volatile struct gptimer_regs *gpt;
+  struct timecounter *tc;
 
   irqmp_ts = &LEON3_IrqCtrl_Regs->timestamp[0];
   gpt = LEON3_Timer_Regs;
+  tc = &leon3_tc;
 
   gpt->timer[LEON3_CLOCK_INDEX].reload =
     rtems_configuration_get_microseconds_per_tick() - 1;
@@ -210,10 +172,8 @@ static void leon3_clock_initialize(void)
 
   if (leon3_up_counter_is_available()) {
     /* Use the LEON4 up-counter if available */
-    leon3_tc.tc.tc_get_timecount = leon3_tc_get_timecount_up_counter;
-    leon3_tc.tc.tc_counter_mask = 0xffffffff;
-    leon3_tc.tc.tc_frequency = leon3_up_counter_frequency();
-    leon3_tc.tc.tc_quality = RTEMS_TIMECOUNTER_QUALITY_CLOCK_DRIVER;
+    tc->tc_get_timecount = _SPARC_Get_timecount_asr23;
+    tc->tc_frequency = leon3_up_counter_frequency();
 
 #ifdef RTEMS_PROFILING
     if (!leon3_irqmp_has_timestamp(irqmp_ts)) {
@@ -222,13 +182,11 @@ static void leon3_clock_initialize(void)
 #endif
 
     leon3_tc_tick = leon3_tc_tick_irqmp_timestamp_init;
-    rtems_timecounter_install(&leon3_tc.tc);
   } else if (leon3_irqmp_has_timestamp(irqmp_ts)) {
     /* Use the interrupt controller timestamp counter if available */
-    leon3_tc.tc.tc_get_timecount = leon3_tc_get_timecount_irqmp;
-    leon3_tc.tc.tc_counter_mask = 0xffffffff;
-    leon3_tc.tc.tc_frequency = ambapp_freq_get(&ambapp_plb, LEON3_Timer_Adev);
-    leon3_tc.tc.tc_quality = RTEMS_TIMECOUNTER_QUALITY_CLOCK_DRIVER;
+    tc->tc_get_timecount = _SPARC_Get_timecount_up;
+    tc->tc_frequency = ambapp_freq_get(&ambapp_plb, LEON3_Timer_Adev);
+
     leon3_tc_tick = leon3_tc_tick_irqmp_timestamp_init;
 
     /*
@@ -236,8 +194,6 @@ static void leon3_clock_initialize(void)
      * counter.  Use an arbitrary interrupt source.
      */
     irqmp_ts->control = 0x1;
-
-    rtems_timecounter_install(&leon3_tc.tc);
   } else {
 #ifdef RTEMS_SMP
     /*
@@ -245,24 +201,32 @@ static void leon3_clock_initialize(void)
      * controller.  At least on SMP configurations we must use a second timer
      * in free running mode for the timecounter.
      */
-    gpt->timer[LEON3_CLOCK_INDEX + 1].ctrl =
+    gpt->timer[LEON3_COUNTER_GPTIMER_INDEX].ctrl =
       GPTIMER_TIMER_CTRL_EN | GPTIMER_TIMER_CTRL_IE;
-    leon3_tc.tc.tc_get_timecount = leon3_tc_get_timecount_second_timer;
-    leon3_tc.tc.tc_counter_mask = 0xffffffff;
-    leon3_tc.tc.tc_frequency = LEON3_GPTIMER_0_FREQUENCY_SET_BY_BOOT_LOADER;
-    leon3_tc.tc.tc_quality = RTEMS_TIMECOUNTER_QUALITY_CLOCK_DRIVER;
-    leon3_tc_tick = leon3_tc_tick_second_timer;
-    rtems_timecounter_install(&leon3_tc.tc);
+
+    tc->tc_get_timecount = _SPARC_Get_timecount_down;
 #else
-    leon3_tc_tick = leon3_tc_tick_simple;
-    rtems_timecounter_simple_install(
-      &leon3_tc,
-      LEON3_GPTIMER_0_FREQUENCY_SET_BY_BOOT_LOADER,
-      rtems_configuration_get_microseconds_per_tick(),
-      leon3_tc_get_timecount
-    );
+    SPARC_Counter *counter;
+
+    counter = &_SPARC_Counter_mutable;
+    counter->read_isr_disabled = _SPARC_Counter_read_clock_isr_disabled;
+    counter->read = _SPARC_Counter_read_clock;
+    counter->counter_register = &gpt->timer[LEON3_CLOCK_INDEX].value;
+    counter->pending_register = &LEON3_IrqCtrl_Regs->ipend;
+    counter->pending_mask = UINT32_C(1) << clkirq;
+    counter->accumulated = rtems_configuration_get_microseconds_per_tick();
+    counter->interval = rtems_configuration_get_microseconds_per_tick();
+
+    tc->tc_get_timecount = _SPARC_Get_timecount_clock;
 #endif
+
+    tc->tc_frequency = LEON3_GPTIMER_0_FREQUENCY_SET_BY_BOOT_LOADER,
+    leon3_tc_tick = leon3_tc_tick_default;
   }
+
+  tc->tc_counter_mask = 0xffffffff;
+  tc->tc_quality = RTEMS_TIMECOUNTER_QUALITY_CLOCK_DRIVER;
+  rtems_timecounter_install(tc);
 }
 
 #define Clock_driver_support_initialize_hardware() \
diff --git a/bsps/sparc/leon3/include/leon.h b/bsps/sparc/leon3/include/leon.h
index 758b760b0b..afe0d91ca4 100644
--- a/bsps/sparc/leon3/include/leon.h
+++ b/bsps/sparc/leon3/include/leon.h
@@ -333,6 +333,12 @@ extern rtems_interrupt_lock LEON3_IrqCtrl_Lock;
   #define LEON3_CLOCK_INDEX 0
 #endif
 
+#if defined(RTEMS_SMP)
+#define LEON3_COUNTER_GPTIMER_INDEX (LEON3_CLOCK_INDEX + 1)
+#else
+#define LEON3_COUNTER_GPTIMER_INDEX LEON3_CLOCK_INDEX
+#endif
+
 /*
  * We assume that a boot loader (usually GRMON) initialized the GPTIMER 0 to
  * run with 1MHz.  This is used to determine all clock frequencies of the PnP
diff --git a/bsps/sparc/leon3/start/cpucounter.c b/bsps/sparc/leon3/start/cpucounter.c
index f583d9b0fb..007bb6d8ec 100644
--- a/bsps/sparc/leon3/start/cpucounter.c
+++ b/bsps/sparc/leon3/start/cpucounter.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014, 2016 embedded brains GmbH.  All rights reserved.
+ * Copyright (c) 2014, 2018 embedded brains GmbH.  All rights reserved.
  *
  *  embedded brains GmbH
  *  Dornierstr. 4
@@ -29,46 +29,38 @@ static void leon3_counter_initialize(void)
 {
   volatile struct irqmp_timestamp_regs *irqmp_ts;
   volatile struct gptimer_regs *gpt;
+  SPARC_Counter *counter;
 
   irqmp_ts = &LEON3_IrqCtrl_Regs->timestamp[0];
   gpt = LEON3_Timer_Regs;
+  counter = &_SPARC_Counter_mutable;
 
   leon3_up_counter_enable();
 
   if (leon3_up_counter_is_available()) {
     /* Use the LEON4 up-counter if available */
-
-    _SPARC_Counter_initialize(
-      _SPARC_Counter_read_asr23,
-      _SPARC_Counter_difference_normal,
-      NULL
-    );
+    counter->read_isr_disabled = _SPARC_Counter_read_asr23;
+    counter->read = _SPARC_Counter_read_asr23;
 
     leon3_counter_frequency = leon3_up_counter_frequency();
   } else if (leon3_irqmp_has_timestamp(irqmp_ts)) {
     /* Use the interrupt controller timestamp counter if available */
+    counter->read_isr_disabled = _SPARC_Counter_read_up;
+    counter->read = _SPARC_Counter_read_up;
+    counter->counter_register = &LEON3_IrqCtrl_Regs->timestamp[0].counter;
 
     /* Enable interrupt timestamping for an arbitrary interrupt line */
     irqmp_ts->control = 0x1;
 
-    _SPARC_Counter_initialize(
-      _SPARC_Counter_read_address,
-      _SPARC_Counter_difference_normal,
-      (volatile const uint32_t *) &irqmp_ts->counter
-    );
-
     leon3_counter_frequency = ambapp_freq_get(&ambapp_plb, LEON3_IrqCtrl_Adev);
   } else if (gpt != NULL) {
     /* Fall back to the first GPTIMER if available */
+    counter->read_isr_disabled = _SPARC_Counter_read_down;
+    counter->read = _SPARC_Counter_read_down;
+    counter->counter_register = &gpt->timer[LEON3_COUNTER_GPTIMER_INDEX].value;
 
     /* Enable timer just in case no clock driver is configured */
-    gpt->timer[LEON3_CLOCK_INDEX].ctrl |= GPTIMER_TIMER_CTRL_EN;
-
-    _SPARC_Counter_initialize(
-      _SPARC_Counter_read_address,
-      _SPARC_Counter_difference_clock_period,
-      (volatile const uint32_t *) &gpt->timer[LEON3_CLOCK_INDEX].value
-    );
+    gpt->timer[LEON3_COUNTER_GPTIMER_INDEX].ctrl |= GPTIMER_TIMER_CTRL_EN;
 
     leon3_counter_frequency = ambapp_freq_get(&ambapp_plb, LEON3_Timer_Adev) /
       (gpt->scaler_reload + 1);