1 files changed, 73 insertions, 27 deletions

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
+      );
+    }
+  }
 }