bsp/leon3: Add LEON3_IRQAMP_PROBE_TIMESTAMP

1 files changed, 66 insertions, 37 deletions

diff --git a/bsps/sparc/leon3/start/cpucounter.c b/bsps/sparc/leon3/start/cpucounter.c
index 57fd487009..18fc8099f2 100644
--- a/bsps/sparc/leon3/start/cpucounter.c
+++ b/bsps/sparc/leon3/start/cpucounter.c
@@ -39,60 +39,89 @@ uint32_t _CPU_Counter_frequency(void)
   return leon3_counter_frequency;
 }
 
+static void leon3_counter_use_up_counter(SPARC_Counter *counter)
+{
+  counter->read_isr_disabled = _SPARC_Counter_read_asr23;
+  counter->read = _SPARC_Counter_read_asr23;
+
+  leon3_counter_frequency = leon3_up_counter_frequency();
+}
+
+#if defined(LEON3_IRQAMP_PROBE_TIMESTAMP)
+static void leon3_counter_use_irqamp_timestamp(
+  SPARC_Counter *counter,
+  irqamp_timestamp *irqmp_ts
+)
+{
+  counter->read_isr_disabled = _SPARC_Counter_read_up;
+  counter->read = _SPARC_Counter_read_up;
+  counter->counter_register = &irqmp_ts->itcnt;
+
+  /* Enable interrupt timestamping for an arbitrary interrupt line */
+  grlib_store_32(&irqmp_ts->itstmpc, IRQAMP_ITSTMPC_TSISEL(1));
+
+#if defined(LEON3_PLB_FREQUENCY_DEFINED_BY_GPTIMER)
+  leon3_counter_frequency = leon3_processor_local_bus_frequency();
+#else
+  leon3_counter_frequency = ambapp_freq_get(ambapp_plb(), LEON3_IrqCtrl_Adev);
+#endif
+}
+#endif
+
+static void leon3_counter_use_gptimer(SPARC_Counter *counter, gptimer *gpt)
+{
+  gptimer_timer *timer;
+
+  timer = &gpt->timer[LEON3_COUNTER_GPTIMER_INDEX];
+  counter->read_isr_disabled = _SPARC_Counter_read_down;
+  counter->read = _SPARC_Counter_read_down;
+  counter->counter_register = &timer->tcntval;
+
+  /* Make timer free running */
+  grlib_store_32(&timer->trldval, 0xffffffff);
+  grlib_store_32(&timer->tctrl, GPTIMER_TCTRL_EN | GPTIMER_TCTRL_RS);
+
+#if defined(LEON3_PLB_FREQUENCY_DEFINED_BY_GPTIMER)
+  leon3_counter_frequency = LEON3_GPTIMER_0_FREQUENCY_SET_BY_BOOT_LOADER;
+#else
+  leon3_counter_frequency = ambapp_freq_get(ambapp_plb(), LEON3_Timer_Adev) /
+    (grlib_load_32(&gpt->sreload) + 1);
+#endif
+}
+
 static void leon3_counter_initialize(void)
 {
+#if defined(LEON3_IRQAMP_PROBE_TIMESTAMP)
   irqamp_timestamp *irqmp_ts;
+#endif
   gptimer *gpt;
   SPARC_Counter *counter;
 
-  irqmp_ts = irqamp_get_timestamp_registers(LEON3_IrqCtrl_Regs);
-  gpt = LEON3_Timer_Regs;
   counter = &_SPARC_Counter_mutable;
 
   leon3_up_counter_enable();
 
   if (leon3_up_counter_is_available()) {
     /* Use the LEON4 up-counter if available */
-    counter->read_isr_disabled = _SPARC_Counter_read_asr23;
-    counter->read = _SPARC_Counter_read_asr23;
-
-    leon3_counter_frequency = leon3_up_counter_frequency();
-  } else if (irqmp_ts != NULL) {
-    /* Use the interrupt controller timestamp counter if available */
-    counter->read_isr_disabled = _SPARC_Counter_read_up;
-    counter->read = _SPARC_Counter_read_up;
-    counter->counter_register = &irqmp_ts->itcnt;
+    leon3_counter_use_up_counter(counter);
+    return;
+  }
 
-    /* Enable interrupt timestamping for an arbitrary interrupt line */
-    grlib_store_32(&irqmp_ts->itstmpc, IRQAMP_ITSTMPC_TSISEL(1));
+#if defined(LEON3_IRQAMP_PROBE_TIMESTAMP)
+  irqmp_ts = irqamp_get_timestamp_registers(LEON3_IrqCtrl_Regs);
 
-#if defined(LEON3_PLB_FREQUENCY_DEFINED_BY_GPTIMER)
-    leon3_counter_frequency = leon3_processor_local_bus_frequency();
-#else
-    leon3_counter_frequency = ambapp_freq_get(ambapp_plb(), LEON3_IrqCtrl_Adev);
+  if (irqmp_ts != NULL) {
+    /* Use the interrupt controller timestamp counter if available */
+    leon3_counter_use_irqamp_timestamp(counter, irqmp_ts);
+    return;
+  }
 #endif
-  } else if (gpt != NULL) {
-    gptimer_timer *timer;
-    uint32_t       tctrl;
-
-    /* Fall back to the first GPTIMER if available */
-    timer = &gpt->timer[LEON3_COUNTER_GPTIMER_INDEX];
-    counter->read_isr_disabled = _SPARC_Counter_read_down;
-    counter->read = _SPARC_Counter_read_down;
-    counter->counter_register = &timer->tcntval;
 
-    /* Enable timer just in case no clock driver is configured */
-    grlib_store_32(&timer->trldval, 0xffffffff);
-    tctrl = grlib_load_32(&timer->tctrl);
-    tctrl |= GPTIMER_TCTRL_EN | GPTIMER_TCTRL_RS | GPTIMER_TCTRL_LD;
-    grlib_store_32(&timer->tctrl, tctrl);
+  gpt = LEON3_Timer_Regs;
 
-#if defined(LEON3_PLB_FREQUENCY_DEFINED_BY_GPTIMER)
-    leon3_counter_frequency = LEON3_GPTIMER_0_FREQUENCY_SET_BY_BOOT_LOADER;
-#else
-    leon3_counter_frequency = ambapp_freq_get(ambapp_plb(), LEON3_Timer_Adev) /
-      (grlib_load_32(&gpt->sreload) + 1);
-#endif
+  if (gpt != NULL) {
+    /* Fall back to the first GPTIMER if available */
+    leon3_counter_use_gptimer(counter, gpt);
   }
 }