sparc: Rework CPU counter support

Rework CPU counter support to enable use of the GR740 up-counter via
%asr22 and %asr23.

6 files changed, 120 insertions, 71 deletions

diff --git a/c/src/lib/libbsp/sparc/erc32/clock/ckinit.c b/c/src/lib/libbsp/sparc/erc32/clock/ckinit.c
index d78fb0ea5d..cb5cee86fc 100644
--- a/c/src/lib/libbsp/sparc/erc32/clock/ckinit.c
+++ b/c/src/lib/libbsp/sparc/erc32/clock/ckinit.c
@@ -83,14 +83,14 @@ static void erc32_tc_tick( void )
   );
 }
 
-static CPU_Counter_ticks erc32_counter_difference(
-  CPU_Counter_ticks second,
-  CPU_Counter_ticks first
-)
+static void erc32_counter_initialize( uint32_t frequency )
 {
-  CPU_Counter_ticks period = rtems_configuration_get_microseconds_per_tick();
-
-  return (first + period - second) % period;
+  _SPARC_Counter_initialize(
+    _SPARC_Counter_read_address,
+    _SPARC_Counter_difference_clock_period,
+    &ERC32_MEC.Real_Time_Clock_Counter
+  );
+  rtems_counter_initialize_converter( frequency );
 }
 
 #define Clock_driver_support_initialize_hardware() \
@@ -117,11 +117,7 @@ static CPU_Counter_ticks erc32_counter_difference(
         rtems_configuration_get_microseconds_per_tick(), \
         erc32_tc_get_timecount \
     ); \
-    _SPARC_Counter_initialize( \
-      &ERC32_MEC.Real_Time_Clock_Counter, \
-      erc32_counter_difference \
-    ); \
-    rtems_counter_initialize_converter( frequency ); \
+    erc32_counter_initialize( frequency ); \
   } while (0)
 
 #define Clock_driver_timecounter_tick() erc32_tc_tick()
