sparc: Fix CPU counter support

The SPARC processors supported by RTEMS have no built-in CPU counter
support.  We have to use some hardware counter module for this purpose.
The BSP must provide a 32-bit register which contains the current CPU
counter value and a function for the difference calculation.  It can use
for example the GPTIMER instance used for the clock driver.

9 files changed, 180 insertions, 38 deletions

diff --git a/c/src/lib/libbsp/sparc/erc32/Makefile.am b/c/src/lib/libbsp/sparc/erc32/Makefile.am
index 08c6a8b0e6..677c44e2b7 100644
--- a/c/src/lib/libbsp/sparc/erc32/Makefile.am
+++ b/c/src/lib/libbsp/sparc/erc32/Makefile.am
@@ -35,7 +35,6 @@ libbsp_a_SOURCES += ../../shared/bspclean.c ../../shared/bsplibc.c \
     ../../shared/sbrk.c startup/setvec.c startup/spurious.c \
     startup/erc32mec.c startup/boardinit.S startup/bspidle.c \
     startup/bspdelay.c ../../sparc/shared/startup/early_malloc.c
-libbsp_a_SOURCES += ../../shared/cpucounterread.c
 # ISR Handler
 libbsp_a_SOURCES += ../../sparc/shared/irq_asm.S
 # gnatsupp
diff --git a/c/src/lib/libbsp/sparc/erc32/clock/ckinit.c b/c/src/lib/libbsp/sparc/erc32/clock/ckinit.c
index 1287645064..111a63d2ca 100644
--- a/c/src/lib/libbsp/sparc/erc32/clock/ckinit.c
+++ b/c/src/lib/libbsp/sparc/erc32/clock/ckinit.c
@@ -24,6 +24,7 @@
 
 #include <bsp.h>
 #include <bspopts.h>
+#include <rtems/counter.h>
 
 #if SIMSPARC_FAST_IDLE==1
 #define CLOCK_DRIVER_USE_FAST_IDLE 1
@@ -63,6 +64,16 @@ uint32_t bsp_clock_nanoseconds_since_last_tick(void)
 #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
+)
+{
+  CPU_Counter_ticks period = rtems_configuration_get_microseconds_per_tick();
+
+  return (first + period - second) % period;
+}
+
 #define Clock_driver_support_initialize_hardware() \
   do { \
     /* approximately 1 us per countdown */ \
@@ -80,6 +91,11 @@ uint32_t bsp_clock_nanoseconds_since_last_tick(void)
       ERC32_MEC_TIMER_COUNTER_ENABLE_COUNTING | \
 	    ERC32_MEC_TIMER_COUNTER_RELOAD_AT_ZERO \
     ); \
+    _SPARC_Counter_initialize( \
+      &ERC32_MEC.Real_Time_Clock_Counter, \
+      erc32_counter_difference \
+    ); \
+    rtems_counter_initialize_converter(1000000); \
   } while (0)
 
 #define Clock_driver_support_shutdown_hardware() \
diff --git a/c/src/lib/libbsp/sparc/leon2/Makefile.am b/c/src/lib/libbsp/sparc/leon2/Makefile.am
index 8bbe69b201..c08f159f81 100644
--- a/c/src/lib/libbsp/sparc/leon2/Makefile.am
+++ b/c/src/lib/libbsp/sparc/leon2/Makefile.am
@@ -52,7 +52,6 @@ libbsp_a_SOURCES += ../../shared/bspclean.c ../../shared/bsplibc.c \
     ../../shared/sbrk.c startup/setvec.c startup/spurious.c startup/bspidle.c \
     ../../shared/bspinit.c startup/bspdelay.c \
     ../../sparc/shared/startup/early_malloc.c
-libbsp_a_SOURCES += ../../shared/cpucounterread.c
 # ISR Handler
 libbsp_a_SOURCES += ../../sparc/shared/irq_asm.S
 # gnatsupp
