[PATCH v2] bsp/leon3: Use ambapp_freq_get() for CPU counter

[Sebastian Huber]

---
 c/src/lib/libbsp/sparc/leon3/amba/amba.c          |    8 ++-
 c/src/lib/libbsp/sparc/leon3/include/leon.h       |    7 +++
 c/src/lib/libbsp/sparc/leon3/startup/cpucounter.c |   54 +++++++--------------
 3 files changed, 31 insertions(+), 38 deletions(-)

diff --git a/c/src/lib/libbsp/sparc/leon3/amba/amba.c b/c/src/lib/libbsp/sparc/leon3/amba/amba.c
index 5cb1880..65550cb 100644
--- a/c/src/lib/libbsp/sparc/leon3/amba/amba.c
+++ b/c/src/lib/libbsp/sparc/leon3/amba/amba.c
@@ -88,7 +88,11 @@ void amba_initialize(void)
     LEON3_Timer_Regs = (volatile struct gptimer_regs *)DEV_TO_APB(adev)->start;
 
     /* Register AMBA Bus Frequency */
-    ambapp_freq_init(&ambapp_plb, adev,
-                     (LEON3_Timer_Regs->scaler_reload + 1) * 1000000);
+    ambapp_freq_init(
+      &ambapp_plb,
+      adev,
+      (LEON3_Timer_Regs->scaler_reload + 1)
+        * LEON3_GPTIMER_0_FREQUENCY_SET_BY_BOOT_LOADER
+    );
   }
 }
diff --git a/c/src/lib/libbsp/sparc/leon3/include/leon.h b/c/src/lib/libbsp/sparc/leon3/include/leon.h
index e4710cd..a2c9bc6 100644
--- a/c/src/lib/libbsp/sparc/leon3/include/leon.h
+++ b/c/src/lib/libbsp/sparc/leon3/include/leon.h
@@ -280,6 +280,13 @@ extern rtems_interrupt_lock LEON3_IrqCtrl_Lock;
   #define LEON3_CLOCK_INDEX 0
 #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
+ * devices.  See also ambapp_freq_init() and ambapp_freq_get().
+ */
+#define LEON3_GPTIMER_0_FREQUENCY_SET_BY_BOOT_LOADER 1000000
+
 /* 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 f27d95a..44faf92 100644
--- a/c/src/lib/libbsp/sparc/leon3/startup/cpucounter.c
+++ b/c/src/lib/libbsp/sparc/leon3/startup/cpucounter.c
@@ -57,6 +57,7 @@ static void gpt_counter_initialize(
 void leon3_cpu_counter_initialize(void)
 {
   struct ambapp_dev *adev;
+  int idx = 1;
 
   adev = (void *) ambapp_for_each(
     &ambapp_plb,
@@ -64,47 +65,28 @@ void leon3_cpu_counter_initialize(void)
     VENDOR_GAISLER,
     GAISLER_GPTIMER,
     ambapp_find_by_idx,
-    NULL
+    &idx
   );
   if (adev != NULL) {
+    /* Use the second GPTIMER if available */
     volatile struct gptimer_regs *gpt = adev_to_gpt(adev);
-    unsigned prescaler = gpt->scaler_reload + 1;
+    uint32_t max_frequency = ambapp_freq_get(&ambapp_plb, adev);
+    unsigned min_prescaler = 8;
+    uint32_t frequency = max_frequency / min_prescaler;
 
-    /* Assume that GRMON initialized the first GPTIMER to 1MHz */
-    uint32_t frequency = 1000000;
+    gpt->scaler_reload = min_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;
-
-    adev = (void *) ambapp_for_each(
-      &ambapp_plb,
-      OPTIONS_ALL | OPTIONS_APB_SLVS,
-      VENDOR_GAISLER,
-      GAISLER_GPTIMER,
-      ambapp_find_by_idx,
-      &idx
+    gpt_counter_initialize(gpt, 0, frequency, free_counter_difference);
+  } else if (LEON3_Timer_Regs != NULL) {
+    /* Fall back to the first GPTIMER if available */
+    gpt_counter_initialize(
+      LEON3_Timer_Regs,
+      LEON3_CLOCK_INDEX,
+      LEON3_GPTIMER_0_FREQUENCY_SET_BY_BOOT_LOADER,
+      clock_counter_difference
     );
-    if (adev != NULL) {
-      /* Use the second GPTIMER if available */
-      unsigned min_prescaler = 8;
-
-      gpt = adev_to_gpt(adev);
-
-      gpt->scaler_reload = min_prescaler - 1;
-      gpt->timer[0].reload = 0xffffffff;
-      gpt->timer[0].ctrl = LEON3_GPTIMER_EN | LEON3_GPTIMER_RL
-        | LEON3_GPTIMER_LD;
-
-      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
-      );
-    }
   }
 }
-- 
1.7.7