@@ -137,3 +133,8 @@ static CPU_Counter_ticks erc32_counter_difference(
 
 #include "../../../shared/clockdrv_shell.h"
 
+SPARC_Counter _SPARC_Counter = {
+  .counter_read = _SPARC_Counter_read_address,
+  .counter_difference = _SPARC_Counter_difference_one,
+  .counter_address = (uint32_t *) &_SPARC_Counter
+};
diff --git a/c/src/lib/libbsp/sparc/leon2/clock/ckinit.c b/c/src/lib/libbsp/sparc/leon2/clock/ckinit.c
index 6c2cf98ce2..f21bd18720 100644
--- a/c/src/lib/libbsp/sparc/leon2/clock/ckinit.c
+++ b/c/src/lib/libbsp/sparc/leon2/clock/ckinit.c
@@ -105,3 +105,9 @@ extern int CLOCK_SPEED;
 #define Clock_driver_timecounter_tick() leon2_tc_tick()
 
 #include "../../../shared/clockdrv_shell.h"
+
+SPARC_Counter _SPARC_Counter = {
+  .counter_read = _SPARC_Counter_read_address,
+  .counter_difference = _SPARC_Counter_difference_one,
+  .counter_address = (uint32_t *) &_SPARC_Counter
+};
diff --git a/c/src/lib/libbsp/sparc/leon3/include/leon.h b/c/src/lib/libbsp/sparc/leon3/include/leon.h
index ba0673fa75..b7511638eb 100644
--- a/c/src/lib/libbsp/sparc/leon3/include/leon.h
+++ b/c/src/lib/libbsp/sparc/leon3/include/leon.h
@@ -387,8 +387,6 @@ extern int leon3_timer_core_index;
  */
 extern unsigned int leon3_timer_prescaler;
 
-void leon3_cpu_counter_initialize(void);
-
 /* GRLIB extended IRQ controller register */
 void leon3_ext_irq_init(void);
 
@@ -457,6 +455,51 @@ static inline bool leon3_irqmp_has_timestamp(
   return (irqmp_ts->control >> 27) > 0;
 }
 
+static inline uint32_t leon3_up_counter_low(void)
+{
+  uint32_t asr23;
+
+  __asm__ volatile (
+    "mov %%asr23, %0"
+    : "=&r" (asr23)
+  );
+
+  return asr23;
+}
+
+static inline uint32_t leon3_up_counter_high(void)
+{
+  uint32_t asr22;
+
+  __asm__ volatile (
+    "mov %%asr22, %0"
+    : "=&r" (asr22)
+  );
+
+  return asr22;
+}
+
+static inline void leon3_up_counter_enable(void)
+{
+  __asm__ volatile (
+    "mov %g0, %asr23"
+  );
+}
+
+static inline bool leon3_up_counter_is_available(void)
+{
+  return leon3_up_counter_low() != leon3_up_counter_low();
+}
+
+static inline uint32_t leon3_up_counter_frequency(void)
+{
+  /*
+   * For simplicity, assume that the interrupt controller uses the processor
+   * clock.  This is at least true on the GR740.
+   */
+  return ambapp_freq_get(&ambapp_plb, LEON3_IrqCtrl_Adev);
+}
+
 #endif /* !ASM */
 
 #ifdef __cplusplus
diff --git a/c/src/lib/libbsp/sparc/leon3/startup/bspstart.c b/c/src/lib/libbsp/sparc/leon3/startup/bspstart.c
index 35fe233561..80c2bc004e 100644
--- a/c/src/lib/libbsp/sparc/leon3/startup/bspstart.c
+++ b/c/src/lib/libbsp/sparc/leon3/startup/bspstart.c
@@ -58,8 +58,6 @@ static inline int set_snooping(void)
 void bsp_start( void )
 {
   CPU_SPARC_HAS_SNOOPING = set_snooping();
-
-  leon3_cpu_counter_initialize();
 }
 
 static void leon3_cpu_index_init(void)
diff --git a/c/src/lib/libbsp/sparc/leon3/startup/cpucounter.c b/c/src/lib/libbsp/sparc/leon3/startup/cpucounter.c
index ea0e67118d..53d921a66a 100644
--- a/c/src/lib/libbsp/sparc/leon3/startup/cpucounter.c
+++ b/c/src/lib/libbsp/sparc/leon3/startup/cpucounter.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014 embedded brains GmbH.  All rights reserved.
+ * Copyright (c) 2014, 2016 embedded brains GmbH.  All rights reserved.
  *
  *  embedded brains GmbH
  *  Dornierstr. 4
@@ -15,69 +15,66 @@
 #include <leon.h>
 
 #include <rtems/counter.h>
+#include <rtems/sysinit.h>
 
-static CPU_Counter_ticks timestamp_counter_difference(
-  CPU_Counter_ticks second,
-  CPU_Counter_ticks first
-)
+static void leon3_counter_initialize(void)
 {
-  return second - first;
-}
-
-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();
+  volatile struct irqmp_timestamp_regs *irqmp_ts;
+  volatile struct gptimer_regs *gpt;
+  unsigned int freq;
 
-  return (first + period - second) % period;
-}
+  irqmp_ts = &LEON3_IrqCtrl_Regs->timestamp[0];
+  gpt = LEON3_Timer_Regs;
 
-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
-  );
+  leon3_up_counter_enable();
 