diff --git a/c/src/lib/libbsp/sparc/leon3/clock/ckinit.c b/c/src/lib/libbsp/sparc/leon3/clock/ckinit.c
index e0556ba31a..d2aae23613 100644
--- a/c/src/lib/libbsp/sparc/leon3/clock/ckinit.c
+++ b/c/src/lib/libbsp/sparc/leon3/clock/ckinit.c
@@ -30,13 +30,6 @@
  *  The Real Time Clock Counter Timer uses this trap type.
  */
 
-#if defined(RTEMS_MULTIPROCESSING)
-  #define LEON3_CLOCK_INDEX \
-    (rtems_configuration_get_user_multiprocessing_table() ? LEON3_Cpu_Index : 0)
-#else
-  #define LEON3_CLOCK_INDEX 0
-#endif
-
 volatile struct gptimer_regs *LEON3_Timer_Regs = 0;
 static int clkirq;
 
diff --git a/c/src/lib/libbsp/sparc/leon3/include/leon.h b/c/src/lib/libbsp/sparc/leon3/include/leon.h
index ccee1a381d..e4710cde7d 100644
--- a/c/src/lib/libbsp/sparc/leon3/include/leon.h
+++ b/c/src/lib/libbsp/sparc/leon3/include/leon.h
@@ -273,6 +273,13 @@ extern rtems_interrupt_lock LEON3_IrqCtrl_Lock;
 #define LEON_REG_TIMER_COUNTER_DEFINED_MASK       0x00000003
 #define LEON_REG_TIMER_COUNTER_CURRENT_MODE_MASK  0x00000003
 
