From 0a2096b7e3db4167a12e8bc660c1c43cd63e9bc4 Mon Sep 17 00:00:00 2001
From: Sebastian Huber <sebastian.huber@embedded-brains.de>
Date: Mon, 24 Feb 2014 17:06:52 +0100
Subject: bsp/leon3: Use ambapp_freq_get() for CPU counter

---
 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 | 58 ++++++++---------------
 3 files changed, 33 insertions(+), 40 deletions(-)

diff --git a/c/src/lib/libbsp/sparc/leon3/amba/amba.c b/c/src/lib/libbsp/sparc/leon3/amba/amba.c
index 5cb1880ac5..65550cb07f 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 e4710cde7d..a2c9bc6bc3 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 f27d95a79a..44faf920ba 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;
-
-    /* Assume that GRMON initialized the first GPTIMER to 1MHz */
-    uint32_t frequency = 1000000;
-
-    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
+    uint32_t max_frequency = ambapp_freq_get(&ambapp_plb, adev);
+    unsigned min_prescaler = 8;
+    uint32_t frequency = max_frequency / min_prescaler;
+
+    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 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
-      );
-    }
   }
 }
-- 
cgit v1.2.3