-  rtems_counter_initialize_converter(frequency);
-}
+  if (leon3_up_counter_is_available()) {
+    /* Use the LEON4 up-counter if available */
 
-void leon3_cpu_counter_initialize(void)
-{
-  volatile struct irqmp_timestamp_regs *irqmp_ts =
-    &LEON3_IrqCtrl_Regs->timestamp[0];
-  unsigned int freq;
+    _SPARC_Counter_initialize(
+      _SPARC_Counter_read_asr23,
+      _SPARC_Counter_difference_normal,
+      NULL
+    );
 
-  if (leon3_irqmp_has_timestamp(irqmp_ts)) {
+    freq = leon3_up_counter_frequency();
+    rtems_counter_initialize_converter(freq);
+  } else if (leon3_irqmp_has_timestamp(irqmp_ts)) {
     /* Use the interrupt controller timestamp counter if available */
 
     /* Enable interrupt timestamping for an arbitrary interrupt line */
     irqmp_ts->control = 0x1;
 
     _SPARC_Counter_initialize(
-      (volatile const uint32_t *) &irqmp_ts->counter,
-      timestamp_counter_difference
+      _SPARC_Counter_read_address,
+      _SPARC_Counter_difference_normal,
+      (volatile const uint32_t *) &irqmp_ts->counter
     );
 
-    /* Get and set the frequency */
-    rtems_counter_initialize_converter(
-      ambapp_freq_get(&ambapp_plb, LEON3_IrqCtrl_Adev));
-  } else if (LEON3_Timer_Regs != NULL) {
-      /* Fall back to the first GPTIMER if available */
-      freq = ambapp_freq_get(&ambapp_plb, LEON3_Timer_Adev);
+    freq = ambapp_freq_get(&ambapp_plb, LEON3_IrqCtrl_Adev);
+    rtems_counter_initialize_converter(freq);
+  } else if (gpt != NULL) {
+    /* Fall back to the first GPTIMER if available */
+
+    _SPARC_Counter_initialize(
+      _SPARC_Counter_read_address,
+      _SPARC_Counter_difference_clock_period,
+      (volatile const uint32_t *) &gpt->timer[LEON3_CLOCK_INDEX].value
+    );
 
-      gpt_counter_initialize(
-        LEON3_Timer_Regs,
-        LEON3_CLOCK_INDEX,
-        freq / (LEON3_Timer_Regs->scaler_reload - 1),
-        clock_counter_difference
-      );
+    freq = ambapp_freq_get(&ambapp_plb, LEON3_Timer_Adev);
+    rtems_counter_initialize_converter(freq / (gpt->scaler_reload - 1));
   }
 }
+
+RTEMS_SYSINIT_ITEM(
+  leon3_counter_initialize,
+  RTEMS_SYSINIT_BSP_START,
+  RTEMS_SYSINIT_ORDER_THIRD
+);
+
+SPARC_Counter _SPARC_Counter = {
+  .counter_read = _SPARC_Counter_read_address,
+  .counter_difference = _SPARC_Counter_difference_one,
+  .counter_address = (uint32_t *) &_SPARC_Counter
+};
diff --git a/c/src/lib/libbsp/sparc/shared/irq_asm.S b/c/src/lib/libbsp/sparc/shared/irq_asm.S
index b7f372c654..4a0c3822c8 100644
--- a/c/src/lib/libbsp/sparc/shared/irq_asm.S
+++ b/c/src/lib/libbsp/sparc/shared/irq_asm.S
@@ -451,10 +451,12 @@ dont_do_the_window:
         subcc    %l7, 1, %l7             ! outermost interrupt handler?
         bnz      dont_switch_stacks      ! No, then do not switch stacks
 
-#if defined( RTEMS_PROFILING )
-         sethi   %hi(SYM(_SPARC_Counter)), %o5
-        ld       [%o5 + %lo(SYM(_SPARC_Counter))], %l4
-        ld       [%l4], %o5
+#if defined(RTEMS_PROFILING)
+         sethi   %hi(_SPARC_Counter), %o5
+        ld       [%o5 + %lo(_SPARC_Counter)], %l4
+        call     %l4, 0
+         nop
+        mov      %o0, %o5
 #else
          nop
 #endif
@@ -536,13 +538,15 @@ pil_fixed:
                                         !   WAS LOADED WHEN ISF WAS SAVED!!!
         mov      %l3, %o0               ! o0 = 1st arg = vector number
         call     %g4, 0
-#if defined( RTEMS_PROFILING )
+#if defined(RTEMS_PROFILING)
          mov     %o5, %l3               ! save interrupt entry instant
         cmp      %l7, 0
         bne      profiling_not_outer_most_exit
          nop
         ta       SPARC_SWTRAP_IRQDIS    ! Call interrupt disable trap handler
-        ld       [%l4], %o2             ! o2 = 3rd arg = interrupt exit instant
+        call     %l4, 0                 ! Call _SPARC_Counter.counter_read
+         nop
+        mov      %o0, %o2               ! o2 = 3rd arg = interrupt exit instant
         mov      %l3, %o1               ! o1 = 2nd arg = interrupt entry instant
         call     SYM(_Profiling_Outer_most_interrupt_entry_and_exit), 0
          mov     %g6, %o0               ! o0 = 1st arg = per-CPU control