+#if defined(RTEMS_MULTIPROCESSING)
+  #define LEON3_CLOCK_INDEX \
+    (rtems_configuration_get_user_multiprocessing_table() ? LEON3_Cpu_Index : 0)
+#else
+  #define LEON3_CLOCK_INDEX 0
+#endif
+
 /* Load 32-bit word by forcing a cache-miss */
 static inline unsigned int leon_r32_no_cache(uintptr_t addr)
 {
diff --git a/c/src/lib/libbsp/sparc/leon3/startup/cpucounter.c b/c/src/lib/libbsp/sparc/leon3/startup/cpucounter.c
index e773d4d6b6..f27d95a79a 100644
--- a/c/src/lib/libbsp/sparc/leon3/startup/cpucounter.c
+++ b/c/src/lib/libbsp/sparc/leon3/startup/cpucounter.c
@@ -12,22 +12,51 @@
  * http://www.rtems.com/license/LICENSE.
  */
 
-#include <bsp.h>
-#include <bsp/fatal.h>
 #include <leon.h>
 
 #include <rtems/counter.h>
 
-static volatile struct gptimer_regs *leon3_cpu_counter_gpt;
+static volatile struct gptimer_regs *adev_to_gpt(struct ambapp_dev *adev)
+{
+  return (volatile struct gptimer_regs *) DEV_TO_APB(adev)->start;
+}
+
+static CPU_Counter_ticks free_counter_difference(
+  CPU_Counter_ticks second,
+  CPU_Counter_ticks first
+)
+{
+  return first - second;
+}
+
+static CPU_Counter_ticks clock_counter_difference(
+  CPU_Counter_ticks second,
+  CPU_Counter_ticks first
+)
+{
+  CPU_Counter_ticks period = rtems_configuration_get_microseconds_per_tick();
+
+  return (first + period - second) % period;
+}
+
+static void gpt_counter_initialize(
+  volatile struct gptimer_regs *gpt,
+  size_t timer_index,
+  uint32_t frequency,
+  SPARC_Counter_difference counter_difference
+)
+{
+  _SPARC_Counter_initialize(
+    (volatile const uint32_t *) &gpt->timer[timer_index].value,
+    counter_difference
+  );
+
+  rtems_counter_initialize_converter(frequency);
+}
 
 void leon3_cpu_counter_initialize(void)
 {
   struct ambapp_dev *adev;
-  int idx = 1;
-  volatile struct gptimer_regs *gpt;
-  unsigned new_prescaler = 8;
-  unsigned prescaler;
-  uint32_t frequency;
 
   adev = (void *) ambapp_for_each(
     &ambapp_plb,
@@ -35,30 +64,47 @@ void leon3_cpu_counter_initialize(void)
     VENDOR_GAISLER,
     GAISLER_GPTIMER,
     ambapp_find_by_idx,
-    &idx
+    NULL
   );
-  if (adev == NULL) {
-    bsp_fatal(LEON3_FATAL_CPU_COUNTER_INIT);
-  }
+  if (adev != NULL) {
+    volatile struct gptimer_regs *gpt = adev_to_gpt(adev);
+    unsigned prescaler = gpt->scaler_reload + 1;
 
-  gpt = (volatile struct gptimer_regs *) DEV_TO_APB(adev)->start;
+    /* Assume that GRMON initialized the first GPTIMER to 1MHz */
+    uint32_t frequency = 1000000;
 
-  prescaler = gpt->scaler_reload + 1;
-  gpt->scaler_reload = new_prescaler - 1;
-  gpt->timer[0].reload = 0xffffffff;
-  gpt->timer[0].ctrl = LEON3_GPTIMER_EN | LEON3_GPTIMER_RL
-    | LEON3_GPTIMER_LD;
+    uint32_t max_frequency = frequency * prescaler;
+    int idx = 1;
 
-  leon3_cpu_counter_gpt = gpt;
+    adev = (void *) ambapp_for_each(
+      &ambapp_plb,
+      OPTIONS_ALL | OPTIONS_APB_SLVS,
+      VENDOR_GAISLER,
+      GAISLER_GPTIMER,
+      ambapp_find_by_idx,
+      &idx
+    );
+    if (adev != NULL) {
+      /* Use the second GPTIMER if available */
+      unsigned min_prescaler = 8;
 
-  /* Assume that GRMON initialized the timer to 1MHz */
-  frequency = UINT32_C(1000000) * (prescaler / new_prescaler);
-  rtems_counter_initialize_converter(frequency);
-}
+      gpt = adev_to_gpt(adev);
 
-CPU_Counter_ticks _CPU_Counter_read(void)
-{
-  volatile struct gptimer_regs *gpt = leon3_cpu_counter_gpt;
+      gpt->scaler_reload = min_prescaler - 1;
+      gpt->timer[0].reload = 0xffffffff;
+      gpt->timer[0].ctrl = LEON3_GPTIMER_EN | LEON3_GPTIMER_RL
+        | LEON3_GPTIMER_LD;
 
-  return 0U - gpt->timer[0].value;
+      frequency = max_frequency / min_prescaler;
+      gpt_counter_initialize(gpt, 0, frequency, free_counter_difference);
+    } else {
+      /* Fall back to the first GPTIMER */
+      gpt_counter_initialize(
+        gpt,
+        LEON3_CLOCK_INDEX,
+        frequency,
+        clock_counter_difference
+      );
+    }
+  }
 }
diff --git a/cpukit/score/cpu/sparc/Makefile.am b/cpukit/score/cpu/sparc/Makefile.am
index 504a5752fe..c6ea1c3da9 100644
--- a/cpukit/score/cpu/sparc/Makefile.am
+++ b/cpukit/score/cpu/sparc/Makefile.am
@@ -11,6 +11,7 @@ include_rtems_score_HEADERS += rtems/score/cpuatomic.h
 
 noinst_LIBRARIES = libscorecpu.a
 libscorecpu_a_SOURCES = cpu.c cpu_asm.S
+libscorecpu_a_SOURCES += sparc-counter.c
 libscorecpu_a_SOURCES += sparcv8-atomic.c
 libscorecpu_a_CPPFLAGS = $(AM_CPPFLAGS)
 
diff --git a/cpukit/score/cpu/sparc/rtems/score/cpu.h b/cpukit/score/cpu/sparc/rtems/score/cpu.h
index ee6f7136e6..47529c3b9e 100644
--- a/cpukit/score/cpu/sparc/rtems/score/cpu.h
+++ b/cpukit/score/cpu/sparc/rtems/score/cpu.h
@@ -1273,14 +1273,61 @@ static inline uint32_t CPU_swap_u32(
 
 typedef uint32_t CPU_Counter_ticks;
 
-CPU_Counter_ticks _CPU_Counter_read( void );
+typedef CPU_Counter_ticks (*SPARC_Counter_difference)(
+  CPU_Counter_ticks second,
+  CPU_Counter_ticks first
+);
+
+/*
+ * The SPARC processors supported by RTEMS have no built-in CPU counter
+ * support.  We have to use some hardware counter module for this purpose.  The
+ * BSP must provide a 32-bit register which contains the current CPU counter
+ * value and a function for the difference calculation.  It can use for example
+ * the GPTIMER instance used for the clock driver.
+ */
+typedef struct {
+  volatile const CPU_Counter_ticks *counter_register;
+  SPARC_Counter_difference counter_difference;
+} SPARC_Counter;
+
+extern SPARC_Counter _SPARC_Counter;
+
+/*
+ * Returns always a value of one regardless of the parameters.  This prevents
+ * an infinite loop in rtems_counter_delay_ticks().  Its only a reasonably safe
+ * default.
+ */
+CPU_Counter_ticks _SPARC_Counter_difference_default(
+  CPU_Counter_ticks second,
+  CPU_Counter_ticks first
+);
+
+static inline bool _SPARC_Counter_is_default( void )
+{
+  return _SPARC_Counter.counter_difference
+    == _SPARC_Counter_difference_default;
+}
+
+static inline void _SPARC_Counter_initialize(
+  volatile const CPU_Counter_ticks *counter_register,
+  SPARC_Counter_difference counter_difference
+)
+{
+  _SPARC_Counter.counter_register = counter_register;
+  _SPARC_Counter.counter_difference = counter_difference;
+}
+
+static inline CPU_Counter_ticks _CPU_Counter_read( void )
+{
+  return *_SPARC_Counter.counter_register;
+}
 
 static inline CPU_Counter_ticks _CPU_Counter_difference(
   CPU_Counter_ticks second,
   CPU_Counter_ticks first
 )
 {
-  return second - first;
+  return (*_SPARC_Counter.counter_difference)( second, first );
 }
 
 #endif /* ASM */
diff --git a/cpukit/score/cpu/sparc/sparc-counter.c b/cpukit/score/cpu/sparc/sparc-counter.c
new file mode 100644
index 0000000000..dc69b6baeb
--- /dev/null
+++ b/cpukit/score/cpu/sparc/sparc-counter.c
@@ -0,0 +1,34 @@
+/*
+ * Copyright (c) 2014 embedded brains GmbH.  All rights reserved.
+ *
+ *  embedded brains GmbH
+ *  Dornierstr. 4
+ *  82178 Puchheim
+ *  Germany
+ *  <rtems@embedded-brains.de>
+ *
+ * The license and distribution terms for this file may be
+ * found in the file LICENSE in this distribution or at
+ * http://www.rtems.com/license/LICENSE.
+ */
+
+#if HAVE_CONFIG_H
+  #include "config.h"
+#endif
+
+#include <rtems/score/cpu.h>
+
+static CPU_Counter_ticks _SPARC_Counter_register_dummy;
+
+CPU_Counter_ticks _SPARC_Counter_difference_default(
+  CPU_Counter_ticks second,
+  CPU_Counter_ticks first
+)
+{
+  return 1;
+}
+
+SPARC_Counter _SPARC_Counter = {
+  .counter_register = &_SPARC_Counter_register_dummy,
+  .counter_difference = _SPARC_Counter_difference_default
+};