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

[Sebastian Huber]

---
 c/src/lib/libbsp/sparc/leon3/startup/cpucounter.c |   41 ++++++++++-----------
 1 files changed, 19 insertions(+), 22 deletions(-)

diff --git a/c/src/lib/libbsp/sparc/leon3/startup/cpucounter.c b/c/src/lib/libbsp/sparc/leon3/startup/cpucounter.c
index f27d95a..1bb908e 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,43 +65,39 @@ 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;
-
-    uint32_t max_frequency = frequency * prescaler;
-    int idx = 1;
+    gpt->scaler_reload = min_prescaler - 1;
+    gpt->timer[0].reload = 0xffffffff;
+    gpt->timer[0].ctrl = LEON3_GPTIMER_EN | LEON3_GPTIMER_RL
+      | LEON3_GPTIMER_LD;
 
+    gpt_counter_initialize(gpt, 0, frequency, free_counter_difference);
+  } else {
     adev = (void *) ambapp_for_each(
       &ambapp_plb,
       OPTIONS_ALL | OPTIONS_APB_SLVS,
       VENDOR_GAISLER,
       GAISLER_GPTIMER,
       ambapp_find_by_idx,
-      &idx
+      NULL
     );
     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;
+      /*
+       * Fall back to the first GPTIMER if available.  Assume that GRMON
+       * initialized the first GPTIMER to 1MHz.
+       */
+      uint32_t frequency = 1000000;
 
-      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,
+        adev_to_gpt(adev),
         LEON3_CLOCK_INDEX,
         frequency,
         clock_counter_difference
-- 
1.